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

Refactor tag verification logic WIP

Browse Source
This commit is contained in:
cesaaargm 2024-03-16 19:05:01 +01:00 committed by Cesare G. Martínez
parent b5765048de
commit 9120f5e59a
4 changed files with 588 additions and 309 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -38,6 +38,7 @@
#if PMT_USES_BOOST_ANY
#include <boost/any.hpp>
#include <iomanip>
namespace wht = boost;
#else
#include <any>
@ -169,7 +170,7 @@ void osnma_msg_receiver::read_dsm_header(uint8_t dsm_header)
}
/*
* accumulates dsm messages until completeness, then calls process_dsm_message
* accumulates dsm messages
* */
void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_msg)
{
@ -559,9 +560,12 @@ void osnma_msg_receiver::read_and_process_mack_block(const std::shared_ptr<OSNMA
index = index + 4;
}
// update the structure with newly coming NavData
d_osnma_data.d_nav_data.init(osnma_msg); // TODO refactor it
add_satellite_data(d_osnma_data.d_nav_data.PRNa,d_osnma_data.d_nav_data.TOW_sf0,d_osnma_data.d_nav_data); // TODO change place
if (d_osnma_data.d_dsm_kroot_message.ts != 0 && is_next_subframe()/* && time_constraint && */) // C: 4 ts < ts < 10
{ // TODO - correct? with this, MACK would not be processed unless a Kroot is available
if (d_osnma_data.d_dsm_kroot_message.ts != 0 /*&& is_next_subframe() && time_constraint && */) // C: 4 ts < ts < 10
{ // TODO - correct? with this, MACK would not be processed unless a Kroot is available -- no, if TK available MACK sould go on, this has to change in future
read_mack_header();
read_mack_body();
process_mack_message(osnma_msg);
@ -833,324 +837,195 @@ void osnma_msg_receiver::read_mack_body()
void osnma_msg_receiver::process_mack_message(const std::shared_ptr<OSNMA_msg>& osnma_msg)
{
d_flag_debug = true;
// populate d_nav_data with needed data from subframe
d_osnma_data.d_nav_data.init(osnma_msg);
// store MACK, KROOT and NavData needed.
d_old_OSNMA_buffer.push_back(d_osnma_data);
if(d_old_OSNMA_buffer.size() < 3)
{
std::cerr << "Galileo OSNMA: MACK cannot be processed. "<< ", "
<< "Not enough OSNMA messages available"
<< "buffer size: "<< d_old_OSNMA_buffer.size() << std::endl;
return;
}
if(d_kroot_verified == false && d_tesla_key_verified == false)
{
std::cerr << "Galileo OSNMA: MACK cannot be processed. "<< ", "
<< "No Kroot nor TESLA key available" << std::endl;
<< "No Kroot nor TESLA key available" << std::endl;
if(!d_flag_debug)
return; // early return, cannot proceed further without one of the two verified.
}
// Verify tesla key
if(d_tesla_key_verified || d_flag_debug)
{
// TODO - find out I bt. both tesla keys, then hash until then, then compare.
// retrieve latest tesla key
// compute hashes needed
// hash current key until num_hashes and compare
}
else
{// have to go until Kroot
uint32_t num_of_hashes_needed = (d_receiver_time - d_GST_0) / 30 + 1; // Eq. 19 ICD
std::cout << "Galileo OSNMA: TESLA verification ("<< num_of_hashes_needed << " hashes) need to be performed. " << std::endl;
auto start = std::chrono::high_resolution_clock::now();
uint32_t GST_SFi = d_receiver_time;
std::vector<uint8_t> K_II = d_osnma_data.d_mack_message.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)
// TODO sizeof() wrong.
std::vector<uint8_t> msg(sizeof(K_II) + 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() != d_osnma_data.d_mack_message.key.size())
{
std::cout << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
return;
}
if (K_II == d_osnma_data.d_mack_message.key)
{
std::cout << "Galileo OSNMA: tesla key verified successfully " << std::endl;
d_tesla_key_verified = true;
// 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
}
else
{
std::cerr << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
if(!d_flag_debug)
return;
}
// 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);
if(retV){
d_tesla_keys.insert(std::pair(d_osnma_data.d_nav_data.TOW_sf0, d_osnma_data.d_mack_message.key));
}
// 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<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);
// 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())
{
sq1 = it->second.sequence1;
sq2 = it->second.sequence2;
}
// 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.
{
applicable_sequence = sq1;
}
else if (applicable_OSNMA.d_nav_data.TOW_sf0 % 60 >= 30)
{
applicable_sequence = sq2;
}
else
{
std::cout << "Galileo OSNMA: Mismatch in the GST verification. " << std::endl;
}
// compare ADKD of Mack tags with MACLT defined ADKDs
if(applicable_OSNMA.d_mack_message.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;
}
std::vector<uint8_t> flxTags {};
std::string tempADKD;
for (uint8_t i = 0; i < applicable_OSNMA.d_mack_message.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]))
{ std::cout << "Galileo OSNMA: Unsuccessful verification of MACSEQ - received ADKD against MAC Look-up table. " << std::endl;
return; // C: suffices one incorrect to abort and not process the rest of the tags
}
}
// MACSEQ verification
// 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[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);
m[4] = static_cast<uint8_t>(d_GST_Sf & 0x000000FF);
// 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 = {0x18, 0x4f, 0x93, 0x53, 0x04, 0x05, 0x0f, 0x1f, 0x0f};
// applicable_key = {0x11, 0x26, 0x47, 0x3b, 0x0e, 0x05, 0x05, 0x35,
// 0xb0, 0xf2, 0xa7, 0x24, 0x00, 0x22, 0xba, 0x8f};
// 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
{
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 twelve MSBits and compare with received MACSEQ
uint16_t mac_msb = 0;
if (!mac.empty())
{
mac_msb = (mac[0] << 8) + mac[1];
}
uint16_t computed_macseq = (mac_msb & 0xFFF0) >> 4;
// Verify tags if MACSEQ is authenticated
if (computed_macseq == applicable_OSNMA.d_mack_message.header.macseq)
{ // TODO this shall affect only flx tags verification - and currently all tags of a MACK are affected which is undesired
std::cout << "OSNMA: MACSEQ authenticated for PRN_A "
<< osnma_msg->PRN << " with WN="
<< osnma_msg->WN_sf0 << ", TOW="
<< osnma_msg->TOW_sf0 << ". Verifying tags. "
<< std::endl;
uint8_t lt_bits = 0;
const auto it2 = OSNMA_TABLE_11.find(d_osnma_data.d_dsm_kroot_message.ts);
if (it2 != OSNMA_TABLE_11.cend())
{
lt_bits = it2->second;
}
if (lt_bits == 0)
{
return; // C: TODO if Tag length is 0, what is the action? no verification possible of NavData for sure.
}
// 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.
// 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)
// 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(*mack);
if (ret || d_flag_debug){
for(auto& tag:mack->tag_and_info)
{
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;
}
Tag t(tag, mack->TOW, mack->PRNa);
d_tags_awaiting_verify.insert(std::pair(mack->TOW, t));
}
if(flag_cancel_tag_verification)
break;
mack = d_macks_awaiting_MACSEQ_verification.erase(mack);
}
// 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)
else
{
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;
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
++mack;
}
}
// 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_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
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)){
ret = verify_tag(it.second);
/* TODO - take into account:
* - COP: if
* - ADKD type
* - NavData the tag verifies (min. number of bits verified to consider NavData OK)
* */
if(ret)
int a;
/* TODO notify PVT via pmt
* have_new_data() true
* signal which one is verified
* communicate to PVT*/
else
int a;
// also
}
}
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;
// }
//
// }
}
bool osnma_msg_receiver::verify_dsm_pkr(DSM_PKR_message message)
@ -1348,6 +1223,91 @@ bool osnma_msg_receiver::verify_tag(MACK_tag_and_info tag_and_info,
}
return verified;
}
bool osnma_msg_receiver::verify_tag(Tag& tag)
{
std::vector<uint8_t> m = tag.build_message();
std::vector<uint8_t> mac;
if (d_osnma_data.d_dsm_kroot_message.mf == 0) // C: HMAC-SHA-256
{
mac = d_crypto->computeHMAC_SHA_256(d_tesla_keys[tag.TOW], m);
}
else if (d_osnma_data.d_dsm_kroot_message.mf == 1) // C: CMAC-AES
{
mac = d_crypto->computeCMAC_AES(d_tesla_keys[tag.TOW], 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(d_osnma_data.d_dsm_kroot_message.ts);
if (it2 != OSNMA_TABLE_11.cend())
{
lt_bits = it2->second;
}
if (lt_bits == 0)
{
return false;
}
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.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;
}
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;
return false;
}
}
/**
* @brief Checks if the current subframe time is bigger than last one received.
*
@ -1367,3 +1327,268 @@ bool osnma_msg_receiver::is_next_subframe()
return is_bigger;
}
void osnma_msg_receiver::add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData& data)
{
while (d_satellite_data[SV_ID].size() >= 25) {
d_satellite_data[SV_ID].erase(d_satellite_data[SV_ID].begin());
}
d_osnma_data[TOW] = crypto; // crypto
d_satellite_data[SV_ID][TOW] = data; // nav
std::cout << "Galileo OSNMA: added element, size is " << d_satellite_data[SV_ID].size() << std::endl;
}
void osnma_msg_receiver::display_data()
{
if(d_satellite_data.empty())
return;
for(const auto& satellite : d_satellite_data) {
std::cout << "SV_ID: " << satellite.first << std::endl;
for(const auto& towData : satellite.second) {
std::cout << "\tTOW: " << towData.first << " key: ";
for(size_t i = 0; i < towData.second.d_mack_message.key.size(); i++) {
std::cout << std::hex << std::setfill('0') << std::setw(2)
<< static_cast<int>(towData.second.d_mack_message.key[i]) << " ";
}
}
}
}
bool osnma_msg_receiver::verify_tesla_key(std::vector<uint8_t>& key)
{
if(d_tesla_key_verified || d_flag_debug)
{
// TODO - find out I bt. both tesla keys, then hash until then, then compare.
// retrieve latest tesla key
// compute hashes needed
// hash current key until num_hashes and compare
}
else
{// have to go until Kroot
uint32_t num_of_hashes_needed = (d_GST_SIS - d_GST_0) / 30 + 1; // Eq. 19 ICD
std::cout << "Galileo OSNMA: TESLA verification ("<< num_of_hashes_needed << " hashes) need to be performed. " << std::endl;
auto start = std::chrono::high_resolution_clock::now();
uint32_t GST_SFi = d_GST_SIS;
std::vector<uint8_t> K_II = applicable_MACK.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() != applicable_MACK.key.size())
{
std::cout << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
return;
}
if (K_II == applicable_MACK.key)
{
std::cout << "Galileo OSNMA: tesla key verified successfully " << std::endl;
d_keys.insert(std::pair(applicable_MACK.TOW,applicable_MACK.key));
d_tesla_key_verified = true;
// 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
}
else
{
std::cerr << "Galileo OSNMA: Error during tesla key verification. " << std::endl;
if(!d_flag_debug)
return;
}
}
}
/**
* @brief Removes the tags that have been verified from the multimap d_tags_awaiting_verify.
*
* This function iterates through the multimap d_tags_awaiting_verify, and removes the tags that have a status of SUCCESS or FAIL.
*/
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);
else
++it;
}
}
/**
* @brief Control the size of the tags awaiting verification multimap.
*
* This function checks the size of the multimap `d_tags_awaiting_verify` and removes
* elements from the beginning until the size is no longer greater than 60.
* The purpose is to limit the size of the multimap and prevent it from consuming
* excessive memory.
*/
void osnma_msg_receiver::control_tags_awaiting_verify_size()
{
while(d_tags_awaiting_verify.size() > 60)
{
d_tags_awaiting_verify.erase(d_tags_awaiting_verify.begin());
}
}
/**
* @brief Verifies the MACSEQ of a received MACK_message.
*
* \details checks for each tag in the retrieved mack message if its flexible (MACSEQ) or not (MACSEQ/MACLT depending on configuration param. (TODO)
* @param message The MACK_message to verify.
* @return True if the MACSEQ is valid, false otherwise.
*/
bool osnma_msg_receiver::verify_macseq(MACK_message& message)
{
// 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<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);
// 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())
{
sq1 = it->second.sequence1;
sq2 = it->second.sequence2;
}
// 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.
{
applicable_sequence = sq1;
}
else if (applicable_OSNMA.d_nav_data.TOW_sf0 % 60 >= 30)
{
applicable_sequence = sq2;
}
else
{
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)
{
std::cout << "Galileo OSNMA: Number of retrieved tags does not match MACLT sequence size!" << std::endl;
return;
}
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++)
{
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]))
{ std::cout << "Galileo OSNMA: Unsuccessful verification of MACSEQ - received ADKD against MAC Look-up table. " << std::endl;
return; // C: suffices one incorrect to abort and not process the rest of the tags
}
}
// 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[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);
m[4] = static_cast<uint8_t>(d_GST_Sf & 0x000000FF);
// 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 = {0x18, 0x4f, 0x93, 0x53, 0x04, 0x05, 0x0f, 0x1f, 0x0f};
// applicable_key = {0x11, 0x26, 0x47, 0x3b, 0x0e, 0x05, 0x05, 0x35,
// 0xb0, 0xf2, 0xa7, 0x24, 0x00, 0x22, 0xba, 0x8f};
// 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
{
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 twelve MSBits and compare with received MACSEQ
uint16_t mac_msb = 0;
if (!mac.empty())
{
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())
return true;
else
return false;
}
bool osnma_msg_receiver::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::map<uint32_t, NavData>& tow_map = prn_it->second;
auto tow_it = tow_map.find(t.TOW);
if (tow_it != tow_map.end()) {
return true;
} else {
// TOW not found
return false;
}
} else {
// PRN was not found
return false;
}
return false;
}

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

