mirror
/
pabr-leansdr
kopia lustrzana https://github.com/pabr/leansdr
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
pabr-leansdr/src/leansdr/hdlc.h

311 wiersze
9.4 KiB
C++
Czysty Wina Historia

// This file is part of LeanSDR Copyright (C) 2016-2018 <pabr@pabr.org>.
// See the toplevel README for more information.
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef LEANSDR_HDLC_H
#define LEANSDR_HDLC_H
#include "leansdr/framework.h"
namespace leansdr {
// HDLC deframer
struct hdlc_dec {
hdlc_dec(int _minframesize, // Including CRC, excluding HDLC flags.
int _maxframesize,
bool _invert)
: minframesize(_minframesize), maxframesize(_maxframesize),
invertmask(_invert?0xff:0),
framebuf(new u8[maxframesize]),
debug(false)
{
reset();
}
void reset() { shiftreg=0; inframe=false; }
void begin_frame() { framesize=0; crc16=crc16_init; }
// Decode (*ppin)[count] as MSB-packed HDLC bitstream.
// Return pointer to buffer[*pdatasize], or NULL if no valid frame.
// Return number of discarded bytes in *discarded.
// Return number of checksum errors in *fcs_errors.
// *ppin will have increased by at least 1 (unless count==0).
u8 *decode(u8 **ppin, int count,
int *pdatasize, int *hdlc_errors, int *fcs_errors) {
*hdlc_errors = 0;
*fcs_errors = 0;
*pdatasize = -1;
u8 *pin=*ppin, *pend=pin+count;
for ( ; pin<pend; ++pin ) {
u8 byte_in = (*pin) ^ invertmask;
for ( int bits=8; bits--; byte_in<<=1 ) {
u8 bit_in = byte_in & 128;
shiftreg = (shiftreg>>1) | bit_in;
if ( ! inframe ) {
if ( shiftreg == 0x7e ) { // HDLC flag 01111110
inframe = true;
nbits_out = 0;
begin_frame();
}
} else {
if ( (shiftreg&0xfe) == 0x7c ) { // 0111110x HDLC stuffing
// Unstuff this 0
} else if ( shiftreg == 0x7e ) { // 01111110 HDLC flag
if ( nbits_out != 7 ) {
// Not at byte boundary
if ( debug ) fprintf(stderr, "^");
++*hdlc_errors;
} else {
// Checksum
crc16 ^= 0xffff;
if ( framesize<2 || framesize<minframesize ||
crc16!=crc16_check ) {
if ( debug ) fprintf(stderr, "!");
++*hdlc_errors;
// Do not report random noise as FCS errors
if ( framesize >= minframesize ) ++*fcs_errors;
} else {
if ( debug ) fprintf(stderr, "_");
// This will trigger output, but we finish the byte first.
*pdatasize = framesize-2;
}
}
nbits_out = 0;
begin_frame();
// Keep processing up to 7 remaining bits from byte_in.
// Special cases 0111111 and 1111111 cannot affect *pdatasize.
} else if ( shiftreg == 0xfe ) { // 11111110 HDLC invalid
if ( framesize ) {
if ( debug ) fprintf(stderr, "^");
++*hdlc_errors;
}
inframe = false;
} else { // Data bit
byte_out = (byte_out>>1) | bit_in; // HDLC is LSB first
++nbits_out;
if ( nbits_out == 8 ) {
if ( framesize < maxframesize ) {
framebuf[framesize++] = byte_out;
crc16_byte(byte_out);
}
nbits_out = 0;
}
}
} // inframe
} // bits
if ( *pdatasize != -1 ) {
// Found a complete frame
*ppin = pin+1;
return framebuf;
}
}
*ppin = pin;
return NULL;
}
private:
// Config
int minframesize, maxframesize;
u8 invertmask;
u8 *framebuf; // [maxframesize]
// State
u8 shiftreg; // Input bitstream
bool inframe; // Currently receiving a frame ?
u8 byte_out; // Accumulator for data bits
int nbits_out; // Number of data bits in byte_out
int framesize; // Number of bytes in framebuf, if inframe
u16 crc16; // CRC of framebuf[framesize]
// CRC
static const u16 crc16_init = 0xffff;
static const u16 crc16_poly = 0x8408; // 0x1021 MSB-first
static const u16 crc16_check = 0x0f47;
void crc16_byte(u8 data) {
crc16 ^= data;
for ( int bit=8; bit--; )
crc16 = (crc16&1) ? (crc16>>1)^crc16_poly : (crc16>>1);
}
public:
bool debug;
}; // hdlc_dec
// HDLC synchronizer with polarity detection
struct hdlc_sync : runnable {
hdlc_sync(scheduler *sch,
pipebuf<u8> &_in, // Packed bits
pipebuf<u8> &_out, // Bytes
int _minframesize, // Including CRC, excluding HDLC flags.
int _maxframesize,
// Status
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)
: runnable(sch, "hdlc_sync"),
minframesize(_minframesize),
maxframesize(_maxframesize),
chunk_size(maxframesize+2),
in(_in), out(_out, _maxframesize+chunk_size),
lock_out(opt_writer(_lock_out)),
framecount_out(opt_writer(_framecount_out)),
fcserrcount_out(opt_writer(_fcserrcount_out)),
hdlcbytecount_out(opt_writer(_hdlcbytecount_out)),
databytecount_out(opt_writer(_databytecount_out)),
cur_sync(0), resync_phase(0),
lock_state(false),
resync_period(32),
header16(false)
{
for ( int s=0; s<NSYNCS; ++s ) {
syncs[s].dec = new hdlc_dec(minframesize, maxframesize, s!=0);
for ( int h=0; h<NERRHIST; ++h ) syncs[s].errhist[h] = 0;
}
syncs[cur_sync].dec->debug = sch->debug;
errslot = 0;
}
void run() {
if ( ! opt_writable(lock_out) ||
! opt_writable(framecount_out) ||
! opt_writable(fcserrcount_out) ||
! opt_writable(hdlcbytecount_out) ||
! opt_writable(databytecount_out) ) return;
bool previous_lock_state = lock_state;
int fcserrcount=0, framecount=0;
int hdlcbytecount=0, databytecount=0;
// Note: hdlc_dec may already hold one frame ready for output.
while ( in.readable() >= chunk_size &&
out.writable() >= maxframesize+chunk_size ) {
if ( ! resync_phase ) {
// Once every resync_phase, try all decoders
for ( int s=0; s<NSYNCS; ++s ) {
if ( s != cur_sync ) syncs[s].dec->reset();
syncs[s].errhist[errslot] = 0;
for ( u8 *pin=in.rd(), *pend=pin+chunk_size; pin<pend; ) {
int datasize, hdlc_errors, fcs_errors;
u8 *f = syncs[s].dec->decode(&pin, pend-pin, &datasize,
&hdlc_errors, &fcs_errors);
syncs[s].errhist[errslot] += hdlc_errors;
if ( s == cur_sync ) {
if ( f ) {
lock_state = true;
output_frame(f, datasize);
databytecount += datasize;
++framecount;
}
fcserrcount += fcs_errors;
framecount += fcs_errors;
}
}
}
errslot = (errslot+1) % NERRHIST;
// Switch to another sync option ?
// Compare total error counts over about NERRHIST frames.
int total_errors[NSYNCS];
for ( int s=0; s<NSYNCS; ++s ) {
total_errors[s] = 0;
for ( int h=0; h<NERRHIST; ++h )
total_errors[s] += syncs[s].errhist[h];
}
int best = cur_sync;
for ( int s=0; s<NSYNCS; ++s )
if ( total_errors[s] < total_errors[best] ) best = s;
if ( best != cur_sync ) {
lock_state = false;
if ( sch->debug ) fprintf(stderr, "[%d:%d->%d:%d]",
cur_sync, total_errors[cur_sync],
best, total_errors[best]);
// No verbose messages on candidate syncs
syncs[cur_sync].dec->debug = false;
cur_sync = best;
syncs[cur_sync].dec->debug = sch->debug;
}
} else {
// Use only the currently selected decoder
for ( u8 *pin=in.rd(), *pend=pin+chunk_size; pin<pend; ) {
int datasize, hdlc_errors, fcs_errors;
u8 *f = syncs[cur_sync].dec->decode(&pin, pend-pin, &datasize,
&hdlc_errors, &fcs_errors);
if ( f ) {
lock_state = true;
output_frame(f, datasize);
databytecount += datasize;
++framecount;
}
fcserrcount += fcs_errors;
framecount += fcs_errors;
}
} // resync_phase
in.read(chunk_size);
hdlcbytecount += chunk_size;
if ( ++resync_phase >= resync_period ) resync_phase = 0;
} // Work to do
if ( lock_state != previous_lock_state )
opt_write(lock_out, lock_state?1:0);
opt_write(framecount_out, framecount);
opt_write(fcserrcount_out, fcserrcount);
opt_write(hdlcbytecount_out, hdlcbytecount);
opt_write(databytecount_out, databytecount);
}
private:
void output_frame(u8 *f, int size) {
if ( header16 ) {
// Removed 16-bit CRC, add 16-bit prefix -> Still <= maxframesize.
out.write(size >> 8);
out.write(size & 255);
}
memcpy(out.wr(), f, size);
out.written(size);
opt_write(framecount_out, 1);
}
int minframesize, maxframesize;
int chunk_size;
pipereader<u8> in;
pipewriter<u8> out;
pipewriter<int> *lock_out;
pipewriter<int> *framecount_out, *fcserrcount_out;
pipewriter<int> *hdlcbytecount_out, *databytecount_out;
static const int NSYNCS = 2; // Two possible polarities
static const int NERRHIST = 2; // Compare error counts over two frames
struct {
hdlc_dec *dec;
int errhist[NERRHIST];
} syncs[NSYNCS];
int errslot;
int cur_sync;
int resync_phase;
bool lock_state;
public:
int resync_period;
bool header16; // Output length prefix
}; // hdlc_sync
} // namespace
#endif // LEANSDR_HDLC_H
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik