diff --git a/include/dsd.h b/include/dsd.h index cff7da8..151a4d6 100644 --- a/include/dsd.h +++ b/include/dsd.h @@ -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 diff --git a/src/dsd_main.c b/src/dsd_main.c index 6a60fd3..d112015 100644 --- a/src/dsd_main.c +++ b/src/dsd_main.c @@ -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 Frame rate for datascope\n"); printf ("\n"); printf ("Input/Output options:\n"); - printf (" -i 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 Audio output device (default is pulse audio)\n"); + printf (" -i 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 Audio output device (default is pulse audio)(null for no audio output)\n"); printf (" -d Create mbe data files, use this directory\n"); printf (" -r Read/Play saved mbe data from file(s)\n"); printf (" -g Audio output gain (default = 0 = auto, disable = -1)\n"); - printf (" -w 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 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 Manually Set P2 Parameters (WACN, SYSID, CC/NAC)\n"); printf (" (-X BEE00ABC123)\n"); + printf ("\n"); // printf (" -A QPSK modulation auto detection threshold (default=26)\n"); // printf (" -S Symbol buffer size for QPSK decision point tracking\n"); // printf (" (default=36)\n"); diff --git a/src/dsd_ncurses.c b/src/dsd_ncurses.c index d719632..efffbbe 100644 --- a/src/dsd_ncurses.c +++ b/src/dsd_ncurses.c @@ -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 diff --git a/src/p25_frequency.c b/src/p25_frequency.c new file mode 100644 index 0000000..bcb651c --- /dev/null +++ b/src/p25_frequency.c @@ -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); + } + +} \ No newline at end of file diff --git a/src/p25_p2.c b/src/p25_p2.c index cb31532..b95db31 100644 --- a/src/p25_p2.c +++ b/src/p25_p2.c @@ -7,22 +7,7 @@ * LWVMOBILE * 2022-09 DSD-FME Florida Man Edition *-----------------------------------------------------------------------------*/ - /* - * Copyright (C) 2010 DSD Author - * GPG Key ID: 0x3F1D7FD0 (74EF 430D F7F2 0A48 FCE6 F630 FAA2 635D 3F1D 7FD0) - * - * Permission to use, copy, modify, and/or distribute this software for any - * purpose with or without fee is hereby granted, provided that the above - * copyright notice and this permission notice appear in all copies. - * - * THE SOFTWARE IS PROVIDED "AS IS" AND ISC DISCLAIMS ALL WARRANTIES WITH - * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY - * AND FITNESS. IN NO EVENT SHALL ISC BE LIABLE FOR ANY SPECIAL, DIRECT, - * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM - * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE - * OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR - * PERFORMANCE OF THIS SOFTWARE. - */ + #include "dsd.h" //DUID Look Up Table from OP25 @@ -79,25 +64,8 @@ char ambe_fr2[4][24] = {0}; char ambe_fr3[4][24] = {0}; char ambe_fr4[4][24] = {0}; -//MAC message lengths, from OP25 -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 //DE = 40? must be multi fragment? - 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 message lengths +//moved to p25p2_vpdu.c int ts_counter = 0; //timeslot counter for time slots 0-11 int p2bit[4320] = {0}; //4320 @@ -105,16 +73,12 @@ int p2lbit[8640] = {0}; //bits generated by lsfr scrambler, doubling up for offs int p2xbit[4320] = {0}; //bits xored from p2bit and p2lbit int dibit = 0; int vc_counter = 0; -int fourv_counter = 0; int framing_counter = 0; unsigned long long int isch = 0; unsigned long long int isch_decoded = 0; int p2_duid[8] = {0}; int16_t duid_decoded = 0; -int mac_opcode = -1; //initialize to -1 -int mac_offset = -1; -int mco = -1; int ess_b[2][96] = {0}; //96 bits for 4 - 24 bit ESS_B fields starting bit 168 (RS 44,16,29) int ess_a[2][168] = {0}; //ESS_A 1 (96 bit) and 2 (72 bit) fields, starting at bit 168 and bit 266 (RS Parity) @@ -125,40 +89,31 @@ int facch_rs[2][114] = {0}; int sacch[2][180] = {0}; int sacch_rs[2][132] = {0}; -//not sure we need this seperation for LCCH, seems to work fine as SACCH -int lcch[2][180] = {0}; -int lcch_rs[2][132] = {0}; - -//How long is a superframe? What is just one superframe? -//superframe is 360ms, contains 12 consecutive 30 msec timeslots numbered 0 through 11 -//superframe contains Logical Channel LCH 0 and LCH 1 (LCH 0 - 0,2,4,6,8,11)(LCH 1 - 1,3,5,7,9,10) -//one ultraframe contains 4 superframes - //store an entire p2 superframe worth of dibits into a bit buffer void p2_dibit_buffer (dsd_opts * opts, dsd_state * state) { - //S-ISCH alternates, so we should start at VC1 right after S-ISCH - for (int i = 0; i < 2140; i++) //2160 for an entire superframe of dibits (Symbols) //2140 //2120 cut off last ISCH + + for (int i = 0; i < 2140; i++) //2160 for an entire superframe of dibits - 20 sync dibits { - //rip dibits directly from the capture bin file instead of getDibit - if (opts->audio_in_type == 4) //4 + //rip dibits directly from the capture bin file instead of getDibit + if (opts->audio_in_type == 4) //4 + { + dibit = fgetc(opts->symbolfile); + //experimental throttle + useconds_t stime = state->symbol_throttle; + if (state->use_throttle == 1) { - dibit = fgetc(opts->symbolfile); - //experimental throttle - useconds_t stime = state->symbol_throttle; - if (state->use_throttle == 1) - { - usleep(stime); - } + usleep(stime); } - else + } + else + { + dibit = getDibit(opts, state); + if (opts->inverted_p2 == 1) { - dibit = getDibit(opts, state); - if (opts->inverted_p2 == 1) - { - dibit = (dibit ^ 2); //invert dibit here since we are reading it upside down - } + dibit = (dibit ^ 2); //invert dibit here since we are reading it upside down } + } p2bit[i*2] = (dibit >> 1) & 1; p2bit[i*2+1] = (dibit & 1); @@ -169,831 +124,51 @@ void p2_dibit_buffer (dsd_opts * opts, dsd_state * state) int *dibit_p; dibit_p = state->dibit_buf_p - 20; //rewind 20 dibits for (int i = 2140; i < 2160; i++) //2160 for an entire superframe of dibits (Symbols) - { + { dibit = *dibit_p; - dibit_p++; + dibit_p++; p2bit[i*2] = (dibit >> 1) & 1; - p2bit[i*2+1] = (dibit & 1); + p2bit[i*2+1] = (dibit & 1); } } void process_Frame_Scramble (dsd_opts * opts, dsd_state * state) { - //The bits of the scramble sequence corresponding to signal bits that are not scrambled or not used are discarded. - //descramble voice scrambled by LFSR of WACN, SysID, and CC(NAC) - unsigned long long int seed = 0; - //below calc is the same as shifting left the required number of bits...neat. - seed = ( (state->p2_wacn * 16777216) + (state->p2_sysid * 4096) + state->p2_cc ); - unsigned long long int bit = 1; //temp bit for storage during LFSR operation + //The bits of the scramble sequence corresponding to signal bits that are not scrambled or not used are discarded. + //descramble frame scrambled by LFSR of WACN, SysID, and CC(NAC) + unsigned long long int seed = 0; - for (int i = 0; i < 4320; i++) - { + //below calc is the same as shifting left the required number of bits. + seed = ( (state->p2_wacn * 16777216) + (state->p2_sysid * 4096) + state->p2_cc ); + + unsigned long long int bit = 1; //temp bit for storage during LFSR operation + + for (int i = 0; i < 4320; i++) + { //External LFSR in figure 7.1 BBAC - //assign our scramble bit to the array - p2lbit[i] = (seed >> 43) & 0x1; - //assign same bit to position +4320 to allow for a rollover with an offset value - p2lbit[i+4320] = (seed >> 43) & 0x1; - //compute our next scramble bit and shift the seed register and append bit to LSB - bit = ((seed >> 33) ^ (seed >> 19) ^ (seed >> 14) ^ (seed >> 8) ^ (seed >> 3) ^ (seed >> 43)) & 0x1; - seed = (seed << 1) | bit; - } + //assign our scramble bit to the array + p2lbit[i] = (seed >> 43) & 0x1; + //assign same bit to position +4320 to allow for a rollover with an offset value + p2lbit[i+4320] = (seed >> 43) & 0x1; + //compute our next scramble bit and shift the seed register and append bit to LSB + bit = ((seed >> 33) ^ (seed >> 19) ^ (seed >> 14) ^ (seed >> 8) ^ (seed >> 3) ^ (seed >> 43)) & 0x1; + seed = (seed << 1) | bit; + } for (int i = 0; i < 4300; i++) - { - //offset by 20 for sync, then 360 for each ts frame off - p2xbit[i] = p2bit[i] ^ p2lbit[i+20+(360*state->p2_scramble_offset)]; + { + //offset by 20 for sync, then 360 for each ts frame off from start of superframe + p2xbit[i] = p2bit[i] ^ p2lbit[i+20+(360*state->p2_scramble_offset)]; } } -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 - //get first b1 and b2 values, and first offset value - //need a lookup table for message length values...aquired one from OP25 +//process_MAC_VPDU +//moved to p25p2_vpdu.c - //b values - 0 = Unique TDMA Message, 1 Phase 1 OSP/ISP abbreviated - // 2 = Manufacturer Message, 3 Phase 1 OSP/ISP extended/explicit - - int b_a = MAC[1] >> 6; - int mco_a = MAC[1] & 0x3F; - int len_a = 0; //initial on zero, then set equal to len_b after first message rep - int len_b = mac_msg_len[MAC[1]]; //thinking the lookup is based on the full byte 1 - - 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; - - //init potential second message info - //this area needs reworking, consider using the DMR 1/2 or 3/4 rate superframe buffer - int b_b = 0; - int mco_b = 0; - - //assigning here if OECI MAC SIGNAL, after passing RS and CRC - if (state->p2_is_lcch == 1) - { - if (slot == 0) state->dmrburstL = 30; - else state->dmrburstR = 30; - } - - //if len is 0, then just skip the check entirely, or greater than 14? See comment below. - if (len_b == 0 || len_b > 14) //DE had a message length of 40! 40 What!? Way out of bounds on the array! - { - goto END_PDU; - } - - for (int i = 0; i < 1; i++) //short loop to get both messages if available - { - - //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); - fprintf (stderr, "%s", KNRM); - } - - } - - //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); - fprintf (stderr, "%s", KNRM); - } - - } - - //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); - fprintf (stderr, "\n Channel 2 [%04X] Group 2 [%d][%04X]", channel2, group2, group2); - - } - - //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); - //maybe, not sure if we want this enabled here or not - if (slot == 0) - { - state->lasttg = group; - } - else state->lasttgR = group; - - } - - //MFID90 Group Regroup Voice Channel User - Abbreviated - //this is the one that was playing but had no group or src in tdma6.bin - 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) - { - //fprintf (stderr, "%s", KCYN); - 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); - fprintf (stderr, "%s", KNRM); - if (state->p2_hardset == 0 ) - { - 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, "%s", KCYN); - 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); - fprintf (stderr, "%s", KNRM); - if (state->p2_hardset == 0 ) - { - 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 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]; - if (1 == 1) //state->p2_is_lcch == 1 - { - //fprintf (stderr, "%s", KCYN); - 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); - fprintf (stderr, "%s", KNRM); - } - - } - - //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]; - //int lcolorcode = ((MAC[12+len_a] & 0xF) << 8) | MAC[13+len_a]; - if (1 == 1) //state->p2_is_lcch == 1 - { - //fprintf (stderr, "%s", KCYN); - 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); - fprintf (stderr, "%s", KNRM); - } - - } - - //set len_b for next potential message - len_b = mac_msg_len[MAC[1+len_b]]; - //set len_a as offset for next message if applicable, or exit loop early - if (len_b != 0 || len_b < 15) //DE had a message length of 40! 40 What!? Way out of bounds on the array! - { - len_a = len_b; - } - else - { - goto END_PDU; - } - - } - - END_PDU: - state->p2_is_lcch = 0; //toggle off so subsequent things can't trip it again - //debug printing - if (opts->payload == 1 && MAC[1] != 0 && MAC[2] != 0) //print only if not a null type - { - fprintf (stderr, "%s", KCYN); - fprintf (stderr, "\n Full MAC vPDU Payload\n "); - for (int i = 0; i < 24; i++) - { - fprintf (stderr, "[%02llX]", MAC[i]); - if (i == 11) fprintf (stderr, "\n "); - } - fprintf (stderr, "%s", KNRM); - } - -} - -//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_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 = 0; - 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 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 on PTT - state->fourv_counter[1] = 0; - - state->payload_algidR = 0; - - 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 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 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, "%s", KMAG); - fprintf (stderr, " MAC_IDLE "); - 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! - } - -} +//process_SACCH_MAC_PDU and process_FACCH_MAC_PDU +//moved to p25p2_xcch.c void process_FACCHc (dsd_opts * opts, dsd_state * state) { @@ -1205,11 +380,11 @@ void process_SACCHs (dsd_opts * opts, dsd_state * state) void process_ISCH (dsd_opts * opts, dsd_state * state) { isch = 0; - for (int i = 0; i < 40; i++) - { - isch = isch << 1; - isch = isch | p2bit[i+320+(360*framing_counter)]; - } + for (int i = 0; i < 40; i++) + { + isch = isch << 1; + isch = isch | p2bit[i+320+(360*framing_counter)]; + } if (isch == 0x575D57F7FF) //S-ISCH frame sync, pass; { @@ -1227,13 +402,9 @@ void process_ISCH (dsd_opts * opts, dsd_state * state) int chan_num = (isch_decoded >> 5) & 0x3; state->p2_vch_chan_num = chan_num; - // fprintf (stderr, "\n I-ISCH = %010llX - Decoded = %02llX -", isch, isch_decoded); - // fprintf (stderr, " UF Count = %1X - Free = %1X - Loc = %1X Chan = %1X TS = %02d - OFF = %02d", uf_count, free, isch_loc, chan_num, framing_counter, state->p2_scramble_offset); - //determine where the offset should be by first finding TS 0 if (chan_num == 0 && isch_loc == 0) { - //this rule seems to satisfy all conditions I've seen in all samples collected so far state->p2_scramble_offset = 11 - framing_counter; } @@ -1295,6 +466,7 @@ void process_4V (dsd_opts * opts, dsd_state * state) } state->fourv_counter[state->currentslot]++; + //sanity check, reset if greater than 3 (bad signal or tuned away) if (state->fourv_counter[state->currentslot] > 3) { @@ -1330,8 +502,8 @@ void process_ESS (dsd_opts * opts, dsd_state * state) parity[i] = ess_a[state->currentslot][i]; } - int ec = 69; //initialize with a bad value - ec = ez_rs28_ess(payload, parity); //working now! + int ec = 69; + ec = ez_rs28_ess(payload, parity); int algid = 0; for (short i = 0; i < 8; i++) @@ -1399,7 +571,7 @@ void process_ESS (dsd_opts * opts, dsd_state * state) } fprintf (stderr, "%s", KNRM); - state->fourv_counter[state->currentslot] = 0; //reset after processing + state->fourv_counter[state->currentslot] = 0; } @@ -1472,10 +644,10 @@ void process_P2_DUID (dsd_opts * opts, dsd_state * state) int err_counter = 0; //add time to mirror printFrameSync - time_t now; - char * getTime(void) //get pretty hh:mm:ss timestamp - { - time_t t = time(NULL); + time_t now; + char * getTime(void) //get pretty hh:mm:ss timestamp + { + time_t t = time(NULL); char * curr; char * stamp = asctime(localtime( & t)); @@ -1486,12 +658,12 @@ void process_P2_DUID (dsd_opts * opts, dsd_state * state) curr = strtok(NULL, " "); return curr; - } + } for (ts_counter = 0; ts_counter < 12; ts_counter++) { - p2_duid[0] = p2bit[0+(ts_counter*360)]; + p2_duid[0] = p2bit[0+(ts_counter*360)]; p2_duid[1] = p2bit[1+(ts_counter*360)]; p2_duid[2] = p2bit[74+(ts_counter*360)]; p2_duid[3] = p2bit[75+(ts_counter*360)]; @@ -1509,124 +681,121 @@ void process_P2_DUID (dsd_opts * opts, dsd_state * state) } duid_decoded = duid_lookup[p2_duid_complete]; - fprintf (stderr, "\n"); - fprintf (stderr,"%s ", getTime()); - fprintf (stderr, " P25p2 "); + fprintf (stderr, "\n"); + fprintf (stderr,"%s ", getTime()); + fprintf (stderr, " P25p2 "); - if (state->currentslot == 0) - { - state->currentslot = 1; - } - else - { - state->currentslot = 0; - } + if (state->currentslot == 0) + { + state->currentslot = 1; + } + else + { + state->currentslot = 0; + } - //print our VCH channel number, or print S for SACCH since its inverted - if (state->currentslot == 0 && duid_decoded != 3 && duid_decoded != 12) - { - fprintf (stderr, "VCH 0 "); - } - else if (state->currentslot == 1 && duid_decoded != 3 && duid_decoded != 12) - { - fprintf (stderr, "VCH 1 "); - } - else fprintf (stderr, "VCH S "); + //print our VCH channel number, or print S for SACCH since its inverted + if (state->currentslot == 0 && duid_decoded != 3 && duid_decoded != 12) + { + fprintf (stderr, "VCH 0 "); + } + else if (state->currentslot == 1 && duid_decoded != 3 && duid_decoded != 12) + { + fprintf (stderr, "VCH 1 "); + } + else fprintf (stderr, "VCH S "); if (duid_decoded == 0) { - fprintf (stderr, " 4V %d", state->fourv_counter[state->currentslot]+1); - if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && - state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) - { - process_4V (opts, state); - } + fprintf (stderr, " 4V %d", state->fourv_counter[state->currentslot]+1); + if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && + state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) + { + process_4V (opts, state); + } } else if (duid_decoded == 6) { - fprintf (stderr, " 2V"); - if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && - state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) - { - process_2V (opts, state); - } + fprintf (stderr, " 2V"); + if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && + state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) + { + process_2V (opts, state); + } } - else if (duid_decoded == 3) + else if (duid_decoded == 3) { - if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && - state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) - { - process_SACCHs(opts, state); - } + if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && + state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) + { + process_SACCHs(opts, state); + } } - else if (duid_decoded == 12) + else if (duid_decoded == 12) { - process_SACCHc(opts, state); + process_SACCHc(opts, state); } - else if (duid_decoded == 15) + else if (duid_decoded == 15) { - process_FACCHc(opts, state); + process_FACCHc(opts, state); } - else if (duid_decoded == 9) + else if (duid_decoded == 9) { - if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && - state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) - { - process_FACCHs(opts, state); - } + if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && + state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) + { + process_FACCHs(opts, state); + } } - else if (duid_decoded == 13) + else if (duid_decoded == 13) { - //fprintf (stderr, " LCCH TDMA CC"); //w/o scrambling OECI - // if (state->currentslot == 0) state->dmrburstL = 30; - // else state->dmrburstR = 30; + state->p2_is_lcch = 1; + process_SACCHc(opts, state); + } + else if (duid_decoded == 4) + { + fprintf (stderr, " LCCH Sc"); //w/ scrambling + // if (state->currentslot == 0) state->dmrburstL = 31; + // else state->dmrburstR = 31; + if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && + state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) + { state->p2_is_lcch = 1; - process_SACCHc(opts, state); - } - else if (duid_decoded == 4) - { - fprintf (stderr, " LCCH Sc"); //w/ scrambling - // if (state->currentslot == 0) state->dmrburstL = 31; - // else state->dmrburstR = 31; - if (state->p2_wacn != 0 && state->p2_cc != 0 && state->p2_sysid != 0 && - state->p2_wacn != 0xFFFFF && state->p2_cc != 0xFFF && state->p2_sysid != 0xFFF) - { - state->p2_is_lcch = 1; - process_SACCHs(opts, state); - } - } - else - { - fprintf (stderr, " DUID ERR %d", duid_decoded); - if (state->currentslot == 0) state->dmrburstL = 12; - else state->dmrburstR = 12; - err_counter++; + process_SACCHs(opts, state); } - if (err_counter > 1) //&& opts->aggressive_framesync == 1 - { - //zero out values when errs accumulate in DUID - //most likely cause will be signal drop or tuning away - state->payload_algid = 0; - state->payload_keyid = 0; - state->payload_algidR = 0; - state->payload_keyidR = 0; - state->lastsrc = 0; - state->lastsrcR = 0; - state->lasttg = 0; - state->lasttgR = 0; - state->p2_is_lcch = 0; - state->fourv_counter[0] = 0; - state->fourv_counter[1] = 0; + } + else + { + fprintf (stderr, " DUID ERR %d", duid_decoded); + if (state->currentslot == 0) state->dmrburstL = 12; + else state->dmrburstR = 12; + err_counter++; + } + if (err_counter > 1) //&& opts->aggressive_framesync == 1 + { + //zero out values when errs accumulate in DUID + //most likely cause will be signal drop or tuning away + state->payload_algid = 0; + state->payload_keyid = 0; + state->payload_algidR = 0; + state->payload_keyidR = 0; + state->lastsrc = 0; + state->lastsrcR = 0; + state->lasttg = 0; + state->lasttgR = 0; + state->p2_is_lcch = 0; + state->fourv_counter[0] = 0; + state->fourv_counter[1] = 0; - goto END; - } - //since we are in a while loop, run ncursesPrinter here. - if (opts->use_ncurses_terminal == 1) - { - ncursesPrinter(opts, state); - } - //add 360 bits to each counter - vc_counter = vc_counter + 360; + goto END; + } + //since we are in a while loop, run ncursesPrinter here. + if (opts->use_ncurses_terminal == 1) + { + ncursesPrinter(opts, state); + } + //add 360 bits to each counter + vc_counter = vc_counter + 360; } END: @@ -1639,41 +808,23 @@ void process_P2_DUID (dsd_opts * opts, dsd_state * state) void processP2 (dsd_opts * opts, dsd_state * state) { - //hard code here for ease of use while figuring this out - //295 samples - // state->p2_wacn = 0xBEE00; //BEE00 - // state->p2_sysid = 0x295; //39A - // state->p2_cc = 0x290; //399 + state->dmr_stereo = 1; + state->currentslot = 1; - //adp_tdma.bin and NJ samples - // state->p2_wacn = 0xBEE00; //BEE00 - // state->p2_sysid = 0x39A; //39A //56C - // state->p2_cc = 0x399; //399 //561 - - //barrow samples - // state->p2_wacn = 0xBEE00; //BEE00 - // state->p2_sysid = 0x56C; //39A //56C - // state->p2_cc = 0x561; //399 //561 - - state->dmr_stereo = 1; //flag on now and flag off on exit - state->currentslot = 1; //start on 1, initial flip will make it zero again - - //collect and store a superframe worth of dibits for now, may change later to just 2 TS frames - p2_dibit_buffer (opts, state); + p2_dibit_buffer (opts, state); //look at our ISCH values and determine location in superframe before running frame scramble - //12 is really the s-isch that initiated this, I just tacked it onto the very end for no reason for (framing_counter = 0; framing_counter < 11; framing_counter++) //11, or 12 { process_ISCH (opts, state); //run ISCH in here so we know when to start descramble offset } //frame_scramble runs lfsr and creates an array of unscrambled bits to pull from - process_Frame_Scramble (opts, state); + process_Frame_Scramble (opts, state); //process DUID will run through all collected frames and handle them appropriately - process_P2_DUID (opts, state); - state->dmr_stereo = 0; //flag off - state->p2_is_lcch = 0; //flag off - fprintf (stderr, "\n"); //print a line break at the end + process_P2_DUID (opts, state); + state->dmr_stereo = 0; + state->p2_is_lcch = 0; + fprintf (stderr, "\n"); } diff --git a/src/p25p1_tsbk.c b/src/p25p1_tsbk.c index d5ff484..f202272 100644 --- a/src/p25p1_tsbk.c +++ b/src/p25p1_tsbk.c @@ -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"); } diff --git a/src/p25p2_vpdu.c b/src/p25p2_vpdu.c new file mode 100644 index 0000000..6154555 --- /dev/null +++ b/src/p25p2_vpdu.c @@ -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); + } + +} diff --git a/src/p25p2_xcch.c b/src/p25p2_xcch.c new file mode 100644 index 0000000..947d260 --- /dev/null +++ b/src/p25p2_xcch.c @@ -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! + } + +} \ No newline at end of file