diff options
author | Matthias P. Braendli <matthias.braendli@mpb.li> | 2019-06-25 17:13:27 +0200 |
---|---|---|
committer | Matthias P. Braendli <matthias.braendli@mpb.li> | 2019-06-25 17:13:27 +0200 |
commit | 899dcb83ec873cb35d38583d6f48922e1312e9be (patch) | |
tree | 42cfe4ddc96b52cb9f365f617c556f7e7153f5eb /src/AVTEDIInput.cpp | |
parent | 8bba5052cf7a3677e8be12315a03959fec33bf17 (diff) | |
download | ODR-SourceCompanion-899dcb83ec873cb35d38583d6f48922e1312e9be.tar.gz ODR-SourceCompanion-899dcb83ec873cb35d38583d6f48922e1312e9be.tar.bz2 ODR-SourceCompanion-899dcb83ec873cb35d38583d6f48922e1312e9be.zip |
Replace socket library
Diffstat (limited to 'src/AVTEDIInput.cpp')
-rw-r--r-- | src/AVTEDIInput.cpp | 738 |
1 files changed, 0 insertions, 738 deletions
diff --git a/src/AVTEDIInput.cpp b/src/AVTEDIInput.cpp deleted file mode 100644 index f8a9e60..0000000 --- a/src/AVTEDIInput.cpp +++ /dev/null @@ -1,738 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 2017 AVT GmbH - Fabien Vercasson - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ - -#include "AVTEDIInput.h" -#include <cstring> -#include <cstdio> -#include <stdint.h> -#include <limits.h> - -#include "crc.h" -#include "OrderedQueue.h" - -extern "C" { -#include <fec.h> -} - -#define SUBCH_QUEUE_SIZE (50) /* In 24ms frames. Intermediate buffer */ - -#define RS_DECODE 1 /* Set to 0 to disable rs decoding */ -#define RS_TEST1 0 /* Remove one fragment on each PFT */ -#define RS_TEST2 0 /* Remove regularily fragments */ -#define RS_TEST2_NBDROP 3 /* For RS_TEST2, nb packet remove on each time */ - -#define PRINTF(fmt, A...) fprintf(stderr, fmt, ##A) -//#define PRINTF(x ...) -#define INFO(fmt, A...) fprintf(stderr, "AVT EDI: " fmt, ##A) -//#define DEBUG(fmt, A...) fprintf(stderr, "AVT EDI: " fmt, ##A) -#define DEBUG(X...) -#define ERROR(fmt, A...) fprintf(stderr, "AVT EDI: ERROR " fmt, ##A) - -static int hideFirstPFTErrors = 30; /* Hide the errors that can occurs on */ - /* the first PFT, as they are likely incomplete */ - -#define TAG_NAME_DETI (('d'<<24)|('e'<<16)|('t'<<8)|('i')) -#define TAG_NAME_EST (('e'<<24)|('s'<<16)|('t'<<8)) - -/* ------------------------------------------------------------------ -static void _dump(const uint8_t* buf, int size) -{ - for( int i = 0 ; i < size ; i ++) - { - PRINTF("%02X ", buf[i]); - if( (i+1) % 16 == 0 ) PRINTF("\n"); - } - if( size % 16 != 0 ) PRINTF("\n"); -} -*/ - -/* ------------------------------------------------------------------ - * - */ -static uint32_t unpack2(const uint8_t* buf) -{ - return( buf[0] << 8 | - buf[1]); -} - -/* ------------------------------------------------------------------ - * - */ -static uint32_t unpack3(const uint8_t* buf) -{ - return( buf[0] << 16 | - buf[1] << 8 | - buf[2]); -} - -/* ------------------------------------------------------------------ - * - */ -static uint32_t unpack4(const uint8_t* buf) -{ - return( buf[0] << 24 | - buf[1] << 16 | - buf[2] << 8 | - buf[3]); -} - -/* ------------------------------------------------------------------ - * bitpos 0 : left most bit. - * - */ -static uint32_t unpack1bit(uint8_t byte, int bitpos) -{ - return (byte & 1 << (7-bitpos)) > (7-bitpos); -} - - -/* ------------------------------------------------------------------ - * - */ -static bool _checkCRC(uint8_t* buf, size_t length) -{ - if (length <= 2) return false; - - uint16_t CRC = unpack2(buf+length-2); - - uint16_t crc = 0xffff; - crc = crc16(crc, buf, length-2); - crc ^= 0xffff; - - return (CRC == crc); -} - -/* ------------------------------------------------------------------ - * - */ -AVTEDIInput::AVTEDIInput(uint32_t fragmentTimeoutMs) - : _fragmentTimeoutMs(fragmentTimeoutMs) -{ - _subChannelQueue = new OrderedQueue(5000, SUBCH_QUEUE_SIZE); -} - -/* ------------------------------------------------------------------ - * - */ -AVTEDIInput::~AVTEDIInput() -{ - PFTIterator it = _pft.begin(); - while (it != _pft.end()) { - delete it->second; - it++; - } - delete _subChannelQueue; -} - -/* ------------------------------------------------------------------ - * - */ -bool AVTEDIInput::pushData(uint8_t* buf, size_t length) -{ - bool identified = false; - - if (length >= 12 && buf[0] == 'P' && buf[1] == 'F') - { - -#if RS_TEST2 - static int count=0; - if (++count%1421<RS_TEST2_NBDROP) - identified = true; - else -#endif // RS_TEST2 - identified = _pushPFTFrag(buf, length); - - } - else if (length >= 10 && buf[0] == 'A' && buf[1] == 'F') - { - identified = _pushAF(buf, length, false); - } - return identified; -} - -/* ------------------------------------------------------------------ - * - */ -size_t AVTEDIInput::popFrame(std::vector<uint8_t>& data, int32_t& frameNumber) -{ - return _subChannelQueue->pop(data, &frameNumber); -} - -/* ------------------------------------------------------------------ - * - */ -bool AVTEDIInput::_pushPFTFrag(uint8_t* buf, size_t length) -{ - PFTFrag* frag = new PFTFrag(buf, length); - bool isValid = frag->isValid(); - if (!isValid) { - delete frag; - } else { - // Find PFT - PFT* pft = NULL; - PFTIterator it = _pft.find(frag->Pseq()); - if (it != _pft.end()) { - pft = it->second; - } else { - // create PFT is new - pft = new PFT(frag->Pseq(), frag->Fcount()); - if (_pft.insert(std::make_pair(frag->Pseq(), pft)).second == false) - { - // Not inserted - delete pft; - pft = NULL; - } - it = _pft.find(frag->Pseq()); - } - - if (pft) { - // Add frag to PFT - pft->pushPFTFrag(frag); - - // If the PFT is complete, extract the AF - if (pft->complete()) { - std::vector<uint8_t> af; - bool ok = pft->extractAF(af); - - if (ok) { - _pushAF(af.data(), af.size(), ok); - } else { - ERROR("AF Frame Corrupted, Size=%zu\n", af.size()); - //_dump(af.data(), 10); - } - - _pft.erase(it); - delete pft; - } - } - } - - // Check old incomplete PFT to either try to extract AF or discard it - // TODO - const auto now = std::chrono::steady_clock::now(); - const auto timeout_duration = std::chrono::milliseconds(_fragmentTimeoutMs); - - PFTIterator it = _pft.begin(); - while (it != _pft.end()) { - PFT* pft = it->second; - bool erased = false; - if (pft) { - const auto creation = pft->creation(); - const auto diff = now - creation; - if (diff > timeout_duration) { - //DEBUG("PFT timeout\n"); - std::vector<uint8_t> af; - bool ok = pft->extractAF(af); - if (ok) { - _pushAF(af.data(), af.size(), ok); - } else { - //ERROR("AF Frame CorruptedSize=%zu\n", af.size()); - //_dump(af.data(), 10); - } - - it = _pft.erase(it); - delete pft; - erased = true; - } - } - if (!erased) ++it; - } - - return isValid; -} - -/* ------------------------------------------------------------------ - * - */ -bool AVTEDIInput::_pushAF(uint8_t* buf, size_t length, bool checked) -{ - bool ok = checked; - - // Check the AF integrity - if (!ok) { - // EDI specific, must have a CRC. - if (length >= 12) { - ok = (buf[0] == 'A' && buf[1] == 'F'); - ok &= _checkCRC(buf, length); - } - } - - int index = 0; - - index += 2; - uint32_t LEN = unpack4(buf+index); index += 4; - ok = (LEN == length-12); - //uint32_t SEQ = unpack2(buf+index); index += 2; - - if (ok) { - uint32_t CF = unpack1bit(buf[index], 0); - uint32_t MAJ = (buf[index]&0x70) >> 4; - uint32_t MIN = (buf[index]&0x0F); - index += 1; - uint32_t PT = buf[index]; index += 1; - - // EDI specific - ok = (CF == 1 && PT == 'T' && MAJ == 1 && MIN == 0); - -// DEBUG("AF Header: LEN=%u SEQ=%u CF=%u MAJ=%u MIN=%u PT=%c ok=%d\n", -// LEN, SEQ, CF, MAJ, MIN, PT, ok); - } - - if (ok) { - // Extract the first stream and FrameCount from AF - int tagIndex = index; - uint32_t frameCount = 0; - bool frameCountFound = false; - int est0Index = 0; - size_t est0Length = 0; - // Iterate through tags - while (tagIndex < (ssize_t)length - 2/*CRC*/ - 8/*Min tag length*/ && (!frameCountFound || est0Index==0) ) - { - uint32_t tagName = unpack4(buf+tagIndex); tagIndex += 4; - uint32_t tagLen = unpack4(buf+tagIndex); tagIndex += 4; - uint32_t tagLenByte = (tagLen+7)/8; -// DEBUG("TAG %c%c%c%c size %u bits %u bytes\n", -// tagName>>24&0xFF, tagName>>16&0xFF, tagName>>8&0xFF, tagName&0xFF, -// tagLen, tagLenByte); - - if (tagName == TAG_NAME_DETI) { - uint32_t FCTH = buf[tagIndex] & 0x1F; - uint32_t FCT = buf[tagIndex+1]; - frameCount = FCTH * 250 + FCT; - frameCountFound = true; -// DEBUG("frameCount=%u\n", frameCount); - } else if ((tagName & 0xFFFFFF00) == TAG_NAME_EST) { - est0Index = tagIndex+3 /*3 bytes SSTC*/; - est0Length = tagLenByte-3; -// DEBUG("Stream found at index %u, size=%zu\n", est0Index, est0Length); - } - - tagIndex += tagLenByte; - } - if (frameCountFound && est0Index !=0) { - _subChannelQueue->push(frameCount, buf+est0Index, est0Length); - } else { - ok = false; - } - } - - return ok; -} - -/* ------------------------------------------------------------------ - * ------------------------------------------------------------------ - * ------------------------------------------------------------------ - * ------------------------------------------------------------------ - */ - -/* ------------------------------------------------------------------ - * - */ -//static int nbPFTFrag = 0; -PFTFrag::PFTFrag(uint8_t* buf, size_t length) -{ - //DEBUG("+ PFTFrag %d\n", ++nbPFTFrag); - _valid = _parse(buf, length); -} - -/* ------------------------------------------------------------------ - * - */ -PFTFrag::~PFTFrag() -{ - //DEBUG("- PFTFrag %d\n", --nbPFTFrag); -} - -/* ------------------------------------------------------------------ - * - */ -bool PFTFrag::_parse(uint8_t* buf, size_t length) -{ - int index = 0; - - // Parse PFT Fragment Header (ETSI TS 102 821 V1.4.1 ch7.1) - index += 2; // Psync - - _Pseq = unpack2(buf+index); index += 2; - _Findex = unpack3(buf+index); index += 3; - _Fcount = unpack3(buf+index); index += 3; - _FEC = unpack1bit(buf[index], 0); - _Addr = unpack1bit(buf[index], 1); - _Plen = unpack2(buf+index) & 0x3FFF; index += 2; - - // Optional RS Header - _RSk = 0; - _RSz = 0; - if (_FEC) { - _RSk = buf[index]; index += 1; - _RSz = buf[index]; index += 1; - } - - // Optional transport header - _Source = 0; - _Dest = 0; - if (_Addr) { - _Source = unpack2(buf+index); index += 2; - _Dest = unpack2(buf+index); index += 2; - } - - index += 2; - bool isValid = (_FEC==0) || _checkCRC(buf, index); - isValid &= length == index + _Plen; - - if (!isValid) { -// DEBUG("PFT isValid=%d Pseq=%u Findex=%u Fcount=%u FEC=%u " -// "Addr=%u Plen=%u", -// isValid, _Pseq, _Findex, _Fcount, _FEC, -// _Addr, _Plen); - if (_FEC) PRINTF(" RSk=%u RSz=%u", _RSk, _RSz); - if (_Addr) PRINTF(" Source=%u Dest=%u", _Source, _Dest); - PRINTF("\n"); - } - - if (isValid) { - _payload.resize(_Plen); - memcpy(_payload.data(), buf+index, _Plen); - } - - return isValid; -} - -/* ------------------------------------------------------------------ - * ------------------------------------------------------------------ - * ------------------------------------------------------------------ - * ------------------------------------------------------------------ - */ -void* PFT::_rs_handler = NULL; - -/* ------------------------------------------------------------------ - * - */ -//static int nbPFT = 0; -PFT::PFT(uint32_t Pseq, uint32_t Fcount) - : _frags(NULL) - , _Pseq(Pseq) - , _Fcount(Fcount) - , _Plen(0) - , _nbFrag(0) - , _RSk(0) - , _RSz(0) - , _cmax(0) - , _rxmin(0) - , _creation(std::chrono::steady_clock::now()) -{ -// DEBUG("+ PFT %d\n", ++nbPFT); - if (Fcount > 0) { - _frags = new PFTFrag* [Fcount]; - memset(_frags, 0, Fcount*sizeof(PFTFrag*)); - } -} - -/* ------------------------------------------------------------------ - * - */ -PFT::~PFT() -{ -// DEBUG("- PFT %d\n", --nbPFT); - if (_frags) { - for (size_t i = 0 ; i < _Fcount ; i++) { - delete _frags[i]; - } - delete [] _frags; - } -} - -/* ------------------------------------------------------------------ - * static - */ -void PFT::_initRSDecoder() -{ -#if RS_DECODE - if (!_rs_handler) { - // From ODR-DabMux: PFT.h/cpp and ReedSolomon.h/cpp - - // Create the RS(k+p,k) encoder - const int firstRoot = 1; // Discovered by analysing EDI dump - const int gfPoly = 0x11d; - - // The encoding has to be 255, 207 always, because the chunk has to - // be padded at the end, and not at the beginning as libfec would - // do - const int N = 255; - const int K = 207; - const int primElem = 1; - const int symsize = 8; - const int nroots = N - K; // For EDI PFT, this must be 48 - const int pad = ((1 << symsize) - 1) - N; // is 255-N - - _rs_handler = init_rs_char(symsize, gfPoly, firstRoot, primElem, nroots, pad); - - -/* TEST RS CODE */ -#if 0 - - // Populate data - uint8_t data[255]; - memset(data, 0x00, 255); - for (int i=0;i<207;i++) data[i] = i%10; - - // Add RS Code - encode_rs_char(_rs_handler, data, data+207); - _dump(data, 255); - - // Disturb data - for (int i=50; i<50+24; i++) data[i]+=0x50; - - // Correct data - int nbErr = decode_rs_char(_rs_handler, data, NULL, 0); - printf("nbErr=%d\n", nbErr); - _dump(data, 255); - - // Check data - for (int i=0;i<207;i++) { - if (data[i] != i%10) { - printf("Error position %d %hhu != %d\n", i, data[i], i%10); - } - } - - // STOP (sorry :-| ) - int* i=0; - *i = 9; -#endif // 0 - } -#endif -} - -/* ------------------------------------------------------------------ - * - */ -void PFT::pushPFTFrag(PFTFrag* frag) -{ - uint32_t Findex = frag->Findex(); -#if RS_TEST1 - if (Findex != 0 && _frags[Findex] == NULL) /* TEST */ -#else - if (_frags[Findex] == NULL) -#endif - { - _frags[Findex] = frag; - _nbFrag++; - - // Calculate the minimum number of fragment necessary to apply FEC - // This can't be done with the last fragment that does may have a smaller size - // ETSI TS 102 821 V1.4.1 ch 7.4.4 - if (_Plen == 0 && (Findex == 0 || Findex < (_Fcount-1))) - { - _Plen = frag->Plen(); - } - - if (_cmax == 0 && frag->FEC() && (Findex == 0 || Findex < (_Fcount-1)) && _Plen>0) - { - _RSk = frag->RSk(); - _RSz = frag->RSz(); - _cmax = (_Fcount*_Plen) / (_RSk+48); - _rxmin = _Fcount - (_cmax*48)/_Plen; - } - } else { - // Already received, delete the fragment - delete frag; - } -} - -/* ------------------------------------------------------------------ - * - */ -bool PFT::complete() -{ -#if RS_TEST1 - return _nbFrag == _Fcount-1; -#else - return _nbFrag == _Fcount; -#endif -} - -/* ------------------------------------------------------------------ - * - */ -bool PFT::_canAttemptToDecode() -{ - if (complete()) return true; - - if (_cmax>0 && _nbFrag >= _rxmin) return true; - - return false; -} - -/* ------------------------------------------------------------------ - * - */ -bool PFT::extractAF(std::vector<uint8_t>& afdata) -{ - bool ok = false; -// DEBUG("extractAF from PFT %u. Fcount=%u nbFrag=%u Plen=%u cmax=%u rxmin=%u RSk=%u RSz=%u\n", -// _Pseq, _Fcount, _nbFrag, _Plen, _cmax, _rxmin, _RSk, _RSz); - - if (_canAttemptToDecode()) { - int totCorrectedErr = 0; - - if (_cmax > 0) // FEC present. - { - uint8_t* p_data_w; - uint8_t* p_data_r; - size_t data_len = 0; - - // Re-assemble RS block - uint8_t rs_block[_Plen*_Fcount]; - int eras_pos[_cmax][/*48*/255]; /* 48 theoritically but ... */ - int no_eras[_cmax]; - memset(no_eras, 0, sizeof(no_eras)); - - p_data_w = rs_block; - for (size_t j = 0; j < _Fcount; ++j) { - if (!_frags[j]) // fill with zeros if fragment is missing - { - for (size_t k = 0; k < _Plen; k++) { - size_t pos = k * _Fcount; - p_data_w[pos] = 0x00; - size_t chunk = pos / (_RSk+48); - size_t chunkpos = (pos) % (_RSk+48); - if (chunkpos > _RSk) { - chunkpos += (207-_RSk); - } - eras_pos[chunk][no_eras[chunk]] = chunkpos; - no_eras[chunk]++; - } - } else { - uint8_t* p_data_r = _frags[j]->payload(); - for (size_t k = 0; k < _frags[j]->Plen(); k++) - p_data_w[k * _Fcount] = *p_data_r++; - for (size_t k = _frags[j]->Plen(); k < _Plen; k++) - p_data_w[k * _Fcount] = 0x00; - } - p_data_w++; - } - - // Apply RS Code -#if RS_DECODE - uint8_t rs_chunks[255 * _cmax]; - _initRSDecoder(); - if (_rs_handler) { - size_t k = _RSk; - memset(rs_chunks, 0, sizeof(rs_chunks)); - p_data_w = rs_chunks; - p_data_r = rs_block; - for (size_t j = 0; j < _cmax; j++) { - memcpy(p_data_w, p_data_r, k); - p_data_w += k; - p_data_r += k; - if (k < 207) - memset(p_data_w, 0, 207 - k); - p_data_w += 207 - k; - memcpy(p_data_w, p_data_r, 48); - p_data_w += 48; - p_data_r += 48; - } - - p_data_r = rs_chunks; - for (size_t j = 0 ; j < _cmax && totCorrectedErr != -1 ; j++) { -#if RS_TEST1 || RS_TEST2 - if (no_eras[j]>0) { - DEBUG("RS Chuck %d: %d errors\n", j, no_eras[j]); - } -#endif - int nbErr = decode_rs_char(_rs_handler, p_data_r, eras_pos[j], no_eras[j]); -// int nbErr = decode_rs_char(_rs_handler, p_data_r, NULL, 0); - if (nbErr >= 0) { -#if RS_TEST1 || RS_TEST2 - if (nbErr > 0) DEBUG("RS Chuck %d: %d corrections\n", j, nbErr); -#endif - totCorrectedErr += nbErr; - } else { -#if RS_TEST1 || RS_TEST2 - DEBUG("RS Chuck %d: too many errors\n", j); -#endif - totCorrectedErr = -1; - } - p_data_r += 255; - } -#if RS_TEST1 || RS_TEST2 - if (totCorrectedErr>0) { - DEBUG("RS corrected %d errors in %d chunks\n", totCorrectedErr, _cmax); - } -#endif - } -#endif // RS_DECODE - // Assemble AF frame from rs code - /* --- re-assemble packet from Reed-Solomon block ----------- */ - afdata.resize(_Plen*_Fcount); - p_data_w = afdata.data(); -#if RS_DECODE - p_data_r = rs_chunks; - for (size_t j = 0; j < _cmax; j++) { - memcpy(p_data_w, p_data_r, _RSk); - p_data_w += _RSk; - p_data_r += 255; - data_len += _RSk; - } -#else - p_data_r = rs_block; - for (size_t j = 0; j < _cmax; j++) { - memcpy(p_data_w, p_data_r, _RSk); - p_data_w += _RSk; - p_data_r += _RSk + 48; - data_len += _RSk; - } -#endif // RS_DECODE - data_len -= _RSz; - afdata.resize(data_len); - } else { // No Fec Just assemble packets - afdata.resize(0); - for (size_t j = 0; j < _Fcount; ++j) { - if (_frags[j]) - { - afdata.insert(afdata.end(), - _frags[j]->payloadVector().begin(), _frags[j]->payloadVector().end()); - } - } - } - - // EDI specific, must have a CRC. - if( afdata.size()>=12 ) { - ok = _checkCRC(afdata.data(), afdata.size()); - if (ok && totCorrectedErr > 0) { - if (hideFirstPFTErrors==0) { - INFO("AF reconstructed from %u/%u PFT fragments\n", _nbFrag, _Fcount); - } - } - if (!ok && totCorrectedErr == -1) { - if (hideFirstPFTErrors==0) { - ERROR("Too many errors to reconstruct AF from %u/%u PFT fragments\n", _nbFrag, _Fcount); - } - } - } - } - else { - if (hideFirstPFTErrors==0) { - ERROR("Not enough fragments to reconstruct AF from %u/%u PFT fragments (min=%u)\n", _nbFrag, _Fcount, _rxmin); - } - } - - if( hideFirstPFTErrors > 0 ) hideFirstPFTErrors--; - - return ok; -} |