HackRF-Treasure-Chest/Software/Universal Radio Hacker/data/decodings/homematic_complete.c
2022-09-22 13:46:47 -07:00

240 lines
7.1 KiB
C

#include <stdio.h>
#include <string.h>
#define SHORTEN_PREAMBLE_TO_32 0
typedef unsigned char byte;
typedef unsigned short uint16;
uint16 crc(byte *data, int len)
{
byte i, x, crcdata;
uint16 crcReg = 0xFFFF;
for (x = 0; x < len; x++)
{
crcdata = data[x];
for (i = 0; i < 8; i++)
{
if (((crcReg & 0x8000) >> 8) ^ (crcdata & 0x80))
crcReg = (crcReg << 1) ^ 0x8005;
else
crcReg = (crcReg << 1);
crcdata <<= 1;
}
}
return crcReg;
}
byte str2byte(char str[8])
{
int ret = 0, i;
for(i = 0; i < 8; i++)
if (str[i]=='1') ret |= (1<<(7-i));
return ret;
}
void print_binary(byte inpt)
{
int i;
for(i = 0; i < 8; i++)
if (inpt & (1<<(7-i))) putchar('1');
else putchar('0');
}
void print_preamble_nibbles(int count)
{
int i;
for (i = 0; i < count; i++)
printf("1010");
}
int find_preamble_start_in_bit(char *string, int len)
{
char homematic_sync[] = "11101001110010101110100111001010";
for(int i = 0, j = 0; i < len; i++)
{
if(string[i] == homematic_sync[j])
{
j++;
if(j == 32 && i>= 63) return i-63;
}
else j = 0;
}
return -1; //not found
}
void xor_lfsr(char *string)
{
int i, j, x, len = strlen(string);
byte polynomial[9] = {0, 0, 0, 1, 0, 0, 0, 0, 1};
byte lfsr_state[9];
byte first_bit;
// Init with 8x 1 Bit
memset(lfsr_state, 1, 9);
for(j = 0; j < 8; j++)
if(string[j]=='1')
string[j]='0';
else
string[j]='1';
for(x = 8; x < len-7; x += 8)
{
for(i = 0; i < 8; i++)
{
first_bit = 255;
for(j = 8; j >= 0; j--)
if(polynomial[j] && lfsr_state[j])
{
if(first_bit == 255)
first_bit = 1;
else
first_bit = (first_bit==1) ? 0 : 1;
}
if(first_bit == 255)
first_bit = 0;
// Clock
for(j = 8; j >= 0; j--)
lfsr_state[j] = lfsr_state[j-1];
lfsr_state[0] = first_bit;
}
// Xor
for(j = 0; j < 8; j++)
if(lfsr_state[j+1] == 1)
if(string[x+j]=='1')
string[x+j]='0';
else
string[x+j]='1';
}
}
int main(int argc, char **argv)
{
int i, j, max, offset, len, preamble_additional_length;
byte dec[1024]={0}, enc[1024]={0}, crc_ok;
char string[65536]={0};
uint16 crcvalue;
offset = 8; // Preamble + Sync
// Copy data (argv[2]) to string if length is ok, shorten to multiple of 8 bit
if (strlen(argv[2]) > 8192*8 || strlen(argv[2]) < 4) return -1;
len = strlen(argv[2]);
i = find_preamble_start_in_bit(argv[2], len);
if(i < 0) return 0; // preamble+sync not found or wrong length
preamble_additional_length = i;
len = (len-i)-(len-i)%8;
memcpy(string, argv[2]+i, len);
if (argc>2)
{
if (argv[1][0]=='d')
{
// Apply datawhitening
xor_lfsr(string+64);
// Pack to bytes
for (i = 0; i < strlen(string)-3; i+=8)
enc[i/8] = str2byte(&string[i]);
max = i/8;
memcpy(&dec, &enc, 1024);
// Check CRC
crcvalue = crc(&dec[8], max-2-8);
crc_ok = 0;
if( ((crcvalue >> 8) & 0xFF) == dec[max-2] && (crcvalue & 0xFF) == dec[max-1]) crc_ok = 1;
/*
* byte[] Dec = new byte[Enc.Length];
* Dec[0] = Enc[0]; //Packet length
* Dec[1] = (byte)((~Enc[1]) ^ 0x89);
* int j;
* for (j = 2; j < Dec[0]; j++)
* Dec[j] = (byte)((Enc[j-1] + 0xdc) ^ Enc[j]);
* Dec[j] = (byte)(Enc[j] ^ Dec[2]);
*/
// Decrypt
dec[offset+0] = enc[offset+0];
dec[offset+1] = (~enc[offset+1])^0x89;
for(i = offset + 2; i < max - 3; i++)
dec[i] = (enc[i-1]+0xdc) ^ enc[i];
dec[i] = enc[i] ^ dec[offset+2];
// Recompute CRC and overwrite with FAKE-CRC, if CRC was OK before
if(crc_ok)
{
crcvalue = crc(&dec[8], max-2-8);
dec[max-1] = crcvalue & 0xFF;
dec[max-2] = (crcvalue >> 8) & 0xFF;
}
else
{
dec[max-1] = 0x0F; // Set magic code for wrong CRC
dec[max-2] = 0xD0;
}
// Prepend preamble longer than 32 bits
if(0 == SHORTEN_PREAMBLE_TO_32 && preamble_additional_length > 0)
print_preamble_nibbles(preamble_additional_length/4);
for(i = 0; i < max; i++)
print_binary(dec[i]);
}
else
{
// Pack to bytes
for (i = 0; i < strlen(string)-3; i+=8)
dec[i/8] = str2byte(&string[i]);
max = i/8;
memcpy(&enc, &dec, 1024);
/*
* byte[] Dec = new byte[Enc.Length];
* Dec[0] = Enc[0]; //Packet length
* Dec[1] = (byte)((~Enc[1]) ^ 0x89);
* int j;
* for (j = 2; j < Dec[0]; j++)
* Dec[j] = (byte)((Enc[j-1] + 0xdc) ^ Enc[j]);
* Dec[j] = (byte)(Enc[j] ^ Dec[2]);
*/
// Encrypt
enc[offset+0] = dec[offset+0];
enc[offset+1] = ~(dec[offset+1])^0x89;
for(i = offset + 2; i < max - 3; i++)
enc[i] = (enc[i-1]+0xdc) ^ dec[i];
enc[i] = dec[i] ^ dec[offset+2];
// Overwrite with correct CRC
crcvalue = crc(&enc[8], max-2-8);
enc[max-1] = crcvalue & 0xFF;
enc[max-2] = (crcvalue >> 8) & 0xFF;
// Convert to string
memset(string, 0, 65536);
for(i = 0; i < max; i++)
{
for(j = 0; j < 8; j++)
if(enc[i] & (1<<(7-j))) string[i*8+j]='1';
else string[i*8+j]='0';
}
// Apply datawhitening
xor_lfsr(string+64);
// Prepend preamble longer than 32 bits
if(0 == SHORTEN_PREAMBLE_TO_32 && preamble_additional_length > 0) // Add preamble longer than 32 bits
print_preamble_nibbles(preamble_additional_length/4);
// Print bits and duplicate last bit
printf("%s%c\n", string, string[strlen(string)-1]);
}
}
else printf("Usage: %s <d/e> <bit sequence>\n\td - decode\n\te - encode\n\tbit sequence as string of 0 and 1.\n", argv[0]);
return 0;
}