From 5fe9dd5a5036e528d6180ef629d38d454e4744a3 Mon Sep 17 00:00:00 2001
From: Arksine <arksine.code@gmail.com>
Date: Thu, 9 Jan 2020 19:24:51 -0500
Subject: [PATCH] avr: implement i2c_read

Signed-off-by:  Eric Callahan <arksine.code@gmail.com>
---
 src/avr/i2c.c | 19 ++++++++++++++++++-
 src/i2ccmds.c |  2 +-
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/avr/i2c.c b/src/avr/i2c.c
index 4632497e..370582cc 100644
--- a/src/avr/i2c.c
+++ b/src/avr/i2c.c
@@ -84,6 +84,14 @@ i2c_send_byte(uint8_t b, uint32_t timeout)
     i2c_wait(timeout);
 }
 
+static void
+i2c_receive_byte(uint8_t *read, uint32_t timeout, uint8_t send_ack)
+{
+    TWCR = (1<<TWEN) | (1<<TWINT) | ((send_ack?1:0)<<TWEA);
+    i2c_wait(timeout);
+    *read = TWDR;
+}
+
 static void
 i2c_stop(uint32_t timeout)
 {
@@ -106,5 +114,14 @@ void
 i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg
          , uint8_t read_len, uint8_t *read)
 {
-    shutdown("i2c_read not supported on avr");
+    uint32_t timeout = timer_read_time() + timer_from_us(5000);
+    i2c_start(timeout);
+    i2c_send_byte(config.addr, timeout);
+    while (reg_len--)
+        i2c_send_byte(*reg++, timeout);
+    i2c_start(timeout);
+    i2c_send_byte(config.addr | 0x1, timeout);
+    while (read_len--)
+        i2c_receive_byte(read++, timeout, read_len);
+    i2c_stop(timeout);
 }
diff --git a/src/i2ccmds.c b/src/i2ccmds.c
index d518c666..388af6ed 100644
--- a/src/i2ccmds.c
+++ b/src/i2ccmds.c
@@ -42,7 +42,7 @@ command_i2c_read(uint32_t * args)
     struct i2cdev_s *i2c = oid_lookup(oid, command_config_i2c);
     uint8_t reg_len = args[1];
     uint8_t *reg = (void*)(size_t)args[2];
-    uint32_t data_len = args[3];
+    uint8_t data_len = args[3];
     uint8_t receive_array[data_len];
     uint8_t *data = (void*)(size_t)receive_array;
     i2c_read(i2c->i2c_config, reg_len, reg, data_len, data);