@ -72,14 +72,23 @@ private:
void read_mack_header();
void read_mack_body();
void process_mack_message(const std::shared_ptr<OSNMA_msg>& osnma_msg);
void add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData &data);
bool verify_tesla_key(std::vector<uint8_t>& key);
void const 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);
//boost::circular_buffer<MACK_message> d_old_mack_message;
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
std::unique_ptr<OSNMA_DSM_Reader> d_dsm_reader;
std::unique_ptr<Gnss_Crypto> d_crypto;
std::array<std::array<uint8_t, 256>, 16> d_dsm_message{}; // C: each dsm[0-15] has 2048 bits
std::array<std::array<uint8_t, 256>, 16> d_dsm_message{}; // structure for recording DSM blocks, when filled it sends them to parse and resets itself.
std::array<std::array<uint8_t, 16>, 16> d_dsm_id_received{};
std::array<uint16_t, 16> d_number_of_blocks{};
std::array<uint8_t, 60> d_mack_message{}; // C: 480 b
@ -103,7 +112,9 @@ private:
enum tags_to_verify{all,utc,slow_eph, eph, none}; // TODO is this safe? I hope so
tags_to_verify d_tags_allowed{tags_to_verify::all};
std::vector<uint8_t> d_tags_to_verify{0,4,12};
bool is_next_subframe();
void remove_verified_tags();
void control_tags_awaiting_verify_size();
bool verify_macseq(MACK_message& message);
};

