Wire: implement slave functionalities

Author: facchinm

libraries/Wire/Wire.cpp

@@ -31,7 +31,8 @@ void arduino::MbedI2C::begin() {
 void arduino::MbedI2C::begin(uint8_t slaveAddr) {
 #ifdef DEVICE_I2CSLAVE
 	slave = new mbed::I2CSlave((PinName)_sda, (PinName)_scl);
-	slave->address(slaveAddr);
+	slave->address(slaveAddr << 1);
+	slave_th.start(mbed::callback(this, &arduino::MbedI2C::receiveThd));
 #endif
 }
 
@@ -118,9 +119,45 @@ int arduino::MbedI2C::peek() {
 void arduino::MbedI2C::flush() {
 }
 
+void arduino::MbedI2C::receiveThd() {
+	while (1) {
+		int i = slave->receive();
+		switch (i) {
+			case mbed::I2CSlave::ReadAddressed:
+				if (onRequestCb != NULL) {
+					onRequestCb();
+				}
+				slave->write((const char *) txBuffer, usedTxBuffer);
+				slave->stop();
+				break;
+			case mbed::I2CSlave::WriteGeneral:
+			case mbed::I2CSlave::WriteAddressed:
+				rxBuffer.clear();
+				char buf[16];
+				while (1) {
+					int c = slave->read(buf, sizeof(buf));
+					for (int i = 0; i < c; i++) {
+						rxBuffer.store_char(uint8_t(buf[i]));
+					}
+					if (c <= sizeof(buf)) {
+						break;
+					}
+				}
+				if (rxBuffer.available() > 0 && onReceiveCb != NULL) {
+					onReceiveCb(rxBuffer.available());
+				}
+				slave->stop();
+				break;
+		}
+	}
+}
 
-void arduino::MbedI2C::onReceive(void(*)(int)) {}
-void arduino::MbedI2C::onRequest(void(*)(void)) {}
+void arduino::MbedI2C::onReceive(voidFuncPtrParamInt cb) {
+	onReceiveCb = cb;
+}
+void arduino::MbedI2C::onRequest(voidFuncPtr cb) {
+	onRequestCb = cb;
+}
 
 
 #if WIRE_HOWMANY > 0

libraries/Wire/Wire.h

@@ -19,9 +19,12 @@
 #pragma once
 
 #include "Arduino.h"
+#include "Print.h"
 #include "drivers/I2C.h"
 #include "drivers/I2CSlave.h"
 
+typedef void (*voidFuncPtrParamInt)(int);
+
 namespace arduino {
 
 class MbedI2C : public HardwareI2C
@@ -46,6 +49,7 @@ class MbedI2C : public HardwareI2C
 
     virtual size_t write(uint8_t data);
     virtual size_t write(const uint8_t* data, int len);
+    using Print::write;
     virtual int read();
     virtual int peek();
     virtual void flush();
@@ -63,6 +67,10 @@ class MbedI2C : public HardwareI2C
     RingBufferN<256> rxBuffer;
     uint8_t txBuffer[256];
     uint32_t usedTxBuffer;
+    voidFuncPtrParamInt onReceiveCb = NULL;
+    voidFuncPtr onRequestCb = NULL;
+    rtos::Thread slave_th;
+    void receiveThd();
 };
 
 }