lib-peripherals/cobs.c at main · HydrogreenPollub/lib-peripherals · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
#include "cobs.h"

size_t cobs_encode(const uint8_t *data, size_t data_len,
                   uint8_t *encoded, size_t encoded_max) {
    if (!data || !encoded || data_len == 0) {
        return 0;
    }

    size_t read_idx = 0;
    size_t write_idx = 0;
    size_t code_idx;
    uint8_t code = 1u;

    /* Reserve slot for the first code byte */
    if (write_idx >= encoded_max) {
        return 0;
    }
    code_idx = write_idx++;

    while (read_idx < data_len) {
        if (data[read_idx] == 0x00u) {
            /* End of block: commit code, start a new block */
            encoded[code_idx] = code;
            code = 1u;
            if (write_idx >= encoded_max) {
                return 0;
            }
            code_idx = write_idx++;
        } else {
            if (write_idx >= encoded_max) {
                return 0;
            }
            encoded[write_idx++] = data[read_idx];
            code++;

            if (code == 0xFFu) {
                /* Block full (254 non-zero bytes): commit, start new block.
                 * No stuffed zero is inserted for 0xFF blocks. */
                encoded[code_idx] = code;
                code = 1u;
                if (read_idx + 1u < data_len) {
                    if (write_idx >= encoded_max) {
                        return 0;
                    }
                    code_idx = write_idx++;
                } else {
                    /* Last byte completed a full 254-byte run; no trailing
                     * code byte needed. */
                    return write_idx;
                }
            }
        }
        read_idx++;
    }

    /* Commit the final code byte */
    encoded[code_idx] = code;
    return write_idx;
}

size_t cobs_decode(const uint8_t *encoded, size_t encoded_len,
                   uint8_t *decoded, size_t decoded_max) {
    if (!encoded || !decoded || encoded_len == 0) {
        return 0;
    }

    size_t read_idx = 0;
    size_t write_idx = 0;

    while (read_idx < encoded_len) {
        uint8_t code = encoded[read_idx++];
        if (code == 0x00u) {
            return 0; /* 0x00 is not valid inside a COBS payload */
        }

        uint8_t block_len = code - 1u;

        for (uint8_t i = 0; i < block_len; i++) {
            if (read_idx >= encoded_len || write_idx >= decoded_max) {
                return 0;
            }
            decoded[write_idx++] = encoded[read_idx++];
        }

        /*
         * After each block, COBS implicitly inserts a 0x00 back – except:
         *   - code == 0xFF: 254 consecutive non-zero bytes, no stuffed zero
         *   - last block: the original data did not end with a 0x00
         */
        if (code != 0xFFu && read_idx < encoded_len) {
            if (write_idx >= decoded_max) {
                return 0;
            }
            decoded[write_idx++] = 0x00u;
        }
    }

    return write_idx;
}

void cobs_parser_init(cobs_parser_t *parser) {
    parser->len = 0;
}

void cobs_parser_feed(cobs_parser_t *parser, uint8_t byte,
                      cobs_frame_cb_t cb, void *user_data) {
    if (byte == 0x00u) {
        if (parser->len > 0) {
            uint8_t decoded[COBS_PARSER_BUF_SIZE];
            size_t  dlen = cobs_decode(parser->buf, parser->len,
                                       decoded, sizeof(decoded));
            if (dlen > 0 && cb) {
                cb(decoded, dlen, user_data);
            }
            parser->len = 0;
        }
    } else {
        if (parser->len < COBS_PARSER_BUF_SIZE) {
            parser->buf[parser->len++] = byte;
        } else {
            /* Buffer overflow — discard frame and reset */
            parser->len = 0;
        }
    }
}