M17_Project
/
gr-m17
mirror of https://github.com/M17-Project/gr-m17.git
1
0
Fork 0
Code Issues Releases Wiki Activity
gr-m17/lib/m17_decoder_impl.cc

788 lines
21 KiB
C++
Raw Blame History

/* -*- c++ -*- */
/*
* Copyright 2023 jmfriedt.
*
* This is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3, or (at your option)
* any later version.
*
* This software is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software; see the file COPYING. If not, write to
* the Free Software Foundation, Inc., 51 Franklin Street,
* Boston, MA 02110-1301, USA.
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <gnuradio/io_signature.h>
#include "m17_decoder_impl.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include "m17.h"
#define CODE_MEAN -0.75 // mean(str_sync_symbols)
#define CODE_STD 8.21583836f //std(str_sync_symbols)*sqrt(length(str_sync_symbols)-1)
namespace gr
{
namespace m17
{
m17_decoder::sptr
m17_decoder::make (bool debug_data, bool debug_ctrl, float threshold,
bool callsign, bool signed_str, int encr_type,
std::string key, std::string seed)
{
return gnuradio::get_initial_sptr
(new
m17_decoder_impl (debug_data, debug_ctrl, threshold, callsign,
signed_str, encr_type, key, seed));
}
/*
* The private constructor
*/
m17_decoder_impl::m17_decoder_impl (bool debug_data, bool debug_ctrl,
float threshold, bool callsign,
bool signed_str, int encr_type,
std::string key, std::string seed):
gr::block ("m17_decoder",
gr::io_signature::make (1, 1, sizeof (float)),
gr::io_signature::make (1, 1, sizeof (char))),
_debug_data (debug_data), _debug_ctrl (debug_ctrl),
_threshold (threshold), _callsign (callsign), _signed_str (signed_str)
{
set_debug_data (debug_data);
set_debug_ctrl (debug_ctrl);
set_threshold (threshold);
set_callsign (callsign);
set_signed (signed_str);
set_key (key);
set_encr_type (encr_type);
_expected_next_fn = 0;
}
/*
* Our virtual destructor.
*/
m17_decoder_impl::~m17_decoder_impl ()
{
}
void m17_decoder_impl::set_threshold (float threshold)
{
_threshold = threshold;
printf ("Threshold: %f\n", _threshold);
}
void m17_decoder_impl::set_debug_data (bool debug)
{
_debug_data = debug;
if (_debug_data == true)
printf ("Data debug: true\n");
else
printf ("Data debug: false\n");
}
void m17_decoder_impl::set_debug_ctrl (bool debug)
{
_debug_ctrl = debug;
if (_debug_ctrl == true)
printf ("Debug control: true\n");
else
printf ("Debug control: false\n");
}
void m17_decoder_impl::set_encr_type (int encr_type)
{
switch (encr_type)
{case 0:_encr_type=ENCR_NONE;break;
case 1:_encr_type=ENCR_SCRAM;break;
case 2:_encr_type=ENCR_AES;break;
case 3:_encr_type=ENCR_RES;break;
default:_encr_type=ENCR_NONE;
}
printf ("new encr type: %x -> ", _encr_type);
}
void m17_decoder_impl::set_callsign (bool callsign)
{
_callsign = callsign;
if (_callsign == true)
printf ("Display callsign\n");
else
printf ("Do not display callsign\n");
}
void m17_decoder_impl::set_signed (bool signed_str)
{
_signed_str = signed_str;
if (_callsign == true)
printf ("Signed\n");
else
printf ("Unsigned\n");
}
void m17_decoder_impl::set_key (std::string arg) // *UTF-8* encoded byte array
{
int length;
printf ("new key: ");
length = arg.size ();
int i = 0, j = 0;
while ((j < 32) && (i < length))
{
if ((unsigned int) arg.data ()[i] < 0xc2) // https://www.utf8-chartable.de/
{
_key[j] = arg.data ()[i];
i++;
j++;
}
else
{
_key[j] = (arg.data ()[i] - 0xc2) * 0x40 + arg.data ()[i + 1];
i += 2;
j++;
}
}
length = j; // index from 0 to length-1
printf ("%d bytes: ", length);
for (i = 0; i < length; i++)
printf ("%02X ", _key[i]);
printf ("\n");
fflush (stdout);
}
void m17_decoder_impl::set_seed (std::string arg) // *UTF-8* encoded byte array
{
int length;
printf ("new seed: ");
length = arg.size ();
int i = 0, j = 0;
while ((j < 3) && (i < length))
{
if ((unsigned int) arg.data ()[i] < 0xc2) // https://www.utf8-chartable.de/
{
_seed[j] = arg.data ()[i];
i++;
j++;
}
else
{
_seed[j] = (arg.data ()[i] - 0xc2) * 0x40 + arg.data ()[i + 1];
i += 2;
j++;
}
}
length = j; // index from 0 to length-1
printf ("%d bytes: ", length);
for (i = 0; i < length; i++)
printf ("%02X ", _seed[i]);
printf ("\n");
fflush (stdout);
if(length<=2)
{
_scrambler_seed = _scrambler_seed >> 16;
fprintf(stderr, "Scrambler key: 0x%02X (8-bit)\n", _scrambler_seed);
}
else if(length<=4)
{
_scrambler_seed = _scrambler_seed >> 8;
fprintf(stderr, "Scrambler key: 0x%04X (16-bit)\n", _scrambler_seed);
}
else
fprintf(stderr, "Scrambler key: 0x%06X (24-bit)\n", _scrambler_seed);
_encr_type=ENCR_SCRAM; //Scrambler key was passed
}
void
m17_decoder_impl::forecast (int noutput_items,
gr_vector_int & ninput_items_required)
{
ninput_items_required[0] = 0; // noutput_items;
}
//this is generating a correct seed value based on the fn value,
//ideally, we would only want to run this under poor signal, frame skips, etc
//Note: Running this every frame will lag if high fn values (observed with test file)
uint32_t m17_decoder_impl::scrambler_seed_calculation (int8_t subtype,
uint32_t key,
int fn)
{
int i;
uint32_t lfsr, bit;
lfsr = key;
bit = 0;
for (i = 0; i < 128 * fn; i++)
{
//get feedback bit with specified taps, depending on the subtype
if (subtype == 0)
bit = (lfsr >> 7) ^ (lfsr >> 5) ^ (lfsr >> 4) ^ (lfsr >> 3);
else if (subtype == 1)
bit = (lfsr >> 15) ^ (lfsr >> 14) ^ (lfsr >> 12) ^ (lfsr >> 3);
else if (subtype == 2)
bit = (lfsr >> 23) ^ (lfsr >> 22) ^ (lfsr >> 21) ^ (lfsr >> 16);
else
bit = 0; //should never get here, but just in case
bit &= 1; //truncate bit to 1 bit
lfsr = (lfsr << 1) | bit; //shift LFSR left once and OR bit onto LFSR's LSB
lfsr &= 0xFFFFFF; //truncate lfsr to 24-bit
}
//truncate seed so subtype will continue to set properly on subsequent passes
if (_scrambler_subtype == 0)
_scrambler_seed &= 0xFF;
else if (_scrambler_subtype == 1)
_scrambler_seed &= 0xFFFF;
else if (_scrambler_subtype == 2)
_scrambler_seed &= 0xFFFFFF;
//debug
//fprintf (stderr, "\nScrambler Key: 0x%06X; Seed: 0x%06X; Subtype: %02d; FN: %05d; ", key, lfsr, subtype, fn);
return lfsr;
}
//scrambler pn sequence generation
void m17_decoder_impl::scrambler_sequence_generator ()
{
int i = 0;
uint32_t lfsr, bit;
lfsr = _scrambler_seed;
//only set if not initially set (first run), it is possible (and observed) that the scrambler_subtype can
//change on subsequent passes if the current SEED for the LFSR falls below one of these thresholds
if (_scrambler_subtype == -1)
{
if (lfsr > 0 && lfsr <= 0xFF)
_scrambler_subtype = 0; // 8-bit key
else if (lfsr > 0xFF && lfsr <= 0xFFFF)
_scrambler_subtype = 1; //16-bit key
else if (lfsr > 0xFFFF && lfsr <= 0xFFFFFF)
_scrambler_subtype = 2; //24-bit key
else
_scrambler_subtype = 0; // 8-bit key (default)
}
//TODO: Set Frame Type based on scrambler_subtype value
if (_debug_ctrl == true)
{
fprintf (stderr,
"\nScrambler Key: 0x%06X; Seed: 0x%06X; Subtype: %02d;",
_scrambler_seed, lfsr, _scrambler_subtype);
fprintf (stderr, "\n pN: ");
}
//run pN sequence with taps specified
for (i = 0; i < 128; i++)
{
//get feedback bit with specified taps, depending on the scrambler_subtype
if (_scrambler_subtype == 0)
bit = (lfsr >> 7) ^ (lfsr >> 5) ^ (lfsr >> 4) ^ (lfsr >> 3);
else if (_scrambler_subtype == 1)
bit = (lfsr >> 15) ^ (lfsr >> 14) ^ (lfsr >> 12) ^ (lfsr >> 3);
else if (_scrambler_subtype == 2)
bit = (lfsr >> 23) ^ (lfsr >> 22) ^ (lfsr >> 21) ^ (lfsr >> 16);
else
bit = 0; //should never get here, but just in case
bit &= 1; //truncate bit to 1 bit (required since I didn't do it above)
lfsr = (lfsr << 1) | bit; //shift LFSR left once and OR bit onto LFSR's LSB
lfsr &= 0xFFFFFF; //truncate lfsr to 24-bit (really doesn't matter)
_scrambler_pn[i] = bit;
}
//pack bit array into byte array for easy data XOR
pack_bit_array_into_byte_array (_scrambler_pn, _scr_bytes, 16);
//save scrambler seed for next round
_scrambler_seed = lfsr;
//truncate seed so subtype will continue to set properly on subsequent passes
if (_scrambler_subtype == 0)
_scrambler_seed &= 0xFF;
else if (_scrambler_subtype == 1)
_scrambler_seed &= 0xFFFF;
else if (_scrambler_subtype == 2)
_scrambler_seed &= 0xFFFFFF;
if (_debug_ctrl == true)
{
//debug packed bytes
for (i = 0; i < 16; i++)
fprintf (stderr, " %02X", _scr_bytes[i]);
fprintf (stderr, "\n");
}
}
//convert a user string (as hex octets) into a uint8_t array for key
void m17_decoder_impl::parse_raw_key_string (uint8_t * dest,
const char *inp)
{
uint16_t len = strlen (inp);
if (len == 0)
return; //return silently and pretend nothing happened
memset (dest, 0, len / 2); //one character represents half of a byte
if (!(len % 2)) //length even?
{
for (uint8_t i = 0; i < len; i += 2)
{
if (inp[i] >= 'a')
dest[i / 2] |= (inp[i] - 'a' + 10) * 0x10;
else if (inp[i] >= 'A')
dest[i / 2] |= (inp[i] - 'A' + 10) * 0x10;
else if (inp[i] >= '0')
dest[i / 2] |= (inp[i] - '0') * 0x10;
if (inp[i + 1] >= 'a')
dest[i / 2] |= inp[i + 1] - 'a' + 10;
else if (inp[i + 1] >= 'A')
dest[i / 2] |= inp[i + 1] - 'A' + 10;
else if (inp[i + 1] >= '0')
dest[i / 2] |= inp[i + 1] - '0';
}
}
else
{
if (inp[0] >= 'a')
dest[0] |= inp[0] - 'a' + 10;
else if (inp[0] >= 'A')
dest[0] |= inp[0] - 'A' + 10;
else if (inp[0] >= '0')
dest[0] |= inp[0] - '0';
for (uint8_t i = 1; i < len - 1; i += 2)
{
if (inp[i] >= 'a')
dest[i / 2 + 1] |= (inp[i] - 'a' + 10) * 0x10;
else if (inp[i] >= 'A')
dest[i / 2 + 1] |= (inp[i] - 'A' + 10) * 0x10;
else if (inp[i] >= '0')
dest[i / 2 + 1] |= (inp[i] - '0') * 0x10;
if (inp[i + 1] >= 'a')
dest[i / 2 + 1] |= inp[i + 1] - 'a' + 10;
else if (inp[i + 1] >= 'A')
dest[i / 2 + 1] |= inp[i + 1] - 'A' + 10;
else if (inp[i + 1] >= '0')
dest[i / 2 + 1] |= inp[i + 1] - '0';
}
}
}
int
m17_decoder_impl::general_work (int noutput_items,
gr_vector_int & ninput_items,
gr_vector_const_void_star & input_items,
gr_vector_void_star & output_items)
{
const float *in = (const float *) input_items[0];
char *out = (char *) output_items[0];
int countout = 0;
float sample; //last raw sample from the stdin
float dist; //Euclidean distance for finding syncwords in the symbol stream
for (int counterin = 0; counterin < ninput_items[0]; counterin++)
{
//wait for another symbol
sample = in[counterin];
if (!syncd)
{
//push new symbol
for (uint8_t i = 0; i < 7; i++)
{
last[i] = last[i + 1];
}
last[7] = sample;
//calculate euclidean norm
dist = eucl_norm (last, str_sync_symbols, 8);
if (dist < _threshold) //frame syncword detected
{
//fprintf(stderr, "str_sync_symbols dist: %3.5f\n", dist);
syncd = 1;
pushed = 0;
fl = 0;
}
else
{
//calculate euclidean norm again, this time against LSF syncword
dist = eucl_norm (last, lsf_sync_symbols, 8);
if (dist < _threshold) //LSF syncword
{
//fprintf(stderr, "lsf_sync dist: %3.5f\n", dist);
syncd = 1;
pushed = 0;
fl = 1;
}
}
}
else
{
pld[pushed++] = sample;
if (pushed == SYM_PER_PLD)
{
//common operations for all frame types
//slice symbols to soft dibits
slice_symbols (soft_bit, pld);
//derandomize
randomize_soft_bits (soft_bit);
//deinterleave
reorder_soft_bits (d_soft_bit, soft_bit);
//if it is a frame
if (!fl)
{
//extract data
for (uint16_t i = 0; i < 272; i++)
{
enc_data[i] = d_soft_bit[96 + i];
}
//decode
uint32_t e =
viterbi_decode_punctured (frame_data, enc_data,
puncture_pattern_2, 272,
12);
uint16_t fn = (frame_data[1] << 8) | frame_data[2];
if (_debug_data == true)
{ //dump data - first byte is empty
printf ("RX FN: %04X PLD: ", fn);
}
for (uint8_t i = 3; i < 19; i++)
{
if (_debug_data == true)
{
printf ("%02X", frame_data[i]);
}
out[countout] = frame_data[i];
countout++;
}
if (_debug_data == true)
{
printf (" e=%1.1f\n", (float) e / 0xFFFF);
}
//send codec2 stream to stdout
//fwrite(&frame_data[3], 16, 1, stdout);
//if the stream is signed
if (_signed_str == true && fn < 0x7FFC)
{
//if thats the first frame (fn=0)
if (fn == 0)
memcpy (_digest, &frame_data[3],
sizeof (_digest));
for (uint8_t i = 0; i < sizeof (_digest); i++)
_digest[i] ^= frame_data[3 + i];
uint8_t tmp = _digest[0];
for (uint8_t i = 0; i < sizeof (_digest) - 1; i++)
_digest[i] = _digest[i + 1];
_digest[sizeof (_digest) - 1] = tmp;
}
//NOTE: Don't attempt decryption when a signed stream is >= 0x7FFC
//The Signature is not encrypted
//AES
if (_encr_type == ENCR_AES)
{
memcpy (_iv, lsf + 14, 14);
_iv[14] = frame_data[1] & 0x7F;
_iv[15] = frame_data[2] & 0xFF;
if (_signed_str == true && (fn % 0x8000) < 0x7FFC) //signed stream
aes_ctr_bytewise_payload_crypt (_iv, _key, frame_data + 3, AES128); //hardcoded for now
else if (_signed_str == false) //non-signed stream
aes_ctr_bytewise_payload_crypt (_iv, _key, frame_data + 3, AES128); //hardcoded for now
}
//Scrambler
if (_encr_type == ENCR_SCRAM)
{
if (fn != 0 && (fn % 0x8000) != _expected_next_fn) //frame skip, etc
_scrambler_seed =
scrambler_seed_calculation (_scrambler_subtype,
_scrambler_key,
fn & 0x7FFF);
else if (fn == 0)
_scrambler_seed = _scrambler_key; //reset back to key value
if (_signed_str == true && (fn % 0x8000) < 0x7FFC) //signed stream
scrambler_sequence_generator ();
else if (_signed_str == false) //non-signed stream
scrambler_sequence_generator ();
for (uint8_t i = 0; i < 16; i++)
{
frame_data[i + 3] ^= _scr_bytes[i];
}
}
//dump data - first byte is empty
printf ("FN: %04X PLD: ", fn);
for (uint8_t i = 3; i < 19; i++)
{
printf ("%02X", frame_data[i]);
}
if (_debug_ctrl == true)
printf (" e=%1.1f\n", (float) e / 0xFFFF);
printf ("\n");
//send codec2 stream to stdout
//fwrite(&frame_data[3], 16, 1, stdout);
//extract LICH
for (uint16_t i = 0; i < 96; i++)
{
lich_chunk[i] = d_soft_bit[i];
}
//Golay decoder
decode_LICH (lich_b, lich_chunk);
lich_cnt = lich_b[5] >> 5;
//If we're at the start of a superframe, or we missed a frame, reset the LICH state
if ((lich_cnt == 0)
|| ((fn % 0x8000) != _expected_next_fn))
lich_chunks_rcvd = 0;
lich_chunks_rcvd |= (1 << lich_cnt);
memcpy (&lsf[lich_cnt * 5], lich_b, 5);
//debug - dump LICH
if (lich_chunks_rcvd == 0x3F) //all 6 chunks received?
{
if (_debug_ctrl == true)
{
if (_callsign == true)
{
decode_callsign_bytes (d_dst, &lsf[0]);
decode_callsign_bytes (d_src, &lsf[6]);
printf ("DST: %-9s ", d_dst); //DST
printf ("SRC: %-9s ", d_src); //SRC
}
else
{
printf ("DST: "); //DST
for (uint8_t i = 0; i < 6; i++)
printf ("%02X", lsf[i]);
printf (" ");
printf ("SRC: "); //SRC
for (uint8_t i = 0; i < 6; i++)
printf ("%02X", lsf[6 + i]);
printf (" ");
}
}
//TYPE
uint16_t type = (uint16_t) lsf[12] * 0x100 + lsf[13]; //big-endian
if (_debug_ctrl == true)
{
printf ("TYPE: %04X (", type);
if (type && 1)
printf ("STREAM: ");
else
printf ("PACKET: "); //shouldn't happen
if (((type >> 1) & 3) == 1)
printf ("DATA, ");
else if (((type >> 1) & 3) == 2)
printf ("VOICE, ");
else if (((type >> 1) & 3) == 3)
printf ("VOICE+DATA, ");
printf ("ENCR: ");
if (((type >> 3) & 3) == 0)
printf ("PLAIN, ");
else if (((type >> 3) & 3) == 1)
{
printf ("SCRAM ");
if (((type >> 5) & 3) == 1)
printf ("8-bit, ");
else if (((type >> 5) & 3) == 2)
printf ("16-bit, ");
else if (((type >> 5) & 3) == 3)
printf ("24-bit, ");
}
else if (((type >> 3) & 3) == 2)
printf ("AES, ");
else
printf ("UNK, ");
printf ("CAN: %d", (type >> 7) & 0xF);
if ((type >> 11) & 1)
printf (", SIGNED");
printf (") ");
}
//META
if (_debug_ctrl == true)
{
printf ("META: ");
for (uint8_t i = 0; i < 14; i++)
printf ("%02X", lsf[14 + i]);
if (CRC_M17 (lsf, 30)) //CRC
printf (" LSF_CRC_ERR");
else
printf (" LSF_CRC_OK ");
printf ("\n");
}
}
//if the contents of the payload is now digital signature, not data/voice
if (fn >= 0x7FFC && _signed_str == true)
{
memcpy (&_sig[((fn & 0x7FFF) - 0x7FFC) * 16],
&frame_data[3], 16);
if (fn == (0x7FFF | 0x8000))
{
//dump data
/*printf("DEC-Digest: ");
for(uint8_t i=0; i<sizeof(digest); i++)
printf("%02X", digest[i]);
printf("\n");
printf("Key: ");
for(uint8_t i=0; i<sizeof(pub_key); i++)
printf("%02X", pub_key[i]);
printf("\n");
printf("Signature: ");
for(uint8_t i=0; i<sizeof(sig); i++)
printf("%02X", sig[i]);
printf("\n"); */
if (uECC_verify
(_key, _digest, sizeof (_digest), _sig,
_curve))
{
printf ("Signature OK\n");
}
else
{
printf ("Signature invalid\n");
}
}
}
_expected_next_fn = (fn + 1) % 0x8000;
}
else //lsf
{
if (_debug_ctrl == true)
{
printf ("{LSF}\n");
}
//decode
uint32_t e =
viterbi_decode_punctured (lsf, d_soft_bit,
puncture_pattern_1,
2 * SYM_PER_PLD, 61);
//shift the buffer 1 position left - get rid of the encoded flushing bits
for (uint8_t i = 0; i < 30; i++)
lsf[i] = lsf[i + 1];
//dump data
if (_debug_ctrl == true)
{
if (_callsign == true)
{
decode_callsign_bytes (d_dst, &lsf[0]);
decode_callsign_bytes (d_src, &lsf[6]);
printf ("DST: %-9s ", d_dst); //DST
printf ("SRC: %-9s ", d_src); //SRC
}
else
{
printf ("DST: "); //DST
for (uint8_t i = 0; i < 6; i++)
printf ("%02X", lsf[i]);
printf (" ");
//SRC
printf ("SRC: ");
for (uint8_t i = 0; i < 6; i++)
printf ("%02X", lsf[6 + i]);
printf (" ");
}
//TYPE
printf ("TYPE: ");
for (uint8_t i = 0; i < 2; i++)
printf ("%02X", lsf[12 + i]);
printf (" ");
//META
printf ("META: ");
for (uint8_t i = 0; i < 14; i++)
printf ("%02X", lsf[14 + i]);
printf (" ");
//CRC
//printf("CRC: ");
//for(uint8_t i=0; i<2; i++)
//printf("%02X", lsf[28+i]);
if (CRC_M17 (lsf, 30))
printf ("LSF_CRC_ERR");
else
printf ("LSF_CRC_OK ");
//Viterbi decoder errors
printf (" e=%1.1f\n", (float) e / 0xFFFF);
}
}
//job done
syncd = 0;
pushed = 0;
for (uint8_t i = 0; i < 8; i++)
last[i] = 0.0;
}
}
}
// Tell runtime system how many input items we consumed on
// each input stream.
consume_each (ninput_items[0]);
// Tell runtime system how many output items we produced.
return countout;
}
} /* namespace m17 */
} /* namespace gr */
Reference in New Issue View Git Blame Copy Permalink