mirror of
https://github.com/lwvmobile/dsd-fme.git
synced 2026-11-14 09:47:30 -05:00
Phase 2 Audio Decoding
Phase 2 Audio Decoding Implemented XDMA decoder class implemented and set to default (-ft) --decoder class includes P25 1, 2, DMR BS/MS, and X2-TDMA -- -fr -T option reworked, -T now for Per Call, use -ft (default) Force Privacy Key --force privacy key use over system FID and SVC options. Per call now works on XDMA decoding and NXDN decoding Limited Phase 1 TSBK and Phase 2 LCCH handling (for Network Status Broadcast) --Need to get WACN, SYSID, and CC values for Phase 2 descrambling Fix most of the compiler warnings
This commit is contained in:
lwvmobile
276
src/p25p1_block.cpp
Normal file
276
src/p25p1_block.cpp
Normal file
@@ -0,0 +1,276 @@
|
||||
/*-------------------------------------------------------------------------------
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
/* 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);
|
||||
}
|
||||
Reference in New Issue
Block a user