mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-05 18:27:02 +00:00
Code Issues Releases Wiki Activity

[TAS-159 ] d_tesla_key_verified => do not hash until Kroot(every time) - improve efficiency of verify_tesla_key by computing only the needed hashes until chronologically closest key, instead of going back to Kroot. Aditional chanches are:

Browse Source 
* rename unused variable d_old_GST_SISto d_last_verified_key_GST and use it for tesla key verification
* fix bug on verify_tesla_key during the comparison of computed and received key
* deleted d_old_OSNMA_buffer
* fill PRNa and TOW for MACK message when parsing it
* fix parameter bug for verify_macseq()
* immplement tag_has_key_available
* delete old verify_tag implementation
This commit is contained in:
cesaaargm 2024-04-07 14:27:14 +02:00
parent 8fa1a86f24
commit a17b04cb22
2 changed files with 211 additions and 371 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -60,8 +60,6 @@ osnma_msg_receiver::osnma_msg_receiver(
{
d_dsm_reader = std::make_unique<OSNMA_DSM_Reader>();
d_crypto = std::make_unique<Gnss_Crypto>(pemFilePath, merkleFilePath);
//d_old_mack_message.set_capacity(10);
d_old_OSNMA_buffer.set_capacity(25);
// register OSNMA input message port from telemetry blocks
this->message_port_register_in(pmt::mp("OSNMA_from_TLM"));
// register OSNMA output message port to PVT block
@ -541,7 +539,6 @@ 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;
}
/**
* @brief Reads the Mack message from the given OSNMA_msg object.
*
@ -574,7 +571,6 @@ 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.
* \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
@ -643,6 +639,8 @@ void osnma_msg_receiver::read_mack_header()
d_osnma_data.d_mack_message.header.tag0 = first_lt_bits;
d_osnma_data.d_mack_message.header.macseq = macseq;
d_osnma_data.d_mack_message.header.cop = cop;
d_osnma_data.d_mack_message.PRNa = d_osnma_data.d_nav_data.PRNa; // FIXME this is ugly.
d_osnma_data.d_mack_message.TOW = d_osnma_data.d_nav_data.TOW_sf0;
}
/**
@ -843,10 +841,10 @@ void osnma_msg_receiver::process_mack_message()
std::cerr << "Galileo OSNMA: MACK cannot be processed. "<< ", "
<< "No Kroot nor TESLA key available" << std::endl;
if(!d_flag_debug)
return; // early return, cannot proceed further without one of the two verified.
return; // early return, cannot proceed further without one of the two verified. this equals to having Kroot but no TESLa key yet.
}
// verify tesla key and add it to the container of verified keys if successful
bool retV = verify_tesla_key(d_osnma_data.d_mack_message.key, d_osnma_data.d_mack_message.TOW);
bool retV = verify_tesla_key(d_osnma_data.d_mack_message.key, d_osnma_data.d_nav_data.TOW_sf0);
if(retV){
d_tesla_keys.insert(std::pair(d_osnma_data.d_nav_data.TOW_sf0, d_osnma_data.d_mack_message.key));
}
@ -854,8 +852,8 @@ void osnma_msg_receiver::process_mack_message()
// MACSEQ - verify current macks, then add current retrieved mack to the end.
auto mack = d_macks_awaiting_MACSEQ_verification.begin();
while (mack != d_macks_awaiting_MACSEQ_verification.end()){
if(d_tesla_keys.find(mack->TOW) != d_tesla_keys.end()){
bool ret = verify_macseq();
if(d_tesla_keys.find(mack->TOW + 30) != d_tesla_keys.end()){
bool ret = verify_macseq(*mack);
if (ret || d_flag_debug){
for(auto& tag:mack->tag_and_info)
{
@ -877,21 +875,10 @@ void osnma_msg_receiver::process_mack_message()
d_macks_awaiting_MACSEQ_verification.push_back(d_osnma_data.d_mack_message);
// d_satellite_data already updated from a msg_handler coming from TD. TODO
// d_old_OSNMA_buffer.push_back(d_osnma_data); // TODO deprecate
// if(d_keys.size() < 2)
// {
// std::cerr << "Galileo OSNMA: MACK cannot be processed. "<< ", "
// << "Not enough OSNMA messages available"
// << "buffer size: "<< d_old_OSNMA_buffer.size() << std::endl;
// return;
// }
// verify tags
// Tag verification
for (auto & it : d_tags_awaiting_verify){
bool ret;
if(d_tesla_keys.find(it.first) != d_tesla_keys.end() && nav_data_available(it.second)){
if(tag_has_key_available(it.second) && tag_has_nav_data_available(it.second)){
ret = verify_tag(it.second);
/* TODO - take into account:
* - COP: if
@ -899,146 +886,52 @@ void osnma_msg_receiver::process_mack_message()
* - NavData the tag verifies (min. number of bits verified to consider NavData OK)
* */
if(ret)
std::cout << "Galileo OSNMA: Tag verification failure at "
<< "TOW="
<< it.second.TOW
<< ", ADKD="
<< it.second.ADKD
<< ", from satellite "
<< it.second.PRN_d
<< std::endl;
{
it.second.status = Tag::SUCCESS;
std::cout << "Galileo OSNMA: Tag verification failure for tag Id= "
<< it.second.tag_id
<< ", TOW="
<< it.second.TOW
<< ", ADKD="
<< static_cast<unsigned>(it.second.ADKD)
<< ", from satellite "
<< it.second.PRNa
<< std::endl;
}
/* TODO notify PVT via pmt
* have_new_data() true
* signal which one is verified
* communicate to PVT*/
else
std::cout << "Galileo OSNMA: Tag verification failure at "
<< "TOW="
{
it.second.status = Tag::FAIL;
std::cout << "Galileo OSNMA: Tag verification failure for tag Id= "
<< it.second.tag_id
<< ", TOW="
<< it.second.TOW
<< ", ADKD="
<< it.second.ADKD
<< static_cast<unsigned>(it.second.ADKD)
<< ", from satellite "
<< it.second.PRN_d
<< it.second.PRNa
<< std::endl;
}
}
else {
std::cout << "Galileo OSNMA: Tag verification skipped for Tag Id= "
<< it.second.tag_id
<< ", TOW="
<< it.second.TOW
<< ", ADKD="
<< static_cast<unsigned>(it.second.ADKD)
<< ", from satellite "
<< it.second.PRNa
<< std::endl;
}
}
remove_verified_tags();
control_tags_awaiting_verify_size(); // remove oldest tags if size is too big.
// deprecated tag verification - it includes steps not yet present on verify_tag, so keep it until then.
// Tag verification
// tag[i-1]:
// adkd = 4/0 : use TK[i], NavData[i-2] to validate Tag[i-1]
// adkd = 12 : ignore it -> not possible to verify yet
// tag[i-10]
// adkd = 4/0 : use TK[i-9], NavData[i-11] to validate Tag[i-10] would already be done by tag[i-1]
// adkd = 12 : use TK[i], NavData[i-11] to validate Tag[i-10] TODO - pending better logic for not repeating this while twice.
// int t = d_old_OSNMA_buffer.size() - 2;
// applicable_OSNMA = d_old_OSNMA_buffer[t]; // former subframe
// d_GST_Sf = d_receiver_time - 30; // time of the start of SF containing MACSEQ
//
// size_t i = 0;
// while (i < applicable_OSNMA.d_mack_message.tag_and_info.size() && // loop over all tags in MACK message
// std::find(d_tags_to_verify.begin(),d_tags_to_verify.end(), // ADKD[i] is within allowed ADKDs
// applicable_OSNMA.d_mack_message.tag_and_info[i].tag_info.ADKD)
// != d_tags_to_verify.end())
// {
// // TODO - if a subsequent tag was already part of the verification (inner loop), this while is going to ignore that and try to validate it anyway.
//
// // check if tag is flx
// bool is_flexible_tag = std::find(flxTags.begin(),flxTags.end(), i) != flxTags.end();
// if(is_flexible_tag && flxTagsV == false){
// //std::cout << "Galileo OSNMA: cannot verify flx tag. " << std::endl;
// continue;
// }
//
//
// // Take tag_k and check its ADKD, COP, PRN_d, this will be the reference for the iteration and search of other Tags
// uint8_t Nt = d_Lt_min / applicable_OSNMA.d_dsm_kroot_message.ts; // Tags needed to be verified
// uint8_t applicable_ADKD = applicable_OSNMA.d_mack_message.tag_and_info[i].tag_info.ADKD;
// uint8_t applicable_COP = applicable_OSNMA.d_mack_message.tag_and_info[i].tag_info.cop; // * d_delta_COP;
// uint8_t counter_COP = 1;
// uint8_t applicable_PRNd = applicable_OSNMA.d_mack_message.tag_and_info[i].tag_info.PRN_d;
// // ADKD=12 or ADKD = 4/0 => pick d_old_OSNMA_buffer.back() or [size-1]
// applicable_key = d_old_OSNMA_buffer[t+1].d_mack_message.key; // current subframe
// NavData applicable_NavData{};
// if((applicable_ADKD == 0 || applicable_ADKD == 4) && d_old_OSNMA_buffer.size() > 3)
// {
// applicable_NavData = d_old_OSNMA_buffer[t-1].d_nav_data;
// }
// else if(applicable_ADKD == 12 && d_old_OSNMA_buffer.size() > 11)
// {
// applicable_NavData = d_old_OSNMA_buffer[t - 11].d_nav_data;
// }
// else
// {
// std::cout << "Galileo OSNMA: MACK message buffer elements not enough. Cannot verify tags. " << std::endl;
// }
//
//
// int k = i + 1;
// uint8_t nt = 0;
// bool flag_cancel_tag_verification = false; // if a tag fails, cancel whole NavData verification set
// // Look for tags relative to reference NavData until Nt achieved,
// // this may require going back in time, as long as COP is valid
// while (nt <= Nt && counter_COP <= applicable_COP && !flag_cancel_tag_verification)
// {
// auto start_it = std::next(applicable_OSNMA.d_mack_message.tag_and_info.begin(), k);
//
// // check the vector of tags of aplicable OSNMA for a match against the chosen
// for (auto it = start_it; it != applicable_OSNMA.d_mack_message.tag_and_info.end() && nt <= Nt; ++it)
// {
// // Check if ADKD, COP, and PRN_d match
// if(it->tag_info.ADKD == applicable_ADKD
// // && it->tag_info.cop == applicable_COP // TODO - I think this may be skipped as the relevant is the COP distance.
// && it->tag_info.PRN_d == applicable_PRNd)
// {
// if(verify_tag(it.operator*(), applicable_OSNMA, k,applicable_key,applicable_NavData))
// {
// nt++;
// }
// else
// {
// // failure, discard this k-th tag
// flag_cancel_tag_verification = true;
// std::cout << "Galileo OSNMA: tag verification failed for PRN_a "
// << applicable_OSNMA.d_nav_data.PRNa << " with WN="
// << applicable_OSNMA.d_nav_data.WN_sf0 << ", TOW="
// << applicable_OSNMA.d_nav_data.TOW_sf0 << ". "
// << std::endl;
// }
//
// }
// if(flag_cancel_tag_verification)
// break;
// }
// // Check if Nt is achieved, if not, switch to older frame
// if(nt < Nt && t > 0 /*not end of buffer*/ && counter_COP <= applicable_COP && !flag_cancel_tag_verification)
// {
// t--;
// applicable_OSNMA = d_old_OSNMA_buffer[t];
// applicable_key = d_old_OSNMA_buffer[t+1].d_mack_message.key;
// applicable_NavData = d_old_OSNMA_buffer[t-1].d_nav_data;
// d_GST_Sf -= 30;
// counter_COP++;
// k = 0;
// }
// }
//
// if (nt >= Nt)
// {
// nt = 0;
// std::cout << "Galileo OSNMA: tag verification accumulation succesful for PRN_a "
// << applicable_OSNMA.d_nav_data.PRNa << " with WN="
// << applicable_OSNMA.d_nav_data.WN_sf0 << ", TOW="
// << applicable_OSNMA.d_nav_data.TOW_sf0 << ". "
// << std::endl;
// }
//
// }
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)
@ -1085,157 +978,6 @@ bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message)
return false;
}
}
bool osnma_msg_receiver::verify_tag(MACK_tag_and_info tag_and_info,
OSNMA_data applicable_OSNMA, uint8_t tag_position,
const std::vector<uint8_t>& applicable_key,
NavData applicable_NavData)
{
bool verified = false;
auto CTR = tag_position + 2; // CTR, first tag is CTR(tag0)=1 + 1 == 2
// NAvData, tag_and_info[i]
// check if enough osnma messages stored in the buffer.
if (tag_and_info.tag_info.ADKD == 0
|| tag_and_info.tag_info.ADKD == 4)
{
if (d_old_OSNMA_buffer.size() < 3)
{
std::cout << "Galileo OSNMA: MACK message buffer empty. Cannot verify tags. " << std::endl;
return verified;
}
}
else if (tag_and_info.tag_info.ADKD == 12)
{
if (d_old_OSNMA_buffer.size() < 10+15)
{
std::cout << "Galileo OSNMA: Tesla key not yet available. Cannot verify slow mac. at " <<
d_receiver_time << "s. "<< std::endl;
return verified;
}
}
else
{
std::cout << "Galileo OSNMA: Unknown ADKD. " << std::endl;
return verified;
}
// compute m
std::vector<uint8_t> m;
m.push_back(tag_and_info.tag_info.PRN_d);
for(int i = 24; i >= 0; i -= 8)
{
m.push_back((applicable_NavData.PRNa >> i) & 0xFF);
}
m.push_back(static_cast<uint8_t>((d_GST_Sf & 0xFF000000) >> 24));
m.push_back(static_cast<uint8_t>((d_GST_Sf & 0x00FF0000) >> 16));
m.push_back(static_cast<uint8_t>((d_GST_Sf & 0x0000FF00) >> 8));
m.push_back(static_cast<uint8_t>(d_GST_Sf & 0x000000FF));
m.push_back(CTR);
m.push_back(applicable_OSNMA.d_nma_header.nmas);
if(tag_and_info.tag_info.ADKD == 0)
{
m.insert(m.end(),
applicable_NavData.ephemeris_iono_vector.begin(),
applicable_NavData.ephemeris_iono_vector.end()) ;
}
else if(tag_and_info.tag_info.ADKD == 4)
{
m.insert(m.end(),applicable_NavData.utc_vector.begin(),applicable_NavData.utc_vector.end()) ;
}
else
{
std::cout << "Galileo OSNMA: Unknown ADKD. " << std::endl;
}
// check that m has an integer number of bytes, if not, add padding zeroes
// padding zeroes until size of vector is an integer number of bytes.
// I think not needed, if bytes of m correctly formatted (i.e. added in big-endianness) -> the unused bits will be zero
// and the vector has an integer number of uint8_t elements.
// compute mac
std::vector<uint8_t> mac;
if (applicable_OSNMA.d_dsm_kroot_message.mf == 0) // C: HMAC-SHA-256
{
mac = d_crypto->computeHMAC_SHA_256(applicable_key, m);
}
else if (applicable_OSNMA.d_dsm_kroot_message.mf == 1) // C: CMAC-AES
{
mac = d_crypto->computeCMAC_AES(applicable_key, m);
}
// truncate the computed mac: trunc(l_t, mac(K,m)) Eq. 23 ICD
uint8_t lt_bits = 0; // TODO - remove this duplication of code.
const auto it2 = OSNMA_TABLE_11.find(applicable_OSNMA.d_dsm_kroot_message.ts);
if (it2 != OSNMA_TABLE_11.cend())
{
lt_bits = it2->second;
}
if (lt_bits == 0)
{
return verified;
}
uint64_t computed_mac = static_cast<uint64_t>(mac[0]) << (lt_bits - 8);
computed_mac += (static_cast<uint64_t>(mac[1]) << (lt_bits - 16));
if (lt_bits == 20)
{
computed_mac += (static_cast<uint64_t>(mac[1] & 0xF0) >> 4);
}
else if (lt_bits == 24)
{
computed_mac += static_cast<uint64_t>(mac[2]);
}
else if (lt_bits == 28)
{
computed_mac += (static_cast<uint64_t>(mac[2]) << 4);
computed_mac += (static_cast<uint64_t>(mac[3] & 0xF0) >> 4);
}
else if (lt_bits == 32)
{
computed_mac += (static_cast<uint64_t>(mac[2]) << 8);
computed_mac += static_cast<uint64_t>(mac[3]);
}
else if (lt_bits == 40)
{
computed_mac += (static_cast<uint64_t>(mac[2]) << 16);
computed_mac += (static_cast<uint64_t>(mac[3]) << 8);
computed_mac += static_cast<uint64_t>(mac[4]);
}
// Compare computed tag with received one truncated
if (tag_and_info.tag == computed_mac)
{
verified = true;
if(tag_and_info.tag_info.ADKD == 0 || tag_and_info.tag_info.ADKD == 12)
{
std::cout << "Galileo OSNMA: tag verification successful for PRN_a "
<< applicable_NavData.PRNa << " with WN="
<< applicable_NavData.WN_sf0 << ", TOW="
<< applicable_NavData.TOW_sf0 << "NavData= "
<< "Ephemeris, Clock and Ionospheric data" << ". "
<< std::endl;
}
else if(tag_and_info.tag_info.ADKD == 4)
{
std::cout << "Galileo OSNMA: tag verification successful for PRN_a "
<< applicable_NavData.PRNa << " with WN="
<< applicable_NavData.WN_sf0 << ", TOW="
<< applicable_NavData.TOW_sf0 << "NavData= "
<< "Timing data" << ". "
<< std::endl;
}
}
else
{
std::cout << "Galileo OSNMA: Tag verification failed for PRN_a "
<< applicable_NavData.PRNa << " with WN="
<< applicable_NavData.WN_sf0 << ", TOW="
<< applicable_NavData.TOW_sf0 << ". "
<< std::endl;
}
return verified;
}
bool osnma_msg_receiver::verify_tag(Tag& tag)
{
std::vector<uint8_t> m = tag.build_message();
@ -1291,55 +1033,36 @@ bool osnma_msg_receiver::verify_tag(Tag& tag)
// Compare computed tag with received one truncated
if (tag.received_tag == computed_mac)
{
if(tag.ADKD == 0 || tag.ADKD == 12)
{
std::cout << "Galileo OSNMA: tag verification successful for PRN_a "
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN="
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW="
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << "NavData= "
<< "Ephemeris, Clock and Ionospheric data" << ". "
<< std::endl;
}
else if(tag.ADKD == 4)
{
std::cout << "Galileo OSNMA: tag verification successful for PRN_a "
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN="
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW="
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << "NavData= "
<< "Timing data" << ". "
<< std::endl;
}
// if(tag.ADKD == 0 || tag.ADKD == 12)
// {
// std::cout << "Galileo OSNMA: tag verification successful for PRN_a "
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN="
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW="
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << "NavData= "
// << "Ephemeris, Clock and Ionospheric data" << ". "
// << std::endl;
// }
// else if(tag.ADKD == 4)
// {
// std::cout << "Galileo OSNMA: tag verification successful for PRN_a "
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN="
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW="
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << "NavData= "
// << "Timing data" << ". "
// << std::endl;
// }
return true;
}
else
{
std::cout << "Galileo OSNMA: Tag verification failed for PRN_a "
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN="
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW="
<< d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << std::endl;
// std::cout << "Galileo OSNMA: Tag verification failed for PRN_a "
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].PRNa << " with WN="
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].WN_sf0 << ", TOW="
// << d_satellite_nav_data[tag.PRN_d][tag.TOW-30].TOW_sf0 << std::endl;
return false;
}
}
/**
* @brief Checks if the current subframe time is bigger than last one received.
*
* \details It compares the current GST value with the previous one and updates the old value with the new one.
*
* @param sharedPtr A shared pointer to an instance of OSNMA_msg.
* @return True if the current subframe is the next subframe, False otherwise.
*/
//bool osnma_msg_receiver::is_next_subframe()
//{
// bool is_bigger = d_GST_SIS > d_old_GST_SIS;
// if(d_GST_SIS != d_old_GST_SIS + 30 && d_old_GST_SIS != 0){
// std::cout << "Galileo OSNMA:: Mack processing - skip " << std::endl;
// }
//
// d_old_GST_SIS = d_GST_SIS;
//
// return is_bigger;
//}
void osnma_msg_receiver::add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data)
{
// control size of container
@ -1368,13 +1091,95 @@ void osnma_msg_receiver::display_data()
}
bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TOW)
{
if(d_tesla_key_verified || d_flag_debug)
if(d_tesla_key_verified)
{
// TODO - find out I bt. both tesla keys, then hash until then, then compare.
// retrieve latest tesla key
// retrieve latest tesla key from d_tesla_keys
std::vector<uint8_t> validated_key = d_tesla_keys.rbegin()->second;
// compute hashes needed
uint32_t num_of_hashes_needed = (d_GST_SIS - d_last_verified_key_GST) / 30; // Eq. 19 ICD modified
std::cout << "Galileo OSNMA: TESLA verification ("<< num_of_hashes_needed << " hashes) need to be performed. " << std::endl;
// hash current key until num_hashes and compare
return false;
auto start = std::chrono::high_resolution_clock::now();
uint32_t GST_SFi = d_GST_SIS; // TODO
std::vector<uint8_t> K_II = key;
std::vector<uint8_t> K_I; // result of the recursive hash operations
const uint8_t lk_bytes = d_dsm_reader->get_lk_bits(d_osnma_data.d_dsm_kroot_message.ks)/8;
// compute the tesla key for current SF (GST_SFi and K_II change in each iteration)
for (uint32_t i = 1; i < num_of_hashes_needed ; i++)
{
// build message digest m = (K_I+1 || GST_SFi || alpha)
std::vector<uint8_t> msg(K_II.size() + sizeof(GST_SFi) + sizeof(d_osnma_data.d_dsm_kroot_message.alpha));
std::copy(K_II.begin(),K_II.end(),msg.begin());
msg.push_back((d_GST_Sf & 0xFF000000) >> 24);
msg.push_back((d_GST_Sf & 0x00FF0000) >> 16);
msg.push_back((d_GST_Sf & 0x0000FF00) >> 8);
msg.push_back(d_GST_Sf & 0x000000FF);
// extract alpha
for (int k = 5; k >= 0;k--)
{
// TODO: static extracts the MSB in case from larger to shorter int?
msg.push_back(static_cast<uint8_t>((d_osnma_data.d_dsm_kroot_message.alpha >> (i * 8)) & 0xFF)); // extract first 6 bytes of alpha.
}
// compute hash
std::vector<uint8_t> hash;
if (d_osnma_data.d_dsm_kroot_message.hf == 0) // Table 8.
{
hash = d_crypto->computeSHA256(msg);
}
else if (d_osnma_data.d_dsm_kroot_message.hf == 2)
{
hash = d_crypto->computeSHA3_256(msg);
}
else
{
hash = std::vector<uint8_t>(32);
}
// truncate hash
K_I.reserve(lk_bytes); // TODO - case hash function has 512 bits
for (uint16_t i = 0; i < lk_bytes; i++)
{
K_I.push_back(hash[i]);
}
// set parameters for next iteration
GST_SFi -= 30; // next SF time is the actual minus 30 seconds
K_II = K_I; // next key is the actual one
K_I.clear(); // empty the actual one for a new computation
}
// compare computed current key against received key
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> elapsed = end - start;
std::cout << "Galileo OSNMA: TESLA verification ("<< num_of_hashes_needed << " hashes) took " << elapsed.count() << " seconds.\n";
if(K_II.size() != key.size())
{
std::cout << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
return false;
}
if (K_II == validated_key)
{
std::cout << "Galileo OSNMA: tesla key verified successfully " << std::endl;
d_tesla_keys.insert(std::pair(TOW,key));
d_last_verified_key_GST = d_GST_SIS;
d_tesla_key_verified = true; // TODO this boolean only shows that a tesla key is verified, re-setting it all the time is not beautiful
// case one verified and after a time another one not, what would happen in the next MAck processed is that because false
// it would try to verifiy against kroot, but it could as well try against last validated key. This needs to be adressed in the future.
}
else
{
std::cerr << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
if(d_flag_debug){
d_last_verified_key_GST = d_GST_SIS;
d_tesla_key_verified = true;
}
else
d_tesla_key_verified = false;
}
return d_tesla_key_verified;
}
else
{// have to go until Kroot
@ -1438,11 +1243,12 @@ bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TO
std::cout << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
return false;
}
if (K_II == key)
if (K_II == d_osnma_data.d_dsm_kroot_message.kroot)
{
std::cout << "Galileo OSNMA: tesla key verified successfully " << std::endl;
d_tesla_keys.insert(std::pair(TOW,key));
d_tesla_key_verified = true;
d_last_verified_key_GST = d_GST_SIS;
// TODO - propagate result
// TODO - save current tesla key as latest one? propose a map with <GST_Sf, TeslaKey>
// TODO - Tags Sequence Verification: check ADKD[i] follows MACLT sequence
@ -1451,8 +1257,10 @@ bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key, uint32_t TO
{
std::cerr << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
if(d_flag_debug)
if(d_flag_debug){
d_last_verified_key_GST = d_GST_SIS;
d_tesla_key_verified = true;
}
}
return d_tesla_key_verified;
}
@ -1467,7 +1275,15 @@ void osnma_msg_receiver::remove_verified_tags()
{
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)
it = d_tags_awaiting_verify.erase(it);
{
std::cout << "Galileo OSNMA: delete tag for tag Id= "
<< it->second.tag_id << ", PRN_a= "
<< it->second.PRNa << ", TOW="
<< it->second.TOW << ", ADKD= "
<< static_cast<unsigned>(it->second.ADKD) << ", status= "
<< it->second.status << std::endl;
it = d_tags_awaiting_verify.erase(it);
}
else
++it;
}
@ -1495,19 +1311,15 @@ void osnma_msg_receiver::control_tags_awaiting_verify_size()
* @param message The MACK_message to verify.
* @return True if the MACSEQ is valid, false otherwise.
*/
bool osnma_msg_receiver::verify_macseq()
bool osnma_msg_receiver::verify_macseq(const MACK_message& mack)
{
// MACSEQ verification
// verify MACK tags - MACSEQ
OSNMA_data applicable_OSNMA = d_old_OSNMA_buffer[d_old_OSNMA_buffer.size() - 2]; // former subframe
//d_GST_Sf = d_GST_SIS - 30; // time of the start of SF containing MACSEQ // TODO buffer with times? since out of debug not every 30 s a Sf is necessarily received..
std::vector<uint8_t> applicable_key = d_old_OSNMA_buffer.back().d_mack_message.key; // current tesla key ie transmitted in the next subframe
std::vector<uint8_t> applicable_key = d_tesla_keys[mack.TOW + 30]; // current tesla key ie transmitted in the next subframe
std::vector<std::string> sq1{};
std::vector<std::string> sq2{};
std::vector<std::string> applicable_sequence;
const auto it = OSNMA_TABLE_16.find(applicable_OSNMA.d_dsm_kroot_message.maclt);
const auto it = OSNMA_TABLE_16.find(d_osnma_data.d_dsm_kroot_message.maclt);
// TODO as per RG example appears that the seq. q shall also be validated ageints either next or former Sf (depending on GST)
if (it != OSNMA_TABLE_16.cend())
{
@ -1516,11 +1328,11 @@ bool osnma_msg_receiver::verify_macseq()
}
// Assign relevant sequence based on subframe time
if (applicable_OSNMA.d_nav_data.TOW_sf0 % 60 < 30) // tried GST_Sf and it does not support the data present.
if (mack.TOW % 60 < 30) // tried GST_Sf and it does not support the data present.
{
applicable_sequence = sq1;
}
else if (applicable_OSNMA.d_nav_data.TOW_sf0 % 60 >= 30)
else if (mack.TOW % 60 >= 30)
{
applicable_sequence = sq2;
}
@ -1528,7 +1340,7 @@ bool osnma_msg_receiver::verify_macseq()
{
std::cout << "Galileo OSNMA: Mismatch in the GST verification. " << std::endl;
}
if(applicable_OSNMA.d_mack_message.tag_and_info.size() != applicable_sequence.size()-1)
if(mack.tag_and_info.size() != applicable_sequence.size() - 1)
{
std::cout << "Galileo OSNMA: Number of retrieved tags does not match MACLT sequence size!" << std::endl;
return false;
@ -1536,14 +1348,14 @@ bool osnma_msg_receiver::verify_macseq()
std::vector<uint8_t> flxTags {};
std::string tempADKD;
// MACLT verification
for (uint8_t i = 0; i < applicable_OSNMA.d_mack_message.tag_and_info.size(); i++)
for (uint8_t i = 0; i < mack.tag_and_info.size(); i++)
{
tempADKD = applicable_sequence[i+1];
if(tempADKD == "FLX")
{
flxTags.push_back(i); // C: just need to save the index in the sequence
}
else if(applicable_OSNMA.d_mack_message.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]))
{ std::cout << "Galileo OSNMA: Unsuccessful verification of MACSEQ - received ADKD against MAC Look-up table. " << std::endl;
return false; // C: suffices one incorrect to abort and not process the rest of the tags
}
@ -1551,7 +1363,7 @@ bool osnma_msg_receiver::verify_macseq()
// Fixed as well as FLX Tags share first part - Eq. 22 ICD
std::vector<uint8_t> m(5 + 2 * flxTags.size()); // each flx tag brings two bytes
m[0] = static_cast<uint8_t>(applicable_OSNMA.d_nav_data.PRNa); // PRN_A - SVID of the satellite transmiting the tag
m[0] = static_cast<uint8_t>(mack.PRNa); // PRN_A - SVID of the satellite transmiting the tag
m[1] = static_cast<uint8_t>((d_GST_Sf & 0xFF000000) >> 24);
m[2] = static_cast<uint8_t>((d_GST_Sf & 0x00FF0000) >> 16);
m[3] = static_cast<uint8_t>((d_GST_Sf & 0x0000FF00) >> 8);
@ -1559,9 +1371,9 @@ bool osnma_msg_receiver::verify_macseq()
// Case tags flexible - Eq. 21 ICD
for (uint8_t i = 0; i < flxTags.size() ; i++)
{
m[2*i + 5] = applicable_OSNMA.d_mack_message.tag_and_info[flxTags[i]].tag_info.PRN_d;
m[2*i + 6] = applicable_OSNMA.d_mack_message.tag_and_info[flxTags[i]].tag_info.ADKD << 4 |
applicable_OSNMA.d_mack_message.tag_and_info[flxTags[i]].tag_info.cop;
m[2*i + 5] = mack.tag_and_info[flxTags[i]].tag_info.PRN_d;
m[2*i + 6] = mack.tag_and_info[flxTags[i]].tag_info.ADKD << 4 |
mack.tag_and_info[flxTags[i]].tag_info.cop;
}
// m = {0x18, 0x4f, 0x93, 0x53, 0x04, 0x05, 0x0f, 0x1f, 0x0f};
// applicable_key = {0x11, 0x26, 0x47, 0x3b, 0x0e, 0x05, 0x05, 0x35,
@ -1569,11 +1381,11 @@ bool osnma_msg_receiver::verify_macseq()
// applicable_OSNMA.d_mack_message.header.macseq = 0xbb8;
// compute mac
std::vector<uint8_t> mac;
if (applicable_OSNMA.d_dsm_kroot_message.mf == 0) // C: HMAC-SHA-256
if (d_osnma_data.d_dsm_kroot_message.mf == 0) // C: HMAC-SHA-256
{
mac = d_crypto->computeHMAC_SHA_256(applicable_key, m);
}
else if (applicable_OSNMA.d_dsm_kroot_message.mf == 1) // C: CMAC-AES
else if (d_osnma_data.d_dsm_kroot_message.mf == 1) // C: CMAC-AES
{
mac = d_crypto->computeCMAC_AES(applicable_key, m);
}
@ -1584,16 +1396,17 @@ bool osnma_msg_receiver::verify_macseq()
mac_msb = (mac[0] << 8) + mac[1];
}
uint16_t computed_macseq = (mac_msb & 0xFFF0) >> 4;
if (computed_macseq == applicable_OSNMA.d_mack_message.header.macseq && !flxTags.empty())
if (computed_macseq == mack.header.macseq && !flxTags.empty())
return true;
else
return false;
}
bool osnma_msg_receiver::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.PRNa);
if (prn_it != d_satellite_nav_data.end()) {
// PRN was found, check if TOW exists in inner map
std::cout << "Galileo OSNMA: hasData = true " << std::endl;
std::map<uint32_t, NavData>& tow_map = prn_it->second;
auto tow_it = tow_map.find(t.TOW);
if (tow_it != tow_map.end()) {
@ -1604,7 +1417,35 @@ bool osnma_msg_receiver::nav_data_available(Tag& t)
}
} else {
// PRN was not found
std::cout << "Galileo OSNMA: hasData = false " << std::endl;
return false;
}
return false;
}
bool osnma_msg_receiver::tag_has_key_available(Tag& t){
// check adkd of tag
// if adkd = 0 or 4 => look for d_tesla_keys[t.TOW+30]
// if adkd = 12 => look for d_tesla_keys[t.TOW+300]
// return true if available, otherwise false
if (t.ADKD == 0 || t.ADKD == 4)
{
auto it = d_tesla_keys.find(t.TOW + 30);
if (it != d_tesla_keys.end())
{
std::cout << "Galileo OSNMA: hasKey = true " << std::endl;
return true;
}
}
else if (t.ADKD == 12)
{
auto it = d_tesla_keys.find(t.TOW + 300);
if (it != d_tesla_keys.end())
{
std::cout << "Galileo OSNMA: hasKey = true " << std::endl;
return true;
}
}
std::cout << "Galileo OSNMA: hasKey = false " << std::endl;
return false;
}

