4 files changed, 71 insertions, 17 deletions

diff --git a/firmware/export/usb.h b/firmware/export/usb.h
index 8954e9cc59..13ef3983b2 100644
--- a/firmware/export/usb.h
+++ b/firmware/export/usb.h
@@ -44,6 +44,8 @@ enum
 #endif
 #ifdef HAVE_USBSTACK
     USB_TRANSFER_COMPLETION, /* Event */
+    USB_NOTIFY_SET_ADDR,     /* Event */
+    USB_NOTIFY_SET_CONFIG,   /* Event */
 #endif
 #ifdef USB_FIREWIRE_HANDLING
     USB_REQUEST_REBOOT,      /* Event */
@@ -130,6 +132,7 @@ void usb_charger_update(void);
 #ifdef HAVE_USBSTACK
 void usb_signal_transfer_completion(
     struct usb_transfer_completion_event_data *event_data);
+void usb_signal_notify(long id, intptr_t data);
 bool usb_driver_enabled(int driver);
 bool usb_exclusive_storage(void); /* storage is available for usb */
 #endif
diff --git a/firmware/export/usb_core.h b/firmware/export/usb_core.h
index 6405aa492d..9e3d0d7916 100644
--- a/firmware/export/usb_core.h
+++ b/firmware/export/usb_core.h
@@ -51,6 +51,10 @@ void usb_core_enable_driver(int driver,bool enabled);
 bool usb_core_driver_enabled(int driver);
 void usb_core_handle_transfer_completion(
         struct usb_transfer_completion_event_data* event);
+void usb_core_handle_notify(long id, intptr_t data);
+/* For controllers which handle SET ADDR and/or SET CONFIG in hardware */
+void usb_core_notify_set_address(uint8_t addr);
+void usb_core_notify_set_config(uint8_t config);
 
 int usb_core_request_endpoint(int type, int dir,struct usb_class_driver* drv);
 void usb_core_release_endpoint(int dir);
diff --git a/firmware/usb.c b/firmware/usb.c
index 6823851e08..b8c9822ff6 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -270,6 +270,11 @@ void usb_signal_transfer_completion(
     queue_post(&usb_queue, USB_TRANSFER_COMPLETION, (intptr_t)event_data);
 }
 
+void usb_signal_notify(long id, intptr_t data)
+{
+    queue_post(&usb_queue, id, data);
+}
+
 #else  /* !HAVE_USBSTACK */
 
 static inline void usb_stack_enable(bool enable)
@@ -431,6 +436,12 @@ static void NORETURN_ATTR usb_thread(void)
         /*** Main USB thread duties ***/
 
 #ifdef HAVE_USBSTACK
+        case USB_NOTIFY_SET_ADDR:
+        case USB_NOTIFY_SET_CONFIG:
+            if(usb_state <= USB_EXTRACTED)
+                break;
+            usb_core_handle_notify(ev.id, ev.data);
+            break;
         case USB_TRANSFER_COMPLETION:
             if(usb_state <= USB_EXTRACTED)
                 break;
diff --git a/firmware/usbstack/usb_core.c b/firmware/usbstack/usb_core.c
index 801325b692..2a3cc1c525 100644
--- a/firmware/usbstack/usb_core.c
+++ b/firmware/usbstack/usb_core.c
@@ -663,10 +663,31 @@ static void request_handler_device_get_descriptor(struct usb_ctrlrequest* req)
     }
 }
 
-static void request_handler_device(struct usb_ctrlrequest* req)
+static void usb_core_do_set_addr(uint8_t address)
 {
-    int i;
+    logf("usb_core: SET_ADR %d", address);
+    usb_address = address;
+    usb_state = ADDRESS;
+}
+
+static void usb_core_do_set_config(uint8_t config)
+{
+    logf("usb_core: SET_CONFIG");
+    if(config) {
+        usb_state = CONFIGURED;
+        for(int i = 0; i < USB_NUM_DRIVERS; i++)
+            if(drivers[i].enabled && drivers[i].init_connection)
+                drivers[i].init_connection();
+    }
+    else
+        usb_state = ADDRESS;
+    #ifdef HAVE_USB_CHARGING_ENABLE
+    usb_charging_maxcurrent_change(usb_charging_maxcurrent());
+    #endif
+}
 
+static void request_handler_device(struct usb_ctrlrequest* req)
+{
     switch(req->bRequest) {
         case USB_REQ_GET_CONFIGURATION: {
                 logf("usb_core: GET_CONFIG");
@@ -676,20 +697,9 @@ static void request_handler_device(struct usb_ctrlrequest* req)
                 break;
             }
         case USB_REQ_SET_CONFIGURATION: {
-                logf("usb_core: SET_CONFIG");
                 usb_drv_cancel_all_transfers();
-                if(req->wValue) {
-                    usb_state = CONFIGURED;
-                    for(i = 0; i < USB_NUM_DRIVERS; i++)
-                        if(drivers[i].enabled && drivers[i].init_connection)
-                            drivers[i].init_connection();
-                }
-                else
-                    usb_state = ADDRESS;
+                usb_core_do_set_config(req->wValue);
                 usb_drv_send(EP_CONTROL, NULL, 0);
-#ifdef HAVE_USB_CHARGING_ENABLE
-                usb_charging_maxcurrent_change(usb_charging_maxcurrent());
-#endif
                 break;
             }
         case USB_REQ_SET_ADDRESS: {
@@ -697,9 +707,8 @@ static void request_handler_device(struct usb_ctrlrequest* req)
                 logf("usb_core: SET_ADR %d", address);
                 usb_drv_send(EP_CONTROL, NULL, 0);
                 usb_drv_cancel_all_transfers();
-                usb_address = address;
-                usb_drv_set_address(usb_address);
-                usb_state = ADDRESS;
+                usb_drv_set_address(address);
+                usb_core_do_set_addr(address);
                 break;
             }
         case USB_REQ_GET_DESCRIPTOR:
@@ -913,6 +922,21 @@ void usb_core_transfer_complete(int endpoint, int dir, int status, int length)
     }
 }
 
+void usb_core_handle_notify(long id, intptr_t data)
+{
+    switch(id)
+    {
+        case USB_NOTIFY_SET_ADDR:
+            usb_core_do_set_addr(data);
+            break;
+        case USB_NOTIFY_SET_CONFIG:
+            usb_core_do_set_config(data);
+            break;
+        default:
+            break;
+    }
+}
+
 /* called by usb_drv_int() */
 void usb_core_control_request(struct usb_ctrlrequest* req)
 {
@@ -928,6 +952,18 @@ void usb_core_control_request(struct usb_ctrlrequest* req)
     usb_signal_transfer_completion(completion_event);
 }
 
+void usb_core_notify_set_address(uint8_t addr)
+{
+    logf("notify set addr received %ld", current_tick);
+    usb_signal_notify(USB_NOTIFY_SET_ADDR, addr);
+}
+
+void usb_core_notify_set_config(uint8_t config)
+{
+    logf("notify set config received %ld", current_tick);
+    usb_signal_notify(USB_NOTIFY_SET_CONFIG, config);
+}
+
 #ifdef HAVE_USB_CHARGING_ENABLE
 void usb_charging_enable(int state)
 {