6
src/core/system_parameters/osnma_data.cc
View File

@ -163,3 +163,9 @@ void NavData::generate_utc_vector()
utc_vector.push_back(static_cast<uint8_t>(bit_buffer));
}
}
std::vector<uint8_t> Tag::build_message()
{
// TODO
return std::vector<uint8_t>();
}

37
src/core/system_parameters/osnma_data.h
View File

@ -121,6 +121,8 @@ public:
MACK_header header;
std::vector<MACK_tag_and_info> tag_and_info;
std::vector<uint8_t> key;
uint32_t TOW; // TODO duplicated variable
uint32_t PRNa;
};
class NavData
@ -157,9 +159,44 @@ public:
DSM_KROOT_message d_dsm_kroot_message;
MACK_message d_mack_message;
NavData d_nav_data;
};
class Tag
{
public:
enum e_verification_status{
SUCCESS,
FAIL,
UNVERIFIED};
Tag(const MACK_tag_and_info& MTI, uint32_t TOW, uint32_t PRNa)
: tag_id(id_counter++),
TOW(TOW),
PRNa(PRNa),
status(UNVERIFIED),
received_tag(MTI.tag),
computed_tag(0),
PRN_d(MTI.tag_info.PRN_d),
ADKD(MTI.tag_info.ADKD),
cop(MTI.tag_info.cop)
{
}
const uint32_t tag_id;
uint32_t TOW;
uint32_t PRNa;
e_verification_status status;
uint64_t received_tag;
std::vector<uint8_t> build_message();
uint32_t static id_counter;
uint64_t computed_tag;
uint8_t PRN_d;
uint8_t ADKD;
uint8_t cop;
};
/** \} */
/** \} */
#endif // GNSS_SDR_OSNMA_DATA_H