From f307bf454d420642d993f6f5654c1b666f0e6bda Mon Sep 17 00:00:00 2001
From: Jonathan BAUDIN <jonathan.baudin@atmel.com>
Date: Mon, 5 May 2014 14:24:10 +0200
Subject: [PATCH]  Complete SERCOM and WIRE classes

---
 cores/arduino/SERCOM.cpp | 192 +++++++++++++++++++++++-
 cores/arduino/SERCOM.h   |  55 ++++++-
 libraries/Wire/Wire.cpp  | 308 +++++++++++++++++++--------------------
 libraries/Wire/Wire.h    |  85 +++++------
 4 files changed, 425 insertions(+), 215 deletions(-)

diff --git a/cores/arduino/SERCOM.cpp b/cores/arduino/SERCOM.cpp
index d37396f8..c69c53a1 100644
--- a/cores/arduino/SERCOM.cpp
+++ b/cores/arduino/SERCOM.cpp
@@ -42,7 +42,7 @@ void initFrame(SercomUartCharSize charSize, SercomDataOrder dataOrder, SercomPar
 	sercom->USART.CTRLA.reg |=	SERCOM_UART_CTRLA_FORM( (parityMode == NO_PARITY ? 0 : 1) ) |
 								dataOrder << SERCOM_UART_CTRLA_DORD_Pos;
 
-	//Setting the CRTLB register
+	//Setting the CTRLB register
 	sercom->USART.CTRLB.reg |=	SERCOM_UART_CTRLB_CHSIZE(charSize) |
 								nbStopBits << SERCOM_UART_CTRLB_SBMODE_Pos |
 								(parityMode == NO_PARITY ? 0 : parityMode) << SERCOM_UART_CTRLB_PMODE_Pos; //If no parity use default value
@@ -54,7 +54,7 @@ void initPads(SercomUartTXPad txPad, SercomRXPad rxPad)
 	sercom->USART.CTRLA.reg |=	SERCOM_UART_CTRLA_TXPO(txPad) |
 								SERCOM_UART_CTRLA_RXPO(rxPad);
 
-	//Setting the CRTLB register (Enabling Transceiver and Receiver)
+	//Setting the CTRLB register (Enabling Transceiver and Receiver)
 	sercom->USART.CTRLB.reg |=	SERCOM_UART_CTRLB_TXEN |
 								SERCOM_UART_CTRLB_RXEN;
 }
@@ -203,7 +203,7 @@ void SERCOM::disableSPI()
 	while(sercom->SPI.SYNCBUSY.bit.ENABLE);
 }
 
-void setDataOrderSPI(SercomDataOrder dataOrder)
+void SERCOM::setDataOrderSPI(SercomDataOrder dataOrder)
 {
 	//Register enable-protected
 	disableSPI();
@@ -213,7 +213,7 @@ void setDataOrderSPI(SercomDataOrder dataOrder)
 	enableSPI();
 }
 
-void setBaudrateSPI(uint8_t divider)
+void SERCOM::setBaudrateSPI(uint8_t divider)
 {
 	//Can't divide by 0
 	if(baudrate == 0)
@@ -227,7 +227,7 @@ void setBaudrateSPI(uint8_t divider)
 	enableSPI();
 }
 
-void setClockModeSPI(SercomSpiClockMode clockMode)
+void SERCOM::setClockModeSPI(SercomSpiClockMode clockMode)
 {
 	int cpha, cpol;
 	if((clockMode & (0x1ul)) == 0)
@@ -248,12 +248,12 @@ void setClockModeSPI(SercomSpiClockMode clockMode)
 	
 	enableSPI();
 }
-void writeDataSPI(uint8_t data)
+void SERCOM::writeDataSPI(uint8_t data)
 {
 	sercom->SPI.DATA.bit.DATA = data;
 }
 
-uint8_t readDataSPI()
+uint8_t SERCOM::readDataSPI()
 {
 	return sercom->SPI.DATA.bit.DATA;
 }
@@ -285,3 +285,181 @@ uint8_t SERCOM::calculateBaudrateSynchronous(uint32_t baudrate)
 {
 	return SERCOM_FREQ_REF / (2 * baudrate) - 1;
 }
