4 files changed, 56 insertions, 7 deletions

diff --git a/firmware/export/config.h b/firmware/export/config.h
index b73f605d9d..e64ca4127f 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -871,6 +871,13 @@ Lyre prototype 1 */
 #endif
 #endif /* HAVE_HEADPHONE_DETECTION */
 
+#if defined(HAVE_USB_CHARGING_ENABLE) && defined(HAVE_USBSTACK)
+/* USB charging support in the USB stack requires timeout objects */
+#ifndef INCLUDE_TIMEOUT_API
+#define INCLUDE_TIMEOUT_API
+#endif
+#endif /* HAVE_USB_CHARGING_ENABLE && HAVE_USBSTACK */
+
 #if defined(HAVE_USBSTACK) || (CONFIG_STORAGE & STORAGE_NAND)
 #define STORAGE_GET_INFO
 #endif
diff --git a/firmware/export/usb.h b/firmware/export/usb.h
index d544f5cab1..55b5f2cffb 100644
--- a/firmware/export/usb.h
+++ b/firmware/export/usb.h
@@ -52,6 +52,9 @@ enum {
     USB_REQUEST_REBOOT,      /* Event */
 #endif
     USB_QUIT,                /* Event */
+#if defined(HAVE_USB_CHARGING_ENABLE) && defined(HAVE_USBSTACK)
+    USB_CHARGER_UPDATE,      /* Event */
+#endif
 };
 
 #ifdef HAVE_USB_POWER
@@ -156,8 +159,11 @@ enum {
  * or target-specific code on others
  */
 void usb_charging_enable(int state);
-#endif
-#endif
+#ifdef HAVE_USBSTACK
+void usb_charger_update(void);
+#endif /* HAVE_USBSTACK */
+#endif /* HAVE_USB_CHARGING_ENABLE */
+#endif /* HAVE_USB_POWER */
 #ifdef HAVE_USBSTACK
 void usb_signal_transfer_completion(
     struct usb_transfer_completion_event_data *event_data);
diff --git a/firmware/usb.c b/firmware/usb.c
index c615e97d2b..31bf93e551 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -432,10 +432,23 @@ static void usb_thread(void)
                     try_reboot();
                 break;
 #endif /* USB_FIREWIRE_HANDLING */
+
+#if defined(HAVE_USB_CHARGING_ENABLE) && defined(HAVE_USBSTACK)
+            case USB_CHARGER_UPDATE:
+                usb_charging_maxcurrent_change(usb_charging_maxcurrent());
+                break;
+#endif
         }
     }
 }
 
+#if defined(HAVE_USB_CHARGING_ENABLE) && defined(HAVE_USBSTACK)
+void usb_charger_update(void)
+{
+    queue_post(&usb_queue, USB_CHARGER_UPDATE, 0);
+}
+#endif
+
 #ifdef USB_STATUS_BY_EVENT
 void usb_status_event(int current_status)
 {
diff --git a/firmware/usbstack/usb_core.c b/firmware/usbstack/usb_core.c
index dfcce05064..d6c8c6e5cc 100644
--- a/firmware/usbstack/usb_core.c
+++ b/firmware/usbstack/usb_core.c
@@ -169,6 +169,16 @@ static enum { DEFAULT, ADDRESS, CONFIGURED } usb_state;
 #ifdef HAVE_USB_CHARGING_ENABLE
 static int usb_charging_mode = USB_CHARGING_DISABLE;
 static int usb_charging_current_requested = 500;
+static struct timeout usb_no_host_timeout;
+static bool usb_no_host = false;
+
+static int usb_no_host_callback(struct timeout *tmo)
+{
+    (void)tmo;
+    usb_no_host = true;
+    usb_charger_update();
+    return 0;
+}
 #endif
 
 static int usb_core_num_interfaces;
@@ -365,6 +375,10 @@ void usb_core_init(void)
 
     initialized = true;
     usb_state = DEFAULT;
+#ifdef HAVE_USB_CHARGING_ENABLE
+    usb_no_host = false;
+    timeout_register(&usb_no_host_timeout, usb_no_host_callback, HZ*10, 0);
+#endif
     logf("usb_core_init() finished");
 }
 
@@ -384,6 +398,7 @@ void usb_core_exit(void)
     }
     usb_state = DEFAULT;
 #ifdef HAVE_USB_CHARGING_ENABLE
+    usb_no_host = false;
     usb_charging_maxcurrent_change(usb_charging_maxcurrent());
 #endif
     logf("usb_core_exit() finished");
@@ -800,6 +815,13 @@ static void request_handler_endpoint(struct usb_ctrlrequest* req)
 /* Handling USB requests starts here */
 static void usb_core_control_request_handler(struct usb_ctrlrequest* req)
 {
+#ifdef HAVE_USB_CHARGING_ENABLE
+    timeout_cancel(&usb_no_host_timeout);
+    if(usb_no_host) {
+        usb_no_host = false;
+        usb_charging_maxcurrent_change(usb_charging_maxcurrent());
+    }
+#endif
     if(usb_state == DEFAULT) {
         set_serial_descriptor();
         usb_core_set_serial_function_id();
@@ -882,11 +904,12 @@ void usb_charging_enable(int state)
 
 int usb_charging_maxcurrent()
 {
-    if (!initialized
-            || usb_charging_mode == USB_CHARGING_DISABLE
-            || usb_state != CONFIGURED)
+    if (!initialized || usb_charging_mode == USB_CHARGING_DISABLE)
         return 100;
-    /* usb_state == CONFIGURED, charging enabled/forced */
-    return usb_charging_current_requested;
+    if (usb_state == CONFIGURED)
+        return usb_charging_current_requested;
+    if (usb_charging_mode == USB_CHARGING_FORCE && usb_no_host)
+        return 500;
+    return 100;
 }
 #endif