ua1zbe
/
dsd-fme_18_05_2023
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

P25 Restructuring

This commit is contained in:
lwvmobile 2022-09-27 17:21:51 -04:00
parent 6714761d8e
commit bab139e2e4
8 changed files with 1335 additions and 1072 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
include/dsd.h
View File

@ -593,6 +593,17 @@ typedef struct
int fourv_counter[2]; //external reference counter for ESS_B fragment collection
int p2_is_lcch; //flag to tell us when a frame is lcch and not sacch
//iden freq storage for frequency calculations
int p25_chan_iden;
int p25_chan_type[16];
int p25_trans_off[16];
int p25_chan_spac[16];
long int p25_base_freq[16];
//p25 frequency storage for display in ncurses
long int p25_cc_freq; //cc freq from net_stat
long int p25_vc_freq[2]; //vc freq from voice grant updates, etc
//experimental symbol file capture read throttle
int symbol_throttle; //throttle speed
int use_throttle; //only use throttle if set to 1
@ -904,6 +915,16 @@ void InitAllFecFunction(void);
void resetState (dsd_state * state);
void dstar_header_decode(dsd_state * state, int radioheaderbuffer[660]);
//P25 PDU Handler
void process_MAC_VPDU(dsd_opts * opts, dsd_state * state, int type, unsigned long long int MAC[24]);
//P25 xCCH Handlers (SACCH, FACCH, LCCH)
void process_SACCH_MAC_PDU (dsd_opts * opts, dsd_state * state, int payload[180]);
void process_FACCH_MAC_PDU (dsd_opts * opts, dsd_state * state, int payload[156]);
//P25 Channel to Frequency
long int process_channel_to_freq (dsd_opts * opts, dsd_state * state, int channel);
#ifdef __cplusplus
extern "C" {
#endif

33
src/dsd_main.c
View File

@ -207,6 +207,10 @@ noCarrier (dsd_opts * opts, dsd_state * state)
state->fourv_counter[0] = 0;
state->fourv_counter[1] = 0;
//values displayed in ncurses terminal
state->p25_vc_freq[0] = 0;
state->p25_vc_freq[1] = 0;
}
void
@ -585,6 +589,20 @@ initState (dsd_state * state)
state->p2_scramble_offset = 0;
state->p2_vch_chan_num = 0;
//p25 iden_up values
state->p25_chan_iden = 0;
for (int i = 0; i < 16; i++)
{
state->p25_chan_type[i] = 0;
state->p25_trans_off[i] = 0;
state->p25_chan_spac[i] = 0;
state->p25_base_freq[i] = 0;
}
//values displayed in ncurses terminal
state->p25_cc_freq = 0;
state->p25_vc_freq[0] = 0;
state->p25_vc_freq[1] = 0;
#ifdef TRACE_DSD
state->debug_sample_index = 0;
state->debug_label_file = NULL;
@ -606,7 +624,7 @@ usage ()
printf ("\n");
printf ("Display Options:\n");
printf (" -N Use NCurses Terminal\n");
printf (" dsd-fme -N 2> log.txt \n");
printf (" dsd-fme -N 2> log.ans \n");
printf (" -e Show Frame Info and errorbars (default)\n");
printf (" -pe Show P25 encryption sync bits\n");
printf (" -pl Show P25 link control bits\n");
@ -619,17 +637,19 @@ usage ()
printf (" -z <num> Frame rate for datascope\n");
printf ("\n");
printf ("Input/Output options:\n");
printf (" -i <device> Audio input device (default is pulse audio), \n - for piped stdin, rtl for rtl device,\n filename.bin for OP25/FME capture bin files\n");
printf (" -o <device> Audio output device (default is pulse audio)\n");
printf (" -i <device> Audio input device (default is pulse audio)\n");
printf (" - for piped stdin, rtl for rtl device\n");
printf (" filename.bin for OP25/FME capture bin files\n");
printf (" -o <device> Audio output device (default is pulse audio)(null for no audio output)\n");
printf (" -d <dir> Create mbe data files, use this directory\n");
printf (" -r <files> Read/Play saved mbe data from file(s)\n");
printf (" -g <num> Audio output gain (default = 0 = auto, disable = -1)\n");
printf (" -w <file> Output synthesized speech to a .wav file\n");
printf (" -T Enable Per Call WAV file saving in XDMA and NXDN decoding classes\n");
printf (" -w <file> Output synthesized speech to a .wav file, legacy auto modes only.\n");
printf (" -T Enable Per Call WAV file saving in XDMA and NXDN decoding classes\n");
printf (" (Per Call can only be used in Ncurses Terminal!)\n");
printf (" (Running in console will use static wav files)\n");
printf (" -a Enable Call Alert Beep (NCurses Terminal Only)\n");
printf (" (Warning! Might be annoying.)");
printf (" (Warning! Might be annoying.)\n");
printf (" -n Throttle Symbol Capture Bin Input\n");
printf (" (useful when reading files still being written to by OP25)");
printf ("\n");
@ -671,6 +691,7 @@ usage ()
printf ("Advanced Decoder options:\n");
printf (" -X <hex> Manually Set P2 Parameters (WACN, SYSID, CC/NAC)\n");
printf (" (-X BEE00ABC123)\n");
printf ("\n");
// printf (" -A <num> QPSK modulation auto detection threshold (default=26)\n");
// printf (" -S <num> Symbol buffer size for QPSK decision point tracking\n");
// printf (" (default=36)\n");

57
src/dsd_ncurses.c
View File

@ -2116,6 +2116,10 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state)
else if (lls == 0 || lls == 1) //P1
{
printw ("P25 P1 - WACN: [%05llX] SYS: [%03llX] NAC: [%03llX] ", state->p2_wacn, state->p2_sysid, state->p2_cc);
if (state->p25_cc_freq != 0)
{
printw ("Freq: [%.06lf] MHz", (double)state->p25_cc_freq/1000000);
}
}
else if (lls == 35 || lls == 36) //P2
{
@ -2126,12 +2130,20 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state)
printw (" Phase 2 Missing Parameters ");
attron(COLOR_PAIR(3));
}
if (state->p2_wacn == 0xFFFFF || state->p2_sysid == 0xFFF || state->p2_cc == 0xFFF)
else if (state->p2_wacn == 0xFFFFF || state->p2_sysid == 0xFFF || state->p2_cc == 0xFFF)
{
attron(COLOR_PAIR(2));
printw (" Phase 2 Invalid Parameters ");
attron(COLOR_PAIR(3));
}
else
{
if (state->p25_cc_freq != 0)
{
printw ("Freq: [%.06lf] MHz", (double)state->p25_cc_freq/1000000);
}
//printw ("Freq: [%.06lf] MHz", (double)state->p25_cc_freq/1000000);
}
}
printw ("\n");
@ -2271,20 +2283,30 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state)
{
attron(COLOR_PAIR(3));
}
}
//LRRP
if(state->dmrburstL != 16) //only during data
//LRRP
if(state->dmrburstL != 16) //only during data
{
attron(COLOR_PAIR(5));
for (short i = 0; i < 5; i++)
{
attron(COLOR_PAIR(5));
for (short i = 0; i < 5; i++)
{
printw ("%s", state->dmr_lrrp[0][i]);
}
attroff(COLOR_PAIR(5));
if (state->carrier == 1)
{
attron(COLOR_PAIR(3));
}
printw ("%s", state->dmr_lrrp[0][i]);
}
attroff(COLOR_PAIR(5));
if (state->carrier == 1)
{
attron(COLOR_PAIR(3));
}
}
if (state->p25_vc_freq[0] != 0)
{
attron(COLOR_PAIR(5));
printw ("Frequency: [%.06lf] MHz", (double)state->p25_vc_freq[0]/1000000);
if (state->carrier == 1)
{
attron(COLOR_PAIR(3));
}
}
printw ("\n");
@ -2437,6 +2459,15 @@ ncursesPrinter (dsd_opts * opts, dsd_state * state)
attron(COLOR_PAIR(3));
}
}
if (state->p25_vc_freq[0] != 0)
{
attron(COLOR_PAIR(5));
printw ("Frequency: [%.06lf] MHz", (double)state->p25_vc_freq[0]/1000000);
if (state->carrier == 1)
{
attron(COLOR_PAIR(3));
}
}
printw ("\n");
} // end if not MS
} //end DMR BS Types

