diff --git a/src/core/libs/CMakeLists.txt b/src/core/libs/CMakeLists.txt index 1c0c6e811..cd6a03ed0 100644 --- a/src/core/libs/CMakeLists.txt +++ b/src/core/libs/CMakeLists.txt @@ -22,6 +22,7 @@ set(CORE_LIBS_SOURCES nav_message_udp_sink.cc galileo_tow_map.cc osnma_msg_receiver.cc + sha256.cc ) set(CORE_LIBS_HEADERS @@ -39,6 +40,7 @@ set(CORE_LIBS_HEADERS nav_message_monitor.h galileo_tow_map.h osnma_msg_receiver.h + sha256.h ) if(ENABLE_FPGA) diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index 89d4789c2..e33f79dee 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -176,9 +176,9 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr& osnma_ // 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) + // is message complete? -> Process DSM message 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))) + (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].cbegin(), d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].cend(), 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++) @@ -190,64 +190,35 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr& osnma_ } d_dsm_message[d_osnma_data.d_dsm_header.dsm_id] = std::array{}; d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id] = std::array{}; - process_dsm_message(dsm_msg); + process_dsm_message(dsm_msg, osnma_msg->hkroot[0]); } } -void osnma_msg_receiver::process_dsm_message(const std::vector& dsm_msg) +void osnma_msg_receiver::process_dsm_message(const std::vector& dsm_msg, uint8_t nma_header) { if (d_osnma_data.d_dsm_header.dsm_id < 12) { LOG(WARNING) << "OSNMA: DSM-KROOT message received."; // DSM-KROOT message d_osnma_data.d_dsm_kroot_message.nb_dk = get_number_blocks_index(dsm_msg[0]); - 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.pkid = get_pkid(dsm_msg); // (dsm_msg[0] & 0b00001111); + d_osnma_data.d_dsm_kroot_message.cidkr = get_cidkr(dsm_msg); // (dsm_msg[1] & 0b11000000) >> 6; + d_osnma_data.d_dsm_kroot_message.reserved1 = get_dsm_reserved1(dsm_msg); // (dsm_msg[1] & 0b00110000) >> 4; + d_osnma_data.d_dsm_kroot_message.hf = get_hf(dsm_msg); // (dsm_msg[1] & 0b00001100) >> 2; + d_osnma_data.d_dsm_kroot_message.mf = get_mf(dsm_msg); // (dsm_msg[1] & 0b00000011); + d_osnma_data.d_dsm_kroot_message.ks = get_ks(dsm_msg); // (dsm_msg[2] & 0b11110000) >> 4; + d_osnma_data.d_dsm_kroot_message.ts = get_ts(dsm_msg); // (dsm_msg[2] & 0b00001111); + d_osnma_data.d_dsm_kroot_message.maclt = get_maclt(dsm_msg); // dsm_msg[3]; + d_osnma_data.d_dsm_kroot_message.reserved = get_dsm_reserved(dsm_msg); // (dsm_msg[4] & 0b11110000) >> 4; + d_osnma_data.d_dsm_kroot_message.wn_k = get_wn_k(dsm_msg); // static_cast((dsm_msg[4] & 0b00001111) << 8) + static_cast(dsm_msg[5]); + d_osnma_data.d_dsm_kroot_message.towh_k = get_towh_k(dsm_msg); // dsm_msg[6]; + d_osnma_data.d_dsm_kroot_message.alpha = get_alpha(dsm_msg); - 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]; + uint16_t bytes_lk = get_lk_bits(d_osnma_data.d_dsm_kroot_message.ks) / 8; + d_osnma_data.d_dsm_kroot_message.kroot = get_kroot(dsm_msg, bytes_lk); - uint16_t l_dk_bits = 0; - const auto it = OSNMA_TABLE_7.find(d_osnma_data.d_dsm_kroot_message.nb_dk); - if (it != OSNMA_TABLE_7.cend()) - { - l_dk_bits = it->second.second; - } - uint16_t lk_bits = 0; - const auto it2 = OSNMA_TABLE_10.find(d_osnma_data.d_dsm_kroot_message.ks); - if (it2 != OSNMA_TABLE_10.cend()) - { - lk_bits = it2->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]); - uint16_t bytes_lk = lk_bits / 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]; - } - - 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; - } + std::string hash_function = get_hash_function(d_osnma_data.d_dsm_kroot_message.hf); uint16_t l_ds_bits = 0; const auto it4 = OSNMA_TABLE_15.find(hash_function); @@ -257,18 +228,43 @@ void osnma_msg_receiver::process_dsm_message(const std::vector& dsm_msg } 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++) + for (uint16_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; + uint16_t l_pdk_bytes = (l_ds_bytes - 13 - bytes_lk - l_ds_bytes); 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++) + for (uint16_t k = 0; k < l_ds_bytes; k++) { d_osnma_data.d_dsm_kroot_message.p_dk[k] = dsm_msg[13 + bytes_lk + l_ds_bytes + k]; } - // validation? + // 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]); + } + sha256.update(&M[0], M.size()); + uint8_t* digest = sha256.digest(); + std::vector p_dk_computed; + for (uint16_t i = 0; i < l_pdk_bytes; i++) + { + p_dk_computed.push_back(digest[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) { @@ -350,6 +346,10 @@ void osnma_msg_receiver::read_mack_header() { lt_bits = it->second; } + if (lt_bits == 0) + { + return; + } } diff --git a/src/core/libs/osnma_msg_receiver.h b/src/core/libs/osnma_msg_receiver.h index 56790d9df..67aff46e5 100644 --- a/src/core/libs/osnma_msg_receiver.h +++ b/src/core/libs/osnma_msg_receiver.h @@ -22,10 +22,11 @@ #include "galileo_inav_message.h" // for OSNMA_msg #include "gnss_block_interface.h" // for gnss_shared_ptr #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 "sha256.h" +#include // for gr::block +#include // for pmt::pmt_t +#include // for std::array +#include // for std::shared_ptr #include /** \addtogroup Core @@ -60,12 +61,13 @@ private: 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 process_dsm_message(const std::vector& dsm_msg, uint8_t nma_header); void read_mack_header(); void read_mack_info_and_tags(); void read_mack_key(); void read_mack_padding(); + SHA256 sha256; std::array, 16> d_dsm_message{}; std::array, 16> d_dsm_id_received{}; diff --git a/src/core/libs/sha256.cc b/src/core/libs/sha256.cc new file mode 100644 index 000000000..e0b90189f --- /dev/null +++ b/src/core/libs/sha256.cc @@ -0,0 +1,193 @@ +/*! + * \file sha256.cc + * \brief Class foir computing SHA256 + * \author Carles Fernandez-Prades, 2023. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "sha256.h" +#include +#include +#include + +SHA256::SHA256() : d_blocklen(0), d_bitlen(0) +{ + d_state[0] = 0x6a09e667; + d_state[1] = 0xbb67ae85; + d_state[2] = 0x3c6ef372; + d_state[3] = 0xa54ff53a; + d_state[4] = 0x510e527f; + d_state[5] = 0x9b05688c; + d_state[6] = 0x1f83d9ab; + d_state[7] = 0x5be0cd19; +} + +void SHA256::update(const uint8_t *data, size_t length) +{ + for (size_t i = 0; i < length; i++) + { + d_data[d_blocklen++] = data[i]; + if (d_blocklen == 64) + { + transform(); + + // End of the block + d_bitlen += 512; + d_blocklen = 0; + } + } +} + +void SHA256::update(const std::string &data) +{ + update(reinterpret_cast(data.c_str()), data.size()); +} + +uint8_t *SHA256::digest() +{ + uint8_t *hash = new uint8_t[32]; + + pad(); + revert(hash); + + return hash; +} + +uint32_t SHA256::rotr(uint32_t x, uint32_t n) +{ + return (x >> n) | (x << (32 - n)); +} + +uint32_t SHA256::choose(uint32_t e, uint32_t f, uint32_t g) +{ + return (e & f) ^ (~e & g); +} + +uint32_t SHA256::majority(uint32_t a, uint32_t b, uint32_t c) +{ + return (a & (b | c)) | (b & c); +} + +uint32_t SHA256::sig0(uint32_t x) +{ + return SHA256::rotr(x, 7) ^ SHA256::rotr(x, 18) ^ (x >> 3); +} + +uint32_t SHA256::sig1(uint32_t x) +{ + return SHA256::rotr(x, 17) ^ SHA256::rotr(x, 19) ^ (x >> 10); +} + +void SHA256::transform() +{ + uint32_t maj, xorA, ch, xorE, sum, newA, newE, m[64]; + uint32_t state[8]; + + for (uint8_t i = 0, j = 0; i < 16; i++, j += 4) + { // Split data in 32 bit blocks for the 16 first words + m[i] = (d_data[j] << 24) | (d_data[j + 1] << 16) | (d_data[j + 2] << 8) | (d_data[j + 3]); + } + + for (uint8_t k = 16; k < 64; k++) + { // Remaining 48 blocks + m[k] = SHA256::sig1(m[k - 2]) + m[k - 7] + SHA256::sig0(m[k - 15]) + m[k - 16]; + } + + for (uint8_t i = 0; i < 8; i++) + { + state[i] = d_state[i]; + } + + for (uint8_t i = 0; i < 64; i++) + { + maj = SHA256::majority(state[0], state[1], state[2]); + xorA = SHA256::rotr(state[0], 2) ^ SHA256::rotr(state[0], 13) ^ SHA256::rotr(state[0], 22); + + ch = choose(state[4], state[5], state[6]); + + xorE = SHA256::rotr(state[4], 6) ^ SHA256::rotr(state[4], 11) ^ SHA256::rotr(state[4], 25); + + sum = m[i] + K[i] + state[7] + ch + xorE; + newA = xorA + maj + sum; + newE = state[3] + sum; + + state[7] = state[6]; + state[6] = state[5]; + state[5] = state[4]; + state[4] = newE; + state[3] = state[2]; + state[2] = state[1]; + state[1] = state[0]; + state[0] = newA; + } + + for (uint8_t i = 0; i < 8; i++) + { + d_state[i] += state[i]; + } +} + +void SHA256::pad() +{ + uint64_t i = d_blocklen; + uint8_t end = d_blocklen < 56 ? 56 : 64; + + d_data[i++] = 0x80; // Append a bit 1 + while (i < end) + { + d_data[i++] = 0x00; // Pad with zeros + } + + if (d_blocklen >= 56) + { + transform(); + memset(d_data, 0, 56); + } + + // Append to the padding the total message's length in bits and transform. + d_bitlen += d_blocklen * 8; + d_data[63] = d_bitlen; + d_data[62] = d_bitlen >> 8; + d_data[61] = d_bitlen >> 16; + d_data[60] = d_bitlen >> 24; + d_data[59] = d_bitlen >> 32; + d_data[58] = d_bitlen >> 40; + d_data[57] = d_bitlen >> 48; + d_data[56] = d_bitlen >> 56; + transform(); +} + +void SHA256::revert(uint8_t *hash) +{ + // SHA uses big endian byte ordering + // Revert all bytes + for (uint8_t i = 0; i < 4; i++) + { + for (uint8_t j = 0; j < 8; j++) + { + hash[i + (j * 4)] = (d_state[j] >> (24 - i * 8)) & 0x000000ff; + } + } +} + +std::string SHA256::toString(const uint8_t *digest) +{ + std::stringstream s; + s << std::setfill('0') << std::hex; + + for (uint8_t i = 0; i < 32; i++) + { + s << std::setw(2) << (unsigned int)digest[i]; + } + + return s.str(); +} \ No newline at end of file diff --git a/src/core/libs/sha256.h b/src/core/libs/sha256.h new file mode 100644 index 000000000..8fb6b783c --- /dev/null +++ b/src/core/libs/sha256.h @@ -0,0 +1,68 @@ +/*! + * \file sha256.h + * \brief Class foir computing SHA256 + * \author Carles Fernandez-Prades, 2023. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +// https://github.com/System-Glitch/SHA256/blob/master/src/SHA256.cpp +#ifndef SHA256_H +#define SHA256_H + +#include +#include + +class SHA256 +{ +public: + SHA256(); + void update(const uint8_t *data, size_t length); + void update(const std::string &data); + uint8_t *digest(); + + static std::string toString(const uint8_t *digest); + +private: + uint8_t d_data[64]; + uint32_t d_blocklen; + uint64_t d_bitlen; + uint32_t d_state[8]; // A, B, C, D, E, F, G, H + + static constexpr std::array K = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, + 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, + 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, + 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, + 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, + 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, + 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2}; + + static uint32_t rotr(uint32_t x, uint32_t n); + static uint32_t choose(uint32_t e, uint32_t f, uint32_t g); + static uint32_t majority(uint32_t a, uint32_t b, uint32_t c); + static uint32_t sig0(uint32_t x); + static uint32_t sig1(uint32_t x); + void transform(); + void pad(); + void revert(uint8_t *hash); +}; + +#endif \ No newline at end of file diff --git a/src/core/system_parameters/Galileo_OSNMA.h b/src/core/system_parameters/Galileo_OSNMA.h index 9d243d6fc..14ccc6ad4 100644 --- a/src/core/system_parameters/Galileo_OSNMA.h +++ b/src/core/system_parameters/Galileo_OSNMA.h @@ -155,7 +155,16 @@ constexpr std::uint8_t mask_cpks{0x07}; constexpr std::uint8_t mask_nma_header_reserved{0x01}; constexpr std::uint8_t mask_dsm_id{0xF0}; constexpr std::uint8_t mask_dsm_block_id{0x0F}; -constexpr std::uint8_t mask_dsm_number_blocks{0XF0}; +constexpr std::uint8_t mask_dsm_number_blocks{0xF0}; +constexpr std::uint8_t mask_dsm_pkid{0x0F}; +constexpr std::uint8_t mask_dsm_cidkr{0xC0}; +constexpr std::uint8_t mask_dsm_reserved1{0x30}; +constexpr std::uint8_t mask_dsm_hf{0x0C}; +constexpr std::uint8_t mask_dsm_mf{0x03}; +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}; #else constexpr std::uint8_t mask_nmas{0b1100'0000}; constexpr std::uint8_t mask_cid{0b0011'0000}; @@ -164,9 +173,18 @@ constexpr std::uint8_t mask_nma_header_reserved{0b0000'0001}; constexpr std::uint8_t mask_dsm_id{0b1111'0000}; constexpr std::uint8_t mask_dsm_block_id{0b0000'1111}; constexpr std::uint8_t mask_dsm_number_blocks{0b1111'0000}; - +constexpr std::uint8_t mask_dsm_pkid{0b0000'1111}; +constexpr std::uint8_t mask_dsm_cidkr{0b1100'0000}; +constexpr std::uint8_t mask_dsm_reserved1{0b0011'0000}; +constexpr std::uint8_t mask_dsm_hf{0b0000'1100}; +constexpr std::uint8_t mask_dsm_mf{0b0000'0011}; +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}; #endif +// // hf = (dsm_msg[1] & 0b00001100) >> 2; uint8_t get_nmas(uint8_t nma_header) { return (nma_header & mask_nmas) >> 6; @@ -197,11 +215,125 @@ uint8_t get_dsm_block_id(uint8_t dsm_header) return dsm_header & mask_dsm_block_id; } -uint8_t get_number_blocks_index(uint8_t dsm_message_0) +uint8_t get_number_blocks_index(uint8_t dsm_msg_0) { - return (dsm_message_0 & mask_dsm_number_blocks) >> 4; + return (dsm_msg_0 & mask_dsm_number_blocks) >> 4; } +uint8_t get_pkid(const std::vector& dsm_msg) +{ + return (dsm_msg[0] & mask_dsm_pkid); +} + +uint8_t get_cidkr(const std::vector& dsm_msg) +{ + return (dsm_msg[1] & mask_dsm_cidkr) >> 6; +} + +uint8_t get_dsm_reserved1(const std::vector& dsm_msg) +{ + return (dsm_msg[1] & mask_dsm_reserved1) >> 4; +} + +uint8_t get_hf(const std::vector& dsm_msg) +{ + return (dsm_msg[1] & mask_dsm_hf) >> 2; +} + +uint8_t get_mf(const std::vector& dsm_msg) +{ + return (dsm_msg[1] & mask_dsm_mf); +} + +uint8_t get_ks(const std::vector& dsm_msg) +{ + return (dsm_msg[2] & mask_dsm_ks) >> 4; +} + +uint8_t get_ts(const std::vector& dsm_msg) +{ + return (dsm_msg[2] & mask_dsm_ts); +} + +uint8_t get_maclt(const std::vector& dsm_msg) +{ + return dsm_msg[3]; +} + +uint8_t get_dsm_reserved(const std::vector& dsm_msg) +{ + return (dsm_msg[4] & mask_dsm_reserved) >> 4; +} + +uint16_t get_wn_k(const std::vector& dsm_msg) +{ + return (static_cast((dsm_msg[4] & mask_dsm_wk_k_msbyte) << 8) + static_cast(dsm_msg[5])); +} + +uint8_t get_towh_k(const std::vector& dsm_msg) +{ + return dsm_msg[6]; +} + +uint64_t get_alpha(const std::vector& dsm_msg) +{ + uint64_t alpha = (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]); + return alpha; +} + +uint16_t get_l_dk_bits(uint8_t nb_dk) +{ + const auto it = OSNMA_TABLE_7.find(nb_dk); + if (it != OSNMA_TABLE_7.cend()) + { + return it->second.second; + } + return 0; +} + +uint16_t get_lk_bits(uint8_t ks) +{ + const auto it = OSNMA_TABLE_10.find(ks); + if (it != OSNMA_TABLE_10.cend()) + { + return it->second; + } + return 0; +} + +std::vector get_kroot(const std::vector& dsm_msg, uint16_t bytes_lk) +{ + std::vector kroot = std::vector(bytes_lk, 0); + if (dsm_msg.size() > 13 + bytes_lk) + { + for (uint16_t k = 0; k < bytes_lk; k++) + { + kroot[k] = dsm_msg[13 + k]; + } + } + return kroot; +} + +std::string get_hash_function(uint8_t hf) +{ + std::string hash_; + const auto it = OSNMA_TABLE_8.find(hf); + if (it != OSNMA_TABLE_8.cend()) + { + hash_ = it->second; + } + 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; +// } /** \} */ /** \} */ #endif // GNSS_SDR_GALILEO_OSNMA_H \ No newline at end of file