mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 12:10:34 +00:00
Refactor tag verification logic WIP
This commit is contained in:
parent
b5765048de
commit
9120f5e59a
@ -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;
|
||||
}
|
||||
|
@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
@ -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>();
|
||||
}
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user