44
src/p25_frequency.c Normal file
View File

@ -0,0 +1,44 @@
/*-------------------------------------------------------------------------------
* p25_frequency.c
* P25 Channel to Frequency Calculator
*
* LWVMOBILE
* 2022-09 DSD-FME Florida Man Edition
*-----------------------------------------------------------------------------*/
#include "dsd.h"
long int process_channel_to_freq (dsd_opts * opts, dsd_state * state, int channel)
{
//RX and SU TX frequencies.
//SU RX = (Base Frequency) + (Channel Number) x (Channel Spacing).
/*
Channel Spacing: This is a frequency multiplier for the channel
number. It is used as a multiplier in other messages that specify
a channel field value. The channel spacing (kHz) is computed as
(Channel Spacing) x (0.125 kHz).
*/
//Note: Base Frequency is calculated as (Base Frequency) x (0.000005 MHz) from the IDEN_UP message.
long int freq = -1;
int iden = channel >> 12;
int type = state->p25_chan_type[iden];
int slots_per_carrier[16] = {1,1,1,2,4,2,2,2,2,2,2,2,2,2,2,2}; //from OP25
int step = (channel & 0xFFF) / slots_per_carrier[type];
if (state->p25_base_freq[iden] != 0)
{
freq = (state->p25_base_freq[iden] * 5) + ( step * state->p25_chan_spac[iden] * 125);
fprintf (stderr, "\n Frequency [%.6lf] MHz", (double)freq/1000000);
return (freq);
}
else
{
fprintf (stderr, "\n Base Frequency Not Found - Iden [%d]", iden);
return (0);
}
}

1179
src/p25_p2.c
View File

File diff suppressed because it is too large Load Diff

97
src/p25p1_tsbk.c
View File

