GitHub/dsd-fme
1
0
Fork 0
mirror of https://github.com/lwvmobile/dsd-fme.git synced 2026-11-13 09:17:20 -05:00
Code Issues Packages Projects Releases Wiki Activity
Files
bf96b96fdf7a247fc8e3cec3bfe2bafd56da9170
dsd-fme/src/p25p2_xcch.c
lwvmobile 676027a56f Tweak Float Auto Gain; Make User Configurable;
2023-07-26 07:21:25 -04:00

612 lines
17 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 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->dropL = 256;
state->dmrburstL = 20;
fprintf (stderr, "\n VCH 0 - ");
state->lastsrc = (SMAC[13] << 16) | (SMAC[14] << 8) | SMAC[15];
state->lasttg = (SMAC[16] << 8) | SMAC[17];
fprintf (stderr, "TG %d ", state->lasttg);
fprintf (stderr, "SRC %d ", state->lastsrc);
state->payload_algid = SMAC[10];
state->payload_keyid = (SMAC[11] << 8) | SMAC[12];
state->payload_miP = (SMAC[1] << 56) | (SMAC[2] << 48) | (SMAC[3] << 40) | (SMAC[4] << 32) |
(SMAC[5] << 24) | (SMAC[6] << 16) | (SMAC[7] << 8) | (SMAC[8] << 0);
if (state->payload_algid != 0x80 && state->payload_algid != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algid);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyid);
fprintf (stderr, " MI 0x%016llX", state->payload_miP);
if (state->R != 0 && state->payload_algid == 0xAA) fprintf (stderr, " Key 0x%010llX", state->R);
fprintf(stderr, " MPTT");
// 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->dropR = 256;
state->payload_algidR = 0; //zero this out as well
state->dmrburstR = 20;
fprintf (stderr, "\n VCH 1 - ");
state->lastsrcR = (SMAC[13] << 16) | (SMAC[14] << 8) | SMAC[15];
state->lasttgR = (SMAC[16] << 8) | SMAC[17];
fprintf (stderr, "TG %d ", state->lasttgR);
fprintf (stderr, "SRC %d ", state->lastsrcR);
state->payload_algidR = SMAC[10];
state->payload_keyidR = (SMAC[11] << 8) | SMAC[12];
state->payload_miN = (SMAC[1] << 56) | (SMAC[2] << 48) | (SMAC[3] << 40) | (SMAC[4] << 32) |
(SMAC[5] << 24) | (SMAC[6] << 16) | (SMAC[7] << 8) | (SMAC[8] << 0);
if (state->payload_algidR != 0x80 && state->payload_algidR != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algidR);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyidR);
fprintf (stderr, " MI 0x%016llX", state->payload_miN);
if (state->RR != 0 && state->payload_algidR == 0xAA) fprintf (stderr, " Key 0x%010llX", state->RR);
fprintf(stderr, " MPTT");
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
//reset gain
if (opts->floating_point == 1)
state->aout_gainR = opts->audio_gain;
}
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->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->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;
}
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);
//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 AERO_BUILD
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);
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->dropL = 256;
state->dmrburstL = 20;
fprintf (stderr, "\n VCH 0 - ");
state->lastsrc = (FMAC[13] << 16) | (FMAC[14] << 8) | FMAC[15];
state->lasttg = (FMAC[16] << 8) | FMAC[17];
fprintf (stderr, "TG %d ", state->lasttg);
fprintf (stderr, "SRC %d ", state->lastsrc);
state->payload_algid = FMAC[10];
state->payload_keyid = (FMAC[11] << 8) | FMAC[12];
state->payload_miP = (FMAC[1] << 56) | (FMAC[2] << 48) | (FMAC[3] << 40) | (FMAC[4] << 32) |
(FMAC[5] << 24) | (FMAC[6] << 16) | (FMAC[7] << 8) | (FMAC[8] << 0);
if (state->payload_algid != 0x80 && state->payload_algid != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algid);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyid);
fprintf (stderr, " MI 0x%016llX", state->payload_miP);
if (state->R != 0 && state->payload_algid == 0xAA) fprintf (stderr, " Key 0x%010llX", state->R);
fprintf(stderr, " MPTT");
// 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->dropR = 256;
state->dmrburstR = 20;
fprintf (stderr, "\n VCH 1 - ");
state->lastsrcR = (FMAC[13] << 16) | (FMAC[14] << 8) | FMAC[15];
state->lasttgR = (FMAC[16] << 8) | FMAC[17];
fprintf (stderr, "TG %d ", state->lasttgR);
fprintf (stderr, "SRC %d ", state->lastsrcR);
state->payload_algidR = FMAC[10];
state->payload_keyidR = (FMAC[11] << 8) | FMAC[12];
state->payload_miN = (FMAC[1] << 56) | (FMAC[2] << 48) | (FMAC[3] << 40) | (FMAC[4] << 32) |
(FMAC[5] << 24) | (FMAC[6] << 16) | (FMAC[7] << 8) | (FMAC[8] << 0);
if (state->payload_algidR != 0x80 && state->payload_algidR != 0x0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, "\n ALG ID 0x%02X", state->payload_algidR);
fprintf (stderr, " KEY ID 0x%04X", state->payload_keyidR);
fprintf (stderr, " MI 0x%016llX", state->payload_miN);
if (state->RR != 0 && state->payload_algidR == 0xAA) fprintf (stderr, " Key 0x%010llX", state->RR);
fprintf(stderr, " MPTT");
// fprintf (stderr, " %s", KRED);
// fprintf (stderr, "ENC");
}
//reset gain
if (opts->floating_point == 1)
state->aout_gainR = opts->audio_gain;
}
fprintf (stderr, "%s", KNRM);
}
if (opcode == 0x2 && err == 0)
{
fprintf (stderr, " MAC_END_PTT ");
fprintf (stderr, "%s", KRED);
if (state->currentslot == 0)
{
state->fourv_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->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;
}
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->lastsrc = 0;
state->lasttg = 0;
}
else
{
state->payload_algidR = 0;
state->payload_keyidR = 0;
state->dmrburstR = 24;
state->fourv_counter[1] = 0;
state->lastsrcR = 0;
state->lasttgR = 0;
}
fprintf (stderr, " MAC_IDLE ");
fprintf (stderr, "%s", KYEL);
process_MAC_VPDU(opts, state, 0, FMAC);
fprintf (stderr, "%s", KNRM);
//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 AERO_BUILD
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