File content as of revision 0:736db4334c89:

/*  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;
    }
}