cobs_encoding.h

//
// Created by Tom Danvers on 17/02/2023.
//
 
#ifndef MRAS_COBS_ENCODING_H
#define MRAS_COBS_ENCODING_H
 
#include <cstddef>
#include <cstdint>
#include <cassert>
 
// This code has been yoinked straight from Wikipedia
// https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
 
size_t cobsEncode(const void *data, size_t length, uint8_t *buffer)
{
    assert(data && buffer);
 
    uint8_t *encode = buffer; // Encoded byte pointer
    uint8_t *codep = encode++; // Output code pointer
    uint8_t code = 1; // Code value
 
    for (const auto *byte = (const uint8_t *)data; length--; ++byte)
    {
        if (*byte) // Byte not zero, write it
            *encode++ = *byte, ++code;
 
        if (!*byte || code == 0xff) // Input is zero or block completed, restart
        {
            *codep = code, code = 1, codep = encode;
            if (!*byte || length)
                ++encode;
        }
    }
    *codep = code; // Write final code value
 
    return (size_t)(encode - buffer);
}
 
size_t cobsDecode(const uint8_t *buffer, size_t length, void *data)
{
    assert(buffer && data);
 
    const uint8_t *byte = buffer; // Encoded input byte pointer
    uint8_t *decode = (uint8_t *)data; // Decoded output byte pointer
 
    for (uint8_t code = 0xff, block = 0; byte < buffer + length; --block)
    {
        if (block) // Decode block byte
            *decode++ = *byte++;
        else
        {
            if (code != 0xff) // Encoded zero, write it
                *decode++ = 0;
            block = code = *byte++; // Next block length
            if (!code) // Delimiter code found
                break;
        }
    }
 
    return (size_t)(decode - (uint8_t *)data);
}
 
#endif //MRAS_COBS_ENCODING_H