/* Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty the Queen in Right of Canada (Communications Research Center Canada) Copyright (C) 2016 Matthias P. Braendli, matthias.braendli@mpb.li http://opendigitalradio.org */ /* This file is part of ODR-DabMod. ODR-DabMod 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-DabMod 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-DabMod. If not, see . */ #include "EtiReader.h" #include "Log.h" #include "PcDebug.h" #include "TimestampDecoder.h" #include #include #include #include #include #include using namespace std; enum ETI_READER_STATE { EtiReaderStateNbFrame, EtiReaderStateFrameSize, EtiReaderStateSync, EtiReaderStateFc, EtiReaderStateNst, EtiReaderStateEoh, EtiReaderStateFic, EtiReaderStateSubch, EtiReaderStateEof, EtiReaderStateTist, EtiReaderStatePad }; EtiReader::EtiReader( double& tist_offset_s, unsigned tist_delay_stages) : state(EtiReaderStateSync), myTimestampDecoder(tist_offset_s, tist_delay_stages), myCurrentFrame(0), eti_fc_valid(false) { rcs.enrol(&myTimestampDecoder); } std::shared_ptr& EtiSource::getFic() { return myFicSource; } unsigned EtiReader::getMode() { if (not eti_fc_valid) { throw std::runtime_error("Trying to access Mode before it is ready!"); } return eti_fc.MID; } unsigned EtiReader::getFp() { if (not eti_fc_valid) { throw std::runtime_error("Trying to access FP before it is ready!"); } return eti_fc.FP; } const std::vector > EtiReader::getSubchannels() const { return mySources; } int EtiReader::process(const Buffer* dataIn) { PDEBUG("EtiReader::process(dataIn: %p)\n", dataIn); PDEBUG(" state: %u\n", state); const unsigned char* in = reinterpret_cast(dataIn->getData()); size_t input_size = dataIn->getLength(); while (input_size > 0) { switch (state) { case EtiReaderStateNbFrame: if (input_size < 4) { return dataIn->getLength() - input_size; } nb_frames = *(uint32_t*)in; input_size -= 4; in += 4; state = EtiReaderStateFrameSize; PDEBUG("Nb frames: %i\n", nb_frames); break; case EtiReaderStateFrameSize: if (input_size < 2) { return dataIn->getLength() - input_size; } framesize = *(uint16_t*)in; input_size -= 2; in += 2; state = EtiReaderStateSync; PDEBUG("Framesize: %i\n", framesize); break; case EtiReaderStateSync: if (input_size < 4) { return dataIn->getLength() - input_size; } framesize = 6144; memcpy(&eti_sync, in, 4); input_size -= 4; framesize -= 4; in += 4; state = EtiReaderStateFc; PDEBUG("Sync.err: 0x%.2x\n", eti_sync.ERR); PDEBUG("Sync.fsync: 0x%.6x\n", eti_sync.FSYNC); break; case EtiReaderStateFc: if (input_size < 4) { return dataIn->getLength() - input_size; } memcpy(&eti_fc, in, 4); eti_fc_valid = true; input_size -= 4; framesize -= 4; in += 4; state = EtiReaderStateNst; PDEBUG("Fc.fct: 0x%.2x\n", eti_fc.FCT); PDEBUG("Fc.ficf: %u\n", eti_fc.FICF); PDEBUG("Fc.nst: %u\n", eti_fc.NST); PDEBUG("Fc.fp: 0x%x\n", eti_fc.FP); PDEBUG("Fc.mid: %u\n", eti_fc.MID); PDEBUG("Fc.fl: %u\n", eti_fc.getFrameLength()); if (!eti_fc.FICF) { throw std::runtime_error("FIC must be present to modulate!"); } if (not myFicSource) { unsigned ficf = eti_fc.FICF; unsigned mid = eti_fc.MID; myFicSource = make_shared(ficf, mid); } break; case EtiReaderStateNst: if (input_size < 4 * (size_t)eti_fc.NST) { return dataIn->getLength() - input_size; } if ((eti_stc.size() != eti_fc.NST) || (memcmp(&eti_stc[0], in, 4 * eti_fc.NST))) { PDEBUG("New stc!\n"); eti_stc.resize(eti_fc.NST); memcpy(&eti_stc[0], in, 4 * eti_fc.NST); mySources.clear(); for (unsigned i = 0; i < eti_fc.NST; ++i) { const auto tpl = eti_stc[i].TPL; mySources.push_back( make_shared( eti_stc[i].getStartAddress(), eti_stc[i].getSTL(), tpl)); PDEBUG("Sstc %u:\n", i); PDEBUG(" Stc%i.scid: %i\n", i, eti_stc[i].SCID); PDEBUG(" Stc%i.sad: %u\n", i, eti_stc[i].getStartAddress()); PDEBUG(" Stc%i.tpl: 0x%.2x\n", i, eti_stc[i].TPL); PDEBUG(" Stc%i.stl: %u\n", i, eti_stc[i].getSTL()); } } input_size -= 4 * eti_fc.NST; framesize -= 4 * eti_fc.NST; in += 4 * eti_fc.NST; state = EtiReaderStateEoh; break; case EtiReaderStateEoh: if (input_size < 4) { return dataIn->getLength() - input_size; } memcpy(&eti_eoh, in, 4); input_size -= 4; framesize -= 4; in += 4; state = EtiReaderStateFic; PDEBUG("Eoh.mnsc: 0x%.4x\n", eti_eoh.MNSC); PDEBUG("Eoh.crc: 0x%.4x\n", eti_eoh.CRC); break; case EtiReaderStateFic: if (eti_fc.MID == 3) { if (input_size < 128) { return dataIn->getLength() - input_size; } PDEBUG("Writting 128 bytes of FIC channel data\n"); Buffer fic = Buffer(128, in); myFicSource->loadFicData(fic); input_size -= 128; framesize -= 128; in += 128; } else { if (input_size < 96) { return dataIn->getLength() - input_size; } PDEBUG("Writting 96 bytes of FIC channel data\n"); Buffer fic = Buffer(96, in); myFicSource->loadFicData(fic); input_size -= 96; framesize -= 96; in += 96; } state = EtiReaderStateSubch; break; case EtiReaderStateSubch: for (size_t i = 0; i < eti_stc.size(); ++i) { unsigned size = mySources[i]->framesize(); PDEBUG("Writting %i bytes of subchannel data\n", size); Buffer subch = Buffer(size, in); mySources[i]->loadSubchannelData(subch); input_size -= size; framesize -= size; in += size; } state = EtiReaderStateEof; break; case EtiReaderStateEof: if (input_size < 4) { return dataIn->getLength() - input_size; } memcpy(&eti_eof, in, 4); input_size -= 4; framesize -= 4; in += 4; state = EtiReaderStateTist; PDEBUG("Eof.crc: %#.4x\n", eti_eof.CRC); PDEBUG("Eof.rfu: %#.4x\n", eti_eof.RFU); break; case EtiReaderStateTist: if (input_size < 4) { return dataIn->getLength() - input_size; } memcpy(&eti_tist, in, 4); input_size -= 4; framesize -= 4; in += 4; state = EtiReaderStatePad; PDEBUG("Tist: 0x%.6x\n", eti_tist.TIST); break; case EtiReaderStatePad: if (framesize > 0) { --input_size; --framesize; ++in; } else { state = EtiReaderStateSync; } break; default: // throw std::runtime_error("Invalid state!"); PDEBUG("Invalid state (%i)!", state); input_size = 0; } } // Update timestamps myTimestampDecoder.updateTimestampEti(eti_fc.FP & 0x3, eti_eoh.MNSC, getPPSOffset(), eti_fc.FCT); return dataIn->getLength() - input_size; } bool EtiReader::sourceContainsTimestamp() { return (ntohl(eti_tist.TIST) & 0xFFFFFF) != 0xFFFFFF; /* See ETS 300 799, Annex C.2.2 */ } uint32_t EtiReader::getPPSOffset() { if (!sourceContainsTimestamp()) { //fprintf(stderr, "****** SOURCE NO TS\n"); return 0.0; } uint32_t timestamp = ntohl(eti_tist.TIST) & 0xFFFFFF; //fprintf(stderr, "****** TIST 0x%x\n", timestamp); return timestamp; } unsigned EdiReader::getMode() { if (not m_fc_valid) { assert(false); throw std::runtime_error("Trying to access Mode before it is ready!"); } return m_fc.mid; } unsigned EdiReader::getFp() { if (not m_fc_valid) { throw std::runtime_error("Trying to access FP before it is ready!"); } return m_fc.fp; } const std::vector > EdiReader::getSubchannels() const { std::vector > sources; sources.resize(m_sources.size()); for (const auto s : m_sources) { if (s.first < sources.size()) { sources.at(s.first) = s.second; } else { throw std::runtime_error("Missing subchannel data in EDI source"); } } return sources; } bool EdiReader::isFrameReady() { return m_frameReady; } void EdiReader::clearFrame() { m_frameReady = false; m_proto_valid = false; m_fc_valid = false; m_fic.clear(); } void EdiReader::update_protocol( const std::string& proto, uint16_t major, uint16_t minor) { m_proto_valid = (proto == "DETI" and major == 0 and minor == 0); if (not m_proto_valid) { throw std::invalid_argument("Wrong EDI protocol"); } } void EdiReader::update_err(uint8_t err) { if (not m_proto_valid) { throw std::logic_error("Cannot update ERR before protocol"); } m_err = err; } void EdiReader::update_fc_data(const EdiDecoder::eti_fc_data& fc_data) { if (not m_proto_valid) { throw std::logic_error("Cannot update FC before protocol"); } m_fc_valid = false; m_fc = fc_data; if (not m_fc.ficf) { throw std::invalid_argument("FIC must be present"); } if (m_fc.mid > 4) { throw std::invalid_argument("Invalid MID"); } if (m_fc.fp > 7) { throw std::invalid_argument("Invalid FP"); } m_fc_valid = true; } void EdiReader::update_fic(const std::vector& fic) { if (not m_proto_valid) { throw std::logic_error("Cannot update FIC before protocol"); } m_fic = fic; } void EdiReader::update_edi_time( uint32_t utco, uint32_t seconds) { if (not m_proto_valid) { throw std::logic_error("Cannot update time before protocol"); } m_utco = utco; m_seconds = seconds; // TODO check validity m_time_valid = true; } void EdiReader::update_mnsc(uint16_t mnsc) { if (not m_proto_valid) { throw std::logic_error("Cannot update MNSC before protocol"); } m_mnsc = mnsc; } void EdiReader::update_rfu(uint16_t rfu) { if (not m_proto_valid) { throw std::logic_error("Cannot update RFU before protocol"); } m_rfu = rfu; } void EdiReader::add_subchannel(const EdiDecoder::eti_stc_data& stc) { if (not m_proto_valid) { throw std::logic_error("Cannot add subchannel before protocol"); } if (m_sources.count(stc.stream_index) == 0) { m_sources[stc.stream_index] = make_shared(stc.sad, stc.stl(), stc.tpl); } auto& source = m_sources[stc.stream_index]; if (source->framesize() != stc.mst.size()) { throw std::invalid_argument( "EDI: MST data length inconsistent with FIC"); } source->loadSubchannelData(stc.mst); if (m_sources.size() > 64) { throw std::invalid_argument("Too many subchannels"); } } void EdiReader::assemble() { etiLog.level(debug) << "Calling assemble"; if (not m_proto_valid) { throw std::logic_error("Cannot assemble EDI data before protocol"); } if (not m_fc_valid) { throw std::logic_error("Cannot assemble EDI data without FC"); } if (m_fic.empty()) { throw std::logic_error("Cannot assemble EDI data without FIC"); } // ETS 300 799 Clause 5.3.2, but we don't support not having // a FIC if ( (m_fc.mid == 3 and m_fic.size() != 32 * 4) or (m_fc.mid != 3 and m_fic.size() != 24 * 4) ) { stringstream ss; ss << "Invalid FIC length " << m_fic.size() << " for MID " << m_fc.mid; throw std::invalid_argument(ss.str()); } if (not myFicSource) { myFicSource = make_shared(m_fc.ficf, m_fc.mid); } myFicSource->loadFicData(m_fic); // Accept zero subchannels, because of an edge-case that can happen // during reconfiguration. See ETS 300 799 Clause 5.3.3 // TODO check time validity /* TODO timestamp myTimestampDecoder.updateTimestampEti( eti_fc.FP & 0x3, eti_eoh.MNSC, getPPSOffset(), eti_fc.FCT); */ m_frameReady = true; } EdiUdpInput::EdiUdpInput(EdiDecoder::ETIDecoder& decoder) : m_enabled(false), m_port(0), m_decoder(decoder) { } int EdiUdpInput::Open(const std::string& uri) { etiLog.level(info) << "Opening EDI :" << uri; int ret = 1; const std::regex re_udp("udp://:([0-9]+)"); std::smatch m; if (std::regex_match(uri, m, re_udp)) { m_port = std::stoi(m[1].str()); etiLog.level(info) << "EDI port :" << m_port; ret = m_sock.reinit(m_port, "0.0.0.0"); m_enabled = (ret == 0); } return ret; } void EdiUdpInput::rxPacket() { const size_t packsize = 8192; UdpPacket packet(packsize); int ret = m_sock.receive(packet); if (ret == 0) { const auto &buf = packet.getBuffer(); if (packet.getSize() == packsize) { fprintf(stderr, "Warning, possible UDP truncation\n"); } m_decoder.push_packet(buf); } else { fprintf(stderr, "Socket error: %s\n", inetErrMsg); } }