From 502bc8c72ddcb92828a4016546439674b2a74d5d Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Thu, 20 Aug 2020 12:33:52 +0200
Subject: [PATCH 01/14] Isolate more pat9125 code

Remove probing from Marlin_main and move it into pat9125_probe so that
it can support the various variants.
---
 Firmware/Marlin_main.cpp | 15 ++++-----------
 Firmware/config.h        |  4 +++-
 Firmware/pat9125.c       | 39 +++++++++++++++++++++++++++++----------
 Firmware/pat9125.h       |  1 +
 4 files changed, 37 insertions(+), 22 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 2a261db2a2..e3be3dfbd8 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -88,18 +88,13 @@
 #include "la10compat.h"
 #endif
 
-#ifdef SWSPI
-#include "swspi.h"
-#endif //SWSPI
-
 #include "spi.h"
 
-#ifdef SWI2C
-#include "swi2c.h"
-#endif //SWI2C
-
 #ifdef FILAMENT_SENSOR
 #include "fsensor.h"
+#ifdef IR_SENSOR
+#include "pat9125.h" // for pat9125_probe
+#endif
 #endif //FILAMENT_SENSOR
 
 #ifdef TMC2130
@@ -864,9 +859,7 @@ static void check_if_fw_is_on_right_printer(){
 #ifdef FILAMENT_SENSOR
   if((PRINTER_TYPE == PRINTER_MK3) || (PRINTER_TYPE == PRINTER_MK3S)){
     #ifdef IR_SENSOR
-    swi2c_init();
-    const uint8_t pat9125_detected = swi2c_readByte_A8(PAT9125_I2C_ADDR,0x00,NULL);
-      if (pat9125_detected){
+      if (pat9125_probe()){
         lcd_show_fullscreen_message_and_wait_P(_i("MK3S firmware detected on MK3 printer"));}////c=20 r=3
     #endif //IR_SENSOR
 
diff --git a/Firmware/config.h b/Firmware/config.h
index 1a0a97008a..b7705584a4 100644
--- a/Firmware/config.h
+++ b/Firmware/config.h
@@ -23,7 +23,6 @@
 #define ADC_CALLBACK      adc_ready //callback function ()
 
 //SWI2C configuration
-#define SWI2C
 //#define SWI2C_SDA         20 //SDA on P3
 //#define SWI2C_SCL         21 //SCL on P3
 #define SWI2C_A8
@@ -31,7 +30,10 @@
 #define SWI2C_TMO         2048 //2048 cycles timeout
 
 //PAT9125 configuration
+//#define PAT9125_SWSPI
 #define PAT9125_SWI2C
+//#define PAT9125_I2C
+
 #define PAT9125_I2C_ADDR  0x75  //ID=LO
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI
 //#define PAT9125_I2C_ADDR  0x73  //ID=NC
diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index de4693d290..10b06b3eaa 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -26,12 +26,15 @@
 #define PAT9125_BANK_SELECTION	0x7f
 
 
-#ifdef PAT9125_SWSPI
+#if defined(PAT9125_SWSPI)
 #include "swspi.h"
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
+#elif defined(PAT9125_SWI2C)
 #include "swi2c.h"
-#endif //PAT9125_SWI2C
+#elif defined(PAT9125_I2C)
+#error not implemented
+#else
+#error unknown PAT9125 communication method
+#endif
 
 
 uint8_t pat9125_PID1 = 0;
@@ -103,14 +106,30 @@ extern FILE _uartout;
 #define uartout (&_uartout)
 
 
+uint8_t pat9125_probe()
+{
+#if defined(PAT9125_SWSPI)
+    swspi_init();
+    //#error not implemented
+#elif defined(PAT9125_SWI2C)
+    swi2c_init();
+    return swi2c_readByte_A8(PAT9125_I2C_ADDR,0x00,NULL);
+#elif defined(PAT9125_I2C)
+    twi_init();
+  #ifdef IR_SENSOR
+    // NOTE: this is called from the MK3S variant, so it should be kept minimal
+    #error not implemented
+  #else
+	return pat9125_rd_reg(PAT9125_PID1) != 0;
+  #endif
+#endif
+}
+
 uint8_t pat9125_init(void)
 {
-#ifdef PAT9125_SWSPI
-	swspi_init();
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
-	swi2c_init();
-#endif //PAT9125_SWI2C
+    if (!pat9125_probe())
+        return 0;
+
 	// Verify that the sensor responds with its correct product ID.
 	pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
 	pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
diff --git a/Firmware/pat9125.h b/Firmware/pat9125.h
index 12f7fe949e..1fb2539f44 100755
--- a/Firmware/pat9125.h
+++ b/Firmware/pat9125.h
@@ -18,6 +18,7 @@ extern int16_t pat9125_y;
 extern uint8_t pat9125_b;
 extern uint8_t pat9125_s;
 
+extern uint8_t pat9125_probe(void);     // Return non-zero if PAT9125 can be trivially detected
 extern uint8_t pat9125_init(void);
 extern uint8_t pat9125_update(void);    // update all sensor data
 extern uint8_t pat9125_update_y(void);  // update _y only

From 240dc1132ee90fe3680e787dcde665306cc3bcf2 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Thu, 20 Aug 2020 12:36:01 +0200
Subject: [PATCH 02/14] Include initial implementation based on Arduino's twi

---
 Firmware/config.h  |   4 +-
 Firmware/pat9125.c |  47 ++--
 Firmware/twi.c     | 561 +++++++++++++++++++++++++++++++++++++++++++++
 Firmware/twi.h     |  55 +++++
 4 files changed, 646 insertions(+), 21 deletions(-)
 create mode 100644 Firmware/twi.c
 create mode 100644 Firmware/twi.h

diff --git a/Firmware/config.h b/Firmware/config.h
index b7705584a4..b107d0ec5f 100644
--- a/Firmware/config.h
+++ b/Firmware/config.h
@@ -31,8 +31,8 @@
 
 //PAT9125 configuration
 //#define PAT9125_SWSPI
-#define PAT9125_SWI2C
-//#define PAT9125_I2C
+//#define PAT9125_SWI2C
+#define PAT9125_I2C
 
 #define PAT9125_I2C_ADDR  0x75  //ID=LO
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI
diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index 10b06b3eaa..bf2c0d9586 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -31,7 +31,7 @@
 #elif defined(PAT9125_SWI2C)
 #include "swi2c.h"
 #elif defined(PAT9125_I2C)
-#error not implemented
+#include "twi.h"
 #else
 #error unknown PAT9125 communication method
 #endif
@@ -253,39 +253,48 @@ uint8_t pat9125_update_bs(void)
 uint8_t pat9125_rd_reg(uint8_t addr)
 {
 	uint8_t data = 0;
-#ifdef PAT9125_SWSPI
+#if defined(PAT9125_SWSPI)
 	swspi_start();
 	swspi_tx(addr & 0x7f);
 	data = swspi_rx();
 	swspi_stop();
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
+#elif defined(PAT9125_SWI2C)
 	if (!swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
-	{
-		pat9125_PID1 = 0xff;
-		pat9125_PID2 = 0xff;
-		return 0;
-	}
-#endif //PAT9125_SWI2C
+        goto error;
+#elif defined(PAT9125_I2C)
+	if (twi_writeTo(PAT9125_I2C_ADDR,&addr,1,1,0) != 0 ||
+        twi_readFrom(PAT9125_I2C_ADDR,&data,1,1) != 1)
+        goto error;
+#endif
 	return data;
+
+ error:
+    pat9125_PID1 = 0xff;
+    pat9125_PID2 = 0xff;
+    return 0;
 }
 
 void pat9125_wr_reg(uint8_t addr, uint8_t data)
 {
-#ifdef PAT9125_SWSPI
+#if defined(PAT9125_SWSPI)
 	swspi_start();
 	swspi_tx(addr | 0x80);
 	swspi_tx(data);
 	swspi_stop();
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
+#elif defined(PAT9125_SWI2C)
 	if (!swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
-	{
-		pat9125_PID1 = 0xff;
-		pat9125_PID2 = 0xff;
-		return;
-	}
-#endif //PAT9125_SWI2C
+        goto error;
+#elif defined(PAT9125_I2C)
+	if (twi_writeTo(PAT9125_I2C_ADDR,&addr,1,1,0) != 0 ||
+        twi_writeTo(PAT9125_I2C_ADDR,&data,1,1,1) != 0)
+        goto error;
+#endif
+    return;
+
+ error:
+    pat9125_PID1 = 0xff;
+    pat9125_PID2 = 0xff;
+    return;
 }
 
 uint8_t pat9125_wr_reg_verify(uint8_t addr, uint8_t data)
diff --git a/Firmware/twi.c b/Firmware/twi.c
new file mode 100644
index 0000000000..171af73037
--- /dev/null
+++ b/Firmware/twi.c
@@ -0,0 +1,561 @@
+/*
+  twi.c - TWI/I2C library for Wiring & Arduino
+  Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+  Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
+*/
+
+#include <math.h>
+#include <stdlib.h>
+#include <inttypes.h>
+#include <avr/io.h>
+#include <avr/interrupt.h>
+#include <compat/twi.h>
+#include "Arduino.h" // for digitalWrite
+
+#ifndef cbi
+#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#endif
+
+#ifndef sbi
+#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+#endif
+
+#include "pins_arduino.h"
+#include "twi.h"
+
+static volatile uint8_t twi_state;
+static volatile uint8_t twi_slarw;
+static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
+static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start
+
+static void (*twi_onSlaveTransmit)(void);
+static void (*twi_onSlaveReceive)(uint8_t*, int);
+
+static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH];
+static volatile uint8_t twi_masterBufferIndex;
+static volatile uint8_t twi_masterBufferLength;
+
+static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH];
+static volatile uint8_t twi_txBufferIndex;
+static volatile uint8_t twi_txBufferLength;
+
+static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH];
+static volatile uint8_t twi_rxBufferIndex;
+
+static volatile uint8_t twi_error;
+
+/* 
+ * Function twi_init
+ * Desc     readys twi pins and sets twi bitrate
+ * Input    none
+ * Output   none
+ */
+void twi_init(void)
+{
+  // initialize state
+  twi_state = TWI_READY;
+  twi_sendStop = true;		// default value
+  twi_inRepStart = false;
+  
+  // activate internal pullups for twi.
+  digitalWrite(SDA, 1);
+  digitalWrite(SCL, 1);
+
+  // initialize twi prescaler and bit rate
+  cbi(TWSR, TWPS0);
+  cbi(TWSR, TWPS1);
+  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
+
+  /* twi bit rate formula from atmega128 manual pg 204
+  SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
+  note: TWBR should be 10 or higher for master mode
+  It is 72 for a 16mhz Wiring board with 100kHz TWI */
+
+  // enable twi module, acks, and twi interrupt
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
+}
+
+/* 
+ * Function twi_disable
+ * Desc     disables twi pins
+ * Input    none
+ * Output   none
+ */
+void twi_disable(void)
+{
+  // disable twi module, acks, and twi interrupt
+  TWCR &= ~(_BV(TWEN) | _BV(TWIE) | _BV(TWEA));
+
+  // deactivate internal pullups for twi.
+  digitalWrite(SDA, 0);
+  digitalWrite(SCL, 0);
+}
+
+/* 
+ * Function twi_slaveInit
+ * Desc     sets slave address and enables interrupt
+ * Input    none
+ * Output   none
+ */
+void twi_setAddress(uint8_t address)
+{
+  // set twi slave address (skip over TWGCE bit)
+  TWAR = address << 1;
+}
+
+/* 
+ * Function twi_setClock
+ * Desc     sets twi bit rate
+ * Input    Clock Frequency
+ * Output   none
+ */
+void twi_setFrequency(uint32_t frequency)
+{
+  TWBR = ((F_CPU / frequency) - 16) / 2;
+  
+  /* twi bit rate formula from atmega128 manual pg 204
+  SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
+  note: TWBR should be 10 or higher for master mode
+  It is 72 for a 16mhz Wiring board with 100kHz TWI */
+}
+
+/* 
+ * Function twi_readFrom
+ * Desc     attempts to become twi bus master and read a
+ *          series of bytes from a device on the bus
+ * Input    address: 7bit i2c device address
+ *          data: pointer to byte array
+ *          length: number of bytes to read into array
+ *          sendStop: Boolean indicating whether to send a stop at the end
+ * Output   number of bytes read
+ */
+uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop)
+{
+  uint8_t i;
+
+  // ensure data will fit into buffer
+  if(TWI_BUFFER_LENGTH < length){
+    return 0;
+  }
+
+  // wait until twi is ready, become master receiver
+  while(TWI_READY != twi_state){
+    continue;
+  }
+  twi_state = TWI_MRX;
+  twi_sendStop = sendStop;
+  // reset error state (0xFF.. no error occured)
+  twi_error = 0xFF;
+
+  // initialize buffer iteration vars
+  twi_masterBufferIndex = 0;
+  twi_masterBufferLength = length-1;  // This is not intuitive, read on...
+  // On receive, the previously configured ACK/NACK setting is transmitted in
+  // response to the received byte before the interrupt is signalled. 
+  // Therefor we must actually set NACK when the _next_ to last byte is
+  // received, causing that NACK to be sent in response to receiving the last
+  // expected byte of data.
+
+  // build sla+w, slave device address + w bit
+  twi_slarw = TW_READ;
+  twi_slarw |= address << 1;
+
+  if (true == twi_inRepStart) {
+    // if we're in the repeated start state, then we've already sent the start,
+    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
+    // We need to remove ourselves from the repeated start state before we enable interrupts,
+    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
+    // up. Also, don't enable the START interrupt. There may be one pending from the 
+    // repeated start that we sent ourselves, and that would really confuse things.
+    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
+    do {
+      TWDR = twi_slarw;
+    } while(TWCR & _BV(TWWC));
+    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
+  }
+  else
+    // send start condition
+    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
+
+  // wait for read operation to complete
+  while(TWI_MRX == twi_state){
+    continue;
+  }
+
+  if (twi_masterBufferIndex < length)
+    length = twi_masterBufferIndex;
+
+  // copy twi buffer to data
+  for(i = 0; i < length; ++i){
+    data[i] = twi_masterBuffer[i];
+  }
+	
+  return length;
+}
+
+/* 
+ * Function twi_writeTo
+ * Desc     attempts to become twi bus master and write a
+ *          series of bytes to a device on the bus
+ * Input    address: 7bit i2c device address
+ *          data: pointer to byte array
+ *          length: number of bytes in array
+ *          wait: boolean indicating to wait for write or not
+ *          sendStop: boolean indicating whether or not to send a stop at the end
+ * Output   0 .. success
+ *          1 .. length to long for buffer
+ *          2 .. address send, NACK received
+ *          3 .. data send, NACK received
+ *          4 .. other twi error (lost bus arbitration, bus error, ..)
+ */
+uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop)
+{
+  uint8_t i;
+
+  // ensure data will fit into buffer
+  if(TWI_BUFFER_LENGTH < length){
+    return 1;
+  }
+
+  // wait until twi is ready, become master transmitter
+  while(TWI_READY != twi_state){
+    continue;
+  }
+  twi_state = TWI_MTX;
+  twi_sendStop = sendStop;
+  // reset error state (0xFF.. no error occured)
+  twi_error = 0xFF;
+
+  // initialize buffer iteration vars
+  twi_masterBufferIndex = 0;
+  twi_masterBufferLength = length;
+  
+  // copy data to twi buffer
+  for(i = 0; i < length; ++i){
+    twi_masterBuffer[i] = data[i];
+  }
+  
+  // build sla+w, slave device address + w bit
+  twi_slarw = TW_WRITE;
+  twi_slarw |= address << 1;
+  
+  // if we're in a repeated start, then we've already sent the START
+  // in the ISR. Don't do it again.
+  //
+  if (true == twi_inRepStart) {
+    // if we're in the repeated start state, then we've already sent the start,
+    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
+    // We need to remove ourselves from the repeated start state before we enable interrupts,
+    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
+    // up. Also, don't enable the START interrupt. There may be one pending from the 
+    // repeated start that we sent outselves, and that would really confuse things.
+    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
+    do {
+      TWDR = twi_slarw;				
+    } while(TWCR & _BV(TWWC));
+    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
+  }
+  else
+    // send start condition
+    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA);	// enable INTs
+
+  // wait for write operation to complete
+  while(wait && (TWI_MTX == twi_state)){
+    continue;
+  }
+  
+  if (twi_error == 0xFF)
+    return 0;	// success
+  else if (twi_error == TW_MT_SLA_NACK)
+    return 2;	// error: address send, nack received
+  else if (twi_error == TW_MT_DATA_NACK)
+    return 3;	// error: data send, nack received
+  else
+    return 4;	// other twi error
+}
+
+/* 
+ * Function twi_transmit
+ * Desc     fills slave tx buffer with data
+ *          must be called in slave tx event callback
+ * Input    data: pointer to byte array
+ *          length: number of bytes in array
+ * Output   1 length too long for buffer
+ *          2 not slave transmitter
+ *          0 ok
+ */
+uint8_t twi_transmit(const uint8_t* data, uint8_t length)
+{
+  uint8_t i;
+
+  // ensure data will fit into buffer
+  if(TWI_BUFFER_LENGTH < (twi_txBufferLength+length)){
+    return 1;
+  }
+  
+  // ensure we are currently a slave transmitter
+  if(TWI_STX != twi_state){
+    return 2;
+  }
+  
+  // set length and copy data into tx buffer
+  for(i = 0; i < length; ++i){
+    twi_txBuffer[twi_txBufferLength+i] = data[i];
+  }
+  twi_txBufferLength += length;
+  
+  return 0;
+}
+
+/* 
+ * Function twi_attachSlaveRxEvent
+ * Desc     sets function called before a slave read operation
+ * Input    function: callback function to use
+ * Output   none
+ */
+void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) )
+{
+  twi_onSlaveReceive = function;
+}
+
+/* 
+ * Function twi_attachSlaveTxEvent
+ * Desc     sets function called before a slave write operation
+ * Input    function: callback function to use
+ * Output   none
+ */
+void twi_attachSlaveTxEvent( void (*function)(void) )
+{
+  twi_onSlaveTransmit = function;
+}
+
+/* 
+ * Function twi_reply
+ * Desc     sends byte or readys receive line
+ * Input    ack: byte indicating to ack or to nack
+ * Output   none
+ */
+void twi_reply(uint8_t ack)
+{
+  // transmit master read ready signal, with or without ack
+  if(ack){
+    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
+  }else{
+	  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
+  }
+}
+
+/* 
+ * Function twi_stop
+ * Desc     relinquishes bus master status
+ * Input    none
+ * Output   none
+ */
+void twi_stop(void)
+{
+  // send stop condition
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);
+
+  // wait for stop condition to be exectued on bus
+  // TWINT is not set after a stop condition!
+  while(TWCR & _BV(TWSTO)){
+    continue;
+  }
+
+  // update twi state
+  twi_state = TWI_READY;
+}
+
+/* 
+ * Function twi_releaseBus
+ * Desc     releases bus control
+ * Input    none
+ * Output   none
+ */
+void twi_releaseBus(void)
+{
+  // release bus
+  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);
+
+  // update twi state
+  twi_state = TWI_READY;
+}
+
+ISR(TWI_vect)
+{
+  switch(TW_STATUS){
+    // All Master
+    case TW_START:     // sent start condition
+    case TW_REP_START: // sent repeated start condition
+      // copy device address and r/w bit to output register and ack
+      TWDR = twi_slarw;
+      twi_reply(1);
+      break;
+
+    // Master Transmitter
+    case TW_MT_SLA_ACK:  // slave receiver acked address
+    case TW_MT_DATA_ACK: // slave receiver acked data
+      // if there is data to send, send it, otherwise stop 
+      if(twi_masterBufferIndex < twi_masterBufferLength){
+        // copy data to output register and ack
+        TWDR = twi_masterBuffer[twi_masterBufferIndex++];
+        twi_reply(1);
+      }else{
+	if (twi_sendStop)
+          twi_stop();
+	else {
+	  twi_inRepStart = true;	// we're gonna send the START
+	  // don't enable the interrupt. We'll generate the start, but we 
+	  // avoid handling the interrupt until we're in the next transaction,
+	  // at the point where we would normally issue the start.
+	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
+	  twi_state = TWI_READY;
+	}
+      }
+      break;
+    case TW_MT_SLA_NACK:  // address sent, nack received
+      twi_error = TW_MT_SLA_NACK;
+      twi_stop();
+      break;
+    case TW_MT_DATA_NACK: // data sent, nack received
+      twi_error = TW_MT_DATA_NACK;
+      twi_stop();
+      break;
+    case TW_MT_ARB_LOST: // lost bus arbitration
+      twi_error = TW_MT_ARB_LOST;
+      twi_releaseBus();
+      break;
+
+    // Master Receiver
+    case TW_MR_DATA_ACK: // data received, ack sent
+      // put byte into buffer
+      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
+    case TW_MR_SLA_ACK:  // address sent, ack received
+      // ack if more bytes are expected, otherwise nack
+      if(twi_masterBufferIndex < twi_masterBufferLength){
+        twi_reply(1);
+      }else{
+        twi_reply(0);
+      }
+      break;
+    case TW_MR_DATA_NACK: // data received, nack sent
+      // put final byte into buffer
+      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
+	if (twi_sendStop)
+          twi_stop();
+	else {
+	  twi_inRepStart = true;	// we're gonna send the START
+	  // don't enable the interrupt. We'll generate the start, but we 
+	  // avoid handling the interrupt until we're in the next transaction,
+	  // at the point where we would normally issue the start.
+	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
+	  twi_state = TWI_READY;
+	}    
+	break;
+    case TW_MR_SLA_NACK: // address sent, nack received
+      twi_stop();
+      break;
+    // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case
+
+    // Slave Receiver
+    case TW_SR_SLA_ACK:   // addressed, returned ack
+    case TW_SR_GCALL_ACK: // addressed generally, returned ack
+    case TW_SR_ARB_LOST_SLA_ACK:   // lost arbitration, returned ack
+    case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack
+      // enter slave receiver mode
+      twi_state = TWI_SRX;
+      // indicate that rx buffer can be overwritten and ack
+      twi_rxBufferIndex = 0;
+      twi_reply(1);
+      break;
+    case TW_SR_DATA_ACK:       // data received, returned ack
+    case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack
+      // if there is still room in the rx buffer
+      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
+        // put byte in buffer and ack
+        twi_rxBuffer[twi_rxBufferIndex++] = TWDR;
+        twi_reply(1);
+      }else{
+        // otherwise nack
+        twi_reply(0);
+      }
+      break;
+    case TW_SR_STOP: // stop or repeated start condition received
+      // ack future responses and leave slave receiver state
+      twi_releaseBus();
+      // put a null char after data if there's room
+      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
+        twi_rxBuffer[twi_rxBufferIndex] = '\0';
+      }
+      // callback to user defined callback
+      twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex);
+      // since we submit rx buffer to "wire" library, we can reset it
+      twi_rxBufferIndex = 0;
+      break;
+    case TW_SR_DATA_NACK:       // data received, returned nack
+    case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack
+      // nack back at master
+      twi_reply(0);
+      break;
+    
+    // Slave Transmitter
+    case TW_ST_SLA_ACK:          // addressed, returned ack
+    case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack
+      // enter slave transmitter mode
+      twi_state = TWI_STX;
+      // ready the tx buffer index for iteration
+      twi_txBufferIndex = 0;
+      // set tx buffer length to be zero, to verify if user changes it
+      twi_txBufferLength = 0;
+      // request for txBuffer to be filled and length to be set
+      // note: user must call twi_transmit(bytes, length) to do this
+      twi_onSlaveTransmit();
+      // if they didn't change buffer & length, initialize it
+      if(0 == twi_txBufferLength){
+        twi_txBufferLength = 1;
+        twi_txBuffer[0] = 0x00;
+      }
+      // transmit first byte from buffer, fall
+    case TW_ST_DATA_ACK: // byte sent, ack returned
+      // copy data to output register
+      TWDR = twi_txBuffer[twi_txBufferIndex++];
+      // if there is more to send, ack, otherwise nack
+      if(twi_txBufferIndex < twi_txBufferLength){
+        twi_reply(1);
+      }else{
+        twi_reply(0);
+      }
+      break;
+    case TW_ST_DATA_NACK: // received nack, we are done 
+    case TW_ST_LAST_DATA: // received ack, but we are done already!
+      // ack future responses
+      twi_reply(1);
+      // leave slave receiver state
+      twi_state = TWI_READY;
+      break;
+
+    // All
+    case TW_NO_INFO:   // no state information
+      break;
+    case TW_BUS_ERROR: // bus error, illegal stop/start
+      twi_error = TW_BUS_ERROR;
+      twi_stop();
+      break;
+  }
+}
+
diff --git a/Firmware/twi.h b/Firmware/twi.h
new file mode 100644
index 0000000000..cb48708c4c
--- /dev/null
+++ b/Firmware/twi.h
@@ -0,0 +1,55 @@
+/*
+  twi.h - TWI/I2C library for Wiring & Arduino
+  Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#ifndef twi_h
+#define twi_h
+
+  #include <inttypes.h>
+
+  //#define ATMEGA8
+
+  #ifndef TWI_FREQ
+  #define TWI_FREQ 400000L
+  #endif
+
+  #ifndef TWI_BUFFER_LENGTH
+  #define TWI_BUFFER_LENGTH 32
+  #endif
+
+  #define TWI_READY 0
+  #define TWI_MRX   1
+  #define TWI_MTX   2
+  #define TWI_SRX   3
+  #define TWI_STX   4
+  
+  void twi_init(void);
+  void twi_disable(void);
+  void twi_setAddress(uint8_t);
+  void twi_setFrequency(uint32_t);
+  uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t);
+  uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t);
+  uint8_t twi_transmit(const uint8_t*, uint8_t);
+  void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) );
+  void twi_attachSlaveTxEvent( void (*)(void) );
+  void twi_reply(uint8_t);
+  void twi_stop(void);
+  void twi_releaseBus(void);
+
+#endif
+

From 7f425120f09dc33b7bd1b75af4d42cdad5ad1441 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Thu, 20 Aug 2020 15:29:57 +0200
Subject: [PATCH 03/14] Strip down the TWI code

- Only implement a single syncronous read/write function to read a byte,
  since that's all we need currently
- Implement a compact IR_SENSOR probe for PAT9125
- Saves 242 bytes compared to PAT9125_SWI2C
---
 Firmware/pat9125.c |  13 +-
 Firmware/twi.c     | 524 +++------------------------------------------
 Firmware/twi.h     |  66 +++---
 3 files changed, 67 insertions(+), 536 deletions(-)

diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index bf2c0d9586..ab6342aaac 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -118,9 +118,10 @@ uint8_t pat9125_probe()
     twi_init();
   #ifdef IR_SENSOR
     // NOTE: this is called from the MK3S variant, so it should be kept minimal
-    #error not implemented
+    uint8_t addr = PAT9125_PID1;
+    return (twi_rw8(PAT9125_I2C_ADDR,TW_READ,&addr) == 0);
   #else
-	return pat9125_rd_reg(PAT9125_PID1) != 0;
+    return (pat9125_rd_reg(PAT9125_PID1) != 0);
   #endif
 #endif
 }
@@ -262,8 +263,8 @@ 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_writeTo(PAT9125_I2C_ADDR,&addr,1,1,0) != 0 ||
-        twi_readFrom(PAT9125_I2C_ADDR,&data,1,1) != 1)
+	if (twi_rw8(PAT9125_I2C_ADDR,TW_WRITE,&addr) ||
+        twi_rw8(PAT9125_I2C_ADDR,TW_READ,&data))
         goto error;
 #endif
 	return data;
@@ -285,8 +286,8 @@ 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_writeTo(PAT9125_I2C_ADDR,&addr,1,1,0) != 0 ||
-        twi_writeTo(PAT9125_I2C_ADDR,&data,1,1,1) != 0)
+	if (twi_rw8(PAT9125_I2C_ADDR,TW_WRITE,&addr) ||
+        twi_rw8(PAT9125_I2C_ADDR,TW_READ,&data))
         goto error;
 #endif
     return;
diff --git a/Firmware/twi.c b/Firmware/twi.c
index 171af73037..161db52ac4 100644
--- a/Firmware/twi.c
+++ b/Firmware/twi.c
@@ -1,5 +1,5 @@
 /*
-  twi.c - TWI/I2C library for Wiring & Arduino
+  twi.c - Stripped-down TWI/I2C library
   Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
 
   This library is free software; you can redistribute it and/or
@@ -20,11 +20,6 @@
 */
 
 #include <math.h>
