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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
|
/* Author: Jan Sucan */
#include "serial_port.h"
#include "bt.h"
#define COMM_PARAMS_COBS_DELIMITER_CHAR '$'
typedef enum {
COBS_WAITING_FOR_THE_FIRST_DELIMITER,
COBS_WAITING_FOR_STUFFED_BYTE,
COBS_WAITING_FOR_NON_STUFFED_BYTE
} cobs_state_t;
static HANDLE bt_serial_port;
static bt_retcode_t bt_retcode_from_usart_retcode(int ur);
void
bt_init(HANDLE serial_port)
{
bt_serial_port = serial_port;
}
bt_retcode_t
bt_receive_cobs_automaton(uint8_t b, uint8_t * const buf, size_t * const i, bool cobs_continue)
{
static cobs_state_t cobs_state = COBS_WAITING_FOR_THE_FIRST_DELIMITER;
static uint8_t next_non_stuffed = 0;
static bool interrupted = false;
bt_retcode_t r = BT_OK;
if (interrupted) {
// Delimiter bol prijaty v minulom volani tejto funkcie, 'b' je uz nasledujuci bajt
interrupted = false;
// Jeden prazdny priechod nadradenym for cyklom a pokracuje sa v aktualnej funkcii
++(*i);
}
// Frame delimiter resetuje stavovy automat a prijem bajtov
if (b == COMM_PARAMS_COBS_DELIMITER_CHAR) {
if (!cobs_continue) {
cobs_state = COBS_WAITING_FOR_STUFFED_BYTE;
*i = 0;
} else {
interrupted = true;
r = BT_RECEIVE_COBS_INTERRUPTED;
}
} else {
switch (cobs_state) {
case COBS_WAITING_FOR_THE_FIRST_DELIMITER:
// Automat deaktivovany, ztial sa neprijal frame delimiter
break;
case COBS_WAITING_FOR_STUFFED_BYTE:
// Prijal sa stuffovany byte, ziskame pocet nasledujuci nestuffovanych bajtov
next_non_stuffed = (b <= COMM_PARAMS_COBS_DELIMITER_CHAR) ? b : (b - 1);
// COBS header sa nezapisuje do dat, vsetky dalsie stuffovane ano
if (!cobs_continue && (*i > 1)) {
buf[*i - 2] = COMM_PARAMS_COBS_DELIMITER_CHAR;
} else {
buf[*i] = COMM_PARAMS_COBS_DELIMITER_CHAR;
}
cobs_state = (next_non_stuffed == 0) ? COBS_WAITING_FOR_STUFFED_BYTE : COBS_WAITING_FOR_NON_STUFFED_BYTE;
break;
case COBS_WAITING_FOR_NON_STUFFED_BYTE:
// Prijal sa nestuffovany bajt, nic sa s nim nerobi
--next_non_stuffed;
// Len sa ulozi
buf[*i - ((cobs_continue) ? 0 : 2)] = b;
cobs_state = (next_non_stuffed == 0) ? COBS_WAITING_FOR_STUFFED_BYTE : COBS_WAITING_FOR_NON_STUFFED_BYTE;
break;
default:
r = BT_RECEIVE_COBS_UNKNOWN_STATE;
break;
}
}
return r;
}
bt_retcode_t
bt_receive_cobs_data(uint8_t * const buf, size_t n, bool cobs_continue)
{
bt_retcode_t r = BT_RECEIVE_BAD_ARGUMENT;
// Kontrola argumentov
if (buf != NULL) {
size_t i;
// Na zaciatku dat sa prijma navyse COBS frame delimiter (1B) a COBS header (1B)
if (!cobs_continue) {
n += 2;
}
for (i = 0; i < n; ++i) {
uint8_t b;
const int ur = serial_port_read_byte(bt_serial_port, &b);
if (ur) {
// Chyba pri prijme bajtu
r = bt_retcode_from_usart_retcode(ur);
break;
}
const bt_retcode_t ar = bt_receive_cobs_automaton(b, buf, &i, cobs_continue);
if (ar != BT_OK) {
r = ar;
break;
}
}
// Uspesny prijem a odsfuttovanie vsetkych bajtov
if (i >= n) {
r = BT_OK;
}
}
return r;
}
void
bt_send_cobs_data_block(const uint8_t *const buf, size_t n)
{
uint8_t comm[2048U];
unsigned ci = 0;
// Kontrola argumentov
if (buf == NULL) {
return;
}
// POZOR: neosetrujeme velkost dat, moze dojst k preteceniu hodnot na stuffovanych bajtoch
// Odosle sa delimiter
comm[ci++] = COMM_PARAMS_COBS_DELIMITER_CHAR;
uint8_t next_non_stuffed = 0;
size_t send_index = 0 - 1;
for (size_t i = 0; i <= n; ++i) {
if ((i == n) || (buf[i] == COMM_PARAMS_COBS_DELIMITER_CHAR)) {
comm[ci++] = (next_non_stuffed >= COMM_PARAMS_COBS_DELIMITER_CHAR) ? (next_non_stuffed + 1) : next_non_stuffed;
// Zacne sa odosielat az za virtualnym, alebo realnym stuffovanym bajtom
++send_index;
// Odoslu sa napocitane bajty
while (next_non_stuffed > 0) {
comm[ci++] = buf[send_index];
++send_index;
--next_non_stuffed;
}
} else {
// Pocitaju sa nestuffovane bajty, zatial sa nic neposiela
++next_non_stuffed;
}
}
serial_port_write_byte(bt_serial_port, comm, ci);
}
bt_retcode_t
bt_retcode_from_usart_retcode(int ur)
{
bt_retcode_t r = BT_RECEIVE_BAD_ARGUMENT;
if (ur == -2) {
r = BT_RECEIVE_TIMEOUT;
} else if (ur != 0) {
r = BT_RECEIVE_ERROR;
} else {
r = BT_OK;
}
return r;
}
|
