From 045c446213a57aa350c7527cc2b90b29425bc58c Mon Sep 17 00:00:00 2001
From: Ivan Grokhotkov <igrokhotkov@gmail.com>
Date: Wed, 21 Jan 2015 04:17:03 +0300
Subject: [PATCH] Use i2c instead of serial for wifio virtual io

---
 variants/wifio/WifioProtocol.h | 48 ++++++++++++++--------------------
 variants/wifio/WifioWiring.cpp | 26 +++++++-----------
 2 files changed, 30 insertions(+), 44 deletions(-)

diff --git a/variants/wifio/WifioProtocol.h b/variants/wifio/WifioProtocol.h
index 42d52f4d2..dbb9d9e7e 100644
--- a/variants/wifio/WifioProtocol.h
+++ b/variants/wifio/WifioProtocol.h
@@ -24,11 +24,12 @@
 
 #include <stdint.h>
 #include "Arduino.h"
+#include "i2c.h"
 
 inline void protocolError()
 {
   delay(5);
-  Serial.flush();
+  i2c_stop();
 }
 
 namespace wifio {
@@ -112,48 +113,39 @@ struct AnalogReadResultCommand {
   uint8_t      parity : 1;
 };
 
+
 template<typename T>
-void sendCommand(Stream& stream, T& t)
+void sendCommand(uint8_t addr, T& t)
 {
   // TODO: calculate parity
   t.parity = 1;
-  stream.write(reinterpret_cast<const uint8_t*>(&t), sizeof(T));
+  if (i2c_master_write_to(addr, reinterpret_cast<const uint8_t*>(&t), sizeof(T), true) != sizeof(T))
+  {
+    protocolError();
+  }
 }
 
-template<typename T>
-bool expectCommand(Stream& stream, enum Command cmd, T& t)
+template<typename TC, typename TR>
+bool sendCommandWaitForReply(uint8_t addr, TC& c, Command rt, TR& r, int32_t d_us)
 {
-  size_t nIt = 0;
-  t = { 0 };
-  while (stream.available() < sizeof(T))
+  c.parity = 1;
+  if (i2c_master_write_to(addr, reinterpret_cast<const uint8_t*>(&c), sizeof(TC), true) != sizeof(TC))
   {
-    if (++nIt == 100) {
-      return false;
-    }
-    delayMicroseconds(10);
+    protocolError();
+    return false;
   }
-  uint8_t* p = reinterpret_cast<uint8_t*>(&t);
-  for (size_t i = 0; i < sizeof(T); ++i)
-    p[i] = stream.read();
-  // TODO : check parity
-  if (static_cast<uint8_t>(cmd) != t.header.cmd) {
+  if (d_us)
+  {
+    delayMicroseconds(d_us);
+  }
+  if (i2c_master_read_from(addr, reinterpret_cast<uint8_t*>(&r), sizeof(TR), true) != sizeof(TR) || r.header.cmd != rt)
+  {
     protocolError();
     return false;
   }
   return true;
 }
 
-inline Command peekCommand(Stream& stream)
-{
-  if (!stream.available())
-    return CMD_NONE;
-
-  uint8_t b = stream.peek();
-  CommandHeader* h = reinterpret_cast<CommandHeader*>(&b);
-  if (h->magic != MAGIC)
-    return CMD_INVALID;
-  return static_cast<Command>(h->cmd);
-}
 
 }// namespace wifio
 
diff --git a/variants/wifio/WifioWiring.cpp b/variants/wifio/WifioWiring.cpp
index 32d1e901e..f66fdae19 100644
--- a/variants/wifio/WifioWiring.cpp
+++ b/variants/wifio/WifioWiring.cpp
@@ -24,41 +24,34 @@
 
 
 namespace wifio {
+    const int WIFIO_AVR_SLAVE_ADDR=0x23;
     
     void pinMode(int avrPin, int mode) {
         PinModeCommand cmd { MAGIC, CMD_PINMODE, static_cast<uint8_t>(avrPin), static_cast<uint8_t>(mode) };
-        sendCommand(Serial, cmd);
+        sendCommand(WIFIO_AVR_SLAVE_ADDR, cmd);
     }
     
     void digitalWrite(int avrPin, int value) {
         DigitalWriteCommand cmd { MAGIC, CMD_DIGITALWRITE, static_cast<uint8_t>(avrPin), static_cast<uint8_t>(value) };
-        sendCommand(Serial, cmd);
+        sendCommand(WIFIO_AVR_SLAVE_ADDR, cmd);
     }
     
     int digitalRead(int avrPin) {
         DigitalReadCommand cmd { MAGIC, CMD_DIGITALREAD, static_cast<uint8_t>(avrPin) };
-        sendCommand(Serial, cmd);
-        DigitalReadResultCommand reply;
-        if (!expectCommand(Serial, CMD_DIGITALREADRESULT, reply) || reply.pin != avrPin) {
-            protocolError();
-            return 0;
-        }
+        DigitalReadResultCommand reply = { 0 };
+        sendCommandWaitForReply(WIFIO_AVR_SLAVE_ADDR, cmd, CMD_DIGITALREADRESULT, reply, 0);
         return reply.value;
     }
     
     void analogWrite(int avrPin, int value) {
         AnalogWriteCommand cmd { MAGIC, CMD_ANALOGWRITE, static_cast<uint8_t>(value), static_cast<uint8_t>(avrPin) };
-        sendCommand(Serial, cmd);
+        sendCommand(WIFIO_AVR_SLAVE_ADDR, cmd);
     }
     
     int analogRead(int avrPin) {
         AnalogReadCommand cmd { MAGIC, CMD_ANALOGREAD, static_cast<uint8_t>(avrPin) };
-        sendCommand(Serial, cmd);
-        AnalogReadResultCommand reply;
-        if (!expectCommand(Serial, CMD_ANALOGREADRESULT, reply) || reply.pin != avrPin) {
-            protocolError();
-            return 0;
-        }
+        AnalogReadResultCommand reply = { 0 };
+        sendCommandWaitForReply(WIFIO_AVR_SLAVE_ADDR, cmd, CMD_ANALOGREADRESULT, reply, 120);
         return ((reply.val_h << 7) | reply.val_l);
     }
     
@@ -119,7 +112,8 @@ extern "C" void analogWrite(uint8_t pin, int value) {
   }
 }
 
+
 void initVariant() 
 { 
-
+  i2c_init(SDA, SCL);
 }