Add working USB HID driver, by Tomer Shalev (part of his GSoC work).

This needs support for usb interrupt transfers, so there are some changes in various USB drivers as well (only usb-drv-arc supports it at this point, others won't have working HID yet).

HID is disabled for now, as the apps/ part is not included yet.


git-svn-id: svn://svn.rockbox.org/rockbox/trunk@20962 a1c6a512-1295-4272-9138-f99709370657

16 files changed, 416 insertions, 234 deletions

diff --git a/firmware/drivers/isp1583.c b/firmware/drivers/isp1583.c
index f373741586..fa97305b52 100644
--- a/firmware/drivers/isp1583.c
+++ b/firmware/drivers/isp1583.c
@@ -29,9 +29,6 @@
 #include "logf.h"
 #include "stdio.h"
 
-#define DIR_RX                      0
-#define DIR_TX                      1
- 
 struct usb_endpoint
 {
     unsigned char *out_buf;
@@ -602,9 +599,12 @@ void usb_drv_cancel_all_transfers(void)
         endpoints[i].halt[0] = endpoints[i].halt[1] = 1;
 }
 
-int usb_drv_request_endpoint(int dir)
+int usb_drv_request_endpoint(int type, int dir)
 {
     int i, bit;
+ 
+    if (type != USB_ENDPOINT_XFER_BULK)
+        return -1;
 
     bit=(dir & USB_DIR_IN)? 2:1;
 
diff --git a/firmware/export/usb.h b/firmware/export/usb.h
index e1bacfffc7..42893c3468 100644
--- a/firmware/export/usb.h
+++ b/firmware/export/usb.h
@@ -105,7 +105,7 @@ struct usb_transfer_completion_event_data
     int length;
     void* data;
 };
-#endif
+#endif /* HAVE_USBSTACK */
 
 void usb_init(void);
 void usb_enable(bool on);
diff --git a/firmware/export/usb_core.h b/firmware/export/usb_core.h
index abf090ed9d..9c105fbc7c 100644
--- a/firmware/export/usb_core.h
+++ b/firmware/export/usb_core.h
@@ -36,6 +36,17 @@
 
 /* endpoints */
 #define EP_CONTROL 0
+
+#define DIR_OUT 0
+#define DIR_IN 1
+
+/* The USB core is a device, and OUT is RX from that P.O.V */
+#define DIR_RX DIR_OUT
+#define DIR_TX DIR_IN
+
+#define EP_DIR(ep) (((ep) & USB_ENDPOINT_DIR_MASK) ? DIR_IN : DIR_OUT)
+#define EP_NUM(ep) ((ep) & USB_ENDPOINT_NUMBER_MASK)
+
 extern int usb_max_pkt_size;
 
 struct usb_class_driver;
@@ -52,7 +63,7 @@ void usb_core_handle_transfer_completion(
         struct usb_transfer_completion_event_data* event);
 int usb_core_ack_control(struct usb_ctrlrequest* req);
 
-int usb_core_request_endpoint(int dir,struct usb_class_driver* drv);
+int usb_core_request_endpoint(int type, int dir,struct usb_class_driver* drv);
 void usb_core_release_endpoint(int dir);
 
 #ifdef HAVE_HOTSWAP
diff --git a/firmware/export/usb_drv.h b/firmware/export/usb_drv.h
index 0c99630b11..fad9c79c10 100644
--- a/firmware/export/usb_drv.h
+++ b/firmware/export/usb_drv.h
@@ -44,7 +44,7 @@ int usb_drv_port_speed(void);
 void usb_drv_cancel_all_transfers(void);
 void usb_drv_set_test_mode(int mode);
 bool usb_drv_connected(void);
-int usb_drv_request_endpoint(int dir);
+int usb_drv_request_endpoint(int type, int dir);
 void usb_drv_release_endpoint(int ep);
 
 #endif /* _USB_DRV_H */
diff --git a/firmware/ifp_usb_serial.c b/firmware/ifp_usb_serial.c
index e6c8dadfc5..06b286ab85 100644
--- a/firmware/ifp_usb_serial.c
+++ b/firmware/ifp_usb_serial.c
@@ -63,9 +63,6 @@
 
 #define ISP1582_UNLOCK_CODE 0xaa37
 
-#define DIR_RX 0
-#define DIR_TX 1
-
 #define TYPE_BULK 2
 
 #define STATE_DEFAULT 0
diff --git a/firmware/target/arm/usb-drv-arc.c b/firmware/target/arm/usb-drv-arc.c
index 200601eb24..8a10c5fbd6 100644
--- a/firmware/target/arm/usb-drv-arc.c
+++ b/firmware/target/arm/usb-drv-arc.c
@@ -248,10 +248,6 @@
 #define EPCTRL_RX_EP_STALL                     (0x00000001)
 
 /* bit 19-18 and 3-2 are endpoint type */
-#define EPCTRL_EP_TYPE_CONTROL                 (0)
-#define EPCTRL_EP_TYPE_ISO                     (1)
-#define EPCTRL_EP_TYPE_BULK                    (2)
-#define EPCTRL_EP_TYPE_INTERRUPT               (3)
 #define EPCTRL_TX_EP_TYPE_SHIFT                (18)
 #define EPCTRL_RX_EP_TYPE_SHIFT                (2)
 
@@ -312,6 +308,14 @@
    transfer size, so it seems like a good size */
 #define NUM_TDS_PER_EP 4
 
+typedef struct usb_endpoint
+{
+    bool allocated[2];
+    short type[2];
+    short max_pkt_size[2];
+} usb_endpoint_t;
+static usb_endpoint_t endpoints[USB_NUM_ENDPOINTS];
+
 /* manual: 32.13.2 Endpoint Transfer Descriptor (dTD) */
 struct transfer_descriptor {
     unsigned int next_td_ptr;           /* Next TD pointer(31-5), T(0) set
@@ -356,16 +360,12 @@ static const unsigned int pipe2mask[] = {
     0x10, 0x100000,
 };
 
-static char ep_allocation[USB_NUM_ENDPOINTS];
-
 /*-------------------------------------------------------------------------*/
 static void transfer_completed(void);
 static void control_received(void);
-static int prime_transfer(int endpoint, void* ptr,
-                          int len, bool send, bool wait);
+static int prime_transfer(int ep_num, void* ptr, int len, bool send, bool wait);
 static void prepare_td(struct transfer_descriptor* td,
-                       struct transfer_descriptor* previous_td,
-                       void *ptr, int len,int pipe);
+        struct transfer_descriptor* previous_td, void *ptr, int len,int pipe);
 static void bus_reset(void);
 static void init_control_queue_heads(void);
 static void init_bulk_queue_heads(void);
@@ -394,7 +394,7 @@ void usb_drv_reset(void)
     while (REG_USBCMD & USBCMD_CTRL_RESET);
 
 #if CONFIG_CPU == PP5022 || CONFIG_CPU == PP5024
-    /* On a CPU which identifies as a PP5022, this 
+    /* On a CPU which identifies as a PP5022, this
        initialization must be done after USB is reset.
      */
     outl(inl(0x70000060) | 0xF, 0x70000060);
@@ -475,6 +475,27 @@ static void _usb_drv_init(bool attach)
     (void)attach;
 }
 