+
+
+/*	=========================
+ *	===== Sercom WIRE
+ *	=========================
+*/
+
+void SERCOM::resetWIRE()
+{
+	//I2CM OR I2CS, no matter SWRST is the same bit.
+
+	//Set the Software bit to 1
+	sercom->I2CM.CTRLA.bit.SWRST = 0x1ul;
+
+	//Wait both bits Software Reset from CTRLA and SYNCBUSY are equal to 0
+	while(sercom->I2CM.CTRLA.bit.SWRST || sercom->I2CM.SYNCBUSY.SWRST);
+}
+
+void SERCOM::enableWIRE()
+{
+	//I2CM OR I2CS, no matter ENABLE is the same bit.
+	
+	//Set the enable bit to 1
+	sercom->I2CM.CTRLA.bit.ENABLE = 0x1ul;
+	
+	//Waiting the enable bit from SYNCBUSY is equal to 0;
+	while(sercom->I2CM.SYNCBUSY.bit.ENABLE);
+}
+
+void SERCOM::initSlaveWIRE(uint8_t address)
+{
+	resetWIRE();
+	
+	//Set slave mode
+	sercom->I2CS.CTRLA.bit.MODE = I2C_SLAVE_OPERATION;
+	
+	//Enable Quick Command
+	sercom->I2CM.CTRLB.bit.QCEN = 0x1ul;
+	
+	sercom->I2CS.ADDR.reg = SERCOM_I2CS_ADDR_ADDR(address & 0x7Ful) | // 0x7F, select only 7 bits
+							SERCOM_I2CS_ADDR_ADDRMASK(0x3FFul);		// 0x3FF all bits set
+							
+	//Waiting the SYSOP bit from SYNCBUSY is equal to 0;
+	while(sercom->I2CM.SYNCBUSY.bit.SYSOP);
+}
+
+void SERCOM::initMasterWIRE(uint32_t baudrate)
+{
+	resetWIRE();
+	
+	//Set master mode and set SCL Clock Stretch mode (stretch after ACK bit)
+	sercom->I2CM.CTRLA.reg = 	SERCOM_I2CM_CTRLA_MODE(I2C_MASTER_OPERATION) |
+								SERCOM_I2CM_CTRLA_SCLSM;
+								
+	//Enable Quick Command
+	sercom->I2CM.CTRLB.bit.QCEN = 0x1ul;
+	
+	//Set bus idle mode
+	sercom->I2CM.STATUS.bit.BUSSTATE = IDLE_STATE;
+	
+	//Set all interrupts
+	sercom->I2CM.INTENSET.reg = SERCOM_I2CM_INTENSET_MB |
+								SERCOM_I2CM_INTENSET_SB |
+								SERCOM_I2CM_INTENSET_ERROR;
+	
+	//Synchronous
+	sercom->I2CM.BAUD.bit.BAUD = SERCOM_FREQ_REF / ( 2 * baudrate) - 1;
+	
+	//Waiting the SYSOP bit from SYNCBUSY is equal to 0;
+	while(sercom->I2CM.SYNCBUSY.bit.SYSOP);
+}
+
+void SERCOM::prepareStopBitWIRE()
+{
+	sercom->I2CM.CTRLB.bit.CMD = ACT_STOP;
+}
+
+bool SERCOM::startTransmissionWIRE(uint8_t address, SercomWireReadWriteFlag flag)
+{
+	//7-bits address + 1-bits R/W
+	address = (address << 1) | flag;
+
+	//Wait idle bus mode
+	while(!isBusIdleWIRE());
+	
+	//Send start and address
+	sercom->I2CM.ADDR.bit.ADDR = address;
+	
+	
+	//Address Transmitted
+	while(!sercom->I2CM.INTFLAG.bit.MB);
+	
+	//ACK received (0: ACK, 1: NACK)
+	if(sercom->I2CM.STATUS.bit.RXNACK)
+		return false;
+	else
+		return true;
+}
+
+bool SERCOM::sendDataMasterWIRE(uint8_t data)
+{
+	//Send data
+	sercom->I2CM.DATA.bit.DATA = data;
+	
+	//Wait transmission successful
+	while(!sercom->I2CM.INTFLAG.bit.MB);
+	
+	//Problems on line? nack received?
+	if(sercom->I2CM.STATUS.bit.RXNACK)
+		return false;
+	else
+		return true;
+}
+
+bool SERCOM::sendDataSlaveWIRE(uint8_t data)
+{
+	//Send data
+	sercom->I2CS.DATA.bit.DATA = data;
+	
+	//Wait data transmission successful
+	while(!sercom->I2CS.INTFLAG.bit.DRDY);
+	
+	//Problems on line? nack received?
+	if(sercom->I2CS.STATUS.bit.RXNACK)
+		return false;
+	else
+		return true;
+}
+
+bool SERCOM::isMasterWIRE()
+{
+	return sercom->I2CS.CTRLA.bit.MODE == I2C_MASTER_OPERATION;
+}
+
+bool SERCOM::isSlaveWIRE()
+{
+	return sercom->I2CS.CTRLA.bit.MODE == I2C_SLAVE_OPERATION;
+}
+
+bool SERCOM::isBusIdleWIRE()
+{
+	return sercom->I2CM.STATE.bit.BUSSTATE == IDLE_STATE;
+}
+
+bool SERCOM::isDataReadyWIRE()
+{
+	return sercom->I2CS.INTFLAG.bit.DRDY;
+}
+
+bool SERCOM::isStopDetectedWIRE()
+{
+	return sercom->I2CS.INTFLAG.bit.PREC;
+}
+
+bool SERCOM::isRestartDetectedWIRE()
+{
+	return sercom->I2CS.STATUS.bit.SR;
+}
+
+bool SERCOM::isAddressMatch()
+{
+	return sercom->I2CS.INTFLAG.bit.AMATCH;
+}
+
+bool SERCOM::isMasterReadOperationWIRE()
+{
+	return sercom->I2CS.STATUS.bit.DIR;
+}
+
+int SERCOM::availableWIRE()
+{
+	return rxBuffer.available();
+}
+
+uint8_t SERCOM::readDataWIRE()
+{
+	return sercom->I2CM.DATA.reg;
+}
diff --git a/cores/arduino/SERCOM.h b/cores/arduino/SERCOM.h
index a7df4cf1..e1262d40 100644
--- a/cores/arduino/SERCOM.h
+++ b/cores/arduino/SERCOM.h
@@ -96,11 +96,39 @@ typedef enum
 	9_BITS = 1
 } SercomSpiCharSize;
 
