1 files changed, 103 insertions, 5 deletions

diff --git a/src/fig/FIG0_9.cpp b/src/fig/FIG0_9.cpp
index 06f2125..cf73625 100644
--- a/src/fig/FIG0_9.cpp
+++ b/src/fig/FIG0_9.cpp
@@ -3,7 +3,7 @@
    2011, 2012 Her Majesty the Queen in Right of Canada (Communications
    Research Center Canada)
 
-   Copyright (C) 2016
+   Copyright (C) 2018
    Matthias P. Braendli, matthias.braendli@mpb.li
    */
 /*
@@ -26,6 +26,9 @@
 #include "fig/FIG0structs.h"
 #include "fig/FIG0_9.h"
 #include "utils.h"
+#include <map>
+
+using namespace std;
 
 namespace FIC {
 
@@ -44,6 +47,14 @@ struct FIGtype0_9 {
     uint8_t tableId;
 } PACKED;
 
+struct FIGtype0_9_Subfield {
+    uint8_t Rfa2:6;
+    uint8_t NumServices:2;
+
+    uint8_t ecc;
+    // Followed by list of NumServices uint16_t SIDs
+} PACKED;
+
 FIG0_9::FIG0_9(FIGRuntimeInformation *rti) :
     m_rti(rti) {}
 
@@ -53,7 +64,64 @@ FillStatus FIG0_9::fill(uint8_t *buf, size_t max_size)
     auto ensemble = m_rti->ensemble;
     size_t remaining = max_size;
 
-    if (remaining < 5) {
+    if (m_extended_fields.empty()) {
+        map<uint8_t, list<uint16_t> > ecc_to_services;
+        for (const auto& srv : ensemble->services) {
+            if (srv->ecc != 0 and srv->ecc != ensemble->ecc and
+                    srv->isProgramme(ensemble)) {
+                if (srv->id > 0xFFFF) {
+                    throw std::logic_error(
+                            "Service ID for programme service > 0xFFFF");
+                }
+                ecc_to_services[srv->ecc].push_back(srv->id);
+            }
+        }
+
+        // Reorganise the data so that it fits into the
+        // extended field. Max 3 SIds per sub-field.
+        for (auto& ecc_sids_pair : ecc_to_services) {
+            FIG0_9_Extended_Field ef;
+            ef.ecc = ecc_sids_pair.first;
+
+            size_t i = 0;
+            for (auto& sid : ecc_sids_pair.second) {
+                if (i == 3) {
+                    m_extended_fields.push_back(ef);
+                    ef.sids.clear();
+                    i = 0;
+                }
+
+                ef.sids.push_back(sid);
+                i++;
+            }
+
+            if (not ef.sids.empty()) {
+                m_extended_fields.push_back(ef);
+            }
+        }
+        m_current_extended_field = m_extended_fields.begin();
+
+        for (const auto& ef : m_extended_fields) {
+            stringstream ss;
+            ss << "FIG0_9 Ext ECC 0x" << hex << (int)ef.ecc << dec << ":";
+
+            for (auto& sid : ef.sids) {
+                ss << " " << hex << (int)sid << dec;
+            }
+            ss << ".";
+            etiLog.level(debug) << ss.str();
+        }
+    }
+
+    size_t required = 5;
+
+    // If we have services with different ECC, avoid transmitting a
+    // FIG0/9 without any extended field.
+    if (m_current_extended_field != m_extended_fields.end()) {
+        required += 2 + 2 * m_current_extended_field->sids.size();
+    }
+
+    if (remaining < required) {
         fs.num_bytes_written = 0;
         return fs;
     }
@@ -66,7 +134,7 @@ FillStatus FIG0_9::fill(uint8_t *buf, size_t max_size)
     fig0_9->PD = 0;
     fig0_9->Extension = 9;
 
-    fig0_9->ext = 0;
+    fig0_9->ext = m_extended_fields.empty() ? 0 : 1;
     fig0_9->rfa1 = 0; // Had a different meaning in EN 300 401 V1.4.1
 
     if (ensemble->lto_auto) {
@@ -89,10 +157,40 @@ FillStatus FIG0_9::fill(uint8_t *buf, size_t max_size)
     buf += 5;
     remaining -= 5;
 
-    /* No extended field, no support for services with different ECC */
+    while (m_current_extended_field != m_extended_fields.end() and
+            remaining > m_current_extended_field->required_bytes()) {
+        if (m_current_extended_field->sids.size() > 3) {
+            throw logic_error("Wrong FIG0/9 subfield generation");
+        }
+
+        auto subfield = (FIGtype0_9_Subfield*)buf;
+        subfield->NumServices = m_current_extended_field->sids.size();
+        subfield->Rfa2 = 0;
+        subfield->ecc = m_current_extended_field->ecc;
+        buf += 2;
+        fig0_9->Length += 2;
+        remaining -= 2;
+
+        for (uint16_t sid : m_current_extended_field->sids) {
+            uint16_t *sid_field = (uint16_t*)buf;
+            *sid_field = htons(sid);
+            buf += 2;
+            fig0_9->Length += 2;
+            remaining -= 2;
+        }
+
+        ++m_current_extended_field;
+    }
 
     fs.num_bytes_written = max_size - remaining;
-    fs.complete_fig_transmitted = true;
+
+    if (m_current_extended_field == m_extended_fields.end()) {
+        fs.complete_fig_transmitted = true;
+        m_current_extended_field = m_extended_fields.begin();
+    }
+    else {
+        fs.complete_fig_transmitted = false;
+    }
     return fs;
 }