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