GitHub/dsd-fme
1
0
Fork 0
mirror of https://github.com/lwvmobile/dsd-fme.git synced 2026-11-14 01:37:25 -05:00
Code Issues Packages Projects Releases Wiki Activity
Files
3312e14422c00ec6139122a6a37eb877c53a57fe
dsd-fme/src/dmr_pi.c
lwvmobile 3312e14422 DMR: Fix DES Alg (0x22) Set; 
Fix Late Entry Mismatch Bug due to variable type change (int to ulli);

More Fixes Tweaks for Hytera Enhanced Encryption Setup;
2025-04-04 17:35:48 -04:00

418 lines
12 KiB
C
Raw Blame History

/*-------------------------------------------------------------------------------
* dmr_pi.c
* DMR Privacy Indicator and LFSR Function
*
* LFSR code courtesy of https://github.com/mattames/LFSR/
*
* LWVMOBILE
* 2022-12 DSD-FME Florida Man Edition
*-----------------------------------------------------------------------------*/
#include "dsd.h"
void dmr_pi (dsd_opts * opts, dsd_state * state, uint8_t PI_BYTE[], uint32_t CRCCorrect, uint32_t IrrecoverableErrors)
{
UNUSED2(opts, CRCCorrect);
uint8_t MFID = PI_BYTE[1];
if((IrrecoverableErrors == 0))
{
//update cc amd vc sync time for trunking purposes (particularly Con+)
if (opts->p25_is_tuned == 1)
{
state->last_vc_sync_time = time(NULL);
state->last_cc_sync_time = time(NULL);
}
if (MFID == 0x68) //Hytera Enhanced
{
if (state->currentslot == 0)
{
state->payload_algid = PI_BYTE[0];
state->payload_keyid = PI_BYTE[2];
state->payload_mi = ((unsigned long long int)PI_BYTE[3] << 32ULL) | ((unsigned long long int)PI_BYTE[4] << 24) |
((unsigned long long int)PI_BYTE[5] << 16) | ((unsigned long long int)PI_BYTE[6] << 8) | ((unsigned long long int)PI_BYTE[7] << 0);
}
else
{
state->payload_algidR = PI_BYTE[0];
state->payload_keyidR = PI_BYTE[2];
state->payload_miR = ((unsigned long long int)PI_BYTE[3] << 32ULL) | ((unsigned long long int)PI_BYTE[4] << 24) |
((unsigned long long int)PI_BYTE[5] << 16) | ((unsigned long long int)PI_BYTE[6] << 8) | ((unsigned long long int)PI_BYTE[7] << 0);
}
fprintf (stderr, "%s ", KYEL);
fprintf (stderr, "\n Slot %d", state->currentslot+1);
fprintf (stderr, " DMR PI H- ALG ID: %02X; KEY ID: %02X; MI(40): %02X%02X%02X%02X%02X;",
PI_BYTE[0], PI_BYTE[2], PI_BYTE[3], PI_BYTE[4], PI_BYTE[5], PI_BYTE[6], PI_BYTE[7]);
//PI_BYTE[9] is a checksum of the other bytes combined
uint8_t checksum = 0;
for (int i = 0; i < 9; i++)
{
checksum += PI_BYTE[i];
checksum &= 0xFF;
}
checksum = ~checksum & 0xFF;
checksum++;
//debug
// fprintf (stderr, " CHK: %02X / %02X;", checksum, PI_BYTE[9]);
if (checksum == PI_BYTE[9])
{
fprintf (stderr, " Hytera Enhanced;");
//disable late entry for DMRA (hopefully, there aren't any systems running both DMRA and Hytera Enhanced mixed together)
opts->dmr_le = 2;
// fprintf (stderr, " (Checksum Okay);");
}
else
{
fprintf (stderr, "%s", KRED);
fprintf (stderr, " (Checksum Err);");
}
fprintf (stderr, "%s", KNRM);
}
else if (MFID == 0x10) //DMRA
{
if (state->currentslot == 0)
{
state->payload_algid = PI_BYTE[0];
state->payload_keyid = PI_BYTE[2];
state->payload_mi = ( ((PI_BYTE[3]) << 24) + ((PI_BYTE[4]) << 16) + ((PI_BYTE[5]) << 8) + (PI_BYTE[6]) );
if (state->payload_algid < 0x26)
{
fprintf (stderr, "%s ", KYEL);
fprintf (stderr, "\n Slot 1");
fprintf (stderr, " DMR PI H- ALG ID: %02X; KEY ID: %02X; MI(32): %08X;", state->payload_algid, state->payload_keyid, state->payload_mi);
//check for any values that aren't 0x2X but just 0x0X
//going to be very generic here to avoid any particular vendors using 0x2X and not 0x0X
if (state->payload_algid & 0x20)
fprintf (stderr, " DMRA");
else fprintf (stderr, " DMRA Compatible");
if ((state->payload_algid & 0x07) == 0x01)
{
fprintf (stderr, " RC4;");
state->payload_algid = 0x21;
}
else if ((state->payload_algid & 0x07) == 0x02)
{
fprintf (stderr, " DES;");
state->payload_algid = 0x22;
}
else if ((state->payload_algid & 0x07) == 0x04)
{
fprintf (stderr, " AES-128;");
state->payload_algid = 0x24;
}
else if ((state->payload_algid & 0x07) == 0x05)
{
fprintf (stderr, " AES-256;");
state->payload_algid = 0x25;
}
fprintf (stderr, "%s ", KNRM);
//expand the 32-bit MI to a 64-bit DES IV
if (state->payload_algid == 0x22)
{
fprintf (stderr, "\n");
LFSR64 (state);
}
//expand the 32-bit MI to a 128-bit AES IV
if (state->payload_algid == 0x24 || state->payload_algid == 0x25)
{
fprintf (stderr, "\n");
LFSR128d (state);
}
}
if (state->payload_algid >= 0x26)
{
state->payload_algid = 0;
state->payload_keyid = 0;
state->payload_mi = 0;
}
}
if (state->currentslot == 1)
{
state->payload_algidR = PI_BYTE[0];
state->payload_keyidR = PI_BYTE[2];
state->payload_miR = ( ((PI_BYTE[3]) << 24) + ((PI_BYTE[4]) << 16) + ((PI_BYTE[5]) << 8) + (PI_BYTE[6]) );
if (state->payload_algidR < 0x26)
{
fprintf (stderr, "%s ", KYEL);
fprintf (stderr, "\n Slot 2");
fprintf (stderr, " DMR PI H- ALG ID: %02X; KEY ID: %02X; MI(32): %08X", state->payload_algidR, state->payload_keyidR, state->payload_miR);
//check for any values that aren't 0x2X but just 0x0X
//going to be very generic here to avoid any particular vendors using 0x2X and not 0x0X
if (state->payload_algidR & 0x20)
fprintf (stderr, " DMRA");
else fprintf (stderr, " DMRA Compatible");
if ((state->payload_algidR & 0x07) == 0x01)
{
fprintf (stderr, " RC4;");
state->payload_algidR = 0x21;
}
else if ((state->payload_algidR & 0x07) == 0x02)
{
fprintf (stderr, " DES;");
state->payload_algidR = 0x22;
}
else if ((state->payload_algidR & 0x07) == 0x04)
{
fprintf (stderr, " AES-128;");
state->payload_algidR = 0x24;
}
else if ((state->payload_algidR & 0x07) == 0x05)
{
fprintf (stderr, " AES-256;");
state->payload_algidR = 0x25;
}
fprintf (stderr, "%s ", KNRM);
//expand the 32-bit MI to a 64-bit DES IV
if (state->payload_algidR == 0x22)
{
fprintf (stderr, "\n");
LFSR64 (state);
}
//expand the 32-bit MI to a 128-bit AES IV
if (state->payload_algidR == 0x24 || state->payload_algidR == 0x25)
{
fprintf (stderr, "\n");
LFSR128d (state);
}
}
if (state->payload_algidR >= 0x26)
{
state->payload_algidR = 0;
state->payload_keyidR = 0;
state->payload_miR = 0;
}
}
} //end DMRA
}
}
void LFSR(dsd_state * state)
{
int lfsr = 0;
if (state->currentslot == 0)
{
lfsr = state->payload_mi;
}
else lfsr = state->payload_miR;
uint8_t cnt = 0;
for(cnt=0;cnt<32;cnt++)
{
// Polynomial is C(x) = x^32 + x^4 + x^2 + 1
int bit = ((lfsr >> 31) ^ (lfsr >> 3) ^ (lfsr >> 1)) & 0x1;
lfsr = (lfsr << 1) | (bit);
}
lfsr &= 0xFFFFFFFF;
if (state->currentslot == 0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, " Slot 1");
fprintf (stderr, " DMR PI C- ALG ID: %02X; KEY ID: %02X;", state->payload_algid, state->payload_keyid);
fprintf (stderr, " MI(32): %08X;", lfsr);
fprintf (stderr, " RC4;");
fprintf (stderr, "%s", KNRM);
state->payload_mi = lfsr;
}
if (state->currentslot == 1)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, " Slot 2");
fprintf (stderr, " DMR PI C- ALG ID: %02X; KEY ID: %02X;", state->payload_algidR, state->payload_keyidR);
fprintf(stderr, " MI(32): %08X;", lfsr);
fprintf (stderr, " RC4;");
fprintf (stderr, "%s", KNRM);
state->payload_miR = lfsr;
}
}
//Expand a 32-bit MI into a 64-bit IV for DES
void LFSR64(dsd_state * state)
{
{
unsigned long long int lfsr = 0;
if (state->currentslot == 0)
{
lfsr = (uint64_t) state->payload_mi;
}
else lfsr = (uint64_t) state->payload_miR;
uint8_t cnt = 0;
for(cnt=0;cnt<32;cnt++)
{
unsigned long long int bit = ( (lfsr >> 31) ^ (lfsr >> 21) ^ (lfsr >> 1) ^ (lfsr >> 0) ) & 0x1;
lfsr = (lfsr << 1) | bit;
}
if (state->currentslot == 0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, " Slot 1");
fprintf (stderr, " DMR PI C- ALG ID: %02X; KEY ID: %02X;", state->payload_algid, state->payload_keyid);
fprintf (stderr, " MI(64): %016llX;", lfsr);
fprintf (stderr, " DES;");
fprintf (stderr, "%s", KNRM);
state->payload_mi = lfsr & 0xFFFFFFFF; //truncate for next repitition and le verification
state->payload_miP = lfsr;
state->DMRvcL = 0;
}
if (state->currentslot == 1)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, " Slot 2");
fprintf (stderr, " DMR PI C- ALG ID: %02X; KEY ID: %02X;", state->payload_algidR, state->payload_keyidR);
fprintf (stderr, " MI(64): %016llX;", lfsr);
fprintf (stderr, " DES;");
fprintf (stderr, "%s", KNRM);
state->payload_miR = lfsr & 0xFFFFFFFF; //truncate for next repitition and le verification
state->payload_miN = lfsr;
state->DMRvcR = 0;
}
}
}
//Expand a 32-bit MI into a 128-bit IV for AES
void LFSR128d(dsd_state * state)
{
unsigned long long int lfsr = 0;
if (state->currentslot == 0)
lfsr = state->payload_mi;
else lfsr = state->payload_miR;
unsigned long long int next_mi;
//start packing aes_iv
if (state->currentslot == 0)
{
state->aes_iv[0] = (lfsr >> 24) & 0xFF;
state->aes_iv[1] = (lfsr >> 16) & 0xFF;
state->aes_iv[2] = (lfsr >> 8 ) & 0xFF;
state->aes_iv[3] = (lfsr >> 0 ) & 0xFF;
}
else if (state->currentslot == 1)
{
state->aes_ivR[0] = (lfsr >> 24) & 0xFF;
state->aes_ivR[1] = (lfsr >> 16) & 0xFF;
state->aes_ivR[2] = (lfsr >> 8 ) & 0xFF;
state->aes_ivR[3] = (lfsr >> 0 ) & 0xFF;
}
int cnt = 0; int x = 32;
unsigned long long int bit;
for(cnt=0;cnt<96;cnt++)
{
//32,22,2,1
bit = ( (lfsr >> 31) ^ (lfsr >> 21) ^ (lfsr >> 1) ^ (lfsr >> 0) ) & 0x1;
lfsr = (lfsr << 1) | bit;
//continue packing aes_iv
if (state->currentslot == 0)
state->aes_iv[x/8] = (state->aes_iv[x/8] << 1) + bit;
else if (state->currentslot == 1)
state->aes_ivR[x/8] = (state->aes_ivR[x/8] << 1) + bit;
x++;
}
//assign the next 32-bit short MI from 4,5,6,7 so it'll match up with OTA late entry
if (state->currentslot == 0)
next_mi = (state->aes_iv[4] << 24) + (state->aes_iv[5] << 16) + (state->aes_iv[6] << 8) + (state->aes_iv[7] << 0);
if (state->currentslot == 1)
next_mi = (state->aes_ivR[4] << 24) + (state->aes_ivR[5] << 16) + (state->aes_ivR[6] << 8) + (state->aes_ivR[7] << 0);
if (state->currentslot == 0)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, " Slot 1");
fprintf (stderr, " DMR PI C- ALG ID: %02X; KEY ID: %02X; MI(128): ", state->payload_algid, state->payload_keyid);
for (x = 0; x < 16; x++)
fprintf (stderr, "%02X", state->aes_iv[x]);
fprintf (stderr, "%s", KNRM);
fprintf (stderr, ";");
if (state->payload_algid == 0x24)
fprintf (stderr, " AES-128;");
else fprintf (stderr, " AES-256;");
state->payload_mi = next_mi;
state->DMRvcL = 0;
}
if (state->currentslot == 1)
{
fprintf (stderr, "%s", KYEL);
fprintf (stderr, " Slot 2");
fprintf (stderr, " DMR PI C- ALG ID: %02X; KEY ID: %02X; MI(128): ", state->payload_algidR, state->payload_keyidR);
for (x = 0; x < 16; x++)
fprintf (stderr, "%02X", state->aes_ivR[x]);
fprintf (stderr, "%s", KNRM);
fprintf (stderr, ";");
if (state->payload_algidR == 0x24)
fprintf (stderr, " AES-128;");
else fprintf (stderr, " AES-256;");
state->payload_miR = next_mi;
state->DMRvcR = 0;
}
}
void hytera_enhanced_enc_setup(dsd_opts * opts, dsd_state * state, unsigned long long int key_value, unsigned long long int mi_value)
{
UNUSED(opts);
UNUSED(state);
UNUSED(key_value);
UNUSED(mi_value);
}
Reference in New Issue View Git Blame Copy Permalink