Add initial copy-pasted code

28 files changed, 5155 insertions, 0 deletions

diff --git a/src/AVTEDIInput.cpp b/src/AVTEDIInput.cpp
new file mode 100644
index 0000000..591fe43
--- /dev/null
+++ b/src/AVTEDIInput.cpp
@@ -0,0 +1,740 @@
+/* ------------------------------------------------------------------
+ * 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;
+        bool frameCountFound = false;
+        int est0Index = 0;
+        size_t est0Length = 0;
+        // Iterate through tags
+        while (tagIndex < 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 (int 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.
+        {
+            int j, k;
+            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 (j = 0; j < _Fcount; ++j) {
+                if (!_frags[j]) // fill with zeros if fragment is missing
+                {
+                    for (int k = 0; k < _Plen; k++) {
+                        int pos = k * _Fcount;
+                        p_data_w[pos] = 0x00;
+                        int chunk = pos / (_RSk+48);
+                        int 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 (k = 0; k < _frags[j]->Plen(); k++)
+                        p_data_w[k * _Fcount] = *p_data_r++;
+                    for (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) {
+                k = _RSk;
+                memset(rs_chunks, 0, sizeof(rs_chunks));
+                p_data_w = rs_chunks;
+                p_data_r = rs_block;
+                for (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 (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 (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 (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 (int 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;
+}
diff --git a/src/AVTEDIInput.h b/src/AVTEDIInput.h
new file mode 100644
index 0000000..4ec6086
--- /dev/null
+++ b/src/AVTEDIInput.h
@@ -0,0 +1,183 @@
+/* ------------------------------------------------------------------
+ * 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.
+ * -------------------------------------------------------------------
+ */
+ 
+ 
+/*! \section AVT Input
+ *
+ * Extract audio frame from EDI frames produced by AVT encoder.
+ *
+ * The EDI frames are not special, it is just assumed that the audio is transported
+ * into the first stream.
+ *
+ * PFT with spreaded packets is supported.	TODO
+ * Error correction is applied			TODO
+ * AF without PFT supported			TODO
+ * Resend not supported
+ * 
+ * ref: ETSI TS 102 821 V1.4.1
+ *      ETSI TS 102 693 V1.1.2
+ */
+
+#ifndef _AVT_EDI_INPUT_
+#define _AVT_EDI_INPUT_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string>
+#include <map>
+#include <vector>
+#include <chrono>
+
+class OrderedQueue;
+class PFTFrag;
+class PFT;
+class EDISubCh;
+
+/* ------------------------------------------------------------------
+ *
+ */
+class AVTEDIInput
+{
+    public:
+        /*\param fragmentTimeoutMs How long to wait for all fragment before applying FEC or dropping old frames*/
+        AVTEDIInput(uint32_t fragmentTimeoutMs = 120);
+        ~AVTEDIInput();
+
+        /*! Push new data to edi decoder
+         * \return false is data is not EDI
+         */
+        bool pushData(uint8_t* buf, size_t length);
+        
+        /*! Give next available audio frame from EDI
+         * \return The size of the buffer. 0 if not data available
+         */
+        size_t popFrame(std::vector<uint8_t>& data, int32_t& frameNumber);
+
+    private:
+        uint32_t _fragmentTimeoutMs;
+        std::map<int, PFT*>  _pft;
+        typedef std::map<int, PFT*>::iterator PFTIterator;
+        
+        OrderedQueue*   _subChannelQueue;
+
+        bool _pushPFTFrag(uint8_t* buf, size_t length);
+        bool _pushAF(uint8_t* buf, size_t length, bool checked);
+};
+
+/* ------------------------------------------------------------------
+ *
+ */
+class PFTFrag
+{
+    public:
+        PFTFrag(uint8_t* buf, size_t length);
+        ~PFTFrag();
+        
+        inline bool isValid() { return _valid; }
+        inline uint32_t Pseq() { return _Pseq; }
+        inline uint32_t Findex() { return _Findex; }
+        inline uint32_t Fcount() { return _Fcount; }
+        inline uint32_t FEC() { return _FEC; }
+        inline uint32_t Plen() { return _Plen; }
+        inline uint32_t RSk() { return _RSk; }
+        inline uint32_t RSz() { return _RSz; }
+        inline uint8_t* payload() { return _payload.data(); }
+        inline const std::vector<uint8_t>& payloadVector()
+            { return _payload; }
+
+    private:
+        std::vector<uint8_t> _payload;
+
+        uint32_t _Pseq;
+        uint32_t _Findex;
+        uint32_t _Fcount;
+        uint32_t _FEC;
+        uint32_t _Addr;
+        uint32_t _Plen;
+        uint32_t _RSk;
+        uint32_t _RSz;
+        uint32_t _Source;
+        uint32_t _Dest;
+        bool _valid;
+        
+        bool _parse(uint8_t* buf, size_t length);       
+};
+
+/* ------------------------------------------------------------------
+ *
+ */
+class PFT
+{
+    public:
+        PFT(uint32_t Pseq, uint32_t Fcount);
+        ~PFT();
+
+        /*! the given frag belongs to the PFT class,
+         *! it will be deleted by the class */
+        void pushPFTFrag(PFTFrag* frag);
+
+        /* \return true if all framgnements are received*/
+        bool complete();
+        
+        /*! try to build the AF with received fragments.
+         *! Apply error correction if necessary (missing packets/CRC errors)
+         * \return true if the AF is completed
+         */
+        bool extractAF(std::vector<uint8_t>& afdata);  
+        
+        inline std::chrono::steady_clock::time_point creation()
+            { return _creation; }
+
+    private:
+        PFTFrag** _frags;
+        uint32_t _Pseq;
+        uint32_t _Fcount;
+        uint32_t _Plen;
+        uint32_t _nbFrag;
+        uint32_t _RSk;
+        uint32_t _RSz;
+        uint32_t _cmax;
+        uint32_t _rxmin;
+
+        std::chrono::steady_clock::time_point _creation;  
+        
+        bool _canAttemptToDecode();
+        
+        static void* _rs_handler;
+        static void _initRSDecoder();
+};
+
+/* ------------------------------------------------------------------
+ *
+ */
+class EDISubCh {
+    public:
+        EDISubCh(uint8_t* buf, size_t length);
+        ~EDISubCh();
+
+        inline uint32_t frameCount() { return _frameCount; }
+        inline uint8_t* payload() { return _payload.data(); }
+        inline const std::vector<uint8_t>& payloadVector()
+            { return _payload; }
+
+    private:
+        uint32_t _frameCount;
+        std::vector<uint8_t> _payload;
+};
+
+#endif // _AVT_EDI_INPUT_
diff --git a/src/AVTInput.cpp b/src/AVTInput.cpp
new file mode 100644
index 0000000..e8dbe16
--- /dev/null
+++ b/src/AVTInput.cpp
@@ -0,0 +1,776 @@
+/* ------------------------------------------------------------------
+ * 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 "AVTInput.h"
+#include <cstring>
+#include <cstdio>
+#include <stdint.h>
+#include <limits.h>
+#include <algorithm>
+
+#include "UdpSocket.h"
+#include "OrderedQueue.h"
+#include "AVTEDIInput.h"
+
+//#define PRINTF(fmt, A...)   fprintf(stderr, fmt, ##A)
+#define PRINTF(x ...)
+#define INFO(fmt, A...)   fprintf(stderr, "AVT: " fmt, ##A)
+//#define DEBUG(fmt, A...)   fprintf(stderr, "AVT: " fmt, ##A)
+#define DEBUG(X...)
+#define ERROR(fmt, A...)   fprintf(stderr, "AVT: ERROR " fmt, ##A)
+
+#define DEF_BR  64
+#define MAX_AVT_FRAME_SIZE  (1500)  /* Max AVT MTU = 1472 */
+
+#define MAX_PAD_FRAME_QUEUE_SIZE  (6)
+
+//#define DISTURB_INPUT 
+
+// ETSI EN 300 797 V1.2.1 ch 8.2.1.2
+uint8_t STI_FSync0[3] = { 0x1F, 0x90, 0xCA };
+uint8_t STI_FSync1[3] = { 0xE0, 0x6F, 0x35 };
+
+// The enum values folown the AVT messages definitions.
+enum {
+    AVT_Mono            = 0,
+    AVT_Mono_SBR,
+    AVT_Stereo,
+    AVT_Stereo_SBR,
+    AVT_Stereo_SBR_PS
+};
+
+enum {
+    AVT_MonoMode_LR2    = 0,
+    AVT_MonoMode_L,
+    AVT_MonoMode_R
+};
+
+enum {
+  AVT_DAC_32            = 0,
+  AVT_DAC_48  
+};
+
+/* ------------------------------------------------------------------
+ *
+ */
+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]);
+}
+
+/* ------------------------------------------------------------------
+ * 
+ */
+AVTInput::AVTInput(const std::string& input_uri, const std::string& output_uri, uint32_t pad_port, size_t jitterBufferSize)
+    :   _input_uri(input_uri),
+        _output_uri(output_uri),
+        _pad_port(pad_port),
+        _jitterBufferSize(jitterBufferSize),
+        _input_socket(NULL),
+        _input_packet(NULL),
+        _output_socket(NULL),
+        _output_packet(NULL),
+        _input_pad_socket(NULL),
+        _input_pad_packet(NULL),
+        _ediInput(NULL),
+        _ordered(NULL),
+        _subChannelIndex(DEF_BR/8),
+        _bitRate(DEF_BR*1000),
+        _audioMode(AVT_Mono),
+        _monoMode(AVT_MonoMode_LR2),
+        _dac(AVT_DAC_48),
+        _dab24msFrameSize(DEF_BR*3),
+        _dummyFrameNumber(0),
+        _frameAlligned(false),
+        _currentFrame(NULL),
+        _currentFrameSize(0),
+        _nbFrames(0),
+        _nextFrameIndex(0),
+        _lastInfoFrameType(_typeCantExtract),
+        _lastInfoSize(0),
+        _infoNbFrame(0)
+{
+
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+AVTInput::~AVTInput()
+{
+    delete _input_packet;
+    delete _input_socket;
+    delete _output_packet;
+    delete _output_socket;
+    delete _input_pad_packet;
+    delete _input_pad_socket;
+    delete _ediInput;
+    delete [] _currentFrame;
+    delete _ordered;
+    while (_padFrameQueue.size() > 0) {
+        std::vector<uint8_t>* frame = _padFrameQueue.front();
+        _padFrameQueue.pop();
+        delete frame;
+    }    
+}
+
+/* ------------------------------------------------------------------
+ * 
+ */
+int AVTInput::prepare(void)
+{   
+    _input_socket = new UdpSocket();
+    _input_packet = new UdpPacket(2048);
+
+    if( !_output_uri.empty() )
+    {
+        _output_socket = new UdpSocket();
+        _output_packet = new UdpPacket(2048);
+    }
+
+    UdpSocket::init();
+
+    INFO("Open input socket\n");
+    int ret = _openSocketSrv(_input_socket, _input_uri.c_str());
+
+    if (ret == 0 && !_output_uri.empty()) {
+        INFO("Open output socket\n");
+        ret = _openSocketCli(_output_socket, _output_packet, _output_uri.c_str());
+    }
+
+    if ( ret == 0 && _pad_port > 0) {
+        INFO("Open PAD Port %d\n", _pad_port);
+        char uri[50];
+        sprintf(uri, "udp://:%d", _pad_port);
+        _input_pad_socket = new UdpSocket();
+        _input_pad_packet = new UdpPacket(2048);        
+        ret = _openSocketSrv(_input_pad_socket, uri);
+        _purgeMessages();
+    }
+    
+    _ediInput = new AVTEDIInput(_jitterBufferSize*24/3);
+
+    return ret;
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+int AVTInput::setDabPlusParameters(int bitrate, int channels, int sample_rate, bool sbr, bool ps)
+{
+    int ret = 0;
+    
+    _subChannelIndex = bitrate / 8;
+    _bitRate = bitrate * 1000;
+    _dab24msFrameSize = bitrate * 3;
+    if (_subChannelIndex * 8 != bitrate || _subChannelIndex < 1 | _subChannelIndex > 24) {
+        ERROR("Bad bitrate for DAB+ (8..192)");
+        return 1;
+    }
+    
+    if ( sample_rate != 48000 && sample_rate != 32000 ) {
+        ERROR("Bad sample rate for DAB+ (32000,48000)");
+        return 1;
+    }
+    _dac = sample_rate == 48000 ? AVT_DAC_48 : AVT_DAC_32;
+    
+    if ( channels != 1 && channels != 2 ) {
+        ERROR("Bad channel number for DAB+ (1,2)");
+        return 1;
+    }   
+    _audioMode = 
+        channels == 1
+            ? (sbr ? AVT_Mono_SBR : AVT_Mono)
+            : ( ps ? AVT_Stereo_SBR_PS : sbr ? AVT_Stereo_SBR : AVT_Stereo );    
+
+    delete _ordered;
+    _ordered = new OrderedQueue(5000, _jitterBufferSize);
+
+    delete [] _currentFrame;
+    _currentFrame = new uint8_t[_subChannelIndex*8*5*3];
+    _currentFrameSize = 0;
+    _nbFrames = 0;
+
+    _sendCtrlMessage(_output_socket, _output_packet);
+
+    return ret;
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+bool AVTInput::_parseURI(const char* uri, std::string& address, long& port)
+{    
+    // Skip the udp:// part if it is present
+    if (strncmp(uri, "udp://", 6) == 0) {
+        address = uri + 6;
+    }
+    else {
+        address = uri;
+    }
+    
+    size_t pos = address.find(':');
+    if (pos == std::string::npos) {
+        fprintf(stderr,
+                "\"%s\" is an invalid format for udp address: "
+                "should be [udp://][address]:port - > aborting\n", uri);
+        return false;        
+    }
+
+    port = strtol(address.c_str()+pos+1, (char **)NULL, 10);
+    if ((port == LONG_MIN) || (port == LONG_MAX)) {
+        fprintf(stderr,
+                "can't convert port number in udp address %s\n",
+                uri);
+        return false;
+    }
+    
+    if ((port <= 0) || (port >= 65536)) {
+        fprintf(stderr, "can't use port number %ld in udp address\n", port);
+        return false;
+    }
+    address.resize(pos);
+
+    DEBUG("_parseURI <%s> -> <%s> : %ld\n", uri, address.c_str(), port);    
+
+    return true;
+}
+
+/* ------------------------------------------------------------------
+ * From dabInputUdp::dabInputUdpOpen
+ */
+int AVTInput::_openSocketSrv(UdpSocket* socket, const char* uri)
+{
+    int returnCode = -1;
+    
+    std::string address;
+    long port;
+    
+    if (_parseURI(uri, address, port)) {
+        returnCode = 0;
+        if (socket->create(port) == -1) {
+            fprintf(stderr, "can't set port %li on Udp input (%s: %s)\n",
+                    port, inetErrDesc, inetErrMsg);
+            returnCode = -1;
+        }
+
+        if (!address.empty()) {
+            // joinGroup should accept const char*
+            if (socket->joinGroup((char*)address.c_str()) == -1) {
+                fprintf(stderr,
+                        "can't join multicast group %s (%s: %s)\n",
+                        address.c_str(), inetErrDesc, inetErrMsg);
+                returnCode = -1;
+            }
+        }
+
+        if (socket->setBlocking(false) == -1) {
+            fprintf(stderr, "can't set Udp input socket in non-blocking mode "
+                    "(%s: %s)\n", inetErrDesc, inetErrMsg);
+            returnCode = -1;
+        }
+    }
+
+    return returnCode;
+}
+
+/* ------------------------------------------------------------------
+ * From ODR-dabMux DabOutputUdp::Open
+ */
+int AVTInput::_openSocketCli(UdpSocket* socket, UdpPacket* packet, const char* uri)
+{
+    std::string address;
+    long port;
+
+    if (!_parseURI(uri, address, port)) {
+        return -1;
+    }
+
+    if (packet->getAddress().setAddress(address.c_str()) == -1) {
+        fprintf(stderr, "Can't set address %s (%s: %s)\n", address.c_str(),
+                inetErrDesc, inetErrMsg);
+        return -1;
+    }
+
+    packet->getAddress().setPort(port);
+
+    if (socket->create() == -1) {
+        fprintf(stderr, "Can't create UDP socket (%s: %s)\n", 
+                inetErrDesc, inetErrMsg);
+        return -1;
+    }
+
+    return 0;
+}
+
+/* ------------------------------------------------------------------
+ * From ODR-Dabmux dabInputUdp::dabInputUdpRead
+ */
+ssize_t AVTInput::_read(uint8_t* buf, size_t size, bool onlyOnePacket)
+{
+    ssize_t nbBytes = 0;
+
+    uint8_t* data = buf;
+
+    if (_input_packet->getLength() == 0) {
+        _input_socket->receive(*_input_packet);
+    }
+
+    while (nbBytes < size) {
+        unsigned freeSize = size - nbBytes;
+        if (_input_packet->getLength() > freeSize) {
+            // Not enought place in output
+            memcpy(&data[nbBytes], _input_packet->getData(), freeSize);
+            nbBytes = size;
+            _input_packet->setOffset(_input_packet->getOffset() + freeSize);
+        } else {
+            unsigned length = _input_packet->getLength();
+            memcpy(&data[nbBytes], _input_packet->getData(), length);
+            nbBytes += length;
+            _input_packet->setOffset(0);
+            
+            _input_socket->receive(*_input_packet);
+            if (_input_packet->getLength() == 0 || onlyOnePacket) {
+                break;
+            }
+        }
+    }
+    bzero(&data[nbBytes], size - nbBytes);
+    
+    return nbBytes;
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+bool AVTInput::_ediPushData(uint8_t* buf, size_t length)
+{
+    return _ediInput->pushData(buf, length);
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+size_t AVTInput::_ediPopFrame(std::vector<uint8_t>& data, int32_t& frameNumber)
+{
+    return _ediInput->popFrame(data, frameNumber);
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+bool AVTInput::_isSTI(const uint8_t* buf)
+{
+    return  (memcmp(buf+1, STI_FSync0, sizeof(STI_FSync0)) == 0) ||
+            (memcmp(buf+1, STI_FSync1, sizeof(STI_FSync1)) == 0);
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+const uint8_t* AVTInput::_findDABFrameFromUDP(const uint8_t* buf, size_t size,
+                                    int32_t& frameNumber, size_t& dataSize)
+{
+    const uint8_t* data = NULL;
+    uint32_t index = 0;
+    
+    bool error = !_isSTI(buf+index);
+    bool rtp = false;
+
+    // RTP Header is optionnal, STI is mandatory
+    if (error)
+    {
+        // Assuming RTP header
+        if (size-index >= 12) {
+            uint32_t version = (buf[index] & 0xC0) >> 6;
+            uint32_t payloadType = (buf[index+1] & 0x7F);
+            if (version == 2 && payloadType == 34) {
+                index += 12; // RTP Header length
+                error = !_isSTI(buf+index);
+                rtp = true;
+            }
+        }
+    }
+    if (!error) {
+        index += 4;
+        //uint32_t DFS = unpack2(buf+index);
+        index += 2;
+        //uint32_t CFS = unpack2(buf+index);
+        index += 2;
+        
+        // FC
+        index += 5;
+        uint32_t DFCTL = buf[index];
+        index += 1;
+        uint32_t DFCTH = buf[index] >> 3;        
+        uint32_t NST   = unpack2(buf+index) & 0x7FF; // 11 bits
+        index += 2;
+
+        if (NST >= 1) {
+            // Take the first stream even if NST > 1
+            uint32_t STL = unpack2(buf+index) & 0x1FFF; // 13 bits
+            uint32_t CRCSTF = buf[index+3] & 0x80 >> 7; // 7th bit
+            index += NST*4+4;
+
+            data = buf+index;
+            dataSize = STL - 2*CRCSTF;
+            frameNumber = DFCTH*250 + DFCTL;     
+            
+            _info(rtp?_typeSTIRTP:_typeSTI, dataSize);
+        } else error = true;
+    }
+
+    if( error ) ERROR("Nothing detected\n");
+        
+    return data;
+}
+
+
+/* ------------------------------------------------------------------
+ * Set AAC Encoder Parameter format:
+ * Flag             : 1 Byte  : 0xFD
+ * Command code     : 1 Byte  : 0x07
+ * SubChannelIndex  : 1 Byte  : DataRate / 8000
+ * AAC Encoder Mode : 1 Byte  :
+ *                       * 0 = Mono
+ *                       * 1 = Mono + SBR
+ *                       * 2 = Stereo
+ *                       * 3 = Stereo + SBR
+ *                       * 4 = Stereo + SBR + PS
+ * DAC Flag         : 1 Byte  : 0 = 32kHz, 1 = 48kHz
+ * Mono mode        : 1 Byte  :
+ *                       * 0 = ( Left + Right ) / 2
+ *                       * 1 = Left
+ *                       * 2 = Right
+ */ 
+void AVTInput::_sendCtrlMessage(UdpSocket* socket, UdpPacket* packet)
+{
+    if (!_output_uri.empty()) {
+        uint8_t data[50];
+        uint32_t index = 0;
+        
+        data[index++] = 0xFD;
+        data[index++] = 0x07;
+        data[index++] = _subChannelIndex;
+        data[index++] = _audioMode;
+        data[index++] = _dac;
+        data[index++] = _monoMode;
+        
+        packet->setOffset(0);
+        packet->setLength(0);
+        packet->addData(data, index);
+        socket->send(*packet);
+        
+        INFO("Send control packet to encoder\n");
+    }
+}
+
+/* ------------------------------------------------------------------
+ * PAD Provision Message format:
+ * Flag         : 1 Byte  : 0xFD
+ * Command code : 1 Byte  : 0x18
+ * Size         : 1 Byte  : Size of data (including AD header)
+ * AD Header    : 1 Byte  : 0xAD
+ *              : 1 Byte  : Size of pad data
+ * Pad datas    : X Bytes : In natural order, strating with FPAD bytes
+ */
+void AVTInput::_sendPADFrame(UdpPacket* packet)
+{
+    if (packet && _padFrameQueue.size() > 0) {
+        std::vector<uint8_t>* frame = _padFrameQueue.front();
+        frame = _padFrameQueue.front();
+        _padFrameQueue.pop();
+ 
+        uint8_t data[500];
+        uint32_t index = 0;
+        
+        data[index++] = 0xFD;
+        data[index++] = 0x18;
+        data[index++] = frame->size()+2;
+        data[index++] = 0xAD;
+        data[index++] = frame->size();
+        memcpy( data+index, frame->data(), frame->size());
+        index += frame->size();
+
+        packet->setOffset(0);
+        packet->setLength(0);
+        packet->addData(data, index);
+
+        _input_pad_socket->send(*packet);
+        
+        delete frame;
+    }
+}
+
+/* ------------------------------------------------------------------
+ * Message format:
+ * Flag         : 1 Byte : 0xFD
+ * Command code : 1 Byte
+ *                  * 0x17 = Request for 1 PAD Frame
+ */
+void AVTInput::_interpretMessage(const uint8_t* data, size_t size, UdpPacket* packet)
+{
+    if (size >= 2) {
+        if (data[0] == 0xFD) {
+            switch (data[1]) {
+                case 0x17:
+                    _sendPADFrame(packet);
+                    break;
+            }
+        }
+    }
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+bool AVTInput::_checkMessage()
+{
+    bool dataRecevied = false;
+
+    if (_input_pad_socket) {
+        if (_input_pad_packet->getLength() == 0) {
+            _input_pad_socket->receive(*_input_pad_packet);
+        }
+
+        if (_input_pad_packet->getLength() > 0) {
+            _interpretMessage((uint8_t*)_input_pad_packet->getData(), _input_pad_packet->getLength(), _input_pad_packet);
+            _input_pad_packet->setOffset(0);
+            _input_pad_socket->receive(*_input_pad_packet);
+
+            dataRecevied = true;
+        }
+    }
+
+    return dataRecevied;
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+void AVTInput::_purgeMessages()
+{
+    if (_input_pad_socket) {
+        bool dataRecevied;
+        int nb = 0;
+        do {
+            dataRecevied = false;
+            if (_input_pad_packet->getLength() == 0) {
+                _input_pad_socket->receive(*_input_pad_packet);
+            }
+
+            if (_input_pad_packet->getLength() > 0) {
+                nb++;
+                _input_pad_packet->setOffset(0);
+                _input_pad_socket->receive(*_input_pad_packet);
+
+                dataRecevied = true;
+            }
+        } while (dataRecevied);
+        if (nb>0) DEBUG("%d messages purged\n", nb);
+    }
+}
+
+
+/* ------------------------------------------------------------------
+ *
+ */
+bool AVTInput::_readFrame()
+{
+    bool dataRecevied = false;
+
+    uint8_t readBuf[MAX_AVT_FRAME_SIZE];
+    int32_t frameNumber;
+    const uint8_t* dataPtr = NULL;
+    size_t dataSize = 0;  
+    std::vector<uint8_t> data;
+
+    size_t readBytes = _read(readBuf, sizeof(readBuf), true/*onlyOnePacket*/);
+    if (readBytes > 0)
+    {
+        dataRecevied = true;
+        
+        if (_ediPushData(readBuf, readBytes)) {
+            dataSize = _ediPopFrame(data, frameNumber);
+            if (dataSize>0) {
+                dataPtr = data.data();
+                _info(_typeEDI, dataSize);
+            }
+        } else {
+            if (readBytes > _dab24msFrameSize) {            
+                // Extract frame data and frame number from buf
+                dataPtr = _findDABFrameFromUDP(readBuf, readBytes, frameNumber, dataSize);
+            }
+//            if (!data) {
+//                    // Assuming pure RAW data
+//                    data = buf;
+//                    dataSize = _dab24msFrameSize;
+//                    frameNumber = _dummyFrameNumber++;
+//            } 
+            if (!dataPtr) {
+                _info(_typeCantExtract, 0);
+            }
+        }
+        if (dataPtr) {
+            if (dataSize == _dab24msFrameSize ) {       
+                if( _frameAlligned || frameNumber%5 == 0)
+                {
+#if defined(DISTURB_INPUT)
+                    // Duplicate a frame
+                    if(frameNumber%250==0) _ordered->push(frameNumber, dataPtr, dataSize);
+
+                    // Invert 2 frames (content inverted, audio distrubed by this test))
+                    if( frameNumber % 200 == 0) frameNumber += 10;
+                    else if( (frameNumber-10) % 200 == 0) frameNumber -= 10;
+
+                    // Remove a frame (audio distrubed, frame missing)
+                    if(frameNumber%300 > 5)
+#endif
+                    _ordered->push(frameNumber, dataPtr, dataSize);
+                    _frameAlligned = true;
+                }
+            }
+            else ERROR("Wrong frame size from encoder %zu != %zu\n", dataSize, _dab24msFrameSize);
+        }
+    }
+
+    return dataRecevied;
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+ssize_t AVTInput::getNextFrame(std::vector<uint8_t> &buf)
+{
+    ssize_t nbBytes = 0;
+
+    //printf("A: _padFrameQueue size=%zu\n", _padFrameQueue.size());
+    
+    // Read all messages from encoder (in priority)
+    // Read all available frames from input socket
+    while (_checkMessage() || _readFrame() );
+
+    //printf("B: _padFrameQueue size=%zu\n", _padFrameQueue.size());
+    
+    // Assemble next frame
+    int32_t nb = 0;
+    std::vector<uint8_t> part;    
+    while (_nbFrames < 5 && (nb = _ordered->pop(part)) != 0)
+    {
+        while (_checkMessage());
+
+        memcpy(_currentFrame+_currentFrameSize, part.data(), nb);
+        _currentFrameSize += nb;
+        _nbFrames ++;
+    }
+
+    if (_nbFrames == 5 && _currentFrameSize <= buf.size()) {     
+        memcpy(&buf[0], _currentFrame, _currentFrameSize);
+        nbBytes = _currentFrameSize;
+        _currentFrameSize = 0;
+        _nbFrames = 0;
+    }
+
+    //printf("C: _padFrameQueue size=%zu\n", _padFrameQueue.size());
+
+    return nbBytes;
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+void AVTInput::pushPADFrame(const uint8_t* buf, size_t size)
+{
+    if (_pad_port == 0) {
+        return;
+    }
+    
+    std::vector<uint8_t>* frame;
+    
+//    while (_padFrameQueue.size() > MAX_PAD_FRAME_QUEUE_SIZE) {
+//        frame = _padFrameQueue.front();
+//        _padFrameQueue.pop();
+//        delete frame;
+//        ERROR("Drop one PAD Frame\n");
+//    }
+
+    if (size > 0) {
+        frame = new std::vector<uint8_t>(size);        
+        memcpy(frame->data(), buf, size);
+        std::reverse(frame->begin(), frame->end());
+        _padFrameQueue.push(frame);
+    }
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+bool AVTInput::padQueueFull()
+{
+    return _padFrameQueue.size() >= MAX_PAD_FRAME_QUEUE_SIZE;
+}
+
+/* ------------------------------------------------------------------
+ *
+ */
+void AVTInput::_info(_frameType type, size_t size)
+{
+    if (_lastInfoFrameType != type || _lastInfoSize != size) {
+        switch (type) {
+            case _typeEDI:
+                INFO("Extracting from EDI frames of size %zu\n", size);
+                break;
+            case _typeSTI:
+                INFO("Extracting from UDP/STI frames of size %zu\n", size);
+                break;                
+            case _typeSTIRTP:
+                INFO("Extracting from UDP/RTP/STI frames of size %zu\n", size);
+                break;                
+            case _typeCantExtract:
+                ERROR("Can't extract data from encoder frame\n");            
+                break;
+        }
+        _lastInfoFrameType = type;
+        _lastInfoSize = size;
+    }
+    if (_lastInfoFrameType != _typeCantExtract) {
+        _infoNbFrame++;
+        if ( (_infoNbFrame == 100) ||
+             (_infoNbFrame < 10000 && _infoNbFrame % 1000 == 0) ||
+             (_infoNbFrame < 100000 && _infoNbFrame % 10000 == 0) ||
+             (_infoNbFrame % 100000 == 0)
+           )
+        {
+            INFO("%zu 24ms-frames received\n", _infoNbFrame);
+        }
+    }
+}
diff --git a/src/AVTInput.h b/src/AVTInput.h
new file mode 100644
index 0000000..8638fc3
--- /dev/null
+++ b/src/AVTInput.h
@@ -0,0 +1,161 @@
+/* ------------------------------------------------------------------
+ * 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.
+ * -------------------------------------------------------------------
+ */
+ 
+ 
+/*! \section AVT Input
+ *
+ * This input comunicate with AVT encoder
+ *
+ * The encoded frames and reassambled
+ * The PAD Frames are sent to the encoder for insertion.
+ * The encoder is remotely controled to set bitrate and audio mode.
+ *
+ */
+
+#ifndef _AVT_INPUT_H_
+#define _AVT_INPUT_H_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string>
+#include <queue>
+#include <vector>
+
+class UdpSocket;
+class UdpPacket;
+class AVTEDIInput;
+class OrderedQueue;
+
+class AVTInput
+{
+    public:
+        AVTInput(const std::string& input_uri, const std::string& output_uri, uint32_t pad_port,
+                size_t jitterBufferSize = 40);
+        ~AVTInput();
+
+        /*! Open the file and prepare the wav decoder.
+         *
+         * \return nonzero on error
+         */
+        int prepare(void);
+                
+        /*! Inform class and remove encoder about the bitrate and audio mode
+         * 
+         * \return nonzero on error
+         */
+        int setDabPlusParameters(int bitrate, int channels, int sample_rate, bool sbr, bool ps);
+
+        /*! Read incomming frames from the encoder, reorder and reassemble then into DAB+ superframes
+         *! Give the next reassembled audio frame (120ms for DAB+) 
+         * 
+         * \return the size of the frame or 0 if none are available yet
+         */
+        ssize_t getNextFrame(std::vector<uint8_t> &buf);
+        
+        /*! Store a new PAD frame.
+         *! Frames are sent to the encoder on request
+         */
+        void pushPADFrame(const uint8_t* buf, size_t size);
+        
+        /* \return true if PAD Frame queue is full */
+        bool padQueueFull();
+        
+
+    private:
+        std::string _input_uri;
+        std::string _output_uri;
+        uint32_t _pad_port;
+        size_t _jitterBufferSize;
+        
+        UdpSocket*      _input_socket;
+        UdpPacket*      _input_packet;
+        UdpSocket*      _output_socket;
+        UdpPacket*      _output_packet;
+        UdpSocket*      _input_pad_socket;
+        UdpPacket*      _input_pad_packet;
+        AVTEDIInput*    _ediInput;
+        OrderedQueue*   _ordered;
+        std::queue< std::vector<uint8_t>* > _padFrameQueue;
+
+        int32_t _subChannelIndex;
+        int32_t _bitRate;
+        int32_t _audioMode;
+        int32_t _monoMode;
+        int32_t _dac;
+        size_t _dab24msFrameSize;
+        uint32_t _dummyFrameNumber;
+        bool _frameAlligned;
+        uint8_t* _currentFrame;
+        size_t _currentFrameSize;
+        int32_t _nbFrames;
+        uint8_t* _nextFrameIndex;
+
+        bool _parseURI(const char* uri, std::string& address, long& port);
+        int _openSocketSrv(UdpSocket* socket, const char* uri);
+        int _openSocketCli(UdpSocket* socket, UdpPacket* packet, const char* uri);
+        
+        void _sendCtrlMessage(UdpSocket* socket, UdpPacket* packet);
+        void _sendPADFrame(UdpPacket* packet = NULL);
+        void _interpretMessage(const uint8_t* data, size_t size, UdpPacket* packet = NULL);
+        bool _checkMessage();
+        void _purgeMessages();
+
+        /*! Read length bytes into buf.
+         *
+         * \return the number of bytes read.
+         */
+        ssize_t _read(uint8_t* buf, size_t length, bool onlyOnePacket=false);
+        
+        /*! Push new data to edi decoder
+         * \return false is data is not EDI
+         */
+        bool _ediPushData(uint8_t* buf, size_t length);
+        
+        size_t _ediPopFrame(std::vector<uint8_t>& data, int32_t& frameNumber);
+        
+        /*! Test Bytes 1,2,3 for STI detection */
+        bool _isSTI(const uint8_t* buf);
+        
+        /*! Find and extract the DAB frame from UDP/RTP/STI received frame
+         * \param   frameNumber will contain the frameNumber
+         * \param   dataSize will contain the actual DAB frame size
+         * \return  Pointer to first byte of the DAB frame, or NULL if not found
+         */
+        const uint8_t* _findDABFrameFromUDP(const uint8_t* buf, size_t size,
+                                    int32_t& frameNumber, size_t& dataSize);
+
+        /*! Read and store one frame from encoder
+         *
+         * \return true if a data has been received
+         */
+        bool _readFrame();
+
+        /*! Output info about received frames*/
+        enum _frameType {
+            _typeEDI,
+            _typeSTI,
+            _typeSTIRTP,
+            _typeCantExtract
+        };
+        _frameType _lastInfoFrameType;
+        size_t _lastInfoSize;
+        size_t _infoNbFrame;
+        void _info(_frameType type, size_t size);
+};
+
+#endif // _AVT_INPUT_H_
diff --git a/src/InetAddress.cpp b/src/InetAddress.cpp
new file mode 100644
index 0000000..3fc33ad
--- /dev/null
+++ b/src/InetAddress.cpp
@@ -0,0 +1,260 @@
+/*
+   Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+   Queen in Right of Canada (Communications Research Center Canada)
+   */
+/*
+   This file is part of ODR-DabMux.
+
+   ODR-DabMux 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.
+
+   ODR-DabMux 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 ODR-DabMux.  If not, see <http://www.gnu.org/licenses/>.
+   */
+
+#include "InetAddress.h"
+#include <iostream>
+#include <stdio.h>
+
+#ifdef _WIN32
+#else
+# include <errno.h>
+# include <string.h>
+#endif
+
+#ifdef TRACE_ON
+# ifndef TRACE_CLASS
+#  define TRACE_CLASS(clas, func) cout <<"-" <<(clas) <<"\t(" <<this <<")::" <<(func) <<endl
+#  define TRACE_STATIC(clas, func) cout <<"-" <<(clas) <<"\t(static)::" <<(func) <<endl
+# endif
+#else
+# ifndef TRACE_CLASS
+#  define TRACE_CLASS(clas, func)
+#  define TRACE_STATIC(clas, func)
+# endif
+#endif
+
+
+int inetErrNo = 0;
+const char *inetErrMsg = NULL;
+const char *inetErrDesc = NULL;
+
+
+/**
+ *  Constructs an IP address.
+ *  @param port The port of this address
+ *  @param name The name of this address
+ */
+InetAddress::InetAddress(int port, const char* name) {
+    TRACE_CLASS("InetAddress", "InetAddress(int, char)");
+    addr.sin_family = PF_INET;
+    addr.sin_addr.s_addr = htons(INADDR_ANY);
+    addr.sin_port = htons(port);
+    if (name)
+        setAddress(name);
+}
+
+
+/**
+ *  Constructs a copy of inet
+ *  @param inet The address to be copied
+ */
+InetAddress::InetAddress(const InetAddress &inet) {
+    TRACE_CLASS("InetAddress", "InetAddress(InetAddress)");
+    memcpy(&addr, &inet.addr, sizeof(addr));
+}
+
+
+/// Destructor
+InetAddress::~InetAddress() {
+    TRACE_CLASS("InetAddress" ,"~InetAddress()");
+}
+
+
+/// Returns the raw IP address of this InetAddress object.
+sockaddr *InetAddress::getAddress() {
+    TRACE_CLASS("InetAddress", "getAddress()");
+    return (sockaddr *)&addr;
+}
+
+
+/// Return the port of this address.
+int InetAddress::getPort()
+{
+    TRACE_CLASS("InetAddress", "getPort()");
+    return ntohs(addr.sin_port);
+}
+
+
+/**
+ *  Returns the IP address string "%d.%d.%d.%d".
+ *  @return IP address
+ */
+const char *InetAddress::getHostAddress() {
+    TRACE_CLASS("InetAddress", "getHostAddress()");
+    return inet_ntoa(addr.sin_addr);
+}
+
+
+/// Returns true if this address is multicast
+bool InetAddress::isMulticastAddress() {
+    TRACE_CLASS("InetAddress", "isMulticastAddress()");
+    return IN_MULTICAST(ntohl(addr.sin_addr.s_addr));		// a modifier
+}
+
+
+/**
+ *  Set the port number
+ *  @param port The new port number
+ */
+void InetAddress::setPort(int port)
+{
+    TRACE_CLASS("InetAddress", "setPort(int)");
+    addr.sin_port = htons(port);
+}
+
+
+/**
+ *  Set the address
+ *  @param name The new address name
+ *  @return 0  if ok
+ *          -1 if error
+ */
+int InetAddress::setAddress(const char *name)
+{
+    TRACE_CLASS("InetAddress", "setAddress(char*)");
+    if (name) {
+        if (atoi(name)) {   // If it start with a number
+            if ((addr.sin_addr.s_addr = inet_addr(name)) == INADDR_NONE) {
+                addr.sin_addr.s_addr = htons(INADDR_ANY);
+                inetErrNo = 0;
+                inetErrMsg = "Invalid address";
+                inetErrDesc = name;
+                return -1;
+            }
+        } else {            // Assume it's a real name
+            hostent *host = gethostbyname(name);
+            if (host) {
+                addr.sin_addr = *(in_addr *)(host->h_addr);
+            } else {
+                addr.sin_addr.s_addr = htons(INADDR_ANY);
+                inetErrNo = 0;
+                inetErrMsg = "Could not find address";
+                inetErrDesc = name;
+                return -1;
+            }
+        }
+    } else {
+        addr.sin_addr.s_addr = INADDR_ANY;
+    }
+    return 0;
+}
+
+
+void setInetError(const char* description)
+{
+    inetErrNo = 0;
+#ifdef _WIN32
+    inetErrNo = WSAGetLastError();
+    switch (inetErrNo) {
+        case WSANOTINITIALISED:
+            inetErrMsg = "WSANOTINITIALISED A successful WSAStartup must occur before using this function.";
+            break;
+        case WSAENETDOWN:
+            inetErrMsg = "WSAENETDOWN The network subsystem has failed.";
+            break;
+        case WSAEFAULT:
+            inetErrMsg = "WSAEFAULT The buf or from parameters are not part of the user address space, or the fromlen parameter is too small to accommodate the peer address.";
+            break;
+        case WSAEINTR:
+            inetErrMsg = "WSAEINTR The (blocking) call was canceled through WSACancelBlockingCall.";
+            break;
+        case WSAEINPROGRESS:
+            inetErrMsg = "WSAEINPROGRESS A blocking Windows Sockets 1.1 call is in progress, or the service provider is still processing a callback function.";
+            break;
+        case WSAEINVAL:
+            inetErrMsg = "WSAEINVAL The socket has not been bound with bind, or an unknown flag was specified, or MSG_OOB was specified for a socket with SO_OOBINLINE enabled, or (for byte stream-style sockets only) len was zero or negative.";
+            break;
+        case WSAEISCONN:
+            inetErrMsg = "WSAEISCONN The socket is connected. This function is not permitted with a connected socket, whether the socket is connection-oriented or connectionless.";
+            break;
+        case WSAENETRESET:
+            inetErrMsg = "WSAENETRESET The connection has been broken due to the \"keep-alive\" activity detecting a failure while the operation was in progress.";
+            break;
+        case WSAENOTSOCK:
+            inetErrMsg = "WSAENOTSOCK The descriptor is not a socket.";
+            break;
+        case WSAEOPNOTSUPP:
+            inetErrMsg = "WSAEOPNOTSUPP MSG_OOB was specified, but the socket is not stream-style such as type SOCK_STREAM, out-of-band data is not supported in the communication domain associated with this socket, or the socket is unidirectional and supports only send operations.";
+            break;
+        case WSAESHUTDOWN:
+            inetErrMsg = "WSAESHUTDOWN The socket has been shut down; it is not possible to recvfrom on a socket after shutdown has been invoked with how set to SD_RECEIVE or SD_BOTH.";
+            break;
+        case WSAEWOULDBLOCK:
+            inetErrMsg = "WSAEWOULDBLOCK The socket is marked as nonblocking and the recvfrom operation would block.";
+            break;
+        case WSAEMSGSIZE:
+            inetErrMsg = "WSAEMSGSIZE The message was too large to fit into the specified buffer and was truncated.";
+            break;
+        case WSAETIMEDOUT:
+            inetErrMsg = "WSAETIMEDOUT The connection has been dropped, because of a network failure or because the system on the other end went down without notice.";
+            break;
+        case WSAECONNRESET:
+            inetErrMsg = "WSAECONNRESET";
+            break;
+        case WSAEACCES:
+            inetErrMsg = "WSAEACCES The requested address is a broadcast address, but the appropriate flag was not set. Call setsockopt with the SO_BROADCAST parameter to allow the use of the broadcast address.";
+            break;
+        case WSAENOBUFS:
+            inetErrMsg = "WSAENOBUFS No buffer space is available.";
+            break;
+        case WSAENOTCONN:
+            inetErrMsg = "WSAENOTCONN The socket is not connected (connection-oriented sockets only)";
+            break;
+        case WSAEHOSTUNREACH:
+            inetErrMsg = "WSAEHOSTUNREACH The remote host cannot be reached from this host at this time.";
+            break;
+        case WSAECONNABORTED:
+            inetErrMsg = "WSAECONNABORTED The virtual circuit was terminated due to a time-out or other failure. The application should close the socket as it is no longer usable.";
+            break;
+        case WSAEADDRNOTAVAIL:
+            inetErrMsg = "WSAEADDRNOTAVAIL The remote address is not a valid address, for example, ADDR_ANY.";
+            break;
+        case WSAEAFNOSUPPORT:
+            inetErrMsg = "WSAEAFNOSUPPORT Addresses in the specified family cannot be used with this socket.";
+            break;
+        case WSAEDESTADDRREQ:
+            inetErrMsg = "WSAEDESTADDRREQ A destination address is required.";
+            break;
+        case WSAENETUNREACH:
+            inetErrMsg = "WSAENETUNREACH The network cannot be reached from this host at this time.";
+            break;
+        case WSAEMFILE:
+            inetErrMsg = "No more socket descriptors are available.";
+            break;
+        case WSAEPROTONOSUPPORT:
+            inetErrMsg = "The specified protocol is not supported.";
+            break;
+        case WSAEPROTOTYPE:
+            inetErrMsg = "The specified protocol is the wrong type for this socket.";
+            break;
+        case WSAESOCKTNOSUPPORT:
+            inetErrMsg = "The specified socket type is not supported in this address family.";
+            break;
+        default:
+            inetErrMsg = "Unknown";
+    };
+#else
+    inetErrNo = errno;
+    inetErrMsg = strerror(inetErrNo);
+#endif
+    inetErrDesc = description;
+}
+
diff --git a/src/InetAddress.h b/src/InetAddress.h
new file mode 100644
index 0000000..266b1fd
--- /dev/null
+++ b/src/InetAddress.h
@@ -0,0 +1,91 @@
+/*
+   Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+   Queen in Right of Canada (Communications Research Center Canada)
+   */
+/*
+   This file is part of ODR-DabMux.
+
+   ODR-DabMux 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.
+
+   ODR-DabMux 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 ODR-DabMux.  If not, see <http://www.gnu.org/licenses/>.
+   */
+
+#ifndef _InetAddress
+#define _InetAddress
+
+#ifdef HAVE_CONFIG_H
+#   include "config.h"
+#endif
+
+// General libraries
+#include <stdlib.h>
+// Linux librairies
+#ifndef _WIN32
+// # include <sys/types.h>
+# include <sys/socket.h>
+# include <netinet/in.h>
+# include <unistd.h>
+# include <netdb.h>
+# include <arpa/inet.h>
+# include <pthread.h>
+# define SOCKET                  int
+# define INVALID_SOCKET  -1
+# define closesocket             ::close
+// Windows librairies
+#else
+#   include <winsock.h>
+#   ifdef _MSC_VER
+#       pragma comment(lib, "wsock32.lib")
+#   elif defined(__BORLANDC__)
+#       pragma(lib, "mswsock.lib")
+#   endif
+#   ifndef IN_MULTICAST
+#       define IN_MULTICAST(a)  ((((unsigned long) (a)) & 0xf0000000) == 0xe0000000)
+#   endif
+#endif
+// General definitions
+#define INVALID_PORT            -1
+
+
+/// The last error number
+extern int inetErrNo;
+/// The last error message
+extern const char *inetErrMsg;
+/// The description of the last error
+extern const char *inetErrDesc;
+/// Set the number, message and description of the last error
+void setInetError(const char* description);
+
+
+/**
+ *  This class represents an Internet Protocol (IP) address.
+ *  @author Pascal Charest pascal.charest@crc.ca
+ */
+class InetAddress {
+ public:
+  InetAddress(int port = 0, const char* name = NULL);
+  InetAddress(const InetAddress &addr);
+  ~InetAddress();
+
+  sockaddr *getAddress();
+  const char *getHostAddress();
+  int getPort();
+  int setAddress(const char *name);
+  void setPort(int port);
+  bool isMulticastAddress();
+
+ private:
+  sockaddr_in addr;
+};
+
+
+#endif
diff --git a/src/OrderedQueue.cpp b/src/OrderedQueue.cpp
new file mode 100644
index 0000000..2aad726
--- /dev/null
+++ b/src/OrderedQueue.cpp
@@ -0,0 +1,158 @@
+/* ------------------------------------------------------------------
+ * 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 "OrderedQueue.h"
+#include <cstring>
+#include <cstdio>
+#include <stdint.h>
+
+#define DEBUG(fmt, A...)   fprintf(stderr, "OrderedQueue: " fmt, ##A)
+//#define DEBUG(x...)
+#define ERROR(fmt, A...)   fprintf(stderr, "OrderedQueue: ERROR " fmt, ##A)
+
+/*
+ * 
+ */
+OrderedQueue::OrderedQueue(int countModulo, size_t capacity)
+    :   _countModulo(countModulo),
+        _capacity(capacity),
+        _duplicated(0),
+        _overruns(0),
+        _lastCount(-1)
+{
+}
+
+/*
+ * 
+ */
+OrderedQueue::~OrderedQueue()
+{
+    StockIterator it = _stock.begin();
+    while (it != _stock.end()) {
+        delete it->second;
+        it++;
+    }
+}
+
+/*
+ * 
+ */
+void OrderedQueue::push(int32_t count, const uint8_t* buf, size_t size)
+{
+//    DEBUG("OrderedQueue::push count=%d\n", count);
+    count = (count+_countModulo) % _countModulo;
+    
+    // First frame makes the count initialisation.
+    if( _lastCount == -1 )
+    {
+        _lastCount = (count+_countModulo-1)%_countModulo;
+    }
+
+    if (_stock.size() < _capacity) {
+        StockIterator it = _stock.find(count);
+        OrderedQueueData* oqd = new OrderedQueueData(buf, size);
+        if (it == _stock.end()) {
+            if (_stock.insert(std::make_pair(count, oqd)).second == false) {
+                ERROR("%d not inserted\n", count);
+                delete oqd;
+            }
+        }
+        else {
+            // count already exists, duplicated frame
+            // Replace the old one by the new one.
+            // the old one could a an old frame from the previous count loop
+            delete it->second;
+            it->second = oqd;
+            _duplicated++;
+            DEBUG("Duplicated count=%d\n", count);
+        }
+    }
+    else {
+        _overruns++;
+        if (_overruns < 100)
+            DEBUG("Overruns (size=%zu) count=%d not inserted\n", _stock.size(), count);
+        else if (_overruns == 100)
+            DEBUG("stop displaying Overruns\n");           
+    }
+}
+
+/*
+ * 
+ */
+bool OrderedQueue::availableData()
+{
+    // TODO Wait for filling gaps
+    return _stock.size() > 0;
+}
+
+/*
+ * 
+ */
+size_t OrderedQueue::pop(std::vector<uint8_t>& buf, int32_t* retCount)
+{    
+    size_t nbBytes = 0;
+    uint32_t gap = 0;
+    
+    if (_stock.size() > 0) {
+        int32_t nextCount = (_lastCount+1)%_countModulo;  
+        bool found = false;
+        do {
+            StockIterator it = _stock.find(nextCount);
+            if (it != _stock.end()) {
+                OrderedQueueData* oqd = it->second;
+                buf.resize(oqd->getSize());
+                memcpy(buf.data(), oqd->getData(), oqd->getSize());
+                nbBytes = oqd->getSize();
+                delete oqd;
+                _stock.erase(it);
+                _lastCount = nextCount;
+                if (retCount) *retCount = _lastCount;
+                found = true;
+            } else
+            {
+                if( _stock.size() < _capacity ) found = true;
+                else {
+                    // Search for the new reference count, starting from the current one                    
+                    // This could be optimised, but the modulo makes things
+                    // not easy.
+                    gap++;
+                    nextCount = (nextCount+1)%_countModulo;
+                }
+            }
+        } while( !found );
+    }
+
+    if( gap > 0 )
+    {
+        DEBUG("Count jump of %d\n", gap);
+    }
+//    if (nbBytes > 0 && retCount) DEBUG("OrderedQueue::pop count=%d\n", *retCount);
+    return nbBytes;
+}
+
+OrderedQueueData::OrderedQueueData(const uint8_t* data, size_t size)
+{
+    _data = new uint8_t[size];
+    memcpy(_data, data, size);
+    _size = size;
+}
+
+OrderedQueueData::~OrderedQueueData()
+{
+    delete [] _data;
+}
diff --git a/src/OrderedQueue.h b/src/OrderedQueue.h
new file mode 100644
index 0000000..5b4a965
--- /dev/null
+++ b/src/OrderedQueue.h
@@ -0,0 +1,65 @@
+/* ------------------------------------------------------------------
+ * 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.
+ * -------------------------------------------------------------------
+ */
+
+#ifndef _ORDERED_QUEUE_H_
+#define _ORDERED_QUEUE_H_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string>
+#include <map>
+#include <vector>
+
+class OrderedQueueData;
+
+class OrderedQueue
+{
+    public:
+        OrderedQueue(int32_t countModulo, size_t capacity);
+        ~OrderedQueue();
+
+        void push(int32_t count, const uint8_t* buf, size_t size);
+        bool availableData();
+        size_t pop(std::vector<uint8_t>& buf, int32_t* retCount=NULL);
+
+    private:
+        int32_t     _countModulo;
+        size_t      _capacity;
+        uint64_t    _duplicated;
+        uint64_t    _overruns;        
+        int32_t     _lastCount;
+
+        std::map<int, OrderedQueueData*> _stock;
+        typedef std::map<int, OrderedQueueData*>::iterator StockIterator;
+};
+
+class OrderedQueueData
+{
+    public:
+        OrderedQueueData(const uint8_t* data, size_t size);
+        ~OrderedQueueData();
+        
+        uint8_t* getData()  { return _data; }
+        size_t   getSize()  { return _size; }
+
+    private:
+        uint8_t* _data;
+        size_t _size;
+};
+
+#endif // _ORDERED_QUEUE_H_
diff --git a/src/UdpSocket.cpp b/src/UdpSocket.cpp
new file mode 100644
index 0000000..8ac3706
--- /dev/null
+++ b/src/UdpSocket.cpp
@@ -0,0 +1,510 @@
+/*
+   Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+   Queen in Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2015 Matthias P. Braendli
+    http://www.opendigitalradio.org
+   */
+/*
+   This file is part of ODR-DabMux.
+
+   ODR-DabMux 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.
+
+   ODR-DabMux 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 ODR-DabMux.  If not, see <http://www.gnu.org/licenses/>.
+   */
+
+#include "UdpSocket.h"
+
+#include <iostream>
+#include <stdio.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <string.h>
+
+#ifdef TRACE_ON
+# ifndef TRACE_CLASS
+#  define TRACE_CLASS(class, func) cout <<"-" <<(class) <<"\t(" <<this <<")::" <<(func) <<endl
+#  define TRACE_STATIC(class, func) cout <<"-" <<(class) <<"\t(static)::" <<(func) <<endl
+# endif
+#else
+# ifndef TRACE_CLASS
+#  define TRACE_CLASS(class, func)
+#  define TRACE_STATIC(class, func)
+# endif
+#endif
+
+
+/// Must be call once before doing any operation on sockets
+int UdpSocket::init()
+{
+#ifdef _WIN32
+  WSADATA wsaData;
+  WORD wVersionRequested = wVersionRequested = MAKEWORD( 2, 2 );
+  
+  int res = WSAStartup( wVersionRequested, &wsaData );
+  if (res) {
+    setInetError("Can't initialize winsock");
+    return -1;
+  }
+#endif
+  return 0;
+}
+
+
+/// Must be call once before leaving application
+int UdpSocket::clean()
+{
+#ifdef _WIN32
+  int res = WSACleanup();
+  if (res) {
+    setInetError("Can't initialize winsock");
+    return -1;
+  }
+#endif
+  return 0;
+}
+
+
+/**
+ *  Two step constructor. Create must be called prior to use this
+ *  socket.
+ */
+UdpSocket::UdpSocket() :
+  listenSocket(INVALID_SOCKET)
+{
+  TRACE_CLASS("UdpSocket", "UdpSocket()");
+}
+
+
+/**
+ *  One step constructor.
+ *  @param port The port number on which the socket will be bind
+ *  @param name The IP address on which the socket will be bind.
+ *              It is used to bind the socket on a specific interface if
+ *              the computer have many NICs.
+ */
+UdpSocket::UdpSocket(int port, char *name) :
+  listenSocket(INVALID_SOCKET)
+{
+  TRACE_CLASS("UdpSocket", "UdpSocket(int, char*)");
+  create(port, name);
+}
+
+
+/**
+ *  This functin set blocking mode. The socket can be blocking or not,
+ *  depending of the parametre. By default, the socket is blocking.
+ *  @param block If true, set the socket blocking, otherwise set non-blocking
+ *  @return 0  if ok
+ *          -1 if error
+ */
+int UdpSocket::setBlocking(bool block)
+{
+#ifdef _WIN32
+	unsigned long res = block ? 0 : 1;
+	if (ioctlsocket(listenSocket, FIONBIO, &res) != 0) {
+        setInetError("Can't change blocking state of socket");
+        return -1;
+	}
+    return 0;
+#else
+  int res;
+  if (block)
+    res = fcntl(listenSocket, F_SETFL, 0);
+  else
+    res = fcntl(listenSocket, F_SETFL, O_NONBLOCK);
+  if (res == SOCKET_ERROR) {
+    setInetError("Can't change blocking state of socket");
+    return -1;
+  }
+  return 0;
+#endif
+}
+
+
+/**
+ *  Two step initializer. This function must be called after the constructor
+ *  without argument as been called.
+ *  @param port The port number on which the socket will be bind
+ *  @param name The IP address on which the socket will be bind.
+ *              It is used to bind the socket on a specific interface if
+ *              the computer have many NICs.
+ *  @return 0  if ok
+ *          -1 if error
+ */
+int UdpSocket::create(int port, char *name)
+{
+  TRACE_CLASS("UdpSocket", "create(int, char*)");
+  if (listenSocket != INVALID_SOCKET)
+    closesocket(listenSocket);
+  address.setAddress(name);
+  address.setPort(port);
+  if ((listenSocket = socket(PF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET) {
+    setInetError("Can't create socket");
+    return -1;
+  }
+  reuseopt_t reuse = 1;
+  if (setsockopt(listenSocket, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse))
+      == SOCKET_ERROR) {
+    setInetError("Can't reuse address");
+    return -1;
+  }
+
+  if (bind(listenSocket, address.getAddress(), sizeof(sockaddr_in)) == SOCKET_ERROR) {
+    setInetError("Can't bind socket");
+    closesocket(listenSocket);
+    listenSocket = INVALID_SOCKET;
+    return -1;
+  }
+  return 0;
+}
+
+
+/// Destructor
+UdpSocket::~UdpSocket() {
+  TRACE_CLASS("UdpSocket", "~UdpSocket()");
+  if (listenSocket != INVALID_SOCKET)
+    closesocket(listenSocket);
+}
+
+
+/**
+ *  Receive an UDP packet.
+ *  @param packet The packet that will receive data. The address will be set
+ *                to the source address.
+ *  @return 0 if ok, -1 if error
+ */
+int UdpSocket::receive(UdpPacket &packet)
+{
+  TRACE_CLASS("UdpSocket", "receive(UdpPacket)");
+  socklen_t addrSize;
+  addrSize = sizeof(*packet.getAddress().getAddress());
+  int ret = recvfrom(listenSocket, packet.getData(), packet.getSize() - packet.getOffset(), 0,
+		     packet.getAddress().getAddress(), &addrSize);
+  if (ret == SOCKET_ERROR) {
+    packet.setLength(0);
+#ifndef _WIN32
+  if (errno == EAGAIN)
+    return 0;
+#endif
+    setInetError("Can't receive UDP packet");
+    return -1;
+  }
+  packet.setLength(ret);
+  if (ret == (long)packet.getSize()) {
+    packet.setSize(packet.getSize() << 1);
+  }
+  return 0;
+}
+
+/**
+ *  Send an UDP packet.
+ *  @param packet The UDP packet to be sent. It includes the data and the
+ *                destination address
+ *  return 0 if ok, -1 if error
+ */
+int UdpSocket::send(UdpPacket &packet)
+{
+#ifdef DUMP
+  TRACE_CLASS("UdpSocket", "send(UdpPacket)");
+#endif
+  int ret = sendto(listenSocket, packet.getData(), packet.getLength(), 0,
+		   packet.getAddress().getAddress(), sizeof(*packet.getAddress().getAddress()));
+  if (ret == SOCKET_ERROR
+#ifndef _WIN32
+      && errno != ECONNREFUSED
+#endif
+      ) {
+    setInetError("Can't send UDP packet");
+    return -1;
+  }
+  return 0;
+}
+
+
+/**
+ *  Send an UDP packet
+ *
+ *  return 0 if ok, -1 if error
+ */
+int UdpSocket::send(std::vector<uint8_t> data, InetAddress destination)
+{
+#ifdef DUMP
+  TRACE_CLASS("UdpSocket", "send(vector<uint8_t>)");
+#endif
+  int ret = sendto(listenSocket, &data[0], data.size(), 0,
+		   destination.getAddress(), sizeof(*destination.getAddress()));
+  if (ret == SOCKET_ERROR
+#ifndef _WIN32
+      && errno != ECONNREFUSED
+#endif
+      ) {
+    setInetError("Can't send UDP packet");
+    return -1;
+  }
+  return 0;
+}
+
+
+/**
+ *  Must be called to receive data on a multicast address.
+ *  @param groupname The multica
+st address to join.
+ *  @return 0 if ok, -1 if error
+ */
+int UdpSocket::joinGroup(char* groupname)
+{
+  TRACE_CLASS("UdpSocket", "joinGroup(char*)");
+#ifdef _WIN32
+  ip_mreq group;
+#else
+  ip_mreqn group;
+#endif
+  if ((group.imr_multiaddr.s_addr = inet_addr(groupname)) == INADDR_NONE) {
+    setInetError(groupname);
+    return -1;
+  }
+  if (!IN_MULTICAST(ntohl(group.imr_multiaddr.s_addr))) {
+    setInetError("Not a multicast address");
+    return -1;
+  }
+#ifdef _WIN32
+  group.imr_interface.s_addr = 0;
+  if (setsockopt(listenSocket, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char*)&group, sizeof(group))
+      == SOCKET_ERROR) {
+    setInetError("Can't join multicast group");
+    return -1;
+  }
+#else
+  group.imr_address.s_addr = htons(INADDR_ANY);;
+  group.imr_ifindex = 0;
+  if (setsockopt(listenSocket, IPPROTO_IP, IP_ADD_MEMBERSHIP, &group, sizeof(group))
+      == SOCKET_ERROR) {
+    setInetError("Can't join multicast group");
+  }
+#endif
+  return 0;
+}
+
+int UdpSocket::setMulticastTTL(int ttl)
+{
+    if (setsockopt(listenSocket, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl))
+            == SOCKET_ERROR) {
+        setInetError("Can't set ttl");
+        return -1;
+    }
+
+    return 0;
+}
+
+int UdpSocket::setMulticastSource(const char* source_addr)
+{
+    struct in_addr addr;
+    if (inet_aton(source_addr, &addr) == 0) {
+        setInetError("Can't parse source address");
+        return -1;
+    }
+
+    if (setsockopt(listenSocket, IPPROTO_IP, IP_MULTICAST_IF, &addr, sizeof(addr))
+            == SOCKET_ERROR) {
+        setInetError("Can't set source address");
+        return -1;
+    }
+
+    return 0;
+}
+
+
+/**
+ *  Constructs an UDP packet.
+ *  @param initSize The initial size of the data buffer
+ */
+UdpPacket::UdpPacket(unsigned int initSize) :
+  dataBuf(new char[initSize]),
+  length(0),
+  size(initSize),
+  offset(0)
+{
+  TRACE_CLASS("UdpPacket", "UdpPacket(unsigned int)");
+  if (dataBuf == NULL)
+    size = 0;
+}
+
+
+/// Destructor
+UdpPacket::~UdpPacket()
+{
+  TRACE_CLASS("UdpPacket", "~UdpPacket()");
+  if (dataBuf != NULL) {
+    delete []dataBuf;
+    dataBuf = NULL;
+  }
+}
+  
+
+/**
+ *  Changes size of the data buffer size. \a Length + \a offset data will be copied
+ *  in the new buffer.
+ *  @warning The pointer to data will be changed
+ *  @param newSize The new data buffer size
+ */
+void UdpPacket::setSize(unsigned newSize)
+{
+  TRACE_CLASS("UdpPacket", "setSize(unsigned)");
+  char *tmp = new char[newSize];
+  if (length > newSize)
+    length = newSize;
+  if (tmp) {
+    memcpy(tmp, dataBuf, length);
+    delete []dataBuf;
+    dataBuf = tmp;
+    size = newSize;
+  }
+}
+
+
+/**
+ *  Give the pointer to data. It is ajusted with the \a offset.
+ *  @warning This pointer change. when the \a size of the buffer and the \a offset change.
+ *  @return The pointer
+ */
+char *UdpPacket::getData()
+{
+  return dataBuf + offset;
+}
+
+
+/**
+ *  Add some data at the end of data buffer and adjust size.
+ *  @param data Pointer to the data to add
+ *  @param size Size in bytes of new data
+ */
+void UdpPacket::addData(const void *data, unsigned size)
+{
+  if (length + size > this->size) {
+    setSize(this->size << 1);
+  }
+  memcpy(dataBuf + length, data, size);
+  length += size;
+}
+
+
+/**
+ *  Returns the length of useful data. Data before the \a offset are ignored.
+ *  @return The data length
+ */
+unsigned long UdpPacket::getLength()
+{
+  return length - offset;
+}
+
+
+/**
+ *  Returns the size of the data buffer.
+ *  @return The data buffer size
+ */
+unsigned long UdpPacket::getSize()
+{
+  return size;
+}
+
+
+/**
+ *  Returns the offset value.
+ *  @return The offset value
+ */
+unsigned long UdpPacket::getOffset()
+{
+  return offset;
+}
+
+
+/**
+ *  Sets the data length value. Data before the \a offset are ignored.
+ *  @param len The new length of data
+ */
+void UdpPacket::setLength(unsigned long len)
+{
+  length = len + offset;
+}
+
+
+/**
+ *  Sets the data offset. Data length is ajusted to ignore data before the \a offset.
+ *  @param val The new data offset.
+ */
+void UdpPacket::setOffset(unsigned long val)
+{
+  offset = val;
+  if (offset > length)
+    length = offset;
+}
+
+
+/**
+ *  Returns the UDP address of the data.
+ *  @return The UDP address
+ */
+InetAddress &UdpPacket::getAddress()
+{
+  return address;
+}
+
+/*
+WSAEINTR
+WSAEBADF
+WSAEACCES
+WSAEFAULT
+WSAEINVAL
+WSAEMFILE
+WSAEWOULDBLOCK
+WSAEINPROGRESS
+WSAEALREADY
+WSAENOTSOCK
+WSAEDESTADDRREQ
+WSAEMSGSIZE
+WSAEPROTOTYPE
+WSAENOPROTOOPT
+WSAEPROTONOSUPPORT
+WSAESOCKTNOSUPPORT
+WSAEOPNOTSUPP
+WSAEPFNOSUPPORT
+WSAEAFNOSUPPORT
+WSAEADDRINUSE
+WSAEADDRNOTAVAIL
+WSAENETDOWN
+WSAENETUNREACH
+WSAENETRESET
+WSAECONNABORTED
+WSAECONNRESET
+WSAENOBUFS
+WSAEISCONN
+WSAENOTCONN
+WSAESHUTDOWN
+WSAETOOMANYREFS
+WSAETIMEDOUT
+WSAECONNREFUSED
+WSAELOOP
+WSAENAMETOOLONG
+WSAEHOSTDOWN
+WSAEHOSTUNREACH
+WSAENOTEMPTY
+WSAEPROCLIM
+WSAEUSERS
+WSAEDQUOT
+WSAESTALE
+WSAEREMOTE
+WSAEDISCON
+WSASYSNOTREADY
+WSAVERNOTSUPPORTED
+WSANOTINITIALISED
+*/
diff --git a/src/UdpSocket.h b/src/UdpSocket.h
new file mode 100644
index 0000000..07e9f0e
--- /dev/null
+++ b/src/UdpSocket.h
@@ -0,0 +1,138 @@
+/*
+   Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+   Queen in Right of Canada (Communications Research Center Canada)
+
+   Copyright (C) 2015 Matthias P. Braendli
+    http://www.opendigitalradio.org
+   */
+/*
+   This file is part of ODR-DabMux.
+
+   ODR-DabMux 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.
+
+   ODR-DabMux 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 ODR-DabMux.  If not, see <http://www.gnu.org/licenses/>.
+   */
+
+#ifndef _UDPSOCKET
+#define _UDPSOCKET
+
+#ifdef HAVE_CONFIG_H
+#   include "config.h"
+#endif
+
+#include "InetAddress.h"
+#ifdef _WIN32
+# include <winsock.h>
+# define socklen_t        int
+# define reuseopt_t       char
+#else
+# include <sys/socket.h>
+# include <netinet/in.h>
+# include <unistd.h>
+# include <netdb.h>
+# include <arpa/inet.h>
+# include <pthread.h>
+# define SOCKET           int
+# define INVALID_SOCKET   -1
+# define SOCKET_ERROR     -1
+# define reuseopt_t       int
+#endif
+//#define INVALID_PORT      -1
+
+#include <stdlib.h>
+#include <iostream>
+#include <vector>
+
+class UdpPacket;
+
+
+/**
+ *  This class represents a socket for sending and receiving UDP packets.
+ *
+ *  A UDP socket is the sending or receiving point for a packet delivery service.
+ *  Each packet sent or received on a datagram socket is individually
+ *  addressed and routed. Multiple packets sent from one machine to another may
+ *  be routed differently, and may arrive in any order.
+ *  @author Pascal Charest pascal.charest@crc.ca
+ */
+class UdpSocket {
+ public:
+  UdpSocket();
+  UdpSocket(int port, char *name = NULL);
+  ~UdpSocket();
+  UdpSocket(const UdpSocket& other) = delete;
+  const UdpSocket& operator=(const UdpSocket& other) = delete;
+
+  static int init();
+  static int clean();
+
+  int create(int port = 0, char *name = NULL);
+
+  int send(UdpPacket &packet);
+  int send(const std::vector<uint8_t> data);
+  int send(std::vector<uint8_t> data, InetAddress destination);
+  int receive(UdpPacket &packet);
+  int joinGroup(char* groupname);
+  int setMulticastSource(const char* source_addr);
+  int setMulticastTTL(int ttl);
+  /**
+   *  Connects the socket on a specific address. Only data from this address
+   *  will be received.
+   *  @param addr The address to connect the socket
+   *  @warning Not implemented yet.
+   */
+  void connect(InetAddress &addr);
+  int setBlocking(bool block);
+
+ protected:
+  /// The address on which the socket is binded.
+  InetAddress address;
+  /// The low-level socket used by system functions.
+  SOCKET listenSocket;
+};
+
+/**
+ *  This class represents a UDP packet. 
+ *
+ *  UDP packets are used to implement a connectionless packet delivery service.
+ *  Each message is routed from one machine to another based solely on
+ *  information contained within that packet. Multiple packets sent from one
+ *  machine to another might be routed differently, and might arrive in any order. 
+ *  @author Pascal Charest pascal.charest@crc.ca
+ */
+class UdpPacket {
+ public:
+  UdpPacket(unsigned int initSize = 1024);
+  UdpPacket(const UdpPacket& packet) = delete;
+  const UdpPacket& operator=(const UdpPacket&) = delete;
+  UdpPacket(const UdpPacket&& packet) = delete;
+  const UdpPacket& operator=(const UdpPacket&&) = delete;
+  ~UdpPacket();
+
+  char *getData();
+  void addData(const void *data, unsigned size);
+  unsigned long getLength();
+  unsigned long getSize();
+  unsigned long getOffset();
+  void setLength(unsigned long len);
+  void setOffset(unsigned long val);
+  void setSize(unsigned newSize);
+  InetAddress &getAddress();
+  
+ private:
+  char *dataBuf;
+  unsigned long length, size, offset;
+  InetAddress address;
+};
+
+#endif // _UDPSOCKET
+
diff --git a/src/crc.c b/src/crc.c
new file mode 100644
index 0000000..cc02473
--- /dev/null
+++ b/src/crc.c
@@ -0,0 +1,266 @@
+/*
+   Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+   Queen in Right of Canada (Communications Research Center Canada)
+   */
+/*
+   This file is part of ODR-DabMux.
+
+   ODR-DabMux 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.
+
+   ODR-DabMux 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 ODR-DabMux.  If not, see <http://www.gnu.org/licenses/>.
+   */
+
+#include "crc.h"
+#ifndef _WIN32
+#    include <unistd.h>
+#    include <netinet/in.h>
+#endif
+#include <stdio.h>
+#include <fcntl.h>
+
+//#define CCITT       0x1021
+
+uint8_t crc8tab[256] = {
+    0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15,
+    0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
+    0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
+    0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
+    0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5,
+    0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
+    0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85,
+    0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
+    0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
+    0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
+    0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2,
+    0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
+    0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32,
+    0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
+    0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
+    0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
+    0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c,
+    0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
+    0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec,
+    0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
+    0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
+    0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
+    0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c,
+    0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
+    0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b,
+    0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
+    0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
+    0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
+    0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb,
+    0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
+    0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb,
+    0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
+};
+
+
+uint16_t crc16tab[256] = {
+    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+    0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+    0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+    0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+    0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+    0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+    0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+    0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+    0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+    0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+    0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+    0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+    0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+    0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+    0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+    0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+    0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+    0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+    0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+    0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+    0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+    0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+    0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+    0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+    0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+    0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+    0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+    0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+    0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+    0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+    0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+    0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
+};
+
+
+uint32_t crc32tab[256] = {
+    0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9,
+    0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
+    0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61,
+    0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
+    0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9,
+    0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75,
+    0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011,
+    0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd,
+    0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039,
+    0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5,
+    0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81,
+    0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d,
+    0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49,
+    0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95,
+    0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1,
+    0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d,
+    0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae,
+    0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072,
+    0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16,
+    0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca,
+    0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde,
+    0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02,
+    0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066,
+    0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba,
+    0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e,
+    0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692,
+    0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6,
+    0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a,
+    0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e,
+    0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2,
+    0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686,
+    0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a,
+    0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637,
+    0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb,
+    0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f,
+    0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53,
+    0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47,
+    0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b,
+    0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff,
+    0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623,
+    0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7,
+    0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b,
+    0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f,
+    0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3,
+    0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7,
+    0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b,
+    0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f,
+    0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3,
+    0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640,
+    0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c,
+    0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8,
+    0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24,
+    0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30,
+    0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec,
+    0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088,
+    0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654,
+    0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0,
+    0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c,
+    0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18,
+    0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4,
+    0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0,
+    0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c,
+    0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668,
+    0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4
+};
+
+// This function can be used to create a new table with a different polynom
+void init_crc8tab(uint8_t l_code, uint8_t l_init)
+{
+    unsigned i, j, msb;
+    uint8_t nb;
+    uint8_t crc;
+
+    for (i = 0; i < 256; ++i) {
+        crc = l_init;
+        nb = i ^ 0xff;
+        for (j = 0; j < 8; ++j) {
+            msb = (nb & (0x80 >> j)) && 1;
+            msb ^= (crc >> 7);
+            crc <<= 1;
+            if (msb)
+                crc ^= l_code;
+        }
+        crc8tab[i] = crc;
+    }
+}
+
+
+void init_crc16tab(uint16_t l_code, uint16_t l_init)
+{
+    unsigned i, j, msb;
+    uint8_t nb;
+    uint16_t crc;
+
+    for (i = 0; i < 256; ++i) {
+        crc = l_init;
+        nb = i ^ 0xff;
+        for (j = 0; j < 8; ++j) {
+            msb = (nb & (0x80 >> j)) && 1;
+            msb ^= (crc >> 15);
+            crc <<= 1;
+            if (msb)
+                crc ^= l_code;
+        }
+        crc ^= 0xff00;
+        crc16tab[i] = crc;
+    }
+}
+
+
+void init_crc32tab(uint32_t l_code, uint32_t l_init)
+{
+    unsigned i, j, msb;
+    uint8_t nb;
+    uint32_t crc;
+
+    for (i = 0; i < 256; ++i) {
+        crc = l_init;
+        nb = i ^ 0xff;
+        for (j = 0; j < 8; ++j) {
+            msb = (nb & (0x80 >> j)) && 1;
+            msb ^= (crc >> 31);
+            crc <<= 1;
+            if (msb)
+                crc ^= l_code;
+        }
+        crc ^= 0xffffff00;
+        crc32tab[i] = crc;
+    }
+}
+
+
+uint8_t crc8(uint8_t l_crc, const void *lp_data, unsigned l_nb)
+{
+    const uint8_t* data = (const uint8_t*)lp_data;
+    while (l_nb--) {
+        l_crc = crc8tab[l_crc ^ *(data++)];
+    }
+    return (l_crc);
+}
+
+
+uint16_t crc16(uint16_t l_crc, const void *lp_data, unsigned l_nb)
+{
+    const uint8_t* data = (const uint8_t*)lp_data;
+    while (l_nb--) {
+        l_crc =
+            (l_crc << 8) ^ crc16tab[(l_crc >> 8) ^ *(data++)];
+    }
+    return (l_crc);
+}
+
+
+uint32_t crc32(uint32_t l_crc, const void *lp_data, unsigned l_nb)
+{
+    const uint8_t* data = (const uint8_t*)lp_data;
+    while (l_nb--) {
+        l_crc =
+            (l_crc << 8) ^ crc32tab[((l_crc >> 24) ^ *(data++)) & 0xff];
+    }
+    return (l_crc);
+}
diff --git a/src/crc.h b/src/crc.h
new file mode 100644
index 0000000..b1785a1
--- /dev/null
+++ b/src/crc.h
@@ -0,0 +1,59 @@
+/*
+   Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+   Queen in Right of Canada (Communications Research Center Canada)
+   */
+/*
+   This file is part of ODR-DabMux.
+
+   ODR-DabMux 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.
+
+   ODR-DabMux 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 ODR-DabMux.  If not, see <http://www.gnu.org/licenses/>.
+   */
+
+#ifndef _CRC
+#define _CRC
+
+#ifdef HAVE_CONFIG_H
+#   include "config.h"
+#endif
+
+#ifndef _WIN32
+  #include <stdint.h>
+#else
+  #include <winsock2.h>	// For types...
+  typedef BYTE uint8_t;
+  typedef WORD uint16_t;
+  typedef DWORD32 uint32_t;
+#endif
+
+
+#ifdef __cplusplus
+extern "C" { // }
+#endif
+
+void init_crc8tab(uint8_t l_code, uint8_t l_init);
+uint8_t crc8(uint8_t l_crc, const void *lp_data, unsigned l_nb);
+extern uint8_t crc8tab[];
+
+void init_crc16tab(uint16_t l_code, uint16_t l_init);
+uint16_t crc16(uint16_t l_crc, const void *lp_data, unsigned l_nb);
+extern uint16_t crc16tab[];
+
+void init_crc32tab(uint32_t l_code, uint32_t l_init);
+uint32_t crc32(uint32_t l_crc, const void *lp_data, unsigned l_nb);
+extern uint32_t crc32tab[];
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //_CRC
diff --git a/src/encryption.c b/src/encryption.c
new file mode 100644
index 0000000..f39fd28
--- /dev/null
+++ b/src/encryption.c
@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2014 Matthias P. Braendli
+ *
+ * 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 "encryption.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+
+int readkey(const char* keyfile, char* key)
+{
+    int fd = open(keyfile, O_RDONLY);
+    if (fd < 0)
+        return fd;
+    int ret = read(fd, key, CURVE_KEYLEN);
+    if (ret < 0)
+        return ret;
+    close(fd);
+
+    /* It needs to be zero-terminated */
+    key[CURVE_KEYLEN] = '\0';
+
+    return 0;
+}
+
diff --git a/src/encryption.h b/src/encryption.h
new file mode 100644
index 0000000..bfe1fc3
--- /dev/null
+++ b/src/encryption.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) 2014 Matthias P. Braendli
+ *
+ * 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.
+ * -------------------------------------------------------------------
+ */
+/* \brief Helper functions for the ZMQ encryption */
+
+#ifndef _ENCRYPTION_H_
+#define _ENCRYPTION_H_
+
+int readkey(const char* keyfile, char* key);
+
+#define CURVE_KEYLEN 40
+
+#endif
+
diff --git a/src/fec/LICENSE b/src/fec/LICENSE
new file mode 100644
index 0000000..5a883d3
--- /dev/null
+++ b/src/fec/LICENSE
@@ -0,0 +1,502 @@
+GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+(This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.)
+
+                            Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+                  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+                            NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    {description}
+    Copyright (C) {year} {fullname}
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library 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
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  {signature of Ty Coon}, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
diff --git a/src/fec/README.md b/src/fec/README.md
new file mode 100644
index 0000000..a44d28d
--- /dev/null
+++ b/src/fec/README.md
@@ -0,0 +1,12 @@
+FEC routines from KA9Q's libfec
+===============================
+
+This folder contains part of the libfec library by KA9Q. Only the
+char-sized Reed-Solomon encoder and decoder is here.
+
+The files have been copied from the libfec fork at
+https://github.com/Opendigitalradio/ka9q-fec
+
+Original code is at http://www.ka9q.net/code/fec/
+
+All files in this folder are licenced under the LGPL v2.1, please see LICENCE
diff --git a/src/fec/char.h b/src/fec/char.h
new file mode 100644
index 0000000..25efd65
--- /dev/null
+++ b/src/fec/char.h
@@ -0,0 +1,24 @@
+/* Stuff specific to the 8-bit symbol version of the general purpose RS codecs
+ *
+ * Copyright 2003, Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+typedef unsigned char data_t;
+
+#define MODNN(x) modnn(rs,x)
+
+#define MM (rs->mm)
+#define NN (rs->nn)
+#define ALPHA_TO (rs->alpha_to) 
+#define INDEX_OF (rs->index_of)
+#define GENPOLY (rs->genpoly)
+#define NROOTS (rs->nroots)
+#define FCR (rs->fcr)
+#define PRIM (rs->prim)
+#define IPRIM (rs->iprim)
+#define PAD (rs->pad)
+#define A0 (NN)
+
+
+
+
diff --git a/src/fec/decode_rs.h b/src/fec/decode_rs.h
new file mode 100644
index 0000000..c165cf3
--- /dev/null
+++ b/src/fec/decode_rs.h
@@ -0,0 +1,298 @@
+/* The guts of the Reed-Solomon decoder, meant to be #included
+ * into a function body with the following typedefs, macros and variables supplied
+ * according to the code parameters:
+
+ * data_t - a typedef for the data symbol
+ * data_t data[] - array of NN data and parity symbols to be corrected in place
+ * retval - an integer lvalue into which the decoder's return code is written
+ * NROOTS - the number of roots in the RS code generator polynomial,
+ *          which is the same as the number of parity symbols in a block.
+            Integer variable or literal.
+ * NN - the total number of symbols in a RS block. Integer variable or literal.
+ * PAD - the number of pad symbols in a block. Integer variable or literal.
+ * ALPHA_TO - The address of an array of NN elements to convert Galois field
+ *            elements in index (log) form to polynomial form. Read only.
+ * INDEX_OF - The address of an array of NN elements to convert Galois field
+ *            elements in polynomial form to index (log) form. Read only.
+ * MODNN - a function to reduce its argument modulo NN. May be inline or a macro.
+ * FCR - An integer literal or variable specifying the first consecutive root of the
+ *       Reed-Solomon generator polynomial. Integer variable or literal.
+ * PRIM - The primitive root of the generator poly. Integer variable or literal.
+ * DEBUG - If set to 1 or more, do various internal consistency checking. Leave this
+ *         undefined for production code
+
+ * The memset(), memmove(), and memcpy() functions are used. The appropriate header
+ * file declaring these functions (usually <string.h>) must be included by the calling
+ * program.
+ */
+
+
+#if !defined(NROOTS)
+#error "NROOTS not defined"
+#endif
+
+#if !defined(NN)
+#error "NN not defined"
+#endif
+
+#if !defined(PAD)
+#error "PAD not defined"
+#endif
+
+#if !defined(ALPHA_TO)
+#error "ALPHA_TO not defined"
+#endif
+
+#if !defined(INDEX_OF)
+#error "INDEX_OF not defined"
+#endif
+
+#if !defined(MODNN)
+#error "MODNN not defined"
+#endif
+
+#if !defined(FCR)
+#error "FCR not defined"
+#endif
+
+#if !defined(PRIM)
+#error "PRIM not defined"
+#endif
+
+#if !defined(NULL)
+#define NULL ((void *)0)
+#endif
+
+#undef MIN
+#define	MIN(a,b)	((a) < (b) ? (a) : (b))
+#undef A0
+#define A0 (NN)
+
+{
+  int deg_lambda, el, deg_omega;
+  int i, j, r,k;
+  data_t u,q,tmp,num1,num2,den,discr_r;
+  data_t lambda[NROOTS+1], s[NROOTS];	/* Err+Eras Locator poly
+					 * and syndrome poly */
+  data_t b[NROOTS+1], t[NROOTS+1], omega[NROOTS+1];
+  data_t root[NROOTS], reg[NROOTS+1], loc[NROOTS];
+  int syn_error, count;
+
+  /* form the syndromes; i.e., evaluate data(x) at roots of g(x) */
+  for(i=0;i<NROOTS;i++)
+    s[i] = data[0];
+
+  for(j=1;j<NN-PAD;j++){
+    for(i=0;i<NROOTS;i++){
+      if(s[i] == 0){
+	s[i] = data[j];
+      } else {
+	s[i] = data[j] ^ ALPHA_TO[MODNN(INDEX_OF[s[i]] + (FCR+i)*PRIM)];
+      }
+    }
+  }
+
+  /* Convert syndromes to index form, checking for nonzero condition */
+  syn_error = 0;
+  for(i=0;i<NROOTS;i++){
+    syn_error |= s[i];
+    s[i] = INDEX_OF[s[i]];
+  }
+
+  if (!syn_error) {
+    /* if syndrome is zero, data[] is a codeword and there are no
+     * errors to correct. So return data[] unmodified
+     */
+    count = 0;
+    goto finish;
+  }
+  memset(&lambda[1],0,NROOTS*sizeof(lambda[0]));
+  lambda[0] = 1;
+
+  if (no_eras > 0) {
+    /* Init lambda to be the erasure locator polynomial */
+    lambda[1] = ALPHA_TO[MODNN(PRIM*(NN-1-eras_pos[0]))];
+    for (i = 1; i < no_eras; i++) {
+      u = MODNN(PRIM*(NN-1-eras_pos[i]));
+      for (j = i+1; j > 0; j--) {
+	tmp = INDEX_OF[lambda[j - 1]];
+	if(tmp != A0)
+	  lambda[j] ^= ALPHA_TO[MODNN(u + tmp)];
+      }
+    }
+
+#if DEBUG >= 1
+    /* Test code that verifies the erasure locator polynomial just constructed
+       Needed only for decoder debugging. */
+    
+    /* find roots of the erasure location polynomial */
+    for(i=1;i<=no_eras;i++)
+      reg[i] = INDEX_OF[lambda[i]];
+
+    count = 0;
+    for (i = 1,k=IPRIM-1; i <= NN; i++,k = MODNN(k+IPRIM)) {
+      q = 1;
+      for (j = 1; j <= no_eras; j++)
+	if (reg[j] != A0) {
+	  reg[j] = MODNN(reg[j] + j);
+	  q ^= ALPHA_TO[reg[j]];
+	}
+      if (q != 0)
+	continue;
+      /* store root and error location number indices */
+      root[count] = i;
+      loc[count] = k;
+      count++;
+    }
+    if (count != no_eras) {
+      printf("count = %d no_eras = %d\n lambda(x) is WRONG\n",count,no_eras);
+      count = -1;
+      goto finish;
+    }
+#if DEBUG >= 2
+    printf("\n Erasure positions as determined by roots of Eras Loc Poly:\n");
+    for (i = 0; i < count; i++)
+      printf("%d ", loc[i]);
+    printf("\n");
+#endif
+#endif
+  }
+  for(i=0;i<NROOTS+1;i++)
+    b[i] = INDEX_OF[lambda[i]];
+  
+  /*
+   * Begin Berlekamp-Massey algorithm to determine error+erasure
+   * locator polynomial
+   */
+  r = no_eras;
+  el = no_eras;
+  while (++r <= NROOTS) {	/* r is the step number */
+    /* Compute discrepancy at the r-th step in poly-form */
+    discr_r = 0;
+    for (i = 0; i < r; i++){
+      if ((lambda[i] != 0) && (s[r-i-1] != A0)) {
+	discr_r ^= ALPHA_TO[MODNN(INDEX_OF[lambda[i]] + s[r-i-1])];
+      }
+    }
+    discr_r = INDEX_OF[discr_r];	/* Index form */
+    if (discr_r == A0) {
+      /* 2 lines below: B(x) <-- x*B(x) */
+      memmove(&b[1],b,NROOTS*sizeof(b[0]));
+      b[0] = A0;
+    } else {
+      /* 7 lines below: T(x) <-- lambda(x) - discr_r*x*b(x) */
+      t[0] = lambda[0];
+      for (i = 0 ; i < NROOTS; i++) {
+	if(b[i] != A0)
+	  t[i+1] = lambda[i+1] ^ ALPHA_TO[MODNN(discr_r + b[i])];
+	else
+	  t[i+1] = lambda[i+1];
+      }
+      if (2 * el <= r + no_eras - 1) {
+	el = r + no_eras - el;
+	/*
+	 * 2 lines below: B(x) <-- inv(discr_r) *
+	 * lambda(x)
+	 */
+	for (i = 0; i <= NROOTS; i++)
+	  b[i] = (lambda[i] == 0) ? A0 : MODNN(INDEX_OF[lambda[i]] - discr_r + NN);
+      } else {
+	/* 2 lines below: B(x) <-- x*B(x) */
+	memmove(&b[1],b,NROOTS*sizeof(b[0]));
+	b[0] = A0;
+      }
+      memcpy(lambda,t,(NROOTS+1)*sizeof(t[0]));
+    }
+  }
+
+  /* Convert lambda to index form and compute deg(lambda(x)) */
+  deg_lambda = 0;
+  for(i=0;i<NROOTS+1;i++){
+    lambda[i] = INDEX_OF[lambda[i]];
+    if(lambda[i] != A0)
+      deg_lambda = i;
+  }
+  /* Find roots of the error+erasure locator polynomial by Chien search */
+  memcpy(&reg[1],&lambda[1],NROOTS*sizeof(reg[0]));
+  count = 0;		/* Number of roots of lambda(x) */
+  for (i = 1,k=IPRIM-1; i <= NN; i++,k = MODNN(k+IPRIM)) {
+    q = 1; /* lambda[0] is always 0 */
+    for (j = deg_lambda; j > 0; j--){
+      if (reg[j] != A0) {
+	reg[j] = MODNN(reg[j] + j);
+	q ^= ALPHA_TO[reg[j]];
+      }
+    }
+    if (q != 0)
+      continue; /* Not a root */
+    /* store root (index-form) and error location number */
+#if DEBUG>=2
+    printf("count %d root %d loc %d\n",count,i,k);
+#endif
+    root[count] = i;
+    loc[count] = k;
+    /* If we've already found max possible roots,
+     * abort the search to save time
+     */
+    if(++count == deg_lambda)
+      break;
+  }
+  if (deg_lambda != count) {
+    /*
+     * deg(lambda) unequal to number of roots => uncorrectable
+     * error detected
+     */
+    count = -1;
+    goto finish;
+  }
+  /*
+   * Compute err+eras evaluator poly omega(x) = s(x)*lambda(x) (modulo
+   * x**NROOTS). in index form. Also find deg(omega).
+   */
+  deg_omega = deg_lambda-1;
+  for (i = 0; i <= deg_omega;i++){
+    tmp = 0;
+    for(j=i;j >= 0; j--){
+      if ((s[i - j] != A0) && (lambda[j] != A0))
+	tmp ^= ALPHA_TO[MODNN(s[i - j] + lambda[j])];
+    }
+    omega[i] = INDEX_OF[tmp];
+  }
+
+  /*
+   * Compute error values in poly-form. num1 = omega(inv(X(l))), num2 =
+   * inv(X(l))**(FCR-1) and den = lambda_pr(inv(X(l))) all in poly-form
+   */
+  for (j = count-1; j >=0; j--) {
+    num1 = 0;
+    for (i = deg_omega; i >= 0; i--) {
+      if (omega[i] != A0)
+	num1  ^= ALPHA_TO[MODNN(omega[i] + i * root[j])];
+    }
+    num2 = ALPHA_TO[MODNN(root[j] * (FCR - 1) + NN)];
+    den = 0;
+    
+    /* lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] */
+    for (i = MIN(deg_lambda,NROOTS-1) & ~1; i >= 0; i -=2) {
+      if(lambda[i+1] != A0)
+	den ^= ALPHA_TO[MODNN(lambda[i+1] + i * root[j])];
+    }
+#if DEBUG >= 1
+    if (den == 0) {
+      printf("\n ERROR: denominator = 0\n");
+      count = -1;
+      goto finish;
+    }
+#endif
+    /* Apply error to data */
+    if (num1 != 0 && loc[j] >= PAD) {
+      data[loc[j]-PAD] ^= ALPHA_TO[MODNN(INDEX_OF[num1] + INDEX_OF[num2] + NN - INDEX_OF[den])];
+    }
+  }
+ finish:
+  if(eras_pos != NULL){
+    for(i=0;i<count;i++)
+      eras_pos[i] = loc[i];
+  }
+  retval = count;
+}
diff --git a/src/fec/decode_rs_char.c b/src/fec/decode_rs_char.c
new file mode 100644
index 0000000..7105233
--- /dev/null
+++ b/src/fec/decode_rs_char.c
@@ -0,0 +1,22 @@
+/* General purpose Reed-Solomon decoder for 8-bit symbols or less
+ * Copyright 2003 Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+
+#ifdef DEBUG
+#include <stdio.h>
+#endif
+
+#include <string.h>
+
+#include "char.h"
+#include "rs-common.h"
+
+int decode_rs_char(void *p, data_t *data, int *eras_pos, int no_eras){
+  int retval;
+  struct rs *rs = (struct rs *)p;
+ 
+#include "decode_rs.h"
+  
+  return retval;
+}
diff --git a/src/fec/encode_rs.h b/src/fec/encode_rs.h
new file mode 100644
index 0000000..2c157f9
--- /dev/null
+++ b/src/fec/encode_rs.h
@@ -0,0 +1,58 @@
+/* The guts of the Reed-Solomon encoder, meant to be #included
+ * into a function body with the following typedefs, macros and variables supplied
+ * according to the code parameters:
+
+ * data_t - a typedef for the data symbol
+ * data_t data[] - array of NN-NROOTS-PAD and type data_t to be encoded
+ * data_t parity[] - an array of NROOTS and type data_t to be written with parity symbols
+ * NROOTS - the number of roots in the RS code generator polynomial,
+ *          which is the same as the number of parity symbols in a block.
+            Integer variable or literal.
+	    * 
+ * NN - the total number of symbols in a RS block. Integer variable or literal.
+ * PAD - the number of pad symbols in a block. Integer variable or literal.
+ * ALPHA_TO - The address of an array of NN elements to convert Galois field
+ *            elements in index (log) form to polynomial form. Read only.
+ * INDEX_OF - The address of an array of NN elements to convert Galois field
+ *            elements in polynomial form to index (log) form. Read only.
+ * MODNN - a function to reduce its argument modulo NN. May be inline or a macro.
+ * GENPOLY - an array of NROOTS+1 elements containing the generator polynomial in index form
+
+ * The memset() and memmove() functions are used. The appropriate header
+ * file declaring these functions (usually <string.h>) must be included by the calling
+ * program.
+
+ * Copyright 2004, Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+
+
+#undef A0
+#define A0 (NN) /* Special reserved value encoding zero in index form */
+
+{
+  int i, j;
+  data_t feedback;
+
+  memset(parity,0,NROOTS*sizeof(data_t));
+
+  for(i=0;i<NN-NROOTS-PAD;i++){
+    feedback = INDEX_OF[data[i] ^ parity[0]];
+    if(feedback != A0){      /* feedback term is non-zero */
+#ifdef UNNORMALIZED
+      /* This line is unnecessary when GENPOLY[NROOTS] is unity, as it must
+       * always be for the polynomials constructed by init_rs()
+       */
+      feedback = MODNN(NN - GENPOLY[NROOTS] + feedback);
+#endif
+      for(j=1;j<NROOTS;j++)
+	parity[j] ^= ALPHA_TO[MODNN(feedback + GENPOLY[NROOTS-j])];
+    }
+    /* Shift */
+    memmove(&parity[0],&parity[1],sizeof(data_t)*(NROOTS-1));
+    if(feedback != A0)
+      parity[NROOTS-1] = ALPHA_TO[MODNN(feedback + GENPOLY[0])];
+    else
+      parity[NROOTS-1] = 0;
+  }
+}
diff --git a/src/fec/encode_rs_char.c b/src/fec/encode_rs_char.c
new file mode 100644
index 0000000..a9bf2b8
--- /dev/null
+++ b/src/fec/encode_rs_char.c
@@ -0,0 +1,15 @@
+/* Reed-Solomon encoder
+ * Copyright 2002, Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+#include <string.h>
+
+#include "char.h"
+#include "rs-common.h"
+
+void encode_rs_char(void *p,data_t *data, data_t *parity){
+  struct rs *rs = (struct rs *)p;
+
+#include "encode_rs.h"
+
+}
diff --git a/src/fec/fec.h b/src/fec/fec.h
new file mode 100644
index 0000000..0d1bae1
--- /dev/null
+++ b/src/fec/fec.h
@@ -0,0 +1,30 @@
+/* Main header for reduced libfec.
+ *
+ * The FEC code in this folder is
+ * Copyright 2003 Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+
+#pragma once
+
+#include <stdlib.h>
+
+#include "char.h"
+#include "rs-common.h"
+
+/* Initialize a Reed-Solomon codec
+ * symsize = symbol size, bits
+ * gfpoly = Field generator polynomial coefficients
+ * fcr = first root of RS code generator polynomial, index form
+ * prim = primitive element to generate polynomial roots
+ * nroots = RS code generator polynomial degree (number of roots)
+ * pad = padding bytes at front of shortened block
+ */
+void *init_rs_char(int symsize,int gfpoly,int fcr,int prim,int nroots,int pad);
+
+int decode_rs_char(void *p, data_t *data, int *eras_pos, int no_eras);
+
+void encode_rs_char(void *p,data_t *data, data_t *parity);
+
+void free_rs_char(void *p);
+
diff --git a/src/fec/init_rs.h b/src/fec/init_rs.h
new file mode 100644
index 0000000..2b2ae98
--- /dev/null
+++ b/src/fec/init_rs.h
@@ -0,0 +1,106 @@
+/* Common code for intializing a Reed-Solomon control block (char or int symbols)
+ * Copyright 2004 Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+#undef NULL
+#define NULL ((void *)0)
+
+{
+  int i, j, sr,root,iprim;
+
+  rs = NULL;
+  /* Check parameter ranges */
+  if(symsize < 0 || symsize > 8*sizeof(data_t)){
+    goto done;
+  }
+
+  if(fcr < 0 || fcr >= (1<<symsize))
+    goto done;
+  if(prim <= 0 || prim >= (1<<symsize))
+    goto done;
+  if(nroots < 0 || nroots >= (1<<symsize))
+    goto done; /* Can't have more roots than symbol values! */
+  if(pad < 0 || pad >= ((1<<symsize) -1 - nroots))
+    goto done; /* Too much padding */
+
+  rs = (struct rs *)calloc(1,sizeof(struct rs));
+  if(rs == NULL)
+    goto done;
+
+  rs->mm = symsize;
+  rs->nn = (1<<symsize)-1;
+  rs->pad = pad;
+
+  rs->alpha_to = (data_t *)malloc(sizeof(data_t)*(rs->nn+1));
+  if(rs->alpha_to == NULL){
+    free(rs);
+    rs = NULL;
+    goto done;
+  }
+  rs->index_of = (data_t *)malloc(sizeof(data_t)*(rs->nn+1));
+  if(rs->index_of == NULL){
+    free(rs->alpha_to);
+    free(rs);
+    rs = NULL;
+    goto done;
+  }
+
+  /* Generate Galois field lookup tables */
+  rs->index_of[0] = A0; /* log(zero) = -inf */
+  rs->alpha_to[A0] = 0; /* alpha**-inf = 0 */
+  sr = 1;
+  for(i=0;i<rs->nn;i++){
+    rs->index_of[sr] = i;
+    rs->alpha_to[i] = sr;
+    sr <<= 1;
+    if(sr & (1<<symsize))
+      sr ^= gfpoly;
+    sr &= rs->nn;
+  }
+  if(sr != 1){
+    /* field generator polynomial is not primitive! */
+    free(rs->alpha_to);
+    free(rs->index_of);
+    free(rs);
+    rs = NULL;
+    goto done;
+  }
+
+  /* Form RS code generator polynomial from its roots */
+  rs->genpoly = (data_t *)malloc(sizeof(data_t)*(nroots+1));
+  if(rs->genpoly == NULL){
+    free(rs->alpha_to);
+    free(rs->index_of);
+    free(rs);
+    rs = NULL;
+    goto done;
+  }
+  rs->fcr = fcr;
+  rs->prim = prim;
+  rs->nroots = nroots;
+
+  /* Find prim-th root of 1, used in decoding */
+  for(iprim=1;(iprim % prim) != 0;iprim += rs->nn)
+    ;
+  rs->iprim = iprim / prim;
+
+  rs->genpoly[0] = 1;
+  for (i = 0,root=fcr*prim; i < nroots; i++,root += prim) {
+    rs->genpoly[i+1] = 1;
+
+    /* Multiply rs->genpoly[] by  @**(root + x) */
+    for (j = i; j > 0; j--){
+      if (rs->genpoly[j] != 0)
+	rs->genpoly[j] = rs->genpoly[j-1] ^ rs->alpha_to[modnn(rs,rs->index_of[rs->genpoly[j]] + root)];
+      else
+	rs->genpoly[j] = rs->genpoly[j-1];
+    }
+    /* rs->genpoly[0] can never be zero */
+    rs->genpoly[0] = rs->alpha_to[modnn(rs,rs->index_of[rs->genpoly[0]] + root)];
+  }
+  /* convert rs->genpoly[] to index form for quicker encoding */
+  for (i = 0; i <= nroots; i++)
+    rs->genpoly[i] = rs->index_of[rs->genpoly[i]];
+ done:;
+
+}
diff --git a/src/fec/init_rs_char.c b/src/fec/init_rs_char.c
new file mode 100644
index 0000000..a51099a
--- /dev/null
+++ b/src/fec/init_rs_char.c
@@ -0,0 +1,35 @@
+/* Initialize a RS codec
+ *
+ * Copyright 2002 Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+#include <stdlib.h>
+
+#include "char.h"
+#include "rs-common.h"
+
+void free_rs_char(void *p){
+  struct rs *rs = (struct rs *)p;
+
+  free(rs->alpha_to);
+  free(rs->index_of);
+  free(rs->genpoly);
+  free(rs);
+}
+
+/* Initialize a Reed-Solomon codec
+ * symsize = symbol size, bits
+ * gfpoly = Field generator polynomial coefficients
+ * fcr = first root of RS code generator polynomial, index form
+ * prim = primitive element to generate polynomial roots
+ * nroots = RS code generator polynomial degree (number of roots)
+ * pad = padding bytes at front of shortened block
+ */
+void *init_rs_char(int symsize,int gfpoly,int fcr,int prim,
+	int nroots,int pad){
+  struct rs *rs;
+
+#include "init_rs.h"
+
+  return rs;
+}
diff --git a/src/fec/rs-common.h b/src/fec/rs-common.h
new file mode 100644
index 0000000..e64eb39
--- /dev/null
+++ b/src/fec/rs-common.h
@@ -0,0 +1,26 @@
+/* Stuff common to all the general-purpose Reed-Solomon codecs
+ * Copyright 2004 Phil Karn, KA9Q
+ * May be used under the terms of the GNU Lesser General Public License (LGPL)
+ */
+
+/* Reed-Solomon codec control block */
+struct rs {
+  int mm;              /* Bits per symbol */
+  int nn;              /* Symbols per block (= (1<<mm)-1) */
+  data_t *alpha_to;     /* log lookup table */
+  data_t *index_of;     /* Antilog lookup table */
+  data_t *genpoly;      /* Generator polynomial */
+  int nroots;     /* Number of generator roots = number of parity symbols */
+  int fcr;        /* First consecutive root, index form */
+  int prim;       /* Primitive element, index form */
+  int iprim;      /* prim-th root of 1, index form */
+  int pad;        /* Padding bytes in shortened block */
+};
+
+static inline int modnn(struct rs *rs,int x){
+  while (x >= rs->nn) {
+    x -= rs->nn;
+    x = (x >> rs->mm) + (x & rs->nn);
+  }
+  return x;
+}
diff --git a/src/odr-sourcecompanion.cpp b/src/odr-sourcecompanion.cpp
new file mode 100644
index 0000000..726a738
--- /dev/null
+++ b/src/odr-sourcecompanion.cpp
@@ -0,0 +1,458 @@
+/* ------------------------------------------------------------------
+ * Copyright (C) 2017 Matthias P. Braendli
+ * Copyright (C) 2017 AVT GmbH - Fabien Vercasson
+ * Copyright (C) 2011 Martin Storsjo
+ *
+ * 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.
+ * -------------------------------------------------------------------
+ */
+
+/*! \mainpage Introduction
+ *  \file odr-sourcecompanion.cpp
+ *  \brief The main file for the audio encoder
+ */
+
+#include "config.h"
+#include "zmq.hpp"
+
+#include "AVTInput.h"
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+extern "C" {
+#include "encryption.h"
+#include "utils.h"
+}
+
+#include <vector>
+#include <deque>
+#include <chrono>
+#include <thread>
+#include <string>
+#include <getopt.h>
+#include <cstdio>
+#include <stdint.h>
+#include <time.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <fcntl.h>
+
+
+using namespace std;
+
+void usage(const char* name) {
+    fprintf(stderr,
+    "ODR-SourceCompanion %s\n"
+    "\nUsage:\n"
+    "%s [INPUT SELECTION] [OPTION...]\n",
+#if defined(GITVERSION)
+    GITVERSION
+#else
+    PACKAGE_VERSION
+#endif
+    , name);
+    fprintf(stderr,
+    "   For the AVT input:\n"
+#if HAVE_AVT
+    "   Using the option -I will switch to AVT encoder reception mode:\n"
+    "        * The internal encoder is not used any more, all input related options are ignored\n"
+    "        * The audio mode and bitrate will be sent to the encoder if option --control-uri\n"
+    "          and DAB+ specific options are set (-b -c -r --aaclc --sbr --ps)\n"
+    "        * PAD Data can be send to the encoder with the options --pad-port --pad --pad-fifo\n"             
+    "     -I, --input-uri=URI                      Input URI. (Supported: 'udp://...')\n"
+    "         --control-uri=URI                    Output control URI (Supported: 'udp://...')\n"
+    "         --timeout=ms                         Maximum frame waiting time, in milliseconds (def=2000)\n"  
+    "         --pad-port=port                      Port opened for PAD Frame requests (def=0 not opened)\n"
+    "         --jitter-size=nbFrames               Jitter buffer size, in 24ms frames (def=40)\n"
+#endif
+    "   Encoder parameters:\n"
+    "     -b, --bitrate={ 8, 16, ..., 192 }    Output bitrate in kbps. Must be a multiple of 8.\n"
+    "     -c, --channels={ 1, 2 }              Nb of input channels (default: 2).\n"
+    "     -r, --rate={ 32000, 48000 }          Input sample rate (default: 48000).\n"
+    "         --aaclc                          Force the usage of AAC-LC (no SBR, no PS)\n"
+    "         --sbr                            Force the usage of SBR\n"
+    "         --ps                             Force the usage of PS\n"
+    "   Output and pad parameters:\n"
+    "     -o, --output=URI                     Output ZMQ uri. (e.g. 'tcp://localhost:9000')\n"
+    "                                     -or- Output file uri. (e.g. 'file.dabp')\n"
+    "                                     -or- a single dash '-' to denote stdout\n"
+    "                                          If more than one ZMQ output is given, the socket\n"
+    "                                          will be connected to all listed endpoints.\n"
+    "     -k, --secret-key=FILE                Enable ZMQ encryption with the given secret key.\n"
+    "     -p, --pad=BYTES                      Set PAD size in bytes.\n"
+    "     -P, --pad-fifo=FILENAME              Set PAD data input fifo name"
+    "                                          (default: /tmp/pad.fifo).\n"
+    "     -l, --level                          Show peak audio level indication.\n"
+    "\n"
+    "Only the tcp:// zeromq transport has been tested until now,\n"
+    " but epgm:// and pgm:// are also accepted\n"
+    );
+
+}
+
+
+#define no_argument 0
+#define required_argument 1
+#define optional_argument 2
+
+int main(int argc, char *argv[])
+{
+    std::string avt_input_uri = "";
+    std::string avt_output_uri = "";
+    int32_t avt_timeout = 2000;
+    uint32_t avt_pad_port = 0;
+    size_t avt_jitterBufferSize = 40;
+    bool avt_mode = false;
+
+    std::vector<std::string> output_uris;
+
+    /* For MOT Slideshow and DLS insertion */
+    const char* pad_fifo = "/tmp/pad.fifo";
+    int pad_fd;
+    int padlen = 0;
+
+    /* Whether to show the 'sox'-like measurement */
+    int show_level = 0;
+
+    /* Data for ZMQ CURVE authentication */
+    char* keyfile = NULL;
+    char secretkey[CURVE_KEYLEN+1];
+
+    const struct option longopts[] = {
+        {"bitrate",                required_argument,  0, 'b'},
+        {"channels",               required_argument,  0, 'c'},
+        {"format",                 required_argument,  0, 'f'},
+        {"output",                 required_argument,  0, 'o'},
+        {"pad",                    required_argument,  0, 'p'},
+        {"pad-fifo",               required_argument,  0, 'P'},
+        {"rate",                   required_argument,  0, 'r'},
+        {"secret-key",             required_argument,  0, 'k'},
+        {"input-uri",              required_argument,  0, 'I'},
+        {"control-uri",            required_argument,  0,  6 },
+        {"timeout",                required_argument,  0,  7 },
+        {"pad-port",               required_argument,  0,  8 },
+        {"jitter-size",            required_argument,  0,  9 },
+        {"aaclc",                  no_argument,        0,  0 },
+        {"help",                   no_argument,        0, 'h'},
+        {"level",                  no_argument,        0, 'l'},
+        {"no-afterburner",         no_argument,        0, 'A'},
+        {"ps",                     no_argument,        0,  2 },
+        {"sbr",                    no_argument,        0,  1 },
+        {0, 0, 0, 0},
+    };
+
+    fprintf(stderr,
+            "Welcome to %s %s, compiled at %s, %s",
+            PACKAGE_NAME,
+#if defined(GITVERSION)
+            GITVERSION,
+#else
+            PACKAGE_VERSION,
+#endif
+            __DATE__, __TIME__);
+    fprintf(stderr, "\n");
+    fprintf(stderr, "  http://opendigitalradio.org\n\n");
+
+
+    if (argc < 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    bool allowSBR = false;
+    bool allowPS  = false;
+
+    int bitrate = 0;
+    int channels = 2;
+    int sample_rate = 48000;
+    char ch = 0;
+    int index;
+    while(ch != -1) {
+        ch = getopt_long(argc, argv, "aAhDlVb:c:f:i:j:k:L:o:r:d:p:P:s:v:w:I:C:Wg:C:", longopts, &index);
+        switch (ch) {
+        case 0: // AAC-LC
+            allowPS = false;
+            allowSBR = false;
+            break;
+        case 1: // SBR
+            allowPS = false;
+            allowSBR = true;
+            break;
+        case 2: // PS
+            allowPS = true;
+            allowSBR = true;
+            break;
+        case 'b':
+            bitrate = atoi(optarg);
+            break;
+        case 'c':
+            channels = atoi(optarg);
+            break;
+        case 'k':
+            keyfile = optarg;
+            break;
+        case 'l':
+            show_level = 1;
+            break;
+        case 'o':
+            output_uris.push_back(optarg);
+            break;
+        case 'p':
+            padlen = atoi(optarg);
+            break;
+        case 'P':
+            pad_fifo = optarg;
+            break;
+        case 'r':
+            sample_rate = atoi(optarg);
+            break;
+        case 'I':
+            avt_input_uri = optarg;
+            avt_mode = true;
+            fprintf(stderr, "AVT Encoder Mode\n");
+            break;
+        case 6:
+            avt_output_uri = optarg;
+            break;
+        case 7:
+            avt_timeout = atoi(optarg);
+            if (avt_timeout < 0) {
+                avt_timeout = 2000;
+            }
+            break;
+        case 8:
+            avt_pad_port = atoi(optarg);
+            break;
+        case 9:
+            avt_jitterBufferSize = atoi(optarg);
+            break;
+        case '?':
+        case 'h':
+            usage(argv[0]);
+            return 1;
+        }
+    }
+
+    if (padlen < 0) {
+        fprintf(stderr, "Invalid PAD length specified\n");
+        return 1;
+    }
+
+    zmq::context_t zmq_ctx;
+    zmq::socket_t zmq_sock(zmq_ctx, ZMQ_PUB);
+
+    if (not output_uris.empty()) {
+        for (auto uri : output_uris) {
+            if (keyfile) {
+                fprintf(stderr, "Enabling encryption\n");
+
+                int rc = readkey(keyfile, secretkey);
+                if (rc) {
+                    fprintf(stderr, "Error reading secret key\n");
+                    return 2;
+                }
+
+                const int yes = 1;
+                zmq_sock.setsockopt(ZMQ_CURVE_SERVER,
+                        &yes, sizeof(yes));
+
+                zmq_sock.setsockopt(ZMQ_CURVE_SECRETKEY,
+                        secretkey, CURVE_KEYLEN);
+            }
+            zmq_sock.connect(uri.c_str());
+        }
+    }
+    else {
+        fprintf(stderr, "No output URI defined\n");
+        return 1;
+    }
+
+    if (padlen != 0) {
+        int flags;
+        if (mkfifo(pad_fifo, S_IWUSR | S_IRUSR | S_IRGRP | S_IROTH) != 0) {
+            if (errno != EEXIST) {
+                fprintf(stderr, "Can't create pad file: %d!\n", errno);
+                return 1;
+            }
+        }
+        pad_fd = open(pad_fifo, O_RDONLY | O_NONBLOCK);
+        if (pad_fd == -1) {
+            fprintf(stderr, "Can't open pad file!\n");
+            return 1;
+        }
+        flags = fcntl(pad_fd, F_GETFL, 0);
+        if (fcntl(pad_fd, F_SETFL, flags | O_NONBLOCK)) {
+            fprintf(stderr, "Can't set non-blocking mode in pad file!\n");
+            return 1;
+        }
+    }
+
+    AVTInput avtinput(avt_input_uri, avt_output_uri, avt_pad_port, avt_jitterBufferSize);
+
+    if (avt_input_uri != "") {
+        if (avtinput.prepare() != 0) {
+            fprintf(stderr, "Fail to connect to AVT encoder in:'%s' out:'%s'\n", avt_input_uri.c_str(), avt_output_uri.c_str());
+            return 1;
+        }
+
+        // Audio parameters
+        if (avtinput.setDabPlusParameters(bitrate, channels, sample_rate, allowSBR, allowPS) != 0) {
+            fprintf(stderr, "Wrong audio parameters for AVT encoder\n");
+            return 1;
+        }
+    }
+    else {
+        fprintf(stderr, "No input defined\n");
+        return 1;
+    }
+
+    int outbuf_size;
+    std::vector<uint8_t> zmqframebuf;
+    std::vector<uint8_t> outbuf;
+
+    outbuf_size = bitrate/8*120;
+    outbuf.resize(24*120);
+    zmqframebuf.resize(ZMQ_HEADER_SIZE + 24*120);
+
+    if(outbuf_size % 5 != 0) {
+        fprintf(stderr, "Warning: (outbuf_size mod 5) = %d\n", outbuf_size % 5);
+    }
+
+    zmq_frame_header_t *zmq_frame_header = (zmq_frame_header_t*)&zmqframebuf[0];
+
+    unsigned char pad_buf[padlen + 1];
+
+    fprintf(stderr, "Starting encoding\n");
+
+    int retval = 0;
+    int send_error_count = 0;
+
+    int peak_left = 0;
+    int peak_right = 0;
+
+    int calls = 0; // for checking
+    ssize_t read_bytes = 0;
+    size_t numOutBytes = 0;
+    do {
+        read_bytes = 0;
+
+        // -------------- Read Data
+        memset(&outbuf[0], 0x00, outbuf_size);
+
+        const auto timeout_start = std::chrono::steady_clock::now();
+        const auto timeout_duration = std::chrono::milliseconds(avt_timeout);
+        int wait_ms = 1;
+
+        bool timedout = false;
+
+        while ( !timedout && numOutBytes == 0 )
+        {
+            // Fill the PAD Frame queue because multiple PAD frame requests
+            // can come for each DAB+ Frames (up to 6),
+            if (padlen != 0) {
+                int ret = 0;
+                do {
+                    ret = 0;
+                    if (!avtinput.padQueueFull()) {
+
+                        // Non blocking read of the pipe
+                        fd_set read_fd_set;
+                        FD_ZERO(&read_fd_set);
+                        FD_SET(pad_fd, &read_fd_set);
+                        struct timeval to = { 0, 0 };
+                        if( select(pad_fd+1, &read_fd_set, NULL, NULL, &to) > 0 ) {
+                            ret = read(pad_fd, pad_buf, padlen + 1);
+                            if (ret>0) {
+                                const int calculated_padlen = pad_buf[padlen];
+                                if (calculated_padlen > 0) {
+                                    avtinput.pushPADFrame(pad_buf + (padlen - calculated_padlen), calculated_padlen);
+                                }
+                            }
+                        }
+                    }
+                } while (ret!=0);
+            }
+
+            numOutBytes = avtinput.getNextFrame(outbuf);
+            if (numOutBytes == 0) {
+                const auto curTime = std::chrono::steady_clock::now();
+                const auto diff = curTime - timeout_start;
+                if (diff > timeout_duration) {
+                    fprintf(stderr, "timeout reached\n");
+                    timedout = true;
+                } else {
+                    usleep(wait_ms * 1000);
+                }
+            }
+        }
+        read_bytes = numOutBytes;
+
+        if (numOutBytes != 0) {
+            // ------------ ZeroMQ transmit
+            try {
+                zmq_frame_header->encoder = ZMQ_ENCODER_FDK;
+                zmq_frame_header->version = 1;
+                zmq_frame_header->datasize = numOutBytes;
+                zmq_frame_header->audiolevel_left = peak_left;
+                zmq_frame_header->audiolevel_right = peak_right;
+
+                assert(ZMQ_FRAME_SIZE(zmq_frame_header) <= zmqframebuf.size());
+
+                memcpy(ZMQ_FRAME_DATA(zmq_frame_header),
+                        &outbuf[0], numOutBytes);
+                zmq_sock.send(&zmqframebuf[0], ZMQ_FRAME_SIZE(zmq_frame_header),
+                        ZMQ_DONTWAIT);
+            }
+            catch (zmq::error_t& e) {
+                fprintf(stderr, "ZeroMQ send error !\n");
+                send_error_count ++;
+            }
+
+            if (send_error_count > 10)
+            {
+                fprintf(stderr, "ZeroMQ send failed ten times, aborting!\n");
+                retval = 4;
+                break;
+            }
+        }
+
+        if (numOutBytes != 0)
+        {
+            if (show_level) {
+                if (channels == 1) {
+                    fprintf(stderr, "\rIn: [%-6s]",
+                            level(1, MAX(peak_right, peak_left)));
+                }
+                else if (channels == 2) {
+                    fprintf(stderr, "\rIn: [%6s|%-6s]", 
+                            level(0, peak_left),
+                            level(1, peak_right));
+                }
+            }
+
+            peak_right = 0;
+            peak_left = 0;
+        }
+    } while (read_bytes > 0);
+
+    fprintf(stderr, "\n");
+
+    zmq_sock.close();
+
+    return retval;
+}
+
diff --git a/src/utils.c b/src/utils.c
new file mode 100644
index 0000000..24da427
--- /dev/null
+++ b/src/utils.c
@@ -0,0 +1,40 @@
+#include "utils.h"
+#include <unistd.h>
+#include <stdint.h>
+#include <math.h>
+
+/* Taken from sox */
+const char* level(int channel, int peak)
+{
+    static char const * const text[][2] = {
+        /* White: 2dB steps */
+        {"", ""}, {"-", "-"}, {"=", "="}, {"-=", "=-"},
+        {"==", "=="}, {"-==", "==-"}, {"===", "==="}, {"-===", "===-"},
+        {"====", "===="}, {"-====", "====-"}, {"=====", "====="},
+        {"-=====", "=====-"}, {"======", "======"},
+        /* Red: 1dB steps */
+        {"!=====", "=====!"},
+    };
+    int const red = 1, white = NUMOF(text) - red;
+
+    double linear = ((double)peak) / INT16_MAX;
+
+    int vu_dB = linear ? floor(2 * white + red + linear_to_dB(linear)) : 0;
+
+    int index = vu_dB < 2 * white ?
+        MAX(vu_dB / 2, 0) :
+        MIN(vu_dB - white, red + white - 1);
+
+    return text[index][channel];
+}
+
+size_t strlen_utf8(const char *s) {
+    size_t result = 0;
+
+    // ignore continuation bytes - only count single/leading bytes
+    while (*s)
+        if ((*s++ & 0xC0) != 0x80)
+            result++;
+
+    return result;
+}
diff --git a/src/utils.h b/src/utils.h
new file mode 100644
index 0000000..83b3e4d
--- /dev/null
+++ b/src/utils.h
@@ -0,0 +1,53 @@
+#ifndef UTILS_H_
+#define UTILS_H_
+
+#include <math.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+
+#define NUMOF(l) (sizeof(l) / sizeof(*l))
+
+#define linear_to_dB(x) (log10(x) * 20)
+
+/*! Calculate the little string containing a bargraph
+ * 'VU-meter' from the peak value measured
+ */
+const char* level(int channel, int peak);
+
+/*! This defines the on-wire representation of a ZMQ message header.
+ * It must be compatible with the definition in ODR-DabMux.
+ *
+ * The data follows right after this header */
+struct zmq_frame_header_t
+{
+    uint16_t version; // we support version=1 now
+    uint16_t encoder; // see ZMQ_ENCODER_XYZ
+
+    /* length of the 'data' field */
+    uint32_t datasize;
+
+    /* Audio level, peak, linear PCM */
+    int16_t audiolevel_left;
+    int16_t audiolevel_right;
+
+    /* Data follows this header */
+} __attribute__ ((packed));
+
+#define ZMQ_ENCODER_FDK 1
+#define ZMQ_ENCODER_TOOLAME 2
+
+#define ZMQ_HEADER_SIZE sizeof(struct zmq_frame_header_t)
+
+/* The expected frame size incl data of the given frame */
+#define ZMQ_FRAME_SIZE(f) (sizeof(struct zmq_frame_header_t) + f->datasize)
+
+#define ZMQ_FRAME_DATA(f) ( ((uint8_t*)f)+sizeof(struct zmq_frame_header_t) )
+
+
+size_t strlen_utf8(const char *s);
+
+#endif
+