7 files changed, 265 insertions, 154 deletions

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);
 }