+#ifdef LOGF_ENABLE
+#define XFER_DIR_STR(dir) ((dir) ? "IN" : "OUT")
+#define XFER_TYPE_STR(type) \
+    ((type) == USB_ENDPOINT_XFER_CONTROL ? "CTRL" : \
+     ((type) == USB_ENDPOINT_XFER_ISOC ? "ISOC" : \
+      ((type) == USB_ENDPOINT_XFER_BULK ? "BULK" : \
+       ((type) == USB_ENDPOINT_XFER_INT ? "INTR" : "INVL"))))
+
+static void log_ep(int ep_num, int ep_dir, char* prefix)
+{
+    usb_endpoint_t* endpoint = &endpoints[ep_num];
+
+    logf("%s: ep%d %s %s %d", prefix, ep_num, XFER_DIR_STR(ep_dir),
+            XFER_TYPE_STR(endpoint->type[ep_dir]),
+            endpoint->max_pkt_size[ep_dir]);
+}
+#else
+#undef log_ep
+#define log_ep(...)
+#endif
+
 void usb_drv_init(void)
 {
     _usb_drv_init(false);
@@ -483,6 +504,7 @@ void usb_drv_init(void)
 /* fully enable driver */
 void usb_drv_attach(void)
 {
+    logf("usb_drv_attach");
     sleep(HZ/10);
     _usb_drv_init(true);
 }
@@ -555,49 +577,51 @@ void usb_drv_int(void)
 bool usb_drv_stalled(int endpoint,bool in)
 {
     if(in) {
-        return ((REG_ENDPTCTRL(endpoint&0x7f) & EPCTRL_TX_EP_STALL)!=0);
+        return ((REG_ENDPTCTRL(EP_NUM(endpoint)) & EPCTRL_TX_EP_STALL)!=0);
     }
     else {
-        return ((REG_ENDPTCTRL(endpoint&0x7f) & EPCTRL_RX_EP_STALL)!=0);
+        return ((REG_ENDPTCTRL(EP_NUM(endpoint)) & EPCTRL_RX_EP_STALL)!=0);
     }
 
 }
-void usb_drv_stall(int endpoint, bool stall,bool in)
+void usb_drv_stall(int endpoint, bool stall, bool in)
 {
-    logf("%sstall %d", stall?"":"un", endpoint&0x7f);
+    int ep_num = EP_NUM(endpoint);
+
+    logf("%sstall %d", stall ? "" : "un", ep_num);
 
     if(in) {
         if (stall) {
-            REG_ENDPTCTRL(endpoint&0x7f) |= EPCTRL_TX_EP_STALL;
+            REG_ENDPTCTRL(ep_num) |= EPCTRL_TX_EP_STALL;
         }
         else {
-            REG_ENDPTCTRL(endpoint&0x7f) &= ~EPCTRL_TX_EP_STALL;
+            REG_ENDPTCTRL(ep_num) &= ~EPCTRL_TX_EP_STALL;
         }
     }
     else {
         if (stall) {
-            REG_ENDPTCTRL(endpoint) |= EPCTRL_RX_EP_STALL;
+            REG_ENDPTCTRL(ep_num) |= EPCTRL_RX_EP_STALL;
         }
         else {
-            REG_ENDPTCTRL(endpoint) &= ~EPCTRL_RX_EP_STALL;
+            REG_ENDPTCTRL(ep_num) &= ~EPCTRL_RX_EP_STALL;
         }
     }
 }
 
 int usb_drv_send_nonblocking(int endpoint, void* ptr, int length)
 {
-    return prime_transfer(endpoint&0x7f, ptr, length, true, false);
+    return prime_transfer(EP_NUM(endpoint), ptr, length, true, false);
 }
 
 int usb_drv_send(int endpoint, void* ptr, int length)
 {
-    return prime_transfer(endpoint&0x7f, ptr, length, true, true);
+    return prime_transfer(EP_NUM(endpoint), ptr, length, true, true);
 }
 
 int usb_drv_recv(int endpoint, void* ptr, int length)
 {
     //logf("usbrecv(%x, %d)", ptr, length);
-    return prime_transfer(endpoint&0x7f, ptr, length, false, false);
+    return prime_transfer(EP_NUM(endpoint), ptr, length, false, false);
 }
 
 int usb_drv_port_speed(void)
@@ -627,7 +651,7 @@ void usb_drv_set_address(int address)
 
 void usb_drv_reset_endpoint(int endpoint, bool send)
 {
-    int pipe = (endpoint&0x7f) * 2 + (send ? 1 : 0);
+    int pipe = EP_NUM(endpoint) * 2 + (send ? 1 : 0);
     unsigned int mask = pipe2mask[pipe];
     REG_ENDPTFLUSH = mask;
     while (REG_ENDPTFLUSH & mask);
@@ -662,23 +686,23 @@ void usb_drv_set_test_mode(int mode)
 /*-------------------------------------------------------------------------*/
 
 /* manual: 32.14.5.2 */
-static int prime_transfer(int endpoint, void* ptr, int len, bool send, bool wait)
+static int prime_transfer(int ep_num, void* ptr, int len, bool send, bool wait)
 {
     int rc = 0;
-    int pipe = endpoint * 2 + (send ? 1 : 0);
+    int pipe = ep_num * 2 + (send ? 1 : 0);
     unsigned int mask = pipe2mask[pipe];
     struct queue_head* qh = &qh_array[pipe];
     static long last_tick;
-    struct transfer_descriptor* new_td,*cur_td,*prev_td;
+    struct transfer_descriptor *new_td, *cur_td, *prev_td;
 
     int oldlevel = disable_irq_save();
 /*
-    if (send && endpoint > EP_CONTROL) {
+    if (send && ep_num > EP_CONTROL) {
         logf("usb: sent %d bytes", len);
     }
 */
     qh->status = 0;
-    qh->wait   = wait;
+    qh->wait = wait;
 
     new_td=&td_array[pipe*NUM_TDS_PER_EP];
     cur_td=new_td;
@@ -694,15 +718,15 @@ static int prime_transfer(int endpoint, void* ptr, int len, bool send, bool wait
         cur_td++;
         len-=tdlen;
     }
-    while(len>0 );
-    //logf("starting ep %d %s",endpoint,send?"send":"receive");
+    while(len>0);
+    //logf("starting ep %d %s",ep_num,send?"send":"receive");
 
     qh->dtd.next_td_ptr = (unsigned int)new_td;
     qh->dtd.size_ioc_sts &= ~(QH_STATUS_HALT | QH_STATUS_ACTIVE);
 
     REG_ENDPTPRIME |= mask;
 
-    if(endpoint == EP_CONTROL && (REG_ENDPTSETUPSTAT & EPSETUP_STATUS_EP0)) {
+    if(ep_num == EP_CONTROL && (REG_ENDPTSETUPSTAT & EPSETUP_STATUS_EP0)) {
         /* 32.14.3.2.2 */
         logf("new setup arrived");
         rc = -4;
@@ -724,11 +748,11 @@ static int prime_transfer(int endpoint, void* ptr, int len, bool send, bool wait
     }
 
     if (!(REG_ENDPTSTATUS & mask)) {
-        logf("no prime! %d %d %x", endpoint, pipe, qh->dtd.size_ioc_sts & 0xff );
+        logf("no prime! %d %d %x", ep_num, pipe, qh->dtd.size_ioc_sts & 0xff);
         rc = -3;
         goto pt_error;
     }
-    if(endpoint == EP_CONTROL && (REG_ENDPTSETUPSTAT & EPSETUP_STATUS_EP0)) {
+    if(ep_num == EP_CONTROL && (REG_ENDPTSETUPSTAT & EPSETUP_STATUS_EP0)) {
         /* 32.14.3.2.2 */
         logf("new setup arrived");
         rc = -4;
@@ -780,17 +804,38 @@ void usb_drv_cancel_all_transfers(void)
     }
 }
 
-int usb_drv_request_endpoint(int dir)
+int usb_drv_request_endpoint(int type, int dir)
 {
-    int i, bit;
+    int ep_num, ep_dir;
+    short ep_type;
+
+    /* Safety */
+    ep_dir = EP_DIR(dir);
+    ep_type = type & USB_ENDPOINT_XFERTYPE_MASK;
 
-    bit=(dir & USB_DIR_IN)? 2:1;
+    logf("req: %s %s", XFER_DIR_STR(ep_dir), XFER_TYPE_STR(ep_type));
 
-    for (i=1; i < USB_NUM_ENDPOINTS; i++) {
-        if((ep_allocation[i] & bit)!=0)
+    /* Find an available ep/dir pair */
+    for (ep_num=1;ep_num<USB_NUM_ENDPOINTS;ep_num++) {
+        usb_endpoint_t* endpoint=&endpoints[ep_num];
+        int other_dir=(ep_dir ? 0:1);
+
+        if (endpoint->allocated[ep_dir])
             continue;
-        ep_allocation[i] |= bit;
-        return i | dir;
+
+        if (endpoint->allocated[other_dir] &&
+                endpoint->type[other_dir] != ep_type) {
+            logf("ep of different type!");
+            return -1;
+        }
+
+        log_ep(ep_num, ep_dir, "add");
+
+        endpoint->allocated[ep_dir] = 1;
+        endpoint->type[ep_dir] = ep_type;
+
+        log_ep(ep_num, ep_dir, "got");
+        return (ep_num | (dir & USB_ENDPOINT_DIR_MASK));
     }
 
     return -1;
@@ -798,8 +843,11 @@ int usb_drv_request_endpoint(int dir)
 
 void usb_drv_release_endpoint(int ep)
 {
-    int mask = (ep & USB_DIR_IN)? ~2:~1;
-    ep_allocation[ep & 0x7f] &= mask;
+    int ep_num = EP_NUM(ep);
+    int ep_dir = EP_DIR(ep);
+
+    log_ep(ep_num, ep_dir, "rel");
+    endpoints[ep_num].allocated[ep_dir] = 0;
 }
 
 
@@ -870,11 +918,12 @@ static void transfer_completed(void)
                 struct transfer_descriptor* td=&td_array[pipe*NUM_TDS_PER_EP];
                 while(td!=(struct transfer_descriptor*)DTD_NEXT_TERMINATE && td!=0)
                 {
-                    length += ((td->reserved & DTD_RESERVED_LENGTH_MASK) - 
+                    length += ((td->reserved & DTD_RESERVED_LENGTH_MASK) -
                         ((td->size_ioc_sts & DTD_PACKET_SIZE) >> DTD_LENGTH_BIT_POS));
                     td=(struct transfer_descriptor*) td->next_td_ptr;
                 }
-                usb_core_transfer_complete(ep, dir?USB_DIR_IN:USB_DIR_OUT, qh->status, length);
+                usb_core_transfer_complete(ep, dir?USB_DIR_IN:USB_DIR_OUT,
+                        qh->status, length);
             }
         }
     }
@@ -906,7 +955,7 @@ static void bus_reset(void)
     }
 
     usb_drv_cancel_all_transfers();
-    
+
     if (!(REG_PORTSC1 & PORTSCX_PORT_RESET)) {
         logf("usb: slow reset!");
     }
@@ -926,39 +975,41 @@ static void init_control_queue_heads(void)
 /* manual: 32.14.4.1 Queue Head Initialization */
 static void init_bulk_queue_heads(void)
 {
-    int tx_packetsize;
-    int rx_packetsize;
+    int packetsize = (usb_drv_port_speed() ? 512 : 64);
     int i;
 
-    if (usb_drv_port_speed()) {
-        rx_packetsize = 512;
-        tx_packetsize = 512;
-    }
-    else {
-        rx_packetsize = 64;
-        tx_packetsize = 64;
-    }
     /* TODO: this should take ep_allocation into account */
-
-    /*** bulk ***/
-    for(i=1;i<USB_NUM_ENDPOINTS;i++) {
-        qh_array[i*2].max_pkt_length = rx_packetsize << QH_MAX_PKT_LEN_POS | QH_ZLT_SEL;
+    for (i=1;i<USB_NUM_ENDPOINTS;i++) {
+        qh_array[i*2].max_pkt_length = packetsize << QH_MAX_PKT_LEN_POS |
+            QH_ZLT_SEL;
         qh_array[i*2].dtd.next_td_ptr = QH_NEXT_TERMINATE;
-        qh_array[i*2+1].max_pkt_length = tx_packetsize << QH_MAX_PKT_LEN_POS | QH_ZLT_SEL;
+        qh_array[i*2+1].max_pkt_length = packetsize << QH_MAX_PKT_LEN_POS |
+            QH_ZLT_SEL;
         qh_array[i*2+1].dtd.next_td_ptr = QH_NEXT_TERMINATE;
     }
 }
 
 static void init_endpoints(void)
 {
-    int i;
-    /* TODO: this should take ep_allocation into account */
-    /* bulk */
-    for(i=1;i<USB_NUM_ENDPOINTS;i++) {
-        REG_ENDPTCTRL(i) =
+    int ep_num;
+
+    logf("init_endpoints");
+    /* RX/TX from the device POV: OUT/IN, respectively */
+    for(ep_num=1;ep_num<USB_NUM_ENDPOINTS;ep_num++) {
+        usb_endpoint_t *endpoint = &endpoints[ep_num];
+
+        /* manual: 32.9.5.18 (Caution) */
+        if (!endpoint->allocated[DIR_OUT]) {
+            endpoint->type[DIR_OUT] = USB_ENDPOINT_XFER_BULK;
+        }
+        if (!endpoint->allocated[DIR_IN]) {
+            endpoint->type[DIR_IN] = USB_ENDPOINT_XFER_BULK;
+        }
+
+        REG_ENDPTCTRL(ep_num) =
             EPCTRL_RX_DATA_TOGGLE_RST | EPCTRL_RX_ENABLE |
             EPCTRL_TX_DATA_TOGGLE_RST | EPCTRL_TX_ENABLE |
-            (EPCTRL_EP_TYPE_BULK << EPCTRL_RX_EP_TYPE_SHIFT) |
-            (EPCTRL_EP_TYPE_BULK << EPCTRL_TX_EP_TYPE_SHIFT);
+            (endpoint->type[DIR_OUT] << EPCTRL_RX_EP_TYPE_SHIFT) |
+            (endpoint->type[DIR_IN] << EPCTRL_TX_EP_TYPE_SHIFT);
     }
 }
diff --git a/firmware/target/arm/usb-tcc.c b/firmware/target/arm/usb-tcc.c
index 611e4be195..2538efd12e 100644
--- a/firmware/target/arm/usb-tcc.c
+++ b/firmware/target/arm/usb-tcc.c
@@ -94,12 +94,15 @@ static struct tcc_ep tcc_endpoints[] = {
 static bool usb_drv_write_ep(struct tcc_ep *ep);
 static void usb_set_speed(int);
 
-int usb_drv_request_endpoint(int dir)
+int usb_drv_request_endpoint(int type, int dir)
 {
     int flags = disable_irq_save();
     size_t ep;
     int ret = 0;
 
+    if (type != USB_ENDPOINT_XFER_BULK)
+        return -1;
+
     if (dir == USB_DIR_IN)
         ep = 1;
     else
diff --git a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
index 14656eed5f..be245ede8c 100644
--- a/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
+++ b/firmware/target/mips/ingenic_jz47xx/usb-jz4740.c
@@ -852,10 +852,13 @@ void usb_drv_release_endpoint(int ep)
     logf("usb_drv_release_endpoint(%d, %s)", (ep & 0x7F), (ep >> 7) ? "IN" : "OUT");
 }
 
-int usb_drv_request_endpoint(int dir)
+int usb_drv_request_endpoint(int type, int dir)
 {
     logf("usb_drv_request_endpoint(%s)", (dir == USB_DIR_IN) ? "IN" : "OUT");
-    
+
+    if (type != USB_ENDPOINT_XFER_BULK)
+        return -1;
+
     /* There are only 3+2 endpoints, so hardcode this ... */
     /* Currently only BULK endpoints ... */
     if(dir == USB_DIR_OUT)
diff --git a/firmware/usb.c b/firmware/usb.c
index 3d94ccfce7..bb00a1be5f 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -273,6 +273,9 @@ static void usb_thread(void)
 #ifdef USB_STORAGE
                     usb_core_enable_driver(USB_DRIVER_MASS_STORAGE, false);
 #endif
+#ifdef USB_HID
+                    usb_core_enable_driver(USB_DRIVER_HID, false);
+#endif
 #ifdef USB_CHARGING_ONLY
                     usb_core_enable_driver(USB_DRIVER_CHARGING_ONLY, true);
 #endif
@@ -291,6 +294,9 @@ static void usb_thread(void)
 #ifdef USB_STORAGE
                 usb_core_enable_driver(USB_DRIVER_MASS_STORAGE, true);
 #endif
+#ifdef USB_HID
+                usb_core_enable_driver(USB_DRIVER_HID, true);
+#endif
 #ifdef USB_CHARGING_ONLY
                 usb_core_enable_driver(USB_DRIVER_CHARGING_ONLY, false);
 #endif
diff --git a/firmware/usbstack/usb_charging_only.c b/firmware/usbstack/usb_charging_only.c
index 3cbc4c3e96..fc6139c1ae 100644
--- a/firmware/usbstack/usb_charging_only.c
+++ b/firmware/usbstack/usb_charging_only.c
@@ -66,7 +66,7 @@ int usb_charging_only_get_config_descriptor(unsigned char *dest,int max_packet_s
     unsigned char *orig_dest = dest;
 
     interface_descriptor.bInterfaceNumber=usb_interface;
-    PACK_DESCRIPTOR(dest, interface_descriptor);
+    PACK_DATA(dest, interface_descriptor);
 
     return (dest-orig_dest);
 }
diff --git a/firmware/usbstack/usb_class_driver.h b/firmware/usbstack/usb_class_driver.h
index 36f2ea9e3d..a9bc759a5b 100644
--- a/firmware/usbstack/usb_class_driver.h
+++ b/firmware/usbstack/usb_class_driver.h
@@ -85,10 +85,10 @@ struct usb_class_driver {
 #endif
 };
 
-#define PACK_DESCRIPTOR(dest, descriptor) \
+#define PACK_DATA(dest, data) \
     do { \
-        memcpy(dest, &(descriptor), sizeof(descriptor)); \
-        dest += sizeof(descriptor); \
+        memcpy(dest, &(data), sizeof(data)); \
+        dest += sizeof(data); \
     } while (0)
 
 #endif
diff --git a/firmware/usbstack/usb_core.c b/firmware/usbstack/usb_core.c
index 1e22ead63b..250544f202 100644
--- a/firmware/usbstack/usb_core.c
+++ b/firmware/usbstack/usb_core.c
@@ -397,7 +397,7 @@ void usb_core_handle_transfer_completion(
                                     (struct usb_ctrlrequest*)event->data);
             break;
         default:
-            handler = ep_data[ep].completion_handler[event->dir>>7];
+            handler = ep_data[ep].completion_handler[EP_DIR(event->dir)];
             if(handler != NULL)
                 handler(ep,event->dir,event->status,event->length);
             break;
@@ -451,17 +451,17 @@ static void usb_core_set_serial_function_id(void)
     usb_string_iSerial.wString[0] = hex[id];
 }
 
-int usb_core_request_endpoint(int dir, struct usb_class_driver* drv)
+int usb_core_request_endpoint(int type, int dir, struct usb_class_driver* drv)
 {
     int ret, ep;
 
-    ret = usb_drv_request_endpoint(dir);
+    ret = usb_drv_request_endpoint(type, dir);
 
     if (ret==-1)
         return -1;
 
-    ep = ret & 0x7f;
-    dir = ret >> 7;
+    dir = EP_DIR(ret);
+    ep = EP_NUM(ret);
 
     ep_data[ep].completion_handler[dir] = drv->transfer_complete;
     ep_data[ep].control_handler[dir] = drv->control_request;
@@ -475,8 +475,8 @@ void usb_core_release_endpoint(int ep)
 
     usb_drv_release_endpoint(ep);
 
-    dir = ep >> 7;
-    ep &= 0x7f;
+    dir = EP_DIR(ep);
+    ep = EP_NUM(ep);
 
     ep_data[ep].completion_handler[dir] = NULL;
     ep_data[ep].control_handler[dir] = NULL;
@@ -512,13 +512,14 @@ static void allocate_interfaces_and_endpoints(void)
 
 static void control_request_handler_drivers(struct usb_ctrlrequest* req)
 {
-    int i;
+    int i, interface = req->wIndex;
     bool handled=false;
+
     for(i=0;i<USB_NUM_DRIVERS;i++) {
         if(drivers[i].enabled &&
                 drivers[i].control_request &&
-                drivers[i].first_interface <= (req->wIndex) &&
-                drivers[i].last_interface > (req->wIndex))
+                drivers[i].first_interface <= interface &&
+                drivers[i].last_interface > interface)
         {
             handled = drivers[i].control_request(req, response_data);
             if(handled)
@@ -528,7 +529,7 @@ static void control_request_handler_drivers(struct usb_ctrlrequest* req)
     if(!handled) {
         /* nope. flag error */
         logf("bad req:desc %d:%d", req->bRequest, req->wValue>>8);
-        usb_drv_stall(EP_CONTROL, true,true);
+        usb_drv_stall(EP_CONTROL, true, true);
         usb_core_ack_control(req);
     }
 }
@@ -730,15 +731,13 @@ static void request_handler_endpoint(struct usb_ctrlrequest* req)
     switch (req->bRequest) {
         case USB_REQ_CLEAR_FEATURE:
             if (req->wValue==USB_ENDPOINT_HALT) {
-                usb_drv_stall(req->wIndex & 0xf, false,
-                        (req->wIndex & USB_DIR_IN)!=0);
+                usb_drv_stall(EP_NUM(req->wIndex), false, EP_DIR(req->wIndex));
             }
             usb_core_ack_control(req);
             break;
         case USB_REQ_SET_FEATURE:
             if (req->wValue==USB_ENDPOINT_HALT) {
-               usb_drv_stall(req->wIndex & 0xf,true,
-                        (req->wIndex & USB_DIR_IN)!=0);
+               usb_drv_stall(EP_NUM(req->wIndex), true, EP_DIR(req->wIndex));
             }
             usb_core_ack_control(req);
             break;
@@ -747,8 +746,8 @@ static void request_handler_endpoint(struct usb_ctrlrequest* req)
             response_data[1]=0;
             logf("usb_core: GET_STATUS");
             if(req->wIndex>0) {
-                response_data[0]=usb_drv_stalled(req->wIndex & 0xf,
-                        (req->wIndex & USB_DIR_IN)!=0);
+                response_data[0]=usb_drv_stalled(EP_NUM(req->wIndex),
+                        EP_DIR(req->wIndex));
             }
             if(!usb_drv_send(EP_CONTROL,response_data,2))
                 usb_core_ack_control(req);
@@ -757,7 +756,8 @@ static void request_handler_endpoint(struct usb_ctrlrequest* req)
                 bool handled;
                 control_handler_t control_handler;
 
-               control_handler=ep_data[req->wIndex & 0xf].control_handler[0];
+               control_handler=
+                   ep_data[EP_NUM(req->wIndex)].control_handler[EP_CONTROL];
                 if (!control_handler)
                     break;
 
@@ -835,9 +835,9 @@ void usb_core_transfer_complete(int endpoint,int dir,int status,int length)
 void usb_core_control_request(struct usb_ctrlrequest* req)
 {
     struct usb_transfer_completion_event_data* completion_event =
-        &ep_data[0].completion_event;
+        &ep_data[EP_CONTROL].completion_event;
 
-    completion_event->endpoint=0;
+    completion_event->endpoint=EP_CONTROL;
     completion_event->dir=0;
     completion_event->data=(void*)req;
     completion_event->status=0;
diff --git a/firmware/usbstack/usb_hid.c b/firmware/usbstack/usb_hid.c
index c3cd5d9a04..0c35da2acb 100644
--- a/firmware/usbstack/usb_hid.c
+++ b/firmware/usbstack/usb_hid.c
@@ -25,49 +25,66 @@
 #include "kernel.h"
 #include "usb_hid.h"
 #include "usb_class_driver.h"
-#define LOGF_ENABLE
+//#define LOGF_ENABLE
 #include "logf.h"
 
 #ifdef USB_HID
 
 #define CONCAT(low, high)               ((high << 8) | low)
-#define SIZE_VALUE                      0x01
+#define PACK_VAL1(dest, val)            *(dest)++ = (val) & 0xff
+#define PACK_VAL2(dest, val)            PACK_VAL1((dest), (val)); \
+                                        PACK_VAL1((dest), (val >> 8))
+
+/* Documents avaiable here: http://www.usb.org/developers/devclass_docs/ */
+
+#define HID_VER                         0x0110 /* 1.1 */
+/* Subclass Codes (HID1_11.pdf, page 18) */
+#define SUBCLASS_BOOT_INTERFACE         1
+/* Protocol Codes (HID1_11.pdf, page 19) */
+#define PROTOCOL_CODE_MOUSE             2
 /* HID main items (HID1_11.pdf, page 38) */
-#define INPUT                           (0x80 | SIZE_VALUE)
-#define OUTPUT                          (0x90 | SIZE_VALUE)
-#define FEATURE                         (0xb0 | SIZE_VALUE)
-#define COLLECTION                      (0xa0 | SIZE_VALUE)
-#define COLLECTION_APPLICATION            CONCAT(COLLECTION, 0x01)
+#define INPUT                           0x80
+#define COLLECTION                      0xa0
+#define COLLECTION_APPLICATION          0x01
 #define END_COLLECTION                  0xc0
+/* Parts (HID1_11.pdf, page 40) */
+#define PREFERERD                       (1 << 5)
+#define NULL_STATE                      (1 << 6)
 /* HID global items (HID1_11.pdf, page 45) */
-#define USAGE_PAGE                      (0x04 | SIZE_VALUE)
-#define LOGICAL_MINIMUM                 (0x14 | SIZE_VALUE)
-#define LOGICAL_MAXIMUM                 (0x24 | SIZE_VALUE)
-#define PHYSICAL_MINIMUM                (0x34 | SIZE_VALUE)
-#define PHYSICAL_MAXIMUM                (0x44 | SIZE_VALUE)
-#define UNIT_EXPONENT                   (0x54 | SIZE_VALUE)
-#define UNIT                            (0x64 | SIZE_VALUE)
-#define REPORT_SIZE                     (0x74 | SIZE_VALUE)
-#define REPORT_ID                       (0x84 | SIZE_VALUE)
-#define REPORT_COUNT                    (0x94 | SIZE_VALUE)
-#define PUSH                            (0xa4 | SIZE_VALUE)
-#define POP                             (0xb4 | SIZE_VALUE)
-/* Hut1_12.pdf, Table 1, page 14 */
-#define USAGE_PAGE_CONSUMER               CONCAT(USAGE_PAGE, 0x0c)
-/* Hut1_12.pdf, Table 17, page 77 */
+#define USAGE_PAGE                      0x04
+#define LOGICAL_MINIMUM                 0x14
+#define LOGICAL_MAXIMUM                 0x24
+#define REPORT_SIZE                     0x74
+#define REPORT_ID                       0x84
+#define REPORT_COUNT                    0x94
+/* HID local items (HID1_11.pdf, page 50) */
+#define USAGE_MINIMUM                   0x18
+#define USAGE_MAXIMUM                   0x28
+/* Types of class descriptors (HID1_11.pdf, page 59) */
+#define USB_DT_HID                      0x21
+#define USB_DT_REPORT                   0x22
+
+/* (Hut1_12.pdf, Table 1, page 14) */
+#define USAGE_PAGE_CONSUMER             0x0c
+
 #define CONSUMER_USAGE                  0x09
-#define CONSUMER_USAGE_CONTROL            CONCAT(CONSUMER_USAGE, 0x01)
-#define CONSUMER_USAGE_MUTE               CONCAT(CONSUMER_USAGE, 0xe2)
-#define CONSUMER_USAGE_VOLUME_INCREMENT   CONCAT(CONSUMER_USAGE, 0xe9)
-#define CONSUMER_USAGE_VOLUME_DECREMENT   CONCAT(CONSUMER_USAGE, 0xea)
-/* Hut1_12.pdf, Table 4, page 20 */
-#define CONSUMER_CONTROL                  CONCAT(COLLECTION_APPLICATION, 0x01)
-
-#define USB_DT_HID                 0x21
-#define USB_DT_REPORT              0x22
-#define USB_DT_PHYSICAL_DESCRIPTOR 0x23
-
-/* serial interface */
+
+/* HID-only class specific requests */
+#define USB_HID_GET_REPORT              0x01
+#define USB_HID_GET_IDLE                0x02
+#define USB_HID_GET_PROTOCOL            0x03
+#define USB_HID_SET_REPORT              0x09
+#define USB_HID_SET_IDLE                0x0a
+#define USB_HID_SET_PROTOCOL            0x0b
+
+#define HID_BUF_SIZE_MSG                16
+#define HID_BUF_SIZE_CMD                5
+#define HID_BUG_SIZE_REPORT             32
+#define HID_NUM_BUFFERS                 5
+
+#define HID_BUF_INC(i) do { (i) = ((i) + 1) % HID_NUM_BUFFERS; } while (0)
+
+/* hid interface */
 static struct usb_interface_descriptor __attribute__((aligned(2)))
                                 interface_descriptor =
 {
@@ -77,12 +94,11 @@ static struct usb_interface_descriptor __attribute__((aligned(2)))
     .bAlternateSetting  = 0,
     .bNumEndpoints      = 1,
     .bInterfaceClass    = USB_CLASS_HID,
-    .bInterfaceSubClass = 0,
-    .bInterfaceProtocol = 0,
+    .bInterfaceSubClass = SUBCLASS_BOOT_INTERFACE,
+    .bInterfaceProtocol = PROTOCOL_CODE_MOUSE,
     .iInterface         = 0
 };
 
-/* USB_DT_HID: Endpoint descriptor */
 struct usb_hid_descriptor {
     uint8_t  bLength;
     uint8_t  bDescriptorType;
@@ -93,19 +109,19 @@ struct usb_hid_descriptor {
     uint16_t wDescriptorLength0;
 } __attribute__ ((packed));
 
-/* USB_DT_REPORT: Endpoint descriptor */
 static struct usb_hid_descriptor __attribute__((aligned(2))) hid_descriptor =
 {
     .bLength            = sizeof(struct usb_hid_descriptor),
     .bDescriptorType    = USB_DT_HID,
-    .wBcdHID            = 0x0100,
+    .wBcdHID            = HID_VER,
     .bCountryCode       = 0,
     .bNumDescriptors    = 1,
-    .bDescriptorType0   = 0x22,
+    .bDescriptorType0   = USB_DT_REPORT,
     .wDescriptorLength0 = 0
 };
 
-static struct usb_endpoint_descriptor __attribute__((aligned(2))) endpoint_descriptor =
+static struct usb_endpoint_descriptor __attribute__((aligned(2)))
+    endpoint_descriptor =
 {
     .bLength          = sizeof(struct usb_endpoint_descriptor),
     .bDescriptorType  = USB_DT_ENDPOINT,
@@ -115,43 +131,46 @@ static struct usb_endpoint_descriptor __attribute__((aligned(2))) endpoint_descr
     .bInterval        = 0
 };
 
-/* USB_DT_REPORT: Endpoint descriptor */
-struct usb_report_descriptor {
-    uint16_t wUsagePage;
-    uint16_t wUsage;
-    uint16_t wCollection;
-    uint16_t wCollectionItems[12];
-    uint8_t wEndCollection;
-} __attribute__ ((packed));
+static unsigned char report_descriptor[HID_BUG_SIZE_REPORT]
+    USB_DEVBSS_ATTR __attribute__((aligned(32)));
 
-static struct usb_report_descriptor __attribute__((aligned(2))) report_descriptor =
-{
-    .wUsagePage        = USAGE_PAGE_CONSUMER,
-    .wUsage            = CONSUMER_USAGE_CONTROL,
-    .wCollection       = COLLECTION_APPLICATION,
-    .wCollectionItems  = {
-        CONCAT(LOGICAL_MINIMUM, 0x0),
-        CONCAT(LOGICAL_MAXIMUM, 0x1),
-        USAGE_PAGE_CONSUMER,
-        CONSUMER_USAGE_MUTE,
-        CONSUMER_USAGE_VOLUME_INCREMENT,
-        CONSUMER_USAGE_VOLUME_DECREMENT,
-        CONCAT(REPORT_COUNT, 0x3),
-        CONCAT(REPORT_SIZE, 0x1),
-        CONCAT(INPUT, 0x42),
-        CONCAT(REPORT_COUNT, 0x5),
-        CONCAT(REPORT_SIZE, 0x1),
-        CONCAT(INPUT, 0x01)
-    },
-    .wEndCollection    = END_COLLECTION
-};
+static unsigned char send_buffer[HID_NUM_BUFFERS][HID_BUF_SIZE_MSG]
+    USB_DEVBSS_ATTR __attribute__((aligned(32)));
+size_t send_buffer_len[HID_NUM_BUFFERS];
+static int cur_buf_prepare;
+static int cur_buf_send;
 
+static uint16_t report_descriptor_len;
+static bool active = false;
 static int ep_in;
 static int usb_interface;
+static uint32_t report_id;
+
+static void usb_hid_try_send_drv(void);
+
+static void pack_parameter(unsigned char **dest, uint8_t parameter,
+        uint32_t value)
+{
+    uint8_t size_value = 1; /* # of bytes */
+    uint32_t v = value;
+
+    while (v >>= 8)
+        size_value++;
+
+    **dest = parameter | size_value;
+    (*dest)++;
+
+    while (size_value--)
+    {
+        **dest = value & 0xff;
+        (*dest)++;
+        value >>= 8;
+    }
+}
 
 int usb_hid_request_endpoints(struct usb_class_driver *drv)
 {
-    ep_in = usb_core_request_endpoint(USB_DIR_IN, drv);
+    ep_in = usb_core_request_endpoint(USB_ENDPOINT_XFER_INT, USB_DIR_IN, drv);
     if (ep_in < 0)
         return -1;
 
@@ -165,25 +184,46 @@ int usb_hid_set_first_interface(int interface)
     return interface + 1;
 }
 
-
 int usb_hid_get_config_descriptor(unsigned char *dest,int max_packet_size)
 {
-    unsigned char *orig_dest = dest;
+    unsigned char *report, *orig_dest = dest;
 
     logf("hid: config desc.");
-    interface_descriptor.bInterfaceNumber = usb_interface;
-    PACK_DESCRIPTOR(dest, interface_descriptor);
-
-    hid_descriptor.wDescriptorLength0 = sizeof(report_descriptor);
-    PACK_DESCRIPTOR(dest, hid_descriptor);
 
-    /* Ignore max_packet_size and set to 1 bytes long packet size */
+    /* Ignore given max_packet_size */
     (void)max_packet_size;
-    endpoint_descriptor.wMaxPacketSize = 1;
+
+    /* TODO: Increment report_id for each report, and send command with
+     * appropriate report id */
+    report_id = 2;
+
+    /* Initialize report descriptor */
+    report = report_descriptor;
+    pack_parameter(&report, USAGE_PAGE, USAGE_PAGE_CONSUMER);
+    PACK_VAL2(report, CONCAT(CONSUMER_USAGE, CONSUMER_CONTROL));
+    pack_parameter(&report, COLLECTION, COLLECTION_APPLICATION);
+    pack_parameter(&report, REPORT_ID, report_id);
+    pack_parameter(&report, REPORT_SIZE, 16);
+    pack_parameter(&report, REPORT_COUNT, 2);
+    pack_parameter(&report, LOGICAL_MINIMUM, 1);
+    pack_parameter(&report, LOGICAL_MAXIMUM, 652);
+    pack_parameter(&report, USAGE_MINIMUM, CONSUMER_CONTROL);
+    pack_parameter(&report, USAGE_MAXIMUM, AC_SEND);
+    pack_parameter(&report, INPUT, PREFERERD | NULL_STATE);
+    PACK_VAL1(report, END_COLLECTION);
+    report_descriptor_len = (uint16_t)((uint32_t)report -
+            (uint32_t)report_descriptor);
+
+    interface_descriptor.bInterfaceNumber = usb_interface;
+    PACK_DATA(dest, interface_descriptor);
+    hid_descriptor.wDescriptorLength0 = report_descriptor_len;
+    PACK_DATA(dest, hid_descriptor);
+
+    endpoint_descriptor.wMaxPacketSize = 8;
     endpoint_descriptor.bInterval = 8;
 
     endpoint_descriptor.bEndpointAddress = ep_in;
-    PACK_DESCRIPTOR(dest, endpoint_descriptor);
+    PACK_DATA(dest, endpoint_descriptor);
 
     return (dest - orig_dest);
 }
@@ -191,38 +231,54 @@ int usb_hid_get_config_descriptor(unsigned char *dest,int max_packet_size)
 void usb_hid_init_connection(void)
 {
     logf("hid: init connection");
+
+    active = true;
 }
 
-/* called by usb_code_init() */
+/* called by usb_core_init() */
 void usb_hid_init(void)
 {
+    int i;
+
     logf("hid: init");
+
+    for (i = 0; i < HID_NUM_BUFFERS; i++)
+        send_buffer_len[i] = 0;
+
+    cur_buf_prepare = 0;
+    cur_buf_send = 0;
+
+    active = true;
 }
 
 void usb_hid_disconnect(void)
 {
     logf("hid: disconnect");
+    active = false;
 }
 
 /* called by usb_core_transfer_complete() */
-void usb_hid_transfer_complete(int ep,int dir, int status, int length)
+void usb_hid_transfer_complete(int ep, int dir, int status, int length)
 {
     (void)ep;
     (void)dir;
     (void)status;
     (void)length;
 
-    logf("hid: transfer complete. ep %d, dir %d, status %d ,length %d",
-        ep, dir, status, length);
-}
+    switch (dir) {
+        case USB_DIR_OUT:
+            break;
+        case USB_DIR_IN: {
+            if (status)
+                break;
 
-/* HID-only class specific requests */
-#define USB_HID_GET_REPORT   0x01
-#define USB_HID_GET_IDLE     0x02
-#define USB_HID_GET_PROTOCOL 0x03
-#define USB_HID_SET_REPORT   0x09
-#define USB_HID_SET_IDLE     0x0a
-#define USB_HID_SET_PROTOCOL 0x0b
+            send_buffer_len[cur_buf_send] = 0;
+            HID_BUF_INC(cur_buf_send);
+            usb_hid_try_send_drv();
+            break;
+        }
+    }
+}
 
 /* called by usb_core_control_request() */
 bool usb_hid_control_request(struct usb_ctrlrequest* req, unsigned char* dest)
@@ -231,29 +287,32 @@ bool usb_hid_control_request(struct usb_ctrlrequest* req, unsigned char* dest)
 
     switch(req->bRequestType & USB_TYPE_MASK) {
         case USB_TYPE_STANDARD: {
-                switch(req->wValue>>8) { /* type */
-                    case USB_DT_REPORT: {
-                        logf("hid: report");
-                        if (dest == NULL) {
-                            logf("dest is NULL!");
-                        }
-                        if (dest) {
-                            unsigned char *orig_dest = dest;
-                            PACK_DESCRIPTOR(dest, report_descriptor);
-                            if(usb_drv_send(EP_CONTROL, orig_dest, dest - orig_dest))
-                                break;
-                            usb_core_ack_control(req);
-
-                        }
-                        handled = true;
-                        break;
-                    }
-                    default:
-                        logf("hid: unsup. std. req");
-                        break;
+            unsigned char *orig_dest = dest;
+
+            switch(req->wValue>>8) { /* type */
+                case USB_DT_HID: {
+                    logf("hid: type hid");
+                    hid_descriptor.wDescriptorLength0 = report_descriptor_len;
+                    PACK_DATA(dest, hid_descriptor);
+                    break;
                 }
-                break;
+                case USB_DT_REPORT: {
+                    logf("hid: type report");
+                    memcpy(dest, &report_descriptor, report_descriptor_len);
+                    dest += report_descriptor_len;
+                    break;
+                }
+                default:
+                    logf("hid: unsup. std. req");
+                    break;
             }
+            if (dest != orig_dest &&
+                    !usb_drv_send(EP_CONTROL, orig_dest, dest - orig_dest)) {
+                usb_core_ack_control(req);
+                handled = true;
+            }
+            break;
+        }
 
         case USB_TYPE_CLASS: {
                 switch (req->bRequest) {
@@ -263,7 +322,6 @@ bool usb_hid_control_request(struct usb_ctrlrequest* req, unsigned char* dest)
                         handled = true;
                         break;
                     default:
-                        //logf("hid: unsup. cls. req");
                         logf("%d: unsup. cls. req", req->bRequest);
                         break;
                 }
@@ -277,12 +335,62 @@ bool usb_hid_control_request(struct usb_ctrlrequest* req, unsigned char* dest)
     return handled;
 }
 
-void usb_hid_send(unsigned char *data, int length)
+static void usb_hid_try_send_drv(void)
 {
-    (void)data;
-    (void)(length);
+    int rc;
+    int length = send_buffer_len[cur_buf_send];
+
+    if (!length)
+        return;
+
+    rc = usb_drv_send_nonblocking(ep_in, send_buffer[cur_buf_send], length);
+    if (rc)
+    {
+        send_buffer_len[cur_buf_send] = 0;
+        return;
+    }
+}
+
+static void usb_hid_queue(unsigned char *data, int length)
+{
+    if (!active || length <= 0)
+        return;
+
+    /* Buffer overflow - item still in use */
+    if (send_buffer_len[cur_buf_prepare])
+        return;
+
+    /* Prepare buffer for sending */
+    if (length > HID_BUF_SIZE_MSG)
+        length = HID_BUF_SIZE_MSG;
+    memcpy(send_buffer[cur_buf_prepare], data, length);
+    send_buffer_len[cur_buf_prepare] = length;
+
+    HID_BUF_INC(cur_buf_prepare);
+}
+
+void usb_hid_send_consumer_usage(consumer_usage_page_t id)
+{
+    static unsigned char buf[HID_BUF_SIZE_CMD] USB_DEVBSS_ATTR
+        __attribute__((aligned(32)));
+    unsigned char *dest = buf;
+
+    memset(buf, 0, sizeof(buf));
+
+    logf("HID: Sending 0x%x", id);
+
+    pack_parameter(&dest, 0, id);
+    buf[0] = report_id;
+
+   /* Key pressed */
+    usb_hid_queue(buf, HID_BUF_SIZE_CMD);
+
+    /* Key released */
+    memset(buf, 0, sizeof(buf));
+    buf[0] = report_id;
+    usb_hid_queue(buf, HID_BUF_SIZE_CMD);
 
-    logf("hid: send %d bytes: \"%s\"", length, data);
+    usb_hid_try_send_drv();
 }
 
 #endif /*USB_HID*/
diff --git a/firmware/usbstack/usb_hid.h b/firmware/usbstack/usb_hid.h
index ff2a33dcbf..bce6943a4d 100644
--- a/firmware/usbstack/usb_hid.h
+++ b/firmware/usbstack/usb_hid.h
@@ -22,6 +22,7 @@
 #define USB_HID_H
 
 #include "usb_ch9.h"
+#include "usb_hid_usage_tables.h"
 
 int usb_hid_request_endpoints(struct usb_class_driver *drv);
 int usb_hid_set_first_interface(int interface);
@@ -32,7 +33,7 @@ void usb_hid_disconnect(void);
 void usb_hid_transfer_complete(int ep, int dir, int status, int length);
 bool usb_hid_control_request(struct usb_ctrlrequest* req, unsigned char* dest);
 
-void usb_hid_send(unsigned char *data,int length);
+void usb_hid_send_consumer_usage(consumer_usage_page_t id);
 
 #endif
 
diff --git a/firmware/usbstack/usb_serial.c b/firmware/usbstack/usb_serial.c
index 370afcb7f1..246938a1ef 100644
--- a/firmware/usbstack/usb_serial.c
+++ b/firmware/usbstack/usb_serial.c
@@ -77,11 +77,12 @@ static int usb_interface;
 
 int usb_serial_request_endpoints(struct usb_class_driver *drv)
 {
-    ep_in = usb_core_request_endpoint(USB_DIR_IN, drv);
+    ep_in = usb_core_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_IN, drv);
     if (ep_in < 0)
         return -1;
 
-    ep_out = usb_core_request_endpoint(USB_DIR_OUT, drv);
+    ep_out = usb_core_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_OUT,
+            drv);
     if (ep_out < 0) {
         usb_core_release_endpoint(ep_in);
         return -1;
@@ -101,15 +102,15 @@ int usb_serial_get_config_descriptor(unsigned char *dest, int max_packet_size)
     unsigned char *orig_dest = dest;
 
     interface_descriptor.bInterfaceNumber = usb_interface;
-    PACK_DESCRIPTOR(dest, interface_descriptor);
+    PACK_DATA(dest, interface_descriptor);
 
     endpoint_descriptor.wMaxPacketSize = max_packet_size;
 
     endpoint_descriptor.bEndpointAddress = ep_in;
-    PACK_DESCRIPTOR(dest, endpoint_descriptor);
+    PACK_DATA(dest, endpoint_descriptor);
 
     endpoint_descriptor.bEndpointAddress = ep_out;
-    PACK_DESCRIPTOR(dest, endpoint_descriptor);
+    PACK_DATA(dest, endpoint_descriptor);
 
     return (dest - orig_dest);
 }
diff --git a/firmware/usbstack/usb_storage.c b/firmware/usbstack/usb_storage.c
index 464283551a..73176ee93e 100644
--- a/firmware/usbstack/usb_storage.c
+++ b/firmware/usbstack/usb_storage.c
@@ -349,12 +349,13 @@ void usb_storage_init(void)
 
 int usb_storage_request_endpoints(struct usb_class_driver *drv)
 {
-    ep_in = usb_core_request_endpoint(USB_DIR_IN, drv);
+    ep_in = usb_core_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_IN, drv);
 
     if(ep_in<0)
         return -1;
 
-    ep_out = usb_core_request_endpoint(USB_DIR_OUT, drv);
+    ep_out = usb_core_request_endpoint(USB_ENDPOINT_XFER_BULK, USB_DIR_OUT,
+            drv);
 
     if(ep_out<0) {
         usb_core_release_endpoint(ep_in);
@@ -375,15 +376,15 @@ int usb_storage_get_config_descriptor(unsigned char *dest,int max_packet_size)
     unsigned char *orig_dest = dest;
 
     interface_descriptor.bInterfaceNumber = usb_interface;
-    PACK_DESCRIPTOR(dest, interface_descriptor);
+    PACK_DATA(dest, interface_descriptor);
 
     endpoint_descriptor.wMaxPacketSize = max_packet_size;
 
     endpoint_descriptor.bEndpointAddress = ep_in;
-    PACK_DESCRIPTOR(dest, endpoint_descriptor);
+    PACK_DATA(dest, endpoint_descriptor);
 
     endpoint_descriptor.bEndpointAddress = ep_out;
-    PACK_DESCRIPTOR(dest, endpoint_descriptor);
+    PACK_DATA(dest, endpoint_descriptor);
 
     return (dest - orig_dest);
 }