-#include <stdlib.h>
-#include <inttypes.h>
-#include <avr/io.h>
-#include <avr/interrupt.h>
-#include <compat/twi.h>
 #include "Arduino.h" // for digitalWrite
 
 #ifndef cbi
@@ -35,43 +30,11 @@
 #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
 #endif
 
-#include "pins_arduino.h"
 #include "twi.h"
 
-static volatile uint8_t twi_state;
-static volatile uint8_t twi_slarw;
-static volatile uint8_t twi_sendStop;			// should the transaction end with a stop
-static volatile uint8_t twi_inRepStart;			// in the middle of a repeated start
 
-static void (*twi_onSlaveTransmit)(void);
-static void (*twi_onSlaveReceive)(uint8_t*, int);
-
-static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH];
-static volatile uint8_t twi_masterBufferIndex;
-static volatile uint8_t twi_masterBufferLength;
-
-static uint8_t twi_txBuffer[TWI_BUFFER_LENGTH];
-static volatile uint8_t twi_txBufferIndex;
-static volatile uint8_t twi_txBufferLength;
-
-static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH];
-static volatile uint8_t twi_rxBufferIndex;
-
-static volatile uint8_t twi_error;
-
-/* 
- * Function twi_init
- * Desc     readys twi pins and sets twi bitrate
- * Input    none
- * Output   none
- */
 void twi_init(void)
 {
-  // initialize state
-  twi_state = TWI_READY;
-  twi_sendStop = true;		// default value
-  twi_inRepStart = false;
-  
   // activate internal pullups for twi.
   digitalWrite(SDA, 1);
   digitalWrite(SCL, 1);
@@ -85,477 +48,46 @@ void twi_init(void)
   SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
   note: TWBR should be 10 or higher for master mode
   It is 72 for a 16mhz Wiring board with 100kHz TWI */
-
-  // enable twi module, acks, and twi interrupt
-  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
 }
 
