257 lines
7.2 KiB
C
257 lines
7.2 KiB
C
/*
|
||
* can_port.c
|
||
*
|
||
* Created on: Sep 16, 2025
|
||
* Author: herli
|
||
*/
|
||
#include "can_port.h"
|
||
|
||
#include <string.h>
|
||
|
||
#ifndef CAN_TXQ_CAP
|
||
#define CAN_TXQ_CAP 16u
|
||
#endif
|
||
|
||
|
||
/* ===== Static state ===== */
|
||
static FDCAN_HandleTypeDef *s_hcan = NULL;
|
||
|
||
typedef struct {
|
||
uint32_t id;
|
||
uint8_t dlc;
|
||
uint8_t data[8];
|
||
} _CanQueuedMsg;
|
||
|
||
/* Map 0..8 payload length to FDCAN DLC enum for classic CAN. */
|
||
static inline uint32_t _dlc_from_len(uint8_t len)
|
||
{
|
||
switch (len) {
|
||
case 0: return FDCAN_DLC_BYTES_0;
|
||
case 1: return FDCAN_DLC_BYTES_1;
|
||
case 2: return FDCAN_DLC_BYTES_2;
|
||
case 3: return FDCAN_DLC_BYTES_3;
|
||
case 4: return FDCAN_DLC_BYTES_4;
|
||
case 5: return FDCAN_DLC_BYTES_5;
|
||
case 6: return FDCAN_DLC_BYTES_6;
|
||
case 7: return FDCAN_DLC_BYTES_7;
|
||
default: return FDCAN_DLC_BYTES_8;
|
||
}
|
||
}
|
||
static inline bool _try_tx_now(FDCAN_HandleTypeDef *h, uint16_t id, const uint8_t data[8], uint8_t dlc)
|
||
{
|
||
FDCAN_TxHeaderTypeDef th = {0};
|
||
th.Identifier = id;
|
||
th.IdType = FDCAN_STANDARD_ID;
|
||
th.TxFrameType = FDCAN_DATA_FRAME;
|
||
th.DataLength = _dlc_from_len(dlc);
|
||
th.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
|
||
th.BitRateSwitch = FDCAN_BRS_OFF;
|
||
th.FDFormat = FDCAN_CLASSIC_CAN;
|
||
th.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
|
||
th.MessageMarker = 0;
|
||
|
||
return (HAL_FDCAN_AddMessageToTxFifoQ(h, &th, (uint8_t*)data) == HAL_OK);
|
||
}
|
||
|
||
static volatile uint16_t _txq_head = 0; // push
|
||
static volatile uint16_t _txq_tail = 0; // pop
|
||
static _CanQueuedMsg _txq_buf[CAN_TXQ_CAP];
|
||
|
||
static inline bool _txq_full(void) { return (uint16_t)((_txq_head + 1u) % CAN_TXQ_CAP) == _txq_tail; }
|
||
static inline bool _txq_empty(void) { return _txq_head == _txq_tail; }
|
||
|
||
static bool _txq_push(const _CanQueuedMsg *m)
|
||
{
|
||
if (_txq_full()) return false;
|
||
uint16_t h = _txq_head;
|
||
_txq_buf[h] = *m;
|
||
__DMB(); // ensure data visible before advancing head
|
||
_txq_head = (uint16_t)((h + 1u) % CAN_TXQ_CAP);
|
||
return true;
|
||
}
|
||
|
||
static bool _txq_pop(_CanQueuedMsg *m)
|
||
{
|
||
if (_txq_empty()) return false;
|
||
uint16_t t = _txq_tail;
|
||
*m = _txq_buf[t];
|
||
__DMB();
|
||
_txq_tail = (uint16_t)((t + 1u) % CAN_TXQ_CAP);
|
||
return true;
|
||
}
|
||
|
||
size_t CAN_TxQueueDepth(void)
|
||
{
|
||
int16_t d = (int16_t)_txq_head - (int16_t)_txq_tail;
|
||
if (d < 0) d += (int16_t)CAN_TXQ_CAP;
|
||
return (size_t)d;
|
||
}
|
||
bool CAN_Service(void)
|
||
{
|
||
if (!s_hcan) return false;
|
||
|
||
bool sent_any = false;
|
||
for (;;) {
|
||
_CanQueuedMsg m;
|
||
if (!_txq_pop(&m)) break;
|
||
|
||
if (!_try_tx_now(s_hcan, (uint16_t)m.id, m.data, m.dlc)) {
|
||
// HW still full -> put back and stop to preserve order
|
||
(void)_txq_push(&m);
|
||
break;
|
||
}
|
||
sent_any = true;
|
||
}
|
||
return sent_any;
|
||
}
|
||
|
||
|
||
/* Adapter used by can_manager to actually TX via HAL */
|
||
static bool _tx_adapter(uint16_t id, const uint8_t data[8], uint8_t dlc)
|
||
{
|
||
if (!s_hcan) return false;
|
||
|
||
// 1) Try to send immediately
|
||
if (_try_tx_now(s_hcan, id, data, dlc)) {
|
||
return true;
|
||
}
|
||
|
||
// 2) HW busy -> enqueue and report success if queued
|
||
_CanQueuedMsg m;
|
||
m.id = id;
|
||
m.dlc = dlc;
|
||
memcpy(m.data, data, 8);
|
||
|
||
return _txq_push(&m);
|
||
}
|
||
|
||
|
||
/* ===== Public API ===== */
|
||
|
||
void can_port_init(FDCAN_HandleTypeDef *hfdcan)
|
||
{
|
||
s_hcan = hfdcan;
|
||
can_manager_init(_tx_adapter);
|
||
//falta el register handler
|
||
|
||
}
|
||
|
||
void can_port_boot(void)
|
||
{
|
||
can_manager_boot();
|
||
|
||
// Send the first EMPF3 immediately
|
||
//can_port_send_msg_def(&MSG_ID_EMPF3);
|
||
|
||
// Schedule the remaining two, 40 ms apart, via timeout index 18
|
||
//Timeout_ResetByIndex(18, TIM16->CNT); // first callback in 40 ms
|
||
}
|
||
|
||
void can_port_handle_rx_fifo(uint32_t fifo_index)
|
||
{
|
||
if (!s_hcan) return;
|
||
|
||
const uint32_t fifo = (fifo_index == 1) ? FDCAN_RX_FIFO1 : FDCAN_RX_FIFO0;
|
||
|
||
while (HAL_FDCAN_GetRxFifoFillLevel(s_hcan, fifo) > 0U) {
|
||
FDCAN_RxHeaderTypeDef rh;
|
||
uint8_t data[8];
|
||
if (HAL_FDCAN_GetRxMessage(s_hcan, fifo, &rh, data) != HAL_OK) {
|
||
/* If needed: handle error or break */
|
||
break;
|
||
}
|
||
/* Pass to the generic manager; it will decode symbols and/or call handlers. */
|
||
can_manager_on_rx((uint16_t)rh.Identifier, data, 8);
|
||
}
|
||
}
|
||
|
||
bool can_port_send_msg(uint16_t can_id)
|
||
{
|
||
return can_manager_send(can_id);
|
||
}
|
||
|
||
bool can_port_send_raw(uint16_t id, const uint8_t data[8], uint8_t dlc)
|
||
{
|
||
return _tx_adapter(id, data, dlc);
|
||
}
|
||
|
||
static int _is_std_id(uint32_t id) { return (id <= 0x7FFu); }
|
||
|
||
size_t can_port_required_std_filters(void)
|
||
{
|
||
size_t count = 0, n = 0;
|
||
const CanMessageDef* const* all = can_db_all_ptr(&n);
|
||
if (!all) return 0;
|
||
|
||
// simple dedup set (tiny nets) — 64 IDs buffer; grow if you need more
|
||
uint16_t seen[64];
|
||
size_t seen_n = 0;
|
||
|
||
for (size_t i = 0; i < n; ++i) {
|
||
const CanMessageDef *m = all[i];
|
||
if (!m || m->dir != CAN_DIR_RX) continue;
|
||
if (!_is_std_id(m->can_id)) continue;
|
||
|
||
uint16_t id = (uint16_t)m->can_id;
|
||
int dup = 0;
|
||
for (size_t k = 0; k < seen_n; ++k) { if (seen[k] == id) { dup = 1; break; } }
|
||
if (!dup) {
|
||
if (seen_n < (sizeof(seen)/sizeof(seen[0]))) seen[seen_n++] = id;
|
||
count++;
|
||
}
|
||
}
|
||
return count;
|
||
}
|
||
|
||
HAL_StatusTypeDef can_port_apply_rx_filters(FDCAN_HandleTypeDef *hfdcan)
|
||
{
|
||
if (!hfdcan) return HAL_ERROR;
|
||
|
||
// 1) Reject everything by default (we’ll explicitly allow only DB RX IDs)
|
||
HAL_StatusTypeDef st =
|
||
HAL_FDCAN_ConfigGlobalFilter(hfdcan,
|
||
FDCAN_REJECT, // non-matching STD
|
||
FDCAN_REJECT, // non-matching EXT
|
||
FDCAN_REJECT_REMOTE, // STD remote
|
||
FDCAN_REJECT_REMOTE);// EXT remote
|
||
if (st != HAL_OK) return st;
|
||
|
||
// 2) Build exact-match STANDARD filters to FIFO0
|
||
size_t n=0, idx=0;
|
||
const CanMessageDef* const* all = can_db_all_ptr(&n);
|
||
if (!all) return HAL_OK;
|
||
|
||
// Dedup pass (same as above, but we’ll store all unique IDs)
|
||
uint16_t ids[64];
|
||
size_t ids_n = 0;
|
||
|
||
for (size_t i = 0; i < n; ++i) {
|
||
const CanMessageDef *m = all[i];
|
||
if (!m || m->dir != CAN_DIR_RX) continue;
|
||
if (!_is_std_id(m->can_id)) continue;
|
||
|
||
uint16_t id = (uint16_t)m->can_id;
|
||
int dup = 0;
|
||
for (size_t k = 0; k < ids_n; ++k) { if (ids[k] == id) { dup = 1; break; } }
|
||
if (!dup && ids_n < (sizeof(ids)/sizeof(ids[0]))) ids[ids_n++] = id;
|
||
}
|
||
|
||
// Guard: hardware must have enough filter elements (StdFiltersNbr >= ids_n)
|
||
// If not, we still program as many as possible.
|
||
for (idx = 0; idx < ids_n; ++idx) {
|
||
FDCAN_FilterTypeDef f = {0};
|
||
f.IdType = FDCAN_STANDARD_ID;
|
||
f.FilterIndex = (uint32_t)idx;
|
||
f.FilterType = FDCAN_FILTER_MASK; // mask-based exact match
|
||
f.FilterConfig = FDCAN_FILTER_TO_RXFIFO0; // route to FIFO0
|
||
f.FilterID1 = ids[idx]; // ID to match
|
||
f.FilterID2 = 0x7FF; // mask: all 11 bits must match
|
||
|
||
st = HAL_FDCAN_ConfigFilter(hfdcan, &f);
|
||
if (st != HAL_OK) return st;
|
||
}
|
||
|
||
return HAL_OK;
|
||
}
|
||
|