1 files changed, 24 insertions, 1 deletions

diff --git a/platforms/chibios/drivers/i2c_master.c b/platforms/chibios/drivers/i2c_master.c
index 981f6fa06..312b4c7d0 100644
--- a/platforms/chibios/drivers/i2c_master.c
+++ b/platforms/chibios/drivers/i2c_master.c
@@ -102,7 +102,7 @@ i2c_status_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, const uint8_t* data,
     i2cStart(&I2C_DRIVER, &i2cconfig);
 
     uint8_t complete_packet[length + 1];
-    for (uint8_t i = 0; i < length; i++) {
+    for (uint16_t i = 0; i < length; i++) {
         complete_packet[i + 1] = data[i];
     }
     complete_packet[0] = regaddr;
@@ -111,6 +111,21 @@ i2c_status_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, const uint8_t* data,
     return chibios_to_qmk(&status);
 }
 
+i2c_status_t i2c_writeReg16(uint8_t devaddr, uint16_t regaddr, const uint8_t* data, uint16_t length, uint16_t timeout) {
+    i2c_address = devaddr;
+    i2cStart(&I2C_DRIVER, &i2cconfig);
+
+    uint8_t complete_packet[length + 2];
+    for (uint16_t i = 0; i < length; i++) {
+        complete_packet[i + 2] = data[i];
+    }
+    complete_packet[0] = regaddr >> 8;
+    complete_packet[1] = regaddr & 0xFF;
+
+    msg_t status = i2cMasterTransmitTimeout(&I2C_DRIVER, (i2c_address >> 1), complete_packet, length + 2, 0, 0, TIME_MS2I(timeout));
+    return chibios_to_qmk(&status);
+}
+
 i2c_status_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout) {
     i2c_address = devaddr;
     i2cStart(&I2C_DRIVER, &i2cconfig);
@@ -118,4 +133,12 @@ i2c_status_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16
     return chibios_to_qmk(&status);
 }
 
+i2c_status_t i2c_readReg16(uint8_t devaddr, uint16_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout) {
+    i2c_address = devaddr;
+    i2cStart(&I2C_DRIVER, &i2cconfig);
+    uint8_t register_packet[2] = {regaddr >> 8, regaddr & 0xFF};
+    msg_t   status             = i2cMasterTransmitTimeout(&I2C_DRIVER, (i2c_address >> 1), &register_packet, 2, data, length, TIME_MS2I(timeout));
+    return chibios_to_qmk(&status);
+}
+
 void i2c_stop(void) { i2cStop(&I2C_DRIVER); }