-/* 
- * Function twi_disable
- * Desc     disables twi pins
- * Input    none
- * Output   none
- */
 void twi_disable(void)
 {
-  // disable twi module, acks, and twi interrupt
-  TWCR &= ~(_BV(TWEN) | _BV(TWIE) | _BV(TWEA));
-
   // deactivate internal pullups for twi.
   digitalWrite(SDA, 0);
   digitalWrite(SCL, 0);
 }
 
-/* 
- * Function twi_slaveInit
- * Desc     sets slave address and enables interrupt
- * Input    none
- * Output   none
- */
-void twi_setAddress(uint8_t address)
+uint8_t twi_waitfor(uint8_t status)
 {
-  // set twi slave address (skip over TWGCE bit)
-  TWAR = address << 1;
-}
-
-/* 
- * Function twi_setClock
- * Desc     sets twi bit rate
- * Input    Clock Frequency
- * Output   none
- */
-void twi_setFrequency(uint32_t frequency)
-{
-  TWBR = ((F_CPU / frequency) - 16) / 2;
-  
-  /* twi bit rate formula from atmega128 manual pg 204
-  SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
-  note: TWBR should be 10 or higher for master mode
-  It is 72 for a 16mhz Wiring board with 100kHz TWI */
-}
-
-/* 
- * Function twi_readFrom
- * Desc     attempts to become twi bus master and read a
- *          series of bytes from a device on the bus
- * Input    address: 7bit i2c device address
- *          data: pointer to byte array
- *          length: number of bytes to read into array
- *          sendStop: Boolean indicating whether to send a stop at the end
- * Output   number of bytes read
- */
-uint8_t twi_readFrom(uint8_t address, uint8_t* data, uint8_t length, uint8_t sendStop)
-{
-  uint8_t i;
-
-  // ensure data will fit into buffer
-  if(TWI_BUFFER_LENGTH < length){
-    return 0;
-  }
-
-  // wait until twi is ready, become master receiver
-  while(TWI_READY != twi_state){
-    continue;
-  }
-  twi_state = TWI_MRX;
-  twi_sendStop = sendStop;
-  // reset error state (0xFF.. no error occured)
-  twi_error = 0xFF;
-
-  // initialize buffer iteration vars
-  twi_masterBufferIndex = 0;
-  twi_masterBufferLength = length-1;  // This is not intuitive, read on...
-  // On receive, the previously configured ACK/NACK setting is transmitted in
-  // response to the received byte before the interrupt is signalled. 
-  // Therefor we must actually set NACK when the _next_ to last byte is
-  // received, causing that NACK to be sent in response to receiving the last
-  // expected byte of data.
-
-  // build sla+w, slave device address + w bit
-  twi_slarw = TW_READ;
-  twi_slarw |= address << 1;
-
-  if (true == twi_inRepStart) {
-    // if we're in the repeated start state, then we've already sent the start,
-    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
-    // We need to remove ourselves from the repeated start state before we enable interrupts,
-    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
-    // up. Also, don't enable the START interrupt. There may be one pending from the 
-    // repeated start that we sent ourselves, and that would really confuse things.
-    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
-    do {
-      TWDR = twi_slarw;
-    } while(TWCR & _BV(TWWC));
-    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
-  }
-  else
-    // send start condition
-    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
-
-  // wait for read operation to complete
-  while(TWI_MRX == twi_state){
-    continue;
-  }
-
-  if (twi_masterBufferIndex < length)
-    length = twi_masterBufferIndex;
-
-  // copy twi buffer to data
-  for(i = 0; i < length; ++i){
-    data[i] = twi_masterBuffer[i];
-  }
-	
-  return length;
-}
-
-/* 
- * Function twi_writeTo
- * Desc     attempts to become twi bus master and write a
- *          series of bytes to a device on the bus
- * Input    address: 7bit i2c device address
- *          data: pointer to byte array
- *          length: number of bytes in array
- *          wait: boolean indicating to wait for write or not
- *          sendStop: boolean indicating whether or not to send a stop at the end
- * Output   0 .. success
- *          1 .. length to long for buffer
- *          2 .. address send, NACK received
- *          3 .. data send, NACK received
- *          4 .. other twi error (lost bus arbitration, bus error, ..)
- */
-uint8_t twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait, uint8_t sendStop)
-{
-  uint8_t i;
-
-  // ensure data will fit into buffer
-  if(TWI_BUFFER_LENGTH < length){
-    return 1;
-  }
-
-  // wait until twi is ready, become master transmitter
-  while(TWI_READY != twi_state){
-    continue;
-  }
-  twi_state = TWI_MTX;
-  twi_sendStop = sendStop;
-  // reset error state (0xFF.. no error occured)
-  twi_error = 0xFF;
-
-  // initialize buffer iteration vars
-  twi_masterBufferIndex = 0;
-  twi_masterBufferLength = length;
-  
-  // copy data to twi buffer
-  for(i = 0; i < length; ++i){
-    twi_masterBuffer[i] = data[i];
-  }
-  
-  // build sla+w, slave device address + w bit
-  twi_slarw = TW_WRITE;
-  twi_slarw |= address << 1;
-  
-  // if we're in a repeated start, then we've already sent the START
-  // in the ISR. Don't do it again.
-  //
-  if (true == twi_inRepStart) {
-    // if we're in the repeated start state, then we've already sent the start,
-    // (@@@ we hope), and the TWI statemachine is just waiting for the address byte.
-    // We need to remove ourselves from the repeated start state before we enable interrupts,
-    // since the ISR is ASYNC, and we could get confused if we hit the ISR before cleaning
-    // up. Also, don't enable the START interrupt. There may be one pending from the 
-    // repeated start that we sent outselves, and that would really confuse things.
-    twi_inRepStart = false;			// remember, we're dealing with an ASYNC ISR
-    do {
-      TWDR = twi_slarw;				
-    } while(TWCR & _BV(TWWC));
-    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE);	// enable INTs, but not START
-  }
-  else
-    // send start condition
-    TWCR = _BV(TWINT) | _BV(TWEA) | _BV(TWEN) | _BV(TWIE) | _BV(TWSTA);	// enable INTs
-
-  // wait for write operation to complete
-  while(wait && (TWI_MTX == twi_state)){
-    continue;
-  }
-  
-  if (twi_error == 0xFF)
-    return 0;	// success
-  else if (twi_error == TW_MT_SLA_NACK)
-    return 2;	// error: address send, nack received
-  else if (twi_error == TW_MT_DATA_NACK)
-    return 3;	// error: data send, nack received
-  else
-    return 4;	// other twi error
+    while(!(TWCR & _BV(TWINT)));
+    return (TW_STATUS != status);
 }
 
