Rework frequency information representation

7 files changed, 477 insertions, 461 deletions

diff --git a/doc/servicelinking.mux b/doc/servicelinking.mux
index e210c75..25425a8 100644
--- a/doc/servicelinking.mux
+++ b/doc/servicelinking.mux
@@ -7,7 +7,7 @@
 ;
 general {
     dabmode 1
-    nbframes 0
+    nbframes 20000
 
     syslog false
     tist false
@@ -101,88 +101,75 @@ linking {
 }
 
 ; According to ETSI TR 101 496-2 Clause 3.6.10.
-; The configuration structure reflects the FIG 0/21 of
+; Each entry corresponds to one frequence information
+; database entry. The multiplexer then transmits this
+; information inside FIG 0/21 defined in
 ; ETSI EN 300 401 Clause 8.1.8
 frequency_information {
-    fi_1 {
+    fi_dab_4fff {
+        ; The database key comprises oe, range_modulation and
+        ; either eid, pi_code, drm_id or amss_id.
+        ; The database key must be unique among all the fi entries.
+        ; RegionId and signalling FI for data services are not implemented.
         oe false
-        fi_dab_1 {
-            range_modulation dab
-            continuity true
-            eid 0x4fff
-            frequencies {
-                ; In DAB, max 2 frequency entries
-                entry_a {
-                    signal_mode_1 true
-                    adjacent true
-                    frequency 234.208
-                }
-                entry_b {
-                    signal_mode_1 true
-                    adjacent true
-                    frequency 188.928
-                }
+        range_modulation dab
+        eid 0x4fff
+
+        continuity true
+        frequencies {
+            freq_a {
+                signal_mode_1 true
+                adjacent true
+                frequency 234.208
+            }
+            freq_b {
+                signal_mode_1 true
+                adjacent true
+                frequency 188.928
+            }
+            freq_c {
+                signal_mode_1 true
+                adjacent false
+                frequency 230.784
             }
         }
-        fi_fm_1 {
-            range_modulation fm
-            continuity true
-            pi_code 0x1234
-            ; in FM, max 7 entries
-            frequencies "87.6 105.2"
-        }
     }
-    fi_other_ensemble {
+    fi_fm_1234 {
+        oe false
+        range_modulation fm
+        pi_code 0x1234
+
+        continuity true
+        frequencies "87.6 105.2"
+    }
+    fi_dab_4fee {
         oe true
-        fi_dab_other {
-            range_modulation dab
-            continuity true
-            eid 0x4fee
-            frequencies {
-                entry_a {
-                    signal_mode_1 true
-                    adjacent true
-                    frequency 230.784
-                }
+        range_modulation dab
+        eid 0x4fee
+
+        continuity true
+        frequencies {
+            freq_a {
+                signal_mode_1 true
+                adjacent false
+                frequency 230.784
             }
         }
     }
-    fi_2 {
+    fi_drm_12ab45 {
         oe false
-        fi_drm_3 {
-            range_modulation drm
-            continuity true
-            drm_id 0x12ab45
-            frequencies "15.21 22.4"
-        }
-        fi_amss_4 {
-            range_modulation amss
-            continuity true
-            amss_id 0x33cc88
-            frequencies "14.8"
-        }
-    }
-}
+        range_modulation drm
+        drm_id 0x12ab45
 
-; For information about the ensemble, service, subchannels, components and outputs,
-; please see doc/example.mux and doc/advanced.mux
-ensemble {
-    id 0x4fff
-    ecc 0xec
-
-    local-time-offset auto
-    label "OpenDigitalRadio"
-    shortlabel "ODR"
-}
-
-services {
-    srv-fu {
-        id 0x8daa
-        label "Funk"
+        continuity true
+        frequencies "15.21 22.4"
     }
-    srv-ri {
-        id 0x8dab
-        label "Rick"
+    fi_amss_33cc88 {
+        range_modulation amss
+        amss_id 0x33cc88
+
+        continuity true
+        frequencies "14.8"
     }
 }
 
@@ -213,6 +200,28 @@ other-services {
     }
 }
 
+; For information about the ensemble, service, subchannels, components and outputs,
+; please see doc/example.mux and doc/advanced.mux
+ensemble {
+    id 0x4fff
+    ecc 0xec
+
+    local-time-offset auto
+    label "OpenDigitalRadio"
+    shortlabel "ODR"
+}
+
+services {
+    srv-fu {
+        id 0x8daa
+        label "Funk"
+    }
+    srv-ri {
+        id 0x8dab
+        label "Rick"
+    }
+}
+
 subchannels {
     sub-fu {
         type dabplus
diff --git a/src/ConfigParser.cpp b/src/ConfigParser.cpp
index 4aee3ac..d6bd553 100644
--- a/src/ConfigParser.cpp
+++ b/src/ConfigParser.cpp
@@ -163,8 +163,7 @@ static void parse_linkage(ptree& pt,
 }
 
 // Parse the FI section
-static void parse_freq_info(ptree& pt,
-        std::shared_ptr<dabEnsemble> ensemble)
+static void parse_freq_info(ptree& pt, std::shared_ptr<dabEnsemble> ensemble)
 {
     auto pt_frequency_information = pt.get_child_optional("frequency_information");
     if (pt_frequency_information)
@@ -173,168 +172,115 @@ static void parse_freq_info(ptree& pt,
             const string fi_uid = it_fi.first;
             const ptree pt_fi = it_fi.second;
 
-            auto fi = make_shared<FrequencyInformation>();
+            FrequencyInformation fi;
 
-            fi->uid = fi_uid;
-            fi->other_ensemble = pt_fi.get("oe", false);
+            fi.uid = fi_uid;
 
-            for (const auto& it_fle : pt_fi) {
-                const string fle_uid = it_fle.first;
-                if (fle_uid == "oe") {
-                    continue;
-                }
+            fi.other_ensemble = pt_fi.get("oe", false);
+            string rm_str = pt_fi.get("range_modulation", "");
+            if (rm_str == "dab") {
+                fi.rm = RangeModulation::dab_ensemble;
+            }
+            else if (rm_str == "fm") {
+                fi.rm = RangeModulation::fm_with_rds;
+            }
+            else if (rm_str == "drm") {
+                fi.rm = RangeModulation::drm;
+            }
+            else if (rm_str == "amss") {
+                fi.rm = RangeModulation::amss;
+            }
+            else if (rm_str == "") {
+                throw runtime_error("Missing range_modulation in FI " + fi_uid);
+            }
+            else {
+                throw runtime_error("Invalid range_modulation '" + rm_str +
+                        "' in FI " + fi_uid);
+            }
 
-                const ptree pt_entry = it_fle.second;
+            fi.continuity = pt_fi.get<bool>("continuity");
 
-                FrequencyListEntry fle;
+            try {
+                switch (fi.rm) {
+                    case RangeModulation::dab_ensemble:
+                        {
+                            fi.fi_dab.eid = hexparse(pt_fi.get<string>("eid"));
 
-                fle.uid = fle_uid;
+                            for (const auto& list_it : pt_fi.get_child("frequencies")) {
+                                const string fi_list_uid = list_it.first;
+                                const ptree pt_list_entry = list_it.second;
 
-                string rm_str = pt_entry.get("range_modulation", "");
-                if (rm_str == "dab") {
-                    fle.rm = RangeModulation::dab_ensemble;
-                }
-                else if (rm_str == "fm") {
-                    fle.rm = RangeModulation::fm_with_rds;
-                }
-                else if (rm_str == "drm") {
-                    fle.rm = RangeModulation::drm;
-                }
-                else if (rm_str == "amss") {
-                    fle.rm = RangeModulation::amss;
-                }
-                else if (rm_str == "") {
-                    throw runtime_error("Missing range_modulation in FI " + fi_uid);
-                }
-                else {
-                    throw runtime_error("Invalid range_modulation '" + rm_str +
-                            "' in FI " + fi_uid);
-                }
+                                FrequencyInfoDab::ListElement el;
+                                el.uid = fi_list_uid;
+                                el.frequency = pt_list_entry.get<float>("frequency");
 
-                fle.continuity = pt_entry.get<bool>("continuity");
+                                bool signal_mode_1 = pt_list_entry.get("signal_mode_1", false);
+                                bool adjacent = pt_list_entry.get("adjacent", false);
 
-                try {
-                    switch (fle.rm) {
-                        case RangeModulation::dab_ensemble:
-                            {
-                                fle.fi_dab.eid = hexparse(pt_entry.get<string>("eid"));
-
-                                for (const auto& list_it : pt_entry.get_child("frequencies")) {
-                                    const string fi_list_uid = list_it.first;
-                                    const ptree pt_list_entry = list_it.second;
-
-                                    FrequencyInfoDab::ListElement el;
-                                    el.uid = fi_list_uid;
-                                    el.frequency = pt_list_entry.get<float>("frequency");
-
-                                    bool signal_mode_1 = pt_list_entry.get("signal_mode_1", false);
-                                    bool adjacent = pt_list_entry.get("adjacent", false);
-
-                                    if (adjacent and signal_mode_1) {
-                                        el.control_field = FrequencyInfoDab::ControlField_e::adjacent_mode1;
-                                    }
-                                    else if (adjacent and not signal_mode_1) {
-                                        el.control_field = FrequencyInfoDab::ControlField_e::adjacent_no_mode;
-                                    }
-                                    if (not adjacent and signal_mode_1) {
-                                        el.control_field = FrequencyInfoDab::ControlField_e::disjoint_mode1;
-                                    }
-                                    else if (not adjacent and not signal_mode_1) {
-                                        el.control_field = FrequencyInfoDab::ControlField_e::disjoint_no_mode;
-                                    }
-                                    fle.fi_dab.frequencies.push_back(el);
-                                }
-                                if (fle.fi_dab.frequencies.size() > 2) {
-                                    throw runtime_error("Too many frequency entries in FI " + fle.uid);
-                                }
-                            } break;
-                        case RangeModulation::fm_with_rds:
-                            {
-                                fle.fi_fm.pi_code = hexparse(pt_entry.get<string>("pi_code"));
-
-                                std::stringstream frequencies_ss;
-                                frequencies_ss << pt_entry.get<string>("frequencies");
-                                for (std::string freq; std::getline(frequencies_ss, freq, ' '); ) {
-                                    fle.fi_fm.frequencies.push_back(std::stof(freq));
-                                }
-                                if (fle.fi_fm.frequencies.size() > 7) {
-                                    throw runtime_error("Too many frequency entries in FI " + fle.uid);
-                                }
-                            } break;
-                        case RangeModulation::drm:
-                            {
-                                fle.fi_drm.drm_service_id = hexparse(pt_entry.get<string>("drm_id"));
-
-                                std::stringstream frequencies_ss;
-                                frequencies_ss << pt_entry.get<string>("frequencies");
-                                for (std::string freq; std::getline(frequencies_ss, freq, ' '); ) {
-                                    fle.fi_drm.frequencies.push_back(std::stof(freq));
+                                if (adjacent and signal_mode_1) {
+                                    el.control_field = FrequencyInfoDab::ControlField_e::adjacent_mode1;
                                 }
-                                if (fle.fi_drm.frequencies.size() > 3) {
-                                    throw runtime_error("Too many frequency entries in FI " + fle.uid);
+                                else if (adjacent and not signal_mode_1) {
+                                    el.control_field = FrequencyInfoDab::ControlField_e::adjacent_no_mode;
                                 }
-                            } break;
-                        case RangeModulation::amss:
-                            {
-                                fle.fi_amss.amss_service_id = hexparse(pt_entry.get<string>("amss_id"));
-
-                                std::stringstream frequencies_ss;
-                                frequencies_ss << pt_entry.get<string>("frequencies");
-                                for (std::string freq; std::getline(frequencies_ss, freq, ' '); ) {
-                                    fle.fi_amss.frequencies.push_back(std::stof(freq));
+                                if (not adjacent and signal_mode_1) {
+                                    el.control_field = FrequencyInfoDab::ControlField_e::disjoint_mode1;
                                 }
-                                if (fle.fi_amss.frequencies.size() > 3) {
-                                    throw runtime_error("Too many frequency entries in FI " + fle.uid);
+                                else if (not adjacent and not signal_mode_1) {
+                                    el.control_field = FrequencyInfoDab::ControlField_e::disjoint_no_mode;
                                 }
-                            } break;
-                    } // switch(rm)
-                }
-                catch (const ptree_error &e) {
-                    throw runtime_error("invalid configuration for FI " + fle_uid);
-                }
-
-                fi->frequency_information.push_back(fle);
-
-            } // for over fle
-
-            size_t required_fi_size = 0; // Without RegionId + length of FI list
-            for (const auto& fle : fi->frequency_information) {
-                size_t list_entry_size = 3; // size of FIG0/21 FI list header
-                switch (fle.rm) {
-                    case RangeModulation::dab_ensemble:
-                        list_entry_size += fle.fi_dab.frequencies.size() * 3;
-                        break;
+                                fi.fi_dab.frequencies.push_back(el);
+                            }
+                            if (fi.fi_dab.frequencies.empty()) {
+                                throw runtime_error("Empty frequency list in FI " + fi.uid);
+                            }
+                        } break;
                     case RangeModulation::fm_with_rds:
-                        list_entry_size += fle.fi_fm.frequencies.size() * 1;
-                        break;
-                    case RangeModulation::amss:
-                        list_entry_size += 1; // Id field 2
-                        list_entry_size += fle.fi_amss.frequencies.size() * 2;
-                        break;
+                        {
+                            fi.fi_fm.pi_code = hexparse(pt_fi.get<string>("pi_code"));
+
+                            std::stringstream frequencies_ss;
+                            frequencies_ss << pt_fi.get<string>("frequencies");
+                            for (std::string freq; std::getline(frequencies_ss, freq, ' '); ) {
+                                fi.fi_fm.frequencies.push_back(std::stof(freq));
+                            }
+                            if (fi.fi_fm.frequencies.empty()) {
+                                throw runtime_error("Empty frequency list in FI " + fi.uid);
+                            }
+                        } break;
                     case RangeModulation::drm:
-                        list_entry_size += 1; // Id field 2
-                        list_entry_size += fle.fi_drm.frequencies.size() * 2;
-                        break;
-                }
-                required_fi_size += list_entry_size;
+                        {
+                            fi.fi_drm.drm_service_id = hexparse(pt_fi.get<string>("drm_id"));
+
+                            std::stringstream frequencies_ss;
+                            frequencies_ss << pt_fi.get<string>("frequencies");
+                            for (std::string freq; std::getline(frequencies_ss, freq, ' '); ) {
+                                fi.fi_drm.frequencies.push_back(std::stof(freq));
+                            }
+                            if (fi.fi_drm.frequencies.empty()) {
+                                throw runtime_error("Empty frequency list in FI " + fi.uid);
+                            }
+                        } break;
+                    case RangeModulation::amss:
+                        {
+                            fi.fi_amss.amss_service_id = hexparse(pt_fi.get<string>("amss_id"));
+
+                            std::stringstream frequencies_ss;
+                            frequencies_ss << pt_fi.get<string>("frequencies");
+                            for (std::string freq; std::getline(frequencies_ss, freq, ' '); ) {
+                                fi.fi_amss.frequencies.push_back(std::stof(freq));
+                            }
+                            if (fi.fi_amss.frequencies.empty()) {
+                                throw runtime_error("Empty frequency list in FI " + fi.uid);
+                            }
+                        } break;
+                } // switch(rm)
             }
-
-            /* Length of FI list: this 5-bit field, expressed as an unsigned
-             * binary number, shall represent the length in bytes of the field
-             * containing FI list q to FI list g (maximum 26 bytes).
-             *
-             * ETSI EN 300 401 Clause 8.1.8
-             */
-            if (required_fi_size > 26) {
-                throw runtime_error("Too much information carried in FI " +
-                        fi_uid + ": " + to_string(required_fi_size) +
-                        " bytes needed out of 26 available");
+            catch (const ptree_error &e) {
+                throw runtime_error("invalid configuration for FI " + fi_uid);
             }
 
-            ensemble->frequency_information.push_back(fi);
-
-
-
+            ensemble->frequency_information.emplace_back(move(fi));
         } // for over fi
 
         /* We sort all FI to have the OE=0 first and the OE=1 afterwards, to
@@ -342,10 +288,10 @@ static void parse_freq_info(ptree& pt,
         std::sort(
                 ensemble->frequency_information.begin(),
                 ensemble->frequency_information.end(),
-                [](const shared_ptr<FrequencyInformation>& first,
-                   const shared_ptr<FrequencyInformation>& second) {
-                    const int oe_first = first->other_ensemble ? 1 : 0;
-                    const int oe_second = second->other_ensemble ? 1 : 0;
+                [](const FrequencyInformation& first,
+                   const FrequencyInformation& second) {
+                    const int oe_first = first.other_ensemble ? 1 : 0;
+                    const int oe_second = second.other_ensemble ? 1 : 0;
                     return oe_first < oe_second;
                 } );
     } // if FI present
diff --git a/src/MuxElements.cpp b/src/MuxElements.cpp
index e07a49d..fe7043a 100644
--- a/src/MuxElements.cpp
+++ b/src/MuxElements.cpp
@@ -755,7 +755,8 @@ LinkageSet::LinkageSet(const std::string& name,
     lsn(lsn),
     active(true),
     hard(hard),
-    international(international)
+    international(international),
+    m_name(name)
 {}
 
 
diff --git a/src/MuxElements.h b/src/MuxElements.h
index 16db414..f28a687 100644
--- a/src/MuxElements.h
+++ b/src/MuxElements.h
@@ -251,7 +251,7 @@ class dabEnsemble : public RemoteControllable {
 
         std::vector<std::shared_ptr<AnnouncementCluster> > clusters;
         std::vector<std::shared_ptr<LinkageSet> > linkagesets;
-        std::vector<std::shared_ptr<FrequencyInformation> > frequency_information;
+        std::vector<FrequencyInformation> frequency_information;
         std::vector<ServiceOtherEnsembleInfo> service_other_ensemble;
 };
 
@@ -531,11 +531,17 @@ struct FrequencyInfoAmss {
     std::vector<float> frequencies;
 };
 
-struct FrequencyListEntry {
+/* One FI database entry. DB key are oe, rm, and idfield, which is
+ * inside the fi_XXX field
+ */
+struct FrequencyInformation {
     std::string uid;
+    bool other_ensemble = false;
 
-    RangeModulation rm;
-    bool continuity;
+    // The latest draft spec does not specify the RegionId anymore, it's
+    // now a reserved field.
+    RangeModulation rm = RangeModulation::dab_ensemble;
+    bool continuity = false;
 
     // Only one of those must contain information, which
     // must be consistent with RangeModulation
@@ -545,16 +551,6 @@ struct FrequencyListEntry {
     FrequencyInfoAmss fi_amss;
 };
 
-struct FrequencyInformation {
-    std::string uid;
-
-    bool other_ensemble = false;
-
-    // The latest draft spec does not specify the RegionId anymore, it's
-    // now a reserved field.
-    std::vector<FrequencyListEntry> frequency_information;
-};
-
 vec_sp_subchannel::iterator getSubchannel(
         vec_sp_subchannel& subchannels,
         int id);
diff --git a/src/fig/FIG0_21.cpp b/src/fig/FIG0_21.cpp
index 8f4f95b..5855fb1 100644
--- a/src/fig/FIG0_21.cpp
+++ b/src/fig/FIG0_21.cpp
@@ -83,6 +83,34 @@ FIG0_21::FIG0_21(FIGRuntimeInformation *rti) :
 {
 }
 
+static size_t get_num_frequencies(
+        std::vector<FrequencyInformation>::iterator fi)
+{
+    switch (fi->rm) {
+        case RangeModulation::dab_ensemble:
+            if (fi->fi_dab.frequencies.empty()) {
+                throw std::logic_error("Empty DAB FI");
+            }
+            return fi->fi_dab.frequencies.size();
+        case RangeModulation::fm_with_rds:
+            if (fi->fi_fm.frequencies.empty()) {
+                throw std::logic_error("Empty FM FI");
+            }
+            return fi->fi_fm.frequencies.size();
+        case RangeModulation::amss:
+            if (fi->fi_amss.frequencies.empty()) {
+                throw std::logic_error("Empty AMSS FI");
+            }
+            return fi->fi_amss.frequencies.size();
+        case RangeModulation::drm:
+            if (fi->fi_drm.frequencies.empty()) {
+                throw std::logic_error("Empty DRM FI");
+            }
+            return fi->fi_drm.frequencies.size();
+    }
+    throw logic_error("Unhandled get_num_frequencies");
+}
+
 FillStatus FIG0_21::fill(uint8_t *buf, size_t max_size)
 {
 #define FIG0_21_TRACE discard
@@ -90,75 +118,77 @@ FillStatus FIG0_21::fill(uint8_t *buf, size_t max_size)
     size_t remaining = max_size;
     auto ensemble = m_rti->ensemble;
 
-    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fill init " <<
-        (m_initialised ? 1 : 0) << " ********************************";
-
     if (not m_initialised) {
         freqInfoFIG0_21 = ensemble->frequency_information.begin();
+        fi_frequency_index = 0;
 
         if (freqInfoFIG0_21 != ensemble->frequency_information.end()) {
-            m_last_oe = (*freqInfoFIG0_21)->other_ensemble;
+            m_last_oe = freqInfoFIG0_21->other_ensemble;
         }
         m_initialised = true;
     }
 
     FIGtype0* fig0 = nullptr;
 
-    for (; freqInfoFIG0_21 != ensemble->frequency_information.end();
-            ++freqInfoFIG0_21) {
+    auto advance_loop = [&](void){
+            if (fi_frequency_index == get_num_frequencies(freqInfoFIG0_21)) {
+                ++freqInfoFIG0_21;
+                fi_frequency_index = 0;
+            }
+        };
 
+    for (; freqInfoFIG0_21 != ensemble->frequency_information.end();
+            advance_loop()) {
+        /* For better usage of FIC capacity, we want to transmit
+         * frequency lists with
+         * 2 DAB frequencies,
+         * 7 FM frequencies,
+         * 3 AM/DRM frequencies
+         */
+
+        // Check we have space for one frequency
         size_t required_fi_size = 2; // RegionId + length of FI list
-        for (const auto& fle : (*freqInfoFIG0_21)->frequency_information) {
-            size_t list_entry_size = sizeof(struct FIGtype0_21_fi_list_header);
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    list_entry_size += fle.fi_dab.frequencies.size() * 3;
-                    break;
-                case RangeModulation::fm_with_rds:
-                    list_entry_size += fle.fi_fm.frequencies.size() * 1;
-                    break;
-                case RangeModulation::amss:
-                    list_entry_size += 1; // Id field 2
-                    list_entry_size += fle.fi_amss.frequencies.size() * 2;
-                    break;
-                case RangeModulation::drm:
-                    list_entry_size += 1; // Id field 2
-                    list_entry_size += fle.fi_drm.frequencies.size() * 2;
-                    break;
-            }
-            required_fi_size += list_entry_size;
+        size_t list_entry_size = sizeof(struct FIGtype0_21_fi_list_header);
+        switch (freqInfoFIG0_21->rm) {
+            case RangeModulation::dab_ensemble:
+                list_entry_size += 3;
+                break;
+            case RangeModulation::fm_with_rds:
+                list_entry_size += 1;
+                break;
+            case RangeModulation::amss:
+                list_entry_size += 1; // Id field 2
+                list_entry_size += 2;
+                break;
+            case RangeModulation::drm:
+                list_entry_size += 1; // Id field 2
+                list_entry_size += 2;
+                break;
         }
+        required_fi_size += list_entry_size;
 
-        const size_t required_size = sizeof(struct FIGtype0_21_header) + required_fi_size;
+        const size_t required_size =
+            sizeof(struct FIGtype0_21_header) + required_fi_size;
 
-        etiLog.level(FIG0_21_TRACE) << "FIG0_21::loop " << (*freqInfoFIG0_21)->uid;
-
-        if (m_last_oe != (*freqInfoFIG0_21)->other_ensemble) {
+        if (m_last_oe != freqInfoFIG0_21->other_ensemble) {
             // Trigger resend of FIG0 when OE changes
             fig0 = nullptr;
-            m_last_oe = (*freqInfoFIG0_21)->other_ensemble;
+            m_last_oe = freqInfoFIG0_21->other_ensemble;
             etiLog.level(FIG0_21_TRACE) << "FIG0_21::switch OE to " <<
-                (*freqInfoFIG0_21)->other_ensemble;
+                freqInfoFIG0_21->other_ensemble;
         }
 
         if (fig0 == nullptr) {
             if (remaining < 2 + required_size) {
-                etiLog.level(FIG0_21_TRACE) << "FIG0_21::no space for fig0: remain " <<
-                    remaining << " require " << 2 + required_size;
                 break;
             }
-            else {
-                etiLog.level(FIG0_21_TRACE) << "FIG0_21::inserting fig0: remain " <<
-                    remaining << " require " << 2 + required_size;
-            }
             fig0 = (FIGtype0*)buf;
             fig0->FIGtypeNumber = 0;
             fig0->Length = 1;
 
             // Database start or continuation flag, EN 300 401 Clause 5.2.2.1 part b)
-            fig0->CN =
-                (freqInfoFIG0_21 == ensemble->frequency_information.begin() ? 0 : 1);
-            fig0->OE = (*freqInfoFIG0_21)->other_ensemble ? 1 : 0;
+            fig0->CN = (fi_frequency_index == 0 ? 0 : 1);
+            fig0->OE = freqInfoFIG0_21->other_ensemble ? 1 : 0;
             fig0->PD = false;
             fig0->Extension = 21;
 
@@ -166,149 +196,185 @@ FillStatus FIG0_21::fill(uint8_t *buf, size_t max_size)
             remaining -= 2;
         }
         else if (remaining < required_size) {
-            etiLog.level(FIG0_21_TRACE) << "FIG0_21::no space";
             break;
         }
 
+        etiLog.level(FIG0_21_TRACE) << "FIG0_21::loop " << freqInfoFIG0_21->uid << " " <<
+            std::distance(ensemble->frequency_information.begin(), freqInfoFIG0_21) <<
+            " freq entry " << fi_frequency_index;
+
         auto *fig0_21_header = (FIGtype0_21_header*)buf;
         fig0_21_header->rfaHigh = 0;
         fig0_21_header->rfaLow = 0;
-        fig0_21_header->length_fi = sizeof(struct FIGtype0_21_fi_list_header);
-
-        fig0->Length += sizeof(struct FIGtype0_21_header);
-        buf += sizeof(struct FIGtype0_21_header);
-        remaining -= sizeof(struct FIGtype0_21_header);
-
-        for (const auto& fle : (*freqInfoFIG0_21)->frequency_information) {
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    fig0_21_header->length_fi += 3 * fle.fi_dab.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr DAB " <<
-                        fle.fi_dab.frequencies.size();
-                    break;
-                case RangeModulation::fm_with_rds:
-                    fig0_21_header->length_fi += fle.fi_fm.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr FM " <<
-                        fle.fi_fm.frequencies.size();
-                    break;
-                case RangeModulation::drm:
-                    fig0_21_header->length_fi += 1; // ID field 2
-                    fig0_21_header->length_fi += 2*fle.fi_drm.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr DRM " <<
-                        fle.fi_drm.frequencies.size();
-                    break;
-                case RangeModulation::amss:
-                    fig0_21_header->length_fi += 1; // ID field 2
-                    fig0_21_header->length_fi += 2*fle.fi_amss.frequencies.size();
-                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle hdr AMSS " <<
-                        fle.fi_amss.frequencies.size();
-                    break;
-            }
-
-            auto *fi_list_header = (FIGtype0_21_fi_list_header*)buf;
-            fig0->Length += sizeof(struct FIGtype0_21_fi_list_header);
-            buf += sizeof(struct FIGtype0_21_fi_list_header);
-            remaining -= sizeof(struct FIGtype0_21_fi_list_header);
-
-            fi_list_header->continuity = fle.continuity;
-            fi_list_header->length_freq_list = 0;
-            fi_list_header->range_modulation = static_cast<uint8_t>(fle.rm);
-
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    fi_list_header->setId(fle.fi_dab.eid);
-                    assert(fle.fi_dab.frequencies.size() < 8);
-
-                    for (const auto& freq : fle.fi_dab.frequencies) {
-                        auto *field = (FIGtype0_21_fi_dab_entry*)buf;
-                        field->control_field = static_cast<uint8_t>(freq.control_field);
-                        field->setFreq(static_cast<uint32_t>(
-                                    freq.frequency * 1000.0f / 16.0f));
-
-                        fi_list_header->addToLength(3);
-                        fig0->Length += 3;
-                        buf += 3;
-                        remaining -= 3;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq " <<
-                            freq.frequency << " rem " << remaining;
+        fig0_21_header->length_fi = sizeof(FIGtype0_21_fi_list_header);
+
+        fig0->Length += sizeof(FIGtype0_21_header);
+        buf          += sizeof(FIGtype0_21_header);
+        remaining    -= sizeof(FIGtype0_21_header);
+
+        auto *fi_list_header = (FIGtype0_21_fi_list_header*)buf;
+        fig0->Length += sizeof(FIGtype0_21_fi_list_header);
+        buf          += sizeof(FIGtype0_21_fi_list_header);
+        remaining    -= sizeof(FIGtype0_21_fi_list_header);
+
+        fi_list_header->continuity = freqInfoFIG0_21->continuity;
+        fi_list_header->length_freq_list = 0;
+        fi_list_header->range_modulation = static_cast<uint8_t>(freqInfoFIG0_21->rm);
+
+        bool continue_loop = true;
+
+        switch (freqInfoFIG0_21->rm) {
+            case RangeModulation::dab_ensemble:
+                fi_list_header->setId(freqInfoFIG0_21->fi_dab.eid);
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 2 and
+                        i < freqInfoFIG0_21->fi_dab.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 3) {
+                        continue_loop = false;
+                        break;
                     }
-                    break;
-                case RangeModulation::fm_with_rds:
-                    fi_list_header->setId(fle.fi_fm.pi_code);
-
-                    for (const auto& freq : fle.fi_fm.frequencies) {
-                        // RealFreq = 87.5 MHz + (F * 100kHz)
-                        // => F = (RealFreq - 87.5 MHz) / 100kHz
-                        // Do the whole calculation in kHz:
-                        *buf = (freq * 1000.0f - 87500.0f) / 100.0f;
-
-                        fi_list_header->addToLength(1);
-                        fig0->Length += 1;
-                        buf += 1;
-                        remaining -= 1;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq " <<
-                            freq;
+
+                    const auto& freq = freqInfoFIG0_21->fi_dab.frequencies.at(i);
+
+                    auto *field = (FIGtype0_21_fi_dab_entry*)buf;
+                    field->control_field = static_cast<uint8_t>(freq.control_field);
+                    field->setFreq(static_cast<uint32_t>(
+                                freq.frequency * 1000.0f / 16.0f));
+
+                    fig0_21_header->length_fi += 3;
+                    fi_list_header->addToLength(3);
+                    fig0->Length += 3;
+                    buf += 3;
+                    remaining -= 3;
+                    etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq " <<
+                        fi_frequency_index << " " << freq.frequency << " rem " << remaining;
+                    fi_frequency_index++;
+                }
+                break;
+            case RangeModulation::fm_with_rds:
+                fi_list_header->setId(freqInfoFIG0_21->fi_fm.pi_code);
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 7 and
+                        i < freqInfoFIG0_21->fi_fm.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 1) {
+                        continue_loop = false;
+                        break;
                     }
-                    break;
-                case RangeModulation::drm:
-                    fi_list_header->setId((fle.fi_drm.drm_service_id) & 0xFFFF);
 
-                    // Id field 2
-                    *buf = (fle.fi_drm.drm_service_id >> 16) & 0xFF;
+                    const auto& freq = freqInfoFIG0_21->fi_fm.frequencies.at(i);
+                    // RealFreq = 87.5 MHz + (F * 100kHz)
+                    // => F = (RealFreq - 87.5 MHz) / 100kHz
+                    // Do the whole calculation in kHz:
+                    *buf = (freq * 1000.0f - 87500.0f) / 100.0f;
 
+                    fig0_21_header->length_fi += 1;
                     fi_list_header->addToLength(1);
                     fig0->Length += 1;
                     buf += 1;
                     remaining -= 1;
-
-                    for (const auto& freq : fle.fi_drm.frequencies) {
-                        uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
-                        buf[0] = freq_field >> 8;
-                        buf[1] = freq_field & 0xFF;
-
-                        fi_list_header->addToLength(2);
-                        fig0->Length += 2;
-                        buf += 2;
-                        remaining -= 2;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq_field " <<
-                            freq_field;
+                    fi_frequency_index++;
+                }
+                break;
+            case RangeModulation::drm:
+                fi_list_header->setId((freqInfoFIG0_21->fi_drm.drm_service_id) & 0xFFFF);
+
+                if (remaining < 3) {
+                    throw logic_error("Incorrect DRM FI size calculation");
+                }
+
+                // Id field 2
+                *buf = (freqInfoFIG0_21->fi_drm.drm_service_id >> 16) & 0xFF;
+                fig0_21_header->length_fi += 1;
+
+                fi_list_header->addToLength(1);
+                fig0->Length += 1;
+                buf += 1;
+                remaining -= 1;
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 3 and
+                        i < freqInfoFIG0_21->fi_drm.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 2) {
+                        continue_loop = false;
+                        break;
                     }
-                    break;
-                case RangeModulation::amss:
-                    fi_list_header->setId((fle.fi_amss.amss_service_id) & 0xFFFF);
 
-                    // Id field 2
-                    *buf = (fle.fi_amss.amss_service_id >> 16) & 0xFF;
+                    const auto& freq = freqInfoFIG0_21->fi_drm.frequencies.at(i);
+                    uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
+                    buf[0] = freq_field >> 8;
+                    buf[1] = freq_field & 0xFF;
+
+                    fi_list_header->addToLength(2);
+                    fig0_21_header->length_fi += 2;
+                    fig0->Length += 2;
+                    buf += 2;
+                    remaining -= 2;
+                    fi_frequency_index++;
+                }
+                break;
+            case RangeModulation::amss:
+                fi_list_header->setId((freqInfoFIG0_21->fi_amss.amss_service_id) & 0xFFFF);
+
+                if (remaining < 3) {
+                    throw logic_error("Incorrect AMSS FI size calculation");
+                }
+
+                // Id field 2
+                *buf = (freqInfoFIG0_21->fi_amss.amss_service_id >> 16) & 0xFF;
+                fig0_21_header->length_fi += 1;
+
+                fi_list_header->addToLength(1);
+                fig0->Length += 1;
+                buf += 1;
+                remaining -= 1;
+
+                for (size_t num_inserted = 0, i = fi_frequency_index;
+                        num_inserted < 3 and
+                        i < freqInfoFIG0_21->fi_amss.frequencies.size();
+                        num_inserted++, i++) {
+                    if (remaining < 2) {
+                        continue_loop = false;
+                        break;
+                    }
 
-                    fi_list_header->addToLength(1);
-                    fig0->Length += 1;
-                    buf += 1;
-                    remaining -= 1;
+                    const auto& freq = freqInfoFIG0_21->fi_amss.frequencies.at(i);
+                    uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
+                    buf[0] = freq_field >> 8;
+                    buf[1] = freq_field & 0xFF;
+
+                    fi_list_header->addToLength(2);
+                    fig0_21_header->length_fi += 2;
+                    fig0->Length += 2;
+                    buf += 2;
+                    remaining -= 2;
+                    fi_frequency_index++;
+                }
+                break;
+        } // switch (RM)
 
-                    for (const auto& freq : fle.fi_amss.frequencies) {
-                        uint16_t freq_field = static_cast<uint16_t>(freq * 1000.0f);
-                        buf[0] = freq_field >> 8;
-                        buf[1] = freq_field & 0xFF;
-
-                        fi_list_header->addToLength(2);
-                        fig0->Length += 2;
-                        buf += 2;
-                        remaining -= 2;
-                        etiLog.level(FIG0_21_TRACE) << "FIG0_21::freq_field " <<
-                            freq_field;
-                    }
-                    break;
-            } // switch (RM)
-            etiLog.level(FIG0_21_TRACE) << "FIG0_21::fle end len=" <<
-                static_cast<size_t>(fig0->Length) << " rem=" << remaining;
-        } // for over fle
-        etiLog.level(FIG0_21_TRACE) << "FIG0_21::next FI "
+        etiLog.level(FIG0_21_TRACE) << "FIG0_21::end " <<
+            (continue_loop ? "same " : "next ") << " len=" <<
+            static_cast<size_t>(fig0->Length) << " rem=" << remaining <<
             " ********************************";
+
+        if (not continue_loop) {
+            // There is no more space left. The freqInfoFIG0_21 iterator
+            // and fi_frequency_index save the position how to continue
+            // next time we are called.
+            break;
+        }
     } // for over FI
 
     if (freqInfoFIG0_21 == ensemble->frequency_information.end()) {
         fs.complete_fig_transmitted = true;
+
         freqInfoFIG0_21 = ensemble->frequency_information.begin();
+        fi_frequency_index = 0;
     }
 
     fs.num_bytes_written = max_size - remaining;
diff --git a/src/fig/FIG0_21.h b/src/fig/FIG0_21.h
index 018727a..0aabf3b 100644
--- a/src/fig/FIG0_21.h
+++ b/src/fig/FIG0_21.h
@@ -49,8 +49,8 @@ class FIG0_21 : public IFIG
         bool m_initialised = false;
         bool m_last_oe = false;
 
-        std::vector<std::shared_ptr<FrequencyInformation> >::iterator
-            freqInfoFIG0_21;
+        std::vector<FrequencyInformation>::iterator freqInfoFIG0_21;
+        size_t fi_frequency_index = 0;
 };
 
 }
diff --git a/src/utils.cpp b/src/utils.cpp
index d5130b3..33b8a98 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -99,7 +99,7 @@ void header_message()
     fprintf(stderr,
             "(Communications Research Centre Canada)\n\n");
     fprintf(stderr,
-            "Copyright (C) 2017 Matthias P. Braendli\n");
+            "Copyright (C) 2018 Matthias P. Braendli\n");
     fprintf(stderr,
             "LICENCE: GPLv3+\n\n");
     fprintf(stderr,
@@ -437,70 +437,68 @@ static void printFrequencyInformation(const shared_ptr<dabEnsemble>& ensemble)
         etiLog.level(info) << "  None ";
     }
     for (const auto& fi : ensemble->frequency_information) {
-        etiLog.level(info) << "  FI " << fi->uid;
-        etiLog.level(info) << "   OE=" << (fi->other_ensemble ? 1 : 0);
-        for (const auto& fle : fi->frequency_information) {
-            etiLog.level(info) << "    continuity " << (fle.continuity ? "true" : "false");
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    etiLog.level(info) << "    RM: DAB";
-                    etiLog.log(info,      "      EId 0x%04x", fle.fi_dab.eid);
-                    break;
-                case RangeModulation::drm:
-                    etiLog.level(info) << "    RM: DRM";
-                    etiLog.log(info,      "      DRM Id 0x%06x", fle.fi_drm.drm_service_id);
-                    break;
-                case RangeModulation::fm_with_rds:
-                    etiLog.level(info) << "    RM: FM (with RDS)";
-                    etiLog.log(info,      "      PI-Code 0x%04x", fle.fi_fm.pi_code);
-                    break;
-                case RangeModulation::amss:
-                    etiLog.level(info) << "    RM: AMSS";
-                    etiLog.log(info,      "      AMSS Id 0x%06x", fle.fi_amss.amss_service_id);
-                    break;
-            }
-
-            switch (fle.rm) {
-                case RangeModulation::dab_ensemble:
-                    for (const auto& f : fle.fi_dab.frequencies) {
-                        stringstream ss;
-                        ss << "      " << f.uid << " ";
-                        switch (f.control_field) {
-                            case FrequencyInfoDab::ControlField_e::adjacent_no_mode:
-                                ss << "Adjacent, w/o mode indication, ";
-                                break;
-                            case FrequencyInfoDab::ControlField_e::adjacent_mode1:
-                                ss << "Adjacent, mode I, ";
-                                break;
-                            case FrequencyInfoDab::ControlField_e::disjoint_no_mode:
-                                ss << "Disjoint, w/o mode indication, ";
-                                break;
-                            case FrequencyInfoDab::ControlField_e::disjoint_mode1:
-                                ss << "Disjoint, mode I, ";
-                                break;
-                        }
-                        ss << f.frequency;
-                        etiLog.level(info) << ss.str();
-                    }
-                    break;
-                case RangeModulation::drm:
-                    etiLog.log(info, "    ID 0x%02x", fle.fi_drm.drm_service_id);
-                    for (const auto& f : fle.fi_drm.frequencies) {
-                        etiLog.level(info) << "        " << f;
-                    }
-                    break;
-                case RangeModulation::fm_with_rds:
-                    for (const auto& f : fle.fi_fm.frequencies) {
-                        etiLog.level(info) << "        " << f;
-                    }
-                    break;
-                case RangeModulation::amss:
-                    etiLog.log(info, "    ID 0x%02x", fle.fi_amss.amss_service_id);
-                    for (const auto& f : fle.fi_amss.frequencies) {
-                        etiLog.level(info) << "        " << f;
+        etiLog.level(info) << "  FI " << fi.uid;
+        etiLog.level(info) << "   OE=" << (fi.other_ensemble ? 1 : 0);
+        switch (fi.rm) {
+            case RangeModulation::dab_ensemble:
+                etiLog.level(info) << "   RM: DAB";
+                etiLog.log(info,      "   EId 0x%04x", fi.fi_dab.eid);
+                break;
+            case RangeModulation::drm:
+                etiLog.level(info) << "   RM: DRM";
+                etiLog.log(info,      "   DRM Id 0x%06x", fi.fi_drm.drm_service_id);
+                break;
+            case RangeModulation::fm_with_rds:
+                etiLog.level(info) << "   RM: FM (with RDS)";
+                etiLog.log(info,      "   PI-Code 0x%04x", fi.fi_fm.pi_code);
+                break;
+            case RangeModulation::amss:
+                etiLog.level(info) << "   RM: AMSS";
+                etiLog.log(info,      "   AMSS Id 0x%06x", fi.fi_amss.amss_service_id);
+                break;
+        }
+        etiLog.level(info) << "   continuity " << (fi.continuity ? "true" : "false");
+
+        switch (fi.rm) {
+            case RangeModulation::dab_ensemble:
+                for (const auto& f : fi.fi_dab.frequencies) {
+                    stringstream ss;
+                    ss << "      " << f.uid << " ";
+                    switch (f.control_field) {
+                        case FrequencyInfoDab::ControlField_e::adjacent_no_mode:
+                            ss << "Adjacent, w/o mode indication, ";
+                            break;
+                        case FrequencyInfoDab::ControlField_e::adjacent_mode1:
+                            ss << "Adjacent, mode I, ";
+                            break;
+                        case FrequencyInfoDab::ControlField_e::disjoint_no_mode:
+                            ss << "Disjoint, w/o mode indication, ";
+                            break;
+                        case FrequencyInfoDab::ControlField_e::disjoint_mode1:
+                            ss << "Disjoint, mode I, ";
+                            break;
                     }
-                    break;
-            }
+                    ss << f.frequency;
+                    etiLog.level(info) << ss.str();
+                }
+                break;
+            case RangeModulation::drm:
+                etiLog.log(info, "    ID 0x%02x", fi.fi_drm.drm_service_id);
+                for (const auto& f : fi.fi_drm.frequencies) {
+                    etiLog.level(info) << "        " << f;
+                }
+                break;
+            case RangeModulation::fm_with_rds:
+                for (const auto& f : fi.fi_fm.frequencies) {
+                    etiLog.level(info) << "        " << f;
+                }
+                break;
+            case RangeModulation::amss:
+                etiLog.log(info, "    ID 0x%02x", fi.fi_amss.amss_service_id);
+                for (const auto& f : fi.fi_amss.frequencies) {
+                    etiLog.level(info) << "        " << f;
+                }
+                break;
         }
     }
 }