Rework frequency information representation

6 files changed, 398 insertions, 391 deletions

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;
         }
     }
 }