-/* 
- * Function twi_transmit
- * Desc     fills slave tx buffer with data
- *          must be called in slave tx event callback
- * Input    data: pointer to byte array
- *          length: number of bytes in array
- * Output   1 length too long for buffer
- *          2 not slave transmitter
- *          0 ok
- */
-uint8_t twi_transmit(const uint8_t* data, uint8_t length)
+uint8_t twi_rw8(uint8_t address, uint8_t mode, uint8_t* data)
 {
-  uint8_t i;
+  // send start condition
+  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTA);
+  if(twi_waitfor(TW_START))
+      return 1;
+
+  // send address
+  TWDR = mode;
+  TWDR |= (address << 1);
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_waitfor(mode == TW_READ? TW_MR_SLA_ACK: TW_MT_SLA_ACK))
+      return 2;
+
+  // send or receive data
+  if(mode == TW_WRITE)
+      TWDR = *data;
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_waitfor(mode == TW_READ? TW_MR_DATA_NACK: TW_MT_DATA_ACK))
+      return 3;
+  if(mode == TW_READ)
+      *data = TWDR;
+
+  // send stop
+  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTO);
 
-  // ensure data will fit into buffer
-  if(TWI_BUFFER_LENGTH < (twi_txBufferLength+length)){
-    return 1;
-  }
-  
-  // ensure we are currently a slave transmitter
-  if(TWI_STX != twi_state){
-    return 2;
-  }
-  
-  // set length and copy data into tx buffer
-  for(i = 0; i < length; ++i){
-    twi_txBuffer[twi_txBufferLength+i] = data[i];
-  }
-  twi_txBufferLength += length;
-  
   return 0;
 }
