From 0ba7b1611e449a0a7d507d6b0fd855849f037427 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 25 May 2023 07:48:27 +0200 Subject: [PATCH] Add work on retrieving OSNMA data --- src/core/libs/osnma_msg_receiver.cc | 244 +++++++++++++++++++++++- src/core/libs/osnma_msg_receiver.h | 10 +- src/core/system_parameters/osnma_data.h | 40 +++- 3 files changed, 281 insertions(+), 13 deletions(-) diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 756b89b5b..759c2db0b 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -18,14 +18,18 @@ #include "osnma_msg_receiver.h" -#include "display.h" // for colors in terminal #include "gnss_sdr_make_unique.h" // for std::make_unique in C++11 #include // for DLOG #include // for gr::io_signature::make #include +#include #include #include +#include +#include #include // for typeid +#include +#include #if HAS_GENERIC_LAMBDA #else @@ -40,6 +44,96 @@ namespace wht = boost; namespace wht = std; #endif + +// OSNMA SIS ICD v1.0 Table 3 +const std::unordered_map> OSNMA_TABLE_3 = { + {0, {0, 0}}, + {1, {0, 0}}, + {2, {0, 0}}, + {3, {0, 0}}, + {4, {0, 0}}, + {5, {0, 0}}, + {6, {0, 0}}, + {7, {13, 1352}}, + {8, {14, 1456}}, + {9, {15, 1560}}, + {10, {16, 1664}}, + {11, {0, 0}}, + {12, {0, 0}}, + {13, {0, 0}}, + {14, {0, 0}}, + {15, {0, 0}}}; // key: nb_dp, value: {num_blocks, l_dp_bits} + +const std::unordered_map OSNMA_TABLE_5 = { + {0, std::string("Reserved")}, + {1, std::string("ECDSA P-256")}, + {2, std::string("Reserved")}, + {3, std::string("ECDSA P-521")}, + {4, std::string("OAM")}, + {5, std::string("Reserved")}, + {6, std::string("Reserved")}, + {7, std::string("Reserved")}, + {8, std::string("Reserved")}, + {9, std::string("Reserved")}, + {10, std::string("Reserved")}, + {11, std::string("Reserved")}, + {12, std::string("Reserved")}, + {13, std::string("Reserved")}, + {14, std::string("Reserved")}, + {15, std::string("Reserved")}}; // key: nptk, value: message + +const std::unordered_map OSNMA_TABLE_6 = { + {std::string("ECDSA P-256"), 264}, + {std::string("ECDSA P-521"), 536}}; + +// OSNMA SIS ICD v1.0 Table 7 +const std::unordered_map> OSNMA_TABLE_7 = { + {0, {0, 0}}, + {1, {7, 728}}, + {2, {8, 832}}, + {3, {9, 936}}, + {4, {10, 1040}}, + {5, {11, 1144}}, + {6, {12, 1248}}, + {7, {13, 1352}}, + {8, {14, 1456}}, + {9, {0, 0}}, + {10, {0, 0}}, + {11, {0, 0}}, + {12, {0, 0}}, + {13, {0, 0}}, + {14, {0, 0}}, + {15, {0, 0}}}; // key: nb_dk, value: {num_blocks, l_dk_bits} + +const std::unordered_map OSNMA_TABLE_8 = { + {0, std::string("SHA-256")}, + {1, std::string("Reserved")}, + {2, std::string("SHA3-256")}, + {3, std::string("Reserved")}}; // key: hs, value: hash_function + +const std::unordered_map OSNMA_TABLE_10 = { + {0, 96}, + {1, 104}, + {2, 112}, + {3, 120}, + {4, 128}, + {5, 160}, + {6, 192}, + {7, 224}, + {8, 256}, + {9, 0}, + {10, 0}, + {11, 0}, + {12, 0}, + {13, 0}, + {15, 0}, + {15, 0}}; // key: ks, value: lk_bits + +const std::unordered_map> OSNMA_TABLE_15 = { + {std::string("ECDSA P-256"), {512, 256}}, + {std::string("ECDSA P-521"), {1059, 521}}}; // key: ECDSA Curve and hash function, value: {l_ds_bits, key_lenght_bits} + + osnma_msg_receiver_sptr osnma_msg_receiver_make() { return osnma_msg_receiver_sptr(new osnma_msg_receiver()); @@ -101,11 +195,12 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg) } -void osnma_msg_receiver::process_osnma_message(std::shared_ptr osnma_msg) +void osnma_msg_receiver::process_osnma_message(const std::shared_ptr& osnma_msg) { const auto hkroot_msg = osnma_msg->hkroot; read_nma_header(hkroot_msg[0]); read_dsm_header(hkroot_msg[1]); + read_dsm_block(osnma_msg); } @@ -122,4 +217,147 @@ void osnma_msg_receiver::read_dsm_header(uint8_t dsm_header) { d_osnma_data.d_dsm_header.dsm_id = (dsm_header & 0b11110000) >> 4; d_osnma_data.d_dsm_header.dsm_block_id = dsm_header & 0b00001111; // BID -} \ No newline at end of file +} + + +void osnma_msg_receiver::read_dsm_block(const std::shared_ptr& osnma_msg) +{ + size_t i = 0; + for (auto it = osnma_msg->hkroot.cbegin() + 2; it != osnma_msg->hkroot.cend(); ++it) + { + d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][13 * d_osnma_data.d_dsm_header.dsm_block_id + i] = *it; + i++; + } + if (d_osnma_data.d_dsm_header.dsm_block_id == 0) + { + // Get number of blocks in message + uint8_t nb = (osnma_msg->hkroot[2] & 0b11110000) >> 4; + uint16_t number_of_blocks = 0; + if (d_osnma_data.d_dsm_header.dsm_id >= 0 && d_osnma_data.d_dsm_header.dsm_id < 12) + { + // Table 3 + const auto it = OSNMA_TABLE_3.find(nb); + if (it != OSNMA_TABLE_3.cend()) + { + number_of_blocks = it->second.first; + } + } + else if (d_osnma_data.d_dsm_header.dsm_id >= 12 && d_osnma_data.d_dsm_header.dsm_id < 16) + { + // Table 7 + const auto it = OSNMA_TABLE_7.find(nb); + if (it != OSNMA_TABLE_7.cend()) + { + number_of_blocks = it->second.first; + } + } + + d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] = number_of_blocks; + } + // Annotate bid + d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id][d_osnma_data.d_dsm_header.dsm_block_id] = 1; + // is message complete? -> process_dsm_message(osnma_msg) + if ((d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] != 0) && (d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] == std::accumulate(d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].begin(), d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].end(), 0))) + { + std::vector dsm_msg(std::size_t(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]) * 13, 0); + for (uint32_t i = 0; i < d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]; i++) + { + for (uint32_t j = 0; j < 14; j++) + { + dsm_msg[i * 13 + j] = d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][i * 13 + j]; + } + } + process_dsm_message(osnma_msg, dsm_msg); + } +} + + +void osnma_msg_receiver::process_dsm_message(const std::shared_ptr& osnma_msg, const std::vector& dsm_msg) +{ + if (d_osnma_data.d_dsm_header.dsm_id >= 0 && d_osnma_data.d_dsm_header.dsm_id < 12) + { + // DSM-KROOT message + d_osnma_data.d_dsm_kroot_message.nb_dk = (dsm_msg[0] & 0b11110000) >> 4; + d_osnma_data.d_dsm_kroot_message.pkid = (dsm_msg[0] & 0b00001111); + d_osnma_data.d_dsm_kroot_message.cidkr = (dsm_msg[1] & 0b11000000) >> 6; + d_osnma_data.d_dsm_kroot_message.reserved1 = (dsm_msg[1] & 0b00110000) >> 4; + d_osnma_data.d_dsm_kroot_message.hf = (dsm_msg[1] & 0b00001100) >> 2; + + d_osnma_data.d_dsm_kroot_message.mf = (dsm_msg[1] & 0b00000011); + d_osnma_data.d_dsm_kroot_message.ks = (dsm_msg[2] & 0b11110000) >> 4; + d_osnma_data.d_dsm_kroot_message.ts = (dsm_msg[2] & 0b00001111); + d_osnma_data.d_dsm_kroot_message.maclt = dsm_msg[3]; + d_osnma_data.d_dsm_kroot_message.reserved = (dsm_msg[4] & 0b11110000) >> 4; + d_osnma_data.d_dsm_kroot_message.wn_k = static_cast((dsm_msg[4] & 0b00001111) << 8) + + static_cast(dsm_msg[5]); + d_osnma_data.d_dsm_kroot_message.towh_k = dsm_msg[6]; + + uint32_t lk = 0; + const auto it = OSNMA_TABLE_10.find(d_osnma_data.d_dsm_kroot_message.ks); + if (it != OSNMA_TABLE_10.cend()) + { + lk = it->second; + } + + d_osnma_data.d_dsm_kroot_message.alpha = (static_cast(dsm_msg[7]) << 40) + + (static_cast(dsm_msg[8]) << 32) + + (static_cast(dsm_msg[9]) << 24) + + (static_cast(dsm_msg[10]) << 16) + + (static_cast(dsm_msg[11]) << 8) + + static_cast(dsm_msg[12]); + uint32_t bytes_lk = lk / 8; + d_osnma_data.d_dsm_kroot_message.kroot = std::vector(bytes_lk, 0); + for (uint32_t k = 0; k < bytes_lk; k++) + { + d_osnma_data.d_dsm_kroot_message.kroot[k] = dsm_msg[13 + k]; + } + // uint32_t l_ld = 0; + // const auto it2 = OSNMA_TABLE_5.find() + // d_osnma_data.d_dsm_kroot_message.ds = "0"; + // d_osnma_data.d_dsm_kroot_message.p_dk; + } + else if (d_osnma_data.d_dsm_header.dsm_id >= 12 && d_osnma_data.d_dsm_header.dsm_id < 16) + { + // DSM-PKR message + d_osnma_data.d_dsm_pkr_message.nb_dp = (dsm_msg[0] & 0b11110000) >> 4; + d_osnma_data.d_dsm_pkr_message.mid = (dsm_msg[0] & 0b00001111); + for (int k = 0; k > 128; k++) + { + d_osnma_data.d_dsm_pkr_message.itn[k] = dsm_msg[k + 1]; + } + + d_osnma_data.d_dsm_pkr_message.npkt = (dsm_msg[129] & 0b11110000) >> 4; + d_osnma_data.d_dsm_pkr_message.npktid = (dsm_msg[129] & 0b00001111); + + // Table 5 + uint32_t l_npk = 0; + const auto it = OSNMA_TABLE_5.find(d_osnma_data.d_dsm_pkr_message.npkt); + if (it != OSNMA_TABLE_5.cend()) + { + const auto it2 = OSNMA_TABLE_6.find(it->second); + if (it2 != OSNMA_TABLE_6.cend()) + { + l_npk = it2->second / 8; + } + } + + if (d_osnma_data.d_dsm_pkr_message.npkt == 4) + { + // OAM + l_npk = 0; // ? + } + + d_osnma_data.d_dsm_pkr_message.npk = std::vector(l_npk, 0); + for (uint32_t k = 0; k > l_npk; k++) + { + d_osnma_data.d_dsm_pkr_message.npk[k] = dsm_msg[k + 130]; + } + uint32_t l_dp = dsm_msg.size(); + uint32_t l_pd = l_dp - 130 - l_npk; + d_osnma_data.d_dsm_pkr_message.p_dp = std::vector(l_pd, 0); + for (uint32_t k = 0; k > l_pd; k++) + { + d_osnma_data.d_dsm_pkr_message.p_dp[k] = dsm_msg[l_dp - l_pd + k]; + } + } +} diff --git a/src/core/libs/osnma_msg_receiver.h b/src/core/libs/osnma_msg_receiver.h index b9b51293b..ba51a874b 100644 --- a/src/core/libs/osnma_msg_receiver.h +++ b/src/core/libs/osnma_msg_receiver.h @@ -24,8 +24,9 @@ #include "osnma_data.h" // for OSNMA_data #include // for gr::block #include // for pmt::pmt_t +#include // for std::array #include // for std::shared_ptr - +#include /** \addtogroup Core * \{ */ @@ -54,10 +55,15 @@ private: osnma_msg_receiver(); void msg_handler_osnma(const pmt::pmt_t& msg); - void process_osnma_message(std::shared_ptr osnma_msg); + void process_osnma_message(const std::shared_ptr& osnma_msg); void read_nma_header(uint8_t nma_header); void read_dsm_header(uint8_t dsm_header); + void read_dsm_block(const std::shared_ptr& osnma_msg); + void process_dsm_message(const std::shared_ptr& osnma_msg, const std::vector& dsm_msg); + std::array, 16> d_dsm_message{}; + std::array, 16> d_dsm_id_received{}; + std::array d_number_of_blocks{}; OSNMA_data d_osnma_data{}; bool d_new_data{false}; }; diff --git a/src/core/system_parameters/osnma_data.h b/src/core/system_parameters/osnma_data.h index 3fa6b6232..966e425ac 100644 --- a/src/core/system_parameters/osnma_data.h +++ b/src/core/system_parameters/osnma_data.h @@ -42,6 +42,36 @@ struct dsm_header uint8_t dsm_block_id; }; +struct DSM_PKR_message +{ + uint8_t nb_dp; + uint8_t mid; + std::array itn; // bitset<1024> + uint8_t npkt; + uint8_t npktid; + std::vector npk; + std::vector p_dp; +}; + +struct DSM_KROOT_message +{ + uint8_t nb_dk; + uint8_t pkid; + uint8_t cidkr; + uint8_t reserved1; + uint8_t hf; + uint8_t mf; + uint8_t ks; + uint8_t ts; + uint8_t maclt; + uint8_t reserved; + uint16_t wn_k; + uint8_t towh_k; + uint64_t alpha; + std::vector kroot; + std::vector ds; + std::vector p_dk; +}; /*! * \brief This class handles ONSMA data @@ -51,16 +81,10 @@ class OSNMA_data { public: OSNMA_data() = default; - - std::string itn; // bitset<1024> - std::string npk; - std::string p_dp; nma_header d_nma_header; dsm_header d_dsm_header; - uint8_t nb_dp; - uint8_t mid; - uint8_t npkt; - uint8_t npktid; + DSM_PKR_message d_dsm_pkr_message; + DSM_KROOT_message d_dsm_kroot_message; };