mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-13 19:50:34 +00:00
Add work on OSNMA rx
This commit is contained in:
Carles Fernandez
parent
ce50e160a7
commit
7147959186
@ -22,6 +22,7 @@ set(CORE_LIBS_SOURCES
|
||||
nav_message_udp_sink.cc
|
||||
galileo_tow_map.cc
|
||||
osnma_msg_receiver.cc
|
||||
sha256.cc
|
||||
)
|
||||
|
||||
set(CORE_LIBS_HEADERS
|
||||
@ -39,6 +40,7 @@ set(CORE_LIBS_HEADERS
|
||||
nav_message_monitor.h
|
||||
galileo_tow_map.h
|
||||
osnma_msg_receiver.h
|
||||
sha256.h
|
||||
)
|
||||
|
||||
if(ENABLE_FPGA)
|
||||
|
||||
@ -176,9 +176,9 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
|
||||
// Annotate bid
|
||||
d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id][d_osnma_data.d_dsm_header.dsm_block_id] = 1;
|
||||
|
||||
// is message complete? -> process_dsm_message(osnma_msg)
|
||||
// is message complete? -> Process DSM message
|
||||
if ((d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] != 0) &&
|
||||
(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] == std::accumulate(d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].begin(), d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].end(), 0)))
|
||||
(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id] == std::accumulate(d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].cbegin(), d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id].cend(), 0)))
|
||||
{
|
||||
std::vector<uint8_t> dsm_msg(std::size_t(d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]) * 13, 0);
|
||||
for (uint32_t i = 0; i < d_number_of_blocks[d_osnma_data.d_dsm_header.dsm_id]; i++)
|
||||
@ -190,64 +190,35 @@ void osnma_msg_receiver::read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_
|
||||
}
|
||||
d_dsm_message[d_osnma_data.d_dsm_header.dsm_id] = std::array<uint8_t, 256>{};
|
||||
d_dsm_id_received[d_osnma_data.d_dsm_header.dsm_id] = std::array<uint8_t, 16>{};
|
||||
process_dsm_message(dsm_msg);
|
||||
process_dsm_message(dsm_msg, osnma_msg->hkroot[0]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg)
|
||||
void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg, uint8_t nma_header)
|
||||
{
|
||||
if (d_osnma_data.d_dsm_header.dsm_id < 12)
|
||||
{
|
||||
LOG(WARNING) << "OSNMA: DSM-KROOT message received.";
|
||||
// DSM-KROOT message
|
||||
d_osnma_data.d_dsm_kroot_message.nb_dk = get_number_blocks_index(dsm_msg[0]);
|
||||
d_osnma_data.d_dsm_kroot_message.pkid = (dsm_msg[0] & 0b00001111);
|
||||
d_osnma_data.d_dsm_kroot_message.cidkr = (dsm_msg[1] & 0b11000000) >> 6;
|
||||
d_osnma_data.d_dsm_kroot_message.reserved1 = (dsm_msg[1] & 0b00110000) >> 4;
|
||||
d_osnma_data.d_dsm_kroot_message.hf = (dsm_msg[1] & 0b00001100) >> 2;
|
||||
d_osnma_data.d_dsm_kroot_message.pkid = get_pkid(dsm_msg); // (dsm_msg[0] & 0b00001111);
|
||||
d_osnma_data.d_dsm_kroot_message.cidkr = get_cidkr(dsm_msg); // (dsm_msg[1] & 0b11000000) >> 6;
|
||||
d_osnma_data.d_dsm_kroot_message.reserved1 = get_dsm_reserved1(dsm_msg); // (dsm_msg[1] & 0b00110000) >> 4;
|
||||
d_osnma_data.d_dsm_kroot_message.hf = get_hf(dsm_msg); // (dsm_msg[1] & 0b00001100) >> 2;
|
||||
d_osnma_data.d_dsm_kroot_message.mf = get_mf(dsm_msg); // (dsm_msg[1] & 0b00000011);
|
||||
d_osnma_data.d_dsm_kroot_message.ks = get_ks(dsm_msg); // (dsm_msg[2] & 0b11110000) >> 4;
|
||||
d_osnma_data.d_dsm_kroot_message.ts = get_ts(dsm_msg); // (dsm_msg[2] & 0b00001111);
|
||||
d_osnma_data.d_dsm_kroot_message.maclt = get_maclt(dsm_msg); // dsm_msg[3];
|
||||
d_osnma_data.d_dsm_kroot_message.reserved = get_dsm_reserved(dsm_msg); // (dsm_msg[4] & 0b11110000) >> 4;
|
||||
d_osnma_data.d_dsm_kroot_message.wn_k = get_wn_k(dsm_msg); // static_cast<uint16_t>((dsm_msg[4] & 0b00001111) << 8) + static_cast<uint16_t>(dsm_msg[5]);
|
||||
d_osnma_data.d_dsm_kroot_message.towh_k = get_towh_k(dsm_msg); // dsm_msg[6];
|
||||
d_osnma_data.d_dsm_kroot_message.alpha = get_alpha(dsm_msg);
|
||||
|
||||
d_osnma_data.d_dsm_kroot_message.mf = (dsm_msg[1] & 0b00000011);
|
||||
d_osnma_data.d_dsm_kroot_message.ks = (dsm_msg[2] & 0b11110000) >> 4;
|
||||
d_osnma_data.d_dsm_kroot_message.ts = (dsm_msg[2] & 0b00001111);
|
||||
d_osnma_data.d_dsm_kroot_message.maclt = dsm_msg[3];
|
||||
d_osnma_data.d_dsm_kroot_message.reserved = (dsm_msg[4] & 0b11110000) >> 4;
|
||||
d_osnma_data.d_dsm_kroot_message.wn_k = static_cast<uint16_t>((dsm_msg[4] & 0b00001111) << 8) +
|
||||
static_cast<uint16_t>(dsm_msg[5]);
|
||||
d_osnma_data.d_dsm_kroot_message.towh_k = dsm_msg[6];
|
||||
uint16_t bytes_lk = get_lk_bits(d_osnma_data.d_dsm_kroot_message.ks) / 8;
|
||||
d_osnma_data.d_dsm_kroot_message.kroot = get_kroot(dsm_msg, bytes_lk);
|
||||
|
||||
uint16_t l_dk_bits = 0;
|
||||
const auto it = OSNMA_TABLE_7.find(d_osnma_data.d_dsm_kroot_message.nb_dk);
|
||||
if (it != OSNMA_TABLE_7.cend())
|
||||
{
|
||||
l_dk_bits = it->second.second;
|
||||
}
|
||||
uint16_t lk_bits = 0;
|
||||
const auto it2 = OSNMA_TABLE_10.find(d_osnma_data.d_dsm_kroot_message.ks);
|
||||
if (it2 != OSNMA_TABLE_10.cend())
|
||||
{
|
||||
lk_bits = it2->second;
|
||||
}
|
||||
|
||||
d_osnma_data.d_dsm_kroot_message.alpha = (static_cast<uint64_t>(dsm_msg[7]) << 40) +
|
||||
(static_cast<uint64_t>(dsm_msg[8]) << 32) +
|
||||
(static_cast<uint64_t>(dsm_msg[9]) << 24) +
|
||||
(static_cast<uint64_t>(dsm_msg[10]) << 16) +
|
||||
(static_cast<uint64_t>(dsm_msg[11]) << 8) +
|
||||
static_cast<uint64_t>(dsm_msg[12]);
|
||||
uint16_t bytes_lk = lk_bits / 8;
|
||||
d_osnma_data.d_dsm_kroot_message.kroot = std::vector<uint8_t>(bytes_lk, 0);
|
||||
for (uint32_t k = 0; k < bytes_lk; k++)
|
||||
{
|
||||
d_osnma_data.d_dsm_kroot_message.kroot[k] = dsm_msg[13 + k];
|
||||
}
|
||||
|
||||
std::string hash_function;
|
||||
const auto it3 = OSNMA_TABLE_8.find(d_osnma_data.d_dsm_kroot_message.hf);
|
||||
if (it3 != OSNMA_TABLE_8.cend())
|
||||
{
|
||||
hash_function = it3->second;
|
||||
}
|
||||
std::string hash_function = get_hash_function(d_osnma_data.d_dsm_kroot_message.hf);
|
||||
|
||||
uint16_t l_ds_bits = 0;
|
||||
const auto it4 = OSNMA_TABLE_15.find(hash_function);
|
||||
@ -257,18 +228,43 @@ void osnma_msg_receiver::process_dsm_message(const std::vector<uint8_t>& dsm_msg
|
||||
}
|
||||
uint16_t l_ds_bytes = l_ds_bits / 8;
|
||||
d_osnma_data.d_dsm_kroot_message.ds = std::vector<uint8_t>(l_ds_bytes, 0);
|
||||
for (uint32_t k = 0; k < l_ds_bytes; k++)
|
||||
for (uint16_t k = 0; k < l_ds_bytes; k++)
|
||||
{
|
||||
d_osnma_data.d_dsm_kroot_message.ds[k] = dsm_msg[13 + bytes_lk + k];
|
||||
}
|
||||
uint16_t l_pdk_bytes = (l_dk_bits - 104 - lk_bits - l_ds_bits) / 8;
|
||||
uint16_t l_pdk_bytes = (l_ds_bytes - 13 - bytes_lk - l_ds_bytes);
|
||||
|
||||
d_osnma_data.d_dsm_kroot_message.p_dk = std::vector<uint8_t>(l_pdk_bytes, 0);
|
||||
for (uint32_t k = 0; k < l_ds_bytes; k++)
|
||||
for (uint16_t k = 0; k < l_ds_bytes; k++)
|
||||
{
|
||||
d_osnma_data.d_dsm_kroot_message.p_dk[k] = dsm_msg[13 + bytes_lk + l_ds_bytes + k];
|
||||
}
|
||||
// validation?
|
||||
// validation of padding
|
||||
std::vector<uint8_t> M;
|
||||
M.push_back(nma_header);
|
||||
for (int i = 1; i < 13; i++)
|
||||
{
|
||||
M.push_back(dsm_msg[i]);
|
||||
}
|
||||
for (uint16_t i = 0; i < bytes_lk; i++)
|
||||
{
|
||||
M.push_back(dsm_msg[13 + i]);
|
||||
}
|
||||
for (uint16_t k = 0; k < l_ds_bytes; k++)
|
||||
{
|
||||
M.push_back(dsm_msg[13 + bytes_lk + k]);
|
||||
}
|
||||
sha256.update(&M[0], M.size());
|
||||
uint8_t* digest = sha256.digest();
|
||||
std::vector<uint8_t> p_dk_computed;
|
||||
for (uint16_t i = 0; i < l_pdk_bytes; i++)
|
||||
{
|
||||
p_dk_computed.push_back(digest[i]);
|
||||
}
|
||||
if (d_osnma_data.d_dsm_kroot_message.p_dk == p_dk_computed)
|
||||
{
|
||||
std::cout << "OSNMA: DSM-KROOT message validated" << std::endl;
|
||||
}
|
||||
}
|
||||
else if (d_osnma_data.d_dsm_header.dsm_id >= 12 && d_osnma_data.d_dsm_header.dsm_id < 16)
|
||||
{
|
||||
@ -350,6 +346,10 @@ void osnma_msg_receiver::read_mack_header()
|
||||
{
|
||||
lt_bits = it->second;
|
||||
}
|
||||
if (lt_bits == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -22,10 +22,11 @@
|
||||
#include "galileo_inav_message.h" // for OSNMA_msg
|
||||
#include "gnss_block_interface.h" // for gnss_shared_ptr
|
||||
#include "osnma_data.h" // for OSNMA_data
|
||||
#include <gnuradio/block.h> // for gr::block
|
||||
#include <pmt/pmt.h> // for pmt::pmt_t
|
||||
#include <array> // for std::array
|
||||
#include <memory> // for std::shared_ptr
|
||||
#include "sha256.h"
|
||||
#include <gnuradio/block.h> // for gr::block
|
||||
#include <pmt/pmt.h> // for pmt::pmt_t
|
||||
#include <array> // for std::array
|
||||
#include <memory> // for std::shared_ptr
|
||||
#include <vector>
|
||||
|
||||
/** \addtogroup Core
|
||||
@ -60,12 +61,13 @@ private:
|
||||
void read_dsm_header(uint8_t dsm_header);
|
||||
void read_dsm_block(const std::shared_ptr<OSNMA_msg>& osnma_msg);
|
||||
void read_mack_block(const std::shared_ptr<OSNMA_msg>& osnma_msg);
|
||||
void process_dsm_message(const std::vector<uint8_t>& dsm_msg);
|
||||
void process_dsm_message(const std::vector<uint8_t>& dsm_msg, uint8_t nma_header);
|
||||
|
||||
void read_mack_header();
|
||||
void read_mack_info_and_tags();
|
||||
void read_mack_key();
|
||||
void read_mack_padding();
|
||||
SHA256 sha256;
|
||||
|
||||
std::array<std::array<uint8_t, 256>, 16> d_dsm_message{};
|
||||
std::array<std::array<uint8_t, 16>, 16> d_dsm_id_received{};
|
||||
|
||||
193
src/core/libs/sha256.cc
Normal file
193
src/core/libs/sha256.cc
Normal file
@ -0,0 +1,193 @@
|
||||
/*!
|
||||
* \file sha256.cc
|
||||
* \brief Class foir computing SHA256
|
||||
* \author Carles Fernandez-Prades, 2023. cfernandez(at)cttc.es
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "sha256.h"
|
||||
#include <cstring>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
|
||||
SHA256::SHA256() : d_blocklen(0), d_bitlen(0)
|
||||
{
|
||||
d_state[0] = 0x6a09e667;
|
||||
d_state[1] = 0xbb67ae85;
|
||||
d_state[2] = 0x3c6ef372;
|
||||
d_state[3] = 0xa54ff53a;
|
||||
d_state[4] = 0x510e527f;
|
||||
d_state[5] = 0x9b05688c;
|
||||
d_state[6] = 0x1f83d9ab;
|
||||
d_state[7] = 0x5be0cd19;
|
||||
}
|
||||
|
||||
void SHA256::update(const uint8_t *data, size_t length)
|
||||
{
|
||||
for (size_t i = 0; i < length; i++)
|
||||
{
|
||||
d_data[d_blocklen++] = data[i];
|
||||
if (d_blocklen == 64)
|
||||
{
|
||||
transform();
|
||||
|
||||
// End of the block
|
||||
d_bitlen += 512;
|
||||
d_blocklen = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SHA256::update(const std::string &data)
|
||||
{
|
||||
update(reinterpret_cast<const uint8_t *>(data.c_str()), data.size());
|
||||
}
|
||||
|
||||
uint8_t *SHA256::digest()
|
||||
{
|
||||
uint8_t *hash = new uint8_t[32];
|
||||
|
||||
pad();
|
||||
revert(hash);
|
||||
|
||||
return hash;
|
||||
}
|
||||
|
||||
uint32_t SHA256::rotr(uint32_t x, uint32_t n)
|
||||
{
|
||||
return (x >> n) | (x << (32 - n));
|
||||
}
|
||||
|
||||
uint32_t SHA256::choose(uint32_t e, uint32_t f, uint32_t g)
|
||||
{
|
||||
return (e & f) ^ (~e & g);
|
||||
}
|
||||
|
||||
uint32_t SHA256::majority(uint32_t a, uint32_t b, uint32_t c)
|
||||
{
|
||||
return (a & (b | c)) | (b & c);
|
||||
}
|
||||
|
||||
uint32_t SHA256::sig0(uint32_t x)
|
||||
{
|
||||
return SHA256::rotr(x, 7) ^ SHA256::rotr(x, 18) ^ (x >> 3);
|
||||
}
|
||||
|
||||
uint32_t SHA256::sig1(uint32_t x)
|
||||
{
|
||||
return SHA256::rotr(x, 17) ^ SHA256::rotr(x, 19) ^ (x >> 10);
|
||||
}
|
||||
|
||||
void SHA256::transform()
|
||||
{
|
||||
uint32_t maj, xorA, ch, xorE, sum, newA, newE, m[64];
|
||||
uint32_t state[8];
|
||||
|
||||
for (uint8_t i = 0, j = 0; i < 16; i++, j += 4)
|
||||
{ // Split data in 32 bit blocks for the 16 first words
|
||||
m[i] = (d_data[j] << 24) | (d_data[j + 1] << 16) | (d_data[j + 2] << 8) | (d_data[j + 3]);
|
||||
}
|
||||
|
||||
for (uint8_t k = 16; k < 64; k++)
|
||||
{ // Remaining 48 blocks
|
||||
m[k] = SHA256::sig1(m[k - 2]) + m[k - 7] + SHA256::sig0(m[k - 15]) + m[k - 16];
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < 8; i++)
|
||||
{
|
||||
state[i] = d_state[i];
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < 64; i++)
|
||||
{
|
||||
maj = SHA256::majority(state[0], state[1], state[2]);
|
||||
xorA = SHA256::rotr(state[0], 2) ^ SHA256::rotr(state[0], 13) ^ SHA256::rotr(state[0], 22);
|
||||
|
||||
ch = choose(state[4], state[5], state[6]);
|
||||
|
||||
xorE = SHA256::rotr(state[4], 6) ^ SHA256::rotr(state[4], 11) ^ SHA256::rotr(state[4], 25);
|
||||
|
||||
sum = m[i] + K[i] + state[7] + ch + xorE;
|
||||
newA = xorA + maj + sum;
|
||||
newE = state[3] + sum;
|
||||
|
||||
state[7] = state[6];
|
||||
state[6] = state[5];
|
||||
state[5] = state[4];
|
||||
state[4] = newE;
|
||||
state[3] = state[2];
|
||||
state[2] = state[1];
|
||||
state[1] = state[0];
|
||||
state[0] = newA;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < 8; i++)
|
||||
{
|
||||
d_state[i] += state[i];
|
||||
}
|
||||
}
|
||||
|
||||
void SHA256::pad()
|
||||
{
|
||||
uint64_t i = d_blocklen;
|
||||
uint8_t end = d_blocklen < 56 ? 56 : 64;
|
||||
|
||||
d_data[i++] = 0x80; // Append a bit 1
|
||||
while (i < end)
|
||||
{
|
||||
d_data[i++] = 0x00; // Pad with zeros
|
||||
}
|
||||
|
||||
if (d_blocklen >= 56)
|
||||
{
|
||||
transform();
|
||||
memset(d_data, 0, 56);
|
||||
}
|
||||
|
||||
// Append to the padding the total message's length in bits and transform.
|
||||
d_bitlen += d_blocklen * 8;
|
||||
d_data[63] = d_bitlen;
|
||||
d_data[62] = d_bitlen >> 8;
|
||||
d_data[61] = d_bitlen >> 16;
|
||||
d_data[60] = d_bitlen >> 24;
|
||||
d_data[59] = d_bitlen >> 32;
|
||||
d_data[58] = d_bitlen >> 40;
|
||||
d_data[57] = d_bitlen >> 48;
|
||||
d_data[56] = d_bitlen >> 56;
|
||||
transform();
|
||||
}
|
||||
|
||||
void SHA256::revert(uint8_t *hash)
|
||||
{
|
||||
// SHA uses big endian byte ordering
|
||||
// Revert all bytes
|
||||
for (uint8_t i = 0; i < 4; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < 8; j++)
|
||||
{
|
||||
hash[i + (j * 4)] = (d_state[j] >> (24 - i * 8)) & 0x000000ff;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::string SHA256::toString(const uint8_t *digest)
|
||||
{
|
||||
std::stringstream s;
|
||||
s << std::setfill('0') << std::hex;
|
||||
|
||||
for (uint8_t i = 0; i < 32; i++)
|
||||
{
|
||||
s << std::setw(2) << (unsigned int)digest[i];
|
||||
}
|
||||
|
||||
return s.str();
|
||||
}
|
||||
68
src/core/libs/sha256.h
Normal file
68
src/core/libs/sha256.h
Normal file
@ -0,0 +1,68 @@
|
||||
/*!
|
||||
* \file sha256.h
|
||||
* \brief Class foir computing SHA256
|
||||
* \author Carles Fernandez-Prades, 2023. cfernandez(at)cttc.es
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2023 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
// https://github.com/System-Glitch/SHA256/blob/master/src/SHA256.cpp
|
||||
#ifndef SHA256_H
|
||||
#define SHA256_H
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
|
||||
class SHA256
|
||||
{
|
||||
public:
|
||||
SHA256();
|
||||
void update(const uint8_t *data, size_t length);
|
||||
void update(const std::string &data);
|
||||
uint8_t *digest();
|
||||
|
||||
static std::string toString(const uint8_t *digest);
|
||||
|
||||
private:
|
||||
uint8_t d_data[64];
|
||||
uint32_t d_blocklen;
|
||||
uint64_t d_bitlen;
|
||||
uint32_t d_state[8]; // A, B, C, D, E, F, G, H
|
||||
|
||||
static constexpr std::array<uint32_t, 64> K = {
|
||||
0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5,
|
||||
0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
|
||||
0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
|
||||
0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
|
||||
0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc,
|
||||
0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
|
||||
0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7,
|
||||
0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
|
||||
0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
|
||||
0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
|
||||
0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3,
|
||||
0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
|
||||
0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
|
||||
0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
|
||||
0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
|
||||
0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2};
|
||||
|
||||
static uint32_t rotr(uint32_t x, uint32_t n);
|
||||
static uint32_t choose(uint32_t e, uint32_t f, uint32_t g);
|
||||
static uint32_t majority(uint32_t a, uint32_t b, uint32_t c);
|
||||
static uint32_t sig0(uint32_t x);
|
||||
static uint32_t sig1(uint32_t x);
|
||||
void transform();
|
||||
void pad();
|
||||
void revert(uint8_t *hash);
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -155,7 +155,16 @@ constexpr std::uint8_t mask_cpks{0x07};
|
||||
constexpr std::uint8_t mask_nma_header_reserved{0x01};
|
||||
constexpr std::uint8_t mask_dsm_id{0xF0};
|
||||
constexpr std::uint8_t mask_dsm_block_id{0x0F};
|
||||
constexpr std::uint8_t mask_dsm_number_blocks{0XF0};
|
||||
constexpr std::uint8_t mask_dsm_number_blocks{0xF0};
|
||||
constexpr std::uint8_t mask_dsm_pkid{0x0F};
|
||||
constexpr std::uint8_t mask_dsm_cidkr{0xC0};
|
||||
constexpr std::uint8_t mask_dsm_reserved1{0x30};
|
||||
constexpr std::uint8_t mask_dsm_hf{0x0C};
|
||||
constexpr std::uint8_t mask_dsm_mf{0x03};
|
||||
constexpr std::uint8_t mask_dsm_ks{0xF0};
|
||||
constexpr std::uint8_t mask_dsm_ts{0x0F};
|
||||
constexpr std::uint8_t mask_dsm_reserved{0xF0};
|
||||
constexpr std::uint8_t mask_dsm_wk_k_msbyte{0x0F};
|
||||
#else
|
||||
constexpr std::uint8_t mask_nmas{0b1100'0000};
|
||||
constexpr std::uint8_t mask_cid{0b0011'0000};
|
||||
@ -164,9 +173,18 @@ constexpr std::uint8_t mask_nma_header_reserved{0b0000'0001};
|
||||
constexpr std::uint8_t mask_dsm_id{0b1111'0000};
|
||||
constexpr std::uint8_t mask_dsm_block_id{0b0000'1111};
|
||||
constexpr std::uint8_t mask_dsm_number_blocks{0b1111'0000};
|
||||
|
||||
constexpr std::uint8_t mask_dsm_pkid{0b0000'1111};
|
||||
constexpr std::uint8_t mask_dsm_cidkr{0b1100'0000};
|
||||
constexpr std::uint8_t mask_dsm_reserved1{0b0011'0000};
|
||||
constexpr std::uint8_t mask_dsm_hf{0b0000'1100};
|
||||
constexpr std::uint8_t mask_dsm_mf{0b0000'0011};
|
||||
constexpr std::uint8_t mask_dsm_ks{0b1111'0000};
|
||||
constexpr std::uint8_t mask_dsm_ts{0b0000'1111};
|
||||
constexpr std::uint8_t mask_dsm_reserved{0b1111'0000};
|
||||
constexpr std::uint8_t mask_dsm_wk_k_msbyte{0b0000'1111};
|
||||
#endif
|
||||
|
||||
// // hf = (dsm_msg[1] & 0b00001100) >> 2;
|
||||
uint8_t get_nmas(uint8_t nma_header)
|
||||
{
|
||||
return (nma_header & mask_nmas) >> 6;
|
||||
@ -197,11 +215,125 @@ uint8_t get_dsm_block_id(uint8_t dsm_header)
|
||||
return dsm_header & mask_dsm_block_id;
|
||||
}
|
||||
|
||||
uint8_t get_number_blocks_index(uint8_t dsm_message_0)
|
||||
uint8_t get_number_blocks_index(uint8_t dsm_msg_0)
|
||||
{
|
||||
return (dsm_message_0 & mask_dsm_number_blocks) >> 4;
|
||||
return (dsm_msg_0 & mask_dsm_number_blocks) >> 4;
|
||||
}
|
||||
|
||||
uint8_t get_pkid(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[0] & mask_dsm_pkid);
|
||||
}
|
||||
|
||||
uint8_t get_cidkr(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[1] & mask_dsm_cidkr) >> 6;
|
||||
}
|
||||
|
||||
uint8_t get_dsm_reserved1(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[1] & mask_dsm_reserved1) >> 4;
|
||||
}
|
||||
|
||||
uint8_t get_hf(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[1] & mask_dsm_hf) >> 2;
|
||||
}
|
||||
|
||||
uint8_t get_mf(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[1] & mask_dsm_mf);
|
||||
}
|
||||
|
||||
uint8_t get_ks(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[2] & mask_dsm_ks) >> 4;
|
||||
}
|
||||
|
||||
uint8_t get_ts(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[2] & mask_dsm_ts);
|
||||
}
|
||||
|
||||
uint8_t get_maclt(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return dsm_msg[3];
|
||||
}
|
||||
|
||||
uint8_t get_dsm_reserved(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (dsm_msg[4] & mask_dsm_reserved) >> 4;
|
||||
}
|
||||
|
||||
uint16_t get_wn_k(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return (static_cast<uint16_t>((dsm_msg[4] & mask_dsm_wk_k_msbyte) << 8) + static_cast<uint16_t>(dsm_msg[5]));
|
||||
}
|
||||
|
||||
uint8_t get_towh_k(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
return dsm_msg[6];
|
||||
}
|
||||
|
||||
uint64_t get_alpha(const std::vector<uint8_t>& dsm_msg)
|
||||
{
|
||||
uint64_t alpha = (static_cast<uint64_t>(dsm_msg[8]) << 32) +
|
||||
(static_cast<uint64_t>(dsm_msg[9]) << 24) +
|
||||
(static_cast<uint64_t>(dsm_msg[10]) << 16) +
|
||||
(static_cast<uint64_t>(dsm_msg[11]) << 8) +
|
||||
static_cast<uint64_t>(dsm_msg[12]);
|
||||
return alpha;
|
||||
}
|
||||
|
||||
uint16_t get_l_dk_bits(uint8_t nb_dk)
|
||||
{
|
||||
const auto it = OSNMA_TABLE_7.find(nb_dk);
|
||||
if (it != OSNMA_TABLE_7.cend())
|
||||
{
|
||||
return it->second.second;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t get_lk_bits(uint8_t ks)
|
||||
{
|
||||
const auto it = OSNMA_TABLE_10.find(ks);
|
||||
if (it != OSNMA_TABLE_10.cend())
|
||||
{
|
||||
return it->second;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> get_kroot(const std::vector<uint8_t>& dsm_msg, uint16_t bytes_lk)
|
||||
{
|
||||
std::vector<uint8_t> kroot = std::vector<uint8_t>(bytes_lk, 0);
|
||||
if (dsm_msg.size() > 13 + bytes_lk)
|
||||
{
|
||||
for (uint16_t k = 0; k < bytes_lk; k++)
|
||||
{
|
||||
kroot[k] = dsm_msg[13 + k];
|
||||
}
|
||||
}
|
||||
return kroot;
|
||||
}
|
||||
|
||||
std::string get_hash_function(uint8_t hf)
|
||||
{
|
||||
std::string hash_;
|
||||
const auto it = OSNMA_TABLE_8.find(hf);
|
||||
if (it != OSNMA_TABLE_8.cend())
|
||||
{
|
||||
hash_ = it->second;
|
||||
}
|
||||
return hash_;
|
||||
}
|
||||
// std::string hash_function;
|
||||
// const auto it3 = OSNMA_TABLE_8.find(d_osnma_data.d_dsm_kroot_message.hf);
|
||||
// if (it3 != OSNMA_TABLE_8.cend())
|
||||
// {
|
||||
// hash_function = it3->second;
|
||||
// }
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_GALILEO_OSNMA_H
|
||||
Loading…
Reference in New Issue
Block a user