COBS functions

cobs.cpp

uint32_t UartDriver::stuffData(const uint8_t *src, uint32_t length, uint8_t *dst)
{
    uint32_t src_index = 0;
    uint32_t dst_index = 0;
    uint32_t last_zero_pos = 0;
    uint32_t last_zero_length = 0;

    if (length == 0)
    {
        return 0;
    }

    dst_index += 1; //First byte is overhead

    for (src_index = 0; src_index < length; src_index++)
    {
        last_zero_length += 1;

        if (last_zero_length == 0xff)
        {
            dst[last_zero_pos] = 0xff;
            last_zero_length = 1;
            last_zero_pos = dst_index;
            dst_index += 1;
        }

        if (src[src_index] == 0)
        {
            dst[last_zero_pos] = last_zero_length;
            last_zero_length = 0;
            last_zero_pos = dst_index;
        }
        else
        {
            dst[dst_index] = src[src_index];
        }

        dst_index += 1;
    }

    dst[last_zero_pos] = (last_zero_length + 1);
    dst[dst_index] = 0x00;
    dst_index += 1;

    return dst_index;
}


/**********************************************************************************************************************/


uint32_t UartDriver::unstuffData(const uint8_t *src, uint32_t length, uint8_t *dst)
{
    uint32_t src_index = 0;
    uint32_t dst_index = 0;
    uint32_t zero_offset = 0;
    uint32_t zero_val = 0xFF;

    while (src_index < length)
    {
        if (zero_offset != 0)
        {
            dst[dst_index] = src[src_index];
            src_index += 1;
            dst_index += 1;
        }
        else
        {
            if (zero_val != 0xFF)
            {
                dst[dst_index] = 0x00;
                dst_index += 1;
            }
            zero_val = src[src_index];
            zero_offset = src[src_index];
            src_index += 1;
            if (zero_val == 0x00)
            {
                break;
            }
        }
        zero_offset -= 1;
    }
    return dst_index;
}


/**********************************************************************************************************************/


QByteArray CobsStuffer::encode(QByteArray data)
{
    QByteArray result;

    int srcIndex = 0;
    int dstIndex = 0;
    int lastZeroPos = 0;
    int lastZeroLength = 0;

    if (data.length() == 0)
    {
        return result;
    }

    result.append(1, static_cast<char>(0xFF)); //temporary
    dstIndex += 1; //First byte is overhead

    for (srcIndex = 0; srcIndex < data.length(); srcIndex++)
    {
        lastZeroLength += 1;

        if (lastZeroLength == 0xff)
        {
            result[lastZeroPos] = static_cast<char>(0xFF);
            lastZeroLength = 1;
            lastZeroPos = dstIndex;
            result.append(1, static_cast<char>(0xFF));
            dstIndex += 1;
        }

        if (static_cast<uint8_t>(data[srcIndex]) == 0)
        {
            result[lastZeroPos] = static_cast<char>(lastZeroLength);
            lastZeroLength = 0;
            lastZeroPos = dstIndex;
            result.append(1, static_cast<char>(0xFF));
            dstIndex += 1;
        }
        else
        {
            result.append(1, data[srcIndex]);
            dstIndex += 1;
        }

    }

    result[lastZeroPos] = static_cast<char>(lastZeroLength + 1);
    result.append(1, 0x00);
    dstIndex += 1;

    return result;
}


/**********************************************************************************************************************/


QByteArray CobsStuffer::decode(QByteArray data)
{
    QByteArray result;

    int srcIndex = 0;
    uint8_t zeroOffset = 0;
    uint8_t zeroValue  = 0xFF;

    if (data.length() == 0)
    {
        return result;
    }

    while (srcIndex < data.length())
    {
        if (zeroOffset != 0)
        {
            result.append(data.at(srcIndex));
            srcIndex += 1;
        }
        else
        {
            if (zeroValue != 0xFF)
            {
                result.append(1, 0x00);
            }
            zeroValue = static_cast<uint8_t>(data.at(srcIndex));
            zeroOffset = static_cast<uint8_t>(data.at(srcIndex));
            srcIndex += 1;
            if (zeroValue == 0x00)
            {
                break;
            }
        }
        zeroOffset -= 1;
    }

    result = result.mid(0, result.length());

    return result;
}