diff --git a/grblHAL_Teensy4/src/eeprom b/grblHAL_Teensy4/src/eeprom
index 292df8b..4b6eb7b 160000
--- a/grblHAL_Teensy4/src/eeprom
+++ b/grblHAL_Teensy4/src/eeprom
@@ -1 +1 @@
-Subproject commit 292df8b7cc2a7d9df7a647483eef6ba8c68c250d
+Subproject commit 4b6eb7b971db033136af2835e3949d2ff6b32da5
diff --git a/grblHAL_Teensy4/src/encoder b/grblHAL_Teensy4/src/encoder
index 8ee8eea..5261a60 160000
--- a/grblHAL_Teensy4/src/encoder
+++ b/grblHAL_Teensy4/src/encoder
@@ -1 +1 @@
-Subproject commit 8ee8eea4e4974ae54787b01aa4a17fa744580246
+Subproject commit 5261a606d1a653d5f571a8f6d1a05eb78c066707
diff --git a/grblHAL_Teensy4/src/grbl b/grblHAL_Teensy4/src/grbl
index 7dab08e..60d8933 160000
--- a/grblHAL_Teensy4/src/grbl
+++ b/grblHAL_Teensy4/src/grbl
@@ -1 +1 @@
-Subproject commit 7dab08e2947afccd2c5431c91235f8d36d313310
+Subproject commit 60d89338fd81a95be030eee127ba0df5c83b9a88
diff --git a/grblHAL_Teensy4/src/i2c.c b/grblHAL_Teensy4/src/i2c.c
index 900d28c..0cd831e 100644
--- a/grblHAL_Teensy4/src/i2c.c
+++ b/grblHAL_Teensy4/src/i2c.c
@@ -94,7 +94,7 @@ static const i2c_hardware_t i2c4_hardware = {
     }
 };
 
-static bool force_clock (i2c_hardware_t *hardware)
+static bool force_clock (const i2c_hardware_t *hardware)
 {
     bool ret = false;
     uint32_t sda_pin = hardware->sda_pin.pin;
@@ -127,7 +127,7 @@ static bool force_clock (i2c_hardware_t *hardware)
     return ret;
 }
 
-static void setClock (IMXRT_LPI2C_t *port, uint32_t frequency)
+static void set_clock (IMXRT_LPI2C_t *port, uint32_t frequency)
 {
     port->MCR = 0;
 
@@ -219,7 +219,7 @@ void i2c_init (void)
         *hardware->clock_gate_register |= hardware->clock_gate_mask;
         port->MCR = LPI2C_MCR_RST;
 
-        setClock(port, 100000);
+        set_clock(port, 100000);
 
         // Setup SDA register
         *(portControlRegister(hardware->sda_pin.pin)) = PINCONFIG;
@@ -238,6 +238,22 @@ void i2c_init (void)
     }
 }
 
+// wait until ready for transfer, try peripheral reset if bus hangs
+inline static bool wait_ready (void)
+{
+    while(i2cIsBusy) {
+        if(port->MSR & LPI2C_MSR_PLTF) {
+            if(force_clock(hardware)) {
+                port->MCR = LPI2C_MCR_RST;
+                set_clock(port, 100000);
+            } else
+                return false;
+        }
+    }
+
+    return true;
+}
+
 // get bytes (max 8 if local buffer, else max 255), waits for result
 uint8_t *I2C_Receive (uint32_t i2cAddr, uint8_t *buf, uint16_t bytes, bool block)
 {
@@ -338,11 +354,10 @@ nvs_transfer_result_t i2c_nvs_transfer (nvs_transfer_t *transfer, bool read)
 
 void I2C_GetKeycode (uint32_t i2cAddr, keycode_callback_ptr callback)
 {
-    while(i2cIsBusy);
-
-    i2c.keycode_callback = callback;
-
-    I2C_Receive(i2cAddr, NULL, 1, false);
+    if(wait_ready()) {
+        i2c.keycode_callback = callback;
+        I2C_Receive(i2cAddr, NULL, 1, false);
+    }
 }
 
 #endif
@@ -504,7 +519,9 @@ hal.stream.write(ASCII_EOL);
             }
           #endif
             break;
-default: break;
+
+        default:
+            break;
     }
 }
 
diff --git a/grblHAL_Teensy4/src/keypad b/grblHAL_Teensy4/src/keypad
index 0ae3a11..639ac77 160000
--- a/grblHAL_Teensy4/src/keypad
+++ b/grblHAL_Teensy4/src/keypad
@@ -1 +1 @@
-Subproject commit 0ae3a110b6064596022aa978a5d0a1f14abaa0d7
+Subproject commit 639ac771ebdf6163300ea111be5725958be7076b
diff --git a/grblHAL_Teensy4/src/laser b/grblHAL_Teensy4/src/laser
index 942f560..6ececce 160000
--- a/grblHAL_Teensy4/src/laser
+++ b/grblHAL_Teensy4/src/laser
@@ -1 +1 @@
-Subproject commit 942f5607ba4d6428d8a2f5b690e251d80d462c61
+Subproject commit 6ececce9e0aa6da9a1ce1fc8cc1861b81b9ff705
diff --git a/grblHAL_Teensy4/src/networking b/grblHAL_Teensy4/src/networking
index 3652e14..4ffa4a9 160000
--- a/grblHAL_Teensy4/src/networking
+++ b/grblHAL_Teensy4/src/networking
@@ -1 +1 @@
-Subproject commit 3652e142c228c7a168cfa929497f570d541511f5
+Subproject commit 4ffa4a9826d557d260faa6d7023e196eeeb62c6e
diff --git a/grblHAL_Teensy4/src/odometer b/grblHAL_Teensy4/src/odometer
index 3ba87d7..66d7789 160000
--- a/grblHAL_Teensy4/src/odometer
+++ b/grblHAL_Teensy4/src/odometer
@@ -1 +1 @@
-Subproject commit 3ba87d7450bb6c4f4ded4cce96a0c71cffc48fa5
+Subproject commit 66d77893856e9e12fd6dfa2f5293716a4eb21a80
diff --git a/grblHAL_Teensy4/src/plasma b/grblHAL_Teensy4/src/plasma
index b6240a9..48c2de6 160000
--- a/grblHAL_Teensy4/src/plasma
+++ b/grblHAL_Teensy4/src/plasma
@@ -1 +1 @@
-Subproject commit b6240a9f9d88b7ef8ac368474589495cf2f43d24
+Subproject commit 48c2de6609d400c8f817e3512c8c29ddbaa4fa9c
diff --git a/grblHAL_Teensy4/src/sdcard b/grblHAL_Teensy4/src/sdcard
index 1f5a0df..ee0d184 160000
--- a/grblHAL_Teensy4/src/sdcard
+++ b/grblHAL_Teensy4/src/sdcard
@@ -1 +1 @@
-Subproject commit 1f5a0df4e0a58480ab7c4e80f0f018152aa2da01
+Subproject commit ee0d184bad9f329d327bf0547a272c717cf55029
diff --git a/grblHAL_Teensy4/src/spindle b/grblHAL_Teensy4/src/spindle
index 2f81fc6..7d8866d 160000
--- a/grblHAL_Teensy4/src/spindle
+++ b/grblHAL_Teensy4/src/spindle
@@ -1 +1 @@
-Subproject commit 2f81fc6ce340133a48bd5a8209c34220d269308a
+Subproject commit 7d8866da54ba5d2b5dc2450a4801909f29c2b7f8