generic binary protocol: add 'word' datatype

[flightgear.git] / src / Network / generic.cxx

diff --git a/src/Network/generic.cxx b/src/Network/generic.cxx
index 90f2395a850319b154b27927445f439979962782..8372a5e4012a1df1addb10f4809ced426d747f1a 100644 (file)
--- a/src/Network/generic.cxx
+++ b/src/Network/generic.cxx
@@ -61,47 +61,46 @@ 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;
+  static const uint8_t  FEND;
+  static const uint8_t  FESC;
+  static const uint8_t  TFEND;
+  static const uint8_t  TFESC;
 };
 
+const uint8_t  FGKissWrapper::FEND  = 0xC0;
+const uint8_t  FGKissWrapper::FESC  = 0xDB;
+const uint8_t  FGKissWrapper::TFEND = 0xDC;
+const uint8_t  FGKissWrapper::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;
+  std::vector<uint8_t> dest;
+  uint8_t *sp = buf;
 
-  *dp++ = FEND;
-  *dp++ = 0; // command/channel always zero
+  dest.push_back(FEND);
+  dest.push_back(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;
+        dest.push_back(FESC);
+        dest.push_back(TFESC);
         break;
 
       case FEND:
-        *dp++ = FESC;
-        *dp++ = TFEND;
+        dest.push_back(FESC);
+        dest.push_back(TFEND);
         break;
 
       default:
-       *dp++ = c;
-       break;
+        dest.push_back(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;
+  dest.push_back(FEND);
+
+  memcpy( buf, dest.data(), dest.size() );
+  return dest.size();
 }
 
 int FGKissWrapper::unwrap( size_t n, uint8_t * buf )
@@ -122,9 +121,7 @@ int FGKissWrapper::unwrap( size_t n, uint8_t * buf )
 
   if( 0 == n ) return 0;
 
-  uint8_t dest[n];
-  uint8_t * dp = dest;
-
+  std::vector<uint8_t> dest;
   {
     bool escaped = false;
 
@@ -137,11 +134,11 @@ int FGKissWrapper::unwrap( size_t n, uint8_t * buf )
         switch( c ) {
 
           case TFESC:
-            *dp++ = FESC;
+            dest.push_back( FESC );
             break;
 
           case TFEND:
-            *dp++ = FEND;
+            dest.push_back( FEND );
             break;
 
           default: // this is an error - ignore and continue
@@ -166,21 +163,15 @@ int FGKissWrapper::unwrap( size_t n, uint8_t * buf )
             break;
 
           default:
-            *dp++ = c;
+            dest.push_back( c );
             break;
-
         }
       }
     }
   }
 
-  n = 0;
-  for( sp = dest; sp != dp; ) {
-    *buf++ = *sp++;
-    n++;
-  }
-
-  return n;
+  memcpy( buf, dest.data(), dest.size() );
+  return dest.size();
 }
 
 
@@ -189,40 +180,40 @@ 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;
+  static const uint8_t  STX;
+  static const uint8_t  ETX;
+  static const uint8_t  DLE;
 };
 
+const uint8_t  FGSTXETXWrapper::STX  = 0x02;
+const uint8_t  FGSTXETXWrapper::ETX  = 0x03;
+const uint8_t  FGSTXETXWrapper::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;
+  std::vector<uint8_t> dest;
+  uint8_t *sp = buf;
 
-  *dp++ = DLE;
-  *dp++ = STX;
+  dest.push_back(DLE);
+  dest.push_back(STX);
 
   while( n > 0 ) {
     n--;
 
     if( DLE == *sp )
-      *dp++ = DLE;
+      dest.push_back(DLE);
 
-    *dp++ = *sp++;
+    dest.push_back(*sp++);
   }
 
-  *dp++ = DLE;
-  *dp++ = ETX;
-
-  for( n = 0, sp = dest; sp < dp; ) {
-    n++;
-    *buf++ = *sp++;
-  } 
+  dest.push_back(DLE);
+  dest.push_back(ETX);
 
-  return n;
+  memcpy( buf, dest.data(), dest.size() ); 
+  return dest.size();
 }
 
 int FGSTXETXWrapper::unwrap( size_t n, uint8_t * buf )
@@ -355,6 +346,16 @@ bool FGGeneric::gen_message_binary() {
             break;
         }
 
+        case FG_WORD:
+        {
+            val = _out_message[i].offset +
+                  _out_message[i].prop->getIntValue() * _out_message[i].factor;
+            int16_t wordVal = val;
+            memcpy(&buf[length], &wordVal, sizeof(int16_t));
+            length += sizeof(int16_t);
+            break;
+        }
+
         default: // SG_STRING
             const char *strdata = _out_message[i].prop->getStringValue();
             int32_t strlength = strlen(strdata);
@@ -422,6 +423,7 @@ bool FGGeneric::gen_message_ascii() {
 
         switch (_out_message[i].type) {
         case FG_BYTE:
+        case FG_WORD:
         case FG_INT:
             val = _out_message[i].offset +
                   _out_message[i].prop->getIntValue() * _out_message[i].factor;
@@ -540,6 +542,16 @@ bool FGGeneric::parse_message_binary(int length) {
             p1 += sizeof(int8_t);
             break;
 
+        case FG_WORD:
+            if (binary_byte_order == BYTE_ORDER_NEEDS_CONVERSION) {
+                tmp32 = sg_bswap_16(*(int16_t *)p1);
+            } else {
+                tmp32 = *(int16_t *)p1;
+            }
+            updateValue(_in_message[i], (int)tmp32);
+            p1 += sizeof(int16_t);
+            break;
+
         default: // SG_STRING
             SG_LOG( SG_IO, SG_ALERT, "Generic protocol: "
                     "Ignoring unsupported binary input chunk type.");
@@ -580,6 +592,7 @@ bool FGGeneric::parse_message_ascii(int length) {
 
         switch (_in_message[i].type) {
         case FG_BYTE:
+        case FG_WORD:
         case FG_INT:
             updateValue(_in_message[i], atoi(p1));
             break;
@@ -940,6 +953,9 @@ FGGeneric::read_config(SGPropertyNode *root, vector<_serial_prot> &msg)
         } else if (type == "byte") {
             chunk.type = FG_BYTE;
             record_length += sizeof(int8_t);
+        } else if (type == "word") {
+            chunk.type = FG_WORD;
+            record_length += sizeof(int16_t);
         } else {
             chunk.type = FG_INT;
             record_length += sizeof(int32_t);