From c2e8d229a751df2451fd49915c55b4d3ba3f52c7 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Mon, 28 Sep 2020 20:21:07 +0200
Subject: [PATCH] Be more compliant in the I2C protocol

- Enter a repeated-start for reading data
- Write in the same session
---
 Firmware/pat9125.c |   6 +--
 Firmware/twi.c     | 110 +++++++++++++++++++++++++++++----------------
 Firmware/twi.h     |  20 ++++++---
 3 files changed, 89 insertions(+), 47 deletions(-)

diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index f47d676bb..c6ffecf3c 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -263,8 +263,7 @@ uint8_t pat9125_rd_reg(uint8_t addr)
 	if (!swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
         goto error;
 #elif defined(PAT9125_I2C)
-	if (twi_rw8(PAT9125_I2C_ADDR,TW_WRITE,&addr) ||
-        twi_rw8(PAT9125_I2C_ADDR,TW_READ,&data))
+	if (twi_r8(PAT9125_I2C_ADDR,addr,&data))
         goto error;
 #endif
 	return data;
@@ -286,8 +285,7 @@ void pat9125_wr_reg(uint8_t addr, uint8_t data)
 	if (!swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
         goto error;
 #elif defined(PAT9125_I2C)
-	if (twi_rw8(PAT9125_I2C_ADDR,TW_WRITE,&addr) ||
-        twi_rw8(PAT9125_I2C_ADDR,TW_READ,&data))
+	if (twi_w8(PAT9125_I2C_ADDR,addr,data))
         goto error;
 #endif
     return;
diff --git a/Firmware/twi.c b/Firmware/twi.c
index f8c077abb..6dd1645c0 100644
--- a/Firmware/twi.c
+++ b/Firmware/twi.c
@@ -48,55 +48,89 @@ void twi_disable(void)
   digitalWrite(SCL, 0);
 }
 
-static void twi_wait()
+
+static void twi_stop()
 {
-    while(!(TWCR & _BV(TWINT)));
+  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTO);
 }
 
-uint8_t twi_rw8(uint8_t address, uint8_t mode, uint8_t* data)
+
+static uint8_t twi_wait(uint8_t status)
+{
+  while(!(TWCR & _BV(TWINT)));
+  if(TW_STATUS != status)
+  {
+      twi_stop();
+      return 1;
+  }
+  return 0;
+}
+
+
+static uint8_t twi_start(uint8_t address, uint8_t reg)
 {
   // send start condition
   TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTA);
-  twi_wait();
-  if(TW_STATUS != TW_START)
+  if(twi_wait(TW_START))
       return 1;
 
   // send address
-  TWDR = mode;
-  TWDR |= (address << 1);
+  TWDR = TW_WRITE | (address << 1);
   TWCR = _BV(TWEN) | _BV(TWINT);
-  twi_wait();
+  if(twi_wait(TW_MT_SLA_ACK))
+      return 2;
 
-  if(mode == TW_WRITE)
-  {
-      if(TW_STATUS != TW_MT_SLA_ACK)
-          return 2;
-
-      // send data
-      TWDR = *data;
-      TWCR = _BV(TWEN) | _BV(TWINT);
-      twi_wait();
-      if(TW_STATUS != TW_MT_DATA_ACK)
-          return 3;
-  }
-  else
-  {
-      if(TW_STATUS != TW_MR_SLA_ACK)
-          return 2;
-
-      // receive data
-      TWCR = _BV(TWEN) | _BV(TWINT);
-      twi_wait();
-
-      // accept ACK or NACK (since only 1 byte is read)
-      if(!(TW_STATUS & TW_MR_DATA_ACK))
-          return 3;
-
-      *data = TWDR;
-  }
-
-  // send stop
-  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTO);
+  // send register
+  TWDR = reg;
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MT_DATA_ACK))
+      return 3;
 
   return 0;
 }
+
+
+uint8_t twi_r8(uint8_t address, uint8_t reg, uint8_t* data)
+{
+  if(twi_start(address, reg))
+      return 1;
+
+  // repeat start
+  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTA);
+  if(twi_wait(TW_REP_START))
+      return 2;
+
+  // start receiving
+  TWDR = TW_READ | (address << 1);
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MR_SLA_ACK))
+      return 3;
+
+  // receive data
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MR_DATA_NACK))
+      return 4;
+
+  *data = TWDR;
+
+  // send stop
+  twi_stop();
+  return 0;
+}
+
+
+uint8_t twi_w8(uint8_t address, uint8_t reg, uint8_t data)
+{
+  if(twi_start(address, reg))
+      return 1;
+
+  // send data
+  TWDR = data;
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MT_DATA_ACK))
+      return 2;
+
+  // send stop
+  twi_stop();
+  return 0;
+}
diff --git a/Firmware/twi.h b/Firmware/twi.h
index abcb8e975..bdb617fcb 100644
--- a/Firmware/twi.h
+++ b/Firmware/twi.h
@@ -43,11 +43,21 @@ void twi_init(void);
 void twi_disable(void);
 
 /*
- * Function twi_rw8
- * Desc     read/write a single byte from a device
+ * Function twi_r8
+ * Desc     read a single byte from a device
  * Input    address: 7bit i2c device address
- *          mode: TW_READ or TW_WRITE
- *          data: pointer to byte
+ *          reg: register address
+ *          data: pointer to byte for result
  * Output   0 on success
  */
-uint8_t twi_rw8(uint8_t address, uint8_t mode, uint8_t* data);
+uint8_t twi_r8(uint8_t address, uint8_t reg, uint8_t* data);
+
+/*
+ * Function twi_w8
+ * Desc     write a single byte from a device
+ * Input    address: 7bit i2c device address
+ *          reg: register address
+ *          data: byte to write
+ * Output   0 on success
+ */
+uint8_t twi_w8(uint8_t address, uint8_t reg, uint8_t data);