-
-/* 
- * Function twi_attachSlaveRxEvent
- * Desc     sets function called before a slave read operation
- * Input    function: callback function to use
- * Output   none
- */
-void twi_attachSlaveRxEvent( void (*function)(uint8_t*, int) )
-{
-  twi_onSlaveReceive = function;
-}
-
-/* 
- * Function twi_attachSlaveTxEvent
- * Desc     sets function called before a slave write operation
- * Input    function: callback function to use
- * Output   none
- */
-void twi_attachSlaveTxEvent( void (*function)(void) )
-{
-  twi_onSlaveTransmit = function;
-}
-
-/* 
- * Function twi_reply
- * Desc     sends byte or readys receive line
- * Input    ack: byte indicating to ack or to nack
- * Output   none
- */
-void twi_reply(uint8_t ack)
-{
-  // transmit master read ready signal, with or without ack
-  if(ack){
-    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
-  }else{
-	  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
-  }
-}
-
-/* 
- * Function twi_stop
- * Desc     relinquishes bus master status
- * Input    none
- * Output   none
- */
-void twi_stop(void)
-{
-  // send stop condition
-  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);
-
-  // wait for stop condition to be exectued on bus
-  // TWINT is not set after a stop condition!
-  while(TWCR & _BV(TWSTO)){
-    continue;
-  }
-
-  // update twi state
-  twi_state = TWI_READY;
-}
-
-/* 
- * Function twi_releaseBus
- * Desc     releases bus control
- * Input    none
- * Output   none
- */
-void twi_releaseBus(void)
-{
-  // release bus
-  TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);
-
-  // update twi state
-  twi_state = TWI_READY;
-}
-
-ISR(TWI_vect)
-{
-  switch(TW_STATUS){
-    // All Master
-    case TW_START:     // sent start condition
-    case TW_REP_START: // sent repeated start condition
-      // copy device address and r/w bit to output register and ack
-      TWDR = twi_slarw;
-      twi_reply(1);
-      break;
-
-    // Master Transmitter
-    case TW_MT_SLA_ACK:  // slave receiver acked address
-    case TW_MT_DATA_ACK: // slave receiver acked data
-      // if there is data to send, send it, otherwise stop 
-      if(twi_masterBufferIndex < twi_masterBufferLength){
-        // copy data to output register and ack
-        TWDR = twi_masterBuffer[twi_masterBufferIndex++];
-        twi_reply(1);
-      }else{
-	if (twi_sendStop)
-          twi_stop();
-	else {
-	  twi_inRepStart = true;	// we're gonna send the START
-	  // don't enable the interrupt. We'll generate the start, but we 
-	  // avoid handling the interrupt until we're in the next transaction,
-	  // at the point where we would normally issue the start.
-	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
-	  twi_state = TWI_READY;
-	}
-      }
-      break;
-    case TW_MT_SLA_NACK:  // address sent, nack received
-      twi_error = TW_MT_SLA_NACK;
-      twi_stop();
-      break;
-    case TW_MT_DATA_NACK: // data sent, nack received
-      twi_error = TW_MT_DATA_NACK;
-      twi_stop();
-      break;
-    case TW_MT_ARB_LOST: // lost bus arbitration
-      twi_error = TW_MT_ARB_LOST;
-      twi_releaseBus();
-      break;
-
-    // Master Receiver
-    case TW_MR_DATA_ACK: // data received, ack sent
-      // put byte into buffer
-      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
-    case TW_MR_SLA_ACK:  // address sent, ack received
-      // ack if more bytes are expected, otherwise nack
-      if(twi_masterBufferIndex < twi_masterBufferLength){
-        twi_reply(1);
-      }else{
-        twi_reply(0);
-      }
-      break;
-    case TW_MR_DATA_NACK: // data received, nack sent
-      // put final byte into buffer
-      twi_masterBuffer[twi_masterBufferIndex++] = TWDR;
-	if (twi_sendStop)
-          twi_stop();
-	else {
-	  twi_inRepStart = true;	// we're gonna send the START
-	  // don't enable the interrupt. We'll generate the start, but we 
-	  // avoid handling the interrupt until we're in the next transaction,
-	  // at the point where we would normally issue the start.
-	  TWCR = _BV(TWINT) | _BV(TWSTA)| _BV(TWEN) ;
-	  twi_state = TWI_READY;
-	}    
-	break;
-    case TW_MR_SLA_NACK: // address sent, nack received
-      twi_stop();
-      break;
-    // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case
-
-    // Slave Receiver
-    case TW_SR_SLA_ACK:   // addressed, returned ack
-    case TW_SR_GCALL_ACK: // addressed generally, returned ack
-    case TW_SR_ARB_LOST_SLA_ACK:   // lost arbitration, returned ack
-    case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack
-      // enter slave receiver mode
-      twi_state = TWI_SRX;
-      // indicate that rx buffer can be overwritten and ack
-      twi_rxBufferIndex = 0;
-      twi_reply(1);
-      break;
-    case TW_SR_DATA_ACK:       // data received, returned ack
-    case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack
-      // if there is still room in the rx buffer
-      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
-        // put byte in buffer and ack
-        twi_rxBuffer[twi_rxBufferIndex++] = TWDR;
-        twi_reply(1);
-      }else{
-        // otherwise nack
-        twi_reply(0);
-      }
-      break;
-    case TW_SR_STOP: // stop or repeated start condition received
-      // ack future responses and leave slave receiver state
-      twi_releaseBus();
-      // put a null char after data if there's room
-      if(twi_rxBufferIndex < TWI_BUFFER_LENGTH){
-        twi_rxBuffer[twi_rxBufferIndex] = '\0';
-      }
-      // callback to user defined callback
-      twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex);
-      // since we submit rx buffer to "wire" library, we can reset it
-      twi_rxBufferIndex = 0;
-      break;
-    case TW_SR_DATA_NACK:       // data received, returned nack
-    case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack
-      // nack back at master
-      twi_reply(0);
-      break;
-    
-    // Slave Transmitter
-    case TW_ST_SLA_ACK:          // addressed, returned ack
-    case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack
-      // enter slave transmitter mode
-      twi_state = TWI_STX;
-      // ready the tx buffer index for iteration
-      twi_txBufferIndex = 0;
-      // set tx buffer length to be zero, to verify if user changes it
-      twi_txBufferLength = 0;
-      // request for txBuffer to be filled and length to be set
-      // note: user must call twi_transmit(bytes, length) to do this
-      twi_onSlaveTransmit();
-      // if they didn't change buffer & length, initialize it
-      if(0 == twi_txBufferLength){
-        twi_txBufferLength = 1;
-        twi_txBuffer[0] = 0x00;
-      }
-      // transmit first byte from buffer, fall
-    case TW_ST_DATA_ACK: // byte sent, ack returned
-      // copy data to output register
-      TWDR = twi_txBuffer[twi_txBufferIndex++];
-      // if there is more to send, ack, otherwise nack
-      if(twi_txBufferIndex < twi_txBufferLength){
-        twi_reply(1);
-      }else{
-        twi_reply(0);
-      }
-      break;
-    case TW_ST_DATA_NACK: // received nack, we are done 
-    case TW_ST_LAST_DATA: // received ack, but we are done already!
-      // ack future responses
-      twi_reply(1);
-      // leave slave receiver state
-      twi_state = TWI_READY;
-      break;
-
-    // All
-    case TW_NO_INFO:   // no state information
-      break;
-    case TW_BUS_ERROR: // bus error, illegal stop/start
-      twi_error = TW_BUS_ERROR;
-      twi_stop();
-      break;
-  }
-}
-
diff --git a/Firmware/twi.h b/Firmware/twi.h
index cb48708c4c..abcb8e9750 100644
--- a/Firmware/twi.h
+++ b/Firmware/twi.h
@@ -1,5 +1,5 @@
 /*
-  twi.h - TWI/I2C library for Wiring & Arduino
+  twi.h - Stripped-down TWI/I2C library
   Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
 
   This library is free software; you can redistribute it and/or
@@ -17,39 +17,37 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
-#ifndef twi_h
-#define twi_h
-
-  #include <inttypes.h>
-
-  //#define ATMEGA8
-
-  #ifndef TWI_FREQ
-  #define TWI_FREQ 400000L
-  #endif
-
-  #ifndef TWI_BUFFER_LENGTH
-  #define TWI_BUFFER_LENGTH 32
-  #endif
-
-  #define TWI_READY 0
-  #define TWI_MRX   1
-  #define TWI_MTX   2
-  #define TWI_SRX   3
-  #define TWI_STX   4
-  
-  void twi_init(void);
-  void twi_disable(void);
-  void twi_setAddress(uint8_t);
-  void twi_setFrequency(uint32_t);
-  uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t);
-  uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t);
-  uint8_t twi_transmit(const uint8_t*, uint8_t);
-  void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) );
-  void twi_attachSlaveTxEvent( void (*)(void) );
-  void twi_reply(uint8_t);
-  void twi_stop(void);
-  void twi_releaseBus(void);
+#pragma once
 
+#include <inttypes.h>
+#include <compat/twi.h>
+
+#ifndef TWI_FREQ
+#define TWI_FREQ 400000L
 #endif
 
+/*
+ * Function twi_init
+ * Desc     readys twi pins and sets twi bitrate
+ * Input    none
+ * Output   none
+ */
+void twi_init(void);
+
+/*
+ * Function twi_disable
+ * Desc     disables twi pins
+ * Input    none
+ * Output   none
+ */
+void twi_disable(void);
+
+/*
+ * Function twi_rw8
+ * Desc     read/write a single byte from a device
+ * Input    address: 7bit i2c device address
+ *          mode: TW_READ or TW_WRITE
+ *          data: pointer to byte
+ * Output   0 on success
+ */
+uint8_t twi_rw8(uint8_t address, uint8_t mode, uint8_t* data);

