Merge branch 'wifi-reliability2' into Ethernewt-

CommandDistributor.cpp

@@ -22,10 +22,10 @@
 
 DCCEXParser * CommandDistributor::parser=0; 
 
-void  CommandDistributor::parse(byte clientId,byte * buffer, Print * streamer) {
+void  CommandDistributor::parse(byte clientId,byte * buffer, RingStream * streamer) {
  if (buffer[0] == '<')  {
     if (!parser) parser = new DCCEXParser();
     parser->parse(streamer, buffer, true); // tell JMRI parser that ACKS are blocking because we can't handle the async
   }
-  else WiThrottle::getThrottle(clientId)->parse(*streamer, buffer);
+  else WiThrottle::getThrottle(clientId)->parse(streamer, buffer);
 }

CommandDistributor.h

@@ -19,11 +19,12 @@
 #ifndef CommandDistributor_h
 #define CommandDistributor_h
 #include "DCCEXParser.h"
+#include "RingStream.h"
 
 class CommandDistributor {
 
 public :
-  static void parse(byte clientId,byte* buffer, Print * streamer);
+  static void parse(byte clientId,byte* buffer, RingStream * streamer);
 private:
    static DCCEXParser * parser;
 };

RingStream.cpp

@@ -28,38 +28,72 @@ RingStream::RingStream( const uint16_t len)
   _pos_read=0;
   _buffer[0]=0;
   _overflow=false;
+  _mark=0;
+  _count=0; 
 }
 
