1 /*---------------------------------------------------------------------------*\
2
3 FILE........: freedv_data_channel.c
4 AUTHOR......: Jeroen Vreeken
5 DATE CREATED: 03 March 2016
6
7 Data channel for ethernet like packets in freedv VHF frames.
8 Currently designed for-
9 * 2 control bits per frame
10 * 4 byte counter bits per frame
11 * 64 bits of data per frame
12
13 \*---------------------------------------------------------------------------*/
14
15 /*
16 Copyright (C) 2016 Jeroen Vreeken
17
18 All rights reserved.
19
20 This program is free software; you can redistribute it and/or modify
21 it under the terms of the GNU Lesser General Public License version 2.1, as
22 published by the Free Software Foundation. This program is
23 distributed in the hope that it will be useful, but WITHOUT ANY
24 WARRANTY; without even the implied warranty of MERCHANTABILITY or
25 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
26 License for more details.
27
28 You should have received a copy of the GNU Lesser General Public License
29 along with this program; if not, see <http://www.gnu.org/licenses/>.
30 */
31
32 #include "freedv_data_channel.h"
33
34 #include <stdlib.h>
35 #include <string.h>
36
37 static unsigned char fdc_header_bcast[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
38
39 /* CCIT CRC table (0x1201 polynomal) */
40 static unsigned short fdc_crc_table[256] = {
41 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
42 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
43 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
44 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
45 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
46 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
47 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
48 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
49 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
50 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
51 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
52 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
53 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
54 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
55 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
56 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
57 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
58 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
59 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
60 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
61 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
62 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
63 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
64 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
65 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
66 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
67 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
68 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
69 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
70 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
71 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
72 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
73 };
74
fdc_crc(unsigned char * buffer,size_t len)75 static unsigned short fdc_crc(unsigned char *buffer, size_t len)
76 {
77 unsigned short crc = 0xffff;
78 size_t i;
79
80 for (i = 0; i < len; i++, buffer++) {
81 crc = (crc >> 8) ^ fdc_crc_table[(crc ^ *buffer) & 0xff];
82 }
83
84 return crc ^ 0xffff;
85 }
86
87 /* CRC4 0x03 polynomal */
fdc_crc4(unsigned char * buffer,size_t len)88 static unsigned char fdc_crc4(unsigned char *buffer, size_t len)
89 {
90 unsigned char crc = 0x0f;
91 size_t i;
92
93 for (i = 0; i < len; i++, buffer++) {
94 int shift;
95
96 for (shift = 7; shift <= 0; shift--) {
97 crc <<= 1;
98 if ((*buffer >> shift) & 0x1)
99 crc |= 1;
100 if (crc & 0x10)
101 crc ^= 0x03;
102 }
103 }
104
105 return crc & 0x0f;
106 }
107
freedv_data_channel_create(void)108 struct freedv_data_channel *freedv_data_channel_create(void)
109 {
110 struct freedv_data_channel *fdc;
111
112 fdc = (struct freedv_data_channel *) calloc(1, sizeof(struct freedv_data_channel));
113 if (fdc == NULL)
114 return NULL;
115
116 freedv_data_set_header(fdc, fdc_header_bcast);
117
118 memcpy(fdc->rx_header, fdc->tx_header, 8);
119
120 return fdc;
121 }
122
freedv_data_channel_destroy(struct freedv_data_channel * fdc)123 void freedv_data_channel_destroy(struct freedv_data_channel *fdc)
124 {
125 free(fdc);
126 }
127
128
freedv_data_set_cb_rx(struct freedv_data_channel * fdc,freedv_data_callback_rx cb,void * state)129 void freedv_data_set_cb_rx(struct freedv_data_channel *fdc, freedv_data_callback_rx cb, void *state)
130 {
131 fdc->cb_rx = cb;
132 fdc->cb_rx_state = state;
133 }
134
freedv_data_set_cb_tx(struct freedv_data_channel * fdc,freedv_data_callback_tx cb,void * state)135 void freedv_data_set_cb_tx(struct freedv_data_channel *fdc, freedv_data_callback_tx cb, void *state)
136 {
137 fdc->cb_tx = cb;
138 fdc->cb_tx_state = state;
139 }
140
freedv_data_channel_rx_frame(struct freedv_data_channel * fdc,unsigned char * data,size_t size,int from_bit,int bcast_bit,int crc_bit,int end_bits)141 void freedv_data_channel_rx_frame(struct freedv_data_channel *fdc, unsigned char *data, size_t size, int from_bit, int bcast_bit, int crc_bit, int end_bits)
142 {
143 int copy_bits;
144 if (end_bits) {
145 copy_bits = end_bits;
146 } else {
147 copy_bits = size;
148 }
149
150 /* New packet? */
151 if (fdc->packet_rx_cnt == 0) {
152 /* Does the packet have a compressed from field? */
153 if (from_bit) {
154 /* Compressed from: take the previously received header */
155 memcpy(fdc->packet_rx + fdc->packet_rx_cnt, fdc->rx_header, 6);
156 fdc->packet_rx_cnt += 6;
157 }
158 if (bcast_bit) {
159 if (!from_bit) {
160 /* Copy from header and modify size and end_bits accordingly */
161 memcpy(fdc->packet_rx + fdc->packet_rx_cnt, data, 6);
162 fdc->packet_rx_cnt += 6;
163 copy_bits -= 6;
164 if (copy_bits < 0)
165 copy_bits = 0;
166 data += 6;
167 }
168 /* Compressed to: fill in broadcast address */
169 memcpy(fdc->packet_rx + fdc->packet_rx_cnt, fdc_header_bcast, sizeof(fdc_header_bcast));
170 fdc->packet_rx_cnt += 6;
171 }
172 if (crc_bit) {
173 unsigned char calc_crc = fdc_crc4(data, size);
174 if (calc_crc == end_bits) {
175 /* It is a single header field, remember it for later */
176 memcpy(fdc->rx_header, data, 6);
177 memcpy(fdc->packet_rx + 6, data, 6);
178 memcpy(fdc->packet_rx, fdc_header_bcast, 6);
179 if (fdc->cb_rx) {
180 fdc->cb_rx(fdc->cb_rx_state, fdc->packet_rx, 12);
181 }
182 }
183 fdc->packet_rx_cnt = 0;
184 return;
185 }
186 }
187
188 if (fdc->packet_rx_cnt + copy_bits >= FREEDV_DATA_CHANNEL_PACKET_MAX) {
189 /* Something went wrong... this can not be a real packet */
190 fdc->packet_rx_cnt = 0;
191 return;
192 }
193
194 memcpy(fdc->packet_rx + fdc->packet_rx_cnt, data, copy_bits);
195 fdc->packet_rx_cnt += copy_bits;
196
197 if (end_bits != 0 && fdc->packet_rx_cnt >= 2) {
198 unsigned short calc_crc = fdc_crc(fdc->packet_rx, fdc->packet_rx_cnt - 2);
199 unsigned short rx_crc;
200 rx_crc = fdc->packet_rx[fdc->packet_rx_cnt - 1] << 8;
201 rx_crc |= fdc->packet_rx[fdc->packet_rx_cnt - 2];
202
203 if (rx_crc == calc_crc) {
204 if (fdc->packet_rx_cnt == size) {
205 /* It is a single header field, remember it for later */
206 memcpy(fdc->rx_header, fdc->packet_rx, 6);
207 }
208
209 /* callback */
210 if (fdc->cb_rx) {
211 unsigned char tmp[6];
212 memcpy(tmp, fdc->packet_rx, 6);
213 memcpy(fdc->packet_rx, fdc->packet_rx + 6, 6);
214 memcpy(fdc->packet_rx + 6, tmp, 6);
215
216 size_t size = fdc->packet_rx_cnt - 2;
217 if (size < 12) {
218 size = 12;
219 memcpy(fdc->packet_rx, fdc_header_bcast, 6);
220 }
221 fdc->cb_rx(fdc->cb_rx_state, fdc->packet_rx, size);
222 }
223 }
224 fdc->packet_rx_cnt = 0;
225 }
226 }
227
freedv_data_channel_tx_frame(struct freedv_data_channel * fdc,unsigned char * data,size_t size,int * from_bit,int * bcast_bit,int * crc_bit,int * end_bits)228 void freedv_data_channel_tx_frame(struct freedv_data_channel *fdc, unsigned char *data, size_t size, int *from_bit, int *bcast_bit, int *crc_bit, int *end_bits)
229 {
230 *from_bit = 0;
231 *bcast_bit = 0;
232 *crc_bit = 0;
233
234 if (!fdc->packet_tx_size) {
235 fdc->packet_tx_cnt = 0;
236
237 if (fdc->cb_tx) {
238 fdc->packet_tx_size = FREEDV_DATA_CHANNEL_PACKET_MAX;
239 fdc->cb_tx(fdc->cb_tx_state, fdc->packet_tx, &fdc->packet_tx_size);
240 }
241 if (!fdc->packet_tx_size) {
242 /* Nothing to send, insert a header frame */
243 memcpy(fdc->packet_tx, fdc->tx_header, size);
244 if (size < 8) {
245 *end_bits = fdc_crc4(fdc->tx_header, size);
246 *crc_bit = 1;
247 memcpy(data, fdc->tx_header, size);
248
249 return;
250 } else {
251 fdc->packet_tx_size = size;
252 }
253 } else {
254 /* new packet */
255 unsigned short crc;
256 unsigned char tmp[6];
257
258 *from_bit = !memcmp(fdc->packet_tx + 6, fdc->tx_header, 6);
259 *bcast_bit = !memcmp(fdc->packet_tx, fdc_header_bcast, 6);
260
261 memcpy(tmp, fdc->packet_tx, 6);
262 memcpy(fdc->packet_tx, fdc->packet_tx + 6, 6);
263 memcpy(fdc->packet_tx + 6, tmp, 6);
264
265 crc = fdc_crc(fdc->packet_tx, fdc->packet_tx_size);
266
267 fdc->packet_tx[fdc->packet_tx_size] = crc & 0xff;
268 fdc->packet_tx_size++;
269 fdc->packet_tx[fdc->packet_tx_size] = (crc >> 8) & 0xff;
270 fdc->packet_tx_size++;
271
272 if (*from_bit) {
273 fdc->packet_tx_cnt = 6;
274 } else {
275 if (*bcast_bit) {
276 memcpy(fdc->packet_tx + 6, fdc->packet_tx, 6);
277 }
278 }
279 if (*bcast_bit) {
280 fdc->packet_tx_cnt += 6;
281 }
282 }
283 }
284 if (fdc->packet_tx_size) {
285 int copy = fdc->packet_tx_size - fdc->packet_tx_cnt;
286
287 if (copy > size) {
288 copy = size;
289 *end_bits = 0;
290 } else {
291 *end_bits = copy;
292 fdc->packet_tx_size = 0;
293 }
294 memcpy(data, fdc->packet_tx + fdc->packet_tx_cnt, copy);
295 fdc->packet_tx_cnt += copy;
296 }
297 }
298
freedv_data_set_header(struct freedv_data_channel * fdc,unsigned char * header)299 void freedv_data_set_header(struct freedv_data_channel *fdc, unsigned char *header)
300 {
301 unsigned short crc = fdc_crc(header, 6);
302
303 memcpy(fdc->tx_header, header, 6);
304 fdc->tx_header[6] = crc & 0xff;
305 fdc->tx_header[7] = (crc >> 8) & 0xff;
306 }
307
freedv_data_get_n_tx_frames(struct freedv_data_channel * fdc,size_t size)308 int freedv_data_get_n_tx_frames(struct freedv_data_channel *fdc, size_t size)
309 {
310 if (fdc->packet_tx_size == 0)
311 return 0;
312 /* packet will be send in 'size' byte frames */
313 return (fdc->packet_tx_size - fdc->packet_tx_cnt + size-1) / size;
314 }
315