From 4ca9f97f516d07d189238d2a247261dfe3afa9a8 Mon Sep 17 00:00:00 2001
From: stonehippo <stonehippo@gmail.com>
Date: Tue, 19 Apr 2022 11:28:45 -0400
Subject: [PATCH] Update Wire peripheral handler

The existing onService method for I2C peripheral (slave) implementation
swapped the sense of onRequest (master read) and onReceive (master
write) handlers. It also didn't handle writes correctly, requiring two
write operations to get bytes out of the buffer and in the onReceive
handler.

This implementation follows the SERCOM I2C flow defined for the
default clock-stretching case (CTRLB.SCLSM=0) as described in the
SAMD21 datasheet.

Make sure the write NACK only happens on a stop
---
 libraries/Wire/Wire.cpp | 129 +++++++++++++++++++++-------------------
 1 file changed, 68 insertions(+), 61 deletions(-)

diff --git a/libraries/Wire/Wire.cpp b/libraries/Wire/Wire.cpp
index bae7b9e6e..9911aacc2 100644
--- a/libraries/Wire/Wire.cpp
+++ b/libraries/Wire/Wire.cpp
@@ -217,67 +217,74 @@ void TwoWire::onRequest(void(*function)(void))
   onRequestCallback = function;
 }
 
-void TwoWire::onService(void)
-{
-  if ( sercom->isSlaveWIRE() )
-  {
-    if(sercom->isStopDetectedWIRE() || 
-        (sercom->isAddressMatch() && sercom->isRestartDetectedWIRE() && !sercom->isMasterReadOperationWIRE())) //Stop or Restart detected
-    {
-      sercom->prepareAckBitWIRE();
-      sercom->prepareCommandBitsWire(0x03);
-
-      //Calling onReceiveCallback, if exists
-      if(onReceiveCallback)
-      {
-        onReceiveCallback(available());
-      }
-      
-      rxBuffer.clear();
-    }
-    else if(sercom->isAddressMatch())  //Address Match
-    {
-      sercom->prepareAckBitWIRE();
-      sercom->prepareCommandBitsWire(0x03);
-
-      if(sercom->isMasterReadOperationWIRE()) //Is a request ?
-      {
-        txBuffer.clear();
-
-        transmissionBegun = true;
-
-        //Calling onRequestCallback, if exists
-        if(onRequestCallback)
-        {
-          onRequestCallback();
-        }
-      }
-    }
-    else if(sercom->isDataReadyWIRE())
-    {
-      if (sercom->isMasterReadOperationWIRE())
-      {
-        uint8_t c = 0xff;
-
-        if( txBuffer.available() ) {
-          c = txBuffer.read_char();
-        }
-
-        transmissionBegun = sercom->sendDataSlaveWIRE(c);
-      } else { //Received data
-        if (rxBuffer.isFull()) {
-          sercom->prepareNackBitWIRE(); 
-        } else {
-          //Store data
-          rxBuffer.store_char(sercom->readDataWIRE());
-
-          sercom->prepareAckBitWIRE(); 
-        }
-
-        sercom->prepareCommandBitsWire(0x03);
-      }
-    }
-  }
+void TwoWire::onService(void) {
+	if (sercom->isSlaveWIRE()) {		
+		if (sercom->isAddressMatch()) { 				// address match interrupt
+			if (sercom->isMasterReadOperationWIRE()) {	// the master is making a read request
+				sercom->prepareAckBitWIRE();
+				sercom->prepareCommandBitsWire(0x03);
+				
+				txBuffer.clear();
+				transmissionBegun = true;
+				
+				// call the onRequest callback to put bytes in the txBuffer
+				if(onRequestCallback) {
+					onRequestCallback();
+				}
+				
+				while(!sercom->isStopDetectedWIRE()) { // write the bytes out to the master
+					if (sercom->isDataReadyWIRE()) {
+						uint8_t c = 0xff;
+						if (txBuffer.available()) {
+							c = txBuffer.read_char();
+						}
+						
+						sercom->sendDataSlaveWIRE(c);
+					}
+				}
+				
+				// done sending bytes, NACK to shut things down
+				sercom->prepareNackBitWIRE();
+				sercom->prepareCommandBitsWire(0x03);
+
+			} else if (!(sercom->isMasterReadOperationWIRE())) { // acknowledge master write request
+				sercom->prepareAckBitWIRE();
+				sercom->prepareCommandBitsWire(0x03);
+				
+				while(!(sercom->isStopDetectedWIRE() || sercom->isRestartDetectedWIRE())) { // read bytes from the master
+					if(sercom->isDataReadyWIRE()) {
+						if (rxBuffer.isFull()) {
+							sercom->prepareNackBitWIRE();
+							sercom->prepareCommandBitsWire(0x03);
+							break;
+						} else {
+							rxBuffer.store_char(sercom->readDataWIRE());
+							sercom->prepareAckBitWIRE();
+							sercom->prepareCommandBitsWire(0x03);
+						}
+					}
+				}
+				
+				if(sercom->isStopDetectedWIRE() || sercom->isRestartDetectedWIRE()) {
+					if (onReceiveCallback) {
+						onReceiveCallback(available());
+					}
+
+					rxBuffer.clear();
+				}
+
+				if(sercom->isStopDetectedWIRE()) {
+					sercom->prepareNackBitWIRE();
+					sercom->prepareCommandBitsWire(0x03);
+				}
+
+			} else {
+				// do nothing
+			}	
+		} else {
+			// do nothing
+		}
+	}
 }
 
 #if WIRE_INTERFACES_COUNT > 0