278 lines
8.1 KiB
C++
278 lines
8.1 KiB
C++
/*-------------------------------------------------------------------------------
|
|
* p25p1_block.c
|
|
* P25 Block Bridge for TSBK Deinterleaving, CRC12 and CRC16 checks
|
|
*
|
|
* original copyrights for portions used below (OP25, Ossman, and LEH)
|
|
*
|
|
* LWVMOBILE
|
|
* 2022-09 DSD-FME Florida Man Edition
|
|
*-----------------------------------------------------------------------------*/
|
|
|
|
#include "dsd.h"
|
|
#include <vector>
|
|
|
|
typedef std::vector<bool> bit_vector;
|
|
|
|
/* adapted from wireshark/plugins/p25/packet-p25cai.c */
|
|
/* Copyright 2008, Michael Ossmann <mike@ossmann.com> */
|
|
/* deinterleave and trellis1_2 decoding, count_bits, and find_min*/
|
|
/* buf is assumed to be a buffer of 12 bytes */
|
|
|
|
//typedef std::vector<bool> bit_vector;
|
|
|
|
//ugly copy and paste below
|
|
static int count_bits(unsigned int n) {
|
|
int i = 0;
|
|
for (i = 0; n != 0; i++)
|
|
n &= n - 1;
|
|
return i;
|
|
}
|
|
|
|
static int find_min(uint8_t list[], int len) {
|
|
int min = list[0];
|
|
int index = 0;
|
|
int unique = 1;
|
|
int i;
|
|
|
|
for (i = 1; i < len; i++) {
|
|
if (list[i] < min) {
|
|
min = list[i];
|
|
index = i;
|
|
unique = 1;
|
|
} else if (list[i] == min) {
|
|
//unique = 0;
|
|
; //do nothing, still can be correct and pass CRC
|
|
}
|
|
}
|
|
/* return -1 if a minimum can't be found */
|
|
if (!unique)
|
|
return -1;
|
|
|
|
return index;
|
|
}
|
|
|
|
static int block_deinterleave(bit_vector& bv, unsigned int start, uint8_t* buf)
|
|
{
|
|
static const uint16_t deinterleave_tb[] = {
|
|
0, 1, 2, 3, 52, 53, 54, 55, 100,101,102,103, 148,149,150,151,
|
|
4, 5, 6, 7, 56, 57, 58, 59, 104,105,106,107, 152,153,154,155,
|
|
8, 9, 10, 11, 60, 61, 62, 63, 108,109,110,111, 156,157,158,159,
|
|
12, 13, 14, 15, 64, 65, 66, 67, 112,113,114,115, 160,161,162,163,
|
|
16, 17, 18, 19, 68, 69, 70, 71, 116,117,118,119, 164,165,166,167,
|
|
20, 21, 22, 23, 72, 73, 74, 75, 120,121,122,123, 168,169,170,171,
|
|
24, 25, 26, 27, 76, 77, 78, 79, 124,125,126,127, 172,173,174,175,
|
|
28, 29, 30, 31, 80, 81, 82, 83, 128,129,130,131, 176,177,178,179,
|
|
32, 33, 34, 35, 84, 85, 86, 87, 132,133,134,135, 180,181,182,183,
|
|
36, 37, 38, 39, 88, 89, 90, 91, 136,137,138,139, 184,185,186,187,
|
|
40, 41, 42, 43, 92, 93, 94, 95, 140,141,142,143, 188,189,190,191,
|
|
44, 45, 46, 47, 96, 97, 98, 99, 144,145,146,147, 192,193,194,195,
|
|
48, 49, 50, 51 };
|
|
|
|
uint8_t hd[4];
|
|
int b, d, j;
|
|
int state = 0;
|
|
uint8_t codeword;
|
|
|
|
static const uint8_t next_words[4][4] = {
|
|
{0x2, 0xC, 0x1, 0xF},
|
|
{0xE, 0x0, 0xD, 0x3},
|
|
{0x9, 0x7, 0xA, 0x4},
|
|
{0x5, 0xB, 0x6, 0x8}
|
|
};
|
|
|
|
memset(buf, 0, 12);
|
|
|
|
for (b=0; b < 98*2; b += 4) {
|
|
codeword = (bv[start+deinterleave_tb[b+0]] << 3) +
|
|
(bv[start+deinterleave_tb[b+1]] << 2) +
|
|
(bv[start+deinterleave_tb[b+2]] << 1) +
|
|
bv[start+deinterleave_tb[b+3]] ;
|
|
|
|
/* try each codeword in a row of the state transition table */
|
|
for (j = 0; j < 4; j++) {
|
|
/* find Hamming distance for candidate */
|
|
hd[j] = count_bits(codeword ^ next_words[state][j]);
|
|
}
|
|
/* find the dibit that matches the most codeword bits (minimum Hamming distance) */
|
|
state = find_min(hd, 4);
|
|
/* error if minimum can't be found */
|
|
if(state == -1)
|
|
return -1; // decode error, return failure
|
|
/* It also might be nice to report a condition where the minimum is
|
|
* non-zero, i.e. an error has been corrected. It probably shouldn't
|
|
* be a permanent failure, though.
|
|
*
|
|
* DISSECTOR_ASSERT(hd[state] == 0);
|
|
*/
|
|
|
|
/* append dibit onto output buffer */
|
|
d = b >> 2; // dibit ctr
|
|
if (d < 48) {
|
|
buf[d >> 2] |= state << (6 - ((d%4) * 2));
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int bd_bridge (int payload[196], uint8_t decoded[12])
|
|
{
|
|
std::vector<bool> vc(196,0);
|
|
int ec = 69; //initialize error value
|
|
|
|
//gather payload from tsbk handler and pass it to the block_deinterleave function
|
|
//and then return its payload back to the tsbk handler
|
|
|
|
//initialize our decoded byte buffer with zeroes
|
|
for (int i = 0; i < 12; i++)
|
|
{
|
|
decoded[i] = 0;
|
|
}
|
|
|
|
//convert/load payload into a vector vc to pass to block_deinterleave
|
|
for (int i = 0; i < 196; i++)
|
|
{
|
|
vc[i] = payload[i];
|
|
}
|
|
int block_count = 0; //
|
|
unsigned int start = block_count*196;
|
|
|
|
ec = block_deinterleave(vc, start, decoded);
|
|
|
|
return ec;
|
|
}
|
|
|
|
//modified from the LEH ComputeCrcCCITT to accept variable len buffer bits
|
|
uint16_t ComputeCrcCCITT16b(const uint8_t buf[], unsigned int len)
|
|
{
|
|
uint32_t i;
|
|
uint16_t CRC = 0x0000; /* Initialization value = 0x0000 */
|
|
/* Polynomial x^16 + x^12 + x^5 + 1
|
|
* Normal = 0x1021
|
|
* Reciprocal = 0x0811
|
|
* Reversed = 0x8408
|
|
* Reversed reciprocal = 0x8810 */
|
|
uint16_t Polynome = 0x1021;
|
|
for(i = 0; i < len; i++)
|
|
{
|
|
if(((CRC >> 15) & 1) ^ (buf[i] & 1))
|
|
{
|
|
CRC = (CRC << 1) ^ Polynome;
|
|
}
|
|
else
|
|
{
|
|
CRC <<= 1;
|
|
}
|
|
}
|
|
|
|
/* Invert the CRC */
|
|
CRC ^= 0xFFFF;
|
|
|
|
/* Return the CRC */
|
|
return CRC;
|
|
} /* End ComputeCrcCCITT() */
|
|
|
|
|
|
//modified from crc12_ok to run a quickie on 16 instead
|
|
static uint16_t crc16_ok(const uint8_t bits[], unsigned int len) {
|
|
uint16_t crc = 0;
|
|
// fprintf (stderr, "\n LEN = %d", len);
|
|
for (int i = 0; i < 16; i++)
|
|
{
|
|
crc = crc << 1;
|
|
crc = crc | bits[i+len];
|
|
}
|
|
// uint16_t check = crc16b(bits,len);
|
|
uint16_t check = ComputeCrcCCITT16b(bits, len);
|
|
|
|
if (crc == check)
|
|
{
|
|
//fprintf (stderr, " CRC = %04X %04X", crc, check);
|
|
return (0);
|
|
}
|
|
else
|
|
{
|
|
//fprintf (stderr, " CRC = %04X %04X", crc, check);
|
|
return (-1);
|
|
}
|
|
|
|
}
|
|
|
|
//TSBK/LCCH CRC16 x-bit bridge to crc16b and crc16b_okay, wip, may need multi pdu format as well
|
|
int crc16_lb_bridge (int payload[190], int len)
|
|
{
|
|
int err = -2;
|
|
uint16_t crc = -2;
|
|
uint8_t buf[190] = {0};
|
|
|
|
for (int i = 0; i < len+16; i++) //add +16 here so we load the entire frame but only run crc on the len portion
|
|
{
|
|
buf[i] = payload[i];
|
|
}
|
|
err = crc16_ok(buf, len);
|
|
return (err);
|
|
}
|
|
|
|
//borrowing crc12 from OP25
|
|
static uint16_t crc12(const uint8_t bits[], unsigned int len) {
|
|
uint16_t crc=0;
|
|
static const unsigned int K = 12;
|
|
//g12(x) = x12 + x11 + x7 + x4 + x2 + x + 1
|
|
static const uint8_t poly[K+1] = {1,1,0,0,0,1,0,0,1,0,1,1,1}; // p25 p2 crc 12 poly
|
|
uint8_t buf[256];
|
|
if (len+K > sizeof(buf)) {
|
|
//fprintf (stderr, "crc12: buffer length %u exceeds maximum %u\n", len+K, sizeof(buf));
|
|
return 0;
|
|
}
|
|
memset (buf, 0, sizeof(buf));
|
|
for (unsigned int i=0; i<len; i++){
|
|
buf[i] = bits[i];
|
|
}
|
|
for (unsigned int i=0; i<len; i++)
|
|
if (buf[i])
|
|
for (unsigned int j=0; j<K+1; j++)
|
|
buf[i+j] ^= poly[j];
|
|
for (unsigned int i=0; i<K; i++){
|
|
crc = (crc << 1) + buf[len + i];
|
|
}
|
|
return crc ^ 0xfff;
|
|
}
|
|
|
|
//borrowing crc12_ok from OP25
|
|
static uint16_t crc12_ok(const uint8_t bits[], unsigned int len) {
|
|
uint16_t crc = 0;
|
|
for (int i = 0; i < 12; i++)
|
|
{
|
|
crc = crc << 1;
|
|
crc = crc | bits[i+len];
|
|
}
|
|
uint16_t check = crc12(bits,len);
|
|
|
|
if (crc == check)
|
|
{
|
|
//fprintf (stderr, " CRC = %03X %03X", crc, check);
|
|
return (0);
|
|
}
|
|
else
|
|
{
|
|
//fprintf (stderr, " CRC = %03X %03X", crc, check);
|
|
return (-1);
|
|
}
|
|
|
|
}
|
|
|
|
//xCCH CRC12 x-bit bridge to crc12b and crc12b_okay
|
|
int crc12_xb_bridge (int payload[190], int len)
|
|
{
|
|
int err = -5;
|
|
uint16_t crc = -2;
|
|
uint8_t buf[190] = {0};
|
|
|
|
//need to load up the 12 crc bits as well, else it will fail on the check, but not on the crc calc
|
|
for (int i = 0; i < len+12; i++)
|
|
{
|
|
buf[i] = payload[i];
|
|
}
|
|
err = crc12_ok(buf, len);
|
|
return (err);
|
|
}
|