MSC16_MSC26.c

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
uint8_t reverse_8(uint8_t d);
uint16_t crc16(uint8_t *addr, size_t addr_length, uint8_t *data, size_t data_length);
void whitening_init(uint8_t val, int *ctx, size_t ctx_length);
void whitening_encode(uint8_t *data, size_t length, int *ctx);
void get_rf_payload16(uint8_t *addr, size_t addr_length, uint8_t *data, size_t data_length, uint8_t *output);
void get_rf_payload26(uint8_t *addr, size_t addr_length, uint8_t *data, size_t data_length, uint8_t *output);
void aes16Encrypt16(unsigned char *mac, unsigned char *uuid, unsigned char groupId, unsigned char cmd, unsigned char sn, unsigned char *data,
                    unsigned char *outputData);
// Reverse byte
uint8_t reverse_8(uint8_t d)
{
    uint8_t result = 0;
    for (int k = 0; k < 8; k++)
    {
        result |= ((d >> k) & 1) << (7 - k);
    }
    return result;
}

// Reverse 16-bit number
uint16_t reverse_16(uint16_t d)
{
    uint16_t result = 0;
    for (int k = 0; k < 16; k++)
    {
        result |= ((d >> k) & 1) << (15 - k);
    }
    return result;
}

// CRC-16 calculation
uint16_t crc16(uint8_t *addr, size_t addr_length, uint8_t *data, size_t data_length)
{
    uint16_t crc = 0xFFFF;

    for (int i = addr_length - 1; i >= 0; i--)
    {
        crc ^= addr[i] << 8;
        for (int j = 0; j < 8; j++)
        {
            uint16_t tmp = crc << 1;
            if (crc & 0x8000)
                tmp ^= 0x1021;
            crc = tmp;
        }
    }

    for (size_t i = 0; i < data_length; i++)
    {
        crc ^= reverse_8(data[i]) << 8;
        for (int j = 0; j < 8; j++)
        {
            uint16_t tmp = crc << 1;
            if (crc & 0x8000)
                tmp ^= 0x1021;
            crc = tmp;
        }
    }

    return (~reverse_16(crc)) & 0xFFFF;
}

// Whitening initialization
void whitening_init(uint8_t val, int *ctx, size_t ctx_length)
{
    uint8_t v0[] = {(val >> 5) & 1, (val >> 4) & 1, (val >> 3) & 1, (val >> 2) & 1};
    ctx[0] = 1;
    ctx[1] = v0[0];
    ctx[2] = v0[1];
    ctx[3] = v0[2];
    ctx[4] = v0[3];
    ctx[5] = (val >> 1) & 1;
    ctx[6] = val & 1;
}

// Whitening encoding
void whitening_encode(uint8_t *data, size_t length, int *ctx)
{
    for (size_t i = 0; i < length; i++)
    {
        uint8_t varC = ctx[3];
        uint8_t var14 = ctx[5];
        uint8_t var18 = ctx[6];
        uint8_t var10 = ctx[4];
        uint8_t var8 = var14 ^ ctx[2];
        uint8_t var4 = var10 ^ ctx[1];
        uint8_t _var = var18 ^ varC;
        uint8_t var0 = _var ^ ctx[0];

        uint8_t c = data[i];
        data[i] = ((c & 0x80) ^ ((var8 ^ var18) << 7)) |
                  ((c & 0x40) ^ (var0 << 6)) |
                  ((c & 0x20) ^ (var4 << 5)) |
                  ((c & 0x10) ^ (var8 << 4)) |
                  ((c & 0x08) ^ (_var << 3)) |
                  ((c & 0x04) ^ (var10 << 2)) |
                  ((c & 0x02) ^ (var14 << 1)) |
                  ((c & 0x01) ^ (var18));

        // Update context
        ctx[2] = var4;
        ctx[3] = var8;
        ctx[4] = var8 ^ varC;
        ctx[5] = var0 ^ var10;
        ctx[6] = var4 ^ var14;
        ctx[0] = var8 ^ var18;
        ctx[1] = var0;
    }
}

