diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 684a06000..b34186da2 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -23,6 +23,7 @@ #include // for DLOG #include // for gr::io_signature::make #include +#include #include #include #include @@ -268,46 +269,54 @@ void osnma_msg_receiver::process_dsm_message(const std::vector& dsm_msg { d_osnma_data.d_dsm_kroot_message.p_dk[k] = dsm_msg[13 + bytes_lk + l_ds_bytes + k]; } - // validation of padding - std::vector M; - M.push_back(nma_header); - for (int i = 1; i < 13; i++) - { - M.push_back(dsm_msg[i]); - } - for (uint16_t i = 0; i < bytes_lk; i++) - { - M.push_back(dsm_msg[13 + i]); - } - for (uint16_t k = 0; k < l_ds_bytes; k++) - { - M.push_back(dsm_msg[13 + bytes_lk + k]); - } - std::vector hash = computeSHA256(M); - std::vector p_dk_computed; - for (uint16_t i = 0; i < l_pdk_bytes; i++) + uint16_t check_l_dk = 104 - std::ceil(1 + (((bytes_lk * 8) + l_ds_bits) / (104))); + if (l_dk_bits != check_l_dk) { - p_dk_computed.push_back(hash[i]); + std::cout << "OSNMA: Failed length reading" << std::endl; } - if (d_osnma_data.d_dsm_kroot_message.p_dk == p_dk_computed) + else { - std::cout << "OSNMA: DSM-KROOT message validated" << std::endl; + // validation of padding + std::vector M; + M.push_back(nma_header); + for (int i = 1; i < 13; i++) + { + M.push_back(dsm_msg[i]); + } + for (uint16_t i = 0; i < bytes_lk; i++) + { + M.push_back(dsm_msg[13 + i]); + } + for (uint16_t k = 0; k < l_ds_bytes; k++) + { + M.push_back(dsm_msg[13 + bytes_lk + k]); + } + + std::vector hash = computeSHA256(M); + std::vector p_dk_computed; + for (uint16_t i = 0; i < l_pdk_bytes; i++) + { + p_dk_computed.push_back(hash[i]); + } + if (d_osnma_data.d_dsm_kroot_message.p_dk == p_dk_computed) + { + std::cout << "OSNMA: DSM-KROOT message validated" << std::endl; + } } } else if (d_osnma_data.d_dsm_header.dsm_id >= 12 && d_osnma_data.d_dsm_header.dsm_id < 16) { LOG(WARNING) << "OSNMA: DSM-PKR message received."; // 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); + d_osnma_data.d_dsm_pkr_message.nb_dp = get_number_blocks_index(dsm_msg[0]); + d_osnma_data.d_dsm_pkr_message.mid = get_mid(dsm_msg); 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); + d_osnma_data.d_dsm_pkr_message.npkt = get_npkt(dsm_msg); + d_osnma_data.d_dsm_pkr_message.npktid = get_npktid(dsm_msg); uint32_t l_npk = 0; const auto it = OSNMA_TABLE_5.find(d_osnma_data.d_dsm_pkr_message.npkt); @@ -333,10 +342,19 @@ void osnma_msg_receiver::process_dsm_message(const std::vector& dsm_msg } 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++) + + uint32_t check_l_dp = 104 - std::ceil((1040 + l_npk * 8) / 104); + if (l_dp != check_l_dp) { - d_osnma_data.d_dsm_pkr_message.p_dp[k] = dsm_msg[l_dp - l_pd + k]; + std::cout << "OSNMA: Failed length reading" << std::endl; + } + else + { + 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]; + } } } else diff --git a/src/core/system_parameters/Galileo_OSNMA.h b/src/core/system_parameters/Galileo_OSNMA.h index a79c88db7..39f32752e 100644 --- a/src/core/system_parameters/Galileo_OSNMA.h +++ b/src/core/system_parameters/Galileo_OSNMA.h @@ -165,6 +165,9 @@ constexpr std::uint8_t mask_dsm_ks{0xF0}; constexpr std::uint8_t mask_dsm_ts{0x0F}; constexpr std::uint8_t mask_dsm_reserved{0xF0}; constexpr std::uint8_t mask_dsm_wk_k_msbyte{0x0F}; +constexpr std::uint8_t mask_dsm_mid{0x0F}; +constexpr std::uint8_t mask_dsm_npkt{0xF0}; +constexpr std::uint8_t mask_dsm_npktid{0x0F}; #else constexpr std::uint8_t mask_nmas{0b1100'0000}; constexpr std::uint8_t mask_cid{0b0011'0000}; @@ -182,6 +185,9 @@ constexpr std::uint8_t mask_dsm_ks{0b1111'0000}; constexpr std::uint8_t mask_dsm_ts{0b0000'1111}; constexpr std::uint8_t mask_dsm_reserved{0b1111'0000}; constexpr std::uint8_t mask_dsm_wk_k_msbyte{0b0000'1111}; +constexpr std::uint8_t mask_dsm_mid{0b0000'1111}; +constexpr std::uint8_t mask_dsm_npkt{0b1111'0000}; +constexpr std::uint8_t mask_dsm_npktid{0b0000'1111}; #endif // // hf = (dsm_msg[1] & 0b00001100) >> 2; @@ -328,12 +334,23 @@ std::string get_hash_function(uint8_t hf) } return hash_; } -// 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()) -// { -// hash_function = it3->second; -// } + +uint8_t get_mid(const std::vector& dsm_msg) +{ + return (dsm_msg[0] & mask_dsm_mid); +} + +uint8_t get_npkt(const std::vector& dsm_msg) +{ + return ((dsm_msg[129] & mask_dsm_npkt) >> 4); +} + +uint8_t get_npktid(const std::vector& dsm_msg) +{ + return (dsm_msg[129] & mask_dsm_npktid); +} + + /** \} */ /** \} */ #endif // GNSS_SDR_GALILEO_OSNMA_H \ No newline at end of file