From 9120f5e59a1f9d0bab01f9ed0ff1082fc49c049e Mon Sep 17 00:00:00 2001 From: cesaaargm Date: Sat, 16 Mar 2024 19:05:01 +0100 Subject: [PATCH] Refactor tag verification logic WIP --- src/core/libs/osnma_msg_receiver.cc | 837 ++++++++++++++--------- src/core/libs/osnma_msg_receiver.h | 17 +- src/core/system_parameters/osnma_data.cc | 6 + src/core/system_parameters/osnma_data.h | 37 + 4 files changed, 588 insertions(+), 309 deletions(-) diff --git a/src/core/libs/osnma_msg_receiver.cc b/src/core/libs/osnma_msg_receiver.cc index f439b97b8..21321e685 100644 --- a/src/core/libs/osnma_msg_receiver.cc +++ b/src/core/libs/osnma_msg_receiver.cc @@ -38,6 +38,7 @@ #if PMT_USES_BOOST_ANY #include +#include namespace wht = boost; #else #include @@ -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) { @@ -559,9 +560,12 @@ void osnma_msg_receiver::read_and_process_mack_block(const std::shared_ptr& 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 K_II = d_osnma_data.d_mack_message.key; - std::vector 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 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((d_osnma_data.d_dsm_kroot_message.alpha >> (i * 8)) & 0xFF)); // extract first 6 bytes of alpha. - } - // compute hash - std::vector 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(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 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 - // 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 applicable_key = d_old_OSNMA_buffer.back().d_mack_message.key; // current tesla key ie transmitted in the next subframe - std::vector sq1{}; - std::vector sq2{}; - std::vector 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 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 m(5 + 2 * flxTags.size()); // each flx tag brings two bytes - m[0] = static_cast(applicable_OSNMA.d_nav_data.PRNa); // PRN_A - SVID of the satellite transmiting the tag - m[1] = static_cast((d_GST_Sf & 0xFF000000) >> 24); - m[2] = static_cast((d_GST_Sf & 0x00FF0000) >> 16); - m[3] = static_cast((d_GST_Sf & 0x0000FF00) >> 8); - m[4] = static_cast(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 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 m = tag.build_message(); + + std::vector 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(mac[0]) << (lt_bits - 8); + computed_mac += (static_cast(mac[1]) << (lt_bits - 16)); + if (lt_bits == 20) + { + computed_mac += (static_cast(mac[1] & 0xF0) >> 4); + } + else if (lt_bits == 24) + { + computed_mac += static_cast(mac[2]); + } + else if (lt_bits == 28) + { + computed_mac += (static_cast(mac[2]) << 4); + computed_mac += (static_cast(mac[3] & 0xF0) >> 4); + } + else if (lt_bits == 32) + { + computed_mac += (static_cast(mac[2]) << 8); + computed_mac += static_cast(mac[3]); + } + else if (lt_bits == 40) + { + computed_mac += (static_cast(mac[2]) << 16); + computed_mac += (static_cast(mac[3]) << 8); + computed_mac += static_cast(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(towData.second.d_mack_message.key[i]) << " "; + } + } + } +} +bool osnma_msg_receiver::verify_tesla_key(std::vector& 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 K_II = applicable_MACK.key; + std::vector 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 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((d_osnma_data.d_dsm_kroot_message.alpha >> (i * 8)) & 0xFF)); // extract first 6 bytes of alpha. + } + // compute hash + std::vector 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(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 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 + // 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 applicable_key = d_old_OSNMA_buffer.back().d_mack_message.key; // current tesla key ie transmitted in the next subframe + std::vector sq1{}; + std::vector sq2{}; + std::vector 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 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 m(5 + 2 * flxTags.size()); // each flx tag brings two bytes + m[0] = static_cast(applicable_OSNMA.d_nav_data.PRNa); // PRN_A - SVID of the satellite transmiting the tag + m[1] = static_cast((d_GST_Sf & 0xFF000000) >> 24); + m[2] = static_cast((d_GST_Sf & 0x00FF0000) >> 16); + m[3] = static_cast((d_GST_Sf & 0x0000FF00) >> 8); + m[4] = static_cast(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 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& 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; +} diff --git a/src/core/libs/osnma_msg_receiver.h b/src/core/libs/osnma_msg_receiver.h index b6a7bb812..9fde2d386 100644 --- a/src/core/libs/osnma_msg_receiver.h +++ b/src/core/libs/osnma_msg_receiver.h @@ -72,14 +72,23 @@ private: void read_mack_header(); void read_mack_body(); void process_mack_message(const std::shared_ptr& osnma_msg); + void add_satellite_data(uint32_t SV_ID, uint32_t TOW, const NavData &data); + bool verify_tesla_key(std::vector& 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& applicable_key, NavData applicable_NavData); + bool verify_tag(Tag& tag); + bool is_next_subframe(); + bool nav_data_available(Tag& t); - //boost::circular_buffer d_old_mack_message; + std::map> d_satellite_nav_data; // map holding NavData sorted by SVID and TOW. boost::circular_buffer d_old_OSNMA_buffer; // buffer that holds last 12 received OSNMA messages, including current one at back() + std::map> d_tesla_keys; // tesla keys over time, sorted by TOW + std::vector d_macks_awaiting_MACSEQ_verification; + std::multimap d_tags_awaiting_verify; // container with tags to verify from arbitrary SVIDs, sorted by TOW std::unique_ptr d_dsm_reader; std::unique_ptr d_crypto; - std::array, 16> d_dsm_message{}; // C: each dsm[0-15] has 2048 bits + std::array, 16> d_dsm_message{}; // structure for recording DSM blocks, when filled it sends them to parse and resets itself. std::array, 16> d_dsm_id_received{}; std::array d_number_of_blocks{}; std::array 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 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); }; diff --git a/src/core/system_parameters/osnma_data.cc b/src/core/system_parameters/osnma_data.cc index 9a291330d..cb76b9107 100644 --- a/src/core/system_parameters/osnma_data.cc +++ b/src/core/system_parameters/osnma_data.cc @@ -163,3 +163,9 @@ void NavData::generate_utc_vector() utc_vector.push_back(static_cast(bit_buffer)); } } + +std::vector Tag::build_message() +{ + // TODO + return std::vector(); +} diff --git a/src/core/system_parameters/osnma_data.h b/src/core/system_parameters/osnma_data.h index 4bc78ebbb..c9598e396 100644 --- a/src/core/system_parameters/osnma_data.h +++ b/src/core/system_parameters/osnma_data.h @@ -121,6 +121,8 @@ public: MACK_header header; std::vector tag_and_info; std::vector 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 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