// Adjusted to include length parameters for addr and data arrays
void get_rf_payload16(uint8_t *addr, size_t addr_length, uint8_t *data, size_t data_length, uint8_t *output)
{
    const size_t data_offset = 0x12;
    const size_t inverse_offset = 0x0F;
    const size_t result_data_size = data_offset + addr_length + data_length;
    uint8_t *resultbuf = (uint8_t *)calloc(result_data_size + 2, sizeof(uint8_t));

    // Hardcoded values
    resultbuf[0x0F] = 0x71;
    resultbuf[0x10] = 0x0F;
    resultbuf[0x11] = 0x55;

    // Reverse copy the address
    for (size_t i = 0; i < addr_length; i++)
    {
        resultbuf[data_offset + addr_length - i - 1] = addr[i];
    }

    // Copy data
    memcpy(&resultbuf[data_offset + addr_length], data, data_length);

    // Reverse certain bytes
    for (size_t i = inverse_offset; i < inverse_offset + addr_length + 3; i++)
    {
        resultbuf[i] = reverse_8(resultbuf[i]);
    }

    // CRC
    uint16_t crc = crc16(addr, addr_length, data, data_length);
    resultbuf[result_data_size] = crc & 0xFF;
    resultbuf[result_data_size + 1] = (crc >> 8) & 0xFF;

    // Output for verification
    printf("resultbuf: ");
    for (size_t i = 0; i < result_data_size + 2; i++)
    {
        printf("%02x ", resultbuf[i]);
    }
    printf("\n");

    // Whitening
    int whiteningContext[7];
    whitening_init(0x25, whiteningContext, 7);
    int whiteningBLE[7];
    whitening_init(0x3F, whiteningBLE, 7);

    whitening_encode(&resultbuf[data_offset], result_data_size + 2 - data_offset, whiteningBLE);

    // Output for verification
    printf("whiteningBLE: ");
    for (size_t i = 0; i < result_data_size + 2 - data_offset; i++)
    {
        printf("%02x ", resultbuf[data_offset + i]);
    }
    printf("\n");

    whitening_encode(&resultbuf[2], result_data_size + 2 - 2, whiteningContext);
    // Output for verification
    // Output for verification
    printf("whiteningContext: ");
    for (size_t i = 0; i < result_data_size; i++)
    {
        printf("%02x ", resultbuf[2 + i]);
    }
    printf("\n");
    // Returning the last 16 bytes of the payload
    memcpy(output, &resultbuf[result_data_size - 16 + 2], 16);

    free(resultbuf);
}
void get_rf_payload26(uint8_t *addr, size_t addr_length, uint8_t *data, size_t data_length, uint8_t *output)
{
    const size_t data_offset = 0x12;
    const size_t inverse_offset = 0x0F;
    const size_t result_data_size = data_offset + addr_length + data_length;
    uint8_t *resultbuf = (uint8_t *)calloc(result_data_size + 2, sizeof(uint8_t));

    // Hardcoded values
    resultbuf[0x0F] = 0x71;
    resultbuf[0x10] = 0x0F;
    resultbuf[0x11] = 0x55;

    // Reverse copy the address
    for (size_t i = 0; i < addr_length; i++)
    {
        resultbuf[data_offset + addr_length - i - 1] = addr[i];
    }

    // Copy data
    memcpy(&resultbuf[data_offset + addr_length], data, data_length);

    // Reverse certain bytes
    for (size_t i = inverse_offset; i < inverse_offset + addr_length + 3; i++)
    {
        resultbuf[i] = reverse_8(resultbuf[i]);
    }

    // CRC
    uint16_t crc = crc16(addr, addr_length, data, data_length);
    resultbuf[result_data_size] = crc & 0xFF;
    resultbuf[result_data_size + 1] = (crc >> 8) & 0xFF;

    // Whitening
    int whiteningContext[7];
    whitening_init(0x25, whiteningContext, 7);

    whitening_encode(&resultbuf[2], result_data_size + 2 - 2, whiteningContext);

    // Returning the last 26 bytes of the payload
    // uint8_t* final_payload = (uint8_t*)malloc(26 * sizeof(uint8_t));
    memcpy(output, &resultbuf[result_data_size - 24], 26);

    free(resultbuf);
}

