From bc1f5ef38172bc77a802fb779e5da60f800c231b Mon Sep 17 00:00:00 2001
From: Ryan <fauxpark@gmail.com>
Date: Thu, 7 Oct 2021 10:29:35 +1100
Subject: i2c_master: Add support for reading/writing to 16-bit registers
 (#14289)

---
 platforms/avr/drivers/i2c_master.c | 56 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 56 insertions(+)

(limited to 'platforms/avr/drivers/i2c_master.c')

diff --git a/platforms/avr/drivers/i2c_master.c b/platforms/avr/drivers/i2c_master.c
index 2773e00778..111b55d6b0 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);
-- 
cgit v1.2.3