-size_t RingStream::write(uint8_t byte) {
+size_t RingStream::write(uint8_t b) {
   if (_overflow) return 0;
-  _buffer[_pos_write] = byte;
+  _buffer[_pos_write] = b;
   ++_pos_write;
-  if (_pos_write>=_len) _pos_write=0;
+  if (_pos_write==_len) _pos_write=0;
   if (_pos_write==_pos_read) {
     _overflow=true; 
-    DIAG(F("\nRingStream(%d) OVERFLOW  %d %d \n"),_len, _pos_write, _pos_read);
     return 0;
   }
+  _count++;
   return 1;
 }
 
 int RingStream::read() {
-  if (_pos_read==_pos_write) return -1; 
+  if ((_pos_read==_pos_write) && !_overflow) return -1;  // empty  
   byte b=_buffer[_pos_read];
   _pos_read++;
-  if (_pos_read>=_len) _pos_read=0;
+  if (_pos_read==_len) _pos_read=0;
   _overflow=false;
   return b;
 }
 
 
 int RingStream::count() {
-  int peek=_pos_read;
+  return (read()<<8) | read(); 
-  int counter=0;
-  while(_buffer[peek]) {
-    counter++;
-    peek++;
-    if (peek >= _len) peek=0;
   }
-  return counter;
+
+int RingStream::freeSpace() {
+  // allow space for client flag and length bytes
+  if (_pos_read>_pos_write) return _pos_read-_pos_write-3;
+  else return _len - _pos_write + _pos_read-3;  
+}
+
+
+// mark start of message with client id (0...9)
+void RingStream::mark(uint8_t b) {
+    _mark=_pos_write;
+    write(b); // client id
+    write((uint8_t)0);  // count MSB placemarker
+    write((uint8_t)0);  // count LSB placemarker
+    _count=0;
+}
+
+bool RingStream::commit() {
+  if (_overflow) {
+        DIAG(F("\nRingStream(%d) commit(%d) OVERFLOW\n"),_len, _count);
+        // just throw it away 
+        _pos_write=_mark;
+        _overflow=false;
+        return false; // commit failed
   }
+  if (_count==0) {
+    // ignore empty response 
+    _pos_write=_mark;
+    return true; // true=commit ok
+  }
+  // Go back to the _mark and inject the count 1 byte later
+  _mark++;
+  if (_mark==_len) _mark=0;
+  _buffer[_mark]=highByte(_count);
+  _mark++;
+  if (_mark==_len) _mark=0;
+  _buffer[_mark]=lowByte(_count);
+  return true; // commit worked
+}

RingStream.h

@@ -30,12 +30,17 @@ class RingStream : public Print {
     using Print::write;
     int read();
     int count();
+    int freeSpace();
+    void mark(uint8_t b);
+    bool commit();
 
  private:
    int _len;
    int _pos_write;
    int _pos_read;
    bool _overflow;
+   int _mark;
+   int _count;
    byte * _buffer;
 };
 

WiThrottle.cpp

@@ -99,7 +99,7 @@ WiThrottle::~WiThrottle() {
   }
 }
 
-void WiThrottle::parse(Print & stream, byte * cmdx) {
+void WiThrottle::parse(RingStream * stream, byte * cmdx) {
   
   byte * cmd=cmdx;
   
@@ -205,7 +205,7 @@ int WiThrottle::getLocoId(byte * cmd) {
     if (cmd[0]!='L' && cmd[0]!='S') return 0; // should not match any locos
     return getInt(cmd+1); 
 }
-void WiThrottle::multithrottle(Print & stream, byte * cmd){ 
+void WiThrottle::multithrottle(RingStream * stream, byte * cmd){ 
           char throttleChar=cmd[1];
           int locoid=getLocoId(cmd+3); // -1 for *
           byte * aval=cmd;
@@ -256,7 +256,7 @@ void WiThrottle::multithrottle(Print & stream, byte * cmd){
             }
 }
 
-void WiThrottle::locoAction(Print & stream, byte* aval, char throttleChar, int cab){
+void WiThrottle::locoAction(RingStream * stream, byte* aval, char throttleChar, int cab){
     // Note cab=-1 for all cabs in the consist called throttleChar.  
 //    DIAG(F("\nLoco Action aval=%c%c throttleChar=%c, cab=%d"), aval[0],aval[1],throttleChar, cab);
      switch (aval[0]) {
@@ -334,10 +334,19 @@ int WiThrottle::WiTToDCCSpeed(int WiTSpeed) {
   return WiTSpeed + 1; //offset others by 1
 }
 
-void WiThrottle::loop() {
+void WiThrottle::loop(RingStream * stream) {
   // for each WiThrottle, check the heartbeat
   for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle) 
      wt->checkHeartbeat();
+
+   // TODO... any broadcasts to be done 
+   (void)stream; 
+   /* MUST follow this model in  this loop. 
+    *   stream->mark();
+    *   send 1 digit client id, and any data 
+    *   stream->commit() 
+     */
+
 }
 
 void WiThrottle::checkHeartbeat() {

WiThrottle.h

@@ -19,6 +19,7 @@
 #ifndef WiThrottle_h
 #define WiThrottle_h
 
+#include "RingStream.h"
 
 struct MYLOCO {
     char throttle; //indicates which throttle letter on client, often '0','1' or '2'
@@ -27,8 +28,8 @@ struct MYLOCO {
 
 class WiThrottle {
   public:  
-    static void loop();
+    static void loop(RingStream * stream);
-    void parse(Print & stream, byte * cmd);
+    void parse(RingStream * stream, byte * cmd);
     static WiThrottle* getThrottle( int wifiClient); 
     static bool annotateLeftRight;
   private: 
@@ -56,9 +57,9 @@ class WiThrottle {
       bool lastPowerState;  // last power state sent to this client
       int DCCToWiTSpeed(int DCCSpeed);
       int WiTToDCCSpeed(int WiTSpeed);
-      void multithrottle(Print & stream, byte * cmd);
+      void multithrottle(RingStream * stream, byte * cmd);
-      void locoAction(Print & stream, byte* aval, char throttleChar, int cab);
+      void locoAction(RingStream * stream, byte* aval, char throttleChar, int cab);
-      void accessory(Print & stream, byte* cmd);
+      void accessory(RingStream *, byte* cmd);
       void checkHeartbeat();  
 };
 #endif

WifiInboundHandler.cpp

@@ -31,20 +31,17 @@ void WifiInboundHandler::loop1() {
    // First handle all inbound traffic events because they will block the sending 
    if (loop2()!=INBOUND_IDLE) return;
 
+   WiThrottle::loop(outboundRing);
+   
     // if nothing is already CIPSEND pending, we can CIPSEND one reply
     if (clientPendingCIPSEND<0) {
-       int next=outboundRing->read();
+       clientPendingCIPSEND=outboundRing->read();
-       if (next>=0) {
+       if (clientPendingCIPSEND>=0) {
          currentReplySize=outboundRing->count();
-         if (currentReplySize==0) {
+         pendingCipsend=true;
-          outboundRing->read(); // drop end marker
-         }
-         else {
-          clientPendingCIPSEND=next-'0'; // convert back to int         
-          pendingCipsend=true;
-         }
        }
-    } 
+     }
+    
 
     if (pendingCipsend) {
          if (Diag::WIFI) DIAG( F("\nWiFi: [[CIPSEND=%d,%d]]"), clientPendingCIPSEND, currentReplySize);
@@ -55,19 +52,20 @@ void WifiInboundHandler::loop1() {
     
     
     // if something waiting to execute, we can call it 
-      int next=inboundRing->read();
+      int clientId=inboundRing->read();
-      if (next>0) {
+      if (clientId>=0) {
-         int clientId=next-'0'; //convert char to int
          int count=inboundRing->count();
-         if (Diag::WIFI) DIAG(F("\nExec waiting %d %d:"),clientId,count); 
+         if (Diag::WIFI) DIAG(F("\nWifi EXEC: %d %d:"),clientId,count); 
          byte cmd[count+1];
          for (int i=0;i<count;i++) cmd[i]=inboundRing->read();   
          cmd[count]=0;
          if (Diag::WIFI) DIAG(F("%e\n"),cmd); 
          
-         outboundRing->print(clientId);
+         outboundRing->mark(clientId);  // remember start of outbound data 
          CommandDistributor::parse(clientId,cmd,outboundRing);
-         outboundRing->write((byte)0);
+         // The commit call will either write the lenbgth bytes 
+         // OR rollback to the mark because the reply is empty or commend generated more than fits the buffer 
+         outboundRing->commit();
          return;
       }
    }
@@ -87,7 +85,7 @@ WifiInboundHandler::INBOUND_STATE WifiInboundHandler::loop2() {
     }
 
     switch (loopState) {
-      case ANYTHING:  // looking for +IPD, > , busy ,  n,CONNECTED, n,CLOSED 
+      case ANYTHING:  // looking for +IPD, > , busy ,  n,CONNECTED, n,CLOSED, ERROR, SEND OK 
         
         if (ch == '+') {
           loopState = IPD;
@@ -95,9 +93,12 @@ WifiInboundHandler::INBOUND_STATE WifiInboundHandler::loop2() {
         }
         
         if (ch=='>') { 
-           if (Diag::WIFI) DIAG(F("[[XMIT %d]]"),currentReplySize);
+           if (Diag::WIFI) DIAG(F("[XMIT %d]"),currentReplySize); 
-           for (int i=0;i<currentReplySize;i++) wifiStream->write(outboundRing->read());
+           for (int i=0;i<currentReplySize;i++) {
-           outboundRing->read(); // drop the end marker
+             int cout=outboundRing->read();
+             wifiStream->write(cout);
+             if (Diag::WIFI) StringFormatter::printEscape(cout); // DIAG in disguise
+           }
            clientPendingCIPSEND=-1;
            pendingCipsend=false;
            loopState=SKIPTOEND;
@@ -109,6 +110,11 @@ WifiInboundHandler::INBOUND_STATE WifiInboundHandler::loop2() {
           break;
         }
        
+        if (ch=='S') { // SEND OK probably 
+          loopState=SKIPTOEND;
+          break;
+        }
+        
         if (ch=='b') {   // This is a busy indicator... probabaly must restart a CIPSEND  
            pendingCipsend=(clientPendingCIPSEND>=0);
            loopState=SKIPTOEND; 
@@ -121,6 +127,15 @@ WifiInboundHandler::INBOUND_STATE WifiInboundHandler::loop2() {
               break;
         }
 
+        if (ch=='E' || ch=='l') { // ERROR or "link is not valid"
+          if (clientPendingCIPSEND>=0) {
+            // A CIPSEND was errored... just toss it away
+            purgeCurrentCIPSEND();  
+          }
+          loopState=SKIPTOEND; 
+          break; 
+        }
+        
         break;
         
       case IPD:  // Looking for I   in +IPD
@@ -159,7 +174,13 @@ WifiInboundHandler::INBOUND_STATE WifiInboundHandler::loop2() {
             break;
           }
           if (Diag::WIFI) DIAG(F("\nWifi inbound data(%d:%d):"),runningClientId,dataLength); 
-          inboundRing->print(runningClientId); // prefix inbound with client id
+          if (inboundRing->freeSpace()<=(dataLength+1)) {
+            // This input would overflow the inbound ring, ignore it  
+            loopState=IPD_IGNORE_DATA;
+            if (Diag::WIFI) DIAG(F("\nWifi OVERFLOW IGNORING:"));    
+            break;
+          }
+          inboundRing->mark(runningClientId);
           loopState=IPD_DATA;
           break; 
         }
@@ -167,33 +188,31 @@ WifiInboundHandler::INBOUND_STATE WifiInboundHandler::loop2() {
         break;
         
       case IPD_DATA: // reading data
-         inboundRing->write(ch);    
+        inboundRing->write(ch);    
         dataLength--;
         if (dataLength == 0) {
-           inboundRing->write((byte)0);    
+          inboundRing->commit();    
           loopState = ANYTHING;
         }
         break;
 
+      case IPD_IGNORE_DATA: // ignoring data that would not fit in inbound ring
+        dataLength--;
+        if (dataLength == 0) loopState = ANYTHING;
+        break;
+
       case GOT_CLIENT_ID:  // got x before CLOSE or CONNECTED
         loopState=(ch==',') ? GOT_CLIENT_ID2: SKIPTOEND;
         break;
         
-      case GOT_CLIENT_ID2:  // got "x,"  before CLOSE or CONNECTED
+      case GOT_CLIENT_ID2:  // got "x,"  
-        loopState=(ch=='C') ? GOT_CLIENT_ID3: SKIPTOEND;
+        if (ch=='C') {
+         // got "x C" before CLOSE or CONNECTED, or CONNECT FAILED
+         if (runningClientId==clientPendingCIPSEND) purgeCurrentCIPSEND();
+        }
+        loopState=SKIPTOEND;   
         break;
          
-      case GOT_CLIENT_ID3:  // got "x C" before CLOSE or CONNECTED (which is ignored)
-         if(ch=='L') {
-          // CLOSE 
-          if (runningClientId==clientPendingCIPSEND) {
-            // clear the outbound for this client
-            for (int i=0;i<=currentReplySize;i++) outboundRing->read(); 
-          }
-         }
-         loopState=SKIPTOEND;   
-         break;
-         
       case SKIPTOEND: // skipping for /n
         if (ch=='\n') loopState=ANYTHING;
         break;
@@ -201,3 +220,11 @@ WifiInboundHandler::INBOUND_STATE WifiInboundHandler::loop2() {
   } // available
   return (loopState==ANYTHING) ? INBOUND_IDLE: INBOUND_BUSY;
 }
+
+void WifiInboundHandler::purgeCurrentCIPSEND() {
+         // A CIPSEND was sent but errored... or the client closed just toss it away
+         if (Diag::WIFI) DIAG(F("Wifi: DROPPING CIPSEND=%d,%d\n"),clientPendingCIPSEND,currentReplySize);
+         for (int i=0;i<=currentReplySize;i++) outboundRing->read();
+         pendingCipsend=false;  
+         clientPendingCIPSEND=-1;
+}

WifiInboundHandler.h

@@ -2,6 +2,7 @@
 #define WifiInboundHandler_h
 
 #include "RingStream.h"
+#include "WiThrottle.h"
 #include "DIAG.h"
 
 class WifiInboundHandler {
@@ -32,6 +33,7 @@ class WifiInboundHandler {
           IPD5,        // got +IPD,c 
           IPD6_LENGTH, // got +IPD,c, reading length 
           IPD_DATA,    // got +IPD,c,ll,: collecting data
+          IPD_IGNORE_DATA, // got +IPD,c,ll,: ignoring the data that won't fit inblound Ring
 
           GOT_CLIENT_ID,  // clientid prefix to CONNECTED / CLOSED
           GOT_CLIENT_ID2,  // clientid prefix to CONNECTED / CLOSED
@@ -42,10 +44,11 @@ class WifiInboundHandler {
    WifiInboundHandler(Stream * ESStream);
    void loop1();
    INBOUND_STATE loop2();
+   void purgeCurrentCIPSEND();
    Stream * wifiStream;
    
-   static const int INBOUND_RING = 200;
+   static const int INBOUND_RING = 512;
-   static const int OUTBOUND_RING = 1024;
+   static const int OUTBOUND_RING = 2048;
  
    RingStream * inboundRing;
    RingStream * outboundRing;

WifiInterface.cpp

@@ -22,7 +22,7 @@
 #include <avr/pgmspace.h>
 #include "DIAG.h"
 #include "StringFormatter.h"
-#include "WiThrottle.h"
+
 #include "WifiInboundHandler.h"
 
 const char  PROGMEM READY_SEARCH[]  = "\r\nready\r\n";
@@ -90,6 +90,11 @@ bool WifiInterface::setup(long serial_link_speed,
   }
 #endif
 
+DCCEXParser::setAtCommandCallback(ATCommand);
+
+// CAUTION... ONLY CALL THIS ONCE 
+WifiInboundHandler::setup(wifiStream);
+  
 return wifiUp; 
 }
 
@@ -109,8 +114,6 @@ bool WifiInterface::setup(Stream & setupStream,  const __FlashStringHelper* SSid
     checkForOK(200, OK_SEARCH, true);      
   }
 
-  DCCEXParser::setAtCommandCallback(ATCommand);
-  WifiInboundHandler::setup(wifiStream);
     
   DIAG(F("\n++ Wifi Setup %S ++\n"), connected ? F("OK") : F("FAILED"));
   return connected;
@@ -297,7 +300,6 @@ bool WifiInterface::checkForOK( const unsigned int timeout, const char * waitfor
 
 void WifiInterface::loop() {
   if (connected) {
-    WiThrottle::loop();
     WifiInboundHandler::loop(); 
   }
 }