From 30e7b777e03054417f23d161b9a30df3b96e89ee Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Thu, 20 Aug 2020 22:13:13 +0200
Subject: [PATCH 04/14] Error-out with PAT9125_SWSPI (not fully implemented)

.. and likely will never was/be.
---
 Firmware/pat9125.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index ab6342aaac..f47d676bbe 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -110,7 +110,7 @@ uint8_t pat9125_probe()
 {
 #if defined(PAT9125_SWSPI)
     swspi_init();
-    //#error not implemented
+  #error not implemented
 #elif defined(PAT9125_SWI2C)
     swi2c_init();
     return swi2c_readByte_A8(PAT9125_I2C_ADDR,0x00,NULL);

From d8a88379385504241747aca0877076049992e3dc Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 25 Aug 2020 11:31:35 +0200
Subject: [PATCH 05/14] Document the 3 possible modes

---
 Firmware/config.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/Firmware/config.h b/Firmware/config.h
index b107d0ec5f..c64bd441e3 100644
--- a/Firmware/config.h
+++ b/Firmware/config.h
@@ -30,9 +30,9 @@
 #define SWI2C_TMO         2048 //2048 cycles timeout
 
 //PAT9125 configuration
-//#define PAT9125_SWSPI
-//#define PAT9125_SWI2C
-#define PAT9125_I2C
+//#define PAT9125_SWSPI // software SPI mode (incomplete)
+//#define PAT9125_SWI2C // software I2C mode
+#define PAT9125_I2C     // hardware I2C mode
 
 #define PAT9125_I2C_ADDR  0x75  //ID=LO
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI

From e37cdab38fd0a8df2bcad4eca3e410e7b9de148f Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 25 Aug 2020 11:58:48 +0200
Subject: [PATCH 06/14] PAT9125_I2C: accept either NACK or ACK in receive

Both would be technically correct.
---
 Firmware/twi.c | 38 ++++++++++++++++++++++++++++----------
 1 file changed, 28 insertions(+), 10 deletions(-)

diff --git a/Firmware/twi.c b/Firmware/twi.c
index 161db52ac4..11c708aa73 100644
--- a/Firmware/twi.c
+++ b/Firmware/twi.c
@@ -57,34 +57,52 @@ void twi_disable(void)
   digitalWrite(SCL, 0);
 }
 
