SDRAngel  4.11.5
Developer docs for <a href="https://github.com/f4exb/sdrangel">SDRangel<\a>, an Open Source Qt5 / OpenGL 3.0+ SDR and signal analyzer frontend to various hardware.
hdlc.h
Go to the documentation of this file.
1 // This file is part of LeanSDR Copyright (C) 2016-2018 <pabr@pabr.org>.
2 // See the toplevel README for more information.
3 //
4 // This program is free software: you can redistribute it and/or modify
5 // it under the terms of the GNU General Public License as published by
6 // the Free Software Foundation, either version 3 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU General Public License for more details.
13 //
14 // You should have received a copy of the GNU General Public License
15 // along with this program. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef LEANSDR_HDLC_H
18 #define LEANSDR_HDLC_H
19 
20 #include "leansdr/framework.h"
21 
22 namespace leansdr
23 {
24 
25 // HDLC deframer
26 
27 struct hdlc_dec
28 {
29 
30  hdlc_dec(int _minframesize, // Including CRC, excluding HDLC flags.
31  int _maxframesize,
32  bool _invert) : minframesize(_minframesize),
33  maxframesize(_maxframesize),
34  invertmask(_invert ? 0xff : 0),
35  framebuf(new u8[maxframesize]),
36  debug(false)
37  {
38  reset();
39  }
40 
41  void reset()
42  {
43  shiftreg = 0;
44  inframe = false;
45  }
46 
47  void begin_frame()
48  {
49  framesize = 0;
50  crc16 = crc16_init;
51  }
52 
53  // Decode (*ppin)[count] as MSB-packed HDLC bitstream.
54  // Return pointer to buffer[*pdatasize], or NULL if no valid frame.
55  // Return number of discarded bytes in *discarded.
56  // Return number of checksum errors in *fcs_errors.
57  // *ppin will have increased by at least 1 (unless count==0).
58 
59  u8 *decode(u8 **ppin, int count, int *pdatasize, int *hdlc_errors, int *fcs_errors)
60  {
61  *hdlc_errors = 0;
62  *fcs_errors = 0;
63  *pdatasize = -1;
64  u8 *pin = *ppin, *pend = pin + count;
65 
66  for (; pin < pend; ++pin)
67  {
68  u8 byte_in = (*pin) ^ invertmask;
69 
70  for (int bits = 8; bits--; byte_in <<= 1)
71  {
72  u8 bit_in = byte_in & 128;
73  shiftreg = (shiftreg >> 1) | bit_in;
74 
75  if (!inframe)
76  {
77  if (shiftreg == 0x7e)
78  { // HDLC flag 01111110
79  inframe = true;
80  nbits_out = 0;
81  begin_frame();
82  }
83  }
84  else
85  {
86  if ((shiftreg & 0xfe) == 0x7c)
87  { // 0111110x HDLC stuffing
88  // Unstuff this 0
89  }
90  else if (shiftreg == 0x7e)
91  { // 01111110 HDLC flag
92  if (nbits_out != 7)
93  {
94  // Not at byte boundary
95  if (debug)
96  fprintf(stderr, "^");
97  ++*hdlc_errors;
98  }
99  else
100  {
101  // Checksum
102  crc16 ^= 0xffff;
103  if (framesize < 2 || framesize < minframesize || crc16 != crc16_check)
104  {
105  if (debug)
106  fprintf(stderr, "!");
107  ++*hdlc_errors;
108  // Do not report random noise as FCS errors
109  if (framesize >= minframesize)
110  ++*fcs_errors;
111  }
112  else
113  {
114  if (debug)
115  fprintf(stderr, "_");
116  // This will trigger output, but we finish the byte first.
117  *pdatasize = framesize - 2;
118  }
119  }
120  nbits_out = 0;
121  begin_frame();
122  // Keep processing up to 7 remaining bits from byte_in.
123  // Special cases 0111111 and 1111111 cannot affect *pdatasize.
124  }
125  else if (shiftreg == 0xfe)
126  { // 11111110 HDLC invalid
127  if (framesize)
128  {
129  if (debug)
130  fprintf(stderr, "^");
131  ++*hdlc_errors;
132  }
133  inframe = false;
134  }
135  else
136  { // Data bit
137  byte_out = (byte_out >> 1) | bit_in; // HDLC is LSB first
138  ++nbits_out;
139  if (nbits_out == 8)
140  {
141  if (framesize < maxframesize)
142  {
145  }
146  nbits_out = 0;
147  }
148  }
149  } // inframe
150  } // bits
151  if (*pdatasize != -1)
152  {
153  // Found a complete frame
154  *ppin = pin + 1;
155  return framebuf;
156  }
157  }
158 
159  *ppin = pin;
160  return NULL;
161  }
162 
163  private:
164  // Config
167  u8 *framebuf; // [maxframesize]
168  // State
169  u8 shiftreg; // Input bitstream
170  bool inframe; // Currently receiving a frame ?
171  u8 byte_out; // Accumulator for data bits
172  int nbits_out; // Number of data bits in byte_out
173  int framesize; // Number of bytes in framebuf, if inframe
174  u16 crc16; // CRC of framebuf[framesize]
175  // CRC
176  static const u16 crc16_init = 0xffff;
177  static const u16 crc16_poly = 0x8408; // 0x1021 MSB-first
178  static const u16 crc16_check = 0x0f47;
179 
180  void crc16_byte(u8 data)
181  {
182  crc16 ^= data;
183 
184  for (int bit = 8; bit--;)
185  {
186  crc16 = (crc16 & 1) ? (crc16 >> 1) ^ crc16_poly : (crc16 >> 1);
187  }
188  }
189 
190  public:
191  bool debug;
192 };
193 // hdlc_dec
194 
195 // HDLC synchronizer with polarity detection
196 
198 {
199  hdlc_sync(scheduler *sch, pipebuf<u8> &_in, // Packed bits
200  pipebuf<u8> &_out, // Bytes
201  int _minframesize, // Including CRC, excluding HDLC flags.
202  int _maxframesize,
203  // Status
204  pipebuf<int> *_lock_out = NULL,
205  pipebuf<int> *_framecount_out = NULL,
206  pipebuf<int> *_fcserrcount_out = NULL,
207  pipebuf<int> *_hdlcbytecount_out = NULL,
208  pipebuf<int> *_databytecount_out = NULL) : runnable(sch, "hdlc_sync"),
209  minframesize(_minframesize),
210  maxframesize(_maxframesize),
211  chunk_size(maxframesize + 2),
212  in(_in),
213  out(_out, _maxframesize + chunk_size),
214  lock_out(opt_writer(_lock_out)),
215  framecount_out(opt_writer(_framecount_out)),
216  fcserrcount_out(opt_writer(_fcserrcount_out)),
217  hdlcbytecount_out(opt_writer(_hdlcbytecount_out)),
218  databytecount_out(opt_writer(_databytecount_out)),
219  cur_sync(0),
220  resync_phase(0),
221  lock_state(false),
222  resync_period(32),
223  header16(false)
224  {
225  for (int s = 0; s < NSYNCS; ++s)
226  {
227  syncs[s].dec = new hdlc_dec(minframesize, maxframesize, s != 0);
228 
229  for (int h = 0; h < NERRHIST; ++h)
230  syncs[s].errhist[h] = 0;
231  }
232 
233  syncs[cur_sync].dec->debug = sch->debug;
234  errslot = 0;
235  }
236 
237  void run()
238  {
239  if (!opt_writable(lock_out) || !opt_writable(framecount_out) || !opt_writable(fcserrcount_out) || !opt_writable(hdlcbytecount_out) || !opt_writable(databytecount_out))
240  {
241  return;
242  }
243 
244  bool previous_lock_state = lock_state;
245  int fcserrcount = 0, framecount = 0;
246  int hdlcbytecount = 0, databytecount = 0;
247 
248  // Note: hdlc_dec may already hold one frame ready for output.
249  while ((long)in.readable() >= chunk_size && (long)out.writable() >= maxframesize + chunk_size)
250  {
251  if (!resync_phase)
252  {
253  // Once every resync_phase, try all decoders
254  for (int s = 0; s < NSYNCS; ++s)
255  {
256  if (s != cur_sync)
257  syncs[s].dec->reset();
258 
259  syncs[s].errhist[errslot] = 0;
260 
261  for (u8 *pin = in.rd(), *pend = pin + chunk_size; pin < pend;)
262  {
263  int datasize, hdlc_errors, fcs_errors;
264  u8 *f = syncs[s].dec->decode(&pin, pend - pin, &datasize, &hdlc_errors, &fcs_errors);
265  syncs[s].errhist[errslot] += hdlc_errors;
266 
267  if (s == cur_sync)
268  {
269  if (f)
270  {
271  lock_state = true;
272  output_frame(f, datasize);
273  databytecount += datasize;
274  ++framecount;
275  }
276 
277  fcserrcount += fcs_errors;
278  framecount += fcs_errors;
279  }
280  }
281  }
282 
283  errslot = (errslot + 1) % NERRHIST;
284  // Switch to another sync option ?
285  // Compare total error counts over about NERRHIST frames.
286  int total_errors[NSYNCS];
287 
288  for (int s = 0; s < NSYNCS; ++s)
289  {
290  total_errors[s] = 0;
291 
292  for (int h = 0; h < NERRHIST; ++h)
293  total_errors[s] += syncs[s].errhist[h];
294  }
295 
296  int best = cur_sync;
297 
298  for (int s = 0; s < NSYNCS; ++s)
299  if (total_errors[s] < total_errors[best])
300  best = s;
301 
302  if (best != cur_sync)
303  {
304  lock_state = false;
305 
306  if (sch->debug)
307  fprintf(stderr, "[%d:%d->%d:%d]", cur_sync, total_errors[cur_sync], best, total_errors[best]);
308 
309  // No verbose messages on candidate syncs
310  syncs[cur_sync].dec->debug = false;
311  cur_sync = best;
312  syncs[cur_sync].dec->debug = sch->debug;
313  }
314  }
315  else
316  {
317  // Use only the currently selected decoder
318  for (u8 *pin = in.rd(), *pend = pin + chunk_size; pin < pend;)
319  {
320  int datasize, hdlc_errors, fcs_errors;
321  u8 *f = syncs[cur_sync].dec->decode(&pin, pend - pin, &datasize, &hdlc_errors, &fcs_errors);
322 
323  if (f)
324  {
325  lock_state = true;
326  output_frame(f, datasize);
327  databytecount += datasize;
328  ++framecount;
329  }
330 
331  fcserrcount += fcs_errors;
332  framecount += fcs_errors;
333  }
334  } // resync_phase
335 
336  in.read(chunk_size);
337  hdlcbytecount += chunk_size;
338 
339  if (++resync_phase >= resync_period)
340  resync_phase = 0;
341  } // Work to do
342 
343  if (lock_state != previous_lock_state)
344  opt_write(lock_out, lock_state ? 1 : 0);
345 
346  opt_write(framecount_out, framecount);
347  opt_write(fcserrcount_out, fcserrcount);
348  opt_write(hdlcbytecount_out, hdlcbytecount);
349  opt_write(databytecount_out, databytecount);
350  }
351 
352  private:
353  void output_frame(u8 *f, int size)
354  {
355  if (header16)
356  {
357  // Removed 16-bit CRC, add 16-bit prefix -> Still <= maxframesize.
358  out.write(size >> 8);
359  out.write(size & 255);
360  }
361 
362  memcpy(out.wr(), f, size);
363  out.written(size);
364  opt_write(framecount_out, 1);
365  }
366 
372  pipewriter<int> *framecount_out, *fcserrcount_out;
373  pipewriter<int> *hdlcbytecount_out, *databytecount_out;
374  static const int NSYNCS = 2; // Two possible polarities
375  static const int NERRHIST = 2; // Compare error counts over two frames
376 
377  struct
378  {
380  int errhist[NERRHIST];
381  } syncs[NSYNCS];
382 
383  int errslot;
384  int cur_sync;
387 
388  public:
390  bool header16; // Output length prefix
391 };
392 // hdlc_sync
393 
394 } // namespace leansdr
395 
396 #endif // LEANSDR_HDLC_H
static const u16 crc16_init
Definition: hdlc.h:176
hdlc_sync(scheduler *sch, pipebuf< u8 > &_in, pipebuf< u8 > &_out, int _minframesize, int _maxframesize, pipebuf< int > *_lock_out=NULL, pipebuf< int > *_framecount_out=NULL, pipebuf< int > *_fcserrcount_out=NULL, pipebuf< int > *_hdlcbytecount_out=NULL, pipebuf< int > *_databytecount_out=NULL)
Definition: hdlc.h:199
void crc16_byte(u8 data)
Definition: hdlc.h:180
static const u16 crc16_check
Definition: hdlc.h:178
unsigned char u8
Definition: framework.h:453
pipewriter< int > * hdlcbytecount_out
Definition: hdlc.h:373
int resync_phase
Definition: hdlc.h:385
pipewriter< int > * lock_out
Definition: hdlc.h:371
void run()
Definition: hdlc.h:237
void opt_write(pipewriter< T > *p, T val)
Definition: framework.h:341
void output_frame(u8 *f, int size)
Definition: hdlc.h:353
pipewriter< int > * framecount_out
Definition: hdlc.h:372
bool inframe
Definition: hdlc.h:170
bool opt_writable(pipewriter< T > *p, int n=1)
Definition: framework.h:335
void reset()
Definition: hdlc.h:41
int minframesize
Definition: hdlc.h:165
pipereader< u8 > in
Definition: hdlc.h:369
pipewriter< T > * opt_writer(pipebuf< T > *buf, unsigned long min_write=1)
Definition: framework.h:329
u8 * decode(u8 **ppin, int count, int *pdatasize, int *hdlc_errors, int *fcs_errors)
Definition: hdlc.h:59
u8 * framebuf
Definition: hdlc.h:167
hdlc_dec * dec
Definition: hdlc.h:379
static const u16 crc16_poly
Definition: hdlc.h:177
void begin_frame()
Definition: hdlc.h:47
hdlc_dec(int _minframesize, int _maxframesize, bool _invert)
Definition: hdlc.h:30
bool lock_state
Definition: hdlc.h:386
int resync_period
Definition: hdlc.h:389
int minframesize
Definition: hdlc.h:367
int maxframesize
Definition: hdlc.h:165
pipewriter< u8 > out
Definition: hdlc.h:370
unsigned short u16
Definition: framework.h:454