forked from waymond91/APM2.5-6_Custom_Firmware
-
Notifications
You must be signed in to change notification settings - Fork 0
/
COBS.h
74 lines (70 loc) · 2.11 KB
/
COBS.h
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
#ifndef _COBS_H
#define _COBS_H
// COBS codec.
// Supports up to 250 bytes of payload.
size_t COBSEncode(const uint8_t * from, const size_t length, uint8_t * dst, const size_t dst_length) {
size_t from_ptr = 0; // Next byte to be read from "from" in loop
size_t dst_ptr = 1; // Next byte to be written to "dst" in loop
size_t code_ptr = 0; // Where to write "code" in "dst"
uint8_t code = 1;
while ((dst_ptr + 1) < dst_length) {
uint8_t in_byte = from[from_ptr];
from_ptr ++;
if (in_byte == 0) {
dst[code_ptr] = code;
// Reset code
code = 1;
code_ptr = dst_ptr;
dst_ptr ++;
} else {
dst[dst_ptr] = in_byte;
dst_ptr ++;
// Special case where code = 255 is not supported
code ++;
}
if (from_ptr >= length) {
dst[code_ptr] = code;
dst[dst_ptr] = 0;
// Return length with terminating 0
return dst_ptr + 1;
}
}
// Something is broken, tell dst_length is 0
return 0;
}
size_t COBSDecode(const uint8_t * encoded, const size_t length, uint8_t * dst, const size_t dst_length) {
// "encoded" contains only one frame and ends with 0
if (encoded[length - 1] != 0) {
// Bad input
return 0;
}
size_t in_ptr = 0; // Next byte to read
size_t out_ptr = 0; // Next byte to write
while (in_ptr < length) {
dst[out_ptr] = 0;
out_ptr ++;
if (encoded[in_ptr] == 0) {
// 0 should never appear in data
return 0;
}
size_t to_copy = encoded[in_ptr] - 1;
for (size_t i = 0; i < to_copy; i++) {
dst[out_ptr] = encoded[in_ptr + 1 + i];
out_ptr ++;
}
in_ptr += (to_copy + 1);
if (encoded[in_ptr] == 0) {
break;
}
}
// Remove leading zero
if (encoded[0] != 1 && dst[0] == 0) {
for (size_t i = 1; i < out_ptr; i++) {
dst[i-1] = dst[i];
}
out_ptr --;
}
// Return number of bytes written
return out_ptr;
}
#endif