int test()
{

    // Test inputs
    uint8_t addr[] = {0xaa, 0x55, 0xcc};
    size_t addr_length = sizeof(addr) / sizeof(addr[0]);

    uint8_t RF_payload[] = {0xc0, 0x1d, 0xe2, 0x1d, 0xa8, 0x15, 0xdd, 0x1d};
    size_t RF_payload_length = sizeof(RF_payload) / sizeof(RF_payload[0]);

    // Expected output
    const uint8_t expected_output[] = {0xf9, 0x08, 0x49, 0xb2, 0xce, 0x2c, 0x47, 0x22, 0x8c, 0x89, 0x16, 0x1f, 0x30, 0x24, 0x58, 0x1d};
    const size_t expected_output_length = sizeof(expected_output) / sizeof(expected_output[0]);
    uint8_t output[expected_output_length];
    // Call the function
    get_rf_payload16(addr, addr_length, RF_payload, RF_payload_length, output);

    // Check the output
    int correct = 1;
    for (size_t i = 0; i < expected_output_length; i++)
    {
        if (output[i] != expected_output[i])
        {
            correct = 0;
            break;
        }
    }

    // Print result
    if (correct)
    {
        printf("Test passed.\n");
    }
    else
    {
        printf("Test failed.\n");
    }

    // Output for verification
    printf("Output: ");
    for (size_t i = 0; i < expected_output_length; i++)
    {
        printf("%02x ", output[i]);
    }
    printf("\n");

    // Test values
    uint8_t addr26[] = {0x20, 0x20, 0x03, 0x05};
    size_t addr_length26 = sizeof(addr26) / sizeof(addr26[0]);

    uint8_t RF_payload26[] = {0x43, 0x26, 0x9e, 0x43, 0x49, 0x43, 0x3c, 0x63, 0x43, 0xee, 0x40, 0x43, 0x96, 0x4c, 0x63, 0xa0, 0x43};
    size_t RF_payload_length26 = sizeof(RF_payload26) / sizeof(RF_payload26[0]);

    // Expected output
    const uint8_t expected_output26[] = {249, 8, 73, 230, 41, 175, 212, 221, 117, 173, 155, 243, 219, 52, 71, 136, 213, 188, 50, 53, 184, 54, 200, 140, 86, 17};
    const size_t expected_output_length26 = sizeof(expected_output26) / sizeof(expected_output26[0]);
    uint8_t output26[expected_output_length26];
    // Call the function
    get_rf_payload26(addr26, addr_length26, RF_payload26, RF_payload_length26, output26);

    // Check the output
    correct = 1;
    for (size_t i = 0; i < expected_output_length26; i++)
    {
        if (output26[i] != expected_output26[i])
        {
            correct = 0;
            break;
        }
    }

    // Print result
    if (correct)
    {
        printf("Test26 passed.\n");
    }
    else
    {
        printf("Test26 failed.\n");
    }
    printf("Output: ");
    for (size_t i = 0; i < expected_output_length26; i++)
    {
        printf("%02x ", output26[i]);
    }

    printf("\n");

    return 0;
}
void test_aes_encrypt()
{
    unsigned char mac[3] = {0xAA, 0x55, 0xCC};
    unsigned char uuid[2] = {0xDD, 0x0B};
    unsigned char groupId = 255;
    unsigned char cmd = 181;
    unsigned char sn = 33; // Sequnce number 0-255
    unsigned char data[3] = {0x00, 0x00, 0x00};
    unsigned char outputData[16];

    aes16Encrypt16(mac, uuid, groupId, cmd, sn, data, outputData);
    // c4 19 e6 19 ac 11 dd 19
    const uint8_t expected_output[] = {0xf9, 0x08, 0x49, 0xb2, 0xce, 0x2c, 0x7b, 0x1e, 0xb0, 0xb5, 0x2a, 0x23, 0x30, 0x18, 0x83, 0xfa};
    int correct = 1;
    for (size_t i = 0; i < 16; i++)
    {
        if (outputData[i] != expected_output[i])
        {
            correct = 0;
            break;
        }
    }

    // Print result
    if (correct)
    {
        printf("Test passed. AES16\n");
    }
    else
    {
        printf("Test failed.\n");
    }

    // Output for verification
    printf("Output: ");
    for (size_t i = 0; i < 16; i++)
    {
        printf("%02x ", outputData[i]);
    }
    printf("\n");
    printf("\n");
}

