1 files changed, 56 insertions, 0 deletions

diff --git a/platforms/avr/drivers/i2c_master.c b/platforms/avr/drivers/i2c_master.c
index 2773e0077..111b55d6b 100644
--- a/platforms/avr/drivers/i2c_master.c
+++ b/platforms/avr/drivers/i2c_master.c
@@ -202,6 +202,25 @@ i2c_status_t i2c_writeReg(uint8_t devaddr, uint8_t regaddr, const uint8_t* data,
     return status;
 }
 
+i2c_status_t i2c_writeReg16(uint8_t devaddr, uint16_t regaddr, const uint8_t* data, uint16_t length, uint16_t timeout) {
+    i2c_status_t status = i2c_start(devaddr | 0x00, timeout);
+    if (status >= 0) {
+        status = i2c_write(regaddr >> 8, timeout);
+
+        if (status >= 0) {
+            status = i2c_write(regaddr & 0xFF, timeout);
+
+            for (uint16_t i = 0; i < length && status >= 0; i++) {
+                status = i2c_write(data[i], timeout);
+            }
+        }
+    }
+
+    i2c_stop();
+
+    return status;
+}
+
 i2c_status_t i2c_readReg(uint8_t devaddr, uint8_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout) {
     i2c_status_t status = i2c_start(devaddr, timeout);
     if (status < 0) {
@@ -235,6 +254,43 @@ error:
     return (status < 0) ? status : I2C_STATUS_SUCCESS;
 }
 
+i2c_status_t i2c_readReg16(uint8_t devaddr, uint16_t regaddr, uint8_t* data, uint16_t length, uint16_t timeout) {
+    i2c_status_t status = i2c_start(devaddr, timeout);
+    if (status < 0) {
+        goto error;
+    }
+
+    status = i2c_write(regaddr >> 8, timeout);
+    if (status < 0) {
+        goto error;
+    }
+    status = i2c_write(regaddr & 0xFF, timeout);
+    if (status < 0) {
+        goto error;
+    }
+
+    status = i2c_start(devaddr | 0x01, timeout);
+
+    for (uint16_t i = 0; i < (length - 1) && status >= 0; i++) {
+        status = i2c_read_ack(timeout);
+        if (status >= 0) {
+            data[i] = status;
+        }
+    }
+
+    if (status >= 0) {
+        status = i2c_read_nack(timeout);
+        if (status >= 0) {
+            data[(length - 1)] = status;
+        }
+    }
+
+error:
+    i2c_stop();
+
+    return (status < 0) ? status : I2C_STATUS_SUCCESS;
+}
+
 void i2c_stop(void) {
     // transmit STOP condition
     TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);