+typedef enum
+{
+	UNKNOWN_STATE = 0,
+	IDLE_STATE,
+	OWNER_STATE,
+	BUSY_STATE
+} SercomWireBusState;
+
+typedef enum
+{
+	WRITE_FLAG = 0,
+	READ_FLAG
+} SercomWireReadWriteFlag;
+
+typedef enum
+{
+	ACT_NO_ACTION = 0,
+	ACT_REPEAT_START,
+	ACT_READ,
+	ACT_STOP
+} SercomMasterCommandWire;
+
+typedef enum
+{
+	ACK_ACTION = 0,
+	NACK_ACTION
+} SercomMasterAckActionWire;
+	
 class SERCOM
 {
 	public:
 		SERCOM(Sercom* sercom);
-	
+	    
 		/* ========== UART ========== */
 		void initUART(SercomUartMode mode, SercomUartSampleRate sampleRate, uint32_t baudrate=0);
 		void initFrame(SercomUartCharSize charSize, SercomDataOrder dataOrder, SercomParityMode parityMode, SercomNumberStopBit nbStopBits);
@@ -117,7 +145,7 @@ class SERCOM
 		bool isDataRegisterEmptyUART()
 		uint8_t readDataUART();
 		int writeDataUART(uint8_t data);
-
+        
 		/* ========== SPI ========== */
 		void initSPI(SercomSpiTXPad mosi, SercomRXPad miso, SercomSpiCharSize charSize, SercomDataOrder dataOrder);
 		void initClock(SercomSpiClockMode clockMode, uint32_t baudrate);
@@ -135,7 +163,28 @@ class SERCOM
 		bool isTransmitCompleteSPI();
 		bool isReceiveCompleteSPI();
 		
-
+		
+		/* ========== WIRE ========== */
+		void initSlaveWIRE(uint8_t address);
+		void initMasterWIRE(uint32_t baudrate);
+		
+		void resetWIRE();
+		void enableWIRE();
+		void prepareStopBitWIRE();
+		bool startTransmissionWIRE(uint8_t address, SercomWireReadWriteFlag flag);
+		bool sendDataMasterWIRE(uint8_t data);
+		bool sendDataSlaveWIRE(uint8_t data);
+		bool isMasterWIRE();
+		bool isSlaveWIRE();
+		bool isBusIdleWIRE();
+		bool isDataReadyWIRE();
+		bool isStopDetectedWIRE();
+		bool isRestartDetectedWIRE();
+		bool isAddressMatch();
+		bool isMasterReadOperationWIRE();
+		int availableWIRE();
+		uint8_t readDataWIRE();
+		
 	private:
 		Sercom* sercom;
 		uint8_t calculateBaudrateSynchronous(uint32_t baudrate);
diff --git a/libraries/Wire/Wire.cpp b/libraries/Wire/Wire.cpp
index cbf46db9..19cc2edd 100644
--- a/libraries/Wire/Wire.cpp
+++ b/libraries/Wire/Wire.cpp
@@ -24,139 +24,56 @@ extern "C" {
 
 #include "Wire.h"
 
-static inline bool TWI_FailedAcknowledge(Twi *pTwi) {
-	return pTwi->TWI_SR & TWI_SR_NACK;
-}
-
-static inline bool TWI_WaitTransferComplete(Twi *_twi, uint32_t _timeout) {
-	while (!TWI_TransferComplete(_twi)) {
-		if (TWI_FailedAcknowledge(_twi))
-			return false;
-		if (--_timeout == 0)
-			return false;
-	}
-	return true;
-}
-
-static inline bool TWI_WaitByteSent(Twi *_twi, uint32_t _timeout) {
-	while (!TWI_ByteSent(_twi)) {
-		if (TWI_FailedAcknowledge(_twi))
-			return false;
-		if (--_timeout == 0)
-			return false;
-	}
-	return true;
-}
-
-static inline bool TWI_WaitByteReceived(Twi *_twi, uint32_t _timeout) {
-	while (!TWI_ByteReceived(_twi)) {
-		if (TWI_FailedAcknowledge(_twi))
-			return false;
-		if (--_timeout == 0)
-			return false;
-	}
-	return true;
-}
-
-static inline bool TWI_STATUS_SVREAD(uint32_t status) {
-	return (status & TWI_SR_SVREAD) == TWI_SR_SVREAD;
-}
-
-static inline bool TWI_STATUS_SVACC(uint32_t status) {
-	return (status & TWI_SR_SVACC) == TWI_SR_SVACC;
-}
-
-static inline bool TWI_STATUS_GACC(uint32_t status) {
-	return (status & TWI_SR_GACC) == TWI_SR_GACC;
-}
-
-static inline bool TWI_STATUS_EOSACC(uint32_t status) {
-	return (status & TWI_SR_EOSACC) == TWI_SR_EOSACC;
-}
-
-static inline bool TWI_STATUS_NACK(uint32_t status) {
-	return (status & TWI_SR_NACK) == TWI_SR_NACK;
-}
-
-TwoWire::TwoWire(Twi *_twi, void(*_beginCb)(void)) :
-	twi(_twi), rxBufferIndex(0), rxBufferLength(0), txAddress(0),
-			txBufferLength(0), srvBufferIndex(0), srvBufferLength(0), status(
-					UNINITIALIZED), onBeginCallback(_beginCb) {
-	// Empty
+TwoWire::TwoWire(SERCOM * sercom)
+{
+	this->sercom = sercom;
+	transmissionBegun = false;
 }
 
 void TwoWire::begin(void) {
-	if (onBeginCallback)
-		onBeginCallback();
-
-	// Disable PDC channel
-	twi->TWI_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS;
-
-	TWI_ConfigureMaster(twi, TWI_CLOCK, VARIANT_MCK);
-	status = MASTER_IDLE;
+	//Master Mode
+	sercom->initMasterWIRE(TWI_CLOCK);
+	sercom->enableWIRE();
 }
 
 void TwoWire::begin(uint8_t address) {
-	if (onBeginCallback)
-		onBeginCallback();
-
-	// Disable PDC channel
-	twi->TWI_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS;
-
-	TWI_ConfigureSlave(twi, address);
-	status = SLAVE_IDLE;
-	TWI_EnableIt(twi, TWI_IER_SVACC);
-	//| TWI_IER_RXRDY | TWI_IER_TXRDY	| TWI_IER_TXCOMP);
-}
-
-void TwoWire::begin(int address) {
-	begin((uint8_t) address);
-}
-
-uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint8_t sendStop) {
-	if (quantity > BUFFER_LENGTH)
-		quantity = BUFFER_LENGTH;
-
-	// perform blocking read into buffer
-	int readed = 0;
-	TWI_StartRead(twi, address, 0, 0);
-	do {
-		// Stop condition must be set during the reception of last byte
-		if (readed + 1 == quantity)
-			TWI_SendSTOPCondition( twi);
-
-		TWI_WaitByteReceived(twi, RECV_TIMEOUT);
-		rxBuffer[readed++] = TWI_ReadByte(twi);
-	} while (readed < quantity);
-	TWI_WaitTransferComplete(twi, RECV_TIMEOUT);
-
-	// set rx buffer iterator vars
-	rxBufferIndex = 0;
-	rxBufferLength = readed;
-
-	return readed;
-}
-
-uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity) {
-	return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) true);
+	//Slave mode
+	sercom->initSlaveWIRE(address);
+	sercom->enableWIRE();
 }
 