void aes16Encrypt16(unsigned char *mac, unsigned char *uuid, unsigned char groupId, unsigned char cmd, unsigned char sn, unsigned char *data,
                    unsigned char *outputData)
{
    unsigned char key;
    unsigned char txData[8];

    txData[5] = data[2] ^ sn;
    txData[6] = data[2] ^ *uuid;
    txData[7] = *data ^ sn;
    txData[0] = txData[5] ^ *uuid;
    txData[1] = txData[5] ^ *data;
    txData[2] = txData[5] ^ groupId;
    txData[3] = txData[5] ^ data[1];
    txData[4] = txData[5] ^ cmd;
    txData[5] = (txData[5] ^ uuid[1]) - 1;
    printf("DATA: ");
    for (size_t i = 0; i < 8; i++)
    {
        printf("%02x ", txData[i]);
    }
    printf("\n");
    get_rf_payload16(mac, 3, txData, 8, outputData);

    printf("\n");
}
void aes26Encrypt(uint8_t *mac, uint8_t *uuid, uint8_t *uid, uint8_t groupId, uint8_t cmd, uint8_t sn, uint8_t *data,
                  uint8_t *outputData)

{
    uint8_t bVar1;
    uint8_t bVar2;
    uint8_t bVar3;
    uint8_t key3;
    uint8_t key4;
    uint8_t txData[17];

    bVar1 = uuid[1];
    bVar2 = uid[2];
    bVar3 = uuid[2];
    txData[8] = bVar2 ^ bVar1 ^ bVar3;
    txData[8] = (txData[8] & 1) - 1 ^ txData[8];
    txData[0] = txData[8] ^ *data;
    txData[2] = txData[8] ^ *uuid;
    txData[3] = txData[8] ^ data[1];
    txData[4] = txData[8] ^ sn;
    txData[12] = bVar1 ^ txData[2];
    txData[13] = bVar2 ^ txData[4];
    txData[5] = txData[8] ^ data[2];
    txData[6] = txData[8] ^ groupId;
    txData[9] = txData[8] ^ cmd;
    txData[7] = txData[8] ^ *uid;
    txData[10] = txData[8] ^ uid[1];
    txData[15] = bVar3 ^ txData[9];
    txData[1] = txData[8] ^
                *data ^ *uuid ^ data[1] ^ sn ^ data[2] ^ groupId ^ *uid ^ cmd ^ uid[1] ^ bVar1 ^ bVar2 ^ bVar3;
    txData[11] = txData[8];
    txData[14] = txData[7];
    txData[16] = txData[8];
    printf("DATA: ");
    for (size_t i = 0; i < 17; i++)
    {
        printf("%02x ", txData[i]);
    }
    printf("\n");
    get_rf_payload26(mac, 4, &txData[0], 0x11, outputData);

    /* WARNING: Subroutine does not return */
}
void test_aes26_encrypt() {
    // Test values
    uint8_t mac[] = {0x20, 0x20, 0x03, 0x05};
    uint8_t uuid[] = {0xdd, 0x08, 0x4e};
    uint8_t uid[] = {0x20, 0x03, 0x05};
    uint8_t groupId = 127;
    uint8_t cmd = 173;
    uint8_t sn = 14;
    uint8_t data[] = {0x00, 0x00, 0x00};
    uint8_t outputData[26]; // To hold the output

    // Expected output for verification
    uint8_t expected_output[] = {0xf9, 0x08, 0x49, 0xe6, 0x29, 0xaf, 0xd4, 0xdd, 0x71, 0xad, 0x9b, 0xf7, 0xdb, 0x34, 0x47, 0x88, 0xd5, 0xbc, 0x32, 0x35, 0xbc, 0x36, 0xc8, 0x8c, 0x6c, 0x5d};

    // Call the function to be tested
    aes26Encrypt(mac, uuid, uid, groupId, cmd, sn, data, outputData);

    // Compare outputData with expected_output
    if (memcmp(outputData, expected_output, sizeof(expected_output)) == 0) {
        printf("Test passed. AES26\n");
    } else {
        printf("Test failed.\n");

        // For detailed debugging
        printf("Output Data:     ");
        for (int i = 0; i < sizeof(outputData); ++i) {
            printf("%02x ", outputData[i]);
        }
        printf("\nExpected Output: ");
        for (int i = 0; i < sizeof(expected_output); ++i) {
            printf("%02x ", expected_output[i]);
        }
        printf("\n");
    }


}














//*
int main()
{
    test();
    test_aes_encrypt();
    test_aes26_encrypt();
    return 0;
}
*/