/* * can_encode.c * * Created on: Sep 16, 2025 * Author: herli */ #include "can_encode.h" #include #include /* -------- 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; } } } }