Fix FIB0 padding, FIG init and deadline handling

5 files changed, 77 insertions, 31 deletions

diff --git a/src/DabMultiplexer.cpp b/src/DabMultiplexer.cpp
index 422f53d..db158da 100644
--- a/src/DabMultiplexer.cpp
+++ b/src/DabMultiplexer.cpp
@@ -634,7 +634,8 @@ void DabMultiplexer::mux_frame(std::vector<boost::shared_ptr<DabOutput> >& outpu
     bool new_fib0_carousel = m_pt.get("general.new_fib0_carousel", false);
     if (new_fib0_carousel) {
         // TODO update currentframe in rti
-        fig_carousel.fib0(&etiFrame[index], 30, currentFrame % 4);
+        figSize += fig_carousel.fib0(&etiFrame[index], 30, currentFrame % 4);
+        index += figSize;
     }
     // Skip creating a block for the else because
     // I don't want to reindent the whole switch block
diff --git a/src/fig/FIG.h b/src/fig/FIG.h
index 318b788..04a2cbe 100644
--- a/src/fig/FIG.h
+++ b/src/fig/FIG.h
@@ -63,7 +63,7 @@ enum class FIG_rate {
 inline int rate_increment_ms(FIG_rate rate)
 {
     switch (rate) {
-        case FIG_rate::FIG0_0:    return 0;        // Is a special case
+        case FIG_rate::FIG0_0:    return 96;        // Is a special case
         case FIG_rate::A:         return 100;
         case FIG_rate::B:         return 1000;
         case FIG_rate::C:         return 10000;
diff --git a/src/fig/FIG0.cpp b/src/fig/FIG0.cpp
index 67c462a..458116e 100644
--- a/src/fig/FIG0.cpp
+++ b/src/fig/FIG0.cpp
@@ -61,15 +61,19 @@ size_t FIG0_0::fill(uint8_t *buf, size_t max_size)
 //=========== FIG 0/1 ===========
 
 FIG0_1::FIG0_1(FIGRuntimeInformation *rti) :
-    m_rti(rti)
+    m_rti(rti),
+    m_initialised(false)
 {
-    subchannelFIG0_1 = m_rti->ensemble->subchannels.end();
 }
 
 size_t FIG0_1::fill(uint8_t *buf, size_t max_size)
 {
     size_t remaining = max_size;
 
+    if (not m_initialised) {
+        subchannelFIG0_1 = m_rti->ensemble->subchannels.end();
+    }
+
     if (max_size < 6) {
         return 0;
     }
@@ -155,9 +159,9 @@ size_t FIG0_1::fill(uint8_t *buf, size_t max_size)
 //=========== FIG 0/2 ===========
 
 FIG0_2::FIG0_2(FIGRuntimeInformation *rti) :
-    m_rti(rti)
+    m_rti(rti),
+    m_initialised(false)
 {
-    serviceFIG0_2 = m_rti->ensemble->services.end();
 }
 
 size_t FIG0_2::fill(uint8_t *buf, size_t max_size)
@@ -166,6 +170,10 @@ size_t FIG0_2::fill(uint8_t *buf, size_t max_size)
     int cur = 0;
     ssize_t remaining = max_size;
 
+    if (not m_initialised) {
+        serviceFIG0_2 = m_rti->ensemble->services.end();
+    }
+
     auto ensemble = m_rti->ensemble;
 
     // Rotate through the subchannels until there is no more
@@ -396,14 +404,19 @@ size_t FIG0_3::fill(uint8_t *buf, size_t max_size)
 //=========== FIG 0/17 ===========
 
 FIG0_17::FIG0_17(FIGRuntimeInformation *rti) :
-    m_rti(rti)
+    m_rti(rti),
+    m_initialised(false)
 {
-    serviceFIG0_17 = m_rti->ensemble->services.end();
 }
 
 size_t FIG0_17::fill(uint8_t *buf, size_t max_size)
 {
     ssize_t remaining = max_size;
+
+    if (not m_initialised) {
+        serviceFIG0_17 = m_rti->ensemble->services.end();
+    }
+
     auto ensemble = m_rti->ensemble;
 
     FIGtype0* fig0 = NULL;
diff --git a/src/fig/FIG0.h b/src/fig/FIG0.h
index 2c7c848..3f02890 100644
--- a/src/fig/FIG0.h
+++ b/src/fig/FIG0.h
@@ -38,7 +38,7 @@ class FIG0_0 : public IFIG
         FIG0_0(FIGRuntimeInformation* rti) :
             m_rti(rti) {}
         virtual size_t fill(uint8_t *buf, size_t max_size);
-        virtual FIG_rate repetition_rate(void) { return FIG_rate::A; }
+        virtual FIG_rate repetition_rate(void) { return FIG_rate::FIG0_0; }
 
         virtual const int figtype(void) const { return 0; }
         virtual const int figextension(void) const { return 0; }
@@ -61,6 +61,7 @@ class FIG0_1 : public IFIG
 
     private:
         FIGRuntimeInformation *m_rti;
+        bool m_initialised;
         std::vector<dabSubchannel*>::iterator subchannelFIG0_1;
 };
 
@@ -78,6 +79,7 @@ class FIG0_2 : public IFIG
 
     private:
         FIGRuntimeInformation *m_rti;
+        bool m_initialised;
         std::vector<std::shared_ptr<DabService> >::iterator serviceFIG0_2;
 };
 
@@ -111,6 +113,7 @@ class FIG0_17 : public IFIG
 
     private:
         FIGRuntimeInformation *m_rti;
+        bool m_initialised;
         std::vector<std::shared_ptr<DabService> >::iterator serviceFIG0_17;
 };
 
diff --git a/src/fig/FIGCarousel.cpp b/src/fig/FIGCarousel.cpp
index 7f3721b..021dd80 100644
--- a/src/fig/FIGCarousel.cpp
+++ b/src/fig/FIGCarousel.cpp
@@ -27,20 +27,17 @@
 */
 
 #include "fig/FIGCarousel.h"
+#include <boost/format.hpp>
 #include <iostream>
+#include <deque>
 
 /**************** FIGCarouselElement ****************/
 void FIGCarouselElement::reduce_deadline()
 {
     deadline -= 24; //ms
-}
-
-void FIGCarouselElement::increase_deadline()
-{
-    deadline += rate_increment_ms(fig->repetition_rate());
 
     std::cerr << "FIG " << fig->name() <<
-        " deadline: " << deadline << std::endl;
+        " deadline decreased to: " << deadline << std::endl;
 
     if (deadline < 0) {
         std::cerr << "FIG " << fig->name() <<
@@ -48,6 +45,14 @@ void FIGCarouselElement::increase_deadline()
     }
 }
 
+void FIGCarouselElement::increase_deadline()
+{
+    deadline += rate_increment_ms(fig->repetition_rate());
+
+    std::cerr << "FIG " << fig->name() <<
+        " deadline increased to: " << deadline << std::endl;
+}
+
 
 /**************** FIGCarousel *****************/
 
@@ -90,6 +95,7 @@ void FIGCarousel::allocate_fig_to_fib(int figtype, int extension, int fib)
         FIGCarouselElement el;
         el.fig = fig->second;
         el.deadline = 0;
+        el.increase_deadline();
         m_fibs[fib].push_back(el);
     }
     else {
@@ -99,46 +105,67 @@ void FIGCarousel::allocate_fig_to_fib(int figtype, int extension, int fib)
     }
 }
 
-size_t FIGCarousel::fib0(uint8_t *buf, size_t bufsize, int framephase) {
-    std::list<FIGCarouselElement> figs = m_fibs[0];
+void dumpfib(uint8_t *buf, size_t bufsize) {
+    std::cerr << "FIB ";
+    for (size_t i = 0; i < bufsize; i++) {
+        std::cerr << boost::format("%02x ") % (unsigned int)buf[i];
+    }
+    std::cerr << std::endl;
+}
+
+size_t FIGCarousel::fib0(uint8_t *fib, const size_t bufsize, int framephase) {
+
+    uint8_t *buf = fib;
+
+    std::list<FIGCarouselElement>& figs = m_fibs[0];
 
     std::cerr << "fib0(framephase=" << framephase << ")" << std::endl;
 
-    std::vector<FIGCarouselElement> sorted_figs;
+    std::deque<FIGCarouselElement*> sorted_figs;
 
     /* Decrement all deadlines */
     for (auto& fig_el : figs) {
         fig_el.reduce_deadline();
 
-        sorted_figs.push_back(fig_el);
+        sorted_figs.push_back(&fig_el);
     }
 
     /* Sort the FIGs in the FIB according to their deadline */
     std::sort(sorted_figs.begin(), sorted_figs.end(),
-            []( const FIGCarouselElement& left,
-                const FIGCarouselElement& right) {
-            return left.deadline < right.deadline;
+            []( const FIGCarouselElement* left,
+                const FIGCarouselElement* right) {
+            return left->deadline < right->deadline;
             });
 
+    std::cerr << "  Sorted figs:" << std::endl;
+    for (auto& fig_el : sorted_figs) {
+        std::cerr << "    " << fig_el->fig->name() <<
+            " d:" << fig_el->deadline << std::endl;
+    }
+
     /* Data structure to carry FIB */
     size_t available_size = bufsize;
 
     /* Take special care for FIG0/0 */
     auto fig0_0 = find_if(sorted_figs.begin(), sorted_figs.end(),
-            [](const FIGCarouselElement& f) {
-            return f.fig->repetition_rate() == FIG_rate::FIG0_0;
+            [](const FIGCarouselElement* f) {
+            std::cerr << "Check fig " << f->fig->name() << " " <<
+                rate_increment_ms(f->fig->repetition_rate()) << std::endl;
+            return f->fig->repetition_rate() == FIG_rate::FIG0_0;
             });
 
     if (fig0_0 != sorted_figs.end()) {
-        std::cerr << "Special FIG 0/0" << std::endl;
         sorted_figs.erase(fig0_0);
 
         if (framephase == 0) { // TODO check for all TM
-            size_t written = fig0_0->fig->fill(buf, available_size);
+            size_t written = (*fig0_0)->fig->fill(buf, available_size);
+            std::cerr << "Special FIG 0/0 wrote " <<
+                written << " bytes" << std::endl;
 
             if (written > 0) {
                 available_size -= written;
                 buf += written;
+                (*fig0_0)->increase_deadline();
             }
             else {
                 throw std::runtime_error("Failed to write FIG0/0");
@@ -146,15 +173,15 @@ size_t FIGCarousel::fib0(uint8_t *buf, size_t bufsize, int framephase) {
         }
     }
 
-    for (auto& fig : sorted_figs) {
-        std::cerr << "   FIG:" << fig.fig->name() << std::endl;
-    }
 
     /* Fill the FIB with the FIGs, taking the earliest deadline first */
     while (available_size > 0 and not sorted_figs.empty()) {
-        auto fig_el = sorted_figs.begin();
+        auto fig_el = sorted_figs[0];
         size_t written = fig_el->fig->fill(buf, available_size);
 
+        std::cerr << "   FIG " << fig_el->fig->name() <<
+           " wrote " << written << " bytes" << std::endl;
+
         if (written > 0) {
             available_size -= written;
             buf += written;
@@ -162,9 +189,11 @@ size_t FIGCarousel::fib0(uint8_t *buf, size_t bufsize, int framephase) {
             fig_el->increase_deadline();
         }
 
-        sorted_figs.erase(fig_el);
+        sorted_figs.pop_front();
     }
 
+    dumpfib(fib, bufsize);
+
     return bufsize - available_size;
 }