From cc8fa9a4d218c32b4ab987c75af927772a3c4b6d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 1 Jun 2023 13:41:30 +0200 Subject: [PATCH] Add work on OSNMA receiver --- src/core/libs/osnma_msg_receiver.cc | 72 +++++++++++++++++++--- src/core/libs/osnma_msg_receiver.h | 8 +++ src/core/system_parameters/Galileo_OSNMA.h | 6 +- src/core/system_parameters/osnma_data.h | 26 ++++++++ 4 files changed, 102 insertions(+), 10 deletions(-) diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 5ea504e4a..0e8996072 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -111,6 +111,7 @@ void osnma_msg_receiver::process_osnma_message(const std::shared_ptr& read_nma_header(osnma_msg->hkroot[0]); read_dsm_header(osnma_msg->hkroot[1]); read_dsm_block(osnma_msg); + read_mack_block(osnma_msg); } @@ -247,16 +248,34 @@ void osnma_msg_receiver::process_dsm_message(const std::vector& dsm_msg { d_osnma_data.d_dsm_kroot_message.kroot[k] = dsm_msg[13 + k]; } - uint16_t l_ds_bits; - const auto it3 = OSNMA_TABLE_15.find(""); - if (it3 != OSNMA_TABLE_15.cend()) + + std::string hash_function; + const auto it3 = OSNMA_TABLE_8.find(d_osnma_data.d_dsm_kroot_message.hf); + if (it3 != OSNMA_TABLE_8.cend()) { - l_ds_bits = it3->second; + hash_function = it3->second; } - // 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; + + uint16_t l_ds_bits = 0; + const auto it4 = OSNMA_TABLE_15.find(hash_function); + if (it4 != OSNMA_TABLE_15.cend()) + { + l_ds_bits = it4->second; + } + uint16_t l_ds_bytes = l_ds_bits / 8; + d_osnma_data.d_dsm_kroot_message.ds = std::vector(l_ds_bytes, 0); + for (uint32_t k = 0; k < l_ds_bytes; k++) + { + d_osnma_data.d_dsm_kroot_message.ds[k] = dsm_msg[13 + bytes_lk + k]; + } + uint16_t l_pdk_bytes = (l_dk_bits - 104 - lk_bits - l_ds_bits) / 8; + + d_osnma_data.d_dsm_kroot_message.p_dk = std::vector(l_pdk_bytes, 0); + for (uint32_t k = 0; k < l_ds_bytes; k++) + { + d_osnma_data.d_dsm_kroot_message.ds[k] = dsm_msg[13 + bytes_lk + l_ds_bytes + k]; + } + // validation? } else if (d_osnma_data.d_dsm_header.dsm_id >= 12 && d_osnma_data.d_dsm_header.dsm_id < 16) { @@ -308,4 +327,41 @@ void osnma_msg_receiver::process_dsm_message(const std::vector& dsm_msg // Reserved message? d_osnma_data = OSNMA_data(); } + d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] = 0; } + + +void osnma_msg_receiver::read_mack_block(const std::shared_ptr& osnma_msg) +{ + uint32_t index = 0; + for (size_t i = 0; i < osnma_msg->mack.size(); i++) + { + uint32_t value = osnma_msg->mack[i]; + d_mack_message[index] = static_cast((value & 0xFF000000) >> 6); + d_mack_message[index + 1] = static_cast((value & 0x00FF0000) >> 4); + d_mack_message[index + 2] = static_cast((value & 0x0000FF00) >> 2); + d_mack_message[index + 3] = static_cast(value & 0x000000FF); + index = index + 4; + } + read_mack_header(osnma_msg); + read_mack_info_and_tags(osnma_msg); + read_mack_key(osnma_msg); + read_mack_padding(osnma_msg); +} + +void osnma_msg_receiver::read_mack_header(const std::shared_ptr& osnma_msg) +{ +} + +void osnma_msg_receiver::read_mack_info_and_tags(const std::shared_ptr& osnma_msg) +{ +} + + +void osnma_msg_receiver::read_mack_key(const std::shared_ptr& osnma_msg) +{ +} + +void osnma_msg_receiver::read_mack_padding(const std::shared_ptr& osnma_msg) +{ +} \ No newline at end of file diff --git a/src/core/libs/osnma_msg_receiver.h b/src/core/libs/osnma_msg_receiver.h index 59c65bfe2..65bcac037 100644 --- a/src/core/libs/osnma_msg_receiver.h +++ b/src/core/libs/osnma_msg_receiver.h @@ -59,11 +59,19 @@ private: 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 read_mack_block(const std::shared_ptr& osnma_msg); void process_dsm_message(const std::vector& dsm_msg); + void read_mack_header(const std::shared_ptr& osnma_msg); + void read_mack_info_and_tags(const std::shared_ptr& osnma_msg); + void read_mack_key(const std::shared_ptr& osnma_msg); + void read_mack_padding(const std::shared_ptr& osnma_msg); + std::array, 16> d_dsm_message{}; std::array, 16> d_dsm_id_received{}; std::array d_number_of_blocks{}; + std::array d_mack_message{}; + OSNMA_data d_osnma_data{}; bool d_new_data{false}; }; diff --git a/src/core/system_parameters/Galileo_OSNMA.h b/src/core/system_parameters/Galileo_OSNMA.h index 200c722fe..6e0b1a6f8 100644 --- a/src/core/system_parameters/Galileo_OSNMA.h +++ b/src/core/system_parameters/Galileo_OSNMA.h @@ -124,8 +124,10 @@ const std::unordered_map OSNMA_TABLE_10 = { {15, 0}}; // key: ks, value: lk_bits const std::unordered_map OSNMA_TABLE_15 = { - {std::string("ECDSA P-256"), {512}}, - {std::string("ECDSA P-521"), {1056}}}; // key: ECDSA Curve and hash function, value: {l_ds_bits} + {std::string("ECDSA P-256"), 512}, + {std::string("ECDSA P-521"), 1056}, + {std::string("SHA-256"), 512}, + {std::string("SHA-512"), 1056}}; // key: ECDSA Curve and hash function, value: {l_ds_bits} #if __cplusplus == 201103L constexpr std::uint8_t mask_nmas{0xC0}; diff --git a/src/core/system_parameters/osnma_data.h b/src/core/system_parameters/osnma_data.h index 966e425ac..aa4eaf21f 100644 --- a/src/core/system_parameters/osnma_data.h +++ b/src/core/system_parameters/osnma_data.h @@ -42,6 +42,24 @@ struct dsm_header uint8_t dsm_block_id; }; +struct mack_header +{ + std::vector tag0; + uint16_t macsec; + uint8_t cop; +}; + +struct tag +{ + std::vector tag; + uint16_t tag_info; +}; + +struct tag_and_info +{ + std::vector tags; +}; + struct DSM_PKR_message { uint8_t nb_dp; @@ -73,6 +91,14 @@ struct DSM_KROOT_message std::vector p_dk; }; +struct MACK_message +{ + mack_header header; + tag_and_info tag_info; + std::vector key; + std::vector padding; +}; + /*! * \brief This class handles ONSMA data * See https://www.gsc-europa.eu/sites/default/files/sites/all/files/Galileo_OSNMA_User_ICD_for_Test_Phase_v1.0.pdf