From 260b955f47229fb093e83c766a3bc7f327615589 Mon Sep 17 00:00:00 2001
From: Malcolm Robb <Support@ATTAvionics.com>
Date: Sun, 23 Feb 2014 01:55:34 +0000
Subject: [PATCH] BUGFIX : Beast Binary Escape characters
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Thanks to Blort on the PP list.

The Beast binary message stream uses the 0x1a character a and escape to
mark the beginning of a new message. However, the 0x1a character could
occur in the body of a message since the message is binary. Therefore,
the 0x1a is repeated  -as Blort put it :

This 56-bit Mode S Frame (containing a 1a) is supposed to look like
this:

1a  32 00 00 48 7b a6 1a 1a 0c  20 28 17 b0 c0 c3 b0

What is actually looks (looked) like (from wireshark) is this:

1a  32 00 00 48 7b a6 1a 0c  20 28 17 b0 c0 c3 b0

There are supposed to be two 1A’s (Gunter’s Escape Character) in a row
(1A 1A) whenever a data byte contains 1A, after the initial two 1A 3x
characters.
---
 net_io.c | 44 +++++++++++++++++++++++++++++++++++---------
 1 file changed, 35 insertions(+), 9 deletions(-)

diff --git a/net_io.c b/net_io.c
index 5d6b834..018c77f 100644
--- a/net_io.c
+++ b/net_io.c
@@ -189,7 +189,9 @@ void modesSendBeastOutput(struct modesMessage *mm) {
     char *p = &Modes.beastOut[Modes.beastOutUsed];
     int  msgLen = mm->msgbits / 8;
     char * pTimeStamp;
+    char ch;
     int  j;
+    int  iOutLen = msgLen + 9; // Escape, message type, timestamp, sigLevel and msg
 
     *p++ = 0x1a;
     if      (msgLen == MODES_SHORT_MSG_BYTES)
@@ -203,14 +205,19 @@ void modesSendBeastOutput(struct modesMessage *mm) {
 
     pTimeStamp = (char *) &mm->timestampMsg;
     for (j = 5; j >= 0; j--) {
-        *p++ = pTimeStamp[j];
+        *p++ = (ch = pTimeStamp[j]);
+        if (0x1A == ch) {*p++ = ch; iOutLen++;} 
     }
 
-    *p++ = mm->signalLevel;
+    *p++ = (ch = mm->signalLevel);
+    if (0x1A == ch) {*p++ = ch; iOutLen++;} 
 
-    memcpy(p, mm->msg, msgLen);
+    for (j = 0; j < msgLen; j++) {
+        *p++ = (ch = mm->msg[j]);
+        if (0x1A == ch) {*p++ = ch; iOutLen++;} 
+    }
 
-    Modes.beastOutUsed += (msgLen + 9);
+    Modes.beastOutUsed +=  iOutLen;
     if (Modes.beastOutUsed >= Modes.net_output_raw_size)
       {
       modesSendAllClients(Modes.bos, Modes.beastOut, Modes.beastOutUsed);
@@ -427,7 +434,9 @@ void modesQueueOutput(struct modesMessage *mm) {
 // case where we want broken messages here to close the client connection.
 //
 int decodeBinMessage(struct client *c, char *p) {
-    int msgLen = 0;
+    int  msgLen = 0;
+    int  j;
+    char ch;
     unsigned char msg[MODES_LONG_MSG_BYTES];
     struct modesMessage mm;
     MODES_NOTUSED(c);
@@ -445,9 +454,17 @@ int decodeBinMessage(struct client *c, char *p) {
         // Mark messages received over the internet as remote so that we don't try to
         // pass them off as being received by this instance when forwarding them
         mm.remote      =    1;
-        p += 7;                 // Skip the timestamp       
-        mm.signalLevel = *p++;  // Grab the signal level
-        memcpy(msg, p, msgLen); // and the data
+        for (j = 0; j < 7; j++) { // Skip the message type and timestamp
+          ch = *p++; 
+          if (0x1A == ch) {p++;}
+        }                        
+        mm.signalLevel = ch = *p++;  // Grab the signal level
+        if (0x1A == ch) {p++;}
+
+        for (j = 0; j < msgLen; j++) { // and the data
+          msg[j] = ch = *p++; 
+          if (0x1A == ch) {p++;}
+          }
 
         if (msgLen == MODEAC_MSG_BYTES) { // ModeA or ModeC
             decodeModeAMessage(&mm, ((msg[0] << 8) | msg[1])); 
@@ -767,7 +784,7 @@ void modesReadFromClient(struct client *c, char *sep,
     int nread;
     int fullmsg;
     int bContinue = 1;
-    char *s, *e;
+    char *s, *e, *p;
 
     while(bContinue) {
 
@@ -817,6 +834,15 @@ void modesReadFromClient(struct client *c, char *sep,
                     left = &(c->buf[c->buflen]) - e;
                     continue;
                 }
+                // we need to be careful of double escape characters in the message body
+                for (p = s; p < e; p++) {
+                    if (0x1A == *p) {
+                        p++; e++;
+                        if (e > &(c->buf[c->buflen])) {
+                            break;
+                        }
+                    }
+                }
                 left = &(c->buf[c->buflen]) - e;
                 if (left < 0) {                                // Incomplete message in buffer
                     e = s - 1;                                 // point back at last found 0x1a.