Add Packet File input

3 files changed, 280 insertions, 35 deletions

diff --git a/src/ConfigParser.cpp b/src/ConfigParser.cpp
index 3167d49..1ed1bac 100644
--- a/src/ConfigParser.cpp
+++ b/src/ConfigParser.cpp
@@ -1003,6 +1003,13 @@ static void setup_subchannel_from_ptree(DabSubchannel* subchan,
             etiLog.level(warn) << "uid " << subchanuid << " of type Dmb uses RAW input";
         }
     }
+    else if (type == "packet" or type == "enhancedpacket") {
+        subchan->type = subchannel_type_t::Packet;
+        subchan->bitrate = DEFAULT_PACKET_BITRATE;
+
+        bool enhanced = (type == "enhancedpacket");
+        subchan->input = make_shared<Inputs::PacketFile>(enhanced);
+    }
     else {
         stringstream ss;
         ss << "Subchannel with uid " << subchanuid << " has unknown type!";
diff --git a/src/input/File.cpp b/src/input/File.cpp
index 732f2a2..5c61fd4 100644
--- a/src/input/File.cpp
+++ b/src/input/File.cpp
@@ -35,9 +35,31 @@
 
 #include "input/File.h"
 #include "mpeg.h"
+#include "ReedSolomon.h"
 
 namespace Inputs {
 
+#ifdef _WIN32
+#   pragma pack(push, 1)
+#endif
+struct packetHeader {
+    unsigned char addressHigh:2;
+    unsigned char last:1;
+    unsigned char first:1;
+    unsigned char continuityIndex:2;
+    unsigned char packetLength:2;
+    unsigned char addressLow;
+    unsigned char dataLength:7;
+    unsigned char command;
+}
+#ifdef _WIN32
+#   pragma pack(pop)
+#else
+__attribute((packed))
+#endif
+;
+
+
 int FileBase::open(const std::string& name)
 {
     m_fd = ::open(name.c_str(), O_RDONLY | O_BINARY);
@@ -50,6 +72,17 @@ int FileBase::open(const std::string& name)
     return 0;
 }
 
+int FileBase::setBitrate(int bitrate)
+{
+    if (bitrate <= 0) {
+        etiLog.log(error, "Invalid bitrate (%i)\n", bitrate);
+        return -1;
+    }
+
+    return bitrate;
+}
+
+
 int FileBase::close()
 {
     if (m_fd != -1) {
@@ -64,6 +97,40 @@ int FileBase::rewind()
     return ::lseek(m_fd, 0, SEEK_SET);
 }
 
+ssize_t FileBase::readFromFile(uint8_t* buffer, size_t size)
+{
+    ssize_t ret = read(m_fd, buffer, size);
+
+    if (ret == -1) {
+        etiLog.log(alert, "ERROR: Can't read file\n");
+        perror("");
+        return -1;
+    }
+
+    if (ret < (ssize_t)size) {
+        ssize_t sizeOut = ret;
+        etiLog.log(info, "reach end of file -> rewinding\n");
+        if (rewind() == -1) {
+            etiLog.log(alert, "ERROR: Can't rewind file\n");
+            return -1;
+        }
+
+        ret = read(m_fd, buffer + sizeOut, size - sizeOut);
+        if (ret == -1) {
+            etiLog.log(alert, "ERROR: Can't read file\n");
+            perror("");
+            return -1;
+        }
+
+        if (ret < (ssize_t)size) {
+            etiLog.log(alert, "ERROR: Not enough data in file\n");
+            return -1;
+        }
+    }
+
+    return size;
+}
+
 int MPEGFile::readFrame(uint8_t* buffer, size_t size)
 {
     int result;
@@ -177,49 +244,200 @@ int MPEGFile::setBitrate(int bitrate)
     return bitrate;
 }
 
-
 int RawFile::readFrame(uint8_t* buffer, size_t size)
 {
-    ssize_t ret = read(m_fd, buffer, size);
+    return readFromFile(buffer, size);
+}
 
-    if (ret == -1) {
-        etiLog.log(alert, "ERROR: Can't read file\n");
-        perror("");
-        return -1;
-    }
+PacketFile::PacketFile(bool enhancedPacketMode)
+{
+    m_enhancedPacketEnabled = enhancedPacketMode;
+}
 
-    if (ret < (ssize_t)size) {
-        ssize_t sizeOut = ret;
-        etiLog.log(info, "reach end of file -> rewinding\n");
-        if (rewind() == -1) {
-            etiLog.log(alert, "ERROR: Can't rewind file\n");
-            return -1;
-        }
+int PacketFile::readFrame(uint8_t* buffer, size_t size)
+{
+    size_t written = 0;
+    int length;
+    packetHeader* header;
+    int indexRow;
+    int indexCol;
 
-        ret = read(m_fd, buffer + sizeOut, size - sizeOut);
-        if (ret == -1) {
-            etiLog.log(alert, "ERROR: Can't read file\n");
-            perror("");
-            return -1;
+    while (written < size) {
+        if (m_enhancedPacketWaiting > 0) {
+            *buffer = 192 - m_enhancedPacketWaiting;
+            *buffer /= 22;
+            *buffer <<= 2;
+            *(buffer++) |= 0x03;
+            *(buffer++) = 0xfe;
+            indexCol = 188;
+            indexCol += (192 - m_enhancedPacketWaiting) / 12;
+            indexRow = 0;
+            indexRow += (192 - m_enhancedPacketWaiting) % 12;
+            for (int j = 0; j < 22; ++j) {
+                if (m_enhancedPacketWaiting == 0) {
+                    *(buffer++) = 0;
+                }
+                else {
+                    *(buffer++) = m_enhancedPacketData[indexRow][indexCol];
+                    if (++indexRow == 12) {
+                        indexRow = 0;
+                        ++indexCol;
+                    }
+                    --m_enhancedPacketWaiting;
+                }
+            }
+            written += 24;
+            if (m_enhancedPacketWaiting == 0) {
+                m_enhancedPacketLength = 0;
+            }
         }
+        else if (m_packetLength != 0) {
+            header = (packetHeader*)(&m_packetData[0]);
+            if (written + m_packetLength > (unsigned)size) {
+                memset(buffer, 0, 22);
+                buffer[22] = 0x60;
+                buffer[23] = 0x4b;
+                length = 24;
+            }
+            else if (m_enhancedPacketEnabled) {
+                if (m_enhancedPacketLength + m_packetLength > (12 * 188)) {
+                    memset(buffer, 0, 22);
+                    buffer[22] = 0x60;
+                    buffer[23] = 0x4b;
+                    length = 24;
+                }
+                else {
+                    std::copy(m_packetData.begin(),
+                            m_packetData.begin() + m_packetLength,
+                            buffer);
+                    length = m_packetLength;
+                    m_packetLength = 0;
+                }
+            }
+            else {
+                std::copy(m_packetData.begin(),
+                        m_packetData.begin() + m_packetLength,
+                        buffer);
+                length = m_packetLength;
+                m_packetLength = 0;
+            }
 
-        if (ret < (ssize_t)size) {
-            etiLog.log(alert, "ERROR: Not enough data in file\n");
-            return -1;
+            if (m_enhancedPacketEnabled) {
+                indexCol = m_enhancedPacketLength / 12;
+                indexRow = m_enhancedPacketLength % 12;     // TODO Check if always 0
+                for (int j = 0; j < length; ++j) {
+                    m_enhancedPacketData[indexRow][indexCol] = buffer[j];
+                    if (++indexRow == 12) {
+                        indexRow = 0;
+                        ++indexCol;
+                    }
+                }
+                m_enhancedPacketLength += length;
+                if (m_enhancedPacketLength >= (12 * 188)) {
+                    m_enhancedPacketLength = (12 * 188);
+                    ReedSolomon encoder(204, 188);
+                    for (int j = 0; j < 12; ++j) {
+                        encoder.encode(&m_enhancedPacketData[j][0], 188);
+                    }
+                    m_enhancedPacketWaiting = 192;
+                }
+            }
+            written += length;
+            buffer += length;
         }
-    }
+        else {
+            int nbBytes = readFromFile(buffer, 3);
+            header = (packetHeader*)buffer;
+            if (nbBytes == -1) {
+                if (errno == EAGAIN) goto END_PACKET;
+                perror("Packet file");
+                return -1;
+            }
+            else if (nbBytes == 0) {
+                if (rewind() == -1) {
+                    goto END_PACKET;
+                }
+                continue;
+            }
+            else if (nbBytes < 3) {
+                etiLog.log(error,
+                        "Error while reading file for packet header; "
+                        "read %i out of 3 bytes\n", nbBytes);
+                break;
+            }
 
-    return size;
-}
+            length = header->packetLength * 24 + 24;
+            if (written + length > size) {
+                memcpy(&m_packetData[0], header, 3);
+                readFromFile(&m_packetData[3], length - 3);
+                m_packetLength = length;
+                continue;
+            }
 
-int RawFile::setBitrate(int bitrate)
-{
-    if (bitrate <= 0) {
-        etiLog.log(error, "Invalid bitrate (%i)\n", bitrate);
-        return -1;
-    }
+            if (m_enhancedPacketEnabled) {
+                if (m_enhancedPacketLength + length > (12 * 188)) {
+                    memcpy(&m_packetData[0], header, 3);
+                    readFromFile(&m_packetData[3], length - 3);
+                    m_packetLength = length;
+                    continue;
+                }
+            }
 
-    return bitrate;
+            nbBytes = readFromFile(buffer + 3, length - 3);
+            if (nbBytes == -1) {
+                perror("Packet file");
+                return -1;
+            }
+            else if (nbBytes == 0) {
+                etiLog.log(info,
+                        "Packet header read, but no data!\n");
+                if (rewind() == -1) {
+                    goto END_PACKET;
+                }
+                continue;
+            }
+            else if (nbBytes < length - 3) {
+                etiLog.log(error, "Error while reading packet file; "
+                        "read %i out of %i bytes\n", nbBytes, length - 3);
+                break;
+            }
+
+            if (m_enhancedPacketEnabled) {
+                indexCol = m_enhancedPacketLength / 12;
+                indexRow = m_enhancedPacketLength % 12;     // TODO Check if always 0
+                for (int j = 0; j < length; ++j) {
+                    m_enhancedPacketData[indexRow][indexCol] = buffer[j];
+                    if (++indexRow == 12) {
+                        indexRow = 0;
+                        ++indexCol;
+                    }
+                }
+                m_enhancedPacketLength += length;
+                if (m_enhancedPacketLength >= (12 * 188)) {
+                    if (m_enhancedPacketLength > (12 * 188)) {
+                        etiLog.log(error,
+                                "Error, too much enhanced packet data!\n");
+                    }
+                    ReedSolomon encoder(204, 188);
+                    for (int j = 0; j < 12; ++j) {
+                        encoder.encode(&m_enhancedPacketData[j][0], 188);
+                    }
+                    m_enhancedPacketWaiting = 192;
+                }
+            }
+            written += length;
+            buffer += length;
+        }
+    }
+END_PACKET:
+    while (written < size) {
+        memset(buffer, 0, 22);
+        buffer[22] = 0x60;
+        buffer[23] = 0x4b;
+        buffer += 24;
+        written += 24;
+    }
+    return written;
 }
 
 };
diff --git a/src/input/File.h b/src/input/File.h
index 99e0a87..bf99748 100644
--- a/src/input/File.h
+++ b/src/input/File.h
@@ -26,6 +26,7 @@
 #pragma once
 
 #include <vector>
+#include <array>
 #include <string>
 #include <stdint.h>
 #include "input/inputs.h"
@@ -37,7 +38,7 @@ class FileBase : public InputBase {
     public:
         virtual int open(const std::string& name);
         virtual int readFrame(uint8_t* buffer, size_t size) = 0;
-        virtual int setBitrate(int bitrate) = 0;
+        virtual int setBitrate(int bitrate);
         virtual int close();
 
         /* Rewind the file
@@ -45,8 +46,13 @@ class FileBase : public InputBase {
          */
         virtual int rewind();
     protected:
+        /* Read len bytes from the file into buf, and return
+         * the number of bytes read, or -1 in case of error.
+         */
+        virtual ssize_t readFromFile(uint8_t* buf, size_t len);
+
         // We use unix open() instead of fopen() because
-        // we want to do non-blocking I/O
+        // we might want to do non-blocking I/O in the future
         int m_fd = -1;
 };
 
@@ -62,7 +68,21 @@ class MPEGFile : public FileBase {
 class RawFile : public FileBase {
     public:
         virtual int readFrame(uint8_t* buffer, size_t size);
-        virtual int setBitrate(int bitrate);
+};
+
+class PacketFile : public FileBase {
+    public:
+        PacketFile(bool enhancedPacketMode);
+        virtual int readFrame(uint8_t* buffer, size_t size);
+
+    protected:
+        std::array<uint8_t,96> m_packetData;
+        size_t m_packetLength;
+
+        bool m_enhancedPacketEnabled = false;
+        std::array<std::array<uint8_t, 204>,12> m_enhancedPacketData;
+        size_t m_enhancedPacketWaiting;
+        size_t m_enhancedPacketLength;
 };
 
 };