From 39c3f8bf613f87d83abb55a716f8569f75bc5c7d Mon Sep 17 00:00:00 2001
From: Matthew Lowe <mattwlowe@gmail.com>
Date: Tue, 30 Jun 2015 22:47:55 -0600
Subject: [PATCH] Support for repeated starts added to SAM Wire library

Additional interface method ported to avr for compatibility
Fix issue #2428.
---
 hardware/arduino/avr/libraries/Wire/Wire.cpp | 24 +++++++++++++++++++-
 hardware/arduino/avr/libraries/Wire/Wire.h   |  1 +
 hardware/arduino/sam/libraries/Wire/Wire.cpp |  8 +++++--
 hardware/arduino/sam/libraries/Wire/Wire.h   |  1 +
 4 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/hardware/arduino/avr/libraries/Wire/Wire.cpp b/hardware/arduino/avr/libraries/Wire/Wire.cpp
index 553add782..835b794d5 100644
--- a/hardware/arduino/avr/libraries/Wire/Wire.cpp
+++ b/hardware/arduino/avr/libraries/Wire/Wire.cpp
@@ -80,8 +80,26 @@ void TwoWire::setClock(uint32_t frequency)
   TWBR = ((F_CPU / frequency) - 16) / 2;
 }
 
-uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop)
+uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop)
 {
+  if (isize > 0) {
+  // send internal address; this mode allows sending a repeated start to access
+  // some devices' internal registers. This function is executed by the hardware
+  // TWI module on other processors (for example Due's TWI_IADR and TWI_MMR registers)
+
+  beginTransmission(address);
+
+  // the maximum size of internal address is 3 bytes
+  if (isize > 3){
+    isize = 3;
+  }
+
+  // write internal register address - most significant byte first
+  while (isize-- > 0)
+    write((uint8_t)(iaddress >> (isize*8)));
+  endTransmission(false);
+  }
+
   // clamp to buffer length
   if(quantity > BUFFER_LENGTH){
     quantity = BUFFER_LENGTH;
@@ -95,6 +113,10 @@ uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop
   return read;
 }
 
+uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop) {
+	return requestFrom((uint8_t)address, (uint8_t)quantity, (uint32_t)0, (uint8_t)0, (uint8_t)sendStop);
+}
+
 uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity)
 {
   return requestFrom((uint8_t)address, (uint8_t)quantity, (uint8_t)true);
diff --git a/hardware/arduino/avr/libraries/Wire/Wire.h b/hardware/arduino/avr/libraries/Wire/Wire.h
index 732bdc314..7d009598e 100644
--- a/hardware/arduino/avr/libraries/Wire/Wire.h
+++ b/hardware/arduino/avr/libraries/Wire/Wire.h
@@ -56,6 +56,7 @@ class TwoWire : public Stream
     uint8_t endTransmission(uint8_t);
     uint8_t requestFrom(uint8_t, uint8_t);
     uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
+	uint8_t requestFrom(uint8_t, uint8_t, uint32_t, uint8_t, uint8_t);
     uint8_t requestFrom(int, int);
     uint8_t requestFrom(int, int, int);
     virtual size_t write(uint8_t);
diff --git a/hardware/arduino/sam/libraries/Wire/Wire.cpp b/hardware/arduino/sam/libraries/Wire/Wire.cpp
index 1e0f4c1e9..696a1ecaa 100644
--- a/hardware/arduino/sam/libraries/Wire/Wire.cpp
+++ b/hardware/arduino/sam/libraries/Wire/Wire.cpp
@@ -131,13 +131,13 @@ void TwoWire::setClock(uint32_t frequency) {
 	TWI_SetClock(twi, twiClock, VARIANT_MCK);
 }
 
-uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop) {
+uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop) {
 	if (quantity > BUFFER_LENGTH)
 		quantity = BUFFER_LENGTH;
 
 	// perform blocking read into buffer
 	int readed = 0;
-	TWI_StartRead(twi, address, 0, 0);
+	TWI_StartRead(twi, address, iaddress, isize);
 	do {
 		// Stop condition must be set during the reception of last byte
 		if (readed + 1 == quantity)
@@ -157,6 +157,10 @@ uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop
 	return readed;
 }
 
+uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop) {
+	return requestFrom((uint8_t) address, (uint8_t) quantity, (uint32_t) 0, (uint8_t) 0, (uint8_t) sendStop);
+}
+
 uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity) {
 	return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) true);
 }
diff --git a/hardware/arduino/sam/libraries/Wire/Wire.h b/hardware/arduino/sam/libraries/Wire/Wire.h
index 9da1e55bc..76cc3d267 100644
--- a/hardware/arduino/sam/libraries/Wire/Wire.h
+++ b/hardware/arduino/sam/libraries/Wire/Wire.h
@@ -42,6 +42,7 @@ public:
     uint8_t endTransmission(uint8_t);
 	uint8_t requestFrom(uint8_t, uint8_t);
     uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
+	uint8_t requestFrom(uint8_t, uint8_t, uint32_t, uint8_t, uint8_t);
 	uint8_t requestFrom(int, int);
     uint8_t requestFrom(int, int, int);
 	virtual size_t write(uint8_t);