2 files changed, 27 insertions, 16 deletions

diff --git a/tmk_core/protocol/chibios/usb_driver.c b/tmk_core/protocol/chibios/usb_driver.c
index 40bfb8eb2b..cc0ce7600f 100644
--- a/tmk_core/protocol/chibios/usb_driver.c
+++ b/tmk_core/protocol/chibios/usb_driver.c
@@ -80,19 +80,7 @@ static bool qmkusb_start_receive(QMKUSBDriver *qmkusbp) {
  * Interface implementation.
  */
 
-static size_t _write(void *ip, const uint8_t *bp, size_t n) {
-    output_buffers_queue_t *obqueue = &((QMKUSBDriver *)ip)->obqueue;
-    chSysLock();
-    const bool full = obqIsFullI(obqueue);
-    chSysUnlock();
-    if (full || bqIsSuspendedX(obqueue)) {
-        /* Discard any writes while the queue is suspended or full, i.e. the hidraw
-           interface is not open. If we tried to send with an infinite timeout, we
-           would deadlock the keyboard otherwise. */
-        return -1;
-    }
-    return obqWriteTimeout(obqueue, bp, n, TIME_INFINITE);
-}
+static size_t _write(void *ip, const uint8_t *bp, size_t n) { return obqWriteTimeout(&((QMKUSBDriver *)ip)->obqueue, bp, n, TIME_INFINITE); }
 
 static size_t _read(void *ip, uint8_t *bp, size_t n) { return ibqReadTimeout(&((QMKUSBDriver *)ip)->ibqueue, bp, n, TIME_INFINITE); }
 
diff --git a/tmk_core/protocol/chibios/usb_main.c b/tmk_core/protocol/chibios/usb_main.c
index 8adecfa719..0703cdc718 100644
--- a/tmk_core/protocol/chibios/usb_main.c
+++ b/tmk_core/protocol/chibios/usb_main.c
@@ -930,9 +930,32 @@ void send_consumer(uint16_t data) {
 #ifdef CONSOLE_ENABLE
 
 int8_t sendchar(uint8_t c) {
-    // The previous implmentation had timeouts, but I think it's better to just slow down
-    // and make sure that everything is transferred, rather than dropping stuff
-    return chnWrite(&drivers.console_driver.driver, &c, 1);
+    static bool timed_out = false;
+    /* The `timed_out` state is an approximation of the ideal `is_listener_disconnected?` state.
+     *
+     * When a 5ms timeout write has timed out, hid_listen is most likely not running, or not
+     * listening to this keyboard, so we go into the timed_out state. In this state we assume
+     * that hid_listen is most likely not gonna be connected to us any time soon, so it would
+     * be wasteful to write follow-up characters with a 5ms timeout, it would all add up and
+     * unncecessarily slow down the firmware. However instead of just dropping the characters,
+     * we write them with a TIME_IMMEDIATE timeout, which is a zero timeout,
+     * and this will succeed only if hid_listen gets connected again. When a write with
+     * TIME_IMMEDIATE timeout succeeds, we know that hid_listen is listening to us again, and
+     * we can go back to the timed_out = false state, and following writes will be executed
+     * with a 5ms timeout. The reason we don't just send all characters with the TIME_IMMEDIATE
+     * timeout is that this could cause bytes to be lost even if hid_listen is running, if there
+     * is a lot of data being sent over the console.
+     *
+     * This logic will work correctly as long as hid_listen is able to receive at least 200
+     * bytes per second. On a heavily overloaded machine that's so overloaded that it's
+     * unusable, and constantly swapping, hid_listen might have trouble receiving 200 bytes per
+     * second, so some bytes might be lost on the console.
+     */
+
+    const sysinterval_t timeout = timed_out ? TIME_IMMEDIATE : TIME_MS2I(5);
+    const size_t result = chnWriteTimeout(&drivers.console_driver.driver, &c, 1, timeout);
+    timed_out = (result == 0);
+    return result;
 }
 
 // Just a dummy function for now, this could be exposed as a weak function