197 lines
7.5 KiB
C
197 lines
7.5 KiB
C
/*
|
|
* can_encode.c
|
|
*
|
|
* Created on: Sep 16, 2025
|
|
* Author: herli
|
|
*/
|
|
#include "can_encode.h"
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
/* -------- Bit helpers (Intel) --------
|
|
* Intel (little-endian bit numbering): bit 0 is LSB of byte 0, bit 7 is MSB of byte 0,
|
|
* then bit 8 is LSB of byte 1, etc.
|
|
*/
|
|
uint64_t _can_get_bits_intel(const uint8_t data[8], uint8_t start_bit, uint8_t bit_len)
|
|
{
|
|
uint64_t acc = 0;
|
|
for (uint8_t i = 0; i < bit_len; ++i) {
|
|
uint16_t bit_index = start_bit + i;
|
|
uint8_t byte_idx = (uint8_t)(bit_index >> 3);
|
|
uint8_t bit_in_byte = (uint8_t)(bit_index & 7);
|
|
uint8_t b = (data[byte_idx] >> bit_in_byte) & 1u;
|
|
acc |= ((uint64_t)b) << i;
|
|
}
|
|
return acc;
|
|
}
|
|
|
|
void _can_set_bits_intel(uint8_t data[8], uint8_t start_bit, uint8_t bit_len, uint64_t raw)
|
|
{
|
|
for (uint8_t i = 0; i < bit_len; ++i) {
|
|
uint16_t bit_index = start_bit + i;
|
|
uint8_t byte_idx = (uint8_t)(bit_index >> 3);
|
|
uint8_t bit_in_byte = (uint8_t)(bit_index & 7);
|
|
uint8_t bit = (uint8_t)((raw >> i) & 1u);
|
|
data[byte_idx] = (uint8_t)((data[byte_idx] & ~(1u << bit_in_byte)) | (bit << bit_in_byte));
|
|
}
|
|
}
|
|
|
|
/* -------- Bit helpers (Motorola) --------
|
|
* Motorola (big-endian) uses a different bit ordering: start_bit points to a bit index where
|
|
* bytes count from MSB to LSB. Implementation below follows standard DBC Motorola mapping.
|
|
* This implementation is conservative and handles the general case.
|
|
*/
|
|
static inline uint8_t _motorola_byte(uint8_t bit_index) { return (uint8_t)(7 - (bit_index & 7)); }
|
|
|
|
uint64_t _can_get_bits_motorola(const uint8_t data[8], uint8_t start_bit, uint8_t bit_len)
|
|
{
|
|
uint64_t acc = 0;
|
|
uint8_t cur_bit = start_bit;
|
|
for (uint8_t i = 0; i < bit_len; ++i) {
|
|
uint8_t byte_idx = (uint8_t)(cur_bit >> 3);
|
|
uint8_t bit_in_byte = _motorola_byte(cur_bit);
|
|
uint8_t b = (data[byte_idx] >> bit_in_byte) & 1u;
|
|
acc = (acc << 1) | b;
|
|
// advance to next bit (Motorola decrements bit index)
|
|
if (bit_in_byte == 0) {
|
|
// next byte
|
|
if (byte_idx == 0) break; // avoid underflow; malformed definition
|
|
cur_bit = (uint8_t)(((byte_idx - 1) << 3) + 7);
|
|
} else {
|
|
cur_bit = (uint8_t)((byte_idx << 3) + (bit_in_byte - 1));
|
|
}
|
|
}
|
|
return acc;
|
|
}
|
|
|
|
void _can_set_bits_motorola(uint8_t data[8], uint8_t start_bit, uint8_t bit_len, uint64_t raw)
|
|
{
|
|
uint8_t cur_bit = start_bit;
|
|
for (uint8_t i = 0; i < bit_len; ++i) {
|
|
uint8_t bit_val = (uint8_t)((raw >> (bit_len - 1 - i)) & 1u);
|
|
uint8_t byte_idx = (uint8_t)(cur_bit >> 3);
|
|
uint8_t bit_in_byte = _motorola_byte(cur_bit);
|
|
data[byte_idx] = (uint8_t)((data[byte_idx] & ~(1u << bit_in_byte)) | (bit_val << bit_in_byte));
|
|
// advance (Motorola)
|
|
if (bit_in_byte == 0) {
|
|
if (byte_idx == 0) break;
|
|
cur_bit = (uint8_t)(((byte_idx - 1) << 3) + 7);
|
|
} else {
|
|
cur_bit = (uint8_t)((byte_idx << 3) + (bit_in_byte - 1));
|
|
}
|
|
}
|
|
}
|
|
|
|
/* -------- Effective factor/offset for a symbol -------- */
|
|
static inline void eff_scale(const CanSymbolDef *s, float *f, float *o)
|
|
{
|
|
if ((s->factor != 0.0f) || (s->offset != 0.0f)) {
|
|
*f = s->factor; *o = s->offset;
|
|
} else if (s->scale) {
|
|
*f = s->scale->factor; *o = s->scale->offset;
|
|
} else {
|
|
*f = 1.0f; *o = 0.0f;
|
|
}
|
|
}
|
|
|
|
/* -------- Public encode/decode -------- */
|
|
void can_encode_message(const CanMessageDef *msg, uint8_t out_data[8])
|
|
{
|
|
if (!msg || !out_data) return;
|
|
|
|
if (msg->tx_template) {
|
|
memcpy(out_data, msg->tx_template, msg->dlc);
|
|
if (msg->dlc < 8) memset(out_data + msg->dlc, 0, 8 - msg->dlc);
|
|
} else {
|
|
memset(out_data, 0, 8);
|
|
}
|
|
|
|
for (uint8_t i = 0; i < msg->symbol_count; ++i) {
|
|
const CanSymbolDef *s = &msg->symbols[i];
|
|
if (!s->ptr || s->bit_len == 0) continue;
|
|
|
|
int64_t raw_q;
|
|
if (s->storage == CAN_STORE_FLOAT) {
|
|
float f, o; eff_scale(s, &f, &o);
|
|
if (f == 0.0f) f = 1.0f; // guard
|
|
float phys = *(const float*)s->ptr;
|
|
float scaled = (phys - o) / f;
|
|
raw_q = (int64_t)llroundf(scaled);
|
|
} else {
|
|
switch (s->storage) {
|
|
case CAN_STORE_U32: raw_q = (int64_t)(*(const uint32_t*)s->ptr); break;
|
|
case CAN_STORE_S32: raw_q = (int64_t)(*(const int32_t *)s->ptr); break;
|
|
case CAN_STORE_U16: raw_q = (int64_t)(*(const uint16_t*)s->ptr); break;
|
|
case CAN_STORE_S16: raw_q = (int64_t)(*(const int16_t *)s->ptr); break;
|
|
case CAN_STORE_U8: raw_q = (int64_t)(*(const uint8_t *)s->ptr); break;
|
|
case CAN_STORE_S8: raw_q = (int64_t)(*(const int8_t *)s->ptr); break;
|
|
default: raw_q = 0; break;
|
|
}
|
|
if(s->inverted){
|
|
raw_q = !raw_q;
|
|
}
|
|
}
|
|
|
|
if (s->raw_type == CAN_SYM_SX) {
|
|
int64_t minv = -(1LL << (s->bit_len - 1));
|
|
int64_t maxv = (1LL << (s->bit_len - 1)) - 1;
|
|
if (raw_q < minv) raw_q = minv;
|
|
if (raw_q > maxv) raw_q = maxv;
|
|
uint64_t raw_u = (uint64_t)((int64_t)raw_q & ((1ULL << s->bit_len) - 1ULL));
|
|
(s->endian == CAN_ENDIAN_INTEL)
|
|
? _can_set_bits_intel(out_data, s->start_bit, s->bit_len, raw_u)
|
|
: _can_set_bits_motorola(out_data, s->start_bit, s->bit_len, raw_u);
|
|
} else {
|
|
if (raw_q < 0) raw_q = 0;
|
|
uint64_t raw_u = (uint64_t)raw_q & ((s->bit_len == 64) ? ~0ULL : ((1ULL << s->bit_len) - 1ULL));
|
|
(s->endian == CAN_ENDIAN_INTEL)
|
|
? _can_set_bits_intel(out_data, s->start_bit, s->bit_len, raw_u)
|
|
: _can_set_bits_motorola(out_data, s->start_bit, s->bit_len, raw_u);
|
|
}
|
|
}
|
|
}
|
|
|
|
void can_decode_message(const CanMessageDef *msg, const uint8_t in_data[8])
|
|
{
|
|
if (!msg || !in_data) return;
|
|
|
|
for (uint8_t i = 0; i < msg->symbol_count; ++i) {
|
|
const CanSymbolDef *s = &msg->symbols[i];
|
|
if (!s->ptr || s->bit_len == 0) continue;
|
|
|
|
uint64_t raw = (s->endian == CAN_ENDIAN_INTEL)
|
|
? _can_get_bits_intel(in_data, s->start_bit, s->bit_len)
|
|
: _can_get_bits_motorola(in_data, s->start_bit, s->bit_len);
|
|
|
|
// 1) Sign-extend (or not) -> produce raw_s
|
|
int64_t raw_s;
|
|
if (s->raw_type == CAN_SYM_SX) {
|
|
uint64_t sign_bit = 1ULL << (s->bit_len - 1);
|
|
raw_s = (raw & sign_bit)
|
|
? (int64_t)(raw | (~((1ULL << s->bit_len) - 1ULL))) // extend
|
|
: (int64_t)raw;
|
|
} else {
|
|
raw_s = (int64_t)raw;
|
|
}
|
|
|
|
// 2) Store according to 'storage'
|
|
if (s->storage == CAN_STORE_FLOAT) {
|
|
float f, o; eff_scale(s, &f, &o);
|
|
// f==0 guard (shouldn't happen if scales are defined, but safe)
|
|
if (f == 0.0f) f = 1.0f;
|
|
*(float*)s->ptr = (float)raw_s * f + o;
|
|
} else {
|
|
switch (s->storage) {
|
|
case CAN_STORE_U32: *(uint32_t*)s->ptr = (uint32_t)raw_s; break;
|
|
case CAN_STORE_S32: *(int32_t *)s->ptr = (int32_t) raw_s; break;
|
|
case CAN_STORE_U16: *(uint16_t*)s->ptr = (uint16_t)raw_s; break;
|
|
case CAN_STORE_S16: *(int16_t *)s->ptr = (int16_t) raw_s; break;
|
|
case CAN_STORE_U8: *(uint8_t *)s->ptr = (uint8_t) raw_s; break;
|
|
case CAN_STORE_S8: *(int8_t *)s->ptr = (int8_t) raw_s; break;
|
|
default: break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|