@ -1,6 +1,6 @@
/*-------------------------------------------------------------------------------
* p25p1_tsbk.c
* P25 TSBK Handler for Network Status Broadcast (WACN, SYSID, NAC/CC), etc.
* P25 Trunking Signal Block Handler
*
* LWVMOBILE
* 2022-09 DSD-FME Florida Man Edition
@ -9,37 +9,33 @@
#include "dsd.h"
void processTSBK(dsd_opts * opts, dsd_state * state)
{
//process TSDU/TSBK and look for relevant data (WACN, SYSID, other goodies later on)
//process TSDU/TSBK and look for relevant data (WACN, SYSID, iden_up, other goodies later on)
int tsbkbit[196] = {0}; //tsbk bit array, 196 trellis encoded bits
int tsbk_dibit[98] = {0};
int dibit = 0;
uint8_t tsbk_byte[12] = {0}; //12 byte return from bd_bridge (block_deinterleave)
unsigned long long int PDU[24] = {0}; //24 byte PDU to send to the tsbk_byte vPDU handler, should be same formats
int tsbk_decoded_bits[190] = {0}; //decoded bits from tsbk_bytes for sending to crc16_lb_bridge
int i, j, k, b;
int i, j, k, b, x, y;
int ec = -2; //error value returned from (block_deinterleave)
int checksum = -2; //checksum returned from crc16, 0 is valid, anything else is invalid
int skipdibit = 14; //initial status dibit will occur at 14, then add 35 each time it occurs
//TSBKs are always single block, but up to three can be transmitted in a row,
//my observation shows that, as far as I know, only the first block of three is good,
//and NULL types can contain some amount of garbage, but the CRC is always 00s.
//Multi-Block PDUs are sent on a different DUID - 0xC
//Best way to proceed for now is going to be to collect all three potentials, and only process on good
//de-interleaves AND good CRCs, otherwise, ignore them
//The Last Block flag doesn't really seem to apply for some reason (byte 0 0x80), or is always on a garbage block
//collect three reps of 101 dibits (98 valid dibits with status dibits interlaced)
for (j = 0; j < 3; j++)
{
k = 0;
//originally skipped 303 dibits, instead we collect three reps of 101 (98 valid dibits)
for (i = 0; i < 101; i++)
{
//rip dibits directly from the symbol capture file
//this method causes issues for some reason, probably to do with mixing reading types? don't know why?
// if (opts->audio_in_type == 4) //4
// {
// dibit = fgetc(opts->symbolfile);
// }
// else
// {
// dibit = getDibit(opts, state);
// if (opts->inverted_p2 == 1)
// {
// dibit = (dibit ^ 2);
// }
// }
dibit = getDibit(opts, state);
@ -51,7 +47,7 @@ void processTSBK(dsd_opts * opts, dsd_state * state)
k++;
}
if (i+(j*101) == skipdibit) //
if (i+(j*101) == skipdibit)
{
skipdibit += 35;
}
@ -65,14 +61,13 @@ void processTSBK(dsd_opts * opts, dsd_state * state)
k = 0;
for (i = 0; i < 12; i++)
{
for (j = 0; j < 8; j++)
for (x = 0; x < 8; x++)
{
tsbk_decoded_bits[k] = ((tsbk_byte[i] << j) & 0x80) >> 7;
tsbk_decoded_bits[k] = ((tsbk_byte[i] << x) & 0x80) >> 7;
k++;
}
}
//crc check works now using the ComputeCrcCCITT method and not the OP25 method (bug?)
int err = -2;
err = crc16_lb_bridge(tsbk_decoded_bits, 80);
if (ec != 0)
@ -84,6 +79,29 @@ void processTSBK(dsd_opts * opts, dsd_state * state)
//fprintf (stderr, "BAD CRC16");
}
//convert tsbk_byte to PDU and send to vPDU handler...may or may not be entirely compatible,
PDU[0] = 0; //convert to MAC_SIGNAL
PDU[1] = tsbk_byte[0];
PDU[2] = tsbk_byte[2];
PDU[3] = tsbk_byte[3];
PDU[4] = tsbk_byte[4];
PDU[5] = tsbk_byte[5];
PDU[6] = tsbk_byte[6];
PDU[7] = tsbk_byte[7];
PDU[8] = tsbk_byte[8];
PDU[9] = tsbk_byte[9];
PDU[10] = tsbk_byte[10];
PDU[11] = tsbk_byte[11];
PDU[1] = PDU[1] ^ 0x40; //flip bit to make it compatible with MAC_PDUs, i.e. 3D to 7D
//Don't run NET_STS out of this, or will set wrong NAC/CC
if (err == 0 && ec == 0 && PDU[1] != 0x7B && PDU[1] != 0xFB)
{
fprintf (stderr, "%s",KMAG);
process_MAC_VPDU(opts, state, 0, PDU);
fprintf (stderr, "%s",KNRM);
}
//set our WACN and SYSID here now that we have valid ec and crc/checksum
if (err == 0 && ec == 0 && tsbk_byte[0] == 0x3B)
{
@ -92,31 +110,32 @@ void processTSBK(dsd_opts * opts, dsd_state * state)
{
state->p2_wacn = (tsbk_byte[3] << 12) | (tsbk_byte[4] << 4) | (tsbk_byte[5] >> 4);
state->p2_sysid = ((tsbk_byte[5] & 0xF) << 8) | tsbk_byte[6];
fprintf (stderr, "%s",KCYN);
fprintf (stderr, "\n Network Status Broadcast TSBK \n");
int channel = (tsbk_byte[7] << 8) | tsbk_byte[8];
fprintf (stderr, "%s",KMAG);
fprintf (stderr, "\n Network Status Broadcast TSBK - Abbreviated \n");
fprintf (stderr, " WACN [%05llX] SYSID [%03llX] NAC [%03llX]", state->p2_wacn, state->p2_sysid, state->p2_cc);
state->p25_cc_freq = process_channel_to_freq(opts, state, channel);
if (opts->payload == 1)
{
fprintf (stderr, "%s",KCYN);
fprintf (stderr, "\n P25 PDU Payload ");
fprintf (stderr, " BD = %d CRC = %d \n ", ec, err);
for (i = 0; i < 12; i++)
{
fprintf (stderr, "[%02X]", tsbk_byte[i]);
}
}
fprintf (stderr, "%s ", KNRM);
//state->p2_cc = state->nac; //set in frame_sync line 450 instead after valid bch check
}
}
// print dump for debug eval
if (opts->payload == 1)
{
fprintf (stderr, "%s",KCYN);
fprintf (stderr, "\n P25 TSBK Byte Payload ");
fprintf (stderr, " BD = %d CRC = %d \n ", ec, err);
for (i = 0; i < 12; i++)
{
fprintf (stderr, "[%02X]", tsbk_byte[i]);
}
fprintf (stderr, "%s ", KNRM);
}
//reset for next rep
ec = -2;
checksum = -2;
}
fprintf (stderr, "\n"); //line break on loop end
fprintf (stderr, "\n");
}

500
src/p25p2_vpdu.c Normal file
View File

@ -0,0 +1,500 @@
/*-------------------------------------------------------------------------------
* p25p2_vpdu.c
* Phase 2 Variable PDU Handling
*
* LWVMOBILE
* 2022-09 DSD-FME Florida Man Edition
*-----------------------------------------------------------------------------*/
#include "dsd.h"
//MAC message lengths
static const uint8_t mac_msg_len[256] = {
0, 7, 8, 7, 0, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //0F
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //1F
0, 14, 15, 0, 0, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //2F
5, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //3F
9, 7, 9, 0, 9, 8, 9, 0, 10, 10, 9, 0, 10, 0, 0, 0, //4F
0, 0, 0, 0, 9, 7, 0, 0, 10, 0, 7, 0, 10, 8, 14, 7, //5F
9, 9, 0, 0, 9, 0, 0, 9, 10, 0, 7, 10, 10, 7, 0, 9, //6F
9, 29, 9, 9, 9, 9, 10, 13, 9, 9, 9, 11, 9, 9, 0, 0, //7F
8, 0, 0, 7, 11, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //8F
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //9F
16, 0, 0, 11, 13, 11, 11, 11, 10, 0, 0, 0, 0, 0, 0, 0, //AF
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, //BF
11, 0, 0, 8, 15, 12, 15, 32, 12, 12, 0, 27, 14, 29, 29, 32, //CF
0, 0, 0, 0, 0, 0, 9, 0, 14, 29, 11, 27, 14, 0, 40, 11, //DF
28, 0, 0, 14, 17, 14, 0, 0, 16, 8, 11, 0, 13, 19, 0, 0, //EF
0, 0, 16, 14, 0, 0, 12, 0, 22, 0, 11, 13, 11, 0, 15, 0 }; //FF
//MAC PDU 3-bit Opcodes BBAC (8.4.1) p 123:
//0 - reserved //1 - Mac PTT //2 - Mac End PTT //3 - Mac Idle //4 - Mac Active
//5 - reserved //6 - Mac Hangtime //7 - reserved //Mac PTT BBAC p80
void process_MAC_VPDU(dsd_opts * opts, dsd_state * state, int type, unsigned long long int MAC[24])
{
//handle variable content MAC PDUs (Active, Idle, Hangtime, or Signal)
//use type to specify SACCH or FACCH, so we know if we should invert the currentslot when assigning ids etc
//b values - 0 = Unique TDMA Message, 1 Phase 1 OSP/ISP abbreviated
// 2 = Manufacturer Message, 3 Phase 1 OSP/ISP extended/explicit
int len_a = 0;
int len_b = mac_msg_len[MAC[1]];
int len_c = 0;
//sanity check
if (len_b < 19 && type == 1)
{
len_c = mac_msg_len[MAC[1+len_b]];
}
if (len_b < 16 && type == 0)
{
len_c = mac_msg_len[MAC[1+len_b]];
}
int slot = 9;
if (type == 1) //0 for F, 1 for S
{
slot = (state->currentslot ^ 1) & 1; //flip slot internally for SACCH
}
else slot = state->currentslot;
//assigning here if OECI MAC SIGNAL, after passing RS and CRC
if (state->p2_is_lcch)
{
if (slot == 0) state->dmrburstL = 30;
else state->dmrburstR = 30;
}
if (len_b == 0 || len_b > 18)
{
goto END_PDU;
}
for (int i = 0; i < 2; i++)
{
//Group Voice Channel Grant (GRP_V_CH_GRANT)
if (MAC[1+len_a] == 0x40)
{
int svc = MAC[2+len_a];
int channel = (MAC[3+len_a] << 8) | MAC[4+len_a];
int group = (MAC[5+len_a] << 8) | MAC[6+len_a];
int source = (MAC[7+len_a] << 16) | (MAC[8+len_a] << 8) | MAC[9+len_a];
fprintf (stderr, "\n Group Voice Channel Grant Update");
fprintf (stderr, "\n SVC [%02X] CHAN [%04X] Group [%d] Source [%d]", svc, channel, group, source);
process_channel_to_freq (opts, state, channel);
}
//Unit-to-Unit Voice Service Channel Grant (UU_V_CH_GRANT)
if (MAC[1+len_a] == 0x44)
{
int channel = (MAC[2+len_a] << 8) | MAC[3+len_a];
int target = (MAC[4+len_a] << 16) | (MAC[5+len_a] << 8) | MAC[6+len_a];
int source = (MAC[7+len_a] << 16) | (MAC[8+len_a] << 8) | MAC[9+len_a];
fprintf (stderr, "\n Unit to Unit Channel Grant");
fprintf (stderr, "\n CHAN [%04X] Source [%d] Target [%d]", channel, source, target);
process_channel_to_freq (opts, state, channel);
}
//Unit-to-Unit Answer Request (UU_ANS_REQ)
if (MAC[1+len_a] == 0x45)
{
int svc = MAC[2+len_a];
int answer = MAC[3+len_a];
int target = (MAC[4+len_a] << 16) | (MAC[5+len_a] << 8) | MAC[6+len_a];
int source = (MAC[7+len_a] << 16) | (MAC[8+len_a] << 8) | MAC[9+len_a];
fprintf (stderr, "\n Unit to Unit Channel Answer Request");
fprintf (stderr, "\n SVC [%02X] Answer [%02X] Source [%d] Target [%d]", svc, answer, source, target);
}
//Telephone Interconnect Voice Channel Grant (TELE_INT_CH_GRANT)
if (MAC[1+len_a] == 0x48)
{
fprintf (stderr, "\n Telephone Interconnect Voice Channel Grant (TELE_INT_CH_GRANT)");
}
//Telephone Interconnect Voice Channel Grant Update (TELE_INT_CH_GRANT_UPDT)
if (MAC[1+len_a] == 0x49)
{
fprintf (stderr, "\n Telephone Interconnect Voice Channel Grant Update (TELE_INT_CH_GRANT_UPDT)");
}
//Synchronization Broadcast (SYNC_BCST)
if (MAC[1+len_a] == 0x70)
{
fprintf (stderr, "\n Synchronization Broadcast (SYNC_BCST)");
}
//identifier update VHF/UHF
if (MAC[1+len_a] == 0x74)
{
state->p25_chan_iden = MAC[2+len_a] >> 4;
int iden = state->p25_chan_iden;
state->p25_chan_type[iden] = 1;
int bw_vu = (MAC[2+len_a] & 0xF);
state->p25_trans_off[iden] = (MAC[3+len_a] << 6) | (MAC[4+len_a] >> 2);
state->p25_chan_spac[iden] = ((MAC[4+len_a] & 0x3) << 8) | MAC[5+len_a];
state->p25_base_freq[iden] = (MAC[6+len_a] << 24) | (MAC[7+len_a] << 16) | (MAC[8+len_a] << 8) | (MAC[9+len_a] << 0);
fprintf (stderr, "\n Identifier Update UHF/VHF\n");
fprintf (stderr, " Channel Identifier [%01X] BW [%01X] Transmit Offset [%04X]\n Channel Spacing [%03X] Base Frequency [%08lX] [%09ld]",
state->p25_chan_iden, bw_vu, state->p25_trans_off[iden], state->p25_chan_spac[iden], state->p25_base_freq[iden], state->p25_base_freq[iden] * 5);
}
//identifier update (Non-TDMA 6.2.22) (Non-VHF-UHF) //with signed offset, bit trans_off >> 8; bit number 9
if (MAC[1+len_a] == 0x7D)
{
state->p25_chan_iden = MAC[2+len_a] >> 4;
int iden = state->p25_chan_iden;
state->p25_chan_type[iden] = 1;
int bw = ((MAC[2+len_a] & 0xF) << 5) | ((MAC[3+len_a] & 0xF8) >> 2);
state->p25_trans_off[iden] = (MAC[3+len_a] << 6) | (MAC[4+len_a] >> 2);
state->p25_chan_spac[iden] = ((MAC[4+len_a] & 0x3) << 8) | MAC[5+len_a];
state->p25_base_freq[iden] = (MAC[6+len_a] << 24) | (MAC[7+len_a] << 16) | (MAC[8+len_a] << 8) | (MAC[9+len_a] << 0);
fprintf (stderr, "\n Identifier Update (8.3.1.23)\n");
fprintf (stderr, " Channel Identifier [%01X] BW [%01X] Transmit Offset [%04X]\n Channel Spacing [%03X] Base Frequency [%08lX] [%09ld]",
state->p25_chan_iden, bw, state->p25_trans_off[iden], state->p25_chan_spac[iden], state->p25_base_freq[iden], state->p25_base_freq[iden] * 5);
}
//identifier update for TDMA, Abbreviated
if (MAC[1+len_a] == 0x73)
{
state->p25_chan_iden = MAC[2+len_a] >> 4;
int iden = state->p25_chan_iden;
state->p25_chan_type[iden] = MAC[2+len_a] & 0xF;
state->p25_trans_off[iden] = (MAC[3+len_a] << 6) | (MAC[4+len_a] >> 2);
state->p25_chan_spac[iden] = ((MAC[4+len_a] & 0x3) << 8) | MAC[5+len_a];
state->p25_base_freq[iden] = (MAC[6+len_a] << 24) | (MAC[7+len_a] << 16) | (MAC[8+len_a] << 8) | (MAC[9+len_a] << 0);
fprintf (stderr, "\n Identifier Update for TDMA - Abbreviated\n");
fprintf (stderr, " Channel Identifier [%01X] Channel Type [%01X] Transmit Offset [%04X]\n Channel Spacing [%03X] Base Frequency [%08lX] [%09ld]",
state->p25_chan_iden, state->p25_chan_type[iden], state->p25_trans_off[iden], state->p25_chan_spac[iden], state->p25_base_freq[iden], state->p25_base_freq[iden] * 5);
}
//identifier update for TDMA, Extended
if (MAC[1+len_a] == 0xF3)
{
state->p25_chan_iden = MAC[3+len_a] >> 4;
int iden = state->p25_chan_iden;
state->p25_chan_type[iden] = MAC[3+len_a] & 0xF;
state->p25_trans_off[iden] = (MAC[4+len_a] << 6) | (MAC[5+len_a] >> 2);
state->p25_chan_spac[iden] = ((MAC[5+len_a] & 0x3) << 8) | MAC[6+len_a];
state->p25_base_freq[iden] = (MAC[7+len_a] << 24) | (MAC[8+len_a] << 16) | (MAC[9+len_a] << 8) | (MAC[10+len_a] << 0);
int lwacn = (MAC[11+len_a] << 12) | (MAC[12+len_a] << 4) | ((MAC[13+len_a] & 0xF0) >> 4);
int lsysid = ((MAC[13+len_a] & 0xF) << 8) | MAC[14+len_a];
fprintf (stderr, "\n Identifier Update for TDMA - Extended\n");
fprintf (stderr, " Channel Identifier [%01X] Channel Type [%01X] Transmit Offset [%04X]\n Channel Spacing [%03X] Base Frequency [%08lX] [%09ld]",
state->p25_chan_iden, state->p25_chan_type[iden], state->p25_trans_off[iden], state->p25_chan_spac[iden], state->p25_base_freq[iden], state->p25_base_freq[iden] * 5);
fprintf (stderr, "\n WACN [%04X] SYSID [%04X]", lwacn, lsysid);
}
//Secondary Control Channel Broadcast, Explicit
if (MAC[1+len_a] == 0xE9)
{
int rfssid = MAC[2+len_a];
int siteid = MAC[3+len_a];
int channelt = (MAC[4+len_a] << 8) | MAC[5+len_a];
int channelr = (MAC[6+len_a] << 8) | MAC[7+len_a];
int sysclass = MAC[8+len_a];
if (1 == 1) //state->p2_is_lcch == 1
{
fprintf (stderr, "\n Secondary Control Channel Broadcast - Explicit\n");
fprintf (stderr, " RFSS[%03d] SITE ID [%03X] CHAN-T [%04X] CHAN-R [%04X] SSC [%02X]", rfssid, siteid, channelt, channelr, sysclass);
process_channel_to_freq (opts, state, channelt);
process_channel_to_freq (opts, state, channelr);
}
}
//Secondary Control Channel Broadcast, Implicit
if (MAC[1+len_a] == 0x79)
{
int rfssid = MAC[2+len_a];
int siteid = MAC[3+len_a];
int channel1 = (MAC[4+len_a] << 8) | MAC[5+len_a];
int sysclass1 = MAC[6+len_a];
int channel2 = (MAC[7+len_a] << 8) | MAC[8+len_a];
int sysclass2 = MAC[9+len_a];
if (1 == 1) //state->p2_is_lcch == 1
{
fprintf (stderr, "\n Secondary Control Channel Broadcast - Implicit\n");
fprintf (stderr, " RFSS[%03d] SITE ID [%03X] CHAN1 [%04X] SSC [%02X] CHAN2 [%04X] SSC [%02X]", rfssid, siteid, channel1, sysclass1, channel2, sysclass2);
process_channel_to_freq (opts, state, channel1);
process_channel_to_freq (opts, state, channel2);
}
}
//Group Voice Channel Grant Update - Implicit
if (MAC[1+len_a] == 0x42)
{
int channel1 = (MAC[2+len_a] << 8) | MAC[3+len_a];
int group1 = (MAC[4+len_a] << 8) | MAC[5+len_a];
int channel2 = (MAC[6+len_a] << 8) | MAC[7+len_a];
int group2 = (MAC[8+len_a] << 8) | MAC[9+len_a];
fprintf (stderr, "\n Group Voice Channel Grant Update - Implicit");
fprintf (stderr, "\n Channel 1 [%04X] Group 1 [%d][%04X]", channel1, group1, group1);
if (state->lasttg == group1)
{
state->p25_vc_freq[0] = process_channel_to_freq (opts, state, channel1);
}
else if (state->lasttgR == group1)
{
state->p25_vc_freq[1] = process_channel_to_freq (opts, state, channel1);
}
else process_channel_to_freq (opts, state, channel1);
//only run next channel if not identical to first channel
if (group1 != group2)
{
fprintf (stderr, "\n Channel 2 [%04X] Group 2 [%d][%04X]", channel2, group2, group2);
if (state->lasttg == group2)
{
state->p25_vc_freq[0] = process_channel_to_freq (opts, state, channel2);
}
else if (state->lasttgR == group2)
{
state->p25_vc_freq[1] = process_channel_to_freq (opts, state, channel2);
}
else process_channel_to_freq (opts, state, channel2);
}
}
//Group Voice Channel Grant Update - Explicit
if (MAC[1+len_a] == 0xC3)
{
int svc = MAC[2+len_a];
int channelt = (MAC[3+len_a] << 8) | MAC[4+len_a];
int channelr = (MAC[5+len_a] << 8) | MAC[6+len_a];
int group = (MAC[7+len_a] << 8) | MAC[8+len_a];
fprintf (stderr, "\n Group Voice Channel Grant Update - Explicit");
fprintf (stderr, "\n SVC [%02X] CHAN-T [%04X] CHAN-R [%04X] Group [%d][%04X]", svc, channelt, channelr, group, group);
process_channel_to_freq (opts, state, channelt);
process_channel_to_freq (opts, state, channelr);
if (slot == 0)
{
state->lasttg = group;
}
else state->lasttgR = group;
}
//MFID90 Group Regroup Voice Channel User - Abbreviated
if (MAC[1+len_a] == 0x80 && MAC[2+len_a] == 0x90)
{
int gr = (MAC[4+len_a] << 8) | MAC[5+len_a];
int src = (MAC[6+len_a] << 16) | (MAC[7+len_a] << 8) | MAC[8+len_a];
fprintf (stderr, "\n VCH %d - Super Group %d SRC %d ", slot, gr, src);
fprintf (stderr, "MFID90 Group Regroup Voice");
if (slot == 0)
{
state->lasttg = gr;
if (src != 0) state->lastsrc = src;
}
else
{
state->lasttgR = gr;
if (src != 0) state->lastsrcR = src;
}
}
//MFID90 Group Regroup Voice Channel User - Extended
if (MAC[1+len_a] == 0xA0 && MAC[2+len_a] == 0x90)
{
int gr = (MAC[5+len_a] << 8) | MAC[6+len_a];
int src = (MAC[7+len_a] << 16) | (MAC[8+len_a] << 8) | MAC[9+len_a];
fprintf (stderr, "\n VCH %d - Super Group %d SRC %d ", slot, gr, src);
fprintf (stderr, "MFID90 Group Regroup Voice");
if (slot == 0)
{
state->lasttg = gr;
if (src != 0) state->lastsrc = src;
}
else
{
state->lasttgR = gr;
if (src != 0) state->lastsrcR = src;
}
}
//1 or 21, group voice channel message, abb and ext
if (MAC[1+len_a] == 0x1 || MAC[1+len_a] == 0x21)
{
int svc = MAC[2+len_a];
int gr = (MAC[3+len_a] << 8) | MAC[4+len_a];
int src = (MAC[5+len_a] << 16) | (MAC[6+len_a] << 8) | MAC[7+len_a];
fprintf (stderr, "\n VCH %d - TG %d SRC %d ", slot, gr, src);
fprintf (stderr, "Group Voice");
if (slot == 0)
{
state->lasttg = gr;
if (src != 0) state->lastsrc = src;
}
else
{
state->lasttgR = gr;
if (src != 0) state->lastsrcR = src;
}
}
//1 or 21, group voice channel message, abb and ext
if (MAC[1+len_a] == 0x2 || MAC[1+len_a] == 0x22)
{
int svc = MAC[2+len_a];
int gr = (MAC[3+len_a] << 16) | (MAC[4+len_a] << 8) | MAC[5+len_a];
int src = (MAC[6+len_a] << 16) | (MAC[7+len_a] << 8) | MAC[8+len_a];
fprintf (stderr, "\n VCH %d - TG %d SRC %d ", slot, gr, src);
fprintf (stderr, "Unit to Unit Voice");
if (slot == 0)
{
state->lasttg = gr;
if (src != 0) state->lastsrc = src;
}
else
{
state->lasttgR = gr;
if (src != 0) state->lastsrcR = src;
}
}
//network status broadcast, abbreviated
if (MAC[1+len_a] == 0x7B)
{
int lra = MAC[2+len_a];
int lwacn = (MAC[3+len_a] << 12) | (MAC[4+len_a] << 4) | ((MAC[5+len_a] & 0xF0) >> 4);
int lsysid = ((MAC[5+len_a] & 0xF) << 8) | MAC[6+len_a];
int channel = (MAC[7+len_a] << 8) | MAC[8+len_a];
int sysclass = MAC[9+len_a];
int lcolorcode = ((MAC[10+len_a] & 0xF) << 8) | MAC[11+len_a];
fprintf (stderr, "\n Network Status Broadcast - Abbreviated \n");
fprintf (stderr, " LRA [%02X] WACN [%05X] SYSID [%03X] NAC [%03X] CHAN-T [%04X]", lra, lwacn, lsysid, lcolorcode, channel);
state->p25_cc_freq = process_channel_to_freq (opts, state, channel);
if (state->p2_hardset == 0 ) //state->p2_is_lcch == 1 shim until CRC is working, prevent bogus data
{
state->p2_wacn = lwacn;
state->p2_sysid = lsysid;
state->p2_cc = lcolorcode;
}
}
//network status broadcast, extended
if (MAC[1+len_a] == 0xFB)
{
int lra = MAC[2+len_a];
int lwacn = (MAC[3+len_a] << 12) | (MAC[4+len_a] << 4) | ((MAC[5+len_a] & 0xF0) >> 4);
int lsysid = ((MAC[5+len_a] & 0xF) << 8) | MAC[6+len_a];
int channelt = (MAC[7+len_a] << 8) | MAC[8+len_a];
int channelr = (MAC[9+len_a] << 8) | MAC[10+len_a];
int sysclass = MAC[9+len_a];
int lcolorcode = ((MAC[12+len_a] & 0xF) << 8) | MAC[13+len_a];
fprintf (stderr, "\n Network Status Broadcast - Extended \n");
fprintf (stderr, " LRA [%02X] WACN [%05X] SYSID [%03X] NAC [%03X] CHAN-T [%04X] CHAN-R [%04X]", lra, lwacn, lsysid, lcolorcode, channelt, channelr);
process_channel_to_freq (opts, state, channelt);
process_channel_to_freq (opts, state, channelr);
if (state->p2_hardset == 0 ) //state->p2_is_lcch == 1 shim until CRC is working, prevent bogus data
{
state->p2_wacn = lwacn;
state->p2_sysid = lsysid;
state->p2_cc = lcolorcode;
}
}
//Adjacent Status Broadcast, abbreviated
if (MAC[1+len_a] == 0x7C)
{
int lra = MAC[2+len_a];
int lsysid = ((MAC[3+len_a] & 0xF) << 8) | MAC[4+len_a];
int rfssid = MAC[5+len_a];
int siteid = MAC[6+len_a];
int channelt = (MAC[7+len_a] << 8) | MAC[8+len_a];
int sysclass = MAC[9+len_a];
if (1 == 1) //state->p2_is_lcch == 1
{
fprintf (stderr, "\n Adjacent Status Broadcast - Abbreviated\n");
fprintf (stderr, " LRA [%02X] RFSS[%03d] SYSID [%03X] CHAN-T [%04X] SSC [%02X]", lra, rfssid, lsysid, channelt, sysclass);
process_channel_to_freq (opts, state, channelt);
}
}
//Adjacent Status Broadcast, extended
if (MAC[1+len_a] == 0xFC)
{
int lra = MAC[2+len_a];
int lsysid = ((MAC[3+len_a] & 0xF) << 8) | MAC[4+len_a];
int rfssid = MAC[5+len_a];
int siteid = MAC[6+len_a];
int channelt = (MAC[7+len_a] << 8) | MAC[8+len_a];
int channelr = (MAC[9+len_a] << 8) | MAC[10+len_a];
int sysclass = MAC[9+len_a];
if (1 == 1) //state->p2_is_lcch == 1
{
fprintf (stderr, "\n Adjacent Status Broadcast - Extended\n");
fprintf (stderr, " LRA [%02X] RFSS[%03d] SYSID [%03X] CHAN-T [%04X] CHAN-R [%04X] SSC [%02X]", lra, rfssid, lsysid, channelt, channelr, sysclass);
process_channel_to_freq (opts, state, channelt);
process_channel_to_freq (opts, state, channelr);
}
}
if ( (len_b + len_c) < 24 && len_c != 0)
{
len_a = len_b;
}
else
{
goto END_PDU;
}
}
END_PDU:
state->p2_is_lcch = 0;
//debug printing
if (opts->payload == 1) //print only if not a null type //&& MAC[1] != 0 //&& MAC[2] != 0
{
fprintf (stderr, "%s", KCYN);
fprintf (stderr, "\n P25 PDU Payload\n ");
for (int i = 0; i < 24; i++)
{
fprintf (stderr, "[%02llX]", MAC[i]);
if (i == 11) fprintf (stderr, "\n ");
}
fprintf (stderr, "%s", KNRM);
}
}

