Add protocol wrappers to the generic protocol

This adds two wrappers to the generic protocol to help interfacing
small microcontrollers (arduino et. al.)
When receiving binary data over a sequential link, it's hard to tell where a packet
starts and where it ends. Two existing solutions gets implemented with this patch:
KISS (http://www.ka9q.net/papers/kiss.html) originally developed for ham radio
packet radio devices and STX/ETX byte stuffing.
This adds only the OUT direction, IN will be implemented over the next weeks.

For implementation details on the client (microcontroller's) side, check out
http://gitorious.org/flightgear-pmpt/cmpt/

This implementation is used in the c172fg project to be presented at FSweekend 2013.

diff --git a/src/Network/generic.cxx b/src/Network/generic.cxx
index 523c19d5e8b582f13033a39c3c05661e30656aad..90f2395a850319b154b27927445f439979962782 100644 (file)
--- a/src/Network/generic.cxx
+++ b/src/Network/generic.cxx
@@ -46,7 +46,191 @@
 
 using simgear::strutils::unescape;
 
-FGGeneric::FGGeneric(vector<string> tokens) : exitOnError(false), initOk(false)
+class FGProtocolWrapper {
+public:
+  virtual ~FGProtocolWrapper() {}
+  virtual int wrap( size_t n, uint8_t * buf ) = 0;
+  virtual int unwrap( size_t n, uint8_t * buf ) = 0;
+};
+
+/**
+ * http://www.ka9q.net/papers/kiss.html
+ */
+class FGKissWrapper : public FGProtocolWrapper {
+public:
+  virtual int wrap( size_t n, uint8_t * buf );
+  virtual int unwrap( size_t n, uint8_t * buf );
+private:
+  static const uint8_t  FEND  = 0xC0;
+  static const uint8_t  FESC  = 0xDB;
+  static const uint8_t  TFEND = 0xDC;
+  static const uint8_t  TFESC = 0xDD;
+};
+
+int FGKissWrapper::wrap( size_t n, uint8_t * buf )
+{
+  uint8_t dest[2*n+3]; // worst case, FEND+command+all bytes escaped+FEND
+  uint8_t *sp = buf, *dp = dest;
+
+  *dp++ = FEND;
+  *dp++ = 0; // command/channel always zero
+  for( size_t i = 0; i < n; i++ ) {
+    uint8_t c = *sp++;
+    switch( c ) {
+      case FESC:
+        *dp++ = FESC;
+        *dp++ = TFESC;
+        break;
+
+      case FEND:
+        *dp++ = FESC;
+        *dp++ = TFEND;
+        break;
+
+      default:
+       *dp++ = c;
+       break;
+    }
+  }
+  *dp++ = FEND;
+
+  // copy the result to the input buffer
+  // and count the buffer size
+  int reply = 0;
+  for( sp = dest; sp < dp; ) {
+    *buf++ = *sp++;
+    reply++;
+  }
+  return reply;
+}
+
+int FGKissWrapper::unwrap( size_t n, uint8_t * buf )
+{
+  uint8_t * sp = buf;
+
+  // look for FEND
+  while( 0 < n && FEND != *sp ) {
+    sp++;
+    n--;
+  }
+
+  // ignore all leading FEND
+  while( 0 < n && FEND == *sp ) {
+    sp++;
+    n--;
+  }
+
+  if( 0 == n ) return 0;
+
+  uint8_t dest[n];
+  uint8_t * dp = dest;
+
+  {
+    bool escaped = false;
+
+    while( 0 < n ) {
+
+      n--;
+      uint8_t c = *sp++;
+
+      if( escaped ) {
+        switch( c ) {
+
+          case TFESC:
+            *dp++ = FESC;
+            break;
+
+          case TFEND:
+            *dp++ = FEND;
+            break;
+
+          default: // this is an error - ignore and continue
+            break;
+        }
+
+        escaped = false;
+
+      } else {
+
+        switch( c ) {
+          case FESC:
+            escaped = true;
+            break;
+
+          case FEND:
+            if( 0 != n ) {
+              SG_LOG(SG_IO, SG_WARN,
+               "KISS frame detected FEND before end of frame. Trailing data dropped." );
+            }
+            n = 0; 
+            break;
+
+          default:
+            *dp++ = c;
+            break;
+
+        }
+      }
+    }
+  }
+
+  n = 0;
+  for( sp = dest; sp != dp; ) {
+    *buf++ = *sp++;
+    n++;
+  }
+
+  return n;
+}
+
+
+class FGSTXETXWrapper : public FGProtocolWrapper {
+public:
+  virtual int wrap( size_t n, uint8_t * buf );
+  virtual int unwrap( size_t n, uint8_t * buf );
+
+  static const uint8_t  STX  = 0x02;
+  static const uint8_t  ETX  = 0x03;
+  static const uint8_t  DLE  = 0x00;
+};
+
+int FGSTXETXWrapper::wrap( size_t n, uint8_t * buf )
+{
+  // stuff payload as
+  // <dle><stx>payload<dle><etx>
+  // if payload contains <dle>, stuff <dle> as <dle><dle>
+  uint8_t dest[2*n+4*sizeof(uint8_t)]; // worst case, all payload is dle plus header plus footer
+  uint8_t *sp = buf, *dp = dest;
+
+  *dp++ = DLE;
+  *dp++ = STX;
+
+  while( n > 0 ) {
+    n--;
+
+    if( DLE == *sp )
+      *dp++ = DLE;
+
+    *dp++ = *sp++;
+  }
+
+  *dp++ = DLE;
+  *dp++ = ETX;
+
+  for( n = 0, sp = dest; sp < dp; ) {
+    n++;
+    *buf++ = *sp++;
+  } 
+
+  return n;
+}
+
+int FGSTXETXWrapper::unwrap( size_t n, uint8_t * buf )
+{
+  return n;
+}
+
+FGGeneric::FGGeneric(vector<string> tokens) : exitOnError(false), initOk(false), wrapper(NULL)
 {
     size_t configToken;
     if (tokens[1] == "socket") {
@@ -77,6 +261,7 @@ FGGeneric::FGGeneric(vector<string> tokens) : exitOnError(false), initOk(false)
 }
 
 FGGeneric::~FGGeneric() {
+  delete wrapper;
 }
 
 union u32 {
@@ -160,6 +345,16 @@ bool FGGeneric::gen_message_binary() {
             break;
         }
 
+        case FG_BYTE:
+        {
+            val = _out_message[i].offset +
+                  _out_message[i].prop->getIntValue() * _out_message[i].factor;
+            int8_t byteVal = val;
+            memcpy(&buf[length], &byteVal, sizeof(int8_t));
+            length += sizeof(int8_t);
+            break;
+        }
+
         default: // SG_STRING
             const char *strdata = _out_message[i].prop->getStringValue();
             int32_t strlength = strlen(strdata);
@@ -182,6 +377,7 @@ bool FGGeneric::gen_message_binary() {
              * length += (strlength % 4 > 0 ? sizeof(int32_t) - strlength % 4 : 0;
              */
             break;
+
         }
     }
 
@@ -205,6 +401,8 @@ bool FGGeneric::gen_message_binary() {
         length += sizeof(int32_t);
     }
 
+    if( wrapper ) length = wrapper->wrap( length, reinterpret_cast<uint8_t*>(buf) );
+
     return true;
 }
 
@@ -223,6 +421,7 @@ bool FGGeneric::gen_message_ascii() {
         string format = simgear::strutils::sanitizePrintfFormat(_out_message[i].format);
 
         switch (_out_message[i].type) {
+        case FG_BYTE:
         case FG_INT:
             val = _out_message[i].offset +
                   _out_message[i].prop->getIntValue() * _out_message[i].factor;
@@ -335,6 +534,12 @@ bool FGGeneric::parse_message_binary(int length) {
             p1 += sizeof(int64_t);
             break;
 
+        case FG_BYTE:
+            tmp32 = *(int8_t *)p1;
+            updateValue(_in_message[i], (int)tmp32);
+            p1 += sizeof(int8_t);
+            break;
+
         default: // SG_STRING
             SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
                     "Ignoring unsupported binary input chunk type.");
@@ -374,6 +579,7 @@ bool FGGeneric::parse_message_ascii(int length) {
         }
 
         switch (_in_message[i].type) {
+        case FG_BYTE:
         case FG_INT:
             updateValue(_in_message[i], atoi(p1));
             break;
@@ -679,6 +885,14 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
                        "byte order, using HOST byte order.");
             }
         }
+
+        if( root->hasValue( "wrapper" ) ) {
+            string w = root->getStringValue( "wrapper" );
+            if( w == "kiss" )  wrapper = new FGKissWrapper();
+            else if( w == "stxetx" )  wrapper = new FGSTXETXWrapper();
+            else SG_LOG(SG_IO, SG_ALERT,
+                       "generic protocol: Undefined binary protocol wrapper '" + w + "' ignored" );
+        }
     }
 
     int record_length = 0; // Only used for binary protocols.
@@ -697,8 +911,13 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
         chunk.wrap = chunks[i]->getBoolValue("wrap");
         chunk.rel = chunks[i]->getBoolValue("relative");
 
-        string node = chunks[i]->getStringValue("node", "/null");
-        chunk.prop = fgGetNode(node.c_str(), true);
+        if( chunks[i]->hasChild("const") ) {
+            chunk.prop = new SGPropertyNode();
+            chunk.prop->setStringValue( chunks[i]->getStringValue("const", "" ) );
+        } else {
+            string node = chunks[i]->getStringValue("node", "/null");
+            chunk.prop = fgGetNode(node.c_str(), true);
+        }
 
         string type = chunks[i]->getStringValue("type");
 
@@ -716,9 +935,12 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
         } else if (type == "fixed") {
             chunk.type = FG_FIXED;
             record_length += sizeof(int32_t);
-        } else if (type == "string")
+        } else if (type == "string") {
             chunk.type = FG_STRING;
-        else {
+        } else if (type == "byte") {
+            chunk.type = FG_BYTE;
+            record_length += sizeof(int8_t);
+        } else {
             chunk.type = FG_INT;
             record_length += sizeof(int32_t);
         }

diff --git a/src/Network/generic.hxx b/src/Network/generic.hxx
index 18493651cb1d3fb64badf305f7ad7d5a666eaab0..50c1805647c9f78a0084c489df3e258e6b0fc98b 100644 (file)
--- a/src/Network/generic.hxx
+++ b/src/Network/generic.hxx
@@ -33,7 +33,6 @@
 
 using std::string;
 
-
 class FGGeneric : public FGProtocol {
 
 public:
@@ -60,7 +59,7 @@ public:
     bool getInitOk(void) { return initOk; }
 protected:
 
-    enum e_type { FG_BOOL=0, FG_INT, FG_FLOAT, FG_DOUBLE, FG_STRING, FG_FIXED };
+    enum e_type { FG_BOOL=0, FG_INT, FG_FLOAT, FG_DOUBLE, FG_STRING, FG_FIXED, FG_BYTE };
 
     typedef struct {
      // string name;
@@ -104,6 +103,8 @@ private:
     bool read_config(SGPropertyNode *root, vector<_serial_prot> &msg);
     bool exitOnError;
     bool initOk;
+
+    class FGProtocolWrapper * wrapper;
     
     template<class T>
     static void updateValue(_serial_prot& prot, const T& val)