11
src/core/libs/osnma_msg_receiver.h
View File

@ -74,14 +74,13 @@ private:
void process_mack_message();
void add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData &data);
bool verify_tesla_key(std::vector<uint8_t>& key, uint32_t TOW);
void display_data();
bool verify_tag(MACK_tag_and_info tag_and_info, OSNMA_data applicable_OSNMA, uint8_t tag_position, const std::vector<uint8_t>& applicable_key, NavData applicable_NavData);
void display_data();bool verify_tag(MACK_tag_and_info tag_and_info, OSNMA_data applicable_OSNMA, uint8_t tag_position, const std::vector<uint8_t>& applicable_key, NavData applicable_NavData);
bool verify_tag(Tag& tag);
bool is_next_subframe();
bool nav_data_available(Tag& t);
bool tag_has_nav_data_available(Tag& t);
bool tag_has_key_available(Tag& t);
std::map<uint32_t, std::map<uint32_t, NavData>> d_satellite_nav_data; // map holding NavData sorted by SVID and TOW.
boost::circular_buffer<OSNMA_data> d_old_OSNMA_buffer; // buffer that holds last 12 received OSNMA messages, including current one at back()
std::map<uint32_t, std::vector<uint8_t>> d_tesla_keys; // tesla keys over time, sorted by TOW
std::vector<MACK_message> d_macks_awaiting_MACSEQ_verification;
std::multimap<uint32_t, Tag> d_tags_awaiting_verify; // container with tags to verify from arbitrary SVIDs, sorted by TOW
@ -100,7 +99,7 @@ private:
bool d_tesla_key_verified{false};
bool d_flag_debug{false};
uint32_t d_GST_Sf {}; // C: used for MACSEQ and Tesla Key verification
uint32_t d_old_GST_SIS{0};
uint32_t d_last_verified_key_GST{0};
uint8_t d_Lt_min {}; // minimum equivalent tag length
uint8_t d_Lt_verified_eph {0}; // verified tag bits - ephemeris
uint8_t d_Lt_verified_utc {0}; // verified tag bits - timing
@ -114,7 +113,7 @@ private:
std::vector<uint8_t> d_tags_to_verify{0,4,12};
void remove_verified_tags();
void control_tags_awaiting_verify_size();
bool verify_macseq();
bool verify_macseq(const MACK_message& mack);
};