mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-08 08:20:33 +00:00
Code Issues Releases Wiki Activity

Merge pull request #5 from carlesfernandez/osnma-cesare-fix4

Browse Source 
Fix building with old compilers
This commit is contained in:
cesaaargm 2024-06-25 14:12:38 +02:00 committed by GitHub
commit 82ef50f8dd
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 508 additions and 397 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

268
src/core/libs/osnma_msg_receiver.cc
View File

@ -24,12 +24,15 @@
#include "osnma_dsm_reader.h" // for OSNMA_DSM_Reader #include "osnma_dsm_reader.h" // for OSNMA_DSM_Reader
#include "osnma_helper.h" #include "osnma_helper.h"
#include <gnuradio/io_signature.h> // for gr::io_signature::make #include <gnuradio/io_signature.h> // for gr::io_signature::make
#include <chrono>
#include <cmath> #include <cmath>
#include <cstddef> #include <cstddef>
#include <cstdint> #include <cstdint>
#include <iostream> #include <iostream>
#include <numeric> #include <numeric>
#include <typeinfo> // for typeid #include <typeinfo> // for typeid
#include <utility>
#if USE_GLOG_AND_GFLAGS #if USE_GLOG_AND_GFLAGS
#include <glog/logging.h> // for DLOG #include <glog/logging.h> // for DLOG
@ -115,26 +118,29 @@ void osnma_msg_receiver::msg_handler_osnma(const pmt::pmt_t& msg)
// TODO - PRNa is a typo here, I think for d_satellite_nav_data, is PRN_d the name to use // TODO - PRNa is a typo here, I think for d_satellite_nav_data, is PRN_d the name to use
const auto inav_data = wht::any_cast<std::shared_ptr<std::tuple<uint32_t, std::string, uint32_t>>>(pmt::any_ref(msg)); const auto inav_data = wht::any_cast<std::shared_ptr<std::tuple<uint32_t, std::string, uint32_t>>>(pmt::any_ref(msg));
uint32_t PRNa = std::get<0>(*inav_data); uint32_t PRNa = std::get<0>(*inav_data);
std::string nav_data = std::get<1>(*inav_data);; std::string nav_data = std::get<1>(*inav_data);
;
uint32_t TOW = std::get<2>(*inav_data); uint32_t TOW = std::get<2>(*inav_data);
// iono data => 549 bits, utc data, 141 bits. // iono data => 549 bits, utc data, 141 bits.
if (nav_data.size() == 549) if (nav_data.size() == 549)
{ {
LOG(INFO) << "Galileo OSNMA: received ADKD=0/12 navData, PRN_d (" << PRNa << ") " << "TOW_sf=" << TOW <<std::endl; LOG(INFO) << "Galileo OSNMA: received ADKD=0/12 navData, PRN_d (" << PRNa << ") "
<< "TOW_sf=" << TOW;
d_satellite_nav_data[PRNa][TOW].ephemeris_iono_vector_2 = nav_data; d_satellite_nav_data[PRNa][TOW].ephemeris_iono_vector_2 = nav_data;
} }
else if (nav_data.size() == 141) else if (nav_data.size() == 141)
{ {
LOG(INFO) << "Galileo OSNMA: received ADKD=4 navData, PRN_d (" << PRNa << ") " << "TOW_sf=" << TOW <<std::endl; LOG(INFO) << "Galileo OSNMA: received ADKD=4 navData, PRN_d (" << PRNa << ") "
<< "TOW_sf=" << TOW;
d_satellite_nav_data[PRNa][TOW].utc_vector_2 = nav_data; d_satellite_nav_data[PRNa][TOW].utc_vector_2 = nav_data;
} }
else else
LOG(ERROR) << "osnma_msg_receiver incorrect navData parsing!"; LOG(WARNING) << "osnma_msg_receiver incorrect navData parsing!";
} }
else else
{ {
LOG(ERROR) << "osnma_msg_receiver received an unknown object type!"; LOG(WARNING) << "osnma_msg_receiver received an unknown object type!";
} }
} }
catch (const wht::bad_any_cast& e) catch (const wht::bad_any_cast& e)
@ -159,7 +165,7 @@ void osnma_msg_receiver::process_osnma_message(const std::shared_ptr<OSNMA_msg>&
read_nma_header(osnma_msg->hkroot[0]); read_nma_header(osnma_msg->hkroot[0]);
if (d_osnma_data.d_nma_header.nmas == 0 || d_osnma_data.d_nma_header.nmas == 3 /*&& d_kroot_verified*/) if (d_osnma_data.d_nma_header.nmas == 0 || d_osnma_data.d_nma_header.nmas == 3 /*&& d_kroot_verified*/)
{ {
LOG(ERROR) << "Galileo OSNMA: NMAS invalid, skipping osnma message\n"; LOG(WARNING) << "Galileo OSNMA: NMAS invalid, skipping osnma message";
return; return;
} }
read_dsm_header(osnma_msg->hkroot[1]); read_dsm_header(osnma_msg->hkroot[1]);
@ -200,8 +206,7 @@ void osnma_msg_receiver::read_dsm_header(uint8_t dsm_header)
LOG(INFO) << "OSNMA: DSM_ID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_id); LOG(INFO) << "OSNMA: DSM_ID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_id);
LOG(INFO) << "OSNMA: DSM_BID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_block_id); LOG(INFO) << "OSNMA: DSM_BID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_block_id);
LOG(INFO) << "Galileo OSNMA: Received block " << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_block_id) LOG(INFO) << "Galileo OSNMA: Received block " << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_block_id)
<< " from DSM_ID " << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_id) << " from DSM_ID " << static_cast<uint32_t>(d_osnma_data.d_dsm_header.dsm_id);
<< std::endl;
} }
/* /*
@ -219,7 +224,6 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
// First block indicates number of blocks in DSM message // First block indicates number of blocks in DSM message
if (d_osnma_data.d_dsm_header.dsm_block_id == 0) if (d_osnma_data.d_dsm_header.dsm_block_id == 0)
{ {
uint8_t nb = d_dsm_reader->get_number_blocks_index(d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][0]); uint8_t nb = d_dsm_reader->get_number_blocks_index(d_dsm_message[d_osnma_data.d_dsm_header.dsm_id][0]);
uint16_t number_of_blocks = 0; uint16_t number_of_blocks = 0;
if (d_osnma_data.d_dsm_header.dsm_id < 12) if (d_osnma_data.d_dsm_header.dsm_id < 12)
@ -283,8 +287,8 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
} }
} }
} }
available_blocks<< "]" << std::endl; available_blocks << "]";
LOG(INFO) << available_blocks.str() << std::endl; LOG(INFO) << available_blocks.str();
} }
/** /**
@ -303,11 +307,9 @@ void osnma_msg_receiver::local_time_verification(const std::shared_ptr<OSNMA_msg
// TODO store list of SVs sending OSNMA and if received ID matches one stored, then just increment time 30s for that ID. // TODO store list of SVs sending OSNMA and if received ID matches one stored, then just increment time 30s for that ID.
if (d_receiver_time != 0) if (d_receiver_time != 0)
{ {
d_receiver_time = d_GST_0 + 30 * std::floor((d_GST_SIS - d_GST_0) / 30); // Eq. 3 R.G. d_receiver_time = d_GST_0 + 30 * std::floor((d_GST_SIS - d_GST_0) / 30); // Eq. 3 R.G.
// d_receiver_time += 30; // d_receiver_time += 30;
// std::cout << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << std::endl; // std::cout << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << std::endl;
} }
else else
{ // local time not initialised -> compute it. { // local time not initialised -> compute it.
@ -320,8 +322,8 @@ void osnma_msg_receiver::local_time_verification(const std::shared_ptr<OSNMA_msg
{ {
d_tags_allowed = tags_to_verify::all; d_tags_allowed = tags_to_verify::all;
d_tags_to_verify = {0, 4, 12}; d_tags_to_verify = {0, 4, 12};
LOG(INFO) << "Galileo OSNMA: time constraint OK (" << delta_T << "\n"; LOG(INFO) << "Galileo OSNMA: time constraint OK (" << delta_T;
LOG(INFO) << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << " d_GST_SIS: " << d_GST_SIS << "\n"; LOG(INFO) << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << " d_GST_SIS: " << d_GST_SIS;
// std::cout << "( |local_t - GST_SIS| < T_L ) [ |" << static_cast<int>(d_receiver_time - d_GST_SIS)<< " | < " << static_cast<int>(d_T_L) << " ]" << std::endl; // std::cout << "( |local_t - GST_SIS| < T_L ) [ |" << static_cast<int>(d_receiver_time - d_GST_SIS)<< " | < " << static_cast<int>(d_T_L) << " ]" << std::endl;
// TODO set flag to false to avoid processing dsm and MACK messages // TODO set flag to false to avoid processing dsm and MACK messages
@ -330,22 +332,20 @@ void osnma_msg_receiver::local_time_verification(const std::shared_ptr<OSNMA_msg
{ {
d_tags_allowed = tags_to_verify::slow_eph; d_tags_allowed = tags_to_verify::slow_eph;
d_tags_to_verify = {12}; d_tags_to_verify = {12};
LOG(WARNING) << "Galileo OSNMA: time constraint allows only slow MACs to be verified\n"; LOG(WARNING) << "Galileo OSNMA: time constraint allows only slow MACs to be verified";
LOG(WARNING) << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << " d_GST_SIS: " << d_GST_SIS << "\n"; LOG(WARNING) << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << " d_GST_SIS: " << d_GST_SIS;
LOG(WARNING)<< "( |local_t - GST_SIS| < T_L ) [ |" << static_cast<int>(d_receiver_time - d_GST_SIS) << " | < " << static_cast<int>(d_T_L) << " ]" << std::endl; LOG(WARNING) << "( |local_t - GST_SIS| < T_L ) [ |" << static_cast<int>(d_receiver_time - d_GST_SIS) << " | < " << static_cast<int>(d_T_L) << " ]";
} }
else else
{ {
d_tags_allowed = tags_to_verify::none; d_tags_allowed = tags_to_verify::none;
d_tags_to_verify = {}; d_tags_to_verify = {};
LOG(ERROR) << "Galileo OSNMA: time constraint violation\n"; LOG(WARNING) << "Galileo OSNMA: time constraint violation";
LOG(ERROR) << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << " d_GST_SIS: " << d_GST_SIS << "\n"; LOG(WARNING) << "Galileo OSNMA: d_receiver_time: " << d_receiver_time << " d_GST_SIS: " << d_GST_SIS;
LOG(ERROR) << "( |local_t - GST_SIS| < T_L ) [ |" << static_cast<int>(d_receiver_time - d_GST_SIS) << " | < " << static_cast<int>(d_T_L) << " ]" << std::endl; LOG(WARNING) << "( |local_t - GST_SIS| < T_L ) [ |" << static_cast<int>(d_receiver_time - d_GST_SIS) << " | < " << static_cast<int>(d_T_L) << " ]";
}
} }
}
/** /**
* @brief Process DSM block of an OSNMA message. * @brief Process DSM block of an OSNMA message.
@ -362,7 +362,8 @@ void osnma_msg_receiver::process_dsm_block(const std::shared_ptr<OSNMA_msg>& osn
if ((d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] != 0) && 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].cbegin(), d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].cend(), 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<uint8_t> dsm_msg(std::size_t(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]) * SIZE_DSM_BLOCKS_BYTES, 0); size_t len = std::size_t(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]) * SIZE_DSM_BLOCKS_BYTES;
std::vector<uint8_t> dsm_msg(len, 0);
for (uint32_t i = 0; i < d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]; i++) for (uint32_t i = 0; i < d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]; i++)
{ {
for (size_t j = 0; j < SIZE_DSM_BLOCKS_BYTES; j++) for (size_t j = 0; j < SIZE_DSM_BLOCKS_BYTES; j++)
@ -376,6 +377,7 @@ void osnma_msg_receiver::process_dsm_block(const std::shared_ptr<OSNMA_msg>& osn
} }
} }
/* /*
* case DSM-Kroot: * case DSM-Kroot:
* - computes the padding and compares with received message * - computes the padding and compares with received message
@ -433,7 +435,7 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
const uint16_t check_l_dk = 104 * std::ceil(1.0 + static_cast<float>((l_lk_bytes * 8.0) + l_ds_bits) / 104.0); const uint16_t check_l_dk = 104 * std::ceil(1.0 + static_cast<float>((l_lk_bytes * 8.0) + l_ds_bits) / 104.0);
if (l_dk_bits != check_l_dk) if (l_dk_bits != check_l_dk)
{ {
LOG(ERROR) << "Galileo OSNMA: Failed length reading of DSM-KROOT message" << std::endl; LOG(WARNING) << "Galileo OSNMA: Failed length reading of DSM-KROOT message";
} }
else else
{ {
@ -479,27 +481,25 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
LOG(INFO) << "Galileo OSNMA: KROOT with CID=" << static_cast<uint32_t>(d_osnma_data.d_nma_header.cid) LOG(INFO) << "Galileo OSNMA: KROOT with CID=" << static_cast<uint32_t>(d_osnma_data.d_nma_header.cid)
<< ", PKID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.pkid) << ", PKID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.pkid)
<< ", WN=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.wn_k) << ", WN=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.wn_k)
<< ", TOW=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.towh_k) * 3600 << std::endl; << ", TOW=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.towh_k) * 3600;
local_time_verification(osnma_msg); local_time_verification(osnma_msg);
d_kroot_verified = d_crypto->verify_signature(message, d_osnma_data.d_dsm_kroot_message.ds); d_kroot_verified = d_crypto->verify_signature(message, d_osnma_data.d_dsm_kroot_message.ds);
if (d_kroot_verified) if (d_kroot_verified)
{ {
LOG(WARNING) << "Galileo OSNMA: KROOT authentication successful !" << std::endl; std::cout << "Galileo OSNMA: KROOT authentication successful!" << std::endl;
LOG(WARNING) << "Galileo OSNMA: KROOT authentication successful !" << std::endl; LOG(INFO) << "Galileo OSNMA: KROOT authentication successful!";
LOG(WARNING) << "Galileo OSNMA: NMA Status is " << d_dsm_reader->get_nmas_status(d_osnma_data.d_nma_header.nmas) << ", " LOG(INFO) << "Galileo OSNMA: NMA Status is " << d_dsm_reader->get_nmas_status(d_osnma_data.d_nma_header.nmas) << ", "
<< "Chain in force is " << static_cast<uint32_t>(d_osnma_data.d_nma_header.cid) << ", " << "Chain in force is " << static_cast<uint32_t>(d_osnma_data.d_nma_header.cid) << ", "
<< "Chain and Public Key Status is " << d_dsm_reader->get_cpks_status(d_osnma_data.d_nma_header.cpks) << std::endl; << "Chain and Public Key Status is " << d_dsm_reader->get_cpks_status(d_osnma_data.d_nma_header.cpks);
} }
else else
{ {
LOG(ERROR) << "Galileo OSNMA: KROOT authentication failed. " << std::endl; LOG(WARNING) << "Galileo OSNMA: KROOT authentication failed.";
LOG(INFO) << "Galileo OSNMA: KROOT authentication failed. " << std::endl;
} }
} }
else else
{ {
LOG(ERROR) << "Galileo OSNMA: Error computing padding bits." << std::endl; LOG(WARNING) << "Galileo OSNMA: Error computing padding bits.";
// TODO - here will have to decide if perform the verification or not. Since this step is not mandatory, one could as well have skipped it. // TODO - here will have to decide if perform the verification or not. Since this step is not mandatory, one could as well have skipped it.
} }
} }
@ -544,7 +544,7 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
uint32_t check_l_dp_bytes = 104 * std::ceil(static_cast<float>(1040.0 + l_npk_bytes * 8.0) / 104.0) / 8; uint32_t check_l_dp_bytes = 104 * std::ceil(static_cast<float>(1040.0 + l_npk_bytes * 8.0) / 104.0) / 8;
if (l_dp_bytes != check_l_dp_bytes) if (l_dp_bytes != check_l_dp_bytes)
{ {
LOG(ERROR) << "Galileo OSNMA: Failed length reading of DSM-PKR message" << std::endl; LOG(WARNING) << "Galileo OSNMA: Failed length reading of DSM-PKR message";
} }
else else
{ {
@ -559,7 +559,7 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
<< ", PKID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_pkr_message.npktid) << ", PKID=" << static_cast<uint32_t>(d_osnma_data.d_dsm_pkr_message.npktid)
/*<< ", WN=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.wn_k) /*<< ", WN=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.wn_k)
<< ", TOW=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.towh_k) * 3600*/ << ", TOW=" << static_cast<uint32_t>(d_osnma_data.d_dsm_kroot_message.towh_k) * 3600*/
<< " received" << std::endl; << " received";
// C: NPK verification against Merkle tree root. // C: NPK verification against Merkle tree root.
if (!d_public_key_verified) if (!d_public_key_verified)
{ {
@ -570,7 +570,6 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
d_crypto->set_public_key(d_osnma_data.d_dsm_pkr_message.npk); d_crypto->set_public_key(d_osnma_data.d_dsm_pkr_message.npk);
} }
} }
} }
} }
else else
@ -582,6 +581,7 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
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] = 0;
} }
/** /**
* @brief Reads the Mack message from the given OSNMA_msg object. * @brief Reads the Mack message from the given OSNMA_msg object.
* *
@ -620,6 +620,7 @@ void osnma_msg_receiver::read_and_process_mack_block(const std::shared_ptr<OSNMA
} }
} }
/** /**
* \brief Reads the MACk header from the d_mack_message array and updates the d_osnma_data structure. * \brief Reads the MACk header from the d_mack_message array and updates the d_osnma_data structure.
* \details This function reads the message MACK header from the d_mack_message array and updates the d_osnma_data structure with the parsed data. The header consists of three fields * \details This function reads the message MACK header from the d_mack_message array and updates the d_osnma_data structure with the parsed data. The header consists of three fields
@ -690,6 +691,7 @@ void osnma_msg_receiver::read_mack_header()
d_osnma_data.d_mack_message.header.cop = cop; d_osnma_data.d_mack_message.header.cop = cop;
} }
/** /**
* @brief Reads the MACK message body * @brief Reads the MACK message body
* *
@ -883,9 +885,11 @@ void osnma_msg_receiver::process_mack_message()
{ {
if (d_kroot_verified == false && d_tesla_key_verified == false) if (d_kroot_verified == false && d_tesla_key_verified == false)
{ {
LOG(WARNING) << "Galileo OSNMA: MACK cannot be processed. "<< ", " LOG(WARNING) << "Galileo OSNMA: MACK cannot be processed. "
<< "No Kroot nor TESLA key available" << std::endl; << ", "
if(!d_flag_debug){ << "No Kroot nor TESLA key available";
if (!d_flag_debug)
{
return; // early return, cannot proceed further without one of the two verified. this equals to having Kroot but no TESLa key yet. return; // early return, cannot proceed further without one of the two verified. this equals to having Kroot but no TESLa key yet.
} }
else else
@ -895,24 +899,27 @@ void osnma_msg_receiver::process_mack_message()
if (d_tesla_keys.find(d_osnma_data.d_nav_data.TOW_sf0) == d_tesla_keys.end()) // check if already available => no need to verify if (d_tesla_keys.find(d_osnma_data.d_nav_data.TOW_sf0) == d_tesla_keys.end()) // check if already available => no need to verify
{ {
bool retV = verify_tesla_key(d_osnma_data.d_mack_message.key, d_osnma_data.d_nav_data.TOW_sf0); bool retV = verify_tesla_key(d_osnma_data.d_mack_message.key, d_osnma_data.d_nav_data.TOW_sf0);
if(retV){ if (retV)
d_tesla_keys.insert(std::pair(d_osnma_data.d_nav_data.TOW_sf0, d_osnma_data.d_mack_message.key)); {
d_tesla_keys.insert(std::pair<uint32_t, std::vector<uint8_t>>(d_osnma_data.d_nav_data.TOW_sf0, d_osnma_data.d_mack_message.key));
} }
} }
// MACSEQ - verify current macks, then add current retrieved mack to the end. // MACSEQ - verify current macks, then add current retrieved mack to the end.
auto mack = d_macks_awaiting_MACSEQ_verification.begin(); auto mack = d_macks_awaiting_MACSEQ_verification.begin();
while (mack != d_macks_awaiting_MACSEQ_verification.end()){ while (mack != d_macks_awaiting_MACSEQ_verification.end())
if(d_tesla_keys.find(mack->TOW + 30) != d_tesla_keys.end()){ {
if (d_tesla_keys.find(mack->TOW + 30) != d_tesla_keys.end())
{
bool ret = verify_macseq(*mack); bool ret = verify_macseq(*mack);
if (ret || d_flag_debug){ if (ret || d_flag_debug)
{
for (std::size_t i = 0; i < mack->tag_and_info.size(); ++i) for (std::size_t i = 0; i < mack->tag_and_info.size(); ++i)
{ {
// add tags of current mack to the verification queue // add tags of current mack to the verification queue
auto& tag = mack->tag_and_info[i]; auto& tag = mack->tag_and_info[i];
Tag t(tag, mack->TOW, mack->WN, mack->PRNa, i + 2); // tag0 (mack header) has CTR1, so first tag of MTI has CTR = 2. Tag t(tag, mack->TOW, mack->WN, mack->PRNa, i + 2); // tag0 (mack header) has CTR1, so first tag of MTI has CTR = 2.
d_tags_awaiting_verify.insert(std::pair(mack->TOW, t)); d_tags_awaiting_verify.insert(std::pair<uint32_t, Tag>(mack->TOW, t));
LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: SUCCESS for Mack at TOW=" << mack->TOW << ", PRN" << mack->PRNa; LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: SUCCESS for Mack at TOW=" << mack->TOW << ", PRN" << mack->PRNa;
} }
std::cout << "Galileo OSNMA: d_tags_awaiting_verify :: size: " << d_tags_awaiting_verify.size() << std::endl; std::cout << "Galileo OSNMA: d_tags_awaiting_verify :: size: " << d_tags_awaiting_verify.size() << std::endl;
@ -923,18 +930,20 @@ void osnma_msg_receiver::process_mack_message()
mack = d_macks_awaiting_MACSEQ_verification.erase(mack); mack = d_macks_awaiting_MACSEQ_verification.erase(mack);
} }
} }
else { // key not yet available - keep in container until then -- might be deleted if container size exceeds max allowed else
{ // key not yet available - keep in container until then -- might be deleted if container size exceeds max allowed
++mack; ++mack;
} }
} }
// add current received MACK to the container to be verified in the next iteration (on this one no key available) // add current received MACK to the container to be verified in the next iteration (on this one no key available)
d_macks_awaiting_MACSEQ_verification.push_back(d_osnma_data.d_mack_message); d_macks_awaiting_MACSEQ_verification.push_back(d_osnma_data.d_mack_message);
// Tag verification // Tag verification
for (auto & it : d_tags_awaiting_verify){ for (auto& it : d_tags_awaiting_verify)
{
bool ret; bool ret;
if(tag_has_key_available(it.second) && tag_has_nav_data_available(it.second)){ if (tag_has_key_available(it.second) && tag_has_nav_data_available(it.second))
{
ret = verify_tag(it.second); ret = verify_tag(it.second);
/* TODO - take into account: /* TODO - take into account:
* - COP: if * - COP: if
@ -955,8 +964,7 @@ void osnma_msg_receiver::process_mack_message()
<< ", PRNa=" << ", PRNa="
<< static_cast<unsigned>(it.second.PRNa) << static_cast<unsigned>(it.second.PRNa)
<< ", PRNd=" << ", PRNd="
<< static_cast<unsigned>(it.second.PRN_d) << static_cast<unsigned>(it.second.PRN_d);
<< std::endl;
} }
/* TODO notify PVT via pmt /* TODO notify PVT via pmt
* have_new_data() true * have_new_data() true
@ -965,7 +973,7 @@ void osnma_msg_receiver::process_mack_message()
else else
{ {
it.second.status = Tag::FAIL; it.second.status = Tag::FAIL;
LOG(ERROR) << "Galileo OSNMA: Tag verification :: FAILURE for tag Id=" LOG(WARNING) << "Galileo OSNMA: Tag verification :: FAILURE for tag Id="
<< it.second.tag_id << it.second.tag_id
<< ", value=0x" << std::setfill('0') << std::setw(10) << std::hex << std::uppercase << ", value=0x" << std::setfill('0') << std::setw(10) << std::hex << std::uppercase
<< it.second.received_tag << std::dec << it.second.received_tag << std::dec
@ -976,11 +984,11 @@ void osnma_msg_receiver::process_mack_message()
<< ", PRNa=" << ", PRNa="
<< static_cast<unsigned>(it.second.PRNa) << static_cast<unsigned>(it.second.PRNa)
<< ", PRNd=" << ", PRNd="
<< static_cast<unsigned>(it.second.PRN_d) << static_cast<unsigned>(it.second.PRN_d);
<< std::endl;
} }
} }
else if(it.second.TOW > d_osnma_data.d_nav_data.TOW_sf0){ else if (it.second.TOW > d_osnma_data.d_nav_data.TOW_sf0)
{
// case 1: adkd=12 and t.Tow + 300 < current TOW // case 1: adkd=12 and t.Tow + 300 < current TOW
// case 2: adkd=0/4 and t.Tow + 30 < current TOW // case 2: adkd=0/4 and t.Tow + 30 < current TOW
// case 3: any adkd and t.Tow > current TOW // case 3: any adkd and t.Tow > current TOW
@ -997,8 +1005,7 @@ void osnma_msg_receiver::process_mack_message()
<< static_cast<unsigned>(it.second.PRNa) << static_cast<unsigned>(it.second.PRNa)
<< ", PRNd=" << ", PRNd="
<< static_cast<unsigned>(it.second.PRN_d) << static_cast<unsigned>(it.second.PRN_d)
<< ". Key available ("<< tag_has_key_available(it.second) <<"), navData ("<< tag_has_nav_data_available(it.second) <<"). " << ". Key available (" << tag_has_key_available(it.second) << "), navData (" << tag_has_nav_data_available(it.second) << "). ";
<< std::endl;
} }
} }
@ -1007,6 +1014,7 @@ void osnma_msg_receiver::process_mack_message()
control_tags_awaiting_verify_size(); // remove the oldest tags if size is too big. control_tags_awaiting_verify_size(); // remove the oldest tags if size is too big.
} }
bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message) bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message)
{ {
// TODO create function for recursively apply hash // TODO create function for recursively apply hash
@ -1022,7 +1030,7 @@ bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message)
// compute intermediate leafs' values // compute intermediate leafs' values
// std::vector<uint8_t> x_0,x_1,x_2,x_3,x_4; // std::vector<uint8_t> x_0,x_1,x_2,x_3,x_4;
LOG(INFO) << "Galileo OSNMA: DSM-PKR :: leaf provided: m_" << static_cast<int>(message.mid) << std::endl; LOG(INFO) << "Galileo OSNMA: DSM-PKR :: leaf provided: m_" << static_cast<int>(message.mid);
std::vector<uint8_t> x_next, x_current = d_crypto->computeSHA256(m_i); std::vector<uint8_t> x_next, x_current = d_crypto->computeSHA256(m_i);
for (size_t i = 0; i < 4; i++) for (size_t i = 0; i < 4; i++)
@ -1030,11 +1038,14 @@ bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message)
x_next.clear(); x_next.clear();
bool leaf_is_on_right = ((message.mid / (1 << (i))) % 2) == 1; bool leaf_is_on_right = ((message.mid / (1 << (i))) % 2) == 1;
if (leaf_is_on_right) { if (leaf_is_on_right)
{
// Leaf is on the right -> first the itn, then concatenate the leaf // Leaf is on the right -> first the itn, then concatenate the leaf
x_next.insert(x_next.end(), &message.itn[32 * i], &message.itn[32 * i + 32]); x_next.insert(x_next.end(), &message.itn[32 * i], &message.itn[32 * i + 32]);
x_next.insert(x_next.end(), x_current.begin(), x_current.end()); x_next.insert(x_next.end(), x_current.begin(), x_current.end());
} else { }
else
{
// Leaf is on the left -> first the leaf, then concatenate the itn // Leaf is on the left -> first the leaf, then concatenate the itn
x_next.insert(x_next.end(), x_current.begin(), x_current.end()); x_next.insert(x_next.end(), x_current.begin(), x_current.end());
x_next.insert(x_next.end(), &message.itn[32 * i], &message.itn[32 * i + 32]); x_next.insert(x_next.end(), &message.itn[32 * i], &message.itn[32 * i + 32]);
@ -1055,6 +1066,8 @@ bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message)
return false; return false;
} }
} }
bool osnma_msg_receiver::verify_tag(Tag& tag) bool osnma_msg_receiver::verify_tag(Tag& tag)
{ {
// TODO case tag0, to be verified here?, PRNd not needed for it // TODO case tag0, to be verified here?, PRNd not needed for it
@ -1129,14 +1142,13 @@ bool osnma_msg_receiver::verify_tag(Tag& tag)
<< ", PRNa=" << ", PRNa="
<< static_cast<unsigned>(tag.PRNa) << static_cast<unsigned>(tag.PRNa)
<< ", PRNd=" << ", PRNd="
<< static_cast<unsigned>(tag.PRN_d) << static_cast<unsigned>(tag.PRN_d);
<< std::endl;
return true; return true;
} }
else
return false; return false;
} }
std::vector<uint8_t> osnma_msg_receiver::build_message(const Tag& tag) std::vector<uint8_t> osnma_msg_receiver::build_message(const Tag& tag)
{ {
std::vector<uint8_t> m; std::vector<uint8_t> m;
@ -1165,42 +1177,53 @@ std::vector<uint8_t> osnma_msg_receiver::build_message(const Tag& tag)
applicable_nav_data = d_satellite_nav_data[tag.PRN_d][tag.TOW - 30].utc_vector_2; applicable_nav_data = d_satellite_nav_data[tag.PRN_d][tag.TOW - 30].utc_vector_2;
} }
else else
LOG(ERROR) <<"Galileo OSNMA :: Tag verification :: unknown ADKD" <<"\n"; LOG(WARNING) << "Galileo OSNMA :: Tag verification :: unknown ADKD";
// convert std::string to vector<uint8_t> // convert std::string to vector<uint8_t>
applicable_nav_data_bytes = d_helper->bytes(applicable_nav_data); applicable_nav_data_bytes = d_helper->bytes(applicable_nav_data);
// Convert and add NavData bytes into the message, taking care of that NMAS has only 2 bits // Convert and add NavData bytes into the message, taking care of that NMAS has only 2 bits
for (uint8_t byte : applicable_nav_data_bytes) { for (uint8_t byte : applicable_nav_data_bytes)
{
m.back() |= (byte >> 2); // First take the 6 MSB bits of byte and add to m m.back() |= (byte >> 2); // First take the 6 MSB bits of byte and add to m
m.push_back(byte << 6); // Then take the last 2 bits of byte, shift them to MSB position and insert the new element into m m.push_back(byte << 6); // Then take the last 2 bits of byte, shift them to MSB position and insert the new element into m
} }
if(m.back() == 0) { if (m.back() == 0)
{
m.pop_back(); // Remove the last element if its value is 0 (only padding was added) m.pop_back(); // Remove the last element if its value is 0 (only padding was added)
} }
else { else
{
// Pad with zeros if the last element wasn't full // Pad with zeros if the last element wasn't full
for (int bits = 2; bits < 8; bits += 2) { for (int bits = 2; bits < 8; bits += 2)
{
// Check if the last element in the vector has 2 '00' bits in its LSB position // Check if the last element in the vector has 2 '00' bits in its LSB position
if((m.back() & 0b00000011) == 0) { if ((m.back() & 0b00000011) == 0)
{
m.back() <<= 2; // Shift the existing bits to make room for new 2 bits m.back() <<= 2; // Shift the existing bits to make room for new 2 bits
} }
else { else
{
break; // If it does not have 2 '00' bits in its LSB position, then the padding is complete break; // If it does not have 2 '00' bits in its LSB position, then the padding is complete
} }
} }
} }
return m; return m;
} }
void osnma_msg_receiver::add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data) void osnma_msg_receiver::add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data)
{ {
// control size of container // control size of container
while (d_satellite_nav_data[SV_ID].size() >= 25) { while (d_satellite_nav_data[SV_ID].size() >= 25)
{
d_satellite_nav_data[SV_ID].erase(d_satellite_nav_data[SV_ID].begin()); d_satellite_nav_data[SV_ID].erase(d_satellite_nav_data[SV_ID].begin());
} }
// d_osnma_data[TOW] = crypto; // crypto // d_osnma_data[TOW] = crypto; // crypto
d_satellite_nav_data[SV_ID][TOW] = data; // nav d_satellite_nav_data[SV_ID][TOW] = data; // nav
// std::cout << "Galileo OSNMA: added element, size is " << d_satellite_nav_data[SV_ID].size() << std::endl; // std::cout << "Galileo OSNMA: added element, size is " << d_satellite_nav_data[SV_ID].size() << std::endl;
} }
void osnma_msg_receiver::display_data() void osnma_msg_receiver::display_data()
{ {
// if(d_satellite_nav_data.empty()) // if(d_satellite_nav_data.empty())
@ -1217,6 +1240,8 @@ void osnma_msg_receiver::display_data()
// } // }
// } // }
} }
bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TOW) bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TOW)
{ {
uint32_t num_of_hashes_needed; uint32_t num_of_hashes_needed;
@ -1224,17 +1249,19 @@ bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TO
std::vector<uint8_t> hash; std::vector<uint8_t> hash;
const uint8_t lk_bytes = d_dsm_reader->get_lk_bits(d_osnma_data.d_dsm_kroot_message.ks) / 8; const uint8_t lk_bytes = d_dsm_reader->get_lk_bits(d_osnma_data.d_dsm_kroot_message.ks) / 8;
// std::vector<uint8_t> validated_key; // std::vector<uint8_t> validated_key;
if(d_tesla_key_verified){ // have to go up to last verified key if (d_tesla_key_verified)
{ // have to go up to last verified key
d_validated_key = d_tesla_keys.rbegin()->second; d_validated_key = d_tesla_keys.rbegin()->second;
num_of_hashes_needed = (d_receiver_time - d_last_verified_key_GST) / 30; // Eq. 19 ICD modified num_of_hashes_needed = (d_receiver_time - d_last_verified_key_GST) / 30; // Eq. 19 ICD modified
LOG(INFO) << "Galileo OSNMA: TESLA verification ("<< num_of_hashes_needed << " hashes) need to be performed up to closest verified TESLA key " << std::endl; LOG(INFO) << "Galileo OSNMA: TESLA verification (" << num_of_hashes_needed << " hashes) need to be performed up to closest verified TESLA key";
hash = hash_chain(num_of_hashes_needed, key, GST_SFi, lk_bytes); hash = hash_chain(num_of_hashes_needed, key, GST_SFi, lk_bytes);
} }
else{// have to go until Kroot else
{ // have to go until Kroot
d_validated_key = d_osnma_data.d_dsm_kroot_message.kroot; d_validated_key = d_osnma_data.d_dsm_kroot_message.kroot;
num_of_hashes_needed = (d_receiver_time - d_GST_0) / 30 + 1; // Eq. 19 IC num_of_hashes_needed = (d_receiver_time - d_GST_0) / 30 + 1; // Eq. 19 IC
LOG(INFO) << "Galileo OSNMA: TESLA verification ("<< num_of_hashes_needed << " hashes) need to be performed up to Kroot " << std::endl; LOG(INFO) << "Galileo OSNMA: TESLA verification (" << num_of_hashes_needed << " hashes) need to be performed up to Kroot";
hash = hash_chain(num_of_hashes_needed, key, GST_SFi, lk_bytes); hash = hash_chain(num_of_hashes_needed, key, GST_SFi, lk_bytes);
} }
@ -1247,23 +1274,26 @@ bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TO
} }
if (computed_key == d_validated_key && num_of_hashes_needed > 0) if (computed_key == d_validated_key && num_of_hashes_needed > 0)
{ {
LOG(WARNING) << "Galileo OSNMA:: TESLA key verification :: SUCCESS! " << std::endl; LOG(WARNING) << "Galileo OSNMA:: TESLA key verification :: SUCCESS!";
d_tesla_keys.insert(std::pair(TOW,key)); d_tesla_keys.insert(std::pair<uint32_t, std::vector<uint8_t>>(TOW, key));
d_tesla_key_verified = true; d_tesla_key_verified = true;
d_last_verified_key_GST = d_receiver_time; d_last_verified_key_GST = d_receiver_time;
} }
else if(num_of_hashes_needed > 0){ else if (num_of_hashes_needed > 0)
LOG(ERROR) << "Galileo OSNMA:: TESLA key verification :: FAILED " << std::endl; {
if(d_flag_debug){ LOG(WARNING) << "Galileo OSNMA:: TESLA key verification :: FAILED";
d_tesla_keys.insert(std::pair(TOW,key)); if (d_flag_debug)
{
d_tesla_keys.insert(std::pair<uint32_t, std::vector<uint8_t>>(TOW, key));
d_last_verified_key_GST = d_receiver_time; d_last_verified_key_GST = d_receiver_time;
d_tesla_key_verified = true; d_tesla_key_verified = true;
// TODO - if intermediate verification fails, can one still use the former verified tesla key or should go to Kroot or even retrieve new Kroot? // TODO - if intermediate verification fails, can one still use the former verified tesla key or should go to Kroot or even retrieve new Kroot?
} }
} }
return d_tesla_key_verified; return d_tesla_key_verified;
} }
/** /**
* @brief Removes the tags that have been verified from the multimap d_tags_awaiting_verify. * @brief Removes the tags that have been verified from the multimap d_tags_awaiting_verify.
* *
@ -1271,7 +1301,8 @@ bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TO
*/ */
void osnma_msg_receiver::remove_verified_tags() void osnma_msg_receiver::remove_verified_tags()
{ {
for (auto it = d_tags_awaiting_verify.begin(); it != d_tags_awaiting_verify.end() ; ){ for (auto it = d_tags_awaiting_verify.begin(); it != d_tags_awaiting_verify.end();)
{
if (it->second.status == Tag::SUCCESS || it->second.status == Tag::FAIL) if (it->second.status == Tag::SUCCESS || it->second.status == Tag::FAIL)
{ {
LOG(INFO) << "Galileo OSNMA: Tag verification :: DELETE tag Id=" LOG(INFO) << "Galileo OSNMA: Tag verification :: DELETE tag Id="
@ -1287,8 +1318,7 @@ void osnma_msg_receiver::remove_verified_tags()
<< ", PRNd=" << ", PRNd="
<< static_cast<unsigned>(it->second.PRN_d) << static_cast<unsigned>(it->second.PRN_d)
<< ", status= " << ", status= "
<< d_helper->verification_status_str(it->second.status) << d_helper->verification_status_str(it->second.status);
<< std::endl;
it = d_tags_awaiting_verify.erase(it); it = d_tags_awaiting_verify.erase(it);
} }
else if (it->second.skipped >= 20) else if (it->second.skipped >= 20)
@ -1306,8 +1336,7 @@ void osnma_msg_receiver::remove_verified_tags()
<< ", PRNd=" << ", PRNd="
<< static_cast<unsigned>(it->second.PRN_d) << static_cast<unsigned>(it->second.PRN_d)
<< ", status= " << ", status= "
<< d_helper->verification_status_str(it->second.status) << d_helper->verification_status_str(it->second.status);
<< std::endl;
it = d_tags_awaiting_verify.erase(it); it = d_tags_awaiting_verify.erase(it);
} }
else else
@ -1315,6 +1344,8 @@ void osnma_msg_receiver::remove_verified_tags()
} }
std::cout << "Galileo OSNMA: d_tags_awaiting_verify :: size: " << d_tags_awaiting_verify.size() << std::endl; std::cout << "Galileo OSNMA: d_tags_awaiting_verify :: size: " << d_tags_awaiting_verify.size() << std::endl;
} }
/** /**
* @brief Control the size of the tags awaiting verification multimap. * @brief Control the size of the tags awaiting verification multimap.
* *
@ -1332,12 +1363,12 @@ void osnma_msg_receiver::control_tags_awaiting_verify_size()
<< "Tag Id= " << it->second.tag_id << "Tag Id= " << it->second.tag_id
<< ", TOW=" << it->first << ", TOW=" << it->first
<< ", ADKD=" << static_cast<unsigned>(it->second.ADKD) << ", ADKD=" << static_cast<unsigned>(it->second.ADKD)
<< ", from satellite " << it->second.PRNa << ", from satellite " << it->second.PRNa;
<< std::endl;
d_tags_awaiting_verify.erase(it); d_tags_awaiting_verify.erase(it);
} }
} }
/** /**
* @brief Verifies the MACSEQ of a received MACK_message. * @brief Verifies the MACSEQ of a received MACK_message.
* *
@ -1373,11 +1404,11 @@ bool osnma_msg_receiver::verify_macseq(const MACK_message& mack)
} }
else else
{ {
LOG(ERROR) << "Galileo OSNMA: Mismatch in the GST verification. " << std::endl; LOG(WARNING) << "Galileo OSNMA: Mismatch in the GST verification.";
} }
if (mack.tag_and_info.size() != applicable_sequence.size() - 1) if (mack.tag_and_info.size() != applicable_sequence.size() - 1)
{ {
LOG(ERROR) << "Galileo OSNMA: Number of retrieved tags does not match MACLT sequence size!" << std::endl; LOG(WARNING) << "Galileo OSNMA: Number of retrieved tags does not match MACLT sequence size!";
return false; return false;
} }
std::vector<uint8_t> flxTags{}; std::vector<uint8_t> flxTags{};
@ -1392,13 +1423,14 @@ bool osnma_msg_receiver::verify_macseq(const MACK_message& mack)
} }
else if (mack.tag_and_info[i].tag_info.ADKD != std::stoi(applicable_sequence[i + 1])) else if (mack.tag_and_info[i].tag_info.ADKD != std::stoi(applicable_sequence[i + 1]))
{ {
LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: FAILURE :: ADKD mismatch against MAC Look-up table. " << std::endl; LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: FAILURE :: ADKD mismatch against MAC Look-up table.";
return false; // C: suffices one incorrect to abort and not process the rest of the tags return false; // C: suffices one incorrect to abort and not process the rest of the tags
} }
} }
if(flxTags.empty()){ if (flxTags.empty())
LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: SUCCESS :: ADKD matches MAC Look-up table. " << std::endl; {
LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: SUCCESS :: ADKD matches MAC Look-up table.";
return true; return true;
} }
// Fixed as well as FLX Tags share first part - Eq. 22 ICD // Fixed as well as FLX Tags share first part - Eq. 22 ICD
@ -1432,39 +1464,51 @@ bool osnma_msg_receiver::verify_macseq(const MACK_message& mack)
mac_msb = (mac[0] << 8) + mac[1]; mac_msb = (mac[0] << 8) + mac[1];
} }
uint16_t computed_macseq = (mac_msb & 0xFFF0) >> 4; uint16_t computed_macseq = (mac_msb & 0xFFF0) >> 4;
if (computed_macseq == mack.header.macseq){ if (computed_macseq == mack.header.macseq)
LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: SUCCESS :: FLX tags verification OK " << std::endl; {
LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: SUCCESS :: FLX tags verification OK";
return true; return true;
} }
else{ else
LOG(ERROR) << "Galileo OSNMA: MACSEQ verification :: FAILURE :: FLX tags verification failed " << std::endl; {
LOG(WARNING) << "Galileo OSNMA: MACSEQ verification :: FAILURE :: FLX tags verification failed";
return false; return false;
} }
} }
bool osnma_msg_receiver::tag_has_nav_data_available(Tag& t) bool osnma_msg_receiver::tag_has_nav_data_available(Tag& t)
{ {
auto prn_it = d_satellite_nav_data.find(t.PRN_d); auto prn_it = d_satellite_nav_data.find(t.PRN_d);
if (prn_it != d_satellite_nav_data.end()) { if (prn_it != d_satellite_nav_data.end())
{
// PRN was found, check if TOW exists in inner map // PRN was found, check if TOW exists in inner map
LOG(INFO) << "Galileo OSNMA: hasData = true " << std::endl; LOG(INFO) << "Galileo OSNMA: hasData = true " << std::endl;
std::map<uint32_t, NavData>& tow_map = prn_it->second; std::map<uint32_t, NavData>& tow_map = prn_it->second;
auto tow_it = tow_map.find(t.TOW - 30); auto tow_it = tow_map.find(t.TOW - 30);
if (tow_it != tow_map.end()) { if (tow_it != tow_map.end())
{
return true; return true;
} else { }
else
{
// TOW not found // TOW not found
return false; return false;
} }
} else { }
else
{
// PRN was not found // PRN was not found
LOG(INFO) << "Galileo OSNMA: hasData = false " << std::endl; LOG(INFO) << "Galileo OSNMA: hasData = false " << std::endl;
return false; return false;
} }
return false; return false;
} }
bool osnma_msg_receiver::tag_has_key_available(Tag& t){
bool osnma_msg_receiver::tag_has_key_available(Tag& t)
{
// check adkd of tag // check adkd of tag
// if adkd = 0 or 4 => look for d_tesla_keys[t.TOW+30] // if adkd = 0 or 4 => look for d_tesla_keys[t.TOW+30]
// if adkd = 12 => look for d_tesla_keys[t.TOW+300] // if adkd = 12 => look for d_tesla_keys[t.TOW+300]
@ -1491,6 +1535,8 @@ bool osnma_msg_receiver::tag_has_key_available(Tag& t){
LOG(INFO) << "Galileo OSNMA: hasKey = false "; LOG(INFO) << "Galileo OSNMA: hasKey = false ";
return false; return false;
} }
std::vector<uint8_t> osnma_msg_receiver::hash_chain(uint32_t num_of_hashes_needed, std::vector<uint8_t> key, uint32_t GST_SFi, const uint8_t lk_bytes) std::vector<uint8_t> osnma_msg_receiver::hash_chain(uint32_t num_of_hashes_needed, std::vector<uint8_t> key, uint32_t GST_SFi, const uint8_t lk_bytes)
{ {
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
@ -1549,12 +1595,12 @@ std::vector<uint8_t> osnma_msg_receiver::hash_chain(uint32_t num_of_hashes_neede
else else
check = GST_SFi + 30 == d_last_verified_key_GST; check = GST_SFi + 30 == d_last_verified_key_GST;
if (!check) if (!check)
LOG(ERROR) << "Galileo OSNMA: TESLA verification error. Kroot time mismatch! \n"; // ICD. Eq. 18 LOG(WARNING) << "Galileo OSNMA: TESLA verification error. Kroot time mismatch!"; // ICD. Eq. 18
else else
LOG(INFO) << "Galileo OSNMA: TESLA verification. Kroot time matches! \n"; // ICD. Eq. 18 LOG(INFO) << "Galileo OSNMA: TESLA verification. Kroot time matches!"; // ICD. Eq. 18
// compare computed current key against received key // compare computed current key against received key
auto end = std::chrono::high_resolution_clock::now(); auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> elapsed = end - start; std::chrono::duration<double> elapsed = end - start;
LOG(INFO) << "Galileo OSNMA: TESLA verification ("<< num_of_hashes_needed << " hashes) took " << elapsed.count() << " seconds.\n"; LOG(INFO) << "Galileo OSNMA: TESLA verification (" << num_of_hashes_needed << " hashes) took " << elapsed.count() << " seconds.";
return K_II; return K_II;
} }

4
src/core/system_parameters/CMakeLists.txt
View File

@ -163,6 +163,10 @@ if(OPENSSL_FOUND)
target_compile_definitions(core_system_parameters PUBLIC -DUSE_OPENSSL_FALLBACK=1 -DUSE_OPENSSL_3=1) target_compile_definitions(core_system_parameters PUBLIC -DUSE_OPENSSL_FALLBACK=1 -DUSE_OPENSSL_3=1)
message("USE_OPENSSL_3: " ${DUSE_OPENSSL_3}) message("USE_OPENSSL_3: " ${DUSE_OPENSSL_3})
message("USE_OPENSSL_FALLBACK:" ${USE_OPENSSL_FALLBACK}) message("USE_OPENSSL_FALLBACK:" ${USE_OPENSSL_FALLBACK})
else()
if(NOT OPENSSL_VERSION VERSION_LESS "1.1.1")
target_compile_definitions(core_system_parameters PRIVATE -DUSE_OPENSSL_FALLBACK=1 -DUSE_OPENSSL_111=1)
endif()
endif() endif()
endif() endif()

220
src/core/system_parameters/gnss_crypto.cc
View File

@ -60,6 +60,11 @@
Gnss_Crypto::Gnss_Crypto() Gnss_Crypto::Gnss_Crypto()
{ {
#if USE_OPENSSL_FALLBACK #if USE_OPENSSL_FALLBACK
#if !(USE_OPENSSL_3 || USE_OPENSSL_111)
LOG(WARNING) << "The OpenSSL library version you are linking against is too old for some OSNMA functions."
<< " Please do not trust OSNMA ouputs or upgrade your system to a newer version of OpenSSL"
<< " and rebuild GNSS-SDR against it.";
#endif
#else // GnuTLS #else // GnuTLS
gnutls_global_init(); gnutls_global_init();
#endif #endif
@ -69,6 +74,11 @@ Gnss_Crypto::Gnss_Crypto()
Gnss_Crypto::Gnss_Crypto(const std::string& certFilePath, const std::string& merkleTreePath) Gnss_Crypto::Gnss_Crypto(const std::string& certFilePath, const std::string& merkleTreePath)
{ {
#if USE_OPENSSL_FALLBACK #if USE_OPENSSL_FALLBACK
#if !(USE_OPENSSL_3 || USE_OPENSSL_111)
LOG(WARNING) << "The OpenSSL library version you are linking against is too old for some OSNMA functions."
<< " Please do not trust OSNMA ouputs or upgrade your system to a newer version of OpenSSL"
<< " and rebuild GNSS-SDR against it.";
#endif
#else // GnuTLS #else // GnuTLS
gnutls_global_init(); gnutls_global_init();
#endif #endif
@ -296,7 +306,7 @@ std::vector<uint8_t> Gnss_Crypto::computeSHA3_256(const std::vector<uint8_t>& in
{ {
std::vector<uint8_t> output(32); // SHA256 hash size std::vector<uint8_t> output(32); // SHA256 hash size
#if USE_OPENSSL_FALLBACK #if USE_OPENSSL_FALLBACK
#if USE_OPENSSL_3 #if USE_OPENSSL_3 || USE_OPENSSL_111
EVP_MD_CTX* mdctx = EVP_MD_CTX_new(); EVP_MD_CTX* mdctx = EVP_MD_CTX_new();
const EVP_MD* md = EVP_sha3_256(); const EVP_MD* md = EVP_sha3_256();
@ -305,7 +315,11 @@ std::vector<uint8_t> Gnss_Crypto::computeSHA3_256(const std::vector<uint8_t>& in
EVP_DigestFinal_ex(mdctx, output.data(), nullptr); EVP_DigestFinal_ex(mdctx, output.data(), nullptr);
EVP_MD_CTX_free(mdctx); EVP_MD_CTX_free(mdctx);
#else #else
// SHA3-256 not implemented in OpenSSL < 3.0 // SHA3-256 not implemented in OpenSSL 1.0, it was introduced in OpenSSL 1.1.1
if (!input.empty())
{
// do nothing
}
#endif #endif
#else // GnuTLS #else // GnuTLS
std::vector<uint8_t> output_aux(32); std::vector<uint8_t> output_aux(32);
@ -380,24 +394,16 @@ std::vector<uint8_t> Gnss_Crypto::computeHMAC_SHA_256(const std::vector<uint8_t>
hmac.resize(output_length); hmac.resize(output_length);
output = hmac; output = hmac;
#else #else
std::vector<uint8_t> hmac(32); unsigned int outputLength = EVP_MAX_MD_SIZE;
// Create HMAC context unsigned char* result = HMAC(EVP_sha256(), key.data(), key.size(), input.data(), input.size(), output.data(), &outputLength);
HMAC_CTX* ctx = HMAC_CTX_new(); if (result == nullptr)
HMAC_Init_ex(ctx, key.data(), key.size(), EVP_sha256(), nullptr); {
LOG(WARNING) << "OSNMA HMAC_SHA_256 computation failed to compute HMAC-SHA256";
return output;
}
// Update HMAC context with the message // Resize the output vector to the actual length of the HMAC-SHA256 output
HMAC_Update(ctx, input.data(), input.size()); output.resize(outputLength);
// Finalize HMAC computation
unsigned int hmacLen;
HMAC_Final(ctx, hmac.data(), &hmacLen);
// Clean up HMAC context
HMAC_CTX_free(ctx);
// Resize the HMAC vector to the actual length
hmac.resize(hmacLen);
output = hmac;
#endif #endif
#else // GnuTLS #else // GnuTLS
std::vector<uint8_t> output_aux(32); std::vector<uint8_t> output_aux(32);
@ -595,6 +601,7 @@ bool Gnss_Crypto::readPublicKeyFromCRT(const std::string& crtFilePath)
// Read the public key from the certificate // Read the public key from the certificate
EVP_PKEY* pubkey = X509_get_pubkey(cert); EVP_PKEY* pubkey = X509_get_pubkey(cert);
#if USE_OPENSSL_3
if (!pubkey) if (!pubkey)
{ {
std::cerr << "Failed to extract the public key" << std::endl; std::cerr << "Failed to extract the public key" << std::endl;
@ -603,6 +610,18 @@ bool Gnss_Crypto::readPublicKeyFromCRT(const std::string& crtFilePath)
} }
pubkey_copy(pubkey, &d_PublicKey); pubkey_copy(pubkey, &d_PublicKey);
EVP_PKEY_free(pubkey); EVP_PKEY_free(pubkey);
#else
EC_KEY* ec_pubkey = EVP_PKEY_get1_EC_KEY(pubkey);
EVP_PKEY_free(pubkey);
if (!ec_pubkey)
{
std::cerr << "Failed to extract the public key" << std::endl;
X509_free(cert);
return false;
}
pubkey_copy(ec_pubkey, &d_PublicKey);
EC_KEY_free(ec_pubkey);
#endif
BIO_free(bio); BIO_free(bio);
X509_free(cert); X509_free(cert);
#else // GnuTLS #else // GnuTLS
@ -735,7 +754,13 @@ bool Gnss_Crypto::verify_signature(const std::vector<uint8_t>& message, const st
LOG(WARNING) << "OpenSSL: OSNMA message authentication failed: " << err; LOG(WARNING) << "OpenSSL: OSNMA message authentication failed: " << err;
} }
#else #else
int verification = ECDSA_verify(0, digest.data(), SHA256_DIGEST_LENGTH, signature.data(), static_cast<int>(signature.size()), d_PublicKey); std::vector<uint8_t> der_sig;
if (!convert_raw_to_der_ecdsa(signature, der_sig))
{
std::cerr << "Failed to convert raw ECDSA signature to DER format" << std::endl;
return false;
}
int verification = ECDSA_verify(0, digest.data(), SHA256_DIGEST_LENGTH, der_sig.data(), static_cast<int>(der_sig.size()), d_PublicKey);
if (verification == 1) if (verification == 1)
{ {
success = true; success = true;
@ -744,6 +769,7 @@ bool Gnss_Crypto::verify_signature(const std::vector<uint8_t>& message, const st
else if (verification == 0) else if (verification == 0)
{ {
std::cerr << "OpenSSL: invalid signature found when verifying message" << std::endl; std::cerr << "OpenSSL: invalid signature found when verifying message" << std::endl;
LOG(WARNING) << "OpenSSL: invalid signature found when verifying message";
} }
else else
{ {
@ -838,12 +864,19 @@ void Gnss_Crypto::set_public_key(const std::vector<uint8_t>& publicKey)
LOG(INFO) << "OpenSSL: error setting the OSNMA public key."; LOG(INFO) << "OpenSSL: error setting the OSNMA public key.";
return; return;
} }
#if USE_OPENSSL_3
if (!pubkey_copy(pkey, &d_PublicKey)) if (!pubkey_copy(pkey, &d_PublicKey))
{ {
return; return;
} }
#else
EC_KEY* ec_pkey = EVP_PKEY_get1_EC_KEY(pkey);
if (!pubkey_copy(ec_pkey, &d_PublicKey))
{
return;
}
EC_KEY_free(ec_pkey);
#endif
EVP_PKEY_free(pkey); EVP_PKEY_free(pkey);
#else // GnuTLS #else // GnuTLS
gnutls_pubkey_t pubkey; gnutls_pubkey_t pubkey;
@ -864,53 +897,6 @@ void Gnss_Crypto::set_public_key(const std::vector<uint8_t>& publicKey)
} }
#if USE_OPENSSL_FALLBACK
bool Gnss_Crypto::pubkey_copy(EVP_PKEY* src, EVP_PKEY** dest)
{
// Open a memory buffer
BIO* mem_bio = BIO_new(BIO_s_mem());
if (mem_bio == nullptr)
{
return false;
}
// Export the public key from src into the memory buffer in PEM format
if (!PEM_write_bio_PUBKEY(mem_bio, src))
{
BIO_free(mem_bio);
return false;
}
// Read the data from the memory buffer
char* bio_data;
long data_len = BIO_get_mem_data(mem_bio, &bio_data);
// Create a new memory buffer and load the data into it
BIO* mem_bio2 = BIO_new_mem_buf(bio_data, data_len);
if (mem_bio2 == nullptr)
{
BIO_free(mem_bio);
return false;
}
// Read the public key from the new memory buffer
*dest = PEM_read_bio_PUBKEY(mem_bio2, nullptr, nullptr, nullptr);
if (*dest == nullptr)
{
BIO_free(mem_bio);
BIO_free(mem_bio2);
return false;
}
// Clean up
BIO_free(mem_bio);
BIO_free(mem_bio2);
return true;
}
#else // GnuTLS-specific functions
bool Gnss_Crypto::convert_raw_to_der_ecdsa(const std::vector<uint8_t>& raw_signature, std::vector<uint8_t>& der_signature) const bool Gnss_Crypto::convert_raw_to_der_ecdsa(const std::vector<uint8_t>& raw_signature, std::vector<uint8_t>& der_signature) const
{ {
if (raw_signature.size() % 2 != 0) if (raw_signature.size() % 2 != 0)
@ -959,6 +945,102 @@ bool Gnss_Crypto::convert_raw_to_der_ecdsa(const std::vector<uint8_t>& raw_signa
} }
#if USE_OPENSSL_FALLBACK
#if USE_OPENSSL_3
bool Gnss_Crypto::pubkey_copy(EVP_PKEY* src, EVP_PKEY** dest)
{
// Open a memory buffer
BIO* mem_bio = BIO_new(BIO_s_mem());
if (mem_bio == nullptr)
{
return false;
}
// Export the public key from src into the memory buffer in PEM format
if (!PEM_write_bio_PUBKEY(mem_bio, src))
{
BIO_free(mem_bio);
return false;
}
// Read the data from the memory buffer
char* bio_data;
long data_len = BIO_get_mem_data(mem_bio, &bio_data);
// Create a new memory buffer and load the data into it
BIO* mem_bio2 = BIO_new_mem_buf(bio_data, data_len);
if (mem_bio2 == nullptr)
{
BIO_free(mem_bio);
return false;
}
// Read the public key from the new memory buffer
*dest = PEM_read_bio_PUBKEY(mem_bio2, nullptr, nullptr, nullptr);
if (*dest == nullptr)
{
BIO_free(mem_bio);
BIO_free(mem_bio2);
return false;
}
// Clean up
BIO_free(mem_bio);
BIO_free(mem_bio2);
return true;
}
#else // OpenSSL 1.x
bool Gnss_Crypto::pubkey_copy(EC_KEY* src, EC_KEY** dest)
{
// Open a memory buffer
BIO* mem_bio = BIO_new(BIO_s_mem());
if (mem_bio == nullptr)
{
return false;
}
// Export the public key from src into the memory buffer in PEM format
if (!PEM_write_bio_EC_PUBKEY(mem_bio, src))
{
BIO_free(mem_bio);
return false;
}
// Read the data from the memory buffer
char* bio_data;
long data_len = BIO_get_mem_data(mem_bio, &bio_data);
// Create a new memory buffer and load the data into it
BIO* mem_bio2 = BIO_new_mem_buf(bio_data, data_len);
if (mem_bio2 == nullptr)
{
BIO_free(mem_bio);
return false;
}
// Read the public key from the new memory buffer
*dest = PEM_read_bio_EC_PUBKEY(mem_bio2, nullptr, nullptr, nullptr);
if (*dest == nullptr)
{
BIO_free(mem_bio);
BIO_free(mem_bio2);
return false;
}
// Clean up
BIO_free(mem_bio);
BIO_free(mem_bio2);
return true;
}
#endif
#else // GnuTLS-specific functions
bool Gnss_Crypto::pubkey_copy(gnutls_pubkey_t src, gnutls_pubkey_t* dest) bool Gnss_Crypto::pubkey_copy(gnutls_pubkey_t src, gnutls_pubkey_t* dest)
{ {
gnutls_datum_t key_datum; gnutls_datum_t key_datum;

9
src/core/system_parameters/gnss_crypto.h
View File

@ -58,18 +58,19 @@ private:
void read_merkle_xml(const std::string& merkleFilePath); void read_merkle_xml(const std::string& merkleFilePath);
void readPublicKeyFromPEM(const std::string& pemFilePath); void readPublicKeyFromPEM(const std::string& pemFilePath);
bool readPublicKeyFromCRT(const std::string& crtFilePath); bool readPublicKeyFromCRT(const std::string& crtFilePath);
bool convert_raw_to_der_ecdsa(const std::vector<uint8_t>& raw_signature, std::vector<uint8_t>& der_signature) const;
std::vector<uint8_t> convert_from_hex_str(const std::string& input) const; std::vector<uint8_t> convert_from_hex_str(const std::string& input) const;
#if USE_OPENSSL_FALLBACK #if USE_OPENSSL_FALLBACK
#if USE_OPENSSL_3 #if USE_OPENSSL_3
bool pubkey_copy(EVP_PKEY* src, EVP_PKEY** dest);
EVP_PKEY* d_PublicKey{}; EVP_PKEY* d_PublicKey{};
#else #else // OpenSSL 1.x
bool pubkey_copy(EC_KEY* src, EC_KEY** dest);
EC_KEY* d_PublicKey = nullptr; EC_KEY* d_PublicKey = nullptr;
#endif #endif
bool pubkey_copy(EVP_PKEY* src, EVP_PKEY** dest);
#else // GnuTLS #else // GnuTLS
gnutls_pubkey_t d_PublicKey{};
bool convert_raw_to_der_ecdsa(const std::vector<uint8_t>& raw_signature, std::vector<uint8_t>& der_signature) const;
bool pubkey_copy(gnutls_pubkey_t src, gnutls_pubkey_t* dest); bool pubkey_copy(gnutls_pubkey_t src, gnutls_pubkey_t* dest);
gnutls_pubkey_t d_PublicKey{};
#endif #endif
std::vector<uint8_t> d_x_4_0; std::vector<uint8_t> d_x_4_0;
std::vector<uint8_t> d_x_3_1; std::vector<uint8_t> d_x_3_1;

22
src/core/system_parameters/osnma_dsm_reader.h
View File

@ -63,7 +63,6 @@ public:
uint8_t get_npktid(const std::vector<uint8_t>& dsm_msg) const; uint8_t get_npktid(const std::vector<uint8_t>& dsm_msg) const;
private: private:
#if __cplusplus == 201103L
static constexpr std::uint8_t mask_nmas{0xC0}; static constexpr std::uint8_t mask_nmas{0xC0};
static constexpr std::uint8_t mask_cid{0x30}; static constexpr std::uint8_t mask_cid{0x30};
static constexpr std::uint8_t mask_cpks{0x07}; static constexpr std::uint8_t mask_cpks{0x07};
@ -83,27 +82,6 @@ private:
static constexpr std::uint8_t mask_dsm_mid{0x0F}; static constexpr std::uint8_t mask_dsm_mid{0x0F};
static constexpr std::uint8_t mask_dsm_npkt{0xF0}; static constexpr std::uint8_t mask_dsm_npkt{0xF0};
static constexpr std::uint8_t mask_dsm_npktid{0x0F}; static constexpr std::uint8_t mask_dsm_npktid{0x0F};
#else
static constexpr std::uint8_t mask_nmas{0b1100'0000};
static constexpr std::uint8_t mask_cid{0b0011'0000};
static constexpr std::uint8_t mask_cpks{0b0000'1110};
static constexpr std::uint8_t mask_nma_header_reserved{0b0000'0001};
static constexpr std::uint8_t mask_dsm_id{0b1111'0000};
static constexpr std::uint8_t mask_dsm_block_id{0b0000'1111};
static constexpr std::uint8_t mask_dsm_number_blocks{0b1111'0000};
static constexpr std::uint8_t mask_dsm_pkid{0b0000'1111};
static constexpr std::uint8_t mask_dsm_cidkr{0b1100'0000};
static constexpr std::uint8_t mask_dsm_reserved1{0b0011'0000};
static constexpr std::uint8_t mask_dsm_hf{0b0000'1100};
static constexpr std::uint8_t mask_dsm_mf{0b0000'0011};
static constexpr std::uint8_t mask_dsm_ks{0b1111'0000};
static constexpr std::uint8_t mask_dsm_ts{0b0000'1111};
static constexpr std::uint8_t mask_dsm_reserved{0b1111'0000};
static constexpr std::uint8_t mask_dsm_wk_k_msbyte{0b0000'1111};
static constexpr std::uint8_t mask_dsm_mid{0b0000'1111};
static constexpr std::uint8_t mask_dsm_npkt{0b1111'0000};
static constexpr std::uint8_t mask_dsm_npktid{0b0000'1111};
#endif
}; };
/** \} */ /** \} */