mirror of
https://github.com/lwvmobile/dsd-fme.git
synced 2026-11-13 09:17:20 -05:00
241 lines
7.3 KiB
C
241 lines
7.3 KiB
C
#include "dsd.h"
|
|
#include "dmr_const.h"
|
|
#include "pc4.h"
|
|
|
|
//interleaved code words for AMBE+2 (as it arrives over the air)
|
|
void ambe2_codeword_print_i (dsd_opts * opts, char ambe_fr[4][24])
|
|
{
|
|
uint8_t interleaved[72];
|
|
memset (interleaved, 0, sizeof(interleaved));
|
|
|
|
//reinterleave the frame
|
|
const int *w, *x, *y, *z;
|
|
w = rW; x = rX; y = rY; z = rZ;
|
|
|
|
for (int8_t i = 0; i < 36; i++)
|
|
{
|
|
interleaved[(i*2)+0] = (uint8_t)ambe_fr[*w][*x];
|
|
interleaved[(i*2)+1] = (uint8_t)ambe_fr[*y][*z];
|
|
|
|
w++;
|
|
x++;
|
|
y++;
|
|
z++;
|
|
}
|
|
|
|
uint8_t bytes[9]; memset(bytes, 0, sizeof(bytes));
|
|
|
|
//pack
|
|
pack_bit_array_into_byte_array(interleaved, bytes, 9);
|
|
|
|
if (opts->payload == 1)
|
|
{
|
|
fprintf (stderr, " AMBE HEX(72) INT: ");
|
|
for (int8_t i = 0; i < 9; i++)
|
|
fprintf (stderr, "%02X", bytes[i]);
|
|
fprintf (stderr, "\n");
|
|
}
|
|
|
|
}
|
|
|
|
//de-interleaved code words for AMBE+2
|
|
void ambe2_codeword_print_b (dsd_opts * opts, char ambe_fr[4][24])
|
|
{
|
|
uint8_t fr_reverse[4][24]; memset(fr_reverse, 0, sizeof(fr_reverse));
|
|
for (int i = 0; i < 24; i++)
|
|
fr_reverse[0][i] = ambe_fr[0][23-i];
|
|
for (int i = 0; i < 23; i++)
|
|
fr_reverse[1][i] = ambe_fr[1][22-i];
|
|
for (int i = 0; i < 11; i++)
|
|
fr_reverse[2][i] = ambe_fr[2][10-i];
|
|
for (int i = 0; i < 14; i++)
|
|
fr_reverse[3][i] = ambe_fr[3][13-i];
|
|
|
|
uint32_t v0 = (uint32_t)convert_bits_into_output((uint8_t *)fr_reverse[0], 24); //24
|
|
uint32_t v1 = (uint32_t)convert_bits_into_output((uint8_t *)fr_reverse[1], 23); //23
|
|
uint32_t v2 = (uint32_t)convert_bits_into_output((uint8_t *)fr_reverse[2], 11); //11
|
|
uint32_t v3 = (uint32_t)convert_bits_into_output((uint8_t *)fr_reverse[3], 14); //14
|
|
|
|
uint32_t c0 = (uint32_t)convert_bits_into_output((uint8_t *)fr_reverse[0], 12);
|
|
uint32_t c1 = (uint32_t)convert_bits_into_output((uint8_t *)fr_reverse[1], 12);
|
|
|
|
//72 bit version
|
|
unsigned long long int hex1 = ((unsigned long long int)v0 << 40ULL) + ((unsigned long long int)v1 << 17ULL) + ((unsigned long long int)v2 << 6ULL) + (v3 >> 8);
|
|
unsigned long long int hex2 = v3 & 0xFF;
|
|
|
|
//49 bit version prior to golay correction and c1 demodulation pN
|
|
unsigned long long int hex49 = ((unsigned long long int)c0 << 37ULL) + ((unsigned long long int)c1 << 25ULL) + ((unsigned long long int)v2 << 14ULL) + v3;
|
|
|
|
UNUSED(opts);
|
|
|
|
if (opts->payload == 1)
|
|
{
|
|
fprintf (stderr, " AMBE HEX(72): %016llX%02llX \n", hex1, hex2);
|
|
fprintf (stderr, " AMBE HEX(49): %014llX\n", hex49 << 7);
|
|
}
|
|
|
|
}
|
|
|
|
//de-interleaved code words for AMBE+2
|
|
void ambe2_codeword_print_f (dsd_opts * opts, char ambe_fr[4][24])
|
|
{
|
|
uint32_t v0 = (uint32_t)convert_bits_into_output((uint8_t *)ambe_fr[0], 24); //24
|
|
uint32_t v1 = (uint32_t)convert_bits_into_output((uint8_t *)ambe_fr[1], 23); //23
|
|
uint32_t v2 = (uint32_t)convert_bits_into_output((uint8_t *)ambe_fr[2], 11); //11
|
|
uint32_t v3 = (uint32_t)convert_bits_into_output((uint8_t *)ambe_fr[3], 14); //14
|
|
|
|
// if (opts->payload == 1)
|
|
// fprintf (stderr, " AMBE V0: %06X; V1: %06X; V2: %03X; V3: %04X; \n", v0, v1, v2, v3);
|
|
|
|
unsigned long long int hex1 = ((unsigned long long int)v0 << 40ULL) + ((unsigned long long int)v1 << 17ULL) + ((unsigned long long int)v2 << 6ULL) + (v3 >> 8);
|
|
unsigned long long int hex2 = v3 & 0xFF;
|
|
|
|
if (opts->payload == 1)
|
|
fprintf (stderr, " AMBE HEX(72): %016llX%02llX \n", hex1, hex2);
|
|
|
|
}
|
|
|
|
//NOTE: This mode DOES NOT work over a repeater, simplex only
|
|
//repeaters may or will attempt to correct the frame errors
|
|
void tyt16_ambe2_codeword_keystream(dsd_state * state, char ambe_fr[4][24], int fnum)
|
|
{
|
|
|
|
char interleaved[72];
|
|
memset (interleaved, 0, sizeof(interleaved));
|
|
|
|
//interleave the frame
|
|
const int *w, *x, *y, *z;
|
|
w = rW; x = rX; y = rY; z = rZ;
|
|
|
|
for (int8_t i = 0; i < 36; i++)
|
|
{
|
|
interleaved[(i*2)+0] = ambe_fr[*w][*x];
|
|
interleaved[(i*2)+1] = ambe_fr[*y][*z];
|
|
|
|
w++;
|
|
x++;
|
|
y++;
|
|
z++;
|
|
}
|
|
|
|
uint8_t ks_bytes[10]; memset(ks_bytes, 0, sizeof(ks_bytes));
|
|
uint8_t ks[80]; memset(ks, 0, sizeof(ks));
|
|
|
|
ks_bytes[0] = (state->H >> 8) & 0xFF;
|
|
ks_bytes[1] = (state->H >> 0) & 0xFF;
|
|
|
|
//copy same bytes into rest of byte array
|
|
for (int16_t i = 2; i < 10; i++)
|
|
ks_bytes[i] = ks_bytes[i%2];
|
|
|
|
//convert byte array into a bit array
|
|
unpack_byte_array_into_bit_array(ks_bytes, ks, 10);
|
|
|
|
//set ks idx position (-1)
|
|
int idx = 0;
|
|
if (fnum == 0)
|
|
idx = 79;
|
|
else idx = 71;
|
|
|
|
//apply keystream to interleave
|
|
for (int8_t i = 0; i < 72; i++)
|
|
interleaved[i] ^= ks[idx--];
|
|
|
|
//deinterleave back into ambe_fr frame
|
|
w = rW; x = rX; y = rY; z = rZ;
|
|
int k = 0;
|
|
for (int8_t i = 0; i < 36; i++)
|
|
{
|
|
ambe_fr[*w][*x] = interleaved[k++];
|
|
ambe_fr[*y][*z] = interleaved[k++];
|
|
|
|
w++;
|
|
x++;
|
|
y++;
|
|
z++;
|
|
}
|
|
|
|
}
|
|
|
|
void tyt_ap_pc4_keystream_creation(dsd_state * state, char * input)
|
|
{
|
|
unsigned char key1[16] = {0};
|
|
unsigned char key2[16] = {0};
|
|
|
|
char buf[1024];
|
|
strncpy(buf, input, sizeof(buf) - 1);
|
|
buf[sizeof(buf) - 1] = '\0';
|
|
|
|
char *pEnd;
|
|
uint64_t K1 = strtoull(buf, &pEnd, 16);
|
|
uint64_t K2 = strtoull(pEnd, &pEnd, 16);
|
|
|
|
u64_to_bytes_be(K1, &key1[0]);
|
|
u64_to_bytes_be(K2, &key1[8]);
|
|
|
|
for (int i=0;i<16;i++) key2[i] = key1[15-i];
|
|
|
|
/* Create key schedule */
|
|
create_keys(&ctx, key2, sizeof(key2));
|
|
ctx.rounds = nbround;
|
|
|
|
fprintf (stderr,"DMR TYT AP (PC4) 128-bit Key %016llX%016llX with Forced Application\n", (unsigned long long int)K1, (unsigned long long int)K2);
|
|
state->tyt_ap = 1;
|
|
|
|
}
|
|
|
|
void tyt_ep_aes_keystream_creation(dsd_state * state, char * input)
|
|
{
|
|
char buf[1024];
|
|
strncpy(buf, input, 1023);
|
|
buf[1023] = '\0';
|
|
|
|
char *pEnd;
|
|
unsigned long long int K1 = strtoull (buf, &pEnd, 16);
|
|
unsigned long long int K2 = strtoull (pEnd, &pEnd, 16);
|
|
|
|
//
|
|
uint8_t static_key[32];
|
|
memset(static_key, 0, sizeof(static_key));
|
|
|
|
//static key value
|
|
static_key[0]=0x6e; static_key[1]=0x02; static_key[2]=0x8d; static_key[3]=0x8a;
|
|
static_key[4]=0xca; static_key[5]=0xeb; static_key[6]=0x9b; static_key[7]=0xbe;
|
|
static_key[8]=0x42; static_key[9]=0x72; static_key[10]=0xfb; static_key[11]=0x82;
|
|
static_key[12]=0x64; static_key[13]=0x56; static_key[14]=0x31; static_key[15]=0xfa;
|
|
|
|
//the key value provided by user
|
|
uint8_t user_key[16];
|
|
memset(user_key, 0, sizeof(user_key));
|
|
|
|
//Load user key into array to manipulate
|
|
for (int i = 0; i < 8; i++)
|
|
{
|
|
user_key[i+0] = (K1 >> (56-(i*8))) & 0xFF;
|
|
user_key[i+8] = (K2 >> (56-(i*8))) & 0xFF;
|
|
}
|
|
|
|
uint8_t input_register[16];
|
|
memset(input_register, 0, sizeof(input_register));
|
|
|
|
//manipulate user provided key by loading bytes in reverse order into the input_register
|
|
for (int i = 0; i < 16; i++)
|
|
input_register[15-i] = user_key[i];
|
|
|
|
uint8_t ks_bytes[16];
|
|
memset(ks_bytes, 0, sizeof(ks_bytes));
|
|
|
|
//create keystream
|
|
aes_ofb_keystream_output(input_register, static_key, ks_bytes, 0, 1);
|
|
uint8_t ks_bits[128];
|
|
memset(ks_bits, 0, sizeof(ks_bits));
|
|
unpack_byte_array_into_bit_array(ks_bytes, ks_bits, 16);
|
|
|
|
//load static keystream into ctx.bits since that isn't ever zeroed out
|
|
for (int i = 0; i < 49; i++)
|
|
ctx.bits[i] = ks_bits[i];
|
|
|
|
fprintf (stderr,"DMR TYT EP (AES-128) Key %016llX%016llX with Forced Application\n", K1, K2);
|
|
state->tyt_ep = 1;
|
|
|
|
} |