-uint8_t TwoWire::requestFrom(int address, int quantity) {
-	return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) true);
+size_t SERCOM::resquestFrom(uint8_t address, size_t quantity, bool stopBit)
+{
+	//Quantity > 0 AND startTransmission worked ?
+	if(quantity == 0 || !sercom->startTransmissionWIRE(address, READ_FLAG))
+		return 0;
+		
+	for(size_t readed = 0; read < quantity; ++readed)
+	{
+		//Prepare stop bit ? user want stop bit ?
+		if(quantity - read == 1 && stopBit)
+			sercom->prepareStopBitWIRE();
+			
+		rxBuffer.store_char(sercom->readDataWIRE());
+	}
+	
+	return quantity;
 }
 
-uint8_t TwoWire::requestFrom(int address, int quantity, int sendStop) {
-	return requestFrom((uint8_t) address, (uint8_t) quantity, (uint8_t) sendStop);
+size_t SERCOM::resquestFrom(uint8_t address, size_t quantity)
+{
+	return requestFrom(address, quantity, true);
 }
 
 void TwoWire::beginTransmission(uint8_t address) {
-	status = MASTER_SEND;
-
-	// save address of target and empty buffer
+	// save address of target and clear buffer
 	txAddress = address;
-	txBufferLength = 0;
+	txBuffer.clear();
+	
+	transmissionBegun = true;
 }
 
+/*
 void TwoWire::beginTransmission(int address) {
 	beginTransmission((uint8_t) address);
 }
@@ -174,7 +91,8 @@ void TwoWire::beginTransmission(int address) {
 //	no call to endTransmission(true) is made. Some I2C
 //	devices will behave oddly if they do not see a STOP.
 //
-uint8_t TwoWire::endTransmission(uint8_t sendStop) {
+uint8_t TwoWire::endTransmission(uint8_t sendStop)
+{
 	// transmit buffer (blocking)
 	TWI_StartWrite(twi, txAddress, 0, 0, txBuffer[0]);
 	TWI_WaitByteSent(twi, XMIT_TIMEOUT);
@@ -192,76 +110,149 @@ uint8_t TwoWire::endTransmission(uint8_t sendStop) {
 	status = MASTER_IDLE;
 	return sent;
 }
+*/
 
-//	This provides backwards compatibility with the original
-//	definition, and expected behaviour, of endTransmission
-//
-uint8_t TwoWire::endTransmission(void)
+// Errors:
+//	0 : Success
+//	1 : Data too long
+//	2 : NACK on transmit of address
+//	3 : NACK on transmit of data
+//	4 : Other error
+uint8_t TwoWire::endTransmission(bool stopBit)
+{
+	transmissionBegun = false;
+	
+	//Check if there are data to send
+	if(!txBuffer.available())
+		return 4;
+		
+	//Start I2C transmission
+	if(!sercom->startTransmissionWIRE(txAddress, WRITE_FLAG))
+		return 2;	//Address error
+	
+	//Send all buffer
+	while(txBuffer.available())
+	{
+		//If is the last data, send STOP bit after it.
+		if(txBuffer.available() == 1)
+			sercom->prepareStopBitWIRE();
+		
+		//Trying to send data
+		if(!sercom->sendDataMasterWIRE(txBuffer.read_char()))
+			return 3;	//Nack or error
+	}
+	
+	return 0;
+}
+
+uint8_t TwoWire::endTransmission()
 {
 	return endTransmission(true);
 }
 
-size_t TwoWire::write(uint8_t data) {
-	if (status == MASTER_SEND) {
-		if (txBufferLength >= BUFFER_LENGTH)
-			return 0;
-		txBuffer[txBufferLength++] = data;
-		return 1;
-	} else {
-		if (srvBufferLength >= BUFFER_LENGTH)
+size_t TwoWire::write(uint8_t data) 
+{
+	if(sercom->isMasterWIRE())
+	{
+		//No writing, without begun transmission or a full buffer
+		if(!transmissionBegun || txBuffer.isFull())
 			return 0;
-		srvBuffer[srvBufferLength++] = data;
+			
+		txBuffer.store_char(data);
 		return 1;
 	}
+	else
+	{
+		sercom->sendDataSlaveWIRE(data);
+	}
 }
 
-size_t TwoWire::write(const uint8_t *data, size_t quantity) {
-	if (status == MASTER_SEND) {
-		for (size_t i = 0; i < quantity; ++i) {
-			if (txBufferLength >= BUFFER_LENGTH)
-				return i;
-			txBuffer[txBufferLength++] = data[i];
-		}
-	} else {
-		for (size_t i = 0; i < quantity; ++i) {
-			if (srvBufferLength >= BUFFER_LENGTH)
-				return i;
-			srvBuffer[srvBufferLength++] = data[i];
-		}
+size_t TwoWire::write(const uint8_t *data, size_t quantity)
+{
+	//Try to store all data
+	for(size_t i = 0; i < quantity; ++i)
+	{
+		//Return the number of data stored, when the buffer is full (if write return 0)
+		if(!write(data[i])
+			return i;
 	}
+	
+	//All data stored
 	return quantity;
 }
 
-int TwoWire::available(void) {
-	return rxBufferLength - rxBufferIndex;
+int TwoWire::available(void)
+{
+	return rxBuffer.available();
 }
 
-int TwoWire::read(void) {
-	if (rxBufferIndex < rxBufferLength)
-		return rxBuffer[rxBufferIndex++];
-	return -1;
+int TwoWire::read(void)
+{
+	return rxBuffer.read_char();
 }
 
-int TwoWire::peek(void) {
-	if (rxBufferIndex < rxBufferLength)
-		return rxBuffer[rxBufferIndex];
-	return -1;
+int TwoWire::peek(void)
+{
+	return rxBuffer.peek();
 }
 
-void TwoWire::flush(void) {
+void TwoWire::flush(void)
+{
 	// Do nothing, use endTransmission(..) to force
 	// data transfer.
 }
 
-void TwoWire::onReceive(void(*function)(int)) {
+void TwoWire::onReceive(void(*function)(int))
+{
 	onReceiveCallback = function;
 }
 
-void TwoWire::onRequest(void(*function)(void)) {
+void TwoWire::onRequest(void(*function)(void))
+{
 	onRequestCallback = function;
 }
 
-void TwoWire::onService(void) {
+
+void TwoWire::onService(void)
+{
+	if(sercom->isSlaveWIRE())
+	{
+		//Received data
+		if(sercom->isDataReadyWIRE())
+		{
+			//Store data
+			rxBuffer.store_char(sercom->readDataWIRE());
+			
+			//Stop or Restart detected
+			if(sercom->isStopDetectedWIRE() || sercom->isRestartDetectedWIRE())
+			{
+				//Calling onReceiveCallback, if exists
+				if(onReceiveCallback)
+				{
+					onReceiveCallback(available());
+				}
+			}
+		}
+		
+		//Address Match
+		if(sercom->isAddressMatch())
+		{
+			//Is a request ?
+			if(sercom->isMasterReadOperationWIRE())
+			{
+				//Calling onRequestCallback, if exists
+				if(onRequestCallback)
+				{
+					onRequestCallback();
+				}
+			}
+		}
+	}
+}
+
+/*
+void TwoWire::onService(void)
+{
 	// Retrieve interrupt status
 	uint32_t sr = TWI_GetStatus(twi);
 
@@ -328,6 +319,7 @@ void TwoWire::onService(void) {
 		}
 	}
 }
+*/
 
 #if WIRE_INTERFACES_COUNT > 0
 static void Wire_Init(void) {
diff --git a/libraries/Wire/Wire.h b/libraries/Wire/Wire.h
index d36faa95..c8d3200c 100644
--- a/libraries/Wire/Wire.h
+++ b/libraries/Wire/Wire.h
@@ -21,30 +21,31 @@
 #ifndef TwoWire_h
 #define TwoWire_h
 
-// Include Atmel CMSIS driver
 #include <include/twi.h>
 
 #include "Stream.h"
 #include "variant.h"
 
+#include "sercom.h"
+#include "RingBuffer.h"
+
 #define BUFFER_LENGTH 32
 
 class TwoWire : public Stream {
 public:
-	TwoWire(Twi *twi, void(*begin_cb)(void));
+	TwoWire(SERCOM *sercom);
 	void begin();
 	void begin(uint8_t);
-	void begin(int);
-	void beginTransmission(uint8_t);
-	void beginTransmission(int);
+	
+	uint8_t beginTransmission(uint8_t);
 	uint8_t endTransmission(void);
-    uint8_t endTransmission(uint8_t);
-	uint8_t requestFrom(uint8_t, uint8_t);
-    uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
-	uint8_t requestFrom(int, int);
-    uint8_t requestFrom(int, int, int);
-	virtual size_t write(uint8_t);
-	virtual size_t write(const uint8_t *, size_t);
+	
+    uint8_t requestFrom(uint8_t address, size_t quantity, bool stopBit);
+	uint8_t requestFrom(uint8_t address, size_t quantity);
+	
+	size_t write(uint8_t data);
+	size_t write(const uint8_t * data, size_t quantity);
+	
 	virtual int available(void);
 	virtual int read(void);
 	virtual int peek(void);
@@ -52,58 +53,49 @@ public:
 	void onReceive(void(*)(int));
 	void onRequest(void(*)(void));
 
-    inline size_t write(unsigned long n) { return write((uint8_t)n); }
-    inline size_t write(long n) { return write((uint8_t)n); }
-    inline size_t write(unsigned int n) { return write((uint8_t)n); }
-    inline size_t write(int n) { return write((uint8_t)n); }
     using Print::write;
 
-	void onService(void);
+	//void onService(void);
 
 private:
+	SERCOM * sercom;
+	bool transmissionBegun;
+	
 	// RX Buffer
-	uint8_t rxBuffer[BUFFER_LENGTH];
-	uint8_t rxBufferIndex;
-	uint8_t rxBufferLength;
-
-	// TX Buffer
+	RingBuffer rxBuffer;
+	
+	//TX buffer
+	RingBuffer txBuffer;
 	uint8_t txAddress;
-	uint8_t txBuffer[BUFFER_LENGTH];
-	uint8_t txBufferLength;
+	
 
 	// Service buffer
-	uint8_t srvBuffer[BUFFER_LENGTH];
-	uint8_t srvBufferIndex;
-	uint8_t srvBufferLength;
+	//uint8_t srvBuffer[BUFFER_LENGTH];
+	//uint8_t srvBufferIndex;
+	//uint8_t srvBufferLength;
 
 	// Callback user functions
 	void (*onRequestCallback)(void);
 	void (*onReceiveCallback)(int);
 
-	// Called before initialization
-	void (*onBeginCallback)(void);
-
-	// TWI instance
-	Twi *twi;
-
 	// TWI state
-	enum TwoWireStatus {
-		UNINITIALIZED,
-		MASTER_IDLE,
-		MASTER_SEND,
-		MASTER_RECV,
-		SLAVE_IDLE,
-		SLAVE_RECV,
-		SLAVE_SEND
-	};
-	TwoWireStatus status;
+	//enum TwoWireStatus {
+	//	UNINITIALIZED,
+	//	MASTER_IDLE,
+	//	MASTER_SEND,
+	//	MASTER_RECV,
+	//	SLAVE_IDLE,
+	//	SLAVE_RECV,
+	//	SLAVE_SEND
+	//};
+	//TwoWireStatus status;
 
 	// TWI clock frequency
 	static const uint32_t TWI_CLOCK = 100000;
 
-	// Timeouts (
-	static const uint32_t RECV_TIMEOUT = 100000;
-	static const uint32_t XMIT_TIMEOUT = 100000;
+	// Timeouts
+	//static const uint32_t RECV_TIMEOUT = 100000;
+	//static const uint32_t XMIT_TIMEOUT = 100000;
 };
 
 #if WIRE_INTERFACES_COUNT > 0
@@ -114,4 +106,3 @@ extern TwoWire Wire1;
 #endif
 
 #endif
-
-- 
GitLab