GitHub/dsd-fme
1
0
Fork 0
mirror of https://github.com/lwvmobile/dsd-fme.git synced 2026-11-13 01:07:24 -05:00
Code Issues Packages Projects Releases Wiki Activity
Files
d1334038df63a8ff95abd1bdd43fd2cb6b4a4d4e
dsd-fme/src/p25p2_xcch.c

954 lines
29 KiB
C
Raw Blame History

/*-------------------------------------------------------------------------------
* 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
//new slot variable with flipped assignment for SACCH
uint8_t slot = (state->currentslot ^ 1) & 1;
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 res = (payload[6] << 1) | (payload[7] << 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);
UNUSED3(mac_offset, b, mco_a);
//get the second mco after determining first message length, see what second mco is and plan accordingly
int mco_b = 69;
UNUSED(mco_b);
//attempt CRC12 check to validate or reject PDU
int err = -2;
if (state->p2_is_lcch == 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 ");
state->p2_is_lcch = 0; //turn flag off here
goto END_SMAC;
}
else //permit MAC_SIGNAL on CRC ERR if -F option called
{
if (opts->aggressive_framesync == 1)
{
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 ");
//warn user instead of failing
if (err != 0)
{
fprintf (stderr, "%s", KRED);
fprintf (stderr, "CRC16 ERR ");
}
fprintf (stderr, "%s", KYEL);
process_MAC_VPDU(opts, state, 1, SMAC);
fprintf (stderr, "%s", KNRM);
}
//do not permit MAC_PTT with CRC errs, help prevent false positives on calls
if (opcode == 0x1 && err == 0)
{
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->voice_counter[0] = 0;
state->dropL = 256;
state->dmrburstL = 20;
fprintf (stderr, "\n VCH 0 - ");
//check that src is not zero first, some harris and other patch systems may do this,
//but that also causes an issue in the new event logger if the active channel has a src, but mac_ptt has 0
uint32_t src = (SMAC[13] << 16) | (SMAC[14] << 8) | SMAC[15];
if (src != 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 ", src);
/*
When the talker radio is initiating an individual call (unit to unit or telephone interconnect), the reserved group ID of zero
is used in the group address portion of the MAC_PTT PDU and the source ID is that of the talker radio.
The outbound SACCH for the talker radio containing the talker ID information is required at the
talker radio site and optional at other sites. (So, if its from an external site, then it can be zero?)
*/
// if (state->lastsrc == 0) fprintf (stderr, "External ");
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");
if (state->R != 0 && state->payload_algid == 0xAA) fprintf (stderr, " Key 0x%010llX", state->R);
if (state->R != 0 && state->payload_algid == 0x81) fprintf (stderr, " Key 0x%016llX", state->R);
if ( (state->payload_algid == 0x84 || state->payload_algid == 0x89) && state->aes_key_loaded[0] == 1)
{
fprintf (stderr, "\n ");
fprintf (stderr, "Key: %016llX %016llX ", state->A1[0], state->A2[0]);
if (state->payload_algid == 0x84)
fprintf (stderr, "%016llX %016llX", state->A3[0], state->A4[0]);
// opts->unmute_encrypted_p25 = 1; //needed?
}
//expand 64-bit MI to 128-bit for AES
if (state->payload_algid == 0x84 || state->payload_algid == 0x89)
{
LFSR128(state);
// fprintf (stderr, "\n");
}
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
//reset gain
if (opts->floating_point == 1)
state->aout_gain = opts->audio_gain;
}
if (state->currentslot == 0)
{
//reset fourv_counter and dropbyte on PTT
state->fourv_counter[1] = 0;
state->voice_counter[1] = 0;
state->dropR = 256;
state->payload_algidR = 0; //zero this out as well
state->dmrburstR = 20;
fprintf (stderr, "\n VCH 1 - ");
//check that src is not zero first, some harris and other patch systems may do this,
//but that also causes an issue in the new event logger if the active channel has a src, but mac_ptt has 0
uint32_t src = (SMAC[13] << 16) | (SMAC[14] << 8) | SMAC[15];
if (src != 0)
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 ", src);
// if (state->lastsrcR == 0) fprintf (stderr, "External ");
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");
if (state->RR != 0 && state->payload_algidR == 0xAA) fprintf (stderr, " Key 0x%010llX", state->RR);
if (state->RR != 0 && state->payload_algidR == 0x81) fprintf (stderr, " Key 0x%016llX", state->RR);
if ( (state->payload_algidR == 0x84 || state->payload_algidR == 0x89) && state->aes_key_loaded[1] == 1)
{
fprintf (stderr, "\n ");
fprintf (stderr, "Key: %016llX %016llX ", state->A1[1], state->A2[1]);
if (state->payload_algidR == 0x84)
fprintf (stderr, "%016llX %016llX", state->A3[1], state->A4[1]);
// opts->unmute_encrypted_p25 = 1; //needed?
}
//expand 64-bit MI to 128-bit for AES
if (state->payload_algidR == 0x84 || state->payload_algidR == 0x89)
{
LFSR128(state);
// fprintf (stderr, "\n");
}
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
//reset gain
if (opts->floating_point == 1)
state->aout_gainR = opts->audio_gain;
}
if (opts->payload == 1)
{
fprintf (stderr, "\n MAC_PTT_PAYLOAD_S OFFSET: %d RES: %d \n ", mac_offset, res);
for (int i = 0; i < 24; i++)
{
if (i == 12) fprintf (stderr, "\n ");
fprintf (stderr, "[%02llX]", SMAC[i]);
}
}
//reset voice counter at MAC_PTT (inverted, triple check please)
if (state->currentslot == 1 && state->payload_algidR == 0x81)
state->DMRvcL = 0;
if (state->currentslot == 0 && state->payload_algid == 0x81)
state->DMRvcR = 0;
//reset voice counter after 2V (AES 256)
if (state->currentslot == 1 && state->payload_algidR == 0x84)
state->DMRvcL = 0;
if (state->currentslot == 0 && state->payload_algid == 0x84)
state->DMRvcR = 0;
//reset voice counter after 2V (AES 128)
if (state->currentslot == 1 && state->payload_algidR == 0x89)
state->DMRvcL = 0;
if (state->currentslot == 0 && state->payload_algid == 0x89)
state->DMRvcR = 0;
fprintf (stderr, "%s", KNRM);
}
//do not permit MAC_PTT_END with CRC errs, help prevent false positives on calls
if (opcode == 0x2 && err == 0)
{
fprintf (stderr, " MAC_END_PTT ");
fprintf (stderr, "%s", KRED);
//remember, slots are inverted here, so set the opposite ones
if (state->currentslot == 1)
{
state->fourv_counter[0] = 0;
state->voice_counter[0] = 0;
state->dropL = 256;
state->dmrburstL = 23;
state->payload_algid = 0;
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;
//close any open MBEout files
if (opts->mbe_out_f != NULL) closeMbeOutFile (opts, state);
//blank the call string here -- slot variable is already flipped accordingly for sacch
sprintf (state->call_string[slot], "%s", " "); //21 spaces -- wrong placement!
//reset gain
if (opts->floating_point == 1)
state->aout_gain = opts->audio_gain;
}
if (state->currentslot == 0)
{
state->fourv_counter[1] = 0;
state->voice_counter[1] = 0;
state->dropR = 256;
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;
//close any open MBEout files
if (opts->mbe_out_fR != NULL) closeMbeOutFileR (opts, state);
//blank the call string here -- slot variable is already flipped accordingly for sacch
sprintf (state->call_string[slot], "%s", " "); //21 spaces -- wrong placement!
//reset gain
if (opts->floating_point == 1)
state->aout_gainR = opts->audio_gain;
}
//Return to CC on MAC_END_PTT if other slot is idle
//TODO: Look at the source address of the END_PTT, if it is 0xFFFFFF, then its from the FNE?
//if from the FNE, that may be when we know channel teardown will really occur vs using MAC_END_PTT directly
/*
Upon receipt of a MAC_END_PTT PDU on the assigned VCH, and having verified it contains the
correct color code, the SU shall return to the idle state on the control channel.
*/
//NOTE: Disable return later on if not desirable to use
//end_ptt in this slot, idle in the other slot (Normal Slots)
if ( ((state->currentslot == 0) && (state->dmrburstL == 24)) ||
((state->currentslot == 1) && (state->dmrburstR == 24)) )
{
if (opts->p25_trunk == 1 && opts->p25_is_tuned == 1)
{
//if P25p2 VCH and going back to P25p1 CC, flip symbolrate
if (state->p25_cc_is_tdma == 0)
{
state->samplesPerSymbol = 10;
state->symbolCenter = 4;
//re-enable both slots
opts->slot1_on = 1;
opts->slot2_on = 1;
}
//rigctl
if (opts->use_rigctl == 1)
{
state->lasttg = 0;
state->lastsrc = 0;
state->lasttgR = 0;
state->lastsrcR = 0;
state->payload_algid = 0;
state->payload_keyid = 0;
state->payload_algidR = 0;
state->payload_keyidR = 0;
// state->payload_miP = 0;
//reset some strings
sprintf (state->call_string[0], "%s", " "); //21 spaces
sprintf (state->call_string[1], "%s", " "); //21 spaces
sprintf (state->active_channel[0], "%s", "");
sprintf (state->active_channel[1], "%s", "");
opts->p25_is_tuned = 0;
state->p25_vc_freq[0] = state->p25_vc_freq[1] = 0;
if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw);
SetFreq(opts->rigctl_sockfd, state->p25_cc_freq);
}
//rtl
else if (opts->audio_in_type == 3)
{
#ifdef USE_RTLSDR
state->lasttg = 0;
state->lastsrc = 0;
state->lasttgR = 0;
state->lastsrcR = 0;
state->payload_algid = 0;
state->payload_keyid = 0;
state->payload_algidR = 0;
state->payload_keyidR = 0;
// state->payload_miP = 0;
//reset some strings
sprintf (state->call_string[0], "%s", " "); //21 spaces
sprintf (state->call_string[1], "%s", " "); //21 spaces
sprintf (state->active_channel[0], "%s", "");
sprintf (state->active_channel[1], "%s", "");
opts->p25_is_tuned = 0;
state->p25_vc_freq[0] = state->p25_vc_freq[1] = 0;
rtl_dev_tune (opts, state->p25_cc_freq);
#endif
}
}
}
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x3 && err == 0)
{
if (state->currentslot == 1) state->dmrburstL = 24;
else state->dmrburstR = 24;
fprintf (stderr, " MAC_IDLE ");
fprintf (stderr, "%s", KYEL);
process_MAC_VPDU(opts, state, 1, SMAC);
fprintf (stderr, "%s", KNRM);
// if (opts->p25_trunk == 1 && opts->p25_is_tuned == 1)
// {
// if (state->currentslot == 1)
// {
// opts->slot1_on = 0;
// opts->slot2_on = 1;
// }
// else
// {
// opts->slot1_on = 1;
// opts->slot2_on = 0;
// }
// }
//blank the call string here -- slot variable is already flipped accordingly for sacch
sprintf (state->call_string[slot], "%s", " "); //21 spaces
}
if (opcode == 0x4 && err == 0)
{
//disable to prevent blinking in ncurses terminal due to OSS preemption shim
#ifdef __CYGWIN__
if (opts->audio_out_type != 5)
{
if (state->currentslot == 1) state->dmrburstL = 21;
else state->dmrburstR = 21;
}
#else
if (state->currentslot == 1) state->dmrburstL = 21;
else state->dmrburstR = 21;
#endif
fprintf (stderr, " MAC_ACTIVE ");
fprintf (stderr, "%s", KYEL);
process_MAC_VPDU(opts, state, 1, SMAC);
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x6 && err == 0)
{
if (state->currentslot == 1)
{
state->dmrburstL = 22;
//close any open MBEout files
if (opts->mbe_out_f != NULL) closeMbeOutFile (opts, state);
}
else
{
state->dmrburstR = 22;
//close any open MBEout files
if (opts->mbe_out_fR != NULL) closeMbeOutFileR (opts, state);
}
fprintf (stderr, " MAC_HANGTIME ");
fprintf (stderr, "%s", KYEL);
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
//new slot variable
uint8_t slot = state->currentslot;
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);
int res = (payload[6] << 1) | (payload[7] << 0);
UNUSED(mac_offset);
//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", KYEL);
// process_MAC_VPDU(opts, state, 0, FMAC);
// fprintf (stderr, "%s", KNRM);
// }
if (opcode == 0x1 && err == 0)
{
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->voice_counter[0] = 0;
state->dropL = 256;
state->dmrburstL = 20;
fprintf (stderr, "\n VCH 0 - ");
//check that src is not zero first, some harris and other patch systems may do this,
//but that also causes an issue in the new event logger if the active channel has a src, but mac_ptt has 0
uint32_t src = (FMAC[13] << 16) | (FMAC[14] << 8) | FMAC[15];
if (src != 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 ", src);
// if (state->lastsrc == 0) fprintf (stderr, "External ");
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");
if (state->R != 0 && state->payload_algid == 0xAA) fprintf (stderr, " Key 0x%010llX", state->R);
if (state->R != 0 && state->payload_algid == 0x81) fprintf (stderr, " Key 0x%016llX", state->R);
if ( (state->payload_algid == 0x84 || state->payload_algid == 0x89) && state->aes_key_loaded[0] == 1)
{
fprintf (stderr, "\n ");
fprintf (stderr, "Key: %016llX %016llX ", state->A1[0], state->A2[0]);
if (state->payload_algid == 0x84)
fprintf (stderr, "%016llX %016llX", state->A3[0], state->A4[0]);
// opts->unmute_encrypted_p25 = 1; //needed?
}
//expand 64-bit MI to 128-bit for AES
if (state->payload_algid == 0x84 || state->payload_algid == 0x89)
{
LFSR128(state);
// fprintf (stderr, "\n");
}
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
//reset gain
if (opts->floating_point == 1)
state->aout_gain = opts->audio_gain;
}
if (state->currentslot == 1)
{
//reset fourv_counter and dropbyte on PTT
state->fourv_counter[1] = 0;
state->voice_counter[1] = 0;
state->dropR = 256;
state->dmrburstR = 20;
fprintf (stderr, "\n VCH 1 - ");
//check that src is not zero first, some harris and other patch systems may do this,
//but that also causes an issue in the new event logger if the active channel has a src, but mac_ptt has 0
uint32_t src = (FMAC[13] << 16) | (FMAC[14] << 8) | FMAC[15];
if (src != 0)
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 ", src);
// if (state->lastsrcR == 0) fprintf (stderr, "External ");
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");
if (state->RR != 0 && state->payload_algidR == 0xAA) fprintf (stderr, " Key 0x%010llX", state->RR);
if (state->RR != 0 && state->payload_algidR == 0x81) fprintf (stderr, " Key 0x%016llX", state->RR);
if ( (state->payload_algidR == 0x84 || state->payload_algidR == 0x89) && state->aes_key_loaded[1] == 1)
{
fprintf (stderr, "\n ");
fprintf (stderr, "Key: %016llX %016llX ", state->A1[1], state->A2[1]);
if (state->payload_algidR == 0x84)
fprintf (stderr, "%016llX %016llX", state->A3[1], state->A4[1]);
// opts->unmute_encrypted_p25 = 1; //needed?
}
//expand 64-bit MI to 128-bit for AES
if (state->payload_algidR == 0x84 || state->payload_algidR == 0x89)
{
LFSR128(state);
// fprintf (stderr, "\n");
}
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
//reset gain
if (opts->floating_point == 1)
state->aout_gainR = opts->audio_gain;
}
if (opts->payload == 1)
{
fprintf (stderr, "\n MAC_PTT_PAYLOAD_F OFFSET: %d RES: %d \n ", mac_offset, res);
for (int i = 0; i < 24; i++)
{
if (i == 12) fprintf (stderr, "\n ");
fprintf (stderr, "[%02llX]", FMAC[i]);
}
}
fprintf (stderr, "%s", KNRM);
//reset voice counter at MAC_PTT
if (state->currentslot == 0 && state->payload_algid == 0x81)
state->DMRvcL = 0;
if (state->currentslot == 1 && state->payload_algidR == 0x81)
state->DMRvcR = 0;
//reset voice counter after 2V (AES 256)
if (state->currentslot == 0 && state->payload_algid == 0x84)
state->DMRvcL = 0;
if (state->currentslot == 1 && state->payload_algidR == 0x84)
state->DMRvcR = 0;
//reset voice counter after 2V (AES 128)
if (state->currentslot == 0 && state->payload_algid == 0x89)
state->DMRvcL = 0;
if (state->currentslot == 1 && state->payload_algidR == 0x89)
state->DMRvcR = 0;
}
if (opcode == 0x2 && err == 0)
{
fprintf (stderr, " MAC_END_PTT ");
fprintf (stderr, "%s", KRED);
if (state->currentslot == 0)
{
state->fourv_counter[0] = 0;
state->voice_counter[0] = 0;
state->dropL = 256;
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;
//close any open MBEout files
if (opts->mbe_out_f != NULL) closeMbeOutFile (opts, state);
//blank the call string here
sprintf (state->call_string[slot], "%s", " "); //21 spaces
//reset gain
if (opts->floating_point == 1)
state->aout_gain = opts->audio_gain; //reset
}
if (state->currentslot == 1)
{
state->fourv_counter[1] = 0;
state->voice_counter[1] = 0;
state->dropR = 256;
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;
//close any open MBEout files
if (opts->mbe_out_fR != NULL) closeMbeOutFileR (opts, state);
//reset gain
if (opts->floating_point == 1)
state->aout_gainR = opts->audio_gain;
}
//Return to CC on MAC_END_PTT if other slot is idle
//NOTE: Disable return later on if not desirable to use
//end_ptt in this slot, idle in the other slot (Normal Slots)
if ( ((state->currentslot == 0) && (state->dmrburstR == 24)) ||
((state->currentslot == 1) && (state->dmrburstL == 24)) )
{
if (opts->p25_trunk == 1 && opts->p25_is_tuned == 1)
{
//if P25p2 VCH and going back to P25p1 CC, flip symbolrate
if (state->p25_cc_is_tdma == 0)
{
state->samplesPerSymbol = 10;
state->symbolCenter = 4;
//re-enable both slots
opts->slot1_on = 1;
opts->slot2_on = 1;
}
//rigctl
if (opts->use_rigctl == 1)
{
state->lasttg = 0;
state->lastsrc = 0;
state->lasttgR = 0;
state->lastsrcR = 0;
state->payload_algid = 0;
state->payload_keyid = 0;
state->payload_algidR = 0;
state->payload_keyidR = 0;
// state->payload_miP = 0;
//reset some strings
sprintf (state->call_string[0], "%s", " "); //21 spaces
sprintf (state->call_string[1], "%s", " "); //21 spaces
sprintf (state->active_channel[0], "%s", "");
sprintf (state->active_channel[1], "%s", "");
opts->p25_is_tuned = 0;
state->p25_vc_freq[0] = state->p25_vc_freq[1] = 0;
if (opts->setmod_bw != 0 ) SetModulation(opts->rigctl_sockfd, opts->setmod_bw);
SetFreq(opts->rigctl_sockfd, state->p25_cc_freq);
}
//rtl
else if (opts->audio_in_type == 3)
{
#ifdef USE_RTLSDR
state->lasttg = 0;
state->lastsrc = 0;
state->lasttgR = 0;
state->lastsrcR = 0;
state->payload_algid = 0;
state->payload_keyid = 0;
state->payload_algidR = 0;
state->payload_keyidR = 0;
// state->payload_miP = 0;
//reset some strings
sprintf (state->call_string[0], "%s", " "); //21 spaces
sprintf (state->call_string[1], "%s", " "); //21 spaces
sprintf (state->active_channel[0], "%s", "");
sprintf (state->active_channel[1], "%s", "");
opts->p25_is_tuned = 0;
state->p25_vc_freq[0] = state->p25_vc_freq[1] = 0;
rtl_dev_tune (opts, state->p25_cc_freq);
#endif
}
}
}
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x3 && err == 0)
{
//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->voice_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->voice_counter[1] = 0;
state->lastsrcR = 0;
state->lasttgR = 0;
}
fprintf (stderr, " MAC_IDLE ");
fprintf (stderr, "%s", KYEL);
process_MAC_VPDU(opts, state, 0, FMAC);
fprintf (stderr, "%s", KNRM);
// if (opts->p25_trunk == 1 && opts->p25_is_tuned == 1)
// {
// if (state->currentslot == 0)
// {
// opts->slot1_on = 0;
// opts->slot2_on = 1;
// }
// else
// {
// opts->slot1_on = 1;
// opts->slot2_on = 0;
// }
// }
//blank the call string here
sprintf (state->call_string[slot], "%s", " "); //21 spaces
}
if (opcode == 0x4 && err == 0)
{
//disable to prevent blinking in ncurses terminal due to OSS preemption shim
#ifdef __CYGWIN__
if (opts->audio_out_type != 5)
{
if (state->currentslot == 0) state->dmrburstL = 21;
else state->dmrburstR = 21;
}
#else
if (state->currentslot == 0) state->dmrburstL = 21;
else state->dmrburstR = 21;
#endif
fprintf (stderr, " MAC_ACTIVE ");
fprintf (stderr, "%s", KYEL);
process_MAC_VPDU(opts, state, 0, FMAC);
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x6 && err == 0)
{
if (state->currentslot == 0)
{
state->dmrburstL = 22;
//close any open MBEout files
if (opts->mbe_out_f != NULL) closeMbeOutFile (opts, state);
}
else
{
state->dmrburstR = 22;
//close any open MBEout files
if (opts->mbe_out_fR != NULL) closeMbeOutFileR (opts, state);
}
fprintf (stderr, " MAC_HANGTIME ");
fprintf (stderr, "%s", KYEL);
process_MAC_VPDU(opts, state, 0, FMAC);
fprintf (stderr, "%s", KNRM);
}
END_FMAC:
if (1 == 2)
{
//CRC Failure!
}
}
Reference in New Issue View Git Blame Copy Permalink