aboutsummaryrefslogtreecommitdiffstats
path: root/lib
diff options
context:
space:
mode:
Diffstat (limited to 'lib')
-rw-r--r--lib/ReedSolomon.cpp116
-rw-r--r--lib/ReedSolomon.h56
-rw-r--r--lib/Socket.cpp898
-rw-r--r--lib/Socket.h294
-rw-r--r--lib/ThreadsafeQueue.h176
-rw-r--r--lib/edi/PFT.cpp574
-rw-r--r--lib/edi/PFT.hpp166
-rw-r--r--lib/edi/STIDecoder.cpp191
-rw-r--r--lib/edi/STIDecoder.hpp122
-rw-r--r--lib/edi/STIWriter.cpp138
-rw-r--r--lib/edi/STIWriter.hpp84
-rw-r--r--lib/edi/buffer_unpack.hpp62
-rw-r--r--lib/edi/common.cpp300
-rw-r--r--lib/edi/common.hpp88
14 files changed, 3265 insertions, 0 deletions
diff --git a/lib/ReedSolomon.cpp b/lib/ReedSolomon.cpp
new file mode 100644
index 0000000..38d8ea8
--- /dev/null
+++ b/lib/ReedSolomon.cpp
@@ -0,0 +1,116 @@
+/*
+ Copyright (C) 2005, 2006, 2007, 2008, 2009 Her Majesty the Queen in Right
+ of Canada (Communications Research Center Canada)
+
+ Copyright (C) 2016
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ 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 "ReedSolomon.h"
+#include <vector>
+#include <algorithm>
+#include <stdexcept>
+#include <sstream>
+#include <stdio.h> // For galois.h ...
+#include <string.h> // For memcpy
+
+extern "C" {
+#include "fec/fec.h"
+}
+#include <assert.h>
+
+#define SYMSIZE 8
+
+
+ReedSolomon::ReedSolomon(int N, int K, bool reverse, int gfpoly, int firstRoot, int primElem)
+{
+ setReverse(reverse);
+
+ m_N = N;
+ m_K = K;
+
+ const int symsize = SYMSIZE;
+ const int nroots = N - K; // For EDI PFT, this must be 48
+ const int pad = ((1 << symsize) - 1) - N; // is 255-N
+
+ rsData = init_rs_char(symsize, gfpoly, firstRoot, primElem, nroots, pad);
+
+ if (rsData == nullptr) {
+ std::stringstream ss;
+ ss << "Invalid Reed-Solomon parameters! " <<
+ "N=" << N << " ; K=" << K << " ; pad=" << pad;
+ throw std::invalid_argument(ss.str());
+ }
+}
+
+
+ReedSolomon::~ReedSolomon()
+{
+ free_rs_char(rsData);
+}
+
+
+void ReedSolomon::setReverse(bool state)
+{
+ reverse = state;
+}
+
+
+int ReedSolomon::encode(void* data, void* fec, size_t size)
+{
+ uint8_t* input = reinterpret_cast<uint8_t*>(data);
+ uint8_t* output = reinterpret_cast<uint8_t*>(fec);
+ int ret = 0;
+
+ if (reverse) {
+ std::vector<uint8_t> buffer(m_N);
+
+ memcpy(&buffer[0], input, m_K);
+ memcpy(&buffer[m_K], output, m_N - m_K);
+
+ ret = decode_rs_char(rsData, &buffer[0], nullptr, 0);
+ if ((ret != 0) && (ret != -1)) {
+ memcpy(input, &buffer[0], m_K);
+ memcpy(output, &buffer[m_K], m_N - m_K);
+ }
+ }
+ else {
+ encode_rs_char(rsData, input, output);
+ }
+
+ return ret;
+}
+
+
+int ReedSolomon::encode(void* data, size_t size)
+{
+ uint8_t* input = reinterpret_cast<uint8_t*>(data);
+ int ret = 0;
+
+ if (reverse) {
+ ret = decode_rs_char(rsData, input, nullptr, 0);
+ }
+ else {
+ encode_rs_char(rsData, input, &input[m_K]);
+ }
+
+ return ret;
+}
diff --git a/lib/ReedSolomon.h b/lib/ReedSolomon.h
new file mode 100644
index 0000000..abcef62
--- /dev/null
+++ b/lib/ReedSolomon.h
@@ -0,0 +1,56 @@
+/*
+ Copyright (C) 2005, 2006, 2007, 2008, 2009 Her Majesty the Queen in Right
+ of Canada (Communications Research Center Canada)
+
+ Copyright (C) 2016
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ 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/>.
+ */
+
+#pragma once
+
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <stdlib.h>
+
+class ReedSolomon
+{
+public:
+ ReedSolomon(int N, int K,
+ bool reverse = false,
+ int gfpoly = 0x11d, int firstRoot = 0, int primElem = 1);
+ ReedSolomon(const ReedSolomon& other) = delete;
+ ReedSolomon operator=(const ReedSolomon& other) = delete;
+ ~ReedSolomon();
+
+ void setReverse(bool state);
+ int encode(void* data, void* fec, size_t size);
+ int encode(void* data, size_t size);
+
+private:
+ int m_N;
+ int m_K;
+
+ void* rsData;
+ bool reverse;
+};
+
diff --git a/lib/Socket.cpp b/lib/Socket.cpp
new file mode 100644
index 0000000..cd70a8e
--- /dev/null
+++ b/lib/Socket.cpp
@@ -0,0 +1,898 @@
+/*
+ Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+ Queen in Right of Canada (Communications Research Center Canada)
+
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://www.opendigitalradio.org
+ */
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <https://www.gnu.org/licenses/>.
+*/
+
+#include "Socket.h"
+
+#include <iostream>
+#include <cstdio>
+#include <cstring>
+#include <cerrno>
+#include <fcntl.h>
+#include <poll.h>
+
+namespace Socket {
+
+using namespace std;
+
+void InetAddress::resolveUdpDestination(const std::string& destination, int port)
+{
+ char service[NI_MAXSERV];
+ snprintf(service, NI_MAXSERV-1, "%d", port);
+
+ struct addrinfo hints;
+ memset(&hints, 0, sizeof(struct addrinfo));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_DGRAM; /* Datagram socket */
+ hints.ai_flags = 0;
+ hints.ai_protocol = 0;
+
+ struct addrinfo *result, *rp;
+ int s = getaddrinfo(destination.c_str(), service, &hints, &result);
+ if (s != 0) {
+ throw runtime_error(string("getaddrinfo failed: ") + gai_strerror(s));
+ }
+
+ for (rp = result; rp != nullptr; rp = rp->ai_next) {
+ // Take the first result
+ memcpy(&addr, rp->ai_addr, rp->ai_addrlen);
+ break;
+ }
+
+ freeaddrinfo(result);
+
+ if (rp == nullptr) {
+ throw runtime_error("Could not resolve");
+ }
+}
+
+UDPPacket::UDPPacket() { }
+
+UDPPacket::UDPPacket(size_t initSize) :
+ buffer(initSize)
+{ }
+
+
+UDPSocket::UDPSocket() :
+ m_sock(INVALID_SOCKET)
+{
+ reinit(0, "");
+}
+
+UDPSocket::UDPSocket(int port) :
+ m_sock(INVALID_SOCKET)
+{
+ reinit(port, "");
+}
+
+UDPSocket::UDPSocket(int port, const std::string& name) :
+ m_sock(INVALID_SOCKET)
+{
+ reinit(port, name);
+}
+
+
+void UDPSocket::setBlocking(bool block)
+{
+ int res = fcntl(m_sock, F_SETFL, block ? 0 : O_NONBLOCK);
+ if (res == -1) {
+ throw runtime_error(string("Can't change blocking state of socket: ") + strerror(errno));
+ }
+}
+
+void UDPSocket::reinit(int port)
+{
+ return reinit(port, "");
+}
+
+void UDPSocket::reinit(int port, const std::string& name)
+{
+ if (m_sock != INVALID_SOCKET) {
+ ::close(m_sock);
+ }
+
+ if (port == 0) {
+ // No need to bind to a given port, creating the
+ // socket is enough
+ m_sock = ::socket(AF_INET, SOCK_DGRAM, 0);
+ return;
+ }
+
+ char service[NI_MAXSERV];
+ snprintf(service, NI_MAXSERV-1, "%d", port);
+
+ struct addrinfo hints;
+ memset(&hints, 0, sizeof(struct addrinfo));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_DGRAM; /* Datagram socket */
+ hints.ai_flags = AI_PASSIVE; /* For wildcard IP address */
+ hints.ai_protocol = 0; /* Any protocol */
+ hints.ai_canonname = nullptr;
+ hints.ai_addr = nullptr;
+ hints.ai_next = nullptr;
+
+ struct addrinfo *result, *rp;
+ int s = getaddrinfo(name.empty() ? nullptr : name.c_str(),
+ port == 0 ? nullptr : service,
+ &hints, &result);
+ if (s != 0) {
+ throw runtime_error(string("getaddrinfo failed: ") + gai_strerror(s));
+ }
+
+ /* getaddrinfo() returns a list of address structures.
+ Try each address until we successfully bind(2).
+ If socket(2) (or bind(2)) fails, we (close the socket
+ and) try the next address. */
+ for (rp = result; rp != nullptr; rp = rp->ai_next) {
+ int sfd = ::socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
+ if (sfd == -1) {
+ continue;
+ }
+
+ if (::bind(sfd, rp->ai_addr, rp->ai_addrlen) == 0) {
+ m_sock = sfd;
+ break;
+ }
+
+ ::close(sfd);
+ }
+
+ freeaddrinfo(result);
+
+ if (rp == nullptr) {
+ throw runtime_error("Could not bind");
+ }
+}
+
+void UDPSocket::close()
+{
+ if (m_sock != INVALID_SOCKET) {
+ ::close(m_sock);
+ }
+
+ m_sock = INVALID_SOCKET;
+}
+
+UDPSocket::~UDPSocket()
+{
+ if (m_sock != INVALID_SOCKET) {
+ ::close(m_sock);
+ }
+}
+
+
+UDPPacket UDPSocket::receive(size_t max_size)
+{
+ UDPPacket packet(max_size);
+ socklen_t addrSize;
+ addrSize = sizeof(*packet.address.as_sockaddr());
+ ssize_t ret = recvfrom(m_sock,
+ packet.buffer.data(),
+ packet.buffer.size(),
+ 0,
+ packet.address.as_sockaddr(),
+ &addrSize);
+
+ if (ret == SOCKET_ERROR) {
+ packet.buffer.resize(0);
+
+ // This suppresses the -Wlogical-op warning
+#if EAGAIN == EWOULDBLOCK
+ if (errno == EAGAIN) {
+#else
+ if (errno == EAGAIN or errno == EWOULDBLOCK) {
+#endif
+ return 0;
+ }
+ throw runtime_error(string("Can't receive data: ") + strerror(errno));
+ }
+
+ packet.buffer.resize(ret);
+ return packet;
+}
+
+void UDPSocket::send(UDPPacket& packet)
+{
+ const int ret = sendto(m_sock, packet.buffer.data(), packet.buffer.size(), 0,
+ packet.address.as_sockaddr(), sizeof(*packet.address.as_sockaddr()));
+ if (ret == SOCKET_ERROR && errno != ECONNREFUSED) {
+ throw runtime_error(string("Can't send UDP packet: ") + strerror(errno));
+ }
+}
+
+
+void UDPSocket::send(const std::vector<uint8_t>& data, InetAddress destination)
+{
+ const int ret = sendto(m_sock, data.data(), data.size(), 0,
+ destination.as_sockaddr(), sizeof(*destination.as_sockaddr()));
+ if (ret == SOCKET_ERROR && errno != ECONNREFUSED) {
+ throw runtime_error(string("Can't send UDP packet: ") + strerror(errno));
+ }
+}
+
+void UDPSocket::joinGroup(const char* groupname, const char* if_addr)
+{
+ ip_mreqn group;
+ if ((group.imr_multiaddr.s_addr = inet_addr(groupname)) == INADDR_NONE) {
+ throw runtime_error("Cannot convert multicast group name");
+ }
+ if (!IN_MULTICAST(ntohl(group.imr_multiaddr.s_addr))) {
+ throw runtime_error("Group name is not a multicast address");
+ }
+
+ if (if_addr) {
+ group.imr_address.s_addr = inet_addr(if_addr);
+ }
+ else {
+ group.imr_address.s_addr = htons(INADDR_ANY);
+ }
+ group.imr_ifindex = 0;
+ if (setsockopt(m_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &group, sizeof(group))
+ == SOCKET_ERROR) {
+ throw runtime_error(string("Can't join multicast group") + strerror(errno));
+ }
+}
+
+void UDPSocket::setMulticastSource(const char* source_addr)
+{
+ struct in_addr addr;
+ if (inet_aton(source_addr, &addr) == 0) {
+ throw runtime_error(string("Can't parse source address") + strerror(errno));
+ }
+
+ if (setsockopt(m_sock, IPPROTO_IP, IP_MULTICAST_IF, &addr, sizeof(addr))
+ == SOCKET_ERROR) {
+ throw runtime_error(string("Can't set source address") + strerror(errno));
+ }
+}
+
+void UDPSocket::setMulticastTTL(int ttl)
+{
+ if (setsockopt(m_sock, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl))
+ == SOCKET_ERROR) {
+ throw runtime_error(string("Can't set multicast ttl") + strerror(errno));
+ }
+}
+
+UDPReceiver::UDPReceiver() { }
+
+UDPReceiver::~UDPReceiver() {
+ m_stop = true;
+ m_sock.close();
+ if (m_thread.joinable()) {
+ m_thread.join();
+ }
+}
+
+void UDPReceiver::start(int port, const string& bindto, const string& mcastaddr, size_t max_packets_queued) {
+ m_port = port;
+ m_bindto = bindto;
+ m_mcastaddr = mcastaddr;
+ m_max_packets_queued = max_packets_queued;
+ m_thread = std::thread(&UDPReceiver::m_run, this);
+}
+
+std::vector<uint8_t> UDPReceiver::get_packet_buffer()
+{
+ if (m_stop) {
+ throw runtime_error("UDP Receiver not running");
+ }
+
+ UDPPacket p;
+ m_packets.wait_and_pop(p);
+
+ return p.buffer;
+}
+
+void UDPReceiver::m_run()
+{
+ // Ensure that stop is set to true in case of exception or return
+ struct SetStopOnDestruct {
+ SetStopOnDestruct(atomic<bool>& stop) : m_stop(stop) {}
+ ~SetStopOnDestruct() { m_stop = true; }
+ private: atomic<bool>& m_stop;
+ } autoSetStop(m_stop);
+
+ if (IN_MULTICAST(ntohl(inet_addr(m_mcastaddr.c_str())))) {
+ m_sock.reinit(m_port, m_mcastaddr);
+ m_sock.setMulticastSource(m_bindto.c_str());
+ m_sock.joinGroup(m_mcastaddr.c_str(), m_bindto.c_str());
+ }
+ else {
+ m_sock.reinit(m_port, m_bindto);
+ }
+
+ while (not m_stop) {
+ constexpr size_t packsize = 8192;
+ try {
+ auto packet = m_sock.receive(packsize);
+ if (packet.buffer.size() == packsize) {
+ // TODO replace fprintf
+ fprintf(stderr, "Warning, possible UDP truncation\n");
+ }
+
+ // If this blocks, the UDP socket will lose incoming packets
+ m_packets.push_wait_if_full(packet, m_max_packets_queued);
+ }
+ catch (const std::runtime_error& e) {
+ // TODO replace fprintf
+ // TODO handle intr
+ fprintf(stderr, "Socket error: %s\n", e.what());
+ m_stop = true;
+ }
+ }
+}
+
+
+TCPSocket::TCPSocket()
+{
+}
+
+TCPSocket::~TCPSocket()
+{
+ if (m_sock != -1) {
+ ::close(m_sock);
+ }
+}
+
+TCPSocket::TCPSocket(TCPSocket&& other) :
+ m_sock(other.m_sock),
+ m_remote_address(move(other.m_remote_address))
+{
+ if (other.m_sock != -1) {
+ other.m_sock = -1;
+ }
+}
+
+TCPSocket& TCPSocket::operator=(TCPSocket&& other)
+{
+ swap(m_remote_address, other.m_remote_address);
+
+ m_sock = other.m_sock;
+ if (other.m_sock != -1) {
+ other.m_sock = -1;
+ }
+
+ return *this;
+}
+
+bool TCPSocket::valid() const
+{
+ return m_sock != -1;
+}
+
+void TCPSocket::connect(const std::string& hostname, int port)
+{
+ if (m_sock != INVALID_SOCKET) {
+ throw std::logic_error("You may only connect an invalid TCPSocket");
+ }
+
+ char service[NI_MAXSERV];
+ snprintf(service, NI_MAXSERV-1, "%d", port);
+
+ /* Obtain address(es) matching host/port */
+ struct addrinfo hints;
+ memset(&hints, 0, sizeof(struct addrinfo));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_STREAM;
+ hints.ai_flags = 0;
+ hints.ai_protocol = 0;
+
+ struct addrinfo *result, *rp;
+ int s = getaddrinfo(hostname.c_str(), service, &hints, &result);
+ if (s != 0) {
+ throw runtime_error(string("getaddrinfo failed: ") + gai_strerror(s));
+ }
+
+ /* getaddrinfo() returns a list of address structures.
+ Try each address until we successfully connect(2).
+ If socket(2) (or connect(2)) fails, we (close the socket
+ and) try the next address. */
+
+ for (rp = result; rp != nullptr; rp = rp->ai_next) {
+ int sfd = ::socket(rp->ai_family, rp->ai_socktype,
+ rp->ai_protocol);
+ if (sfd == -1)
+ continue;
+
+ int ret = ::connect(sfd, rp->ai_addr, rp->ai_addrlen);
+ if (ret != -1 or (ret == -1 and errno == EINPROGRESS)) {
+ // As the TCPClient could set the socket to nonblocking, we
+ // must handle EINPROGRESS here
+ m_sock = sfd;
+ break;
+ }
+
+ ::close(sfd);
+ }
+
+ if (m_sock != INVALID_SOCKET) {
+#if defined(HAVE_SO_NOSIGPIPE)
+ int val = 1;
+ if (setsockopt(m_sock, SOL_SOCKET, SO_NOSIGPIPE, &val, sizeof(val))
+ == SOCKET_ERROR) {
+ throw std::runtime_error("Can't set SO_NOSIGPIPE");
+ }
+#endif
+ }
+
+ freeaddrinfo(result); /* No longer needed */
+
+ if (rp == nullptr) {
+ throw runtime_error("Could not connect");
+ }
+
+}
+
+void TCPSocket::listen(int port, const string& name)
+{
+ if (m_sock != INVALID_SOCKET) {
+ throw std::logic_error("You may only listen with an invalid TCPSocket");
+ }
+
+ char service[NI_MAXSERV];
+ snprintf(service, NI_MAXSERV-1, "%d", port);
+
+ struct addrinfo hints;
+ memset(&hints, 0, sizeof(struct addrinfo));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_STREAM;
+ hints.ai_flags = AI_PASSIVE; /* For wildcard IP address */
+ hints.ai_protocol = 0;
+ hints.ai_canonname = nullptr;
+ hints.ai_addr = nullptr;
+ hints.ai_next = nullptr;
+
+ struct addrinfo *result, *rp;
+ int s = getaddrinfo(name.empty() ? nullptr : name.c_str(), service, &hints, &result);
+ if (s != 0) {
+ throw runtime_error(string("getaddrinfo failed: ") + gai_strerror(s));
+ }
+
+ /* getaddrinfo() returns a list of address structures.
+ Try each address until we successfully bind(2).
+ If socket(2) (or bind(2)) fails, we (close the socket
+ and) try the next address. */
+ for (rp = result; rp != nullptr; rp = rp->ai_next) {
+ int sfd = ::socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
+ if (sfd == -1) {
+ continue;
+ }
+
+ if (::bind(sfd, rp->ai_addr, rp->ai_addrlen) == 0) {
+ m_sock = sfd;
+ break;
+ }
+
+ ::close(sfd);
+ }
+
+ freeaddrinfo(result);
+
+ if (m_sock != INVALID_SOCKET) {
+#if defined(HAVE_SO_NOSIGPIPE)
+ int val = 1;
+ if (setsockopt(m_sock, SOL_SOCKET, SO_NOSIGPIPE,
+ &val, sizeof(val)) < 0) {
+ throw std::runtime_error("Can't set SO_NOSIGPIPE");
+ }
+#endif
+
+ int ret = ::listen(m_sock, 0);
+ if (ret == -1) {
+ throw std::runtime_error(string("Could not listen: ") + strerror(errno));
+ }
+ }
+
+ if (rp == nullptr) {
+ throw runtime_error("Could not bind");
+ }
+}
+
+void TCPSocket::close()
+{
+ ::close(m_sock);
+ m_sock = -1;
+}
+
+TCPSocket TCPSocket::accept(int timeout_ms)
+{
+ if (timeout_ms == 0) {
+ InetAddress remote_addr;
+ socklen_t client_len = sizeof(remote_addr.addr);
+ int sockfd = ::accept(m_sock, remote_addr.as_sockaddr(), &client_len);
+ TCPSocket s(sockfd, remote_addr);
+ return s;
+ }
+ else {
+ struct pollfd fds[1];
+ fds[0].fd = m_sock;
+ fds[0].events = POLLIN;
+
+ int retval = poll(fds, 1, timeout_ms);
+
+ if (retval == -1) {
+ std::string errstr(strerror(errno));
+ throw std::runtime_error("TCP Socket accept error: " + errstr);
+ }
+ else if (retval > 0) {
+ InetAddress remote_addr;
+ socklen_t client_len = sizeof(remote_addr.addr);
+ int sockfd = ::accept(m_sock, remote_addr.as_sockaddr(), &client_len);
+ TCPSocket s(sockfd, remote_addr);
+ return s;
+ }
+ else {
+ TCPSocket s(-1);
+ return s;
+ }
+ }
+}
+
+ssize_t TCPSocket::sendall(const void *buffer, size_t buflen)
+{
+ uint8_t *buf = (uint8_t*)buffer;
+ while (buflen > 0) {
+ /* On Linux, the MSG_NOSIGNAL flag ensures that the process
+ * would not receive a SIGPIPE and die.
+ * Other systems have SO_NOSIGPIPE set on the socket for the
+ * same effect. */
+#if defined(HAVE_MSG_NOSIGNAL)
+ const int flags = MSG_NOSIGNAL;
+#else
+ const int flags = 0;
+#endif
+ ssize_t sent = ::send(m_sock, buf, buflen, flags);
+ if (sent < 0) {
+ return -1;
+ }
+ else {
+ buf += sent;
+ buflen -= sent;
+ }
+ }
+ return buflen;
+}
+
+ssize_t TCPSocket::send(const void* data, size_t size, int timeout_ms)
+{
+ if (timeout_ms) {
+ struct pollfd fds[1];
+ fds[0].fd = m_sock;
+ fds[0].events = POLLOUT;
+
+ const int retval = poll(fds, 1, timeout_ms);
+
+ if (retval == -1) {
+ throw std::runtime_error(string("TCP Socket send error on poll(): ") + strerror(errno));
+ }
+ else if (retval == 0) {
+ // Timed out
+ return 0;
+ }
+ }
+
+ /* On Linux, the MSG_NOSIGNAL flag ensures that the process would not
+ * receive a SIGPIPE and die.
+ * Other systems have SO_NOSIGPIPE set on the socket for the same effect. */
+#if defined(HAVE_MSG_NOSIGNAL)
+ const int flags = MSG_NOSIGNAL;
+#else
+ const int flags = 0;
+#endif
+ const ssize_t ret = ::send(m_sock, (const char*)data, size, flags);
+
+ if (ret == SOCKET_ERROR) {
+ throw std::runtime_error(string("TCP Socket send error: ") + strerror(errno));
+ }
+ return ret;
+}
+
+ssize_t TCPSocket::recv(void *buffer, size_t length, int flags)
+{
+ ssize_t ret = ::recv(m_sock, buffer, length, flags);
+ if (ret == -1) {
+ std::string errstr(strerror(errno));
+ throw std::runtime_error("TCP receive error: " + errstr);
+ }
+ return ret;
+}
+
+ssize_t TCPSocket::recv(void *buffer, size_t length, int flags, int timeout_ms)
+{
+ struct pollfd fds[1];
+ fds[0].fd = m_sock;
+ fds[0].events = POLLIN;
+
+ int retval = poll(fds, 1, timeout_ms);
+
+ if (retval == -1 and errno == EINTR) {
+ throw Interrupted();
+ }
+ else if (retval == -1) {
+ std::string errstr(strerror(errno));
+ throw std::runtime_error("TCP receive with poll() error: " + errstr);
+ }
+ else if (retval > 0 and (fds[0].revents | POLLIN)) {
+ ssize_t ret = ::recv(m_sock, buffer, length, flags);
+ if (ret == -1) {
+ if (errno == ECONNREFUSED) {
+ return 0;
+ }
+ std::string errstr(strerror(errno));
+ throw std::runtime_error("TCP receive after poll() error: " + errstr);
+ }
+ return ret;
+ }
+ else {
+ throw Timeout();
+ }
+}
+
+TCPSocket::TCPSocket(int sockfd) :
+ m_sock(sockfd),
+ m_remote_address()
+{ }
+
+TCPSocket::TCPSocket(int sockfd, InetAddress remote_address) :
+ m_sock(sockfd),
+ m_remote_address(remote_address)
+{ }
+
+void TCPClient::connect(const std::string& hostname, int port)
+{
+ m_hostname = hostname;
+ m_port = port;
+ reconnect();
+}
+
+ssize_t TCPClient::recv(void *buffer, size_t length, int flags, int timeout_ms)
+{
+ try {
+ ssize_t ret = m_sock.recv(buffer, length, flags, timeout_ms);
+
+ if (ret == 0) {
+ m_sock.close();
+
+ TCPSocket newsock;
+ m_sock = std::move(newsock);
+ reconnect();
+ }
+
+ return ret;
+ }
+ catch (const TCPSocket::Interrupted&) {
+ return -1;
+ }
+ catch (const TCPSocket::Timeout&) {
+ return 0;
+ }
+
+ return 0;
+}
+
+void TCPClient::reconnect()
+{
+ int flags = fcntl(m_sock.m_sock, F_GETFL);
+ if (fcntl(m_sock.m_sock, F_SETFL, flags | O_NONBLOCK) == -1) {
+ std::string errstr(strerror(errno));
+ throw std::runtime_error("TCP: Could not set O_NONBLOCK: " + errstr);
+ }
+
+ m_sock.connect(m_hostname, m_port);
+}
+
+TCPConnection::TCPConnection(TCPSocket&& sock) :
+ queue(),
+ m_running(true),
+ m_sender_thread(),
+ m_sock(move(sock))
+{
+#if MISSING_OWN_ADDR
+ auto own_addr = m_sock.getOwnAddress();
+ auto addr = m_sock.getRemoteAddress();
+ etiLog.level(debug) << "New TCP Connection on port " <<
+ own_addr.getPort() << " from " <<
+ addr.getHostAddress() << ":" << addr.getPort();
+#endif
+ m_sender_thread = std::thread(&TCPConnection::process, this);
+}
+
+TCPConnection::~TCPConnection()
+{
+ m_running = false;
+ vector<uint8_t> termination_marker;
+ queue.push(termination_marker);
+ m_sender_thread.join();
+}
+
+void TCPConnection::process()
+{
+ while (m_running) {
+ vector<uint8_t> data;
+ queue.wait_and_pop(data);
+
+ if (data.empty()) {
+ // empty vector is the termination marker
+ m_running = false;
+ break;
+ }
+
+ try {
+ ssize_t remaining = data.size();
+ const uint8_t *buf = reinterpret_cast<const uint8_t*>(data.data());
+ const int timeout_ms = 10; // Less than one ETI frame
+
+ while (m_running and remaining > 0) {
+ const ssize_t sent = m_sock.send(buf, remaining, timeout_ms);
+ if (sent < 0 or sent > remaining) {
+ throw std::logic_error("Invalid TCPSocket::send() return value");
+ }
+ remaining -= sent;
+ buf += sent;
+ }
+ }
+ catch (const std::runtime_error& e) {
+ m_running = false;
+ }
+ }
+
+#if MISSING_OWN_ADDR
+ auto own_addr = m_sock.getOwnAddress();
+ auto addr = m_sock.getRemoteAddress();
+ etiLog.level(debug) << "Dropping TCP Connection on port " <<
+ own_addr.getPort() << " from " <<
+ addr.getHostAddress() << ":" << addr.getPort();
+#endif
+}
+
+
+TCPDataDispatcher::TCPDataDispatcher(size_t max_queue_size) :
+ m_max_queue_size(max_queue_size)
+{
+}
+
+TCPDataDispatcher::~TCPDataDispatcher()
+{
+ m_running = false;
+ m_connections.clear();
+ m_listener_socket.close();
+ if (m_listener_thread.joinable()) {
+ m_listener_thread.join();
+ }
+}
+
+void TCPDataDispatcher::start(int port, const string& address)
+{
+ m_listener_socket.listen(port, address);
+
+ m_running = true;
+ m_listener_thread = std::thread(&TCPDataDispatcher::process, this);
+}
+
+void TCPDataDispatcher::write(const vector<uint8_t>& data)
+{
+ if (not m_running) {
+ throw runtime_error(m_exception_data);
+ }
+
+ for (auto& connection : m_connections) {
+ connection.queue.push(data);
+ }
+
+ m_connections.remove_if(
+ [&](const TCPConnection& conn){ return conn.queue.size() > m_max_queue_size; });
+}
+
+void TCPDataDispatcher::process()
+{
+ try {
+ const int timeout_ms = 1000;
+
+ while (m_running) {
+ // Add a new TCPConnection to the list, constructing it from the client socket
+ auto sock = m_listener_socket.accept(timeout_ms);
+ if (sock.valid()) {
+ m_connections.emplace(m_connections.begin(), move(sock));
+ }
+ }
+ }
+ catch (const std::runtime_error& e) {
+ m_exception_data = string("TCPDataDispatcher error: ") + e.what();
+ m_running = false;
+ }
+}
+
+TCPReceiveServer::TCPReceiveServer(size_t blocksize) :
+ m_blocksize(blocksize)
+{
+}
+
+void TCPReceiveServer::start(int listen_port, const std::string& address)
+{
+ m_listener_socket.listen(listen_port, address);
+
+ m_running = true;
+ m_listener_thread = std::thread(&TCPReceiveServer::process, this);
+}
+
+TCPReceiveServer::~TCPReceiveServer()
+{
+ m_running = false;
+ if (m_listener_thread.joinable()) {
+ m_listener_thread.join();
+ }
+}
+
+vector<uint8_t> TCPReceiveServer::receive()
+{
+ vector<uint8_t> buffer;
+ m_queue.try_pop(buffer);
+
+ // we can ignore try_pop()'s return value, because
+ // if it is unsuccessful the buffer is not touched.
+ return buffer;
+}
+
+void TCPReceiveServer::process()
+{
+ constexpr int timeout_ms = 1000;
+ constexpr int disconnect_timeout_ms = 10000;
+ constexpr int max_num_timeouts = disconnect_timeout_ms / timeout_ms;
+
+ while (m_running) {
+ auto sock = m_listener_socket.accept(timeout_ms);
+
+ int num_timeouts = 0;
+
+ while (m_running and sock.valid()) {
+ try {
+ vector<uint8_t> buf(m_blocksize);
+ ssize_t r = sock.recv(buf.data(), buf.size(), 0, timeout_ms);
+ if (r < 0) {
+ throw logic_error("Invalid recv return value");
+ }
+ else if (r == 0) {
+ sock.close();
+ break;
+ }
+ else {
+ buf.resize(r);
+ m_queue.push(move(buf));
+ }
+ }
+ catch (const TCPSocket::Interrupted&) {
+ break;
+ }
+ catch (const TCPSocket::Timeout&) {
+ num_timeouts++;
+ }
+
+ if (num_timeouts > max_num_timeouts) {
+ sock.close();
+ }
+ }
+ }
+}
+
+}
diff --git a/lib/Socket.h b/lib/Socket.h
new file mode 100644
index 0000000..8bb7fe1
--- /dev/null
+++ b/lib/Socket.h
@@ -0,0 +1,294 @@
+/*
+ Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008, 2009 Her Majesty the
+ Queen in Right of Canada (Communications Research Center Canada)
+
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://www.opendigitalradio.org
+ */
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <https://www.gnu.org/licenses/>.
+*/
+
+#pragma once
+
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "ThreadsafeQueue.h"
+#include <cstdlib>
+#include <iostream>
+#include <vector>
+#include <atomic>
+#include <thread>
+#include <list>
+
+#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
+
+
+namespace Socket {
+
+struct InetAddress {
+ struct sockaddr_storage addr;
+
+ struct sockaddr *as_sockaddr() { return reinterpret_cast<sockaddr*>(&addr); };
+
+ void resolveUdpDestination(const std::string& destination, int port);
+};
+
+/** This class represents a UDP packet.
+ *
+ * A UDP packet contains a payload (sequence of bytes) and an address. For
+ * outgoing packets, the address is the destination address. For incoming
+ * packets, the address tells the user from what source the packet arrived from.
+ */
+class UDPPacket
+{
+ public:
+ UDPPacket();
+ UDPPacket(size_t initSize);
+
+ std::vector<uint8_t> buffer;
+ InetAddress address;
+};
+
+/**
+ * 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.
+ */
+class UDPSocket
+{
+ public:
+ /** Create a new socket that will not be bound to any port. To be used
+ * for data output.
+ */
+ UDPSocket();
+ /** Create a new socket.
+ * @param port The port number on which the socket will be bound
+ */
+ UDPSocket(int port);
+ /** Create a new socket.
+ * @param port The port number on which the socket will be bound
+ * @param name The IP address on which the socket will be bound.
+ * It is used to bind the socket on a specific interface if
+ * the computer have many NICs.
+ */
+ UDPSocket(int port, const std::string& name);
+ ~UDPSocket();
+ UDPSocket(const UDPSocket& other) = delete;
+ const UDPSocket& operator=(const UDPSocket& other) = delete;
+
+ /** Close the already open socket, and create a new one. Throws a runtime_error on error. */
+ void reinit(int port);
+ void reinit(int port, const std::string& name);
+
+ void close(void);
+ void send(UDPPacket& packet);
+ void send(const std::vector<uint8_t>& data, InetAddress destination);
+ UDPPacket receive(size_t max_size);
+ void joinGroup(const char* groupname, const char* if_addr = nullptr);
+ void setMulticastSource(const char* source_addr);
+ void setMulticastTTL(int ttl);
+
+ /** Set blocking mode. By default, the socket is blocking.
+ * throws a runtime_error on error.
+ */
+ void setBlocking(bool block);
+
+ protected:
+ SOCKET m_sock;
+};
+
+/* Threaded UDP receiver */
+class UDPReceiver {
+ public:
+ UDPReceiver();
+ ~UDPReceiver();
+ UDPReceiver(const UDPReceiver&) = delete;
+ UDPReceiver operator=(const UDPReceiver&) = delete;
+
+ // Start the receiver in a separate thread
+ void start(int port, const std::string& bindto, const std::string& mcastaddr, size_t max_packets_queued);
+
+ // Get the data contained in a UDP packet, blocks if none available
+ // In case of error, throws a runtime_error
+ std::vector<uint8_t> get_packet_buffer(void);
+
+ private:
+ void m_run(void);
+
+ int m_port = 0;
+ std::string m_bindto;
+ std::string m_mcastaddr;
+ size_t m_max_packets_queued = 1;
+ std::thread m_thread;
+ std::atomic<bool> m_stop = ATOMIC_VAR_INIT(false);
+ ThreadsafeQueue<UDPPacket> m_packets;
+ UDPSocket m_sock;
+};
+
+class TCPSocket {
+ public:
+ TCPSocket();
+ ~TCPSocket();
+ TCPSocket(const TCPSocket& other) = delete;
+ TCPSocket& operator=(const TCPSocket& other) = delete;
+ TCPSocket(TCPSocket&& other);
+ TCPSocket& operator=(TCPSocket&& other);
+
+ bool valid(void) const;
+ void connect(const std::string& hostname, int port);
+ void listen(int port, const std::string& name);
+ void close(void);
+
+ /* throws a runtime_error on failure, an invalid socket on timeout */
+ TCPSocket accept(int timeout_ms);
+
+ /* returns -1 on error, doesn't work on nonblocking sockets */
+ ssize_t sendall(const void *buffer, size_t buflen);
+
+ /** Send data over the TCP connection.
+ * @param data The buffer that will be sent.
+ * @param size Number of bytes to send.
+ * @param timeout_ms number of milliseconds before timeout, or 0 for infinite timeout
+ * return number of bytes sent, 0 on timeout, or throws runtime_error.
+ */
+ ssize_t send(const void* data, size_t size, int timeout_ms=0);
+
+ /* Returns number of bytes read, 0 on disconnect. Throws a
+ * runtime_error on error */
+ ssize_t recv(void *buffer, size_t length, int flags);
+
+ class Timeout {};
+ class Interrupted {};
+ /* Returns number of bytes read, 0 on disconnect or refused connection.
+ * Throws a Timeout on timeout, Interrupted on EINTR, a runtime_error
+ * on error
+ */
+ ssize_t recv(void *buffer, size_t length, int flags, int timeout_ms);
+
+ private:
+ explicit TCPSocket(int sockfd);
+ explicit TCPSocket(int sockfd, InetAddress remote_address);
+ SOCKET m_sock = -1;
+
+ InetAddress m_remote_address;
+
+ friend class TCPClient;
+};
+
+/* Implements a TCP receiver that auto-reconnects on errors */
+class TCPClient {
+ public:
+ void connect(const std::string& hostname, int port);
+
+ /* Returns numer of bytes read, 0 on auto-reconnect, -1
+ * on interruption.
+ * Throws a runtime_error on error */
+ ssize_t recv(void *buffer, size_t length, int flags, int timeout_ms);
+
+ private:
+ void reconnect(void);
+ TCPSocket m_sock;
+ std::string m_hostname;
+ int m_port;
+};
+
+/* Helper class for TCPDataDispatcher, contains a queue of pending data and
+ * a sender thread. */
+class TCPConnection
+{
+ public:
+ TCPConnection(TCPSocket&& sock);
+ TCPConnection(const TCPConnection&) = delete;
+ TCPConnection& operator=(const TCPConnection&) = delete;
+ ~TCPConnection();
+
+ ThreadsafeQueue<std::vector<uint8_t> > queue;
+
+ private:
+ std::atomic<bool> m_running;
+ std::thread m_sender_thread;
+ TCPSocket m_sock;
+
+ void process(void);
+};
+
+/* Send a TCP stream to several destinations, and automatically disconnect destinations
+ * whose buffer overflows.
+ */
+class TCPDataDispatcher
+{
+ public:
+ TCPDataDispatcher(size_t max_queue_size);
+ ~TCPDataDispatcher();
+ TCPDataDispatcher(const TCPDataDispatcher&) = delete;
+ TCPDataDispatcher& operator=(const TCPDataDispatcher&) = delete;
+
+ void start(int port, const std::string& address);
+ void write(const std::vector<uint8_t>& data);
+
+ private:
+ void process();
+
+ size_t m_max_queue_size;
+
+ std::atomic<bool> m_running;
+ std::string m_exception_data;
+ std::thread m_listener_thread;
+ TCPSocket m_listener_socket;
+ std::list<TCPConnection> m_connections;
+};
+
+/* A TCP Server to receive data, which abstracts the handling of connects and disconnects.
+ */
+class TCPReceiveServer {
+ public:
+ TCPReceiveServer(size_t blocksize);
+ ~TCPReceiveServer();
+ TCPReceiveServer(const TCPReceiveServer&) = delete;
+ TCPReceiveServer& operator=(const TCPReceiveServer&) = delete;
+
+ void start(int listen_port, const std::string& address);
+
+ // Return a vector that contains up to blocksize bytes of data, or
+ // and empty vector if no data is available.
+ std::vector<uint8_t> receive();
+
+ private:
+ void process();
+
+ size_t m_blocksize = 0;
+ ThreadsafeQueue<std::vector<uint8_t> > m_queue;
+ std::atomic<bool> m_running;
+ std::string m_exception_data;
+ std::thread m_listener_thread;
+ TCPSocket m_listener_socket;
+};
+
+}
diff --git a/lib/ThreadsafeQueue.h b/lib/ThreadsafeQueue.h
new file mode 100644
index 0000000..62f4c96
--- /dev/null
+++ b/lib/ThreadsafeQueue.h
@@ -0,0 +1,176 @@
+/*
+ Copyright (C) 2007, 2008, 2009, 2010, 2011 Her Majesty the Queen in
+ Right of Canada (Communications Research Center Canada)
+
+ Copyright (C) 2018
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ An implementation for a threadsafe queue, depends on C++11
+
+ When creating a ThreadsafeQueue, one can specify the minimal number
+ of elements it must contain before it is possible to take one
+ element out.
+ */
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <mutex>
+#include <condition_variable>
+#include <queue>
+#include <utility>
+
+/* This queue is meant to be used by two threads. One producer
+ * that pushes elements into the queue, and one consumer that
+ * retrieves the elements.
+ *
+ * The queue can make the consumer block until an element
+ * is available, or a wakeup requested.
+ */
+
+/* Class thrown by blocking pop to tell the consumer
+ * that there's a wakeup requested. */
+class ThreadsafeQueueWakeup {};
+
+template<typename T>
+class ThreadsafeQueue
+{
+public:
+ /* Push one element into the queue, and notify another thread that
+ * might be waiting.
+ *
+ * returns the new queue size.
+ */
+ size_t push(T const& val)
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ the_queue.push(val);
+ size_t queue_size = the_queue.size();
+ lock.unlock();
+
+ the_rx_notification.notify_one();
+
+ return queue_size;
+ }
+
+ size_t push(T&& val)
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ the_queue.emplace(std::move(val));
+ size_t queue_size = the_queue.size();
+ lock.unlock();
+
+ the_rx_notification.notify_one();
+
+ return queue_size;
+ }
+
+ /* Push one element into the queue, but wait until the
+ * queue size goes below the threshold.
+ *
+ * Notify waiting thread.
+ *
+ * returns the new queue size.
+ */
+ size_t push_wait_if_full(T const& val, size_t threshold)
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ while (the_queue.size() >= threshold) {
+ the_tx_notification.wait(lock);
+ }
+ the_queue.push(val);
+ size_t queue_size = the_queue.size();
+ lock.unlock();
+
+ the_rx_notification.notify_one();
+
+ return queue_size;
+ }
+
+ /* Trigger a wakeup event on a blocking consumer, which
+ * will receive a ThreadsafeQueueWakeup exception.
+ */
+ void trigger_wakeup(void)
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ wakeup_requested = true;
+ lock.unlock();
+ the_rx_notification.notify_one();
+ }
+
+ /* Send a notification for the receiver thread */
+ void notify(void)
+ {
+ the_rx_notification.notify_one();
+ }
+
+ bool empty() const
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ return the_queue.empty();
+ }
+
+ size_t size() const
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ return the_queue.size();
+ }
+
+ bool try_pop(T& popped_value)
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ if (the_queue.empty()) {
+ return false;
+ }
+
+ popped_value = the_queue.front();
+ the_queue.pop();
+
+ lock.unlock();
+ the_tx_notification.notify_one();
+
+ return true;
+ }
+
+ void wait_and_pop(T& popped_value, size_t prebuffering = 1)
+ {
+ std::unique_lock<std::mutex> lock(the_mutex);
+ while (the_queue.size() < prebuffering and
+ not wakeup_requested) {
+ the_rx_notification.wait(lock);
+ }
+
+ if (wakeup_requested) {
+ wakeup_requested = false;
+ throw ThreadsafeQueueWakeup();
+ }
+ else {
+ std::swap(popped_value, the_queue.front());
+ the_queue.pop();
+
+ lock.unlock();
+ the_tx_notification.notify_one();
+ }
+ }
+
+private:
+ std::queue<T> the_queue;
+ mutable std::mutex the_mutex;
+ std::condition_variable the_rx_notification;
+ std::condition_variable the_tx_notification;
+ bool wakeup_requested = false;
+};
+
diff --git a/lib/edi/PFT.cpp b/lib/edi/PFT.cpp
new file mode 100644
index 0000000..aff7929
--- /dev/null
+++ b/lib/edi/PFT.cpp
@@ -0,0 +1,574 @@
+/* ------------------------------------------------------------------
+ * Copyright (C) 2017 AVT GmbH - Fabien Vercasson
+ * Copyright (C) 2017 Matthias P. Braendli
+ * matthias.braendli@mpb.li
+ *
+ * http://opendigitalradio.org
+ *
+ * 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 <stdio.h>
+#include <cassert>
+#include <cstring>
+#include <sstream>
+#include <stdexcept>
+#include <algorithm>
+#include "crc.h"
+#include "PFT.hpp"
+#include "Log.h"
+#include "buffer_unpack.hpp"
+extern "C" {
+#include "fec/fec.h"
+}
+
+namespace EdiDecoder {
+namespace PFT {
+
+using namespace std;
+
+const findex_t NUM_AFBUILDERS_TO_KEEP = 10;
+
+static bool checkCRC(const uint8_t *buf, size_t size)
+{
+ const uint16_t crc_from_packet = read_16b(buf + size - 2);
+ uint16_t crc_calc = 0xffff;
+ crc_calc = crc16(crc_calc, buf, size - 2);
+ crc_calc ^= 0xffff;
+
+ return crc_from_packet == crc_calc;
+}
+
+class FECDecoder {
+ public:
+ FECDecoder() {
+ m_rs_handler = init_rs_char(
+ symsize, gfPoly, firstRoot, primElem, nroots, pad);
+ }
+ FECDecoder(const FECDecoder& other) = delete;
+ FECDecoder& operator=(const FECDecoder& other) = delete;
+ ~FECDecoder() {
+ free_rs_char(m_rs_handler);
+ }
+
+ // return -1 in case of failure, non-negative value if errors
+ // were corrected.
+ // Known positions of erasures should be given in eras_pos to
+ // improve decoding probability. After calling this function
+ // eras_pos will contain the positions of the corrected errors.
+ int decode(vector<uint8_t> &data, vector<int> &eras_pos) {
+ assert(data.size() == N);
+ const size_t no_eras = eras_pos.size();
+
+ eras_pos.resize(nroots);
+ int num_err = decode_rs_char(m_rs_handler, data.data(),
+ eras_pos.data(), no_eras);
+ if (num_err > 0) {
+ eras_pos.resize(num_err);
+ }
+ return num_err;
+ }
+
+ // return -1 in case of failure, non-negative value if errors
+ // were corrected. No known erasures.
+ int decode(vector<uint8_t> &data) {
+ assert(data.size() == N);
+ int num_err = decode_rs_char(m_rs_handler, data.data(), nullptr, 0);
+ return num_err;
+ }
+
+ private:
+ void* m_rs_handler;
+
+ 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 size_t N = 255;
+ const size_t K = 207;
+ const int primElem = 1;
+ const int symsize = 8;
+ const size_t nroots = N - K; // For EDI PFT, this must be 48
+ const size_t pad = ((1 << symsize) - 1) - N; // is 255-N
+
+};
+
+size_t Fragment::loadData(const std::vector<uint8_t> &buf)
+{
+ const size_t header_len = 14;
+ if (buf.size() < header_len) {
+ return 0;
+ }
+
+ size_t index = 0;
+
+ // Parse PFT Fragment Header (ETSI TS 102 821 V1.4.1 ch7.1)
+ if (not (buf[0] == 'P' and buf[1] == 'F') ) {
+ throw invalid_argument("Invalid PFT SYNC bytes");
+ }
+ index += 2; // Psync
+
+ _Pseq = read_16b(buf.begin()+index); index += 2;
+ _Findex = read_24b(buf.begin()+index); index += 3;
+ _Fcount = read_24b(buf.begin()+index); index += 3;
+ _FEC = unpack1bit(buf[index], 0);
+ _Addr = unpack1bit(buf[index], 1);
+ _Plen = read_16b(buf.begin()+index) & 0x3FFF; index += 2;
+
+ const size_t required_len = header_len +
+ (_FEC ? 1 : 0) +
+ (_Addr ? 2 : 0) +
+ 2; // CRC
+ if (buf.size() < required_len) {
+ return 0;
+ }
+
+ // 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 = read_16b(buf.begin()+index); index += 2;
+ _Dest = read_16b(buf.begin()+index); index += 2;
+ }
+
+ index += 2;
+ const bool crc_valid = checkCRC(buf.data(), index);
+ const bool buf_has_enough_data = (buf.size() >= index + _Plen);
+
+ if (not buf_has_enough_data) {
+ return 0;
+ }
+
+ _valid = ((not _FEC) or crc_valid) and buf_has_enough_data;
+
+#if 0
+ if (!_valid) {
+ stringstream ss;
+ ss << "Invalid PF fragment: ";
+ if (_FEC) {
+ ss << " RSk=" << (uint32_t)_RSk << " RSz=" << (uint32_t)_RSz;
+ }
+
+ if (_Addr) {
+ ss << " Source=" << _Source << " Dest=" << _Dest;
+ }
+ etiLog.log(debug, "%s\n", ss.str().c_str());
+ }
+#endif
+
+ _payload.clear();
+ if (_valid) {
+ copy( buf.begin()+index,
+ buf.begin()+index+_Plen,
+ back_inserter(_payload));
+ index += _Plen;
+ }
+
+ return index;
+}
+
+
+AFBuilder::AFBuilder(pseq_t Pseq, findex_t Fcount, size_t lifetime)
+{
+ _Pseq = Pseq;
+ _Fcount = Fcount;
+ assert(lifetime > 0);
+ lifeTime = lifetime;
+}
+
+void AFBuilder::pushPFTFrag(const Fragment &frag)
+{
+ if (_Pseq != frag.Pseq() or _Fcount != frag.Fcount()) {
+ throw invalid_argument("Invalid PFT fragment Pseq or Fcount");
+ }
+ const auto Findex = frag.Findex();
+ const bool fragment_already_received = _fragments.count(Findex);
+
+ if (not fragment_already_received)
+ {
+ _fragments[Findex] = frag;
+ }
+}
+
+bool Fragment::checkConsistency(const Fragment& other) const
+{
+ /* Consistency check, TS 102 821 Clause 7.3.2.
+ *
+ * Every PFT Fragment produced from a single AF or RS Packet shall have
+ * the same values in all of the PFT Header fields except for the Findex,
+ * Plen and HCRC fields.
+ */
+
+ return other._Fcount == _Fcount and
+ other._FEC == _FEC and
+ other._RSk == _RSk and
+ other._RSz == _RSz and
+ other._Addr == _Addr and
+ other._Source == _Source and
+ other._Dest == _Dest and
+
+ /* The Plen field of all fragments shall be the s for the initial f-1
+ * fragments and s - (L%f) for the final fragment.
+ * Note that when Reed Solomon has been used, all fragments will be of
+ * length s.
+ */
+ (_FEC ? other._Plen == _Plen : true);
+}
+
+
+AFBuilder::decode_attempt_result_t AFBuilder::canAttemptToDecode() const
+{
+ if (_fragments.empty()) {
+ return AFBuilder::decode_attempt_result_t::no;
+ }
+
+ if (_fragments.size() == _Fcount) {
+ return AFBuilder::decode_attempt_result_t::yes;
+ }
+
+ /* Check that all fragments are consistent */
+ const Fragment& first = _fragments.begin()->second;
+ if (not std::all_of(_fragments.begin(), _fragments.end(),
+ [&](const pair<int, Fragment>& pair) {
+ const Fragment& frag = pair.second;
+ return first.checkConsistency(frag) and _Pseq == frag.Pseq();
+ }) ) {
+ throw invalid_argument("Inconsistent PFT fragments");
+ }
+
+ // Calculate the minimum number of fragments necessary to apply FEC.
+ // This can't be done with the last fragment that may have a
+ // smaller size
+ // ETSI TS 102 821 V1.4.1 ch 7.4.4
+ auto frag_it = _fragments.begin();
+ if (frag_it->second.Fcount() == _Fcount - 1) {
+ frag_it++;
+
+ if (frag_it == _fragments.end()) {
+ return AFBuilder::decode_attempt_result_t::no;
+ }
+ }
+
+ const Fragment& frag = frag_it->second;
+
+ if ( frag.FEC() )
+ {
+ const uint16_t _Plen = frag.Plen();
+
+ /* max number of RS chunks that may have been sent */
+ const uint32_t _cmax = (_Fcount*_Plen) / (frag.RSk()+48);
+ assert(_cmax > 0);
+
+ /* Receiving _rxmin fragments does not guarantee that decoding
+ * will succeed! */
+ const uint32_t _rxmin = _Fcount - (_cmax*48)/_Plen;
+
+ if (_fragments.size() >= _rxmin) {
+ return AFBuilder::decode_attempt_result_t::maybe;
+ }
+ }
+
+ return AFBuilder::decode_attempt_result_t::no;
+}
+
+std::vector<uint8_t> AFBuilder::extractAF() const
+{
+ if (not _af_packet.empty()) {
+ return _af_packet;
+ }
+
+ bool ok = false;
+
+ if (canAttemptToDecode() != AFBuilder::decode_attempt_result_t::no) {
+
+ auto frag_it = _fragments.begin();
+ if (frag_it->second.Fcount() == _Fcount - 1) {
+ frag_it++;
+
+ if (frag_it == _fragments.end()) {
+ throw std::runtime_error("Invalid attempt at extracting AF");
+ }
+ }
+
+ const Fragment& ref_frag = frag_it->second;
+ const auto RSk = ref_frag.RSk();
+ const auto RSz = ref_frag.RSz();
+ const auto Plen = ref_frag.Plen();
+
+ if ( ref_frag.FEC() )
+ {
+ const uint32_t cmax = (_Fcount*Plen) / (RSk+48);
+
+ // Keep track of erasures (missing fragments) for
+ // every chunk
+ map<int, vector<int> > erasures;
+
+
+ // Assemble fragments into a RS block, immediately
+ // deinterleaving it.
+ vector<uint8_t> rs_block(Plen * _Fcount);
+ for (size_t j = 0; j < _Fcount; j++) {
+ const bool fragment_present = _fragments.count(j);
+ if (fragment_present) {
+ const auto& fragment = _fragments.at(j).payload();
+
+ if (j != _Fcount - 1 and fragment.size() != Plen) {
+ throw runtime_error("Incorrect fragment length " +
+ to_string(fragment.size()) + " " +
+ to_string(Plen));
+ }
+
+ if (j == _Fcount - 1 and fragment.size() > Plen) {
+ throw runtime_error("Incorrect last fragment length " +
+ to_string(fragment.size()) + " " +
+ to_string(Plen));
+ }
+
+ size_t k = 0;
+ for (; k < fragment.size(); k++) {
+ rs_block[k * _Fcount + j] = fragment[k];
+ }
+
+ for (; k < Plen; k++) {
+ rs_block[k * _Fcount + j] = 0x00;
+ }
+ }
+ else {
+ // fill with zeros if fragment is missing
+ for (size_t k = 0; k < Plen; k++) {
+ rs_block[k * _Fcount + j] = 0x00;
+
+ const size_t chunk_ix = (k * _Fcount + j) / (RSk + 48);
+ const size_t chunk_offset = (k * _Fcount + j) % (RSk + 48);
+ erasures[chunk_ix].push_back(chunk_offset);
+ }
+ }
+ }
+
+ // The RS block is a concatenation of chunks of RSk bytes + 48 parity
+ // followed by RSz padding
+
+ FECDecoder fec;
+ for (size_t i = 0; i < cmax; i++) {
+ // We need to pad the chunk ourself
+ vector<uint8_t> chunk(255);
+ const auto& block_begin = rs_block.begin() + (RSk + 48) * i;
+ copy(block_begin, block_begin + RSk, chunk.begin());
+ // bytes between RSk and 207 are 0x00 already
+ copy(block_begin + RSk, block_begin + RSk + 48,
+ chunk.begin() + 207);
+
+ int errors_corrected = -1;
+ if (erasures.count(i)) {
+ errors_corrected = fec.decode(chunk, erasures[i]);
+ }
+ else {
+ errors_corrected = fec.decode(chunk);
+ }
+
+ if (errors_corrected == -1) {
+ _af_packet.clear();
+ return {};
+ }
+
+#if 0
+ if (errors_corrected > 0) {
+ etiLog.log(debug, "Corrected %d errors at ", errors_corrected);
+ for (const auto &index : erasures[i]) {
+ etiLog.log(debug, " %d", index);
+ }
+ etiLog.log(debug, "\n");
+ }
+#endif
+
+ _af_packet.insert(_af_packet.end(), chunk.begin(), chunk.begin() + RSk);
+ }
+
+ _af_packet.resize(_af_packet.size() - RSz);
+ }
+ else {
+ // No FEC: just assemble fragments
+
+ for (size_t j = 0; j < _Fcount; ++j) {
+ const bool fragment_present = _fragments.count(j);
+ if (fragment_present)
+ {
+ const auto& fragment = _fragments.at(j);
+
+ _af_packet.insert(_af_packet.end(),
+ fragment.payload().begin(),
+ fragment.payload().end());
+ }
+ else {
+ throw logic_error("Missing fragment");
+ }
+ }
+ }
+
+ // EDI specific, must have a CRC.
+ if( _af_packet.size() >= 12 ) {
+ ok = checkCRC(_af_packet.data(), _af_packet.size());
+
+ if (not ok) {
+ etiLog.log(debug, "Too many errors to reconstruct AF from %zu/%u"
+ " PFT fragments\n", _fragments.size(), _Fcount);
+ }
+ }
+ }
+
+ if (not ok) {
+ _af_packet.clear();
+ }
+
+ return _af_packet;
+}
+
+std::string AFBuilder::visualise() const
+{
+ stringstream ss;
+ ss << "|";
+ for (size_t i = 0; i < _Fcount; i++) {
+ if (_fragments.count(i)) {
+ ss << ".";
+ }
+ else {
+ ss << " ";
+ }
+ }
+ ss << "| " << AFBuilder::dar_to_string(canAttemptToDecode()) << " " << lifeTime;
+ return ss.str();
+}
+
+void PFT::pushPFTFrag(const Fragment &fragment)
+{
+ // Start decoding the first pseq we receive. In normal
+ // operation without interruptions, the map should
+ // never become empty
+ if (m_afbuilders.empty()) {
+ m_next_pseq = fragment.Pseq();
+ etiLog.log(debug,"Initialise next_pseq to %u\n", m_next_pseq);
+ }
+
+ if (m_afbuilders.count(fragment.Pseq()) == 0) {
+ // The AFBuilder wants to know the lifetime in number of fragments,
+ // we know the delay in number of AF packets. Every AF packet
+ // is cut into Fcount fragments.
+ const size_t lifetime = fragment.Fcount() * m_max_delay;
+
+ // Build the afbuilder in the map in-place
+ m_afbuilders.emplace(std::piecewise_construct,
+ /* key */
+ std::forward_as_tuple(fragment.Pseq()),
+ /* builder */
+ std::forward_as_tuple(fragment.Pseq(), fragment.Fcount(), lifetime));
+ }
+
+ auto& p = m_afbuilders.at(fragment.Pseq());
+ p.pushPFTFrag(fragment);
+
+ if (m_verbose) {
+ etiLog.log(debug, "Got frag %u:%u, afbuilders: ",
+ fragment.Pseq(), fragment.Findex());
+ for (const auto &k : m_afbuilders) {
+ const bool isNextPseq = (m_next_pseq == k.first);
+ etiLog.level(debug) << (isNextPseq ? "->" : " ") <<
+ k.first << " " << k.second.visualise();
+ }
+ }
+}
+
+
+std::vector<uint8_t> PFT::getNextAFPacket()
+{
+ if (m_afbuilders.count(m_next_pseq) == 0) {
+ if (m_afbuilders.size() > m_max_delay) {
+ m_afbuilders.clear();
+ etiLog.level(debug) << " Reinit";
+ }
+
+ return {};
+ }
+
+ auto &builder = m_afbuilders.at(m_next_pseq);
+
+ using dar_t = AFBuilder::decode_attempt_result_t;
+
+ if (builder.canAttemptToDecode() == dar_t::yes) {
+ auto afpacket = builder.extractAF();
+ assert(not afpacket.empty());
+ incrementNextPseq();
+ return afpacket;
+ }
+ else if (builder.canAttemptToDecode() == dar_t::maybe) {
+ if (builder.lifeTime > 0) {
+ builder.lifeTime--;
+ }
+
+ if (builder.lifeTime == 0) {
+ // Attempt Reed-Solomon decoding
+ auto afpacket = builder.extractAF();
+
+ if (afpacket.empty()) {
+ etiLog.log(debug,"pseq %d timed out after RS", m_next_pseq);
+ }
+ incrementNextPseq();
+ return afpacket;
+ }
+ }
+ else {
+ if (builder.lifeTime > 0) {
+ builder.lifeTime--;
+ }
+
+ if (builder.lifeTime == 0) {
+ etiLog.log(debug, "pseq %d timed out\n", m_next_pseq);
+ incrementNextPseq();
+ }
+ }
+
+ return {};
+}
+
+void PFT::setMaxDelay(size_t num_af_packets)
+{
+ m_max_delay = num_af_packets;
+}
+
+void PFT::setVerbose(bool enable)
+{
+ m_verbose = enable;
+}
+
+void PFT::incrementNextPseq()
+{
+ if (m_afbuilders.count(m_next_pseq - NUM_AFBUILDERS_TO_KEEP) > 0) {
+ m_afbuilders.erase(m_next_pseq - NUM_AFBUILDERS_TO_KEEP);
+ }
+
+ m_next_pseq++;
+}
+
+}
+}
diff --git a/lib/edi/PFT.hpp b/lib/edi/PFT.hpp
new file mode 100644
index 0000000..779509b
--- /dev/null
+++ b/lib/edi/PFT.hpp
@@ -0,0 +1,166 @@
+/* ------------------------------------------------------------------
+ * Copyright (C) 2017 AVT GmbH - Fabien Vercasson
+ * Copyright (C) 2017 Matthias P. Braendli
+ * matthias.braendli@mpb.li
+ *
+ * http://opendigitalradio.org
+ *
+ * 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.
+ * -------------------------------------------------------------------
+ */
+
+#pragma once
+#include <stdio.h>
+#include <vector>
+#include <map>
+#include <stdint.h>
+
+namespace EdiDecoder {
+namespace PFT {
+
+using pseq_t = uint16_t;
+using findex_t = uint32_t; // findex is a 24-bit value
+
+class Fragment
+{
+ public:
+ // Load the data for one fragment from buf into
+ // the Fragment.
+ // \returns the number of bytes of useful data found in buf
+ // A non-zero return value doesn't imply a valid fragment
+ // the isValid() method must be used to verify this.
+ size_t loadData(const std::vector<uint8_t> &buf);
+
+ bool isValid() const { return _valid; }
+ pseq_t Pseq() const { return _Pseq; }
+ findex_t Findex() const { return _Findex; }
+ findex_t Fcount() const { return _Fcount; }
+ bool FEC() const { return _FEC; }
+ uint16_t Plen() const { return _Plen; }
+ uint8_t RSk() const { return _RSk; }
+ uint8_t RSz() const { return _RSz; }
+ const std::vector<uint8_t>& payload() const
+ { return _payload; }
+
+ bool checkConsistency(const Fragment& other) const;
+
+ private:
+ std::vector<uint8_t> _payload;
+
+ pseq_t _Pseq = 0;
+ findex_t _Findex = 0;
+ findex_t _Fcount = 0;
+ bool _FEC = false;
+ bool _Addr = false;
+ uint16_t _Plen = 0;
+ uint8_t _RSk = 0;
+ uint8_t _RSz = 0;
+ uint16_t _Source = 0;
+ uint16_t _Dest = 0;
+ bool _valid = false;
+};
+
+/* The AFBuilder collects Fragments and builds an Application Frame
+ * out of them. It does error correction if necessary
+ */
+class AFBuilder
+{
+ public:
+ enum class decode_attempt_result_t {
+ yes, // The AF packet can be build because all fragments are present
+ maybe, // RS decoding may correctly decode the AF packet
+ no, // Not enough fragments present to permit RS
+ };
+
+ static std::string dar_to_string(decode_attempt_result_t dar) {
+ switch (dar) {
+ case decode_attempt_result_t::yes: return "y";
+ case decode_attempt_result_t::no: return "n";
+ case decode_attempt_result_t::maybe: return "m";
+ }
+ return "?";
+ }
+
+ AFBuilder(pseq_t Pseq, findex_t Fcount, size_t lifetime);
+
+ void pushPFTFrag(const Fragment &frag);
+
+ /* Assess if it may be possible to decode this AF packet */
+ decode_attempt_result_t canAttemptToDecode() const;
+
+ /* Try to build the AF with received fragments.
+ * Apply error correction if necessary (missing packets/CRC errors)
+ * \return an empty vector if building the AF is not possible
+ */
+ std::vector<uint8_t> extractAF(void) const;
+
+ std::pair<findex_t, findex_t>
+ numberOfFragments(void) const {
+ return {_fragments.size(), _Fcount};
+ }
+
+ std::string visualise(void) const;
+
+ /* The user of this instance can keep track of the lifetime of this
+ * builder
+ */
+ size_t lifeTime;
+
+ private:
+
+ // A map from fragment index to fragment
+ std::map<findex_t, Fragment> _fragments;
+
+ // cached version of decoded AF packet
+ mutable std::vector<uint8_t> _af_packet;
+
+ pseq_t _Pseq;
+ findex_t _Fcount;
+};
+
+class PFT
+{
+ public:
+ void pushPFTFrag(const Fragment &fragment);
+
+ /* Try to build the AF packet for the next pseq. This might
+ * skip one or more pseq according to the maximum delay setting.
+ *
+ * \return an empty vector if building the AF is not possible
+ */
+ std::vector<uint8_t> getNextAFPacket(void);
+
+ /* Set the maximum delay in number of AF Packets before we
+ * abandon decoding a given pseq.
+ */
+ void setMaxDelay(size_t num_af_packets);
+
+ /* Enable verbose fprintf */
+ void setVerbose(bool enable);
+
+ private:
+ void incrementNextPseq(void);
+
+ pseq_t m_next_pseq;
+ size_t m_max_delay = 10; // in AF packets
+
+ // Keep one AFBuilder for each Pseq
+ std::map<pseq_t, AFBuilder> m_afbuilders;
+
+ bool m_verbose = 0;
+};
+
+}
+
+}
diff --git a/lib/edi/STIDecoder.cpp b/lib/edi/STIDecoder.cpp
new file mode 100644
index 0000000..ca8cead
--- /dev/null
+++ b/lib/edi/STIDecoder.cpp
@@ -0,0 +1,191 @@
+/*
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://opendigitalradio.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include "STIDecoder.hpp"
+#include "buffer_unpack.hpp"
+#include "crc.h"
+#include "Log.h"
+#include <stdio.h>
+#include <cassert>
+#include <sstream>
+
+namespace EdiDecoder {
+
+using namespace std;
+
+STIDecoder::STIDecoder(STIDataCollector& data_collector, bool verbose) :
+ m_data_collector(data_collector),
+ m_dispatcher(std::bind(&STIDecoder::packet_completed, this), verbose)
+{
+ using std::placeholders::_1;
+ using std::placeholders::_2;
+ m_dispatcher.register_tag("*ptr",
+ std::bind(&STIDecoder::decode_starptr, this, _1, _2));
+ m_dispatcher.register_tag("dsti",
+ std::bind(&STIDecoder::decode_dsti, this, _1, _2));
+ m_dispatcher.register_tag("ss",
+ std::bind(&STIDecoder::decode_ssn, this, _1, _2));
+ m_dispatcher.register_tag("*dmy",
+ std::bind(&STIDecoder::decode_stardmy, this, _1, _2));
+}
+
+void STIDecoder::push_bytes(const vector<uint8_t> &buf)
+{
+ m_dispatcher.push_bytes(buf);
+}
+
+void STIDecoder::push_packet(const vector<uint8_t> &buf)
+{
+ m_dispatcher.push_packet(buf);
+}
+
+void STIDecoder::setMaxDelay(int num_af_packets)
+{
+ m_dispatcher.setMaxDelay(num_af_packets);
+}
+
+#define AFPACKET_HEADER_LEN 10 // includes SYNC
+
+bool STIDecoder::decode_starptr(const vector<uint8_t> &value, uint16_t)
+{
+ if (value.size() != 0x40 / 8) {
+ etiLog.log(warn, "Incorrect length %02lx for *PTR", value.size());
+ return false;
+ }
+
+ char protocol_sz[5];
+ protocol_sz[4] = '\0';
+ copy(value.begin(), value.begin() + 4, protocol_sz);
+ string protocol(protocol_sz);
+
+ uint16_t major = read_16b(value.begin() + 4);
+ uint16_t minor = read_16b(value.begin() + 6);
+
+ m_data_collector.update_protocol(protocol, major, minor);
+
+ return true;
+}
+
+bool STIDecoder::decode_dsti(const vector<uint8_t> &value, uint16_t)
+{
+ size_t offset = 0;
+
+ const uint16_t dstiHeader = read_16b(value.begin() + offset);
+ offset += 2;
+
+ sti_management_data md;
+
+ md.stihf = (dstiHeader >> 15) & 0x1;
+ md.atstf = (dstiHeader >> 14) & 0x1;
+ md.rfadf = (dstiHeader >> 13) & 0x1;
+ uint8_t dfcth = (dstiHeader >> 8) & 0x1F;
+ uint8_t dfctl = dstiHeader & 0xFF;
+
+ md.dflc = dfcth * 250 + dfctl; // modulo 5000 counter
+
+ const size_t expected_length = 2 +
+ (md.stihf ? 3 : 0) +
+ (md.atstf ? 1 + 4 + 3 : 0) +
+ (md.rfadf ? 9 : 0);
+
+ if (value.size() != expected_length) {
+ throw std::logic_error("EDI dsti: Assertion error:"
+ "value.size() != expected_length: " +
+ to_string(value.size()) + " " +
+ to_string(expected_length));
+ }
+
+ if (md.stihf) {
+ const uint8_t stat = value[offset++];
+ const uint16_t spid = read_16b(value.begin() + offset);
+ m_data_collector.update_stat(stat, spid);
+ offset += 2;
+ }
+
+ if (md.atstf) {
+ uint8_t utco = value[offset];
+ offset++;
+
+ uint32_t seconds = read_32b(value.begin() + offset);
+ offset += 4;
+
+ m_data_collector.update_edi_time(utco, seconds);
+
+ md.tsta = read_24b(value.begin() + offset);
+ offset += 3;
+ }
+ else {
+ // Null timestamp, ETSI ETS 300 799, C.2.2
+ md.tsta = 0xFFFFFF;
+ }
+
+
+ if (md.rfadf) {
+ std::array<uint8_t, 9> rfad;
+ copy(value.cbegin() + offset,
+ value.cbegin() + offset + 9,
+ rfad.begin());
+ offset += 9;
+
+ m_data_collector.update_rfad(rfad);
+ }
+
+ m_data_collector.update_sti_management(md);
+
+ return true;
+}
+
+bool STIDecoder::decode_ssn(const vector<uint8_t> &value, uint16_t n)
+{
+ sti_payload_data sti;
+
+ sti.stream_index = n - 1; // n is 1-indexed
+ sti.rfa = value[0] >> 3;
+ sti.tid = value[0] & 0x07;
+
+ uint16_t istc = read_24b(value.begin() + 1);
+ sti.tidext = istc >> 13;
+ sti.crcstf = (istc >> 12) & 0x1;
+ sti.stid = istc & 0xFFF;
+
+ if (sti.rfa != 0) {
+ etiLog.level(warn) << "EDI: rfa field in SSnn tag non-null";
+ }
+
+ copy( value.cbegin() + 3,
+ value.cend(),
+ back_inserter(sti.istd));
+
+ m_data_collector.add_payload(move(sti));
+
+ return true;
+}
+
+bool STIDecoder::decode_stardmy(const vector<uint8_t>& /*value*/, uint16_t)
+{
+ return true;
+}
+
+void STIDecoder::packet_completed()
+{
+ m_data_collector.assemble();
+}
+
+}
diff --git a/lib/edi/STIDecoder.hpp b/lib/edi/STIDecoder.hpp
new file mode 100644
index 0000000..201a176
--- /dev/null
+++ b/lib/edi/STIDecoder.hpp
@@ -0,0 +1,122 @@
+/*
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://opendigitalradio.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#pragma once
+
+#include "common.hpp"
+#include <cstdint>
+#include <deque>
+#include <string>
+#include <vector>
+
+namespace EdiDecoder {
+
+// Information for STI-D Management
+struct sti_management_data {
+ bool stihf;
+ bool atstf;
+ bool rfadf;
+ uint16_t dflc;
+
+ uint32_t tsta;
+};
+
+// Information for a subchannel available in EDI
+struct sti_payload_data {
+ uint16_t stream_index;
+ uint8_t rfa;
+ uint8_t tid;
+ uint8_t tidext;
+ bool crcstf;
+ uint16_t stid;
+ std::vector<uint8_t> istd;
+
+ // Return the length of ISTD in bytes
+ uint16_t stl(void) const { return istd.size(); }
+};
+
+/* A class that receives STI data must implement the interface described
+ * in the STIDataCollector. This can be e.g. a converter to ETI, or something that
+ * prepares data structures for a modulator.
+ */
+class STIDataCollector {
+ public:
+ // Tell the ETIWriter what EDI protocol we receive in *ptr.
+ // This is not part of the ETI data, but is used as check
+ virtual void update_protocol(
+ const std::string& proto,
+ uint16_t major,
+ uint16_t minor) = 0;
+
+ // STAT error field and service provider ID
+ virtual void update_stat(uint8_t stat, uint16_t spid) = 0;
+
+ // In addition to TSTA in ETI, EDI also transports more time
+ // stamp information.
+ virtual void update_edi_time(uint32_t utco, uint32_t seconds) = 0;
+
+ virtual void update_rfad(std::array<uint8_t, 9> rfad) = 0;
+ virtual void update_sti_management(const sti_management_data& data) = 0;
+
+ virtual void add_payload(sti_payload_data&& payload) = 0;
+
+ virtual void assemble() = 0;
+};
+
+/* The STIDecoder takes care of decoding the EDI TAGs related to the transport
+ * of ETI(NI) data inside AF and PF packets.
+ *
+ * PF packets are handed over to the PFT decoder, which will in turn return
+ * AF packets. AF packets are directly handled (TAG extraction) here.
+ */
+class STIDecoder {
+ public:
+ STIDecoder(STIDataCollector& data_collector, bool verbose);
+
+ /* Push bytes into the decoder. The buf can contain more
+ * than a single packet. This is useful when reading from streams
+ * (files, TCP)
+ */
+ void push_bytes(const std::vector<uint8_t> &buf);
+
+ /* Push a complete packet into the decoder. Useful for UDP and other
+ * datagram-oriented protocols.
+ */
+ void push_packet(const std::vector<uint8_t> &buf);
+
+ /* Set the maximum delay in number of AF Packets before we
+ * abandon decoding a given pseq.
+ */
+ void setMaxDelay(int num_af_packets);
+
+ private:
+ bool decode_starptr(const std::vector<uint8_t> &value, uint16_t);
+ bool decode_dsti(const std::vector<uint8_t> &value, uint16_t);
+ bool decode_ssn(const std::vector<uint8_t> &value, uint16_t n);
+ bool decode_stardmy(const std::vector<uint8_t> &value, uint16_t);
+
+ void packet_completed();
+
+ STIDataCollector& m_data_collector;
+ TagDispatcher m_dispatcher;
+
+};
+
+}
diff --git a/lib/edi/STIWriter.cpp b/lib/edi/STIWriter.cpp
new file mode 100644
index 0000000..6964eb1
--- /dev/null
+++ b/lib/edi/STIWriter.cpp
@@ -0,0 +1,138 @@
+/*
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://opendigitalradio.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include "STIWriter.hpp"
+#include "crc.h"
+#include "Log.h"
+#include <cstdio>
+#include <cassert>
+#include <stdexcept>
+#include <sstream>
+#include <ctime>
+#include <iostream>
+#include <iomanip>
+#include <sstream>
+
+namespace EdiDecoder {
+
+using namespace std;
+
+void STIWriter::update_protocol(
+ const std::string& proto,
+ uint16_t major,
+ uint16_t minor)
+{
+ m_proto_valid = (proto == "DSTI" and major == 0 and minor == 0);
+
+ if (not m_proto_valid) {
+ throw std::invalid_argument("Wrong EDI protocol");
+ }
+}
+
+void STIWriter::reinit()
+{
+ m_proto_valid = false;
+ m_management_data_valid = false;
+ m_stat_valid = false;
+ m_time_valid = false;
+ m_payload_valid = false;
+ m_stiFrame.frame.clear();
+}
+
+void STIWriter::update_stat(uint8_t stat, uint16_t spid)
+{
+ m_stat = stat;
+ m_spid = spid;
+ m_stat_valid = true;
+
+ if (m_stat != 0xFF) {
+ etiLog.log(warn, "STI errorlevel %02x", m_stat);
+ }
+}
+
+void STIWriter::update_rfad(std::array<uint8_t, 9> rfad)
+{
+ (void)rfad;
+}
+
+void STIWriter::update_sti_management(const sti_management_data& data)
+{
+ m_management_data = data;
+ m_management_data_valid = true;
+}
+
+void STIWriter::add_payload(sti_payload_data&& payload)
+{
+ m_payload = move(payload);
+ m_payload_valid = true;
+}
+
+void STIWriter::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 STIWriter::assemble()
+{
+ if (not m_proto_valid) {
+ throw std::logic_error("Cannot assemble STI before protocol");
+ }
+
+ if (not m_management_data_valid) {
+ throw std::logic_error("Cannot assemble STI before management data");
+ }
+
+ if (not m_payload_valid) {
+ throw std::logic_error("Cannot assemble STI without frame data");
+ }
+
+ // TODO check time validity
+
+ // Do copies so as to preserve existing payload data
+ m_stiFrame.frame = m_payload.istd;
+ m_stiFrame.timestamp.seconds = m_seconds;
+ m_stiFrame.timestamp.utco = m_utco;
+}
+
+sti_frame_t STIWriter::getFrame()
+{
+ if (m_stiFrame.frame.empty()) {
+ return {};
+ }
+
+ sti_frame_t sti;
+ swap(sti, m_stiFrame);
+ reinit();
+ return sti;
+}
+
+}
+
diff --git a/lib/edi/STIWriter.hpp b/lib/edi/STIWriter.hpp
new file mode 100644
index 0000000..a75cb69
--- /dev/null
+++ b/lib/edi/STIWriter.hpp
@@ -0,0 +1,84 @@
+/*
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://opendigitalradio.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#pragma once
+
+#include "common.hpp"
+#include "STIDecoder.hpp"
+#include <cstdint>
+#include <string>
+#include <vector>
+#include <list>
+
+namespace EdiDecoder {
+
+struct sti_frame_t {
+ std::vector<uint8_t> frame;
+ frame_timestamp_t timestamp;
+};
+
+class STIWriter : public STIDataCollector {
+ public:
+ // Tell the ETIWriter what EDI protocol we receive in *ptr.
+ // This is not part of the ETI data, but is used as check
+ virtual void update_protocol(
+ const std::string& proto,
+ uint16_t major,
+ uint16_t minor);
+
+ virtual void update_stat(uint8_t stat, uint16_t spid);
+
+ virtual void update_edi_time(
+ uint32_t utco,
+ uint32_t seconds);
+
+ virtual void update_rfad(std::array<uint8_t, 9> rfad);
+ virtual void update_sti_management(const sti_management_data& data);
+ virtual void add_payload(sti_payload_data&& payload);
+
+ virtual void assemble(void);
+
+ // Return the assembled frame or an empty frame if not ready
+ sti_frame_t getFrame();
+
+ private:
+ void reinit(void);
+
+ bool m_proto_valid = false;
+
+ bool m_management_data_valid = false;
+ sti_management_data m_management_data;
+
+ bool m_stat_valid = false;
+ uint8_t m_stat = 0;
+ uint16_t m_spid = 0;
+
+ bool m_time_valid = false;
+ uint32_t m_utco = 0;
+ uint32_t m_seconds = 0;
+
+ bool m_payload_valid = false;
+ sti_payload_data m_payload;
+
+ sti_frame_t m_stiFrame;
+};
+
+}
+
diff --git a/lib/edi/buffer_unpack.hpp b/lib/edi/buffer_unpack.hpp
new file mode 100644
index 0000000..05a1534
--- /dev/null
+++ b/lib/edi/buffer_unpack.hpp
@@ -0,0 +1,62 @@
+/*
+ Copyright (C) 2016
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://opendigitalradio.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#pragma once
+#include <stdint.h>
+
+namespace EdiDecoder {
+
+template<class T>
+uint16_t read_16b(T buf)
+{
+ uint16_t value = 0;
+ value = (uint16_t)(buf[0]) << 8;
+ value |= (uint16_t)(buf[1]);
+ return value;
+}
+
+template<class T>
+uint32_t read_24b(T buf)
+{
+ uint32_t value = 0;
+ value = (uint32_t)(buf[0]) << 16;
+ value |= (uint32_t)(buf[1]) << 8;
+ value |= (uint32_t)(buf[2]);
+ return value;
+}
+
+template<class T>
+uint32_t read_32b(T buf)
+{
+ uint32_t value = 0;
+ value = (uint32_t)(buf[0]) << 24;
+ value |= (uint32_t)(buf[1]) << 16;
+ value |= (uint32_t)(buf[2]) << 8;
+ value |= (uint32_t)(buf[3]);
+ return value;
+}
+
+inline uint32_t unpack1bit(uint8_t byte, int bitpos)
+{
+ return (byte & 1 << (7-bitpos)) > (7-bitpos);
+}
+
+}
diff --git a/lib/edi/common.cpp b/lib/edi/common.cpp
new file mode 100644
index 0000000..bc0fa1b
--- /dev/null
+++ b/lib/edi/common.cpp
@@ -0,0 +1,300 @@
+/*
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://opendigitalradio.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include "common.hpp"
+#include "buffer_unpack.hpp"
+#include "Log.h"
+#include "crc.h"
+#include <iomanip>
+#include <sstream>
+#include <cassert>
+#include <cstdio>
+
+namespace EdiDecoder {
+
+using namespace std;
+
+string frame_timestamp_t::to_string() const
+{
+ const time_t seconds_in_unix_epoch = to_unix_epoch();
+
+ stringstream ss;
+ ss << "Timestamp: " << std::put_time(std::gmtime(&seconds_in_unix_epoch), "%c %Z");
+ return ss.str();
+}
+
+time_t frame_timestamp_t::to_unix_epoch() const
+{
+ // EDI epoch: 2000-01-01T00:00:00Z
+ // Convert using
+ // TZ=UTC python -c 'import datetime; print(datetime.datetime(2000,1,1,0,0,0,0).strftime("%s"))'
+ return 946684800 + seconds - utco;
+}
+
+
+TagDispatcher::TagDispatcher(
+ std::function<void()>&& af_packet_completed, bool verbose) :
+ m_af_packet_completed(move(af_packet_completed))
+{
+ m_pft.setVerbose(verbose);
+}
+
+void TagDispatcher::push_bytes(const vector<uint8_t> &buf)
+{
+ copy(buf.begin(), buf.end(), back_inserter(m_input_data));
+
+ while (m_input_data.size() > 2) {
+ if (m_input_data[0] == 'A' and m_input_data[1] == 'F') {
+ const decode_state_t st = decode_afpacket(m_input_data);
+
+ if (st.num_bytes_consumed == 0 and not st.complete) {
+ // We need to refill our buffer
+ break;
+ }
+
+ if (st.num_bytes_consumed) {
+ vector<uint8_t> remaining_data;
+ copy(m_input_data.begin() + st.num_bytes_consumed,
+ m_input_data.end(),
+ back_inserter(remaining_data));
+ m_input_data = remaining_data;
+ }
+
+ if (st.complete) {
+ m_af_packet_completed();
+ }
+ }
+ else if (m_input_data[0] == 'P' and m_input_data[1] == 'F') {
+ PFT::Fragment fragment;
+ const size_t fragment_bytes = fragment.loadData(m_input_data);
+
+ if (fragment_bytes == 0) {
+ // We need to refill our buffer
+ break;
+ }
+
+ vector<uint8_t> remaining_data;
+ copy(m_input_data.begin() + fragment_bytes,
+ m_input_data.end(),
+ back_inserter(remaining_data));
+ m_input_data = remaining_data;
+
+ if (fragment.isValid()) {
+ m_pft.pushPFTFrag(fragment);
+ }
+
+ auto af = m_pft.getNextAFPacket();
+ if (not af.empty()) {
+ decode_state_t st = decode_afpacket(af);
+
+ if (st.complete) {
+ m_af_packet_completed();
+ }
+ }
+ }
+ else {
+ etiLog.log(warn,"Unknown %c!", *m_input_data.data());
+ m_input_data.erase(m_input_data.begin());
+ }
+ }
+}
+
+void TagDispatcher::push_packet(const vector<uint8_t> &buf)
+{
+ if (buf.size() < 2) {
+ throw std::invalid_argument("Not enough bytes to read EDI packet header");
+ }
+
+ if (buf[0] == 'A' and buf[1] == 'F') {
+ const decode_state_t st = decode_afpacket(buf);
+
+ if (st.complete) {
+ m_af_packet_completed();
+ }
+
+ }
+ else if (buf[0] == 'P' and buf[1] == 'F') {
+ PFT::Fragment fragment;
+ fragment.loadData(buf);
+
+ if (fragment.isValid()) {
+ m_pft.pushPFTFrag(fragment);
+ }
+
+ auto af = m_pft.getNextAFPacket();
+ if (not af.empty()) {
+ const decode_state_t st = decode_afpacket(af);
+
+ if (st.complete) {
+ m_af_packet_completed();
+ }
+ }
+ }
+ else {
+ const char packettype[3] = {(char)buf[0], (char)buf[1], '\0'};
+ std::stringstream ss;
+ ss << "Unknown EDI packet ";
+ ss << packettype;
+ throw std::invalid_argument(ss.str());
+ }
+}
+
+void TagDispatcher::setMaxDelay(int num_af_packets)
+{
+ m_pft.setMaxDelay(num_af_packets);
+}
+
+
+#define AFPACKET_HEADER_LEN 10 // includes SYNC
+decode_state_t TagDispatcher::decode_afpacket(
+ const std::vector<uint8_t> &input_data)
+{
+ if (input_data.size() < AFPACKET_HEADER_LEN) {
+ return {false, 0};
+ }
+
+ // read length from packet
+ uint32_t taglength = read_32b(input_data.begin() + 2);
+ uint16_t seq = read_16b(input_data.begin() + 6);
+
+ const size_t crclength = 2;
+ if (input_data.size() < AFPACKET_HEADER_LEN + taglength + crclength) {
+ return {false, 0};
+ }
+
+ if (m_last_seq + 1 != seq) {
+ etiLog.level(warn) << "EDI AF Packet sequence error, " << seq;
+ }
+ m_last_seq = seq;
+
+ bool has_crc = (input_data[8] & 0x80) ? true : false;
+ uint8_t major_revision = (input_data[8] & 0x70) >> 4;
+ uint8_t minor_revision = input_data[8] & 0x0F;
+ if (major_revision != 1 or minor_revision != 0) {
+ throw invalid_argument("EDI AF Packet has wrong revision " +
+ to_string(major_revision) + "." + to_string(minor_revision));
+ }
+ uint8_t pt = input_data[9];
+ if (pt != 'T') {
+ // only support Tag
+ return {false, 0};
+ }
+
+
+ if (not has_crc) {
+ throw invalid_argument("AF packet not supported, has no CRC");
+ }
+
+ uint16_t crc = 0xffff;
+ for (size_t i = 0; i < AFPACKET_HEADER_LEN + taglength; i++) {
+ crc = crc16(crc, &input_data[i], 1);
+ }
+ crc ^= 0xffff;
+
+ uint16_t packet_crc = read_16b(input_data.begin() + AFPACKET_HEADER_LEN + taglength);
+
+ if (packet_crc != crc) {
+ throw invalid_argument(
+ "AF Packet crc wrong");
+ }
+ else {
+ vector<uint8_t> payload(taglength);
+ copy(input_data.begin() + AFPACKET_HEADER_LEN,
+ input_data.begin() + AFPACKET_HEADER_LEN + taglength,
+ payload.begin());
+
+ return {decode_tagpacket(payload),
+ AFPACKET_HEADER_LEN + taglength + 2};
+ }
+}
+
+void TagDispatcher::register_tag(const std::string& tag, tag_handler&& h)
+{
+ m_handlers[tag] = move(h);
+}
+
+
+bool TagDispatcher::decode_tagpacket(const vector<uint8_t> &payload)
+{
+ size_t length = 0;
+
+ bool success = true;
+
+ for (size_t i = 0; i + 8 < payload.size(); i += 8 + length) {
+ char tag_sz[5];
+ tag_sz[4] = '\0';
+ copy(payload.begin() + i, payload.begin() + i + 4, tag_sz);
+
+ string tag(tag_sz);
+
+ uint32_t taglength = read_32b(payload.begin() + i + 4);
+
+ if (taglength % 8 != 0) {
+ etiLog.log(warn, "Invalid tag length!");
+ break;
+ }
+ taglength /= 8;
+
+ length = taglength;
+
+ vector<uint8_t> tag_value(taglength);
+ copy( payload.begin() + i+8,
+ payload.begin() + i+8+taglength,
+ tag_value.begin());
+
+ bool tagsuccess = false;
+ bool found = false;
+ for (auto tag_handler : m_handlers) {
+ if (tag_handler.first.size() == 4 and tag_handler.first == tag) {
+ found = true;
+ tagsuccess = tag_handler.second(tag_value, 0);
+ }
+ else if (tag_handler.first.size() == 3 and
+ tag.substr(0, 3) == tag_handler.first) {
+ found = true;
+ uint8_t n = tag_sz[3];
+ tagsuccess = tag_handler.second(tag_value, n);
+ }
+ else if (tag_handler.first.size() == 2 and
+ tag.substr(0, 2) == tag_handler.first) {
+ found = true;
+ uint16_t n = 0;
+ n = (uint16_t)(tag_sz[2]) << 8;
+ n |= (uint16_t)(tag_sz[3]);
+ tagsuccess = tag_handler.second(tag_value, n);
+ }
+ }
+
+ if (not found) {
+ etiLog.log(warn, "Ignoring unknown TAG %s", tag.c_str());
+ break;
+ }
+
+ if (not tagsuccess) {
+ etiLog.log(warn, "Error decoding TAG %s", tag.c_str());
+ success = tagsuccess;
+ break;
+ }
+ }
+
+ return success;
+}
+
+}
diff --git a/lib/edi/common.hpp b/lib/edi/common.hpp
new file mode 100644
index 0000000..1433004
--- /dev/null
+++ b/lib/edi/common.hpp
@@ -0,0 +1,88 @@
+/*
+ Copyright (C) 2019
+ Matthias P. Braendli, matthias.braendli@mpb.li
+
+ http://opendigitalradio.org
+
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along
+ with this program; if not, write to the Free Software Foundation, Inc.,
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#pragma once
+
+#include "PFT.hpp"
+#include <functional>
+#include <map>
+#include <string>
+#include <vector>
+#include <cstddef>
+#include <ctime>
+
+namespace EdiDecoder {
+
+struct frame_timestamp_t {
+ uint32_t seconds = 0;
+ uint32_t utco = 0;
+
+ std::string to_string() const;
+ time_t to_unix_epoch() const;
+};
+
+struct decode_state_t {
+ decode_state_t(bool _complete, size_t _num_bytes_consumed) :
+ complete(_complete), num_bytes_consumed(_num_bytes_consumed) {}
+ bool complete;
+ size_t num_bytes_consumed;
+};
+
+/* The TagDispatcher takes care of decoding EDI, with or without PFT, and
+ * will call functions when TAGs are encountered.
+ *
+ * PF packets are handed over to the PFT decoder, which will in turn return
+ * AF packets. AF packets are directly dispatched to the TAG functions.
+ */
+class TagDispatcher {
+ public:
+ TagDispatcher(std::function<void()>&& af_packet_completed, bool verbose);
+
+ /* Push bytes into the decoder. The buf can contain more
+ * than a single packet. This is useful when reading from streams
+ * (files, TCP)
+ */
+ void push_bytes(const std::vector<uint8_t> &buf);
+
+ /* Push a complete packet into the decoder. Useful for UDP and other
+ * datagram-oriented protocols.
+ */
+ void push_packet(const std::vector<uint8_t> &buf);
+
+ /* Set the maximum delay in number of AF Packets before we
+ * abandon decoding a given pseq.
+ */
+ void setMaxDelay(int num_af_packets);
+
+ using tag_handler = std::function<bool(std::vector<uint8_t>, uint16_t)>;
+ void register_tag(const std::string& tag, tag_handler&& h);
+
+ private:
+ decode_state_t decode_afpacket(const std::vector<uint8_t> &input_data);
+ bool decode_tagpacket(const std::vector<uint8_t> &payload);
+
+ PFT::PFT m_pft;
+ uint16_t m_last_seq = 0;
+ std::vector<uint8_t> m_input_data;
+ std::map<std::string, tag_handler> m_handlers;
+ std::function<void()> m_af_packet_completed;
+};
+
+}