refactor: crc calculation
this one's a copilot special, but looks good to me
This commit is contained in:
parent
7fbd335017
commit
009a4d1cf5
SSH Key Fingerprint:
SHA256:mXpyZkK7RDiJ7qeHCKJX108woM0cl5TrCvNBJASu6lM
|
|
@ -48,208 +48,87 @@ static HAL_StatusTypeDef mcuSPITransmitReceive(uint8_t* rxbuffer, uint8_t* txbuf
|
|||
//CRC-15
|
||||
//x^15 + x^14 + x^10 + x^8 + x^7 + x^4 + x^3 + 1
|
||||
|
||||
static uint16_t updateCommandPEC(uint16_t currentPEC, uint8_t din) {
|
||||
din = (din >> 7) & 0x01;
|
||||
uint8_t in0 = din ^ ((currentPEC >> 14) & 0x01);
|
||||
uint8_t in3 = in0 ^ ((currentPEC >> 2) & 0x01);
|
||||
uint8_t in4 = in0 ^ ((currentPEC >> 3) & 0x01);
|
||||
uint8_t in7 = in0 ^ ((currentPEC >> 6) & 0x01);
|
||||
uint8_t in8 = in0 ^ ((currentPEC >> 7) & 0x01);
|
||||
uint8_t in10 = in0 ^ ((currentPEC >> 9) & 0x01);
|
||||
uint8_t in14 = in0 ^ ((currentPEC >> 13) & 0x01);
|
||||
|
||||
uint16_t newPEC = 0;
|
||||
|
||||
newPEC |= in14 << 14;
|
||||
newPEC |= (currentPEC & (0x01 << 12)) << 1;
|
||||
newPEC |= (currentPEC & (0x01 << 11)) << 1;
|
||||
newPEC |= (currentPEC & (0x01 << 10)) << 1;
|
||||
newPEC |= in10 << 10;
|
||||
newPEC |= (currentPEC & (0x01 << 8)) << 1;
|
||||
newPEC |= in8 << 8;
|
||||
newPEC |= in7 << 7;
|
||||
newPEC |= (currentPEC & (0x01 << 5)) << 1;
|
||||
newPEC |= (currentPEC & (0x01 << 4)) << 1;
|
||||
newPEC |= in4 << 4;
|
||||
newPEC |= in3 << 3;
|
||||
newPEC |= (currentPEC & (0x01 << 1)) << 1;
|
||||
newPEC |= (currentPEC & (0x01)) << 1;
|
||||
newPEC |= in0;
|
||||
|
||||
return newPEC;
|
||||
static uint16_t computeCRC15(const uint8_t* data, size_t length) {
|
||||
uint16_t remainder = INITIAL_COMMAND_PEC;
|
||||
for (size_t i = 0; i < length; i++) {
|
||||
remainder ^= (data[i] << 7);
|
||||
for (int b = 0; b < 8; b++) {
|
||||
if (remainder & 0x4000) {
|
||||
remainder = (uint16_t)((remainder << 1) ^ 0xC599);
|
||||
} else {
|
||||
remainder <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return remainder;
|
||||
}
|
||||
|
||||
static uint8_t calculateCommandPEC(uint8_t* data, uint8_t datalen) {
|
||||
uint16_t currentpec = INITIAL_COMMAND_PEC;
|
||||
if (datalen >= 3) {
|
||||
for (int i = 0; i < (datalen - 2); i++) {
|
||||
for (int n = 0; n < 8; n++) {
|
||||
uint8_t din = data[i] << (n);
|
||||
currentpec = updateCommandPEC(currentpec, din);
|
||||
}
|
||||
}
|
||||
|
||||
data[datalen - 2] = (currentpec >> 7) & 0xFF;
|
||||
data[datalen - 1] = (currentpec << 1) & 0xFF;
|
||||
if (datalen < 3) { return 1; }
|
||||
uint16_t pec = computeCRC15(data, datalen - 2);
|
||||
data[datalen - 2] = (uint8_t)((pec >> 7) & 0xFF);
|
||||
data[datalen - 1] = (uint8_t)((pec << 1) & 0xFF);
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t checkCommandPEC(uint8_t* data, uint8_t datalen) {
|
||||
if (datalen <= 3) {
|
||||
return 255;
|
||||
}
|
||||
|
||||
uint16_t currentpec = INITIAL_COMMAND_PEC;
|
||||
|
||||
for (int i = 0; i < (datalen - 2); i++) {
|
||||
for (int n = 0; n < 8; n++) {
|
||||
uint8_t din = data[i] << (n);
|
||||
currentpec = updateCommandPEC(currentpec, din);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t pechigh = (currentpec >> 7) & 0xFF;
|
||||
uint8_t peclow = (currentpec << 1) & 0xFF;
|
||||
|
||||
if ((pechigh == data[datalen - 2]) && (peclow == data[datalen - 1])) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 1;
|
||||
if (datalen <= 3) { return 255; }
|
||||
uint16_t pec = computeCRC15(data, datalen - 2);
|
||||
uint8_t pech = (uint8_t)((pec >> 7) & 0xFF);
|
||||
uint8_t pecl = (uint8_t)((pec << 1) & 0xFF);
|
||||
return ((pech == data[datalen - 2]) && (pecl == data[datalen - 1])) ? 0 : 1;
|
||||
}
|
||||
|
||||
//data PEC calculation
|
||||
//CRC-10
|
||||
//x^10 + x^7 + x^3 + x^2 + x + 1
|
||||
|
||||
static uint16_t pec10_calc(bool rx_cmd, int len, uint8_t* data) {
|
||||
uint16_t remainder = 16; /* PEC_SEED; 0000010000 */
|
||||
uint16_t polynom = 0x8F; /* x10 + x7 + x3 + x2 + x + 1 <- the CRC15 polynomial
|
||||
100 1000 1111 48F */
|
||||
static uint16_t computeCRC10(const uint8_t* data, size_t length, bool rx_cmd) {
|
||||
uint16_t remainder = INITIAL_DATA_PEC;
|
||||
const uint16_t poly = 0x8F;
|
||||
size_t limit = length;
|
||||
|
||||
/* Perform modulo-2 division, a byte at a time. */
|
||||
for (uint8_t pbyte = 0; pbyte < len; ++pbyte) {
|
||||
/* Bring the next byte into the remainder. */
|
||||
remainder ^= (uint16_t)(data[pbyte] << 2);
|
||||
/* Perform modulo-2 division, a bit at a time.*/
|
||||
for (uint8_t bit_ = 8; bit_ > 0; --bit_) {
|
||||
/* Try to divide the current data bit. */
|
||||
if ((remainder & 0x200) >
|
||||
0) // equivalent to remainder & 2^14 simply check for MSB
|
||||
{
|
||||
remainder = (uint16_t)((remainder << 1));
|
||||
remainder = (uint16_t)(remainder ^ polynom);
|
||||
if (rx_cmd && (length > 0)) {
|
||||
limit = length - 1;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < limit; i++) {
|
||||
remainder ^= (uint16_t)(data[i] << 2);
|
||||
for (int b = 0; b < 8; b++) {
|
||||
if (remainder & 0x200) {
|
||||
remainder = (uint16_t)((remainder << 1) ^ poly);
|
||||
} else {
|
||||
remainder = (uint16_t)(remainder << 1);
|
||||
remainder <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (rx_cmd == true) {
|
||||
remainder ^= (uint16_t)((data[len] & 0xFC) << 2);
|
||||
/* Perform modulo-2 division, a bit at a time */
|
||||
for (uint8_t bit_ = 6; bit_ > 0; --bit_) {
|
||||
/* Try to divide the current data bit */
|
||||
if ((remainder & 0x200) >
|
||||
0) // equivalent to remainder & 2^14 simply check for MSB
|
||||
{
|
||||
remainder = (uint16_t)((remainder << 1));
|
||||
remainder = (uint16_t)(remainder ^ polynom);
|
||||
|
||||
// If receiving a command, handle leftover bits
|
||||
if (rx_cmd && (length > 0)) {
|
||||
remainder ^= (uint16_t)((data[length - 1] & 0xFC) << 2);
|
||||
for (int b = 0; b < 6; b++) {
|
||||
if (remainder & 0x200) {
|
||||
remainder = (uint16_t)((remainder << 1) ^ poly);
|
||||
} else {
|
||||
remainder = (uint16_t)((remainder << 1));
|
||||
remainder <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ((uint16_t)(remainder & 0x3FF));
|
||||
return (uint16_t)(remainder & 0x3FF);
|
||||
}
|
||||
|
||||
typedef uint16_t crc;
|
||||
static crc F_CRC_CalculaCheckSum(uint8_t const AF_Datos[], uint16_t VF_nBytes);
|
||||
|
||||
static uint8_t calculateDataPEC(uint8_t* data, uint8_t datalen) {
|
||||
|
||||
if (datalen >= 3) {
|
||||
|
||||
|
||||
crc currentpec = pec10_calc(true, datalen - 2, data) & 0x3FF; // mask to 10 bits
|
||||
|
||||
// memory layout is [[zeroes], PEC[9:8]], [PEC[7:0]]
|
||||
data[datalen - 2] = (currentpec >> 8) & 0xFF;
|
||||
data[datalen - 1] = currentpec & 0xFF;
|
||||
|
||||
if (datalen < 3) { return 1; }
|
||||
uint16_t crcVal = computeCRC10(data, datalen - 2, true);
|
||||
data[datalen - 2] = (uint8_t)((crcVal >> 8) & 0xFF);
|
||||
data[datalen - 1] = (uint8_t)(crcVal & 0xFF);
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t checkDataPEC(uint8_t* data, uint8_t len) {
|
||||
if (len <= 2) {
|
||||
return 255;
|
||||
}
|
||||
|
||||
crc currentpec = F_CRC_CalculaCheckSum(data, len);
|
||||
|
||||
return (currentpec == 0) ? 0 : 1;
|
||||
}
|
||||
|
||||
|
||||
static crc F_CRC_ObtenValorDeTabla(uint8_t VP_Pos_Tabla) {
|
||||
crc VP_CRCTableValue = 0;
|
||||
uint8_t VP_Pos_bit = 0;
|
||||
|
||||
VP_CRCTableValue = ((crc)(VP_Pos_Tabla)) << (10 - 8);
|
||||
|
||||
for (VP_Pos_bit = 0; VP_Pos_bit < 8; VP_Pos_bit++) {
|
||||
if (VP_CRCTableValue & (((crc)1) << (10 - 1))) {
|
||||
VP_CRCTableValue = (VP_CRCTableValue << 1) ^ 0x8F;
|
||||
} else {
|
||||
VP_CRCTableValue = (VP_CRCTableValue << 1);
|
||||
}
|
||||
}
|
||||
return ((VP_CRCTableValue));
|
||||
}
|
||||
static crc F_CRC_CalculaCheckSum(uint8_t const AF_Datos[], uint16_t VF_nBytes) {
|
||||
crc VP_CRCTableValue = 16;
|
||||
int16_t VP_bytes = 0;
|
||||
|
||||
for (VP_bytes = 0; VP_bytes < VF_nBytes; VP_bytes++) {
|
||||
|
||||
VP_CRCTableValue = (VP_CRCTableValue << 8) ^
|
||||
F_CRC_ObtenValorDeTabla(
|
||||
((uint8_t)((VP_CRCTableValue >> (10 - 8)) & 0xFF)) ^
|
||||
AF_Datos[VP_bytes]);
|
||||
}
|
||||
|
||||
if ((8 * sizeof(crc)) > 10) {
|
||||
VP_CRCTableValue = VP_CRCTableValue & ((((crc)(1)) << 10) - 1);
|
||||
}
|
||||
|
||||
return (VP_CRCTableValue ^ 0x0000);
|
||||
}
|
||||
|
||||
static uint16_t updateDataPEC(uint16_t currentPEC, uint8_t din) {
|
||||
din = (din >> 7) & 0x01;
|
||||
uint8_t in0 = din ^ ((currentPEC >> 9) & 0x01);
|
||||
uint8_t in2 = in0 ^ ((currentPEC >> 1) & 0x01);
|
||||
uint8_t in3 = in0 ^ ((currentPEC >> 2) & 0x01);
|
||||
uint8_t in7 = in0 ^ ((currentPEC >> 6) & 0x01);
|
||||
|
||||
uint16_t newPEC = 0;
|
||||
|
||||
newPEC |= (currentPEC & (0x01 << 8)) << 1;
|
||||
newPEC |= (currentPEC & (0x01 << 7)) << 1;
|
||||
newPEC |= in7 << 7;
|
||||
newPEC |= (currentPEC & (0x01 << 5)) << 1;
|
||||
newPEC |= (currentPEC & (0x01 << 4)) << 1;
|
||||
newPEC |= in3 << 3;
|
||||
newPEC |= in2 << 2;
|
||||
newPEC |= (currentPEC & (0x01)) << 1;
|
||||
newPEC |= in0;
|
||||
|
||||
return newPEC;
|
||||
if (len <= 2) { return 255; }
|
||||
// Zero remainder means a valid CRC.
|
||||
return (computeCRC10(data, len, false) == 0) ? 0 : 1;
|
||||
}
|
||||
|
||||
HAL_StatusTypeDef ___writeCMD(uint16_t command, uint8_t * args, size_t arglen) {
|
||||
|
|
@ -294,8 +173,6 @@ HAL_StatusTypeDef ___writeCMD(uint16_t command, uint8_t * args, size_t arglen) {
|
|||
return ret;
|
||||
}
|
||||
|
||||
|
||||
|
||||
HAL_StatusTypeDef ___readCMD(uint16_t command, uint8_t * buffer, size_t arglen) {
|
||||
buffer[0] = (command >> 8) & 0xFF;
|
||||
buffer[1] = (command)&0xFF;
|
||||
|
|
|
|||
Loading…
Reference in New Issue