1 /*---------------------------------------------------------------------------*\
2
3 FILE........: fsk.c
4 AUTHOR......: Brady O'Brien
5 DATE CREATED: 11 February 2016
6
7 Framer and deframer for VHF FreeDV modes 'A' and 'B'
8 Currently designed for-
9 * 40ms ota modem frames
10 * 40ms Codec2 1300 frames
11 * 52 bits of Codec2 per frame
12 * 16 bits of unique word per frame
13 * 28 'spare' bits per frame
14 * - 4 spare bits at front and end of frame (8 total) for padding
15 * - 20 'protocol' bits, either for higher layers of 'protocol' or
16 * - 18 'protocol' bits and 2 vericode sidechannel bits
17
18 \*---------------------------------------------------------------------------*/
19
20 /*
21 Copyright (C) 2016 David Rowe
22
23 All rights reserved.
24
25 This program is free software; you can redistribute it and/or modify
26 it under the terms of the GNU Lesser General Public License version 2.1, as
27 published by the Free Software Foundation. This program is
28 distributed in the hope that it will be useful, but WITHOUT ANY
29 WARRANTY; without even the implied warranty of MERCHANTABILITY or
30 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
31 License for more details.
32
33 You should have received a copy of the GNU Lesser General Public License
34 along with this program; if not, see <http://www.gnu.org/licenses/>.
35 */
36
37
38 #include <stdint.h>
39 #include <stdlib.h>
40 #include <stdio.h>
41 #include <string.h>
42 #include <assert.h>
43 #include "freedv_vhf_framing.h"
44 #include "freedv_api_internal.h"
45
46 /* The voice UW of the VHF type A frame */
47 static const uint8_t A_uw_v[] = {0,1,1,0,0,1,1,1,
48 1,0,1,0,1,1,0,1};
49
50 /* The data UW of the VHF type A frame */
51 static const uint8_t A_uw_d[] = {1,1,1,1,0,0,0,1,
52 1,1,1,1,1,1,0,0};
53
54 /* Blank VHF type A frame */
55 static const uint8_t A_blank[] = {1,0,1,0,0,1,1,1, /* Padding[0:3] Proto[0:3] */
56 1,0,1,0,0,1,1,1, /* Proto[4:11] */
57 0,0,0,0,0,0,0,0, /* Voice[0:7] */
58 0,0,0,0,0,0,0,0, /* Voice[8:15] */
59 0,0,0,0,0,0,0,0, /* Voice[16:23] */
60 0,1,1,0,0,1,1,1, /* UW[0:7] */
61 1,0,1,0,1,1,0,1, /* UW[8:15] */
62 0,0,0,0,0,0,0,0, /* Voice[24:31] */
63 0,0,0,0,0,0,0,0, /* Voice[32:39] */
64 0,0,0,0,0,0,0,0, /* Voice[40:47] */
65 0,0,0,0,0,0,1,0, /* Voice[48:51] Proto[12:15] */
66 0,1,1,1,0,0,1,0};/* Proto[16:19] Padding[4:7] */
67
68 /* Blank VHF type AT (A for TDMA; padding bits not transmitted) frame */
69 static const uint8_t AT_blank[] = { 0,1,1,1, /* Proto[0:3] */
70 1,0,1,0,0,1,1,1, /* Proto[4:11] */
71 0,0,0,0,0,0,0,0, /* Voice[0:7] */
72 0,0,0,0,0,0,0,0, /* Voice[8:15] */
73 0,0,0,0,0,0,0,0, /* Voice[16:23] */
74 0,1,1,0,0,1,1,1, /* UW[0:7] */
75 1,0,1,0,1,1,0,1, /* UW[8:15] */
76 0,0,0,0,0,0,0,0, /* Voice[24:31] */
77 0,0,0,0,0,0,0,0, /* Voice[32:39] */
78 0,0,0,0,0,0,0,0, /* Voice[40:47] */
79 0,0,0,0,0,0,1,0, /* Voice[48:51] Proto[12:15] */
80 0,1,1,1 };/* Proto[16:19] */
81
82 /* HF Type B voice UW */
83 static const uint8_t B_uw_v[] = {0,1,1,0,0,1,1,1};
84
85 /* HF Type B data UW */
86 static const uint8_t B_uw_d[] = {1,1,1,1,0,0,1,0};
87
88 /* Blank HF type B frame */
89 static const uint8_t B_blank[] = {0,1,1,0,0,1,1,1, /* UW[0:7] */
90 0,0,0,0,0,0,0,0, /* Voice1[0:7] */
91 0,0,0,0,0,0,0,0, /* Voice1[8:15] */
92 0,0,0,0,0,0,0,0, /* Voice1[16:23] */
93 0,0,0,0,0,0,0,0, /* Voice1[24:28] Voice2[0:3] */
94 0,0,0,0,0,0,0,0, /* Voice2[4:11] */
95 0,0,0,0,0,0,0,0, /* Voice2[12:19] */
96 0,0,0,0,0,0,0,0};/* Voice2[20:28] */
97
98 /* States */
99 #define ST_NOSYNC 0 /* Not synchronized */
100 #define ST_SYNC 1 /* Synchronized */
101
102 /* Get a single bit out of an MSB-first packed byte array */
103 #define UNPACK_BIT_MSBFIRST(bytes,bitidx) ((bytes)[(bitidx)>>3]>>(7-((bitidx)&0x7)))&0x1
104
105 enum frame_payload_type {
106 FRAME_PAYLOAD_TYPE_VOICE,
107 FRAME_PAYLOAD_TYPE_DATA,
108 };
109
110 /* Place codec and other bits into a frame */
fvhff_frame_bits(int frame_type,uint8_t bits_out[],uint8_t codec2_in[],uint8_t proto_in[],uint8_t vc_in[])111 void fvhff_frame_bits( int frame_type,
112 uint8_t bits_out[],
113 uint8_t codec2_in[],
114 uint8_t proto_in[],
115 uint8_t vc_in[]){
116 int i,ibit;
117 if(frame_type == FREEDV_VHF_FRAME_A){
118 /* Fill out frame with blank frame prototype */
119 for(i=0; i<96; i++)
120 bits_out[i] = A_blank[i];
121
122 /* Fill in protocol bits, if present */
123 if(proto_in!=NULL){
124 ibit = 0;
125 /* First half of protocol bits */
126 /* Extract and place in frame, MSB first */
127 for(i=4 ; i<16; i++){
128 bits_out[i] = UNPACK_BIT_MSBFIRST(proto_in,ibit);
129 ibit++;
130 }
131 /* Last set of protocol bits */
132 for(i=84; i<92; i++){
133 bits_out[i] = UNPACK_BIT_MSBFIRST(proto_in,ibit);
134 ibit++;
135 }
136 }
137
138 /* Fill in varicode bits, if present */
139 if(vc_in!=NULL){
140 bits_out[90] = vc_in[0];
141 bits_out[91] = vc_in[1];
142 }
143
144 /* Fill in codec2 bits, present or not */
145 ibit = 0;
146 for(i=16; i<40; i++){ /* First half */
147 bits_out[i] = UNPACK_BIT_MSBFIRST(codec2_in,ibit);
148 ibit++;
149 }
150 for(i=56; i<84; i++){ /* Second half */
151 bits_out[i] = UNPACK_BIT_MSBFIRST(codec2_in,ibit);
152 ibit++;
153 }
154 }else if(frame_type == FREEDV_HF_FRAME_B){
155 /* Pointers to both c2 frames so the bit unpack macro works */
156 uint8_t * codec2_in1 = &codec2_in[0];
157 uint8_t * codec2_in2 = &codec2_in[4];
158 /* Fill out frame with blank prototype */
159 for(i=0; i<64; i++)
160 bits_out[i] = B_blank[i];
161
162 /* Fill out first codec2 block */
163 ibit=0;
164 for(i=8; i<36; i++){
165 bits_out[i] = UNPACK_BIT_MSBFIRST(codec2_in1,ibit);
166 ibit++;
167 }
168 /* Fill out second codec2 block */
169 ibit=0;
170 for(i=36; i<64; i++){
171 bits_out[i] = UNPACK_BIT_MSBFIRST(codec2_in2,ibit);
172 ibit++;
173 }
174 }else if(frame_type == FREEDV_VHF_FRAME_AT){
175 /* Fill out frame with blank frame prototype */
176 for(i=0; i<88; i++)
177 bits_out[i] = AT_blank[i];
178
179 /* Fill in protocol bits, if present */
180 if(proto_in!=NULL){
181 ibit = 0;
182 /* First half of protocol bits */
183 /* Extract and place in frame, MSB first */
184 for(i=0 ; i<12; i++){
185 bits_out[i] = UNPACK_BIT_MSBFIRST(proto_in,ibit);
186 ibit++;
187 }
188 /* Last set of protocol bits */
189 for(i=80; i<88; i++){
190 bits_out[i] = UNPACK_BIT_MSBFIRST(proto_in,ibit);
191 ibit++;
192 }
193 }
194
195 /* Fill in varicode bits, if present */
196 if(vc_in!=NULL){
197 bits_out[86] = vc_in[0];
198 bits_out[87] = vc_in[1];
199 }
200
201 /* Fill in codec2 bits, present or not */
202 ibit = 0;
203 for(i=12; i<36; i++){ /* First half */
204 bits_out[i] = UNPACK_BIT_MSBFIRST(codec2_in,ibit);
205 ibit++;
206 }
207 for(i=52; i<80; i++){ /* Second half */
208 bits_out[i] = UNPACK_BIT_MSBFIRST(codec2_in,ibit);
209 ibit++;
210 }
211 }
212 }
213
214 /* Place data and other bits into a frame */
fvhff_frame_data_bits(struct freedv_vhf_deframer * def,int frame_type,uint8_t bits_out[])215 void fvhff_frame_data_bits(struct freedv_vhf_deframer * def, int frame_type,
216 uint8_t bits_out[]){
217 int i,ibit;
218 if(frame_type == FREEDV_VHF_FRAME_A){
219 uint8_t data[8];
220 int end_bits;
221 int from_bit;
222 int bcast_bit;
223 int crc_bit;
224
225 /* Fill out frame with blank frame prototype */
226 for(i=0; i<4; i++)
227 bits_out[i] = A_blank[i];
228 for(i=92; i<96; i++)
229 bits_out[i] = A_blank[i];
230
231 /* UW data */
232 for (i=0; i < 16; i++)
233 bits_out[40 + i] = A_uw_d[i];
234
235 if (def->fdc)
236 freedv_data_channel_tx_frame(def->fdc, data, 8, &from_bit, &bcast_bit, &crc_bit, &end_bits);
237 else
238 return;
239
240 bits_out[4] = from_bit;
241 bits_out[5] = bcast_bit;
242 bits_out[6] = 0; /* unused */
243 bits_out[7] = 0; /* unused */
244
245 /* Fill in data bits */
246 ibit = 0;
247 for(i=8; i<40; i++){ /* First half */
248 bits_out[i] = UNPACK_BIT_MSBFIRST(data,ibit);
249 ibit++;
250 }
251 for(i=56; i<88; i++){ /* Second half */
252 bits_out[i] = UNPACK_BIT_MSBFIRST(data,ibit);
253 ibit++;
254 }
255
256 for (i = 0; i < 4; i++)
257 bits_out[88 + i] = (end_bits >> (3-i)) & 0x1;
258 } else if (frame_type == FREEDV_HF_FRAME_B){
259 uint8_t data[6];
260 int end_bits;
261 int from_bit;
262 int bcast_bit;
263 int crc_bit;
264
265 /* Fill out frame with blank prototype */
266 for(i=0; i<64; i++)
267 bits_out[i] = B_blank[i];
268
269 /* UW data */
270 for (i=0; i < 8; i++)
271 bits_out[0 + i] = B_uw_d[i];
272
273 if (def->fdc)
274 freedv_data_channel_tx_frame(def->fdc, data, 6, &from_bit, &bcast_bit, &crc_bit, &end_bits);
275 else
276 return;
277
278 bits_out[56] = from_bit;
279 bits_out[57] = bcast_bit;
280 bits_out[58] = crc_bit;
281 bits_out[59] = 0; /* unused */
282
283 /* Fill in data bits */
284 ibit = 0;
285 for(i=8; i<56; i++){ /* First half */
286 bits_out[i] = UNPACK_BIT_MSBFIRST(data,ibit);
287 ibit++;
288 }
289 for (i = 0; i < 4; i++)
290 bits_out[60 + i] = (end_bits >> (3-i)) & 0x1;
291 }
292 }
293
294 /* Init and allocate memory for a freedv-vhf framer/deframer */
fvhff_create_deframer(uint8_t frame_type,int enable_bit_flip)295 struct freedv_vhf_deframer * fvhff_create_deframer(uint8_t frame_type, int enable_bit_flip){
296 struct freedv_vhf_deframer * deframer;
297 uint8_t *bits,*invbits;
298 int frame_size;
299 int uw_size;
300
301 assert( (frame_type == FREEDV_VHF_FRAME_A) || (frame_type == FREEDV_HF_FRAME_B) );
302
303 /* It's a Type A frame */
304 if(frame_type == FREEDV_VHF_FRAME_A){
305 frame_size = 96;
306 uw_size = 16;
307 }else if(frame_type == FREEDV_HF_FRAME_B){
308 frame_size = 64;
309 uw_size = 8;
310 }else{
311 return NULL;
312 }
313
314 /* Allocate memory for the thing */
315 deframer = malloc(sizeof(struct freedv_vhf_deframer));
316 if(deframer == NULL)
317 return NULL;
318
319 /* Allocate the not-bit buffer */
320 if(enable_bit_flip){
321 invbits = malloc(sizeof(uint8_t)*frame_size);
322 if(invbits == NULL) {
323 free(deframer);
324 return NULL;
325 }
326 }else{
327 invbits = NULL;
328 }
329
330 /* Allocate the bit buffer */
331 bits = malloc(sizeof(uint8_t)*frame_size);
332 if(bits == NULL) {
333 free(deframer);
334 return NULL;
335 }
336
337 deframer->bits = bits;
338 deframer->invbits = invbits;
339 deframer->ftype = frame_type;
340 deframer->state = ST_NOSYNC;
341 deframer->bitptr = 0;
342 deframer->last_uw = 0;
343 deframer->miss_cnt = 0;
344 deframer->frame_size = frame_size;
345 deframer->uw_size = uw_size;
346 deframer->on_inv_bits = 0;
347 deframer->sym_size = 1;
348
349 deframer->ber_est = 0;
350 deframer->total_uw_bits = 0;
351 deframer->total_uw_err = 0;
352
353 deframer->fdc = NULL;
354
355 return deframer;
356 }
357
358 /* Get size of frame in bits */
fvhff_get_frame_size(struct freedv_vhf_deframer * def)359 int fvhff_get_frame_size(struct freedv_vhf_deframer * def){
360 return def->frame_size;
361 }
362
363 /* Codec2 size in bytes */
fvhff_get_codec2_size(struct freedv_vhf_deframer * def)364 int fvhff_get_codec2_size(struct freedv_vhf_deframer * def){
365 if(def->ftype == FREEDV_VHF_FRAME_A){
366 return 7;
367 } else if(def->ftype == FREEDV_HF_FRAME_B){
368 return 8;
369 } else{
370 return 0;
371 }
372 }
373
374 /* Protocol bits in bits */
fvhff_get_proto_size(struct freedv_vhf_deframer * def)375 int fvhff_get_proto_size(struct freedv_vhf_deframer * def){
376 if(def->ftype == FREEDV_VHF_FRAME_A){
377 return 20;
378 } else if(def->ftype == FREEDV_HF_FRAME_B){
379 return 0;
380 } else{
381 return 0;
382 }
383 }
384
385 /* Varicode bits in bits */
fvhff_get_varicode_size(struct freedv_vhf_deframer * def)386 int fvhff_get_varicode_size(struct freedv_vhf_deframer * def){
387 if(def->ftype == FREEDV_VHF_FRAME_A){
388 return 2;
389 } else if(def->ftype == FREEDV_HF_FRAME_B){
390 return 0;
391 } else{
392 return 0;
393 }
394 }
395
fvhff_destroy_deframer(struct freedv_vhf_deframer * def)396 void fvhff_destroy_deframer(struct freedv_vhf_deframer * def){
397 freedv_data_channel_destroy(def->fdc);
398 free(def->bits);
399 free(def);
400 }
401
fvhff_synchronized(struct freedv_vhf_deframer * def)402 int fvhff_synchronized(struct freedv_vhf_deframer * def){
403 return (def->state) == ST_SYNC;
404 }
405
406 /* Search for a complete UW in a buffer of bits */
fvhff_search_uw(const uint8_t bits[],size_t nbits,const uint8_t uw[],size_t uw_len,size_t * delta_out,size_t bits_per_sym)407 size_t fvhff_search_uw(const uint8_t bits[],size_t nbits,
408 const uint8_t uw[], size_t uw_len,
409 size_t * delta_out, size_t bits_per_sym){
410
411 size_t ibits,iuw;
412 size_t delta_min = uw_len;
413 size_t delta;
414 size_t offset_min = 0;
415 /* Walk through buffer bits */
416 for(ibits = 0; ibits < nbits-uw_len; ibits+=bits_per_sym){
417 delta = 0;
418 for(iuw = 0; iuw < uw_len; iuw++){
419 if(bits[ibits+iuw] != uw[iuw]) delta++;
420 }
421 if( delta < delta_min ){
422 delta_min = delta;
423 offset_min = ibits;
424 }
425 }
426 if(delta_out != NULL) *delta_out = delta_min;
427 return offset_min;
428 }
429
430 /* See if the UW is where it should be, to within a tolerance, in a bit buffer */
fvhff_match_uw(struct freedv_vhf_deframer * def,uint8_t bits[],int tol,int * rdiff,enum frame_payload_type * pt)431 static int fvhff_match_uw(struct freedv_vhf_deframer * def,uint8_t bits[],int tol,int *rdiff, enum frame_payload_type *pt){
432 int frame_type = def->ftype;
433 int bitptr = def->bitptr;
434 int frame_size = def->frame_size;
435 int uw_len = def->uw_size;
436 int iuw,ibit;
437 const uint8_t * uw[2];
438 int uw_offset;
439 int diff[2] = { 0, 0 };
440 int i;
441 int match[2];
442 int r;
443
444 /* defaults to make compiler happy on -O3 */
445
446 *pt = FRAME_PAYLOAD_TYPE_VOICE;
447 *rdiff = 0;
448
449 /* Set up parameters for the standard type of frame */
450 if(frame_type == FREEDV_VHF_FRAME_A){
451 uw[0] = A_uw_v;
452 uw[1] = A_uw_d;
453 uw_len = 16;
454 uw_offset = 40;
455 } else if(frame_type == FREEDV_HF_FRAME_B){
456 uw[0] = B_uw_v;
457 uw[1] = B_uw_d;
458 uw_len = 8;
459 uw_offset = 0;
460 } else {
461 return 0;
462 }
463
464 /* Check both the voice and data UWs */
465 for (i = 0; i < 2; i++) {
466 /* Start bit pointer where UW should be */
467 ibit = bitptr + uw_offset;
468 if(ibit >= frame_size) ibit -= frame_size;
469 /* Walk through and match bits in frame with bits of UW */
470 for(iuw=0; iuw<uw_len; iuw++){
471 if(bits[ibit] != uw[i][iuw]) diff[i]++;
472 ibit++;
473 if(ibit >= frame_size) ibit = 0;
474 }
475 match[i] = diff[i] <= tol;
476 //fprintf(stderr, "diff[%d]: %d tol: %d\n", i, diff[i], tol);
477 }
478 /* Pick the best matching UW */
479
480 if (diff[0] < diff[1]) {
481 r = match[0];
482 *rdiff = diff[0];
483 *pt = FRAME_PAYLOAD_TYPE_VOICE;
484 } else {
485 r = match[1];
486 *rdiff = diff[1];
487 *pt = FRAME_PAYLOAD_TYPE_DATA;
488 }
489
490 return r;
491 }
492
fvhff_extract_frame_voice(struct freedv_vhf_deframer * def,uint8_t bits[],uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[])493 static void fvhff_extract_frame_voice(struct freedv_vhf_deframer * def,uint8_t bits[],
494 uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[]){
495 int frame_type = def->ftype;
496 int bitptr = def->bitptr;
497 int frame_size = def->frame_size;
498 int iframe,ibit;
499
500 if(frame_type == FREEDV_VHF_FRAME_A){
501 /* Extract codec2 bits */
502 memset(codec2_out,0,7);
503 ibit = 0;
504 /* Extract and pack first half, MSB first */
505 iframe = bitptr+16;
506 if(iframe >= frame_size) iframe-=frame_size;
507 for(;ibit<24;ibit++){
508 codec2_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
509 iframe++;
510 if(iframe >= frame_size) iframe=0;
511 }
512
513 /* Extract and pack last half, MSB first */
514 iframe = bitptr+56;
515 if(iframe >= frame_size) iframe-=frame_size;
516 for(;ibit<52;ibit++){
517 codec2_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
518 iframe++;
519 if(iframe >= frame_size) iframe=0;
520 }
521 /* Extract varicode bits, if wanted */
522 if(vc_out!=NULL){
523 iframe = bitptr+90;
524 if(iframe >= frame_size) iframe-=frame_size;
525 vc_out[0] = bits[iframe];
526 iframe++;
527 vc_out[1] = bits[iframe];
528 }
529 /* Extract protocol bits, if proto is passed through */
530 if(proto_out!=NULL){
531 /* Clear protocol bit array */
532 memset(proto_out,0,3);
533 ibit = 0;
534 /* Extract and pack first half, MSB first */
535 iframe = bitptr+4;
536 if(iframe >= frame_size) iframe-=frame_size;
537 for(;ibit<12;ibit++){
538 proto_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
539 iframe++;
540 if(iframe >= frame_size) iframe=0;
541 }
542
543 /* Extract and pack last half, MSB first */
544 iframe = bitptr+84;
545 if(iframe >= frame_size) iframe-=frame_size;
546 for(;ibit<20;ibit++){
547 proto_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
548 iframe++;
549 if(iframe >= frame_size) iframe=0;
550 }
551 }
552
553 }else if(frame_type == FREEDV_HF_FRAME_B){
554 /* Pointers to both c2 frames */
555 uint8_t * codec2_out1 = &codec2_out[0];
556 uint8_t * codec2_out2 = &codec2_out[4];
557
558 /* Extract codec2 bits */
559 memset(codec2_out,0,8);
560 ibit = 0;
561
562 /* Extract and pack first c2 frame, MSB first */
563 iframe = bitptr+8;
564 if(iframe >= frame_size) iframe-=frame_size;
565 for(;ibit<28;ibit++){
566 codec2_out1[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
567 iframe++;
568 if(iframe >= frame_size) iframe=0;
569 }
570
571 /* Extract and pack second c2 frame, MSB first */
572 iframe = bitptr+36;
573 ibit = 0;
574 if(iframe >= frame_size) iframe-=frame_size;
575 for(;ibit<28;ibit++){
576 codec2_out2[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
577 iframe++;
578 if(iframe >= frame_size) iframe=0;
579 }
580 }else if(frame_type == FREEDV_VHF_FRAME_AT){
581 /* Extract codec2 bits */
582 memset(codec2_out,0,7);
583 ibit = 0;
584 /* Extract and pack first half, MSB first */
585 iframe = bitptr+12;
586 if(iframe >= frame_size) iframe-=frame_size;
587 for(;ibit<24;ibit++){
588 codec2_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
589 iframe++;
590 if(iframe >= frame_size) iframe=0;
591 }
592
593 /* Extract and pack last half, MSB first */
594 iframe = bitptr+52;
595 if(iframe >= frame_size) iframe-=frame_size;
596 for(;ibit<52;ibit++){
597 codec2_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
598 iframe++;
599 if(iframe >= frame_size) iframe=0;
600 }
601 /* Extract varicode bits, if wanted */
602 if(vc_out!=NULL){
603 iframe = bitptr+86;
604 if(iframe >= frame_size) iframe-=frame_size;
605 vc_out[0] = bits[iframe];
606 iframe++;
607 vc_out[1] = bits[iframe];
608 }
609 /* Extract protocol bits, if proto is passed through */
610 if(proto_out!=NULL){
611 /* Clear protocol bit array */
612 memset(proto_out,0,3);
613 ibit = 0;
614 /* Extract and pack first half, MSB first */
615 iframe = bitptr+4;
616 if(iframe >= frame_size) iframe-=frame_size;
617 for(;ibit<12;ibit++){
618 proto_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
619 iframe++;
620 if(iframe >= frame_size) iframe=0;
621 }
622
623 /* Extract and pack last half, MSB first */
624 iframe = bitptr+84;
625 if(iframe >= frame_size) iframe-=frame_size;
626 for(;ibit<20;ibit++){
627 proto_out[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
628 iframe++;
629 if(iframe >= frame_size) iframe=0;
630 }
631 }
632
633 }
634 }
635
fvhff_extract_frame_data(struct freedv_vhf_deframer * def,uint8_t bits[])636 static void fvhff_extract_frame_data(struct freedv_vhf_deframer * def,uint8_t bits[]){
637 int frame_type = def->ftype;
638 int bitptr = def->bitptr;
639 int frame_size = def->frame_size;
640 int iframe,ibit;
641
642 if(frame_type == FREEDV_VHF_FRAME_A){
643 uint8_t data[8];
644 int end_bits = 0;
645 int from_bit;
646 int bcast_bit;
647
648 iframe = bitptr+4;
649 if(iframe >= frame_size) iframe-=frame_size;
650 from_bit = bits[iframe];
651 iframe++;
652 if(iframe >= frame_size) iframe-=frame_size;
653 bcast_bit = bits[iframe];
654
655 /* Extract data bits */
656 memset(data,0,8);
657 ibit = 0;
658 /* Extract and pack first half, MSB first */
659 iframe = bitptr+8;
660 if(iframe >= frame_size) iframe-=frame_size;
661 for(;ibit<32;ibit++){
662 data[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
663 iframe++;
664 if(iframe >= frame_size) iframe=0;
665 }
666
667 /* Extract and pack last half, MSB first */
668 iframe = bitptr+56;
669 if(iframe >= frame_size) iframe-=frame_size;
670 for(;ibit<64;ibit++){
671 data[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
672 iframe++;
673 if(iframe >= frame_size) iframe=0;
674 }
675
676 /* Extract endbits value, MSB first*/
677 iframe = bitptr+88;
678 ibit = 0;
679 if(iframe >= frame_size) iframe-=frame_size;
680 for(;ibit<4;ibit++){
681 end_bits |= (bits[iframe]&0x1)<<(3-(ibit));
682 iframe++;
683 if(iframe >= frame_size) iframe=0;
684 }
685
686 if (def->fdc) {
687 freedv_data_channel_rx_frame(def->fdc, data, 8, from_bit, bcast_bit, 0, end_bits);
688 }
689 } else if(frame_type == FREEDV_HF_FRAME_B){
690 uint8_t data[6];
691 int end_bits = 0;
692 int from_bit;
693 int bcast_bit;
694 int crc_bit;
695
696 ibit = 0;
697 memset(data,0,6);
698
699 /* Extract and pack first c2 frame, MSB first */
700 iframe = bitptr+8;
701 if(iframe >= frame_size) iframe-=frame_size;
702 for(;ibit<48;ibit++){
703 data[ibit>>3] |= (bits[iframe]&0x1)<<(7-(ibit&0x7));
704 iframe++;
705 if(iframe >= frame_size) iframe=0;
706 }
707
708 iframe = bitptr+56;
709 if(iframe >= frame_size) iframe-=frame_size;
710 from_bit = bits[iframe];
711 iframe++;
712 if(iframe >= frame_size) iframe-=frame_size;
713 bcast_bit = bits[iframe];
714 iframe++;
715 if(iframe >= frame_size) iframe-=frame_size;
716 crc_bit = bits[iframe];
717
718 /* Extract endbits value, MSB first*/
719 iframe = bitptr+60;
720 ibit = 0;
721 if(iframe >= frame_size) iframe-=frame_size;
722 for(;ibit<4;ibit++){
723 end_bits |= (bits[iframe]&0x1)<<(3-(ibit));
724 iframe++;
725 if(iframe >= frame_size) iframe=0;
726 }
727
728 if (def->fdc) {
729 freedv_data_channel_rx_frame(def->fdc, data, 6, from_bit, bcast_bit, crc_bit, end_bits);
730 }
731 }
732 }
733
fvhff_extract_frame(struct freedv_vhf_deframer * def,uint8_t bits[],uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[],enum frame_payload_type pt)734 static void fvhff_extract_frame(struct freedv_vhf_deframer * def,uint8_t bits[],uint8_t codec2_out[],
735 uint8_t proto_out[],uint8_t vc_out[],enum frame_payload_type pt){
736 switch (pt) {
737 case FRAME_PAYLOAD_TYPE_VOICE:
738 fvhff_extract_frame_voice(def, bits, codec2_out, proto_out, vc_out);
739 break;
740 case FRAME_PAYLOAD_TYPE_DATA:
741 fvhff_extract_frame_data(def, bits);
742 break;
743 }
744 }
745
746 /*
747 * Try to find the UW and extract codec/proto/vc bits in def->frame_size bits
748 */
fvhff_deframe_bits(struct freedv_vhf_deframer * def,uint8_t codec2_out[],uint8_t proto_out[],uint8_t vc_out[],uint8_t bits_in[])749 int fvhff_deframe_bits(struct freedv_vhf_deframer * def,uint8_t codec2_out[],uint8_t proto_out[],
750 uint8_t vc_out[],uint8_t bits_in[]){
751 uint8_t * strbits = def->bits;
752 uint8_t * invbits = def->invbits;
753 uint8_t * bits;
754 int on_inv_bits = def->on_inv_bits;
755 int frame_type = def->ftype;
756 int state = def->state;
757 int bitptr = def->bitptr;
758 int last_uw = def->last_uw;
759 int miss_cnt = def->miss_cnt;
760 int frame_size = def->frame_size;
761 int uw_size = def->uw_size;
762 int uw_diff;
763 int i;
764 int uw_first_tol;
765 int uw_sync_tol;
766 int miss_tol;
767 int extracted_frame = 0;
768 enum frame_payload_type pt = FRAME_PAYLOAD_TYPE_VOICE;
769
770 /* Possibly set up frame-specific params here */
771 if(frame_type == FREEDV_VHF_FRAME_A){
772 uw_first_tol = 1; /* The UW bit-error tolerance for the first frame */
773 uw_sync_tol = 3; /* The UW bit error tolerance for frames after sync */
774 miss_tol = 4; /* How many UWs may be missed before going into the de-synced state */
775 }else if(frame_type == FREEDV_HF_FRAME_B){
776 uw_first_tol = 0; /* The UW bit-error tolerance for the first frame */
777 uw_sync_tol = 1; /* The UW bit error tolerance for frames after sync */
778 miss_tol = 3; /* How many UWs may be missed before going into the de-synced state */
779 }else{
780 return 0;
781 }
782
783 /* Skip N bits for multi-bit symbol modems */
784 for(i=0; i<frame_size; i++){
785 /* Put a bit in the buffer */
786 strbits[bitptr] = bits_in[i];
787 /* If we're checking the inverted bitstream, put a bit in it */
788 if(invbits!=NULL)
789 invbits[bitptr] = bits_in[i]?0:1;
790
791 bitptr++;
792 if(bitptr >= frame_size) bitptr -= frame_size;
793 def->bitptr = bitptr;
794 /* Enter state machine */
795 if(state==ST_SYNC){
796 /* Already synchronized, just wait till UW is back where it should be */
797 last_uw++;
798 if(invbits!=NULL){
799 if(on_inv_bits)
800 bits = invbits;
801 else
802 bits = strbits;
803 }else{
804 bits=strbits;
805 }
806 /* UW should be here. We're sunk, so deframe anyway */
807 if(last_uw == frame_size){
808 last_uw = 0;
809
810 if(!fvhff_match_uw(def,bits,uw_sync_tol,&uw_diff, &pt))
811 miss_cnt++;
812 else
813 miss_cnt=0;
814
815 /* If we go over the miss tolerance, go into no-sync */
816 if(miss_cnt>miss_tol){
817 state = ST_NOSYNC;
818 }
819 /* Extract the bits */
820 extracted_frame = 1;
821 fvhff_extract_frame(def,bits,codec2_out,proto_out,vc_out,pt);
822
823 /* Update BER estimate */
824 def->ber_est = (.995*def->ber_est) + (.005*((float)uw_diff)/((float)uw_size));
825 def->total_uw_bits += uw_size;
826 def->total_uw_err += uw_diff;
827 }
828 /* Not yet sunk */
829 }else{
830 /* It's a sync!*/
831 if(invbits!=NULL){
832 if(fvhff_match_uw(def,invbits,uw_first_tol, &uw_diff, &pt)){
833 state = ST_SYNC;
834 last_uw = 0;
835 miss_cnt = 0;
836 extracted_frame = 1;
837 on_inv_bits = 1;
838 fvhff_extract_frame(def,invbits,codec2_out,proto_out,vc_out,pt);
839 /* Update BER estimate */
840 def->ber_est = (.995*def->ber_est) + (.005*((float)uw_diff)/((float)uw_size));
841 def->total_uw_bits += uw_size;
842 def->total_uw_err += uw_diff;
843 }
844 }
845 if(fvhff_match_uw(def,strbits,uw_first_tol, &uw_diff, &pt)){
846 state = ST_SYNC;
847 last_uw = 0;
848 miss_cnt = 0;
849 extracted_frame = 1;
850 on_inv_bits = 0;
851 fvhff_extract_frame(def,strbits,codec2_out,proto_out,vc_out,pt);
852 /* Update BER estimate */
853 def->ber_est = (.995*def->ber_est) + (.005*((float)uw_diff)/((float)uw_size));
854 def->total_uw_bits += uw_size;
855 def->total_uw_err += uw_diff;
856 }
857 }
858 }
859 def->state = state;
860 def->last_uw = last_uw;
861 def->miss_cnt = miss_cnt;
862 def->on_inv_bits = on_inv_bits;
863 /* return sync state and presence of extracted voice bits.
864 only sync for data frames, they are already handled by callback */
865 return (extracted_frame ? FREEDV_RX_SYNC : 0) | (pt == FRAME_PAYLOAD_TYPE_VOICE ? FREEDV_RX_BITS : 0);
866 }
867