1 /*
2 * Copyright (c) 2024 Kelly Helmut Lord
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <errno.h>
8 #include <stdint.h>
9 #include <zephyr/data/cobs.h>
10
cobs_encode(struct net_buf * src,struct net_buf * dst,uint32_t flags)11 int cobs_encode(struct net_buf *src, struct net_buf *dst, uint32_t flags)
12 {
13 uint8_t delimiter = COBS_FLAG_CUSTOM_DELIMITER(flags);
14
15 /* Calculate required space for worst case */
16 size_t max_encoded_size = cobs_max_encoded_len(src->len, flags);
17
18 /* Check if destination has enough space */
19 if (net_buf_tailroom(dst) < max_encoded_size) {
20 return -ENOMEM;
21 }
22
23 uint8_t *code_ptr = net_buf_add(dst, 1);
24 uint8_t code = 1;
25
26 /* Process all input bytes */
27 uint8_t data = 0;
28
29 while (src->len > 0) {
30 data = net_buf_pull_u8(src);
31 if (data == delimiter) {
32 /* Delimiter found - write current code and start new block */
33 *code_ptr = code;
34 code_ptr = net_buf_add(dst, 1);
35 code = 1;
36 } else {
37 /* Add non-zero byte to output */
38 net_buf_add_u8(dst, data);
39 code++;
40
41 /* If we've reached maximum block size, start a new block */
42 if (code == 0xFF && (src->len - 1 >= 0)) {
43 *code_ptr = code;
44 code_ptr = net_buf_add(dst, 1);
45 code = 1;
46 }
47 }
48 }
49
50 *code_ptr = code;
51
52 if (flags & COBS_FLAG_TRAILING_DELIMITER) {
53 /* Add final delimiter */
54 net_buf_add_u8(dst, delimiter);
55 }
56
57 return 0;
58 }
59
cobs_decode(struct net_buf * src,struct net_buf * dst,uint32_t flags)60 int cobs_decode(struct net_buf *src, struct net_buf *dst, uint32_t flags)
61 {
62 uint8_t delimiter = COBS_FLAG_CUSTOM_DELIMITER(flags);
63
64 if (flags & COBS_FLAG_TRAILING_DELIMITER) {
65 uint8_t end_delim = net_buf_remove_u8(src);
66
67 if (end_delim != delimiter) {
68 return -EINVAL;
69 }
70 }
71
72 while (src->len > 0) {
73 /* Pull the COBS offset byte */
74 uint8_t offset = net_buf_pull_u8(src);
75
76 if (offset == delimiter && !(flags & COBS_FLAG_TRAILING_DELIMITER)) {
77 return -EINVAL;
78 }
79
80 /* Verify we have enough data */
81 if (src->len < (offset - 1)) {
82 return -EINVAL;
83 }
84
85 /* Copy offset-1 bytes */
86 for (uint8_t i = 0; i < offset - 1; i++) {
87 uint8_t byte = net_buf_pull_u8(src);
88
89 if (byte == delimiter) {
90 return -EINVAL;
91 }
92 net_buf_add_u8(dst, byte);
93 }
94
95 /* If this wasn't a maximum offset and we have more data,
96 * there was a delimiter here in the original data
97 */
98 if (offset != 0xFF && src->len > 0) {
99 net_buf_add_u8(dst, delimiter);
100 }
101 }
102
103 return 0;
104 }
105
