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
|
/*
* KISS encode/decode.
* SPDX-License-Identifier: GPL-3.0-or-later
*/
#include "kiss.h"
void kiss_rx_init(kiss_rx_t *kx)
{
kx->len = 0;
kx->in_frame = false;
kx->esc = false;
}
bool kiss_rx_byte(kiss_rx_t *kx, uint8_t b, uint8_t *out, size_t *out_len)
{
if (b == KISS_FEND) {
if (kx->in_frame && kx->len >= 1) {
uint8_t cmd = kx->buf[0];
if ((cmd & 0x0Fu) == KISS_CMD_DATA && kx->len > 1) {
size_t n = kx->len - 1;
for (size_t i = 0; i < n; i++) {
out[i] = kx->buf[i + 1];
}
*out_len = n;
kx->len = 0;
kx->esc = false;
kx->in_frame = true;
return true;
}
}
kx->len = 0;
kx->esc = false;
kx->in_frame = true;
return false;
}
if (!kx->in_frame) {
return false;
}
if (kx->esc) {
if (b == KISS_TFEND) {
b = KISS_FEND;
} else if (b == KISS_TFESC) {
b = KISS_FESC;
}
kx->esc = false;
} else if (b == KISS_FESC) {
kx->esc = true;
return false;
}
if (kx->len < sizeof(kx->buf)) {
kx->buf[kx->len++] = b;
} else {
kx->in_frame = false;
kx->len = 0;
}
return false;
}
static bool push(uint8_t *dst, size_t cap, size_t *n, uint8_t b)
{
if (*n >= cap) {
return false;
}
dst[(*n)++] = b;
return true;
}
static bool push_esc(uint8_t *dst, size_t cap, size_t *n, uint8_t b)
{
if (b == KISS_FEND) {
return push(dst, cap, n, KISS_FESC) && push(dst, cap, n, KISS_TFEND);
}
if (b == KISS_FESC) {
return push(dst, cap, n, KISS_FESC) && push(dst, cap, n, KISS_TFESC);
}
return push(dst, cap, n, b);
}
size_t kiss_encode_data(const uint8_t *payload, size_t len,
uint8_t *dst, size_t dst_cap)
{
size_t n = 0;
if (!push(dst, dst_cap, &n, KISS_FEND)) {
return 0;
}
if (!push_esc(dst, dst_cap, &n, KISS_CMD_DATA)) {
return 0;
}
for (size_t i = 0; i < len; i++) {
if (!push_esc(dst, dst_cap, &n, payload[i])) {
return 0;
}
}
if (!push(dst, dst_cap, &n, KISS_FEND)) {
return 0;
}
return n;
}
|
