Marcel Peterkau
98629b744d
Some checks failed
CI-Build/Kettenoeler/pipeline/head There was a failure building this commit
459 lines
10 KiB
C++
459 lines
10 KiB
C++
#include "can_hal.h"
|
||
#include "dtc.h"
|
||
#include <string.h> // memcpy, memcmp
|
||
|
||
// =====================
|
||
// Interner Zustand/Helper
|
||
// =====================
|
||
MCP_CAN CAN0(GPIO_CS_CAN);
|
||
|
||
static bool s_ready = false;
|
||
static uint8_t s_nextFiltSlot = 0; // 0..5 (MCP2515 hat 6 Filter-Slots)
|
||
static uint16_t s_modeSettleMs = 10; // Default aus Config
|
||
|
||
// Trace-Hook
|
||
static CanTraceSink s_traceSink = nullptr;
|
||
|
||
// RAW-Sniffer-Steuerung (Filter offen + Restore der vorherigen Konfig)
|
||
static bool s_rawSnifferEnabled = false;
|
||
|
||
// Spiegel der "Normal"-Konfiguration (damit wir nach RAW wiederherstellen können)
|
||
static uint16_t s_savedStdMask[2] = {0x000, 0x000};
|
||
static struct
|
||
{
|
||
uint32_t id;
|
||
bool ext;
|
||
} s_savedFilt[6];
|
||
static uint8_t s_savedFiltCount = 0;
|
||
|
||
// 11-bit: Lib erwartet (value << 16)
|
||
static inline uint32_t _std_to_hw(uint16_t v11) { return ((uint32_t)v11) << 16; }
|
||
|
||
// „Bestätigter“ Mode-Wechsel mithilfe der Lib-Funktion setMode(newMode)
|
||
static bool _trySetMode(uint8_t mode, uint16_t settleMs)
|
||
{
|
||
const uint32_t t0 = millis();
|
||
do
|
||
{
|
||
if (CAN0.setMode(mode) == CAN_OK)
|
||
return true;
|
||
delay(1);
|
||
} while ((millis() - t0) < settleMs);
|
||
return false;
|
||
}
|
||
|
||
// LOOPBACK-Selftest (ohne Bus)
|
||
static bool _selftest_loopback(uint16_t windowMs)
|
||
{
|
||
if (!_trySetMode(MCP_LOOPBACK, s_modeSettleMs))
|
||
return false;
|
||
|
||
const unsigned long tid = 0x123;
|
||
uint8_t tx[8] = {0xA5, 0x5A, 0x11, 0x22, 0x33, 0x44, 0x77, 0x88};
|
||
if (CAN0.sendMsgBuf(tid, 0, 8, tx) != CAN_OK)
|
||
{
|
||
(void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||
return false;
|
||
}
|
||
|
||
bool got = false;
|
||
const uint32_t t0 = millis();
|
||
while ((millis() - t0) < windowMs)
|
||
{
|
||
if (CAN0.checkReceive() == CAN_MSGAVAIL)
|
||
{
|
||
unsigned long rid;
|
||
uint8_t len, rx[8];
|
||
if (CAN0.readMsgBuf(&rid, &len, rx) == CAN_OK)
|
||
{
|
||
if (rid == tid && len == 8 && memcmp(tx, rx, 8) == 0)
|
||
{
|
||
got = true;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
delay(1);
|
||
}
|
||
|
||
(void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||
return got;
|
||
}
|
||
|
||
// Optional: kurzer ListenOnly-Hörtest (nur Heuristik, keine DTC-Änderung)
|
||
static void _probe_listen_only(uint16_t ms)
|
||
{
|
||
if (ms == 0)
|
||
return;
|
||
if (!_trySetMode(MCP_LISTENONLY, s_modeSettleMs))
|
||
return;
|
||
const uint32_t t0 = millis();
|
||
while ((millis() - t0) < ms)
|
||
{
|
||
if (CAN0.checkReceive() == CAN_MSGAVAIL)
|
||
break;
|
||
delay(1);
|
||
}
|
||
(void)_trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||
}
|
||
|
||
// Offen konfigurieren (RAW-Sniffer)
|
||
static bool _apply_open_filters()
|
||
{
|
||
if (!_trySetMode(MODE_CONFIG, s_modeSettleMs))
|
||
return false;
|
||
|
||
// Masken 0 -> alles durchlassen
|
||
CAN0.init_Mask(0, 0, _std_to_hw(0x000));
|
||
CAN0.init_Mask(1, 0, _std_to_hw(0x000));
|
||
// Filter egal
|
||
for (uint8_t i = 0; i < 6; ++i)
|
||
{
|
||
CAN0.init_Filt(i, 0, _std_to_hw(0x000));
|
||
}
|
||
s_nextFiltSlot = 0;
|
||
|
||
return _trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||
}
|
||
|
||
// Gespeicherte Normal-Konfiguration anwenden
|
||
static bool _apply_saved_filters()
|
||
{
|
||
if (!_trySetMode(MODE_CONFIG, s_modeSettleMs))
|
||
return false;
|
||
|
||
CAN0.init_Mask(0, 0, _std_to_hw(s_savedStdMask[0]));
|
||
CAN0.init_Mask(1, 0, _std_to_hw(s_savedStdMask[1]));
|
||
|
||
// Erst alle Filter neutralisieren
|
||
for (uint8_t i = 0; i < 6; ++i)
|
||
{
|
||
CAN0.init_Filt(i, 0, _std_to_hw(0x000));
|
||
}
|
||
|
||
// Dann gespeicherte Filter wieder setzen
|
||
s_nextFiltSlot = 0;
|
||
for (uint8_t i = 0; i < s_savedFiltCount && s_nextFiltSlot < 6; ++i)
|
||
{
|
||
const auto &F = s_savedFilt[i];
|
||
const uint32_t hwId = F.ext ? F.id : _std_to_hw((uint16_t)F.id);
|
||
CAN0.init_Filt(s_nextFiltSlot++, F.ext ? 1 : 0, hwId);
|
||
}
|
||
|
||
return _trySetMode(MCP_NORMAL, s_modeSettleMs);
|
||
}
|
||
|
||
// =====================
|
||
// Öffentliche API
|
||
// =====================
|
||
|
||
void CAN_HAL_SetTraceSink(CanTraceSink sink)
|
||
{
|
||
s_traceSink = sink;
|
||
}
|
||
|
||
void CAN_HAL_EnableRawSniffer(bool enable)
|
||
{
|
||
if (enable == s_rawSnifferEnabled)
|
||
return;
|
||
|
||
if (enable)
|
||
{
|
||
// Auf RAW öffnen
|
||
if (_apply_open_filters())
|
||
{
|
||
s_rawSnifferEnabled = true;
|
||
}
|
||
else
|
||
{
|
||
// Falls es nicht klappt, lieber Defekt melden als im Zwischending zu bleiben
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Gespeicherte "Normal"-Konfiguration wieder aktivieren
|
||
if (_apply_saved_filters())
|
||
{
|
||
s_rawSnifferEnabled = false;
|
||
}
|
||
else
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
}
|
||
}
|
||
}
|
||
|
||
bool CAN_HAL_IsRawSnifferEnabled()
|
||
{
|
||
return s_rawSnifferEnabled;
|
||
}
|
||
|
||
bool CAN_HAL_Init(const CanHalConfig &cfg)
|
||
{
|
||
s_ready = false;
|
||
s_modeSettleMs = cfg.modeSettleMs ? cfg.modeSettleMs : 10;
|
||
s_traceSink = nullptr;
|
||
s_rawSnifferEnabled = false;
|
||
|
||
// 1) SPI/MCP starten (STDEXT ist robust gegen Fehlpfade in Lib-Forks)
|
||
if (CAN0.begin(MCP_STDEXT, cfg.baud, cfg.clock) != CAN_OK)
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
|
||
// 2) Loopback-Selftest (ohne Bus)
|
||
if (!_selftest_loopback(20))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
|
||
// 3) Optional Listen-Only-Probe (nur Info)
|
||
_probe_listen_only(cfg.listenOnlyProbeMs);
|
||
|
||
// 4) Default: Filter/Masks offen, Mode NORMAL
|
||
if (!_apply_open_filters())
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
|
||
// Initiale "Normal"-Spiegelung: alles offen (bis die App später echte Filter setzt)
|
||
s_savedStdMask[0] = 0x000;
|
||
s_savedStdMask[1] = 0x000;
|
||
s_savedFiltCount = 0;
|
||
for (uint8_t i = 0; i < 6; ++i)
|
||
{
|
||
s_savedFilt[i].id = 0;
|
||
s_savedFilt[i].ext = false;
|
||
}
|
||
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false);
|
||
s_ready = true;
|
||
return true;
|
||
}
|
||
|
||
bool CAN_HAL_IsReady() { return s_ready; }
|
||
|
||
bool CAN_HAL_SetMode(uint8_t mode)
|
||
{
|
||
const bool ok = _trySetMode(mode, s_modeSettleMs);
|
||
if (!ok)
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return ok;
|
||
}
|
||
|
||
bool CAN_HAL_SetMask(uint8_t bank, bool ext, uint32_t rawMask)
|
||
{
|
||
if (bank > 1)
|
||
return false;
|
||
|
||
if (!CAN_HAL_SetMode(MODE_CONFIG))
|
||
return false;
|
||
|
||
const bool ok = (CAN0.init_Mask(bank, ext ? 1 : 0, rawMask) == CAN_OK);
|
||
|
||
// Spiegeln (nur STD-11 Spiegel führen wir – ext-Masken selten; bei ext ignorieren)
|
||
if (!ext)
|
||
{
|
||
const uint16_t m11 = (uint16_t)(rawMask >> 16);
|
||
s_savedStdMask[bank] = m11;
|
||
}
|
||
|
||
if (!CAN_HAL_SetMode(MCP_NORMAL))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
return ok;
|
||
}
|
||
|
||
bool CAN_HAL_SetStdMask11(uint8_t bank, uint16_t mask11)
|
||
{
|
||
return CAN_HAL_SetMask(bank, false, _std_to_hw(mask11));
|
||
}
|
||
|
||
void CAN_HAL_ClearFilters()
|
||
{
|
||
if (!CAN_HAL_SetMode(MODE_CONFIG))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return;
|
||
}
|
||
|
||
CAN0.init_Mask(0, 0, _std_to_hw(0x000));
|
||
CAN0.init_Mask(1, 0, _std_to_hw(0x000));
|
||
for (uint8_t i = 0; i < 6; ++i)
|
||
{
|
||
CAN0.init_Filt(i, 0, _std_to_hw(0x000));
|
||
}
|
||
s_nextFiltSlot = 0;
|
||
|
||
// Spiegel auch zurücksetzen
|
||
s_savedStdMask[0] = 0x000;
|
||
s_savedStdMask[1] = 0x000;
|
||
s_savedFiltCount = 0;
|
||
for (uint8_t i = 0; i < 6; ++i)
|
||
{
|
||
s_savedFilt[i].id = 0;
|
||
s_savedFilt[i].ext = false;
|
||
}
|
||
|
||
if (!CAN_HAL_SetMode(MCP_NORMAL))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
}
|
||
}
|
||
|
||
bool CAN_HAL_AddFilter(const CanFilter &f)
|
||
{
|
||
if (s_nextFiltSlot >= 6)
|
||
return false;
|
||
if (!CAN_HAL_SetMode(MODE_CONFIG))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
|
||
const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id);
|
||
const uint8_t slot = s_nextFiltSlot++;
|
||
const bool ok = (CAN0.init_Filt(slot, f.ext ? 1 : 0, hwId) == CAN_OK);
|
||
|
||
// Spiegeln
|
||
if (ok)
|
||
{
|
||
if (s_savedFiltCount < 6)
|
||
{
|
||
s_savedFilt[s_savedFiltCount].id = f.ext ? f.id : (uint32_t)((uint16_t)f.id);
|
||
s_savedFilt[s_savedFiltCount].ext = f.ext;
|
||
++s_savedFiltCount;
|
||
}
|
||
}
|
||
|
||
if (!CAN_HAL_SetMode(MCP_NORMAL))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
return ok;
|
||
}
|
||
|
||
bool CAN_HAL_SetFilters(const CanFilter *list, size_t count)
|
||
{
|
||
if (!CAN_HAL_SetMode(MODE_CONFIG))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
|
||
// Slots zurücksetzen
|
||
s_nextFiltSlot = 0;
|
||
for (uint8_t i = 0; i < 6; ++i)
|
||
{
|
||
CAN0.init_Filt(i, 0, _std_to_hw(0x000));
|
||
}
|
||
|
||
// Setzen
|
||
for (size_t i = 0; i < count && s_nextFiltSlot < 6; ++i)
|
||
{
|
||
const auto &f = list[i];
|
||
const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id);
|
||
CAN0.init_Filt(s_nextFiltSlot++, f.ext ? 1 : 0, hwId);
|
||
}
|
||
|
||
// Spiegel aktualisieren
|
||
s_savedFiltCount = 0;
|
||
for (size_t i = 0; i < count && i < 6; ++i)
|
||
{
|
||
s_savedFilt[i].id = list[i].ext ? list[i].id : (uint32_t)((uint16_t)list[i].id);
|
||
s_savedFilt[i].ext = list[i].ext;
|
||
++s_savedFiltCount;
|
||
}
|
||
|
||
if (!CAN_HAL_SetMode(MCP_NORMAL))
|
||
{
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool CAN_HAL_Read(unsigned long &id, uint8_t &len, uint8_t data[8])
|
||
{
|
||
if (CAN0.checkReceive() != CAN_MSGAVAIL)
|
||
return false;
|
||
|
||
if (CAN0.readMsgBuf(&id, &len, data) != CAN_OK)
|
||
{
|
||
// Echte Lese-Fehler -> vermutlich SPI/Controller-Problem
|
||
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
|
||
return false;
|
||
}
|
||
|
||
// MCP_CAN schreibt Flags in das ID-Wort:
|
||
// bit31 = EXT, bit30 = RTR, Rest = rohe ID (11 oder 29 Bit)
|
||
const bool ext = (id & 0x80000000UL) != 0;
|
||
const bool rtr = (id & 0x40000000UL) != 0; // aktuell nur informativ
|
||
|
||
// "Saubere" ID für Aufrufer herstellen
|
||
const uint32_t clean_id = ext ? (id & 0x1FFFFFFFUL) : (id & 0x7FFUL);
|
||
id = clean_id;
|
||
|
||
// Trace-Hook (RX)
|
||
if (s_traceSink)
|
||
{
|
||
CanLogFrame f{};
|
||
f.ts_ms = millis();
|
||
f.id = clean_id;
|
||
f.ext = ext;
|
||
f.rx = true;
|
||
f.dlc = len;
|
||
if (len)
|
||
memcpy(f.data, data, len);
|
||
s_traceSink(f);
|
||
}
|
||
|
||
// Optional: Wenn du RTR-Frames speziell behandeln willst, könntest du hier
|
||
// (rtr==true) markieren/loggen oder len=0 erzwingen. Für jetzt: einfach durchreichen.
|
||
(void)rtr;
|
||
|
||
return true;
|
||
}
|
||
|
||
uint8_t CAN_HAL_Send(unsigned long id, bool ext, uint8_t len, const uint8_t *data)
|
||
{
|
||
// Senden
|
||
uint8_t st = CAN0.sendMsgBuf(id, ext ? 1 : 0, len, const_cast<uint8_t *>(data));
|
||
|
||
// Trace-Hook (TX) nur bei Erfolg loggen – optional: immer loggen
|
||
if (st == CAN_OK && s_traceSink)
|
||
{
|
||
CanLogFrame f{};
|
||
f.ts_ms = millis();
|
||
f.id = id;
|
||
f.ext = ext;
|
||
f.rx = false;
|
||
f.dlc = len;
|
||
if (len)
|
||
memcpy(f.data, data, len);
|
||
s_traceSink(f);
|
||
}
|
||
return st;
|
||
}
|
||
|
||
// ==== Diagnose/Utilities ====
|
||
|
||
uint8_t CAN_HAL_GetErrorFlags()
|
||
{
|
||
// getError() liefert MCP_EFLG Snapshot (Lib-abhängig)
|
||
return CAN0.getError();
|
||
}
|
||
|
||
void CAN_HAL_GetErrorCounters(uint8_t &tec, uint8_t &rec)
|
||
{
|
||
tec = CAN0.errorCountTX();
|
||
rec = CAN0.errorCountRX();
|
||
}
|