Diff: COBS.cpp

Revision:

0:736db4334c89

Child:

1:2b84a7482df6

diff -r 000000000000 -r 736db4334c89 COBS.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/COBS.cpp	Tue Feb 10 02:29:49 2015 +0000
@@ -0,0 +1,62 @@
+/*  Consistent Overhead Byte Stuffing (COBS)  
+   by Stuart Cheshire and Mary Baker
+
+ reference: http://conferences.sigcomm.org/sigcomm/1997/papers/p062.pdf
+
+
+COBS is a byte stuffing algorithm that encodes data bytes into a sequence that is easily 
+decoded, but simple to identify when blocks of data start.  The idea is elegant.  The sequence starts with 
+a zero, followed by the number of bytes until the next zero, in that location is the number of bytes again, etc.  
+To keep a data stream synchronized, all one must do is watch for the zeros, which will never occur except when
+a new byte sequence is starting.
+
+*/
+#include "COBS.h"
+/*
+* StuffData byte stuffs “length” bytes of
+* data at the location pointed to by “ptr”,
+* writing the output to the location pointed
+* to by “dst”.
+*/
+#define FinishBlock(X) \
+(*code_ptr = (X), \
+code_ptr = dst++, \
+code = 0x01)
+
+/* Stuff the data into an array */
+void COBS::StuffData(unsigned char *ptr, unsigned long length, unsigned char *dst)
+{
+    const unsigned char *end = ptr + length;
+    unsigned char *code_ptr = dst++;
+    unsigned char code = 0x01;
+    while (ptr < end)
+    {
+        if (*ptr == 0) FinishBlock(code);
+        else
+        {
+            *dst++ = *ptr;
+            code++;
+            if (code == 0xFF) FinishBlock(code);
+        }
+    ptr++;
+    }
+    
+    FinishBlock(code);
+}
+
+/*
+* UnStuffData decodes “length” bytes of
+* data at the location pointed to by “ptr”,
+* writing the output to the location pointed
+* to by “dst”.
+*/
+void COBS::UnStuffData(unsigned char *ptr, unsigned long length, unsigned char *dst)
+{
+    const unsigned char *end = ptr + length;
+    while (ptr < end)
+    {
+        int i, code = *ptr++;
+        for (i=1; i<code; i++) *dst++ = *ptr++;
+        if (code < 0xFF) *dst++ = 0;
+    }
+}
\ No newline at end of file