mirror of
https://github.com/OneOfEleven/uv-k5-firmware-custom.git
synced 2025-04-28 14:21:25 +03:00
284 lines
6.5 KiB
C
284 lines
6.5 KiB
C
|
|
#include <string.h>
|
|
|
|
#include "bsp/dp32g030/crc.h"
|
|
#include "mdc1200.h"
|
|
#include "misc.h"
|
|
|
|
// MDC1200 sync bit reversals and packet magic
|
|
//
|
|
// 24-bit pre-amble
|
|
// 40-bit sync
|
|
//
|
|
//static const uint8_t header[] = {0x00, 0x00, 0x05, 0x55, 0x55, 0x55, 0x55, 0x07, 0x09, 0x2a, 0x44, 0x6f};
|
|
//static const uint8_t header[] = {0x00, 0x00, 0x0A, 0xAA, 0xAA, 0xAA, 0xAA, 0x07, 0x09, 0x2a, 0x44, 0x6f};
|
|
//
|
|
//static const uint8_t header[] = {0x00, 0x00, 0x0A, 0xAA, 0xAA, 0xAA, 0xA0, 0xb6, 0x8e, 0x03, 0xbb, 0x14};
|
|
static const uint8_t header[] = {0x00, 0x00, 0x05, 0x55, 0x55, 0x55, 0x50, 0x29, 0x71, 0xfc, 0x44, 0xeb};
|
|
|
|
uint8_t bit_reverse_8(uint8_t n)
|
|
{
|
|
n = ((n >> 1) & 0x55u) | ((n << 1) & 0xAAu);
|
|
n = ((n >> 2) & 0x33u) | ((n << 2) & 0xCCu);
|
|
n = ((n >> 4) & 0x0Fu) | ((n << 4) & 0xF0u);
|
|
return n;
|
|
}
|
|
|
|
uint16_t bit_reverse_16(uint16_t n)
|
|
{ // untested
|
|
n = ((n >> 1) & 0x5555u) | ((n << 1) & 0xAAAAu);
|
|
n = ((n >> 2) & 0x3333u) | ((n << 2) & 0xCCCCu);
|
|
n = ((n >> 4) & 0x0F0Fu) | ((n << 4) & 0xF0F0u);
|
|
n = ((n >> 8) & 0x00FFu) | ((n << 8) & 0xFF00u);
|
|
return n;
|
|
}
|
|
|
|
uint32_t bit_reverse_32(uint32_t n)
|
|
{
|
|
n = ((n >> 1) & 0x55555555u) | ((n << 1) & 0xAAAAAAAAu);
|
|
n = ((n >> 2) & 0x33333333u) | ((n << 2) & 0xCCCCCCCCu);
|
|
n = ((n >> 4) & 0x0F0F0F0Fu) | ((n << 4) & 0xF0F0F0F0u);
|
|
n = ((n >> 8) & 0x00FF00FFu) | ((n << 8) & 0xFF00FF00u);
|
|
n = ((n >> 16) & 0x0000FFFFu) | ((n << 16) & 0xFFFF0000u);
|
|
return n;
|
|
}
|
|
|
|
uint16_t reverse_bits(const uint16_t bits_in, const unsigned int num_bits)
|
|
{
|
|
uint16_t i;
|
|
uint16_t bit;
|
|
uint16_t bits_out;
|
|
for (i = 1u << (num_bits - 1), bit = 1u, bits_out = 0u; i != 0; i >>= 1)
|
|
{
|
|
if (bits_in & i)
|
|
bits_out |= bit;
|
|
bit <<= 1;
|
|
}
|
|
return bits_out;
|
|
}
|
|
|
|
#if 0
|
|
uint16_t compute_crc(const uint8_t *data, const unsigned int data_len)
|
|
{ // using the reverse computation avoids having to reverse the bit order during and after
|
|
uint16_t crc = 0;
|
|
for (i = 0; i < len; i++)
|
|
{
|
|
unsigned int k;
|
|
crc ^= data[i];
|
|
for (k = 8; k > 0; k--)
|
|
crc = (crc & 1u) ? (crc >> 1) ^ 0x8408 : crc >> 1;
|
|
}
|
|
crc ^= 0xffff;
|
|
return crc;
|
|
}
|
|
#else
|
|
uint16_t compute_crc(const uint8_t *data, const unsigned int data_len)
|
|
{
|
|
|
|
// this can be done using the CPU's own CRC calculator once we know we're ok
|
|
|
|
unsigned int i;
|
|
|
|
#if 0
|
|
uint16_t crc;
|
|
|
|
CRC_CR = (CRC_CR & ~CRC_CR_CRC_EN_MASK) | CRC_CR_CRC_EN_BITS_ENABLE;
|
|
#else
|
|
uint16_t crc = 0;
|
|
#endif
|
|
|
|
for (i = 0; i < data_len; i++)
|
|
{
|
|
#if 0
|
|
|
|
// bit reverse each data byte before adding it to the CRC
|
|
// the cortex CPU might have an instruction to bit reverse for us ?
|
|
//
|
|
CRC_DATAIN = reverse_bits(data[i], 8);
|
|
//CRC_DATAIN = bit_reverse_8(data[i]);
|
|
|
|
#else
|
|
uint8_t mask;
|
|
|
|
// bit reverse each data byte before adding it to the CRC
|
|
// the cortex CPU might have an instruction to bit reverse for us ?
|
|
//
|
|
const uint8_t bits = reverse_bits(data[i], 8);
|
|
//const uint8_t bits = bit_reverse_8(*data++);
|
|
|
|
for (mask = 0x0080; mask != 0; mask >>= 1)
|
|
{
|
|
uint16_t msb = crc & 0x8000;
|
|
if (bits & mask)
|
|
msb ^= 0x8000;
|
|
crc <<= 1;
|
|
if (msb)
|
|
crc ^= 0x1021;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#if 0
|
|
crc = (uint16_t)CRC_DATAOUT;
|
|
CRC_CR = (CRC_CR & ~CRC_CR_CRC_EN_MASK) | CRC_CR_CRC_EN_BITS_DISABLE;
|
|
#endif
|
|
|
|
// bit reverse and invert the final CRC
|
|
return reverse_bits(crc, 16) ^ 0xffff;
|
|
// return bit_reverse_16(crc) ^ 0xffff;
|
|
}
|
|
#endif
|
|
|
|
uint8_t * encode_data(uint8_t *data)
|
|
{
|
|
unsigned int i;
|
|
unsigned int k;
|
|
unsigned int m;
|
|
uint8_t csr[7];
|
|
uint8_t lbits[(ARRAY_SIZE(csr) * 2) * 8];
|
|
|
|
for (i = 0; i < ARRAY_SIZE(csr); i++)
|
|
csr[i] = 0;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(csr); i++)
|
|
{
|
|
unsigned int bit;
|
|
data[i + ARRAY_SIZE(csr)] = 0;
|
|
for (bit = 0; bit < 8; bit++)
|
|
{
|
|
uint8_t b;
|
|
for (k = 6; k > 0; k--)
|
|
csr[k] = csr[k - 1];
|
|
csr[0] = (data[i] >> bit) & 1u;
|
|
b = csr[0] + csr[2] + csr[5] + csr[6];
|
|
data[i + ARRAY_SIZE(csr)] |= (b & 1u) << bit;
|
|
}
|
|
}
|
|
|
|
for (i = 0, k = 0, m = 0; i < (ARRAY_SIZE(csr) * 2); i++)
|
|
{
|
|
unsigned int bit;
|
|
for (bit = 0; bit < 8; bit++)
|
|
{
|
|
lbits[k] = (data[i] >> bit) & 1u;
|
|
k += 16;
|
|
if (k >= ARRAY_SIZE(lbits))
|
|
k = ++m;
|
|
}
|
|
}
|
|
|
|
for (i = 0, k = 0; i < (ARRAY_SIZE(csr) * 2); i++)
|
|
{
|
|
int bit;
|
|
data[i] = 0;
|
|
for (bit = 7; bit >= 0; bit--)
|
|
if (lbits[k++])
|
|
data[i] |= 1u << bit;
|
|
}
|
|
|
|
return data + 14;
|
|
}
|
|
|
|
void delta_modulation(uint8_t *data, const unsigned int size)
|
|
{ // xor succesive bits in the entire packet, including the bit reversing pre-amble
|
|
uint8_t b1;
|
|
unsigned int i;
|
|
for (i = 0, b1 = 1u; i < size; i++)
|
|
{
|
|
int bit_num;
|
|
uint8_t in = data[i];
|
|
uint8_t out = 0;
|
|
for (bit_num = 7; bit_num >= 0; bit_num--)
|
|
{
|
|
const uint8_t b2 = (in >> bit_num) & 1u;
|
|
if (b1 != b2)
|
|
out |= 1u << bit_num; // previous bit and new bit are different
|
|
b1 = b2;
|
|
}
|
|
data[i] = out;
|
|
}
|
|
}
|
|
|
|
unsigned int MDC1200_encode_single_packet(uint8_t *data, const uint8_t op, const uint8_t arg, const uint16_t unit_id)
|
|
{
|
|
unsigned int size;
|
|
uint16_t crc;
|
|
uint8_t *p = data;
|
|
|
|
memcpy(p, header, sizeof(header));
|
|
p += sizeof(header);
|
|
|
|
p[0] = op;
|
|
p[1] = arg;
|
|
p[2] = (unit_id >> 8) & 0x00ff;
|
|
p[3] = (unit_id >> 0) & 0x00ff;
|
|
crc = compute_crc(p, 4);
|
|
p[4] = (crc >> 0) & 0x00ff;
|
|
p[5] = (crc >> 8) & 0x00ff;
|
|
p[6] = 0;
|
|
|
|
p = encode_data(p);
|
|
|
|
#if 1
|
|
{ // op 0x01, arg 0x80, id 0xB183
|
|
const uint8_t test_packet[] = {0x07, 0x25, 0xDD, 0xD5, 0x9F, 0xC5, 0x3D, 0x89, 0x2D, 0xBD, 0x57, 0x35, 0xE7, 0x44};
|
|
memcpy(data + sizeof(header), test_packet, sizeof(test_packet));
|
|
}
|
|
#endif
|
|
|
|
size = (unsigned int)(p - data);
|
|
|
|
delta_modulation(data, size);
|
|
|
|
return size;
|
|
// return 26;
|
|
}
|
|
|
|
unsigned int MDC1200_encode_double_packet(uint8_t *data, const uint8_t op, const uint8_t arg, const uint16_t unit_id, const uint8_t b0, const uint8_t b1, const uint8_t b2, const uint8_t b3)
|
|
{
|
|
unsigned int size;
|
|
uint16_t crc;
|
|
uint8_t *p = data;
|
|
|
|
memcpy(p, header, sizeof(header));
|
|
p += sizeof(header);
|
|
|
|
p[0] = op;
|
|
p[1] = arg;
|
|
p[2] = (unit_id >> 8) & 0x00ff;
|
|
p[3] = (unit_id >> 0) & 0x00ff;
|
|
crc = compute_crc(p, 4);
|
|
p[4] = (crc >> 0) & 0x00ff;
|
|
p[5] = (crc >> 8) & 0x00ff;
|
|
p[6] = 0;
|
|
|
|
p = encode_data(p);
|
|
|
|
p[0] = b0;
|
|
p[1] = b1;
|
|
p[2] = b2;
|
|
p[3] = b3;
|
|
crc = compute_crc(p, 4);
|
|
p[4] = (crc >> 0) & 0x00ff;
|
|
p[5] = (crc >> 8) & 0x00ff;
|
|
p[6] = 0;
|
|
|
|
p = encode_data(p);
|
|
|
|
size = (unsigned int)(p - data);
|
|
|
|
delta_modulation(data, size);
|
|
|
|
return size;
|
|
// return 40;
|
|
}
|
|
/*
|
|
void test(void)
|
|
{
|
|
uint8_t data[14 + 14 + 5 + 7];
|
|
|
|
const int size = MDC1200_encode_single_packet(data, 0x12, 0x34, 0x5678);
|
|
|
|
// const int size = MDC1200_encode_double_packet(data, 0x55, 0x34, 0x5678, 0x0a, 0x0b, 0x0c, 0x0d);
|
|
}
|
|
*/ |