476
src/p25p2_xcch.c Normal file
View File

@ -0,0 +1,476 @@
/*-------------------------------------------------------------------------------
* p25p2_xcch.c
* Phase 2 SACCH/FACCH/LCCH Handling
*
* LWVMOBILE
* 2022-09 DSD-FME Florida Man Edition
*-----------------------------------------------------------------------------*/
#include "dsd.h"
void process_SACCH_MAC_PDU (dsd_opts * opts, dsd_state * state, int payload[180])
{
//Figure out which PDU we are looking at, see above info on 8.4.1
//reorganize bits into bytes and process accordingly
unsigned long long int SMAC[24] = {0}; //22.5 bytes for SACCH MAC PDUs
int byte = 0;
int k = 0;
for (int j = 0; j < 22; j++)
{
for (int i = 0; i < 8; i++)
{
byte = byte << 1;
byte = byte | payload[k];
k++;
}
SMAC[j] = byte;
byte = 0; //reset byte
}
SMAC[22] = (payload[176] << 7) | (payload[177] << 6) | (payload[178] << 5) | (payload[179] << 4);
int opcode = 0;
opcode = (payload[0] << 2) | (payload[1] << 1) | (payload[2] << 0);
int mac_offset = 0;
mac_offset = (payload[3] << 2) | (payload[4] << 1) | (payload[5] << 0);
int b = 9;
b = (payload[8] << 1) | (payload[9] << 0); //combined b1 and b2
int mco_a = 69;
//mco will tell us the number of octets to use in variable length MAC PDUs, need a table or something
mco_a = (payload[10] << 5) | (payload[11] << 4) | (payload[12] << 3) | (payload[13] << 2) | (payload[14] << 0) | (payload[15] << 0);
//get the second mco after determining first message length, see what second mco is and plan accordingly
int mco_b = 69;
//attempt CRC12 check to validate or reject PDU
int err = -2;
if (state->p2_is_lcch == 0)
{
int len =
err = crc12_xb_bridge(payload, 180-12);
if (err != 0) //CRC Failure, warn or skip.
{
if (SMAC[1] == 0x0) //NULL PDU Check, pass if NULL type
{
//fprintf (stderr, " NULL ");
}
else
{
fprintf (stderr, " CRC12 ERR S");
if (state->currentslot == 0) state->dmrburstL = 14;
else state->dmrburstR = 14;
goto END_SMAC;
}
}
}
if (state->p2_is_lcch == 1)
{
int len = 0;
//int len = mac_msg_len[SMAC[1]] * 8;
//if (len > 164) len = 164; //prevent potential stack smash or other issue.
len = 164;
err = crc16_lb_bridge(payload, len);
if (err != 0) //CRC Failure, warn or skip.
{
if (SMAC[1] == 0x0) //NULL PDU Check, pass if NULL type
{
//fprintf (stderr, " NULL ");
}
else
{
fprintf (stderr, " CRC16 ERR L");
state->p2_is_lcch = 0; //turn flag off here
if (state->currentslot == 0) state->dmrburstL = 14;
else state->dmrburstR = 14;
goto END_SMAC;
}
}
}
//remember, slots are inverted here, so set the opposite ones
//monitor, test, and remove these if they cause issues due to inversion
if (opcode == 0x0)
{
fprintf (stderr, " MAC_SIGNAL ");
fprintf (stderr, "%s", KMAG);
process_MAC_VPDU(opts, state, 1, SMAC);
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x1)
{
fprintf (stderr, " MAC_PTT ");
fprintf (stderr, "%s", KGRN);
//remember, slots are inverted here, so set the opposite ones
if (state->currentslot == 1)
{
//reset fourv_counter and dropbyte on PTT
state->fourv_counter[0] = 0;
state->dmrburstL = 20;
fprintf (stderr, "\n VCH 0 - ");
state->lastsrc = (SMAC[13] << 16) | (SMAC[14] << 8) | SMAC[15];
state->lasttg = (SMAC[16] << 8) | SMAC[17];
fprintf (stderr, "TG %d ", state->lasttg);
fprintf (stderr, "SRC %d ", state->lastsrc);
state->payload_algid = SMAC[10];
state->payload_keyid = (SMAC[11] << 8) | SMAC[12];
state->payload_miP = (SMAC[1] << 56) | (SMAC[2] << 48) | (SMAC[3] << 40) | (SMAC[4] << 32) |
(SMAC[5] << 24) | (SMAC[6] << 16) | (SMAC[7] << 8) | (SMAC[8] << 0);
if (state->payload_algid != 0x80 && state->payload_algid != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algid);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyid);
fprintf (stderr, " MI 0x%016llX", state->payload_miP);
fprintf(stderr, " MPTT");
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
}
if (state->currentslot == 0)
{
//reset fourv_counter and dropbyte on PTT
state->fourv_counter[1] = 0;
state->payload_algidR = 0; //zero this out as well
state->dmrburstR = 20;
fprintf (stderr, "\n VCH 1 - ");
state->lastsrcR = (SMAC[13] << 16) | (SMAC[14] << 8) | SMAC[15];
state->lasttgR = (SMAC[16] << 8) | SMAC[17];
fprintf (stderr, "TG %d ", state->lasttgR);
fprintf (stderr, "SRC %d ", state->lastsrcR);
state->payload_algidR = SMAC[10];
state->payload_keyidR = (SMAC[11] << 8) | SMAC[12];
state->payload_miN = (SMAC[1] << 56) | (SMAC[2] << 48) | (SMAC[3] << 40) | (SMAC[4] << 32) |
(SMAC[5] << 24) | (SMAC[6] << 16) | (SMAC[7] << 8) | (SMAC[8] << 0);
if (state->payload_algidR != 0x80 && state->payload_algidR != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algidR);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyidR);
fprintf (stderr, " MI 0x%016llX", state->payload_miN);
fprintf(stderr, " MPTT");
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
}
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x2)
{
fprintf (stderr, " MAC_END_PTT ");
fprintf (stderr, "%s", KRED);
//remember, slots are inverted here, so set the opposite ones
if (state->currentslot == 1)
{
//reset fourv_counter and dropbyte on PTT END
state->fourv_counter[0] = 0;
state->dmrburstL = 23;
state->payload_algid = 0; //zero this out as well
state->payload_keyid = 0; //and this
fprintf (stderr, "\n VCH 0 - ");
fprintf (stderr, "TG %d ", state->lasttg);
fprintf (stderr, "SRC %d ", state->lastsrc);
//print it and then zero out
state->lastsrc = 0;
state->lasttg = 0;
}
if (state->currentslot == 0)
{
//reset fourv_counter and dropbyte on PTT END
state->fourv_counter[1] = 0;
state->dmrburstR = 23;
state->payload_algidR = 0;
state->payload_keyidR = 0;
fprintf (stderr, "\n VCH 1 - ");
fprintf (stderr, "TG %d ", state->lasttgR);
fprintf (stderr, "SRC %d ", state->lastsrcR);
//print it and then zero out
state->lastsrcR = 0;
state->lasttgR = 0;
}
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x3)
{
if (state->currentslot == 1) state->dmrburstL = 24;
else state->dmrburstR = 24;
fprintf (stderr, " MAC_IDLE ");
fprintf (stderr, "%s", KMAG);
process_MAC_VPDU(opts, state, 1, SMAC);
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x4)
{
if (state->currentslot == 1) state->dmrburstL = 21;
else state->dmrburstR = 21;
fprintf (stderr, " MAC_ACTIVE ");
fprintf (stderr, "%s", KMAG);
process_MAC_VPDU(opts, state, 1, SMAC);
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x6)
{
if (state->currentslot == 1) state->dmrburstL = 22;
else state->dmrburstR = 22;
fprintf (stderr, " MAC_HANGTIME ");
fprintf (stderr, "%s", KMAG);
process_MAC_VPDU(opts, state, 1, SMAC);
fprintf (stderr, "%s", KNRM);
}
END_SMAC:
if (1 == 2)
{
//CRC Failure!
}
}
void process_FACCH_MAC_PDU (dsd_opts * opts, dsd_state * state, int payload[156])
{
//Figure out which PDU we are looking at, see above info on 8.4.1
//reorganize bits into bytes and process accordingly
unsigned long long int FMAC[24] = {0}; //19.5 bytes for FACCH MAC PDUs, add padding to end
int byte = 0;
int k = 0;
for (int j = 0; j < 19; j++)
{
for (int i = 0; i < 8; i++)
{
byte = byte << 1;
byte = byte | payload[k];
k++;
}
FMAC[j] = byte;
byte = 0; //reset byte
}
FMAC[19] = (payload[152] << 7) | (payload[153] << 6) | (payload[154] << 5) | (payload[155] << 4);
//add padding bytes so we can have a unified variable MAC PDU handler
for (int i = 0; i < 3; i++)
{
FMAC[i+20] = 0;
}
int opcode = 0;
opcode = (payload[0] << 2) | (payload[1] << 1) | (payload[2] << 0);
int mac_offset = 0;
mac_offset = (payload[3] << 2) | (payload[4] << 1) | (payload[5] << 0);
//attempt CRC check to validate or reject PDU
int err = -2;
if (state->p2_is_lcch == 0)
{
err = crc12_xb_bridge(payload, 156-12);
if (err != 0) //CRC Failure, warn or skip.
{
if (FMAC[1] == 0x0) //NULL PDU Check, pass if NULL
{
//fprintf (stderr, " NULL ");
}
else
{
fprintf (stderr, " CRC12 ERR F");
if (state->currentslot == 0) state->dmrburstL = 14;
else state->dmrburstR = 14;
goto END_FMAC;
}
}
}
//Not sure if a MAC Signal will come on a FACCH or not, so disable to prevent falsing
// if (opcode == 0x0)
// {
// fprintf (stderr, " MAC_SIGNAL ");
// fprintf (stderr, "%s", KMAG);
// process_MAC_VPDU(opts, state, 0, FMAC);
// fprintf (stderr, "%s", KNRM);
// }
if (opcode == 0x1)
{
fprintf (stderr, " MAC_PTT ");
fprintf (stderr, "%s", KGRN);
if (state->currentslot == 0)
{
//reset fourv_counter and dropbyte on PTT
state->fourv_counter[0] = 0;
state->dmrburstL = 20;
fprintf (stderr, "\n VCH 0 - ");
state->lastsrc = (FMAC[13] << 16) | (FMAC[14] << 8) | FMAC[15];
state->lasttg = (FMAC[16] << 8) | FMAC[17];
fprintf (stderr, "TG %d ", state->lasttg);
fprintf (stderr, "SRC %d ", state->lastsrc);
state->payload_algid = FMAC[10];
state->payload_keyid = (FMAC[11] << 8) | FMAC[12];
state->payload_miP = (FMAC[1] << 56) | (FMAC[2] << 48) | (FMAC[3] << 40) | (FMAC[4] << 32) |
(FMAC[5] << 24) | (FMAC[6] << 16) | (FMAC[7] << 8) | (FMAC[8] << 0);
if (state->payload_algid != 0x80 && state->payload_algid != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algid);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyid);
fprintf (stderr, " MI 0x%016llX", state->payload_miP);
fprintf(stderr, " MPTT");
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
}
if (state->currentslot == 1)
{
//reset fourv_counter and dropbyte on PTT
state->fourv_counter[1] = 0;
state->dmrburstR = 20;
fprintf (stderr, "\n VCH 1 - ");
state->lastsrcR = (FMAC[13] << 16) | (FMAC[14] << 8) | FMAC[15];
state->lasttgR = (FMAC[16] << 8) | FMAC[17];
fprintf (stderr, "TG %d ", state->lasttgR);
fprintf (stderr, "SRC %d ", state->lastsrcR);
state->payload_algidR = FMAC[10];
state->payload_keyidR = (FMAC[11] << 8) | FMAC[12];
state->payload_miN = (FMAC[1] << 56) | (FMAC[2] << 48) | (FMAC[3] << 40) | (FMAC[4] << 32) |
(FMAC[5] << 24) | (FMAC[6] << 16) | (FMAC[7] << 8) | (FMAC[8] << 0);
if (state->payload_algidR != 0x80 && state->payload_algidR != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algidR);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyidR);
fprintf (stderr, " MI 0x%016llX", state->payload_miN);
fprintf(stderr, " MPTT");
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
}
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x2)
{
fprintf (stderr, " MAC_END_PTT ");
fprintf (stderr, "%s", KRED);
if (state->currentslot == 0)
{
//reset fourv_counter and dropbyte on PTT END
state->fourv_counter[0] = 0;
state->dmrburstL = 23;
state->payload_algid = 0; //zero this out as well
state->payload_keyid = 0;
fprintf (stderr, "\n VCH 0 - ");
fprintf (stderr, "TG %d ", state->lasttg);
fprintf (stderr, "SRC %d ", state->lastsrc);
//print it and then zero out
state->lastsrc = 0;
state->lasttg = 0;
}
if (state->currentslot == 1)
{
//reset fourv_counter and dropbyte on PTT END
state->fourv_counter[1] = 0;
state->dmrburstR = 23;
state->payload_algidR = 0; //zero this out as well
state->payload_keyidR = 0;
fprintf (stderr, "\n VCH 1 - ");
fprintf (stderr, "TG %d ", state->lasttgR);
fprintf (stderr, "SRC %d ", state->lastsrcR);
//print it and then zero out
state->lastsrcR = 0;
state->lasttgR = 0;
}
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x3)
{
//what else should we zero out here?
//disable any of the lines below if issues are observed
if (state->currentslot == 0)
{
state->payload_algid = 0;
state->payload_keyid = 0;
state->dmrburstL = 24;
state->fourv_counter[0] = 0;
state->lastsrc = 0;
state->lasttg = 0;
}
else
{
state->payload_algidR = 0;
state->payload_keyidR = 0;
state->dmrburstR = 24;
state->fourv_counter[1] = 0;
state->lastsrcR = 0;
state->lasttgR = 0;
}
fprintf (stderr, " MAC_IDLE ");
fprintf (stderr, "%s", KMAG);
process_MAC_VPDU(opts, state, 0, FMAC);
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x4)
{
if (state->currentslot == 0) state->dmrburstL = 21;
else state->dmrburstR = 21;
fprintf (stderr, " MAC_ACTIVE ");
fprintf (stderr, "%s", KMAG);
process_MAC_VPDU(opts, state, 0, FMAC);
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x6)
{
if (state->currentslot == 0) state->dmrburstL = 22;
else state->dmrburstR = 22;
fprintf (stderr, " MAC_HANGTIME ");
fprintf (stderr, "%s", KMAG);
process_MAC_VPDU(opts, state, 0, FMAC);
fprintf (stderr, "%s", KNRM);
}
END_FMAC:
if (1 == 2)
{
//CRC Failure!
}
}