1 files changed, 14 insertions, 7 deletions

diff --git a/tmk_core/protocol/chibios/usb_main.c b/tmk_core/protocol/chibios/usb_main.c
index e3a8489011..e05c4c5055 100644
--- a/tmk_core/protocol/chibios/usb_main.c
+++ b/tmk_core/protocol/chibios/usb_main.c
@@ -668,10 +668,8 @@ uint8_t keyboard_leds(void) { return keyboard_led_stats; }
 void send_keyboard(report_keyboard_t *report) {
     osalSysLock();
     if (usbGetDriverStateI(&USB_DRIVER) != USB_ACTIVE) {
-        osalSysUnlock();
-        return;
+        goto unlock;
     }
-    osalSysUnlock();
 
 #ifdef NKRO_ENABLE
     if (keymap_config.nkro && keyboard_protocol) { /* NKRO protocol */
@@ -680,28 +678,35 @@ void send_keyboard(report_keyboard_t *report) {
          * until *after* the packet has been transmitted. I think
          * this is more efficient */
         /* busy wait, should be short and not very common */
-        osalSysLock();
         if (usbGetTransmitStatusI(&USB_DRIVER, SHARED_IN_EPNUM)) {
             /* Need to either suspend, or loop and call unlock/lock during
              * every iteration - otherwise the system will remain locked,
              * no interrupts served, so USB not going through as well.
              * Note: for suspend, need USB_USE_WAIT == TRUE in halconf.h */
             osalThreadSuspendS(&(&USB_DRIVER)->epc[SHARED_IN_EPNUM]->in_state->thread);
+
+            /* after osalThreadSuspendS returns USB status might have changed */
+            if (usbGetDriverStateI(&USB_DRIVER) != USB_ACTIVE) {
+                goto unlock;
+            }
         }
         usbStartTransmitI(&USB_DRIVER, SHARED_IN_EPNUM, (uint8_t *)report, sizeof(struct nkro_report));
-        osalSysUnlock();
     } else
 #endif /* NKRO_ENABLE */
     {  /* regular protocol */
         /* need to wait until the previous packet has made it through */
         /* busy wait, should be short and not very common */
-        osalSysLock();
         if (usbGetTransmitStatusI(&USB_DRIVER, KEYBOARD_IN_EPNUM)) {
             /* Need to either suspend, or loop and call unlock/lock during
              * every iteration - otherwise the system will remain locked,
              * no interrupts served, so USB not going through as well.
              * Note: for suspend, need USB_USE_WAIT == TRUE in halconf.h */
             osalThreadSuspendS(&(&USB_DRIVER)->epc[KEYBOARD_IN_EPNUM]->in_state->thread);
+
+            /* after osalThreadSuspendS returns USB status might have changed */
+            if (usbGetDriverStateI(&USB_DRIVER) != USB_ACTIVE) {
+                goto unlock;
+            }
         }
         uint8_t *data, size;
         if (keyboard_protocol) {
@@ -712,9 +717,11 @@ void send_keyboard(report_keyboard_t *report) {
             size = 8;
         }
         usbStartTransmitI(&USB_DRIVER, KEYBOARD_IN_EPNUM, data, size);
-        osalSysUnlock();
     }
     keyboard_report_sent = *report;
+
+unlock:
+    osalSysUnlock();
 }
 
 /* ---------------------------------------------------------