-uint8_t twi_waitfor(uint8_t status)
+static void twi_wait()
 {
     while(!(TWCR & _BV(TWINT)));
-    return (TW_STATUS != status);
 }
 
 uint8_t twi_rw8(uint8_t address, uint8_t mode, uint8_t* data)
 {
   // send start condition
   TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTA);
-  if(twi_waitfor(TW_START))
+  twi_wait();
+  if(TW_STATUS != TW_START)
       return 1;
 
   // send address
   TWDR = mode;
   TWDR |= (address << 1);
   TWCR = _BV(TWEN) | _BV(TWINT);
-  if(twi_waitfor(mode == TW_READ? TW_MR_SLA_ACK: TW_MT_SLA_ACK))
-      return 2;
+  twi_wait();
 
-  // send or receive data
   if(mode == TW_WRITE)
+  {
+      if(TW_STATUS != TW_MT_SLA_ACK)
+          return 2;
+
+      // send data
       TWDR = *data;
-  TWCR = _BV(TWEN) | _BV(TWINT);
-  if(twi_waitfor(mode == TW_READ? TW_MR_DATA_NACK: TW_MT_DATA_ACK))
-      return 3;
-  if(mode == TW_READ)
+      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);

From 6d476d71447e707df2ee5b3c367a650235357f9b Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sun, 27 Sep 2020 14:29:07 +0200
Subject: [PATCH 07/14] Still use SWI2C on RAMBo10a boards

The wiring for the PAT9125 on RAMBo10a boards is not directly connected
to the SCL pin and requires the sw mode.

Detect this requirement by checking the definition for the SWI2C_SCL pin
in the board definition.

Remove SWI2C_SCL/SDA from the other boards to use the HW mode.
---
 Firmware/config.h         | 5 ++++-
 Firmware/pins_Einsy_1_0.h | 6 ------
 Firmware/pins_Rambo_1_3.h | 3 ---
 Firmware/swi2c.c          | 3 +++
 4 files changed, 7 insertions(+), 10 deletions(-)

diff --git a/Firmware/config.h b/Firmware/config.h
index c64bd441e3..922a53989c 100644
--- a/Firmware/config.h
+++ b/Firmware/config.h
@@ -31,8 +31,11 @@
 
 //PAT9125 configuration
 //#define PAT9125_SWSPI // software SPI mode (incomplete)
-//#define PAT9125_SWI2C // software I2C mode
+#ifdef SWI2C_SCL
+#define PAT9125_SWI2C   // software I2C mode
+#else
 #define PAT9125_I2C     // hardware I2C mode
+#endif
 
 #define PAT9125_I2C_ADDR  0x75  //ID=LO
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI
diff --git a/Firmware/pins_Einsy_1_0.h b/Firmware/pins_Einsy_1_0.h
index 14b5623374..21578ebd0f 100755
--- a/Firmware/pins_Einsy_1_0.h
+++ b/Firmware/pins_Einsy_1_0.h
@@ -18,12 +18,6 @@
 #define W25X20CL                 // external 256kB flash
 #define BOOTAPP                  // bootloader support
 
-
-#define SWI2C_SDA      20 //SDA on P3
-#define SWI2C_SCL      21 //SCL on P3
-
-
-
 #define X_TMC2130_CS           41
 #define X_TMC2130_DIAG         64 // !!! changed from 40 (EINY03)
 #define X_STEP_PIN             37
diff --git a/Firmware/pins_Rambo_1_3.h b/Firmware/pins_Rambo_1_3.h
index 538fb4f351..522ad28f80 100644
--- a/Firmware/pins_Rambo_1_3.h
+++ b/Firmware/pins_Rambo_1_3.h
@@ -11,9 +11,6 @@
 
 #define PINDA_THERMISTOR
 
-#define SWI2C_SDA      20 //SDA on P3
-#define SWI2C_SCL      21 //SCL on P3
-
 #ifdef MICROMETER_LOGGING
 #define D_DATACLOCK		24	//Y_MAX (green)
 #define D_DATA			30	//X_MAX (blue)
diff --git a/Firmware/swi2c.c b/Firmware/swi2c.c
index 49fbc5efc7..62a28e1a9d 100644
--- a/Firmware/swi2c.c
+++ b/Firmware/swi2c.c
@@ -7,6 +7,7 @@
 #include "pins.h"
 #include "io_atmega2560.h"
 
+#ifdef SWI2C_SCL
 
 #define SWI2C_RMSK   0x01 //read mask (bit0 = 1)
 #define SWI2C_WMSK   0x00 //write mask (bit0 = 0)
@@ -187,3 +188,5 @@ uint8_t swi2c_writeByte_A16(uint8_t dev_addr, unsigned short addr, uint8_t* pbyt
 }
 
 #endif //SWI2C_A16
+
+#endif //SWI2C_SCL

From 384f40956c4292c33ec087e90f9dfa6da5383800 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sun, 27 Sep 2020 16:42:20 +0200
Subject: [PATCH 08/14] Remove obsolete cbi/sbi

---
 Firmware/twi.c | 11 +----------
 1 file changed, 1 insertion(+), 10 deletions(-)

diff --git a/Firmware/twi.c b/Firmware/twi.c
index 11c708aa73..f8c077abb8 100644
--- a/Firmware/twi.c
+++ b/Firmware/twi.c
@@ -22,14 +22,6 @@
 #include <math.h>
 #include "Arduino.h" // for digitalWrite
 
-#ifndef cbi
-#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
-#endif
-
-#ifndef sbi
-#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
-#endif
-
 #include "twi.h"
 
 
@@ -40,8 +32,7 @@ void twi_init(void)
   digitalWrite(SCL, 1);
 
   // initialize twi prescaler and bit rate
