diff --git a/include/dsd.h b/include/dsd.h index fef1b0d..9d35dd6 100644 --- a/include/dsd.h +++ b/include/dsd.h @@ -1300,6 +1300,17 @@ void dmr_reset_blocks (dsd_opts * opts, dsd_state * state); void dmr_lrrp (dsd_opts * opts, dsd_state * state, uint16_t len, uint32_t source, uint32_t dest, uint8_t * DMR_PDU); void dmr_locn (dsd_opts * opts, dsd_state * state, uint16_t len, uint8_t * DMR_PDU); +//p25 pdu handling +uint8_t p25_decrypt_pdu(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t alg_id, uint16_t key_id, unsigned long long int mi, int len); +uint8_t p25_decode_es_header(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t * sap, int * ptr, int len); +uint8_t p25_decode_es_header_2(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t * sap, int * ptr, int len); +void p25_decode_extended_address(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t * sap, int * ptr); +void p25_decode_pdu_trunking(dsd_opts * opts, dsd_state * state, uint8_t * mpdu_byte); +void p25_decode_pdu_header(dsd_opts * opts, dsd_state * state, uint8_t * input); +void p25_decode_pdu_data(dsd_opts * opts, dsd_state * state, uint8_t * input, int len); +void p25_decode_rsp(uint8_t C, uint8_t T, uint8_t S, char * rsp_string); +void p25_decode_sap(uint8_t SAP, char * sap_string); + //Time and Date Functions char * getTime(); char * getTimeC(); diff --git a/src/dmr_pdu.c b/src/dmr_pdu.c index be3de66..c09655c 100644 --- a/src/dmr_pdu.c +++ b/src/dmr_pdu.c @@ -268,6 +268,19 @@ void dmr_ip_pdu (dsd_opts * opts, dsd_state * state, uint16_t len, uint8_t * DMR uint8_t icmp_code = DMR_PDU[21]; uint16_t icmp_chk = port2; fprintf (stderr, "\n ICMP Protocol; Type: %02X; Code: %02X; Checksum: %02X;", icmp_type, icmp_code, icmp_chk); + if (icmp_type == 3) + { + fprintf (stderr, " Destination"); + if (icmp_code == 0) + fprintf (stderr, " Network"); + else if (icmp_code == 1) + fprintf (stderr, " Host"); + else if (icmp_code == 2) + fprintf (stderr, " Protocol"); + else if (icmp_code == 3) + fprintf (stderr, " Port"); + fprintf (stderr, " Unreachable;"); + } //see: https://en.wikipedia.org/wiki/Internet_Control_Message_Protocol //look at attached message, if present if (DMR_PDU[28] == 0x45) //if another chained IPv4 header and/or message diff --git a/src/p25p1_mdpu.c b/src/p25p1_mdpu.c index 1061469..e8739b7 100644 --- a/src/p25p1_mdpu.c +++ b/src/p25p1_mdpu.c @@ -1,69 +1,14 @@ /*------------------------------------------------------------------------------- * p25p1_mpdu.c - * P25p1 Multi Block PDU and Multi Block Trunking + * P25p1 Multi Block PDU Assembly * * LWVMOBILE - * 2024-12 DSD-FME Florida Man Edition + * 2025-03 DSD-FME Florida Man Edition *-----------------------------------------------------------------------------*/ #include "dsd.h" -void processRSP(uint8_t C, uint8_t T, uint8_t S, char * rsp_string) -{ - - if (C == 0) sprintf (rsp_string, " ACK (Success);"); - else if (C == 2) sprintf (rsp_string, " SACK (Retry);"); - else if (C == 1) - { - if (T == 0) sprintf (rsp_string, " NACK (Illegal Format);"); - else if (T == 1) sprintf (rsp_string, " NACK (CRC32 Failure);"); - else if (T == 2) sprintf (rsp_string, " NACK (Memory Full);"); - else if (T == 3) sprintf (rsp_string, " NACK (FSN Sequence Error);"); - else if (T == 4) sprintf (rsp_string, " NACK (Undeliverable);"); - else if (T == 5) sprintf (rsp_string, " NACK (NS/VR Sequence Error);"); //depreciated - else if (T == 6) sprintf (rsp_string, " NACK (Invalid User on System);"); - } - - fprintf (stderr, " Response Packet:%s C: %X; T: %X; S: %X; ", rsp_string, C, T, S); - -} - -void processSAP(uint8_t SAP, char * sap_string) -{ - - if (SAP == 0) sprintf (sap_string, " User Data;"); - else if (SAP == 1) sprintf (sap_string, " Encrypted User Data;"); - else if (SAP == 2) sprintf (sap_string, " Circuit Data;"); - else if (SAP == 3) sprintf (sap_string, " Circuit Data Control;"); - else if (SAP == 4) sprintf (sap_string, " Packet Data;"); - else if (SAP == 5) sprintf (sap_string, " Address Resolution Protocol;"); - else if (SAP == 6) sprintf (sap_string, " SNDCP Packet Data Control;"); - else if (SAP == 15) sprintf (sap_string, " Packet Data Scan Preamble;"); - else if (SAP == 29) sprintf (sap_string, " Packet Data Encryption Support;"); - else if (SAP == 31) sprintf (sap_string, " Extended Address;"); - else if (SAP == 32) sprintf (sap_string, " Registration and Authorization;"); - else if (SAP == 33) sprintf (sap_string, " Channel Reassignment;"); - else if (SAP == 34) sprintf (sap_string, " System Configuration;"); - else if (SAP == 35) sprintf (sap_string, " Mobile Radio Loopback;"); - else if (SAP == 36) sprintf (sap_string, " Mobile Radio Statistics;"); - else if (SAP == 37) sprintf (sap_string, " Mobile Radio Out of Service;"); - else if (SAP == 38) sprintf (sap_string, " Mobile Radio Paging;"); - else if (SAP == 39) sprintf (sap_string, " Mobile Radio Configuration;"); - else if (SAP == 40) sprintf (sap_string, " Unencrypted Key Management;"); - else if (SAP == 41) sprintf (sap_string, " Encrypted Key Management;"); - else if (SAP == 48) sprintf (sap_string, " Location Service;"); - else if (SAP == 61) sprintf (sap_string, " Trunking Control;"); - else if (SAP == 63) sprintf (sap_string, " Encrypted Trunking Control;"); - - //catch all for everything else - else sprintf (sap_string, " Unknown SAP;"); - - fprintf (stderr, "SAP: 0x%02X;%s ", SAP, sap_string); - -} - -// static uint32_t crc32mbf(uint8_t buf[], int len) -static uint32_t crc32mbf(uint8_t * buf, int len) +uint32_t crc32mbf(uint8_t * buf, int len) { uint32_t g = 0x04c11db7; uint64_t crc = 0; @@ -137,8 +82,6 @@ void processMPDU(dsd_opts * opts, dsd_state * state) memset (ec, -2, sizeof(ec)); memset (err, -2, sizeof(err)); - int MFID = 0xFF; //Manufacturer ID - uint8_t mpdu_byte[18*129]; memset (mpdu_byte, 0, sizeof(mpdu_byte)); @@ -147,20 +90,6 @@ void processMPDU(dsd_opts * opts, dsd_state * state) uint8_t fmt = 0; uint8_t sap = 0; uint8_t blks = 0; - uint8_t opcode = 0; - - uint32_t address = 0; //source, or destination address depending on the io bit (outbound is target) - uint8_t fmf = 0; - uint8_t pad = 0; - uint8_t ns = 0; - uint8_t fsnf = 0; - uint8_t offset = 0; - - //response packet - uint8_t class = 0; - uint8_t type = 0; - uint8_t status = 0; - int end = 3; //ending value for data gathering repetitions (default at 3) //CRC32 @@ -193,16 +122,6 @@ void processMPDU(dsd_opts * opts, dsd_state * state) { skipdibit = 0; status_count++; - - // fprintf (stderr, " S:%02d; D:%03d; i:%03d;", status_count, (j*101)+i+57, i); //debug Status Count, Total Dibit Count, and i (compare to symbol rx table) - // fprintf (stderr, " S:%02d; CD:%02d; i:%03d; d:%d;", status_count, dibit_count, i, dibit); //debug Status Count, Current Dibit Count, i, and status dibit - - //debug, intepret status dibit (no fec or parity, so could easily be errroneous, could also be different on a data channel vs conventional?) - // if (dibit == 0) fprintf (stderr, " Unknown - Use for Talk Around;"); - // if (dibit == 1) fprintf (stderr, " Inbound Channel Busy;"); - // if (dibit == 2) fprintf (stderr, " Unknown - Use for Inbound or Outbound;"); - // if (dibit == 3) fprintf (stderr, " Inbound Channel Idle;"); - } skipdibit++; //increment @@ -212,8 +131,6 @@ void processMPDU(dsd_opts * opts, dsd_state * state) break; } - // fprintf (stderr, " DC: %d\n", dibit_count); //debug - //send header to p25_12 and return tsbk_byte if (j == 0) ec[j] = p25_12 (tsbk_dibit, tsbk_byte); @@ -279,27 +196,14 @@ void processMPDU(dsd_opts * opts, dsd_state * state) mpdu_byte[i+(j*12)] = byte; //add to completed MBF format 12 rate bytes } - //check header data to see if this is a 12 rate, or 4 rate packet data unit - if (j == 0 && err[0] == 0) + //check header data to see if this is a 12 rate, or 34 rate packet data unit + if ( (j == 0 && err[0] == 0) || (j == 0 && opts->aggressive_framesync == 0)) { an = (mpdu_byte[0] >> 6) & 0x1; io = (mpdu_byte[0] >> 5) & 0x1; fmt = mpdu_byte[0] & 0x1F; sap = mpdu_byte[1] & 0x3F; blks = mpdu_byte[6] & 0x7F; - MFID = mpdu_byte[2]; - address = (mpdu_byte[3] << 16) | (mpdu_byte[4] << 8) | mpdu_byte[5]; //LLID, inbound is the source, outbound is the target - - fmf = (mpdu_byte[6] >> 7) & 0x1; - pad = mpdu_byte[7] & 0x1F; - ns = (mpdu_byte[8] >> 4) & 0x7; - fsnf = mpdu_byte[8] & 0xF; - offset = mpdu_byte[9] & 0x3F; - - //response packet - class = (mpdu_byte[1] >> 6) & 0x3; - type = (mpdu_byte[1] >> 3) & 0x7; - status = (mpdu_byte[1] >> 0) & 0x7; if (an == 1 && fmt == 0b10110) //confirmed data packet header block r34 = 1; @@ -316,604 +220,33 @@ void processMPDU(dsd_opts * opts, dsd_state * state) } if (err[0] == 0) - { - fprintf (stderr, "%s",KGRN); - fprintf (stderr, " P25 Data - AN: %d; IO: %d; FMT: %02X; ", an, io, fmt); - char sap_string[40]; - char rsp_string[40]; - if (fmt != 3) processSAP (sap, sap_string); //decode SAP to see what kind of data we are dealing with - else processRSP (class, type, status, rsp_string); //decode the response type (ack, nack, sack) - if (sap != 61 && sap != 63) //Not too interested in viewing these on trunking control, just data packets mostly - fprintf (stderr, "\n F: %d; Blocks: %02X; Pad: %d; NS: %d; FSNF: %d; Offset: %d; MFID: %02X;", fmf, blks, pad, ns, fsnf, offset, MFID); - if (io == 1 && sap != 61 && sap != 63) //destination address if IO bit set - fprintf (stderr, " Address: %d;", address); + p25_decode_pdu_header (opts, state, mpdu_byte); + else if (opts->aggressive_framesync == 0) + p25_decode_pdu_header (opts, state, mpdu_byte); - //Print to Data Call String for Ncurses Terminal (if not trunking control or response packet) - if (sap != 61 && sap != 63 && fmt != 3) - sprintf (state->dmr_lrrp_gps[0], "Data Call:%s SAP:%02X; Address: %d; ", sap_string, sap, address); - - } - else //crc error, so we can't validate information as accurate + if (err[0] != 0) //crc error, so we can't validate information as accurate { fprintf (stderr, "%s",KRED); fprintf (stderr, " P25 Data Header CRC Error"); fprintf (stderr, "%s",KNRM); - end = 1; //go ahead and end after this loop + if (opts->aggressive_framesync != 0) + end = 1; //go ahead and end after this loop } //trunking blocks if ((sap == 0x3D) && ((fmt == 0x17) || (fmt == 0x15))) { - if (fmt == 0x15) fprintf (stderr, " UNC"); - else fprintf (stderr, " ALT"); - fprintf (stderr, " MBT"); - if (fmt == 0x17) opcode = mpdu_byte[7] & 0x3F; //alt MBT - else opcode = mpdu_byte[12] & 0x3F; //unconf MBT - fprintf (stderr, " - OP: %02X", opcode); - - + //CRC32 is now working! CRCExtracted = (mpdu_byte[(12*(blks+1))-4] << 24) | (mpdu_byte[(12*(blks+1))-3] << 16) | (mpdu_byte[(12*(blks+1))-2] << 8) | (mpdu_byte[(12*(blks+1))-1] << 0); CRCComputed = crc32mbf(mpdu_byte+12, (96*blks)-32); if (CRCComputed == CRCExtracted) err[1] = 0; - //group list mode so we can look and see if we need to block tuning any groups, etc - char mode[8]; //allow, block, digital, enc, etc - sprintf (mode, "%s", ""); - - //if we are using allow/whitelist mode, then write 'B' to mode for block - //comparison below will look for an 'A' to write to mode if it is allowed - if (opts->trunk_use_allow_list == 1) sprintf (mode, "%s", "B"); - - //just going to do a couple for now -- the extended format will not be vPDU compatible :( + //pass the PDU to p25_decode_pdu_trunking if (err[0] == 0 && err[1] == 0 && io == 1 && fmt == 0x17) //ALT Format - { - //NET_STS_BCST -- TIA-102.AABC-D 6.2.11.2 - if (opcode == 0x3B) - { - int lra = mpdu_byte[3]; - int sysid = ((mpdu_byte[4] & 0xF) << 8) | mpdu_byte[5]; - int res_a = mpdu_byte[8]; - int res_b = mpdu_byte[9]; - long int wacn = (mpdu_byte[12] << 12) | (mpdu_byte[13] << 4) | (mpdu_byte[14] >> 4); - int channelt = (mpdu_byte[15] << 8) | mpdu_byte[16]; - int channelr = (mpdu_byte[17] << 8) | mpdu_byte[18]; - int ssc = mpdu_byte[19]; - UNUSED3(res_a, res_b, ssc); - fprintf (stderr, "%s",KYEL); - fprintf (stderr, "\n Network Status Broadcast MBT - Extended \n"); - fprintf (stderr, " LRA [%02X] WACN [%05lX] SYSID [%03X] NAC [%03llX]\n", lra, wacn, sysid, state->p2_cc); - fprintf (stderr, " CHAN-T [%04X] CHAN-R [%04X]", channelt, channelr); - long int ct_freq = process_channel_to_freq(opts, state, channelt); - long int cr_freq = process_channel_to_freq(opts, state, channelr); - UNUSED(cr_freq); - - state->p25_cc_freq = ct_freq; - state->p25_cc_is_tdma = 0; //flag off for CC tuning purposes when system is qpsk - - //place the cc freq into the list at index 0 if 0 is empty, or not the same, - //so we can hunt for rotating CCs without user LCN list - if (state->trunk_lcn_freq[0] == 0 || state->trunk_lcn_freq[0] != state->p25_cc_freq) - { - state->trunk_lcn_freq[0] = state->p25_cc_freq; - } - - //only set IF these values aren't already hard set by the user - if (state->p2_hardset == 0) - { - state->p2_wacn = wacn; - state->p2_sysid = sysid; - } - } - //RFSS Status Broadcast - Extended 6.2.15.2 - else if (opcode == 0x3A) - { - int lra = mpdu_byte[3]; - int lsysid = ((mpdu_byte[4] & 0xF) << 8) | mpdu_byte[5]; - int rfssid = mpdu_byte[12]; - int siteid = mpdu_byte[13]; - int channelt = (mpdu_byte[14] << 8) | mpdu_byte[15]; - int channelr = (mpdu_byte[16] << 8) | mpdu_byte[17]; - int sysclass = mpdu_byte[18]; - fprintf (stderr, "%s",KYEL); - fprintf (stderr, "\n RFSS Status Broadcast MBF - Extended \n"); - fprintf (stderr, " LRA [%02X] SYSID [%03X] RFSS ID [%03d] SITE ID [%03d]\n CHAN-T [%04X] CHAN-R [%02X] SSC [%02X] ", lra, lsysid, rfssid, siteid, channelt, channelr, sysclass); - process_channel_to_freq (opts, state, channelt); - process_channel_to_freq (opts, state, channelr); - - state->p2_siteid = siteid; - state->p2_rfssid = rfssid; - } - - //Adjacent Status Broadcast (ADJ_STS_BCST) Extended 6.2.2.2 - else if (opcode == 0x3C) - { - int lra = mpdu_byte[3]; - int cfva = mpdu_byte[4] >> 4; - int lsysid = ((mpdu_byte[4] & 0xF) << 8) | mpdu_byte[5]; - int rfssid = mpdu_byte[8]; - int siteid = mpdu_byte[9]; - int channelt = (mpdu_byte[12] << 8) | mpdu_byte[13]; - int channelr = (mpdu_byte[14] << 8) | mpdu_byte[15]; - int sysclass = mpdu_byte[16]; - long int wacn = (mpdu_byte[17] << 12) | (mpdu_byte[18] << 4) | (mpdu_byte[19] >> 4); - fprintf (stderr, "%s",KYEL); - fprintf (stderr, "\n Adjacent Status Broadcast - Extended\n"); - fprintf (stderr, " LRA [%02X] CFVA [%X] RFSS[%03d] SITE [%03d] SYSID [%03X]\n CHAN-T [%04X] CHAN-R [%04X] SSC [%02X] WACN [%05lX]\n ", lra, cfva, rfssid, siteid, lsysid, channelt, channelr, sysclass, wacn); - if (cfva & 0x8) fprintf (stderr, " Conventional"); - if (cfva & 0x4) fprintf (stderr, " Failure Condition"); - if (cfva & 0x2) fprintf (stderr, " Up to Date (Correct)"); - else fprintf (stderr, " Last Known"); - if (cfva & 0x1) fprintf (stderr, " Valid RFSS Connection Active"); - process_channel_to_freq (opts, state, channelt); - process_channel_to_freq (opts, state, channelr); - - } - - //Group Voice Channel Grant - Extended - else if (opcode == 0x0) - { - int svc = mpdu_byte[8]; - int channelt = (mpdu_byte[14] << 8) | mpdu_byte[15]; - int channelr = (mpdu_byte[16] << 8) | mpdu_byte[17]; - long int source = (mpdu_byte[3] << 16) |(mpdu_byte[4] << 8) | mpdu_byte[5]; - int group = (mpdu_byte[18] << 8) | mpdu_byte[19]; - long int freq1 = 0; - long int freq2 = 0; - UNUSED2(source, freq2); - fprintf (stderr, "%s\n ",KYEL); - if (svc & 0x80) fprintf (stderr, " Emergency"); - if (svc & 0x40) fprintf (stderr, " Encrypted"); - - if (opts->payload == 1) //hide behind payload due to len - { - if (svc & 0x20) fprintf (stderr, " Duplex"); - if (svc & 0x10) fprintf (stderr, " Packet"); - else fprintf (stderr, " Circuit"); - if (svc & 0x8) fprintf (stderr, " R"); //reserved bit is on - fprintf (stderr, " Priority %d", svc & 0x7); //call priority - } - fprintf (stderr, " Group Voice Channel Grant Update - Extended"); - fprintf (stderr, "\n SVC [%02X] CHAN-T [%04X] CHAN-R [%04X] Group [%d][%04X]", svc, channelt, channelr, group, group); - freq1 = process_channel_to_freq (opts, state, channelt); - freq2 = process_channel_to_freq (opts, state, channelr); - - //add active channel to string for ncurses display - sprintf (state->active_channel[0], "Active Ch: %04X TG: %d ", channelt, group); - state->last_active_time = time(NULL); - - for (int i = 0; i < state->group_tally; i++) - { - if (state->group_array[i].groupNumber == group) - { - fprintf (stderr, " [%s]", state->group_array[i].groupName); - strcpy (mode, state->group_array[i].groupMode); - break; - } - } - - //TG hold on P25p1 Ext -- block non-matching target, allow matching group - if (state->tg_hold != 0 && state->tg_hold != group) sprintf (mode, "%s", "B"); - if (state->tg_hold != 0 && state->tg_hold == group) sprintf (mode, "%s", "A"); - - //Skip tuning group calls if group calls are disabled - if (opts->trunk_tune_group_calls == 0) goto SKIPCALL; - - //Skip tuning encrypted calls if enc calls are disabled - if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; - - //tune if tuning available - if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) - { - //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot - if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq1 != 0) //if we aren't already on a VC and have a valid frequency - { - - //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK - if (1 == 1) - { - if (state->p25_chan_tdma[channelt >> 12] == 1) - { - state->samplesPerSymbol = 8; - state->symbolCenter = 3; - - //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to - //this will only occur in realtime tuning, not not required .bin or .wav playback - if (channelt & 1) //VCH1 - { - opts->slot1_on = 0; - opts->slot2_on = 1; - } - else //VCH0 - { - opts->slot1_on = 1; - opts->slot2_on = 0; - } - - } - } - //rigctl - if (opts->use_rigctl == 1) - { - if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); - SetFreq(opts->rigctl_sockfd, freq1); - state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; - opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop - state->last_vc_sync_time = time(NULL); - } - //rtl - else if (opts->audio_in_type == 3) - { - #ifdef USE_RTLSDR - rtl_dev_tune (opts, freq1); - state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; - opts->p25_is_tuned = 1; - state->last_vc_sync_time = time(NULL); - #endif - } - } - } - } - - //Unit to Unit Voice Channel Grant - Extended - else if (opcode == 0x6) - { - //I'm not doing EVERY element of this, just enough for tuning! - int svc = mpdu_byte[8]; - int channelt = (mpdu_byte[22] << 8) | mpdu_byte[23]; - int channelr = (mpdu_byte[24] << 8) | mpdu_byte[25]; //optional! - //using source and target address, not source and target id (is this correct?) - long int source = (mpdu_byte[3] << 16) | (mpdu_byte[4] << 8) | mpdu_byte[5]; - long int target = (mpdu_byte[19] << 16) |(mpdu_byte[20] << 8) | mpdu_byte[21]; - //TODO: Test Full Values added here for SUID, particular tgt_nid and tgt_sid - long int src_nid = (mpdu_byte[12] << 24) | (mpdu_byte[13] << 16) | (mpdu_byte[14] << 8) | mpdu_byte[15]; - long int src_sid = (mpdu_byte[16] << 16) | (mpdu_byte[17] << 8) | mpdu_byte[18]; - long int tgt_nid = (mpdu_byte[26] << 16) | (mpdu_byte[27] << 8) | mpdu_byte[28]; //only has 3 octets on tgt nid, partial only? - long int tgt_sid = (mpdu_byte[29] << 16) | (mpdu_byte[30] << 8) | mpdu_byte[31]; - long int freq1 = 0; - long int freq2 = 0; - UNUSED(freq2); - fprintf (stderr, "%s\n ",KYEL); - if (svc & 0x80) fprintf (stderr, " Emergency"); - if (svc & 0x40) fprintf (stderr, " Encrypted"); - - if (opts->payload == 1) //hide behind payload due to len - { - if (svc & 0x20) fprintf (stderr, " Duplex"); - if (svc & 0x10) fprintf (stderr, " Packet"); - else fprintf (stderr, " Circuit"); - if (svc & 0x8) fprintf (stderr, " R"); //reserved bit is on - fprintf (stderr, " Priority %d", svc & 0x7); //call priority - } - fprintf (stderr, " Unit to Unit Voice Channel Grant Update - Extended"); - fprintf (stderr, "\n SVC: %02X; CHAN-T: %04X; CHAN-R: %04X; SRC: %ld; TGT: %ld; FULL SRC: %08lX-%08ld; FULL TGT: %08lX-%08ld;", svc, channelt, channelr, source, target, src_nid, src_sid, tgt_nid, tgt_sid); - freq1 = process_channel_to_freq (opts, state, channelt); - freq2 = process_channel_to_freq (opts, state, channelr); //optional! - - //add active channel to string for ncurses display - sprintf (state->active_channel[0], "Active Ch: %04X TGT: %ld; ", channelt, target); - - for (int i = 0; i < state->group_tally; i++) - { - if (state->group_array[i].groupNumber == target) - { - fprintf (stderr, " [%s]", state->group_array[i].groupName); - strcpy (mode, state->group_array[i].groupMode); - break; - } - } - - //TG hold on P25p1 Ext UU -- will want to disable UU_V grants while TG Hold enabled - if (state->tg_hold != 0 && state->tg_hold != target) sprintf (mode, "%s", "B"); - // if (state->tg_hold != 0 && state->tg_hold == target) sprintf (mode, "%s", "A"); - - //Skip tuning private calls if group calls are disabled - if (opts->trunk_tune_private_calls == 0) goto SKIPCALL; - - //Skip tuning encrypted calls if enc calls are disabled - if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; - - //tune if tuning available - if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) - { - //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot - if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq1 != 0) //if we aren't already on a VC and have a valid frequency - { - - //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK - if (1 == 1) - { - if (state->p25_chan_tdma[channelt >> 12] == 1) - { - state->samplesPerSymbol = 8; - state->symbolCenter = 3; - - //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to - //this will only occur in realtime tuning, not not required .bin or .wav playback - if (channelt & 1) //VCH1 - { - opts->slot1_on = 0; - opts->slot2_on = 1; - } - else //VCH0 - { - opts->slot1_on = 1; - opts->slot2_on = 0; - } - - } - } - //rigctl - if (opts->use_rigctl == 1) - { - if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); - SetFreq(opts->rigctl_sockfd, freq1); - state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; - opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop - state->last_vc_sync_time = time(NULL); - } - //rtl - else if (opts->audio_in_type == 3) - { - #ifdef USE_RTLSDR - rtl_dev_tune (opts, freq1); - state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; - opts->p25_is_tuned = 1; - state->last_vc_sync_time = time(NULL); - #endif - } - } - } - } - - //Telephone Interconnect Voice Channel Grant (or Update) -- Explicit Channel Form - else if ( (opcode == 0x8 || opcode == 0x9) && MFID < 2) //This form does allow for other MFID's but Moto has a seperate function on 9 - { - //TELE_INT_CH_GRANT or TELE_INT_CH_GRANT_UPDT - int svc = mpdu_byte[8]; - int channel = (mpdu_byte[12] << 8) | mpdu_byte[13]; - int timer = (mpdu_byte[16] << 8) | mpdu_byte[17]; - int target = (mpdu_byte[3] << 16) | (mpdu_byte[4] << 8) | mpdu_byte[5]; - long int freq = 0; - fprintf (stderr, "\n"); - if (svc & 0x80) fprintf (stderr, " Emergency"); - if (svc & 0x40) fprintf (stderr, " Encrypted"); - - if (opts->payload == 1) //hide behind payload due to len - { - if (svc & 0x20) fprintf (stderr, " Duplex"); - if (svc & 0x10) fprintf (stderr, " Packet"); - else fprintf (stderr, " Circuit"); - if (svc & 0x8) fprintf (stderr, " R"); //reserved bit is on - fprintf (stderr, " Priority %d", svc & 0x7); //call priority - } - - fprintf (stderr, " Telephone Interconnect Voice Channel Grant"); - if ( opcode & 1) fprintf (stderr, " Update"); - fprintf (stderr, " Extended"); - fprintf (stderr, "\n CHAN: %04X; Timer: %f Seconds; Target: %d;", channel, (float)timer*0.1f, target); //timer unit is 100 ms, or 0.1 seconds - freq = process_channel_to_freq (opts, state, channel); - - //add active channel to string for ncurses display - sprintf (state->active_channel[0], "Active Tele Ch: %04X TGT: %d; ", channel, target); - state->last_active_time = time(NULL); - - //Skip tuning private calls if private calls is disabled (are telephone int calls private, or talkgroup?) - if (opts->trunk_tune_private_calls == 0) goto SKIPCALL; - - //Skip tuning encrypted calls if enc calls are disabled - if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; - - //telephone only has a target address (manual shows combined source/target of 24-bits) - for (int i = 0; i < state->group_tally; i++) - { - if (state->group_array[i].groupNumber == target) - { - fprintf (stderr, " [%s]", state->group_array[i].groupName); - strcpy (mode, state->group_array[i].groupMode); - break; - } - } - - //TG hold on UU_V -- will want to disable UU_V grants while TG Hold enabled - if (state->tg_hold != 0 && state->tg_hold != target) sprintf (mode, "%s", "B"); - - //tune if tuning available - if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) - { - //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot - if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq != 0) //if we aren't already on a VC and have a valid frequency - { - //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK - if (1 == 1) - { - if (state->p25_chan_tdma[channel >> 12] == 1) - { - state->samplesPerSymbol = 8; - state->symbolCenter = 3; - - //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to - //this will only occur in realtime tuning, not not required .bin or .wav playback - if (channel & 1) //VCH1 - { - opts->slot1_on = 0; - opts->slot2_on = 1; - } - else //VCH0 - { - opts->slot1_on = 1; - opts->slot2_on = 0; - } - } - - } - - //rigctl - if (opts->use_rigctl == 1) - { - if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); - SetFreq(opts->rigctl_sockfd, freq); - if (state->synctype == 0 || state->synctype == 1) state->p25_vc_freq[0] = freq; - opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop - state->last_vc_sync_time = time(NULL); - } - //rtl - else if (opts->audio_in_type == 3) - { - #ifdef USE_RTLSDR - rtl_dev_tune (opts, freq); - if (state->synctype == 0 || state->synctype == 1) state->p25_vc_freq[0] = freq; - opts->p25_is_tuned = 1; - state->last_vc_sync_time = time(NULL); - #endif - } - } - } - if (opts->p25_trunk == 0) - { - if (target == state->lasttg || target == state->lasttgR) - { - //P1 FDMA - if (state->synctype == 0 || state->synctype == 1) state->p25_vc_freq[0] = freq; - //P2 TDMA - else state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq; - } - } - - } - - //look at Harris Opcodes and payload portion of MPDU - else if (MFID == 0xA4) - { - //TODO: Add Known Opcodes from Manual (all one of them) - fprintf (stderr, "%s",KCYN); - fprintf (stderr, "\n MFID A4 (Harris); Opcode: %02X; ", opcode); - for (i = 0; i < (12*(blks+1)%37); i++) - fprintf (stderr, "%02X", mpdu_byte[i]); - fprintf (stderr, " %s",KNRM); - } - - //look at Motorola Opcodes and payload portion of MPDU - else if (MFID == 0x90) - { - //TIA-102.AABH - if (opcode == 0x02) - { - int svc = mpdu_byte[8]; //Just the Res, P-bit, and more res bits - int channelt = (mpdu_byte[12] << 8) | mpdu_byte[13]; - int channelr = (mpdu_byte[14] << 8) | mpdu_byte[15]; - long int source = (mpdu_byte[3] << 16) |(mpdu_byte[4] << 8) | mpdu_byte[5]; - int group = (mpdu_byte[16] << 8) | mpdu_byte[17]; - long int freq1 = 0; - long int freq2 = 0; - UNUSED2(source, freq2); - fprintf (stderr, "%s\n ",KYEL); - - if (svc & 0x40) fprintf (stderr, " Encrypted"); //P-bit - - fprintf (stderr, " MFID90 Group Regroup Channel Grant - Explicit"); - fprintf (stderr, "\n RES/P [%02X] CHAN-T [%04X] CHAN-R [%04X] SG [%d][%04X]", svc, channelt, channelr, group, group); - freq1 = process_channel_to_freq (opts, state, channelt); - freq2 = process_channel_to_freq (opts, state, channelr); - - //add active channel to string for ncurses display - sprintf (state->active_channel[0], "MFID90 Ch: %04X SG: %d ", channelt, group); - state->last_active_time = time(NULL); - - for (int i = 0; i < state->group_tally; i++) - { - if (state->group_array[i].groupNumber == group) - { - fprintf (stderr, " [%s]", state->group_array[i].groupName); - strcpy (mode, state->group_array[i].groupMode); - break; - } - } - - //TG hold on MFID90 GRG -- block non-matching target, allow matching group - if (state->tg_hold != 0 && state->tg_hold != group) sprintf (mode, "%s", "B"); - if (state->tg_hold != 0 && state->tg_hold == group) sprintf (mode, "%s", "A"); - - //Skip tuning group calls if group calls are disabled - if (opts->trunk_tune_group_calls == 0) goto SKIPCALL; - - //Skip tuning encrypted calls if enc calls are disabled - if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; - - //tune if tuning available - if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) - { - //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot - if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq1 != 0) //if we aren't already on a VC and have a valid frequency - { - - //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK - if (1 == 1) - { - if (state->p25_chan_tdma[channelt >> 12] == 1) - { - state->samplesPerSymbol = 8; - state->symbolCenter = 3; - - //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to - //this will only occur in realtime tuning, not not required .bin or .wav playback - if (channelt & 1) //VCH1 - { - opts->slot1_on = 0; - opts->slot2_on = 1; - } - else //VCH0 - { - opts->slot1_on = 1; - opts->slot2_on = 0; - } - - } - } - //rigctl - if (opts->use_rigctl == 1) - { - if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); - SetFreq(opts->rigctl_sockfd, freq1); - state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; - opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop - state->last_vc_sync_time = time(NULL); - } - //rtl - else if (opts->audio_in_type == 3) - { - #ifdef USE_RTLSDR - rtl_dev_tune (opts, freq1); - state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; - opts->p25_is_tuned = 1; - state->last_vc_sync_time = time(NULL); - #endif - } - } - } - } - - else - { - fprintf (stderr, "%s",KCYN); - fprintf (stderr, "\n MFID 90 (Moto); Opcode: %02X; ", mpdu_byte[0] & 0x3F); - for (i = 0; i < (12*(blks+1)%37); i++) - fprintf (stderr, "%02X", mpdu_byte[i]); - fprintf (stderr, " %s",KNRM); - } - } - - else - { - fprintf (stderr, "%s",KCYN); - fprintf (stderr, "\n MFID %02X (Unknown); Opcode: %02X; ", MFID, mpdu_byte[0] & 0x3F); - for (i = 0; i < (12*(blks+1)%37); i++) - fprintf (stderr, "%02X", mpdu_byte[i]); - fprintf (stderr, " %s",KNRM); - } - } - - SKIPCALL: ; //do nothing + p25_decode_pdu_trunking(opts, state, mpdu_byte); + // else if (opts->aggressive_framesync == 0 && io == 1 && fmt == 0x17) + // p25_decode_pdu_trunking(opts, state, mpdu_byte); if (opts->payload == 1) { @@ -938,11 +271,12 @@ void processMPDU(dsd_opts * opts, dsd_state * state) fprintf (stderr, " (HDR CRC16 ERR)"); fprintf (stderr, "%s",KCYN); } + //Completed MBF if (err[1] != 0) { fprintf (stderr, "%s",KRED); - fprintf (stderr, " (MBF CRC32 ERR)"); + fprintf (stderr, " (MBT CRC32 ERR)"); fprintf (stderr, "%s",KCYN); } @@ -953,173 +287,116 @@ void processMPDU(dsd_opts * opts, dsd_state * state) } //end trunking block format - else if (r34 == 1 && err[0] == 0) //start 34 rate dump if good header crc + else if (r34 == 1) // && err[0] == 0) //start r34 { + //TODO: Cleanup and make more elegant (maybe just use the crc_bytes on the payload dump) + uint8_t crc_bytes[127*18]; + memset (crc_bytes, 0, sizeof(crc_bytes)); + for (i = 0; i < 16*(blks+1); i++) + crc_bytes[i] = (uint8_t)ConvertBitIntoBytes(&mpdu_crc_bits[i*8], 8); - //ES Auxiliary Header - if (sap == 1) + CRCExtracted = (uint32_t)ConvertBitIntoBytes(&mpdu_crc_bits[(128*blks)-32], 32); + CRCComputed = crc32mbf(crc_bytes, (128*blks)-32); + if (CRCComputed == CRCExtracted) err[1] = 0; + + //reset mpdu_byte to load only the data, and not the dbsn and crc into + memset (mpdu_byte+12, 0, sizeof(mpdu_byte)-12); + int mpdu_idx = 12; + int next = 0; + + //arrays for each block + uint8_t block_ptr = 0; //ptr to current block + uint8_t dbsn[127]; memset(dbsn, 0, sizeof(dbsn)); + uint16_t crc9_ext[127]; memset(crc9_ext, 0, sizeof(crc9_ext)); + uint16_t crc9_cmp[127]; memset(crc9_cmp, 0, sizeof(crc9_cmp)); + + //reconstruct the message + for (i = 2; i <= 18*blks; i++) { - fprintf (stderr, "%s",KYEL); - unsigned long long int mi = (unsigned long long int)ConvertBitIntoBytes(&mpdu_crc_bits[0], 64); - uint8_t mi_res = (uint8_t)ConvertBitIntoBytes(&mpdu_crc_bits[64], 8); - uint8_t alg_id = (uint8_t)ConvertBitIntoBytes(&mpdu_crc_bits[72], 8); - uint16_t key_id = (uint16_t)ConvertBitIntoBytes(&mpdu_crc_bits[80], 16); - fprintf (stderr, "\n ALG: %02X; KEY ID: %04X; MI: %016llX; ", alg_id, key_id, mi); - if (mi_res != 0) - fprintf (stderr, "RES: %02X;", mi_res); + if ( (i != 0) && ((i % 18) == 0)) + { + dbsn[block_ptr] = r34bytes[i-18] >> 1; //get the previous DBSN at this point + crc9_ext[block_ptr] = ((r34bytes[i-18] & 1) << 8) | r34bytes[i-17]; + crc9_cmp[block_ptr] = ComputeCrc9Bit(mpdu_crc9_bits+next, 135); + next += 135; + block_ptr++; + if (i != 18*blks) i += 2; //skip the next DBSN/CRC9 + } + mpdu_byte[mpdu_idx++] = r34bytes[i]; + } - //The Auxiliary Header signals the actual SAP value of the encrypted message (this byte is not encrypted) - uint8_t aux_res = (uint8_t)ConvertBitIntoBytes(&mpdu_crc_bits[96], 2); //these two bits should always be signalled as 1's, so 0b11, and if combined with the 2ndary SAP, 0xC0 if SAP == 0x00 - uint8_t aux_sap = (uint8_t)ConvertBitIntoBytes(&mpdu_crc_bits[98], 6); //the SAP of the message that is encrypted immediately after - char aux_sap_string[99]; - processSAP (aux_sap, aux_sap_string); - fprintf (stderr, "%s",KNRM); - UNUSED(aux_res); - } + //minus 1 to offset the last rounds mpdu_idx++ + if (err[1] == 0 && blks != 0) + p25_decode_pdu_data(opts, state, mpdu_byte, mpdu_idx-1); + else if (opts->aggressive_framesync == 0 && blks != 0) + p25_decode_pdu_data(opts, state, mpdu_byte, mpdu_idx-1); - // if (opts->payload == 1) //always dump these for now since users are actively choosing to tune to these grants + if (opts->payload == 1) { + block_ptr = 0; fprintf (stderr, "%s",KCYN); - fprintf (stderr, "\n P25 MPDU 34 Rate Payload \n "); + fprintf (stderr, "\n P25 MPDU 34 Rate Payload \n "); for (i = 0; i < 12; i++) //header fprintf (stderr, "%02X", mpdu_byte[i]); - fprintf (stderr, " Header \n "); + fprintf (stderr, " Header \n "); - memset (mpdu_byte, 0, sizeof(mpdu_byte)); - for (i = 0; i < 16*(blks+1); i++) //16*10 - mpdu_byte[i] = (uint8_t)ConvertBitIntoBytes(&mpdu_crc_bits[i*8], 8); - - CRCExtracted = (uint32_t)ConvertBitIntoBytes(&mpdu_crc_bits[(128*blks)-32], 32); - CRCComputed = crc32mbf(mpdu_byte, (128*blks)-32); - if (CRCComputed == CRCExtracted) err[1] = 0; - - //reset mpdu_byte to load only the data, and not the dbsn and crc into - memset (mpdu_byte, 0, sizeof(mpdu_byte)); - int mpdu_idx = 0; - int next = 0; - //variable len printing - for (i = 2; i <= 18*blks; i++) //<= only because we want that final DBSN/CRC9 printed out + for (i = 12; i < mpdu_idx; i++) { - if ( (i != 0) && ((i % 18) == 0)) + if ( ((i - 12) != 0) && (((i-12)%16) == 0)) { - uint8_t dbsn = r34bytes[i-18] >> 1; //get the previous DBSN at this point - uint16_t crc9_ext = ((r34bytes[i-18] & 1) << 8) | r34bytes[i-17]; - uint16_t crc9_cmp = ComputeCrc9Bit(mpdu_crc9_bits+next, 135); - next += 135; - if (crc9_ext == crc9_cmp) - fprintf (stderr, " DBSN: %d;", dbsn+1); + if (crc9_ext[block_ptr] == crc9_cmp[block_ptr]) + fprintf (stderr, " DBSN: %d;", dbsn[block_ptr]+1); else { fprintf (stderr, "%s",KRED); fprintf (stderr, " CRC ERR;"); fprintf (stderr, "%s",KCYN); - // fprintf (stderr, " EXT: %03X; CMP: %03X", crc9_ext, crc9_cmp); + fprintf (stderr, " EXT: %03X; CMP: %03X", crc9_ext[block_ptr], crc9_cmp[block_ptr]); } - if (i != 18*blks) i += 2; //skip the next DBSN/CRC9 - if (i != 18*blks) fprintf (stderr, "\n "); + if (i != (mpdu_idx-1)) + fprintf (stderr, "\n "); + block_ptr++; } - if (i != 18*blks) fprintf (stderr, "%02X", r34bytes[i]); - mpdu_byte[mpdu_idx++] = r34bytes[i]; + if (i != (mpdu_idx-1)) + fprintf (stderr, "%02X", mpdu_byte[i]); } if (err[1] != 0) { fprintf (stderr, "%s",KRED); - fprintf (stderr, "\n (MPDU CRC32 ERR)"); + fprintf (stderr, "\n (MPDU CRC32 ERR)"); fprintf (stderr, "%s",KCYN); fprintf (stderr, " CRC EXT %08X CMP %08X", CRCExtracted, CRCComputed); } - // else - // TODO: decode PDU function - - #define P25_ASCII_TEST34 - #ifdef P25_ASCII_TEST34 - // if (sap == 0) //user data - { - fprintf (stderr, "%s",KCYN); - fprintf (stderr, "\n P25 MPDU ASCII: "); - start = 0; //offset for any sort of aux headers - if (sap == 1) start = 13; - for (i = start; i < mpdu_idx-pad-4; i++) - { - if (mpdu_byte[i] <= 0x7E && mpdu_byte[i] >=0x20) - fprintf (stderr, "%c", mpdu_byte[i]); - else fprintf (stderr, " "); - } - } - #endif - - // #define P25_UTF16_TEST - #ifdef P25_UTF16_TEST - //utf-16 text messaging (debug) - // if (utf-16) - { - fprintf (stderr, "%s", KCYN); - fprintf (stderr, "\n P25 MPDU UTF16: "); - uint16_t ch16 = 0; - start = 0; //offset for any sort of aux headers - if (sap == 1) start = 13; - for (i = start; i < mpdu_idx-pad-4;) - { - ch16 = (uint16_t)mpdu_byte[i+0]; - ch16 <<= 8; - ch16 |= (uint16_t)mpdu_byte[i+1]; - // fprintf (stderr, " %04X; ", ch16); //debug for raw values to check grouping for offset - - if (ch16 >= 0x20) //if not a linebreak or terminal commmands - fprintf (stderr, "%lc", ch16); - else if (ch16 == 0) //if padding - fprintf (stderr, "_"); - else fprintf (stderr, " "); - - i += 2; - } - } - #endif - - fprintf (stderr, "%s ", KNRM); - fprintf (stderr, "\n"); } - } //end r34 - else if (err[0] == 0) //12 rate unconfirmed data - { + fprintf (stderr, "%s ", KNRM); + fprintf (stderr, "\n"); + + } //end r34 + + else if (r34 == 0) //12 rate unconfirmed data //err[0] == 0 + { + int len = 12*(blks+1); if (blks != 0) { - CRCExtracted = (mpdu_byte[(12*(blks+1))-4] << 24) | (mpdu_byte[(12*(blks+1))-3] << 16) | (mpdu_byte[(12*(blks+1))-2] << 8) | (mpdu_byte[(12*(blks+1))-1] << 0); + CRCExtracted = (mpdu_byte[len-4] << 24) | (mpdu_byte[len-3] << 16) | (mpdu_byte[len-2] << 8) | (mpdu_byte[len-1] << 0); CRCComputed = crc32mbf(mpdu_byte+12, (96*blks)-32); if (CRCComputed == CRCExtracted) err[1] = 0; } else err[1] = 0; //No CRC32 on a lonely header - //ES Auxiliary Header (need to find samples to confirm presence and correctness on 1/2 rate data) - if (sap == 1 && blks != 0) //may not need blocks check, since if this is present, multiple blocks will be required - { - fprintf (stderr, "%s",KYEL); - unsigned long long int mi = ((unsigned long long int)mpdu_byte[12] << 56ULL) | ((unsigned long long int)mpdu_byte[13] << 48ULL) | ((unsigned long long int)mpdu_byte[14] << 40ULL) | ((unsigned long long int)mpdu_byte[15] << 32ULL) | - ((unsigned long long int)mpdu_byte[16] << 24ULL) | ((unsigned long long int)mpdu_byte[17] << 16ULL) | ((unsigned long long int)mpdu_byte[18] << 8ULL) | ((unsigned long long int)mpdu_byte[19] << 0ULL); - uint8_t mi_res = mpdu_byte[20]; - uint8_t alg_id = mpdu_byte[21]; - uint16_t key_id = (mpdu_byte[22] << 8) | mpdu_byte[23]; - fprintf (stderr, "\n ALG: %02X; KEY ID: %04X; MI: %016llX; ", alg_id, key_id, mi); - if (mi_res != 0) - fprintf (stderr, "RES: %02X;", mi_res); + if (err[1] == 0 && blks != 0) + p25_decode_pdu_data(opts, state, mpdu_byte, len); + else if (opts->aggressive_framesync == 0 && blks != 0) + p25_decode_pdu_data(opts, state, mpdu_byte, len); - //The Auxiliary Header signals the actual SAP value of the encrypted message (this byte is not encrypted) - uint8_t aux_res = mpdu_byte[24] >> 6; //these two bits should always be signalled as 1's, so 0b11, and if combined with the 2ndary SAP, 0xC0 if SAP == 0x00 - uint8_t aux_sap = mpdu_byte[24] & 0x3F; //the SAP of the message that is encrypted immediately after - char aux_sap_string[99]; - processSAP (aux_sap, aux_sap_string); - fprintf (stderr, "%s",KNRM); - UNUSED(aux_res); - } - - // if (opts->payload == 1) + if (opts->payload == 1) { fprintf (stderr, "%s",KCYN); fprintf (stderr, "\n P25 MPDU 12 Rate Payload: \n "); - for (i = 0; i < 12*(blks+1); i++) //header and payload combined + for (i = 0; i < len; i++) //header and payload combined { if (i == 12) fprintf (stderr, " Header"); if ( (i != 0) && ((i % 12) == 0)) @@ -1131,57 +408,10 @@ void processMPDU(dsd_opts * opts, dsd_state * state) if (err[1] != 0) { fprintf (stderr, "%s",KRED); - fprintf (stderr, "\n (MPDU CRC32 ERR)"); + fprintf (stderr, "\n (MPDU CRC32 ERR)"); fprintf (stderr, "%s",KCYN); fprintf (stderr, " CRC EXT %08X CMP %08X", CRCExtracted, CRCComputed); } - // else if (blks != 0) - // TODO: decode PDU function - - #define P25_ASCII_TEST12 - #ifdef P25_ASCII_TEST12 - // if (opts->payload == 1) - { - fprintf (stderr, "%s",KCYN); - fprintf (stderr, "\n P25 MPDU ASCII: "); - start = 0; //offset for any sort of aux headers - if (sap == 1) start = 13; - for (i = start; i < (12*(blks+1))-4-pad; i++) - { - if (mpdu_byte[i] <= 0x7E && mpdu_byte[i] >=0x20) - fprintf (stderr, "%c", mpdu_byte[i]); - else fprintf (stderr, " "); - } - } - #endif - - // #define P25_UTF16_TEST12 - #ifdef P25_UTF16_TEST12 - //utf-16 text messaging (debug) - // if (utf-16) - { - fprintf (stderr, "%s", KCYN); - fprintf (stderr, "\n P25 MPDU UTF16: "); - uint16_t ch16 = 0; - start = 0; //offset for any sort of aux headers - if (sap == 1) start = 13; - for (i = start; i < (12*blks)-4-pad;) - { - ch16 = (uint16_t)mpdu_byte[i+0]; - ch16 <<= 8; - ch16 |= (uint16_t)mpdu_byte[i+1]; - // fprintf (stderr, " %04X; ", ch16); //debug for raw values to check grouping for offset - - if (ch16 >= 0x20) //if not a linebreak or terminal commmands - fprintf (stderr, "%lc", ch16); - else if (ch16 == 0) //if padding - fprintf (stderr, "_"); - else fprintf (stderr, " "); - - i += 2; - } - } - #endif fprintf (stderr, "%s",KNRM); fprintf (stderr, "\n"); diff --git a/src/p25p1_pdu_data.c b/src/p25p1_pdu_data.c new file mode 100644 index 0000000..49fbd19 --- /dev/null +++ b/src/p25p1_pdu_data.c @@ -0,0 +1,272 @@ +/*------------------------------------------------------------------------------- + * p25p1_pdu_data.c + * P25p1 PDU Data Decoding + * + * LWVMOBILE + * 2025-03 DSD-FME Florida Man Edition + *-----------------------------------------------------------------------------*/ + +#include "dsd.h" + +void p25_decode_rsp(uint8_t C, uint8_t T, uint8_t S, char * rsp_string) +{ + + if (C == 0) sprintf (rsp_string, " ACK (Success);"); + else if (C == 2) sprintf (rsp_string, " SACK (Retry);"); + else if (C == 1) + { + if (T == 0) sprintf (rsp_string, " NACK (Illegal Format);"); + else if (T == 1) sprintf (rsp_string, " NACK (CRC32 Failure);"); + else if (T == 2) sprintf (rsp_string, " NACK (Memory Full);"); + else if (T == 3) sprintf (rsp_string, " NACK (FSN Sequence Error);"); + else if (T == 4) sprintf (rsp_string, " NACK (Undeliverable);"); + else if (T == 5) sprintf (rsp_string, " NACK (NS/VR Sequence Error);"); //depreciated + else if (T == 6) sprintf (rsp_string, " NACK (Invalid User on System);"); + } + + fprintf (stderr, " Response Packet:%s C: %X; T: %X; S: %X; ", rsp_string, C, T, S); + +} + +void p25_decode_sap(uint8_t SAP, char * sap_string) +{ + + if (SAP == 0) sprintf (sap_string, " User Data;"); + else if (SAP == 1) sprintf (sap_string, " Encrypted User Data;"); + else if (SAP == 2) sprintf (sap_string, " Circuit Data;"); + else if (SAP == 3) sprintf (sap_string, " Circuit Data Control;"); + else if (SAP == 4) sprintf (sap_string, " Packet Data;"); + else if (SAP == 5) sprintf (sap_string, " Address Resolution Protocol;"); + else if (SAP == 6) sprintf (sap_string, " SNDCP Packet Data Control;"); + else if (SAP == 15) sprintf (sap_string, " Packet Data Scan Preamble;"); + else if (SAP == 29) sprintf (sap_string, " Packet Data Encryption Support;"); + else if (SAP == 31) sprintf (sap_string, " Extended Address;"); + else if (SAP == 32) sprintf (sap_string, " Registration and Authorization;"); + else if (SAP == 33) sprintf (sap_string, " Channel Reassignment;"); + else if (SAP == 34) sprintf (sap_string, " System Configuration;"); + else if (SAP == 35) sprintf (sap_string, " Mobile Radio Loopback;"); + else if (SAP == 36) sprintf (sap_string, " Mobile Radio Statistics;"); + else if (SAP == 37) sprintf (sap_string, " Mobile Radio Out of Service;"); + else if (SAP == 38) sprintf (sap_string, " Mobile Radio Paging;"); + else if (SAP == 39) sprintf (sap_string, " Mobile Radio Configuration;"); + else if (SAP == 40) sprintf (sap_string, " Unencrypted Key Management;"); + else if (SAP == 41) sprintf (sap_string, " Encrypted Key Management;"); + else if (SAP == 48) sprintf (sap_string, " Location Service;"); + else if (SAP == 61) sprintf (sap_string, " Trunking Control;"); + else if (SAP == 63) sprintf (sap_string, " Encrypted Trunking Control;"); + + //catch all for everything else + else sprintf (sap_string, " Unknown SAP;"); + + fprintf (stderr, "SAP: 0x%02X;%s ", SAP, sap_string); + +} + + +uint8_t p25_decrypt_pdu(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t alg_id, uint16_t key_id, unsigned long long int mi, int len) +{ + + uint8_t encrypted = 1; + + UNUSED(opts); + UNUSED(state); + UNUSED(input); + UNUSED(mi); + UNUSED(key_id); + UNUSED(alg_id); + UNUSED(len); + + + return encrypted; +} + +//SAP 1 +uint8_t p25_decode_es_header(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t * sap, int * ptr, int len) +{ + + uint8_t encrypted = 0; + + uint8_t bits[13*8]; memset (bits, 0, sizeof(bits)); + unpack_byte_array_into_bit_array(input, bits, 13); + + fprintf (stderr, "%s",KYEL); + unsigned long long int mi = (unsigned long long int)ConvertBitIntoBytes(bits, 64); + uint8_t mi_res = (uint8_t)ConvertBitIntoBytes(bits+64, 8); + uint8_t alg_id = (uint8_t)ConvertBitIntoBytes(bits+72, 8); + uint16_t key_id = (uint16_t)ConvertBitIntoBytes(bits+80, 16); + fprintf (stderr, "\n ES Aux Encryption Header; ALG: %02X; KEY ID: %04X; MI: %016llX; ", alg_id, key_id, mi); + if (mi_res != 0) + fprintf (stderr, " RES: %02X;", mi_res); + + //The Auxiliary Header signals the actual SAP value of the encrypted message (this byte is not encrypted) + uint8_t aux_res = (uint8_t)ConvertBitIntoBytes(&bits[96], 2); //these two bits should always be signalled as 1's, so 0b11, and if combined with the 2ndary SAP, 0xC0 if SAP == 0x00 + uint8_t aux_sap = (uint8_t)ConvertBitIntoBytes(&bits[98], 6); //the SAP of the message that is encrypted immediately after + char aux_sap_string[99]; + p25_decode_sap (aux_sap, aux_sap_string); + fprintf (stderr, "%s",KNRM); + UNUSED(aux_res); + + //Decrypt PDU + if (alg_id != 0x80) + encrypted = p25_decrypt_pdu(opts, state, input+13, alg_id, key_id, mi, len-13); + + *sap = aux_sap; + *ptr += 13; + + return encrypted; + +} + +//alternate configuration for this +uint8_t p25_decode_es_header_2(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t * sap, int * ptr, int len) +{ + + uint8_t encrypted = 0; + + uint8_t bits[13*8]; memset (bits, 0, sizeof(bits)); + unpack_byte_array_into_bit_array(input, bits, 13); + + fprintf (stderr, "%s",KYEL); + unsigned long long int mi = (unsigned long long int)ConvertBitIntoBytes(bits+24, 64); + uint8_t mi_res = (uint8_t)ConvertBitIntoBytes(bits+64+24, 8); + uint8_t alg_id = (uint8_t)ConvertBitIntoBytes(bits+16, 8); + uint16_t key_id = (uint16_t)ConvertBitIntoBytes(bits, 16); + fprintf (stderr, "\n ES Aux Encryption Header 2; ALG: %02X; KEY ID: %04X; MI: %016llX; ", alg_id, key_id, mi); + if (mi_res != 0) + fprintf (stderr, " RES: %02X;", mi_res); + + //The Auxiliary Header signals the actual SAP value of the encrypted message (this byte is not encrypted) + uint8_t aux_res = (uint8_t)ConvertBitIntoBytes(&bits[96], 2); //these two bits should always be signalled as 1's, so 0b11, and if combined with the 2ndary SAP, 0xC0 if SAP == 0x00 + uint8_t aux_sap = (uint8_t)ConvertBitIntoBytes(&bits[98], 6); //the SAP of the message that is encrypted immediately after + char aux_sap_string[99]; + p25_decode_sap (aux_sap, aux_sap_string); + fprintf (stderr, "%s",KNRM); + UNUSED(aux_res); + + if (alg_id != 0x80) + encrypted = 1; + + //Decrypt PDU + encrypted = p25_decrypt_pdu(opts, state, input+13, alg_id, key_id, mi, len-13); + + *sap = aux_sap; + *ptr += 13; + + return encrypted; + +} + +//SAP 31 //Extended Addressing +void p25_decode_extended_address(dsd_opts * opts, dsd_state * state, uint8_t * input, uint8_t * sap, int * ptr) +{ + + UNUSED(opts); + + uint8_t bits[12*8]; memset (bits, 0, sizeof(bits)); + unpack_byte_array_into_bit_array(input, bits, 12); + + uint8_t ea_sap = (uint8_t)ConvertBitIntoBytes(bits+10, 6); + uint8_t ea_mfid = (uint8_t)ConvertBitIntoBytes(bits+16, 6); + uint32_t ea_llid = (uint32_t)ConvertBitIntoBytes(bits+24, 24); + uint32_t ea_res = (uint32_t)ConvertBitIntoBytes(bits+48, 32); + uint16_t ea_crc = (uint16_t)ConvertBitIntoBytes(bits+80, 16); + + fprintf (stderr, "\n Extended Addressing Header; MFID: %02X; SRC LLID: %d; RES: %08X; CRC: %04X; ", ea_mfid, ea_llid, ea_res, ea_crc); + char ea_sap_string[99]; + p25_decode_sap (ea_sap, ea_sap_string); + UNUSED(ea_sap_string); + + //Print to Data Call String for Ncurses Terminal + sprintf (state->dmr_lrrp_gps[0], "Data Call:%s SAP:%02X; LLID: %d; ", ea_sap_string, ea_sap, ea_llid); + + *sap = ea_sap; + *ptr += 12; + +} + +//PDU Format Header Decode +void p25_decode_pdu_header(dsd_opts * opts, dsd_state * state, uint8_t * input) +{ + + UNUSED(opts); + + uint8_t an = (input[0] >> 6) & 0x1; + uint8_t io = (input[0] >> 5) & 0x1; + uint8_t fmt = input[0] & 0x1F; + uint8_t sap = input[1] & 0x3F; + uint8_t MFID = input[2]; + uint32_t address = (input[3] << 16) | (input[4] << 8) | input[5]; + uint8_t blks = input[6] & 0x7F; + + uint8_t fmf = (input[6] >> 7) & 0x1; + uint8_t pad = input[7] & 0x1F; + uint8_t ns = (input[8] >> 4) & 0x7; + uint8_t fsnf = input[8] & 0xF; + uint8_t offset = input[9] & 0x3F; + + //response packet + uint8_t class = (input[1] >> 6) & 0x3; + uint8_t type = (input[1] >> 3) & 0x7; + uint8_t status = (input[1] >> 0) & 0x7; + + fprintf (stderr, "%s",KGRN); + fprintf (stderr, " P25 Data - AN: %d; IO: %d; FMT: %02X; ", an, io, fmt); + char sap_string[40]; + char rsp_string[40]; + if (fmt != 3) p25_decode_sap (sap, sap_string); //decode SAP to see what kind of data we are dealing with + else p25_decode_rsp (class, type, status, rsp_string); //decode the response type (ack, nack, sack) + if (sap != 61 && sap != 63) //Not too interested in viewing these on trunking control, just data packets mostly + fprintf (stderr, "\n F: %d; Blocks: %02X; Pad: %d; NS: %d; FSNF: %d; Offset: %d; MFID: %02X;", fmf, blks, pad, ns, fsnf, offset, MFID); + if (io == 1 && sap != 61 && sap != 63) //destination address if IO bit set + fprintf (stderr, " DST LLID: %d;", address); + else if (io == 0 && sap != 61 && sap != 63) //Source address if IO bit not set + fprintf (stderr, " SRC LLID: %d;", address); + //Print to Data Call String for Ncurses Terminal + if (sap != 61 && sap != 63 && fmt != 3) + sprintf (state->dmr_lrrp_gps[0], "Data Call:%s SAP:%02X; LLID: %d; ", sap_string, sap, address); +} + +//user or other data delivered via PDU format +void p25_decode_pdu_data(dsd_opts * opts, dsd_state * state, uint8_t * input, int len) +{ + + uint8_t sap = input[1] & 0x3F; + uint8_t pad = input[7] & 0x1F; + uint8_t offset = input[9] & 0x3F; UNUSED(offset); //determine the best way to use this + uint8_t encrypted = 0; + int ptr = 12; //initial ptr index value past the first header + + //may need a sanity check on this value to make sure it doesn't go negative + if (len > (12 + 4 + pad)) + len -= (12 + 4 + pad); //substract the header, crc, and padding bytes from total len value + + //debug + fprintf (stderr, " PDU Len: %d;", len); + + //check for any additional headers first + if (sap == 1) //encryption sync header + encrypted = p25_decode_es_header(opts, state, input+ptr, &sap, &ptr, len); + + if (!encrypted) + { + //additional header (will be encrypted, so check above first) + if (sap == 31) //extended address header + p25_decode_extended_address(opts, state, input+ptr, &sap, &ptr); + + //test if an offset value set, then take the difference between it and the ptr and and add that to the ptr + //or perhaps, just assign the ptr to that value + 12? + if (offset) ptr = 12 + offset; + + //now start checking for the actual message + if (sap == 0 || sap == 4) //User Data or Packet Data (both are UDP typically, same format dmr UDP/IP data) + dmr_ip_pdu (opts, state, len, input+ptr); //need to work on gettting that offset value to this point +2 on the 64414 + + if (sap == 48) //Tier 1 Location Service (or does it depend on the io bit?) + utf8_to_text(state, 0, len-ptr, input+ptr); //TODO, read initial string, i.e., $GPRMC and properly decode + } + else + { + fprintf (stderr, "Encrypted PDU;"); + } + +} \ No newline at end of file diff --git a/src/p25p1_pdu_trunking.c b/src/p25p1_pdu_trunking.c new file mode 100644 index 0000000..5889cc5 --- /dev/null +++ b/src/p25p1_pdu_trunking.c @@ -0,0 +1,579 @@ +/*------------------------------------------------------------------------------- + * p25p1_pdu_trunking.c + * P25p1 PDU Alt Format Trunking + * + * LWVMOBILE + * 2025-03 DSD-FME Florida Man Edition + *-----------------------------------------------------------------------------*/ + +#include "dsd.h" + +//trunking data delivered via PDU format +void p25_decode_pdu_trunking(dsd_opts * opts, dsd_state * state, uint8_t * mpdu_byte) +{ + //group list mode so we can look and see if we need to block tuning any groups, etc + char mode[8]; //allow, block, digital, enc, etc + sprintf (mode, "%s", ""); + + //if we are using allow/whitelist mode, then write 'B' to mode for block + //comparison below will look for an 'A' to write to mode if it is allowed + if (opts->trunk_use_allow_list == 1) sprintf (mode, "%s", "B"); + + uint8_t fmt = mpdu_byte[0] & 0x1F; + uint8_t MFID = mpdu_byte[2]; + int blks = mpdu_byte[6] & 0x7F; + uint8_t opcode = 0; + + if (fmt == 0x15) fprintf (stderr, " UNC"); + else fprintf (stderr, " ALT"); + fprintf (stderr, " MBT"); + if (fmt == 0x17) opcode = mpdu_byte[7] & 0x3F; //alt MBT + else opcode = mpdu_byte[12] & 0x3F; //unconf MBT + fprintf (stderr, " - OP: %02X", opcode); + + //NET_STS_BCST -- TIA-102.AABC-D 6.2.11.2 + if (opcode == 0x3B) + { + int lra = mpdu_byte[3]; + int sysid = ((mpdu_byte[4] & 0xF) << 8) | mpdu_byte[5]; + int res_a = mpdu_byte[8]; + int res_b = mpdu_byte[9]; + long int wacn = (mpdu_byte[12] << 12) | (mpdu_byte[13] << 4) | (mpdu_byte[14] >> 4); + int channelt = (mpdu_byte[15] << 8) | mpdu_byte[16]; + int channelr = (mpdu_byte[17] << 8) | mpdu_byte[18]; + int ssc = mpdu_byte[19]; + UNUSED3(res_a, res_b, ssc); + fprintf (stderr, "%s",KYEL); + fprintf (stderr, "\n Network Status Broadcast MBT - Extended \n"); + fprintf (stderr, " LRA [%02X] WACN [%05lX] SYSID [%03X] NAC [%03llX]\n", lra, wacn, sysid, state->p2_cc); + fprintf (stderr, " CHAN-T [%04X] CHAN-R [%04X]", channelt, channelr); + long int ct_freq = process_channel_to_freq(opts, state, channelt); + long int cr_freq = process_channel_to_freq(opts, state, channelr); + UNUSED(cr_freq); + + state->p25_cc_freq = ct_freq; + state->p25_cc_is_tdma = 0; //flag off for CC tuning purposes when system is qpsk + + //place the cc freq into the list at index 0 if 0 is empty, or not the same, + //so we can hunt for rotating CCs without user LCN list + if (state->trunk_lcn_freq[0] == 0 || state->trunk_lcn_freq[0] != state->p25_cc_freq) + { + state->trunk_lcn_freq[0] = state->p25_cc_freq; + } + + //only set IF these values aren't already hard set by the user + if (state->p2_hardset == 0) + { + state->p2_wacn = wacn; + state->p2_sysid = sysid; + } + } + //RFSS Status Broadcast - Extended 6.2.15.2 + else if (opcode == 0x3A) + { + int lra = mpdu_byte[3]; + int lsysid = ((mpdu_byte[4] & 0xF) << 8) | mpdu_byte[5]; + int rfssid = mpdu_byte[12]; + int siteid = mpdu_byte[13]; + int channelt = (mpdu_byte[14] << 8) | mpdu_byte[15]; + int channelr = (mpdu_byte[16] << 8) | mpdu_byte[17]; + int sysclass = mpdu_byte[18]; + fprintf (stderr, "%s",KYEL); + fprintf (stderr, "\n RFSS Status Broadcast MBF - Extended \n"); + fprintf (stderr, " LRA [%02X] SYSID [%03X] RFSS ID [%03d] SITE ID [%03d]\n CHAN-T [%04X] CHAN-R [%02X] SSC [%02X] ", lra, lsysid, rfssid, siteid, channelt, channelr, sysclass); + process_channel_to_freq (opts, state, channelt); + process_channel_to_freq (opts, state, channelr); + + state->p2_siteid = siteid; + state->p2_rfssid = rfssid; + } + + //Adjacent Status Broadcast (ADJ_STS_BCST) Extended 6.2.2.2 + else if (opcode == 0x3C) + { + int lra = mpdu_byte[3]; + int cfva = mpdu_byte[4] >> 4; + int lsysid = ((mpdu_byte[4] & 0xF) << 8) | mpdu_byte[5]; + int rfssid = mpdu_byte[8]; + int siteid = mpdu_byte[9]; + int channelt = (mpdu_byte[12] << 8) | mpdu_byte[13]; + int channelr = (mpdu_byte[14] << 8) | mpdu_byte[15]; + int sysclass = mpdu_byte[16]; + long int wacn = (mpdu_byte[17] << 12) | (mpdu_byte[18] << 4) | (mpdu_byte[19] >> 4); + fprintf (stderr, "%s",KYEL); + fprintf (stderr, "\n Adjacent Status Broadcast - Extended\n"); + fprintf (stderr, " LRA [%02X] CFVA [%X] RFSS[%03d] SITE [%03d] SYSID [%03X]\n CHAN-T [%04X] CHAN-R [%04X] SSC [%02X] WACN [%05lX]\n ", lra, cfva, rfssid, siteid, lsysid, channelt, channelr, sysclass, wacn); + if (cfva & 0x8) fprintf (stderr, " Conventional"); + if (cfva & 0x4) fprintf (stderr, " Failure Condition"); + if (cfva & 0x2) fprintf (stderr, " Up to Date (Correct)"); + else fprintf (stderr, " Last Known"); + if (cfva & 0x1) fprintf (stderr, " Valid RFSS Connection Active"); + process_channel_to_freq (opts, state, channelt); + process_channel_to_freq (opts, state, channelr); + + } + + //Group Voice Channel Grant - Extended + else if (opcode == 0x0) + { + int svc = mpdu_byte[8]; + int channelt = (mpdu_byte[14] << 8) | mpdu_byte[15]; + int channelr = (mpdu_byte[16] << 8) | mpdu_byte[17]; + long int source = (mpdu_byte[3] << 16) |(mpdu_byte[4] << 8) | mpdu_byte[5]; + int group = (mpdu_byte[18] << 8) | mpdu_byte[19]; + long int freq1 = 0; + long int freq2 = 0; + UNUSED2(source, freq2); + fprintf (stderr, "%s\n ",KYEL); + if (svc & 0x80) fprintf (stderr, " Emergency"); + if (svc & 0x40) fprintf (stderr, " Encrypted"); + + if (opts->payload == 1) //hide behind payload due to len + { + if (svc & 0x20) fprintf (stderr, " Duplex"); + if (svc & 0x10) fprintf (stderr, " Packet"); + else fprintf (stderr, " Circuit"); + if (svc & 0x8) fprintf (stderr, " R"); //reserved bit is on + fprintf (stderr, " Priority %d", svc & 0x7); //call priority + } + fprintf (stderr, " Group Voice Channel Grant Update - Extended"); + fprintf (stderr, "\n SVC [%02X] CHAN-T [%04X] CHAN-R [%04X] Group [%d][%04X]", svc, channelt, channelr, group, group); + freq1 = process_channel_to_freq (opts, state, channelt); + freq2 = process_channel_to_freq (opts, state, channelr); + + //add active channel to string for ncurses display + sprintf (state->active_channel[0], "Active Ch: %04X TG: %d ", channelt, group); + state->last_active_time = time(NULL); + + for (int i = 0; i < state->group_tally; i++) + { + if (state->group_array[i].groupNumber == group) + { + fprintf (stderr, " [%s]", state->group_array[i].groupName); + strcpy (mode, state->group_array[i].groupMode); + break; + } + } + + //TG hold on P25p1 Ext -- block non-matching target, allow matching group + if (state->tg_hold != 0 && state->tg_hold != group) sprintf (mode, "%s", "B"); + if (state->tg_hold != 0 && state->tg_hold == group) sprintf (mode, "%s", "A"); + + //Skip tuning group calls if group calls are disabled + if (opts->trunk_tune_group_calls == 0) goto SKIPCALL; + + //Skip tuning encrypted calls if enc calls are disabled + if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; + + //tune if tuning available + if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) + { + //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot + if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq1 != 0) //if we aren't already on a VC and have a valid frequency + { + + //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK + if (1 == 1) + { + if (state->p25_chan_tdma[channelt >> 12] == 1) + { + state->samplesPerSymbol = 8; + state->symbolCenter = 3; + + //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to + //this will only occur in realtime tuning, not not required .bin or .wav playback + if (channelt & 1) //VCH1 + { + opts->slot1_on = 0; + opts->slot2_on = 1; + } + else //VCH0 + { + opts->slot1_on = 1; + opts->slot2_on = 0; + } + + } + } + //rigctl + if (opts->use_rigctl == 1) + { + if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); + SetFreq(opts->rigctl_sockfd, freq1); + state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; + opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop + state->last_vc_sync_time = time(NULL); + } + //rtl + else if (opts->audio_in_type == 3) + { + #ifdef USE_RTLSDR + rtl_dev_tune (opts, freq1); + state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; + opts->p25_is_tuned = 1; + state->last_vc_sync_time = time(NULL); + #endif + } + } + } + } + + //Unit to Unit Voice Channel Grant - Extended + else if (opcode == 0x6) + { + //I'm not doing EVERY element of this, just enough for tuning! + int svc = mpdu_byte[8]; + int channelt = (mpdu_byte[22] << 8) | mpdu_byte[23]; + int channelr = (mpdu_byte[24] << 8) | mpdu_byte[25]; //optional! + //using source and target address, not source and target id (is this correct?) + long int source = (mpdu_byte[3] << 16) | (mpdu_byte[4] << 8) | mpdu_byte[5]; + long int target = (mpdu_byte[19] << 16) |(mpdu_byte[20] << 8) | mpdu_byte[21]; + //TODO: Test Full Values added here for SUID, particular tgt_nid and tgt_sid + long int src_nid = (mpdu_byte[12] << 24) | (mpdu_byte[13] << 16) | (mpdu_byte[14] << 8) | mpdu_byte[15]; + long int src_sid = (mpdu_byte[16] << 16) | (mpdu_byte[17] << 8) | mpdu_byte[18]; + long int tgt_nid = (mpdu_byte[26] << 16) | (mpdu_byte[27] << 8) | mpdu_byte[28]; //only has 3 octets on tgt nid, partial only? + long int tgt_sid = (mpdu_byte[29] << 16) | (mpdu_byte[30] << 8) | mpdu_byte[31]; + long int freq1 = 0; + long int freq2 = 0; + UNUSED(freq2); + fprintf (stderr, "%s\n ",KYEL); + if (svc & 0x80) fprintf (stderr, " Emergency"); + if (svc & 0x40) fprintf (stderr, " Encrypted"); + + if (opts->payload == 1) //hide behind payload due to len + { + if (svc & 0x20) fprintf (stderr, " Duplex"); + if (svc & 0x10) fprintf (stderr, " Packet"); + else fprintf (stderr, " Circuit"); + if (svc & 0x8) fprintf (stderr, " R"); //reserved bit is on + fprintf (stderr, " Priority %d", svc & 0x7); //call priority + } + fprintf (stderr, " Unit to Unit Voice Channel Grant Update - Extended"); + fprintf (stderr, "\n SVC: %02X; CHAN-T: %04X; CHAN-R: %04X; SRC: %ld; TGT: %ld; FULL SRC: %08lX-%08ld; FULL TGT: %08lX-%08ld;", svc, channelt, channelr, source, target, src_nid, src_sid, tgt_nid, tgt_sid); + freq1 = process_channel_to_freq (opts, state, channelt); + freq2 = process_channel_to_freq (opts, state, channelr); //optional! + + //add active channel to string for ncurses display + sprintf (state->active_channel[0], "Active Ch: %04X TGT: %ld; ", channelt, target); + + for (int i = 0; i < state->group_tally; i++) + { + if (state->group_array[i].groupNumber == target) + { + fprintf (stderr, " [%s]", state->group_array[i].groupName); + strcpy (mode, state->group_array[i].groupMode); + break; + } + } + + //TG hold on P25p1 Ext UU -- will want to disable UU_V grants while TG Hold enabled + if (state->tg_hold != 0 && state->tg_hold != target) sprintf (mode, "%s", "B"); + // if (state->tg_hold != 0 && state->tg_hold == target) sprintf (mode, "%s", "A"); + + //Skip tuning private calls if group calls are disabled + if (opts->trunk_tune_private_calls == 0) goto SKIPCALL; + + //Skip tuning encrypted calls if enc calls are disabled + if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; + + //tune if tuning available + if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) + { + //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot + if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq1 != 0) //if we aren't already on a VC and have a valid frequency + { + + //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK + if (1 == 1) + { + if (state->p25_chan_tdma[channelt >> 12] == 1) + { + state->samplesPerSymbol = 8; + state->symbolCenter = 3; + + //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to + //this will only occur in realtime tuning, not not required .bin or .wav playback + if (channelt & 1) //VCH1 + { + opts->slot1_on = 0; + opts->slot2_on = 1; + } + else //VCH0 + { + opts->slot1_on = 1; + opts->slot2_on = 0; + } + + } + } + //rigctl + if (opts->use_rigctl == 1) + { + if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); + SetFreq(opts->rigctl_sockfd, freq1); + state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; + opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop + state->last_vc_sync_time = time(NULL); + } + //rtl + else if (opts->audio_in_type == 3) + { + #ifdef USE_RTLSDR + rtl_dev_tune (opts, freq1); + state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; + opts->p25_is_tuned = 1; + state->last_vc_sync_time = time(NULL); + #endif + } + } + } + } + + //Telephone Interconnect Voice Channel Grant (or Update) -- Explicit Channel Form + else if ( (opcode == 0x8 || opcode == 0x9) && MFID < 2) //This form does allow for other MFID's but Moto has a seperate function on 9 + { + //TELE_INT_CH_GRANT or TELE_INT_CH_GRANT_UPDT + int svc = mpdu_byte[8]; + int channel = (mpdu_byte[12] << 8) | mpdu_byte[13]; + int timer = (mpdu_byte[16] << 8) | mpdu_byte[17]; + int target = (mpdu_byte[3] << 16) | (mpdu_byte[4] << 8) | mpdu_byte[5]; + long int freq = 0; + fprintf (stderr, "\n"); + if (svc & 0x80) fprintf (stderr, " Emergency"); + if (svc & 0x40) fprintf (stderr, " Encrypted"); + + if (opts->payload == 1) //hide behind payload due to len + { + if (svc & 0x20) fprintf (stderr, " Duplex"); + if (svc & 0x10) fprintf (stderr, " Packet"); + else fprintf (stderr, " Circuit"); + if (svc & 0x8) fprintf (stderr, " R"); //reserved bit is on + fprintf (stderr, " Priority %d", svc & 0x7); //call priority + } + + fprintf (stderr, " Telephone Interconnect Voice Channel Grant"); + if ( opcode & 1) fprintf (stderr, " Update"); + fprintf (stderr, " Extended"); + fprintf (stderr, "\n CHAN: %04X; Timer: %f Seconds; Target: %d;", channel, (float)timer*0.1f, target); //timer unit is 100 ms, or 0.1 seconds + freq = process_channel_to_freq (opts, state, channel); + + //add active channel to string for ncurses display + sprintf (state->active_channel[0], "Active Tele Ch: %04X TGT: %d; ", channel, target); + state->last_active_time = time(NULL); + + //Skip tuning private calls if private calls is disabled (are telephone int calls private, or talkgroup?) + if (opts->trunk_tune_private_calls == 0) goto SKIPCALL; + + //Skip tuning encrypted calls if enc calls are disabled + if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; + + //telephone only has a target address (manual shows combined source/target of 24-bits) + for (int i = 0; i < state->group_tally; i++) + { + if (state->group_array[i].groupNumber == target) + { + fprintf (stderr, " [%s]", state->group_array[i].groupName); + strcpy (mode, state->group_array[i].groupMode); + break; + } + } + + //TG hold on UU_V -- will want to disable UU_V grants while TG Hold enabled + if (state->tg_hold != 0 && state->tg_hold != target) sprintf (mode, "%s", "B"); + + //tune if tuning available + if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) + { + //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot + if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq != 0) //if we aren't already on a VC and have a valid frequency + { + //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK + if (1 == 1) + { + if (state->p25_chan_tdma[channel >> 12] == 1) + { + state->samplesPerSymbol = 8; + state->symbolCenter = 3; + + //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to + //this will only occur in realtime tuning, not not required .bin or .wav playback + if (channel & 1) //VCH1 + { + opts->slot1_on = 0; + opts->slot2_on = 1; + } + else //VCH0 + { + opts->slot1_on = 1; + opts->slot2_on = 0; + } + } + + } + + //rigctl + if (opts->use_rigctl == 1) + { + if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); + SetFreq(opts->rigctl_sockfd, freq); + if (state->synctype == 0 || state->synctype == 1) state->p25_vc_freq[0] = freq; + opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop + state->last_vc_sync_time = time(NULL); + } + //rtl + else if (opts->audio_in_type == 3) + { + #ifdef USE_RTLSDR + rtl_dev_tune (opts, freq); + if (state->synctype == 0 || state->synctype == 1) state->p25_vc_freq[0] = freq; + opts->p25_is_tuned = 1; + state->last_vc_sync_time = time(NULL); + #endif + } + } + } + if (opts->p25_trunk == 0) + { + if (target == state->lasttg || target == state->lasttgR) + { + //P1 FDMA + if (state->synctype == 0 || state->synctype == 1) state->p25_vc_freq[0] = freq; + //P2 TDMA + else state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq; + } + } + + } + + //look at Harris Opcodes and payload portion of MPDU + else if (MFID == 0xA4) + { + //TODO: Add Known Opcodes from Manual (all one of them) + fprintf (stderr, "%s",KCYN); + fprintf (stderr, "\n MFID A4 (Harris); Opcode: %02X; ", opcode); + for (int i = 0; i < (12*(blks+1)%37); i++) + fprintf (stderr, "%02X", mpdu_byte[i]); + fprintf (stderr, " %s",KNRM); + } + + //look at Motorola Opcodes and payload portion of MPDU + else if (MFID == 0x90) + { + //TIA-102.AABH + if (opcode == 0x02) + { + int svc = mpdu_byte[8]; //Just the Res, P-bit, and more res bits + int channelt = (mpdu_byte[12] << 8) | mpdu_byte[13]; + int channelr = (mpdu_byte[14] << 8) | mpdu_byte[15]; + long int source = (mpdu_byte[3] << 16) |(mpdu_byte[4] << 8) | mpdu_byte[5]; + int group = (mpdu_byte[16] << 8) | mpdu_byte[17]; + long int freq1 = 0; + long int freq2 = 0; + UNUSED2(source, freq2); + fprintf (stderr, "%s\n ",KYEL); + + if (svc & 0x40) fprintf (stderr, " Encrypted"); //P-bit + + fprintf (stderr, " MFID90 Group Regroup Channel Grant - Explicit"); + fprintf (stderr, "\n RES/P [%02X] CHAN-T [%04X] CHAN-R [%04X] SG [%d][%04X]", svc, channelt, channelr, group, group); + freq1 = process_channel_to_freq (opts, state, channelt); + freq2 = process_channel_to_freq (opts, state, channelr); + + //add active channel to string for ncurses display + sprintf (state->active_channel[0], "MFID90 Ch: %04X SG: %d ", channelt, group); + state->last_active_time = time(NULL); + + for (int i = 0; i < state->group_tally; i++) + { + if (state->group_array[i].groupNumber == group) + { + fprintf (stderr, " [%s]", state->group_array[i].groupName); + strcpy (mode, state->group_array[i].groupMode); + break; + } + } + + //TG hold on MFID90 GRG -- block non-matching target, allow matching group + if (state->tg_hold != 0 && state->tg_hold != group) sprintf (mode, "%s", "B"); + if (state->tg_hold != 0 && state->tg_hold == group) sprintf (mode, "%s", "A"); + + //Skip tuning group calls if group calls are disabled + if (opts->trunk_tune_group_calls == 0) goto SKIPCALL; + + //Skip tuning encrypted calls if enc calls are disabled + if ( (svc & 0x40) && opts->trunk_tune_enc_calls == 0) goto SKIPCALL; + + //tune if tuning available + if (opts->p25_trunk == 1 && (strcmp(mode, "DE") != 0) && (strcmp(mode, "B") != 0)) + { + //reworked to set freq once on any call to process_channel_to_freq, and tune on that, independent of slot + if (state->p25_cc_freq != 0 && opts->p25_is_tuned == 0 && freq1 != 0) //if we aren't already on a VC and have a valid frequency + { + + //changed to allow symbol rate change on C4FM Phase 2 systems as well as QPSK + if (1 == 1) + { + if (state->p25_chan_tdma[channelt >> 12] == 1) + { + state->samplesPerSymbol = 8; + state->symbolCenter = 3; + + //shim fix to stutter/lag by only enabling slot on the target/channel we tuned to + //this will only occur in realtime tuning, not not required .bin or .wav playback + if (channelt & 1) //VCH1 + { + opts->slot1_on = 0; + opts->slot2_on = 1; + } + else //VCH0 + { + opts->slot1_on = 1; + opts->slot2_on = 0; + } + + } + } + //rigctl + if (opts->use_rigctl == 1) + { + if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw); + SetFreq(opts->rigctl_sockfd, freq1); + state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; + opts->p25_is_tuned = 1; //set to 1 to set as currently tuned so we don't keep tuning nonstop + state->last_vc_sync_time = time(NULL); + } + //rtl + else if (opts->audio_in_type == 3) + { + #ifdef USE_RTLSDR + rtl_dev_tune (opts, freq1); + state->p25_vc_freq[0] = state->p25_vc_freq[1] = freq1; + opts->p25_is_tuned = 1; + state->last_vc_sync_time = time(NULL); + #endif + } + } + } + } + + else + { + fprintf (stderr, "%s",KCYN); + fprintf (stderr, "\n MFID 90 (Moto); Opcode: %02X; ", mpdu_byte[0] & 0x3F); + for (int i = 0; i < (12*(blks+1)%37); i++) + fprintf (stderr, "%02X", mpdu_byte[i]); + fprintf (stderr, " %s",KNRM); + } + } + + else + { + fprintf (stderr, "%s",KCYN); + fprintf (stderr, "\n MFID %02X (Unknown); Opcode: %02X; ", MFID, mpdu_byte[0] & 0x3F); + for (int i = 0; i < (12*(blks+1)%37); i++) + fprintf (stderr, "%02X", mpdu_byte[i]); + fprintf (stderr, " %s",KNRM); + } + + SKIPCALL: ; //do nothing +} \ No newline at end of file