COBS.cpp

00001 /*  Consistent Overhead Byte Stuffing (COBS)  
00002    by Stuart Cheshire and Mary Baker
00003 
00004  reference: http://conferences.sigcomm.org/sigcomm/1997/papers/p062.pdf
00005 
00006 
00007 COBS is a byte stuffing algorithm that encodes data bytes into a sequence that is easily 
00008 decoded, but simple to identify when blocks of data start.  The idea is elegant.  The sequence starts with 
00009 a zero, followed by the number of bytes until the next zero, in that location is the number of bytes again, etc.  
00010 To keep a data stream synchronized, all one must do is watch for the zeros, which will never occur except when
00011 a new byte sequence is starting.
00012 
00013 */
00014 #include "COBS.h"
00015 /*
00016 * StuffData byte stuffs “length” bytes of
00017 * data at the location pointed to by “ptr”,
00018 * writing the output to the location pointed
00019 * to by “dst”.
00020 */
00021 #define FinishBlock(X) \
00022 (*code_ptr = (X), \
00023 code_ptr = dst++, \
00024 code = 0x01)
00025 
00026 /* Stuff the data into an array 
00027    Note: the ptr and dst buffers cannot be the same  */
00028 void COBS::StuffData(unsigned char *ptr, unsigned long length, unsigned char *dst)
00029 {
00030     const unsigned char *end = ptr + length;
00031     unsigned char *code_ptr = dst++;
00032     unsigned char code = 0x01;
00033     while (ptr < end)
00034     {
00035         if (*ptr == 0) FinishBlock(code);
00036         else
00037         {
00038             *dst++ = *ptr;
00039             code++;
00040             if (code == 0xFF) FinishBlock(code);
00041         }
00042     ptr++;
00043     }
00044     
00045     FinishBlock(code);
00046 }
00047 
00048 /*
00049 * UnStuffData decodes “length” bytes of
00050 * data at the location pointed to by “ptr”,
00051 * writing the output to the location pointed
00052 * to by “dst”.
00053 */
00054 int COBS::UnStuffData(unsigned char *ptr, unsigned long length, unsigned char *dst)
00055 {
00056     const unsigned char *end = ptr + length;
00057     while (ptr < end)
00058     {
00059         int i, code = *ptr++;
00060                 if (ptr+code-1 > end) return 1;  //if we will overun the end of the buffer exit
00061                                                                          //this is most likely to happen when decoding a malformed message
00062         for (i=1; i<code; i++)  *dst++ = *ptr++;
00063         if (code < 0xFF) *dst++ = 0;
00064     }
00065         return 0;
00066 }