-  cbi(TWSR, TWPS0);
-  cbi(TWSR, TWPS1);
+  TWSR &= ~(_BV(TWPS0) | _BV(TWPS1));
   TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
 
   /* twi bit rate formula from atmega128 manual pg 204

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 09/14] 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     | 100 ++++++++++++++++++++++++++++++---------------
 Firmware/twi.h     |  20 ++++++---
 3 files changed, 84 insertions(+), 42 deletions(-)

diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index f47d676bbe..c6ffecf3ce 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 f8c077abb8..6dd1645c08 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;
+  // send register
+  TWDR = reg;
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MT_DATA_ACK))
+      return 3;
 
-      // receive data
-      TWCR = _BV(TWEN) | _BV(TWINT);
-      twi_wait();
+  return 0;
+}
 
-      // accept ACK or NACK (since only 1 byte is read)
-      if(!(TW_STATUS & TW_MR_DATA_ACK))
-          return 3;
 
-      *data = TWDR;
-  }
+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
-  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTO);
+  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 abcb8e9750..bdb617fcb5 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);

From df824414eface1c89175eb23e41ca06210bda334 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Mon, 28 Sep 2020 21:02:06 +0200
Subject: [PATCH 10/14] Fix probing in IR_SENSOR

---
 Firmware/pat9125.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index c6ffecf3ce..58308a9a75 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -118,8 +118,8 @@ uint8_t pat9125_probe()
     twi_init();
   #ifdef IR_SENSOR
     // NOTE: this is called from the MK3S variant, so it should be kept minimal
-    uint8_t addr = PAT9125_PID1;
-    return (twi_rw8(PAT9125_I2C_ADDR,TW_READ,&addr) == 0);
+    uint8_t data;
+    return (twi_r8(PAT9125_I2C_ADDR,PAT9125_PID1,&data) == 0);
   #else
     return (pat9125_rd_reg(PAT9125_PID1) != 0);
   #endif

From 30262b0a6eeecade45186a5d708f0c94d4cff20f Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Fri, 29 Jan 2021 17:30:04 +0100
Subject: [PATCH 11/14] Remove redundant definitions of CRITICAL_SECTION_*

Move CRITICAL_SECTION_START/END into fastio.h, where it's needed.
---
 Firmware/Marlin.h | 5 -----
 Firmware/fastio.h | 7 +++++++
 Firmware/tone04.c | 9 ---------
 3 files changed, 7 insertions(+), 14 deletions(-)

diff --git a/Firmware/Marlin.h b/Firmware/Marlin.h
index 697f2f72c7..f81abd8f76 100755
--- a/Firmware/Marlin.h
+++ b/Firmware/Marlin.h
@@ -287,11 +287,6 @@ FORCE_INLINE unsigned long millis_nc() {
 void setPwmFrequency(uint8_t pin, int val);
 #endif
 
-#ifndef CRITICAL_SECTION_START
-  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
-  #define CRITICAL_SECTION_END    SREG = _sreg;
-#endif //CRITICAL_SECTION_START
-
 extern bool fans_check_enabled;
 extern float homing_feedrate[];
 extern uint8_t axis_relative_modes;
diff --git a/Firmware/fastio.h b/Firmware/fastio.h
index e4f25ed8b5..c4fee1019d 100644
--- a/Firmware/fastio.h
+++ b/Firmware/fastio.h
@@ -17,6 +17,13 @@
 #define MASK(PIN)  (1 << PIN)
 #endif
 
+#ifndef CRITICAL_SECTION_START
+  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
+  #define CRITICAL_SECTION_END    SREG = _sreg;
+  #include <avr/interrupt.h>
+#endif //CRITICAL_SECTION_START
+
+
 /*
   magic I/O routines
   now you can simply SET_OUTPUT(STEP); WRITE(STEP, 1); WRITE(STEP, 0);
diff --git a/Firmware/tone04.c b/Firmware/tone04.c
index 41f904a915..5c36a53704 100644
--- a/Firmware/tone04.c
+++ b/Firmware/tone04.c
@@ -7,16 +7,7 @@
 
 #ifdef SYSTEM_TIMER_2
 
-#include <avr/io.h>
-#include <avr/interrupt.h>
 #include "pins.h"
-
-#ifndef CRITICAL_SECTION_START
-	#define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
-	#define CRITICAL_SECTION_END    SREG = _sreg;
-#endif //CRITICAL_SECTION_START
-
-
 #include "fastio.h"
 
 void timer4_init(void)

From 1fa7b8cd8d0f04b0332dd242283c57caf600d103 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Fri, 29 Jan 2021 17:48:59 +0100
Subject: [PATCH 12/14] Move SDA/SCL pins into pins.h for fastio compatibility

fastio relies on macros for pin definitions, so we cannot use the const
declaration in Sd2PinMap or the arduino's definition.

Declare SDA/SCL_PIN into pins.h based on the current MCU, which is
identical in all our variants.

Remove the conflicting/unused declaration in Sd2PinMap.
---
 Firmware/Sd2PinMap.h | 6 +-----
 Firmware/pins.h      | 5 +++++
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/Firmware/Sd2PinMap.h b/Firmware/Sd2PinMap.h
index 8a608731ee..da50958f0f 100644
--- a/Firmware/Sd2PinMap.h
+++ b/Firmware/Sd2PinMap.h
@@ -37,10 +37,6 @@ struct pin_map_t {
 || defined(__AVR_ATmega2560__)
 // Mega
 
-// Two Wire (aka I2C) ports
-uint8_t const SDA_PIN = 20;  // D1
-uint8_t const SCL_PIN = 21;  // D0
-
 #undef MOSI_PIN
 #undef MISO_PIN
 // SPI port
@@ -365,4 +361,4 @@ static inline __attribute__((always_inline))
 #endif  // Sd2PinMap_h
 
 
-#endif
\ No newline at end of file
+#endif
diff --git a/Firmware/pins.h b/Firmware/pins.h
index 5d3b4f8326..1c20a001a7 100644
--- a/Firmware/pins.h
+++ b/Firmware/pins.h
@@ -25,6 +25,11 @@
 #error Unknown MOTHERBOARD value in configuration.h
 #endif
 
+#if !defined(SDA_PIN) && (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__))
+#define SDA_PIN 20
+#define SCL_PIN 21
+#endif
+
 //List of pins which to ignore when asked to change by gcode, 0 and 1 are RX and TX, do not mess with those!
 #define _E0_PINS E0_STEP_PIN, E0_DIR_PIN, E0_ENABLE_PIN, HEATER_0_PIN,
 #if EXTRUDERS > 1

From 2d71a071f0d4f63c4dbc2bfa56294c497c5988da Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Fri, 29 Jan 2021 17:51:38 +0100
Subject: [PATCH 13/14] Switch twi.c to fastio

---
 Firmware/twi.c | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/Firmware/twi.c b/Firmware/twi.c
index 6dd1645c08..e8c9a378e2 100644
--- a/Firmware/twi.c
+++ b/Firmware/twi.c
@@ -19,17 +19,18 @@
   Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
 */
 
-#include <math.h>
-#include "Arduino.h" // for digitalWrite
 
+#include <math.h>
+#include "config.h"
+#include "fastio.h"
 #include "twi.h"
 
 
 void twi_init(void)
 {
   // activate internal pullups for twi.
-  digitalWrite(SDA, 1);
-  digitalWrite(SCL, 1);
+  WRITE(SDA_PIN, 1);
+  WRITE(SCL_PIN, 1);
 
   // initialize twi prescaler and bit rate
   TWSR &= ~(_BV(TWPS0) | _BV(TWPS1));
@@ -44,8 +45,8 @@ void twi_init(void)
 void twi_disable(void)
 {
   // deactivate internal pullups for twi.
-  digitalWrite(SDA, 0);
-  digitalWrite(SCL, 0);
+  WRITE(SDA_PIN, 0);
+  WRITE(SCL_PIN, 0);
 }
 
 

From b8b75186fe96a695521402edc1dd015dfc684ffa Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Fri, 29 Jan 2021 18:30:16 +0100
Subject: [PATCH 14/14] Remove the extra copy of CRITICAL_SECTION from fastio

---
 Firmware/fastio.h | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/Firmware/fastio.h b/Firmware/fastio.h
index 94a029a626..855c000e22 100644
--- a/Firmware/fastio.h
+++ b/Firmware/fastio.h
@@ -9,12 +9,6 @@
 #include <avr/io.h>
 #include "macros.h"
 
-#ifndef CRITICAL_SECTION_START
-  #define CRITICAL_SECTION_START  unsigned char _sreg = SREG; cli();
-  #define CRITICAL_SECTION_END    SREG = _sreg;
-  #include <avr/interrupt.h>
-#endif //CRITICAL_SECTION_START
-
 
 /*
   magic I/O routines