vee
333 lines
9.8 KiB
C
Executable File
333 lines
9.8 KiB
C
Executable File
#include "safety_manager.h"
|
|
|
|
#include "cy_project.h"
|
|
#include "cy_device_headers.h"
|
|
#include "motor_control.h"
|
|
#include "seat_motor_manager.h"
|
|
#include "seat_move_config.h"
|
|
|
|
#if defined(CoreDebug) && defined(CoreDebug_DHCSR_C_DEBUGEN_Msk)
|
|
#define SAFETY_DEBUG_DETACH_SUPPORTED (1u)
|
|
#else
|
|
#define SAFETY_DEBUG_DETACH_SUPPORTED (0u)
|
|
#endif
|
|
|
|
static volatile safety_fault_t latchedFault = SAFETY_FAULT_NONE;
|
|
static bool watchdogReset = false;
|
|
static bool watchdogInitialized = false;
|
|
static bool hilWatchdogResetArmed = false;
|
|
static bool hilWatchdogRecovered = false;
|
|
#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
|
|
static bool debuggerWasAttached = false;
|
|
#endif
|
|
|
|
static bool SafetyManager_DebuggerAttached(void)
|
|
{
|
|
#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
|
|
return ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) != 0ul);
|
|
#else
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
static uint32_t SafetyManager_ReadDebugMarker(void)
|
|
{
|
|
return BACKUP->unBREG[SEAT_MOVE_DEBUG_SESSION_BREG_INDEX].u32Register;
|
|
}
|
|
|
|
static bool SafetyManager_WriteDebugMarker(uint32_t marker)
|
|
{
|
|
BACKUP->unBREG[SEAT_MOVE_DEBUG_SESSION_BREG_INDEX].u32Register = marker;
|
|
__DSB();
|
|
return (SafetyManager_ReadDebugMarker() == marker);
|
|
}
|
|
|
|
static bool SafetyManager_WaitForWatchdogState(bool enabled)
|
|
{
|
|
uint32_t remaining = 200ul;
|
|
uint32_t expected = enabled ? 1ul : 0ul;
|
|
|
|
/* WDT ENABLE is an asynchronous request bit; ENABLED is the synchronized
|
|
* LF-clock state. Configuration writes made before disable completes are
|
|
* ignored by the hardware. */
|
|
while((WDT->unCTL.stcField.u1ENABLED != expected) && (remaining > 0ul))
|
|
{
|
|
Cy_SysTick_DelayInUs(1u);
|
|
remaining--;
|
|
}
|
|
return (WDT->unCTL.stcField.u1ENABLED == expected);
|
|
}
|
|
|
|
static void SafetyManager_ClearHilWatchdogCredential(void)
|
|
{
|
|
BACKUP->unBREG[SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register = 0u;
|
|
BACKUP->unBREG[SEAT_MOVE_HIL_WDT_TOKEN_BREG_INDEX].u32Register = 0u;
|
|
BACKUP->unBREG[SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register = 0u;
|
|
__DSB();
|
|
}
|
|
|
|
void SafetyManager_Init(uint32_t resetReason)
|
|
{
|
|
const uint32_t watchdogMask = CY_SYSRESET_WDT |
|
|
CY_SYSRESET_MCWDT0 |
|
|
CY_SYSRESET_MCWDT1 |
|
|
CY_SYSRESET_MCWDT2 |
|
|
CY_SYSRESET_MCWDT3;
|
|
bool watchdogStopped;
|
|
bool debuggerAttached;
|
|
uint32_t marker;
|
|
uint32_t hilMagic;
|
|
uint32_t hilToken;
|
|
uint32_t hilCheck;
|
|
bool authorizedHilReset;
|
|
bool markerOk = true;
|
|
|
|
/* A watchdog instance is retained across its own reset on this device.
|
|
* Stop it before reading credentials or initializing peripherals; the
|
|
* 100 ms bench timeout otherwise permits a reset loop. */
|
|
Cy_WDT_Disable();
|
|
watchdogStopped = SafetyManager_WaitForWatchdogState(false);
|
|
|
|
debuggerAttached = SafetyManager_DebuggerAttached();
|
|
marker = SafetyManager_ReadDebugMarker();
|
|
hilMagic = BACKUP->unBREG[
|
|
SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register;
|
|
hilToken = BACKUP->unBREG[
|
|
SEAT_MOVE_HIL_WDT_TOKEN_BREG_INDEX].u32Register;
|
|
hilCheck = BACKUP->unBREG[
|
|
SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register;
|
|
authorizedHilReset =
|
|
(hilMagic == SEAT_MOVE_HIL_WDT_MARKER) &&
|
|
(hilCheck == (hilMagic ^ hilToken ^ 0xFFFFFFFFu)) &&
|
|
(((resetReason & watchdogMask) != 0ul) ||
|
|
((resetReason & CY_SYSRESET_SOFT) != 0ul));
|
|
|
|
latchedFault = SAFETY_FAULT_NONE;
|
|
watchdogInitialized = false;
|
|
hilWatchdogResetArmed = false;
|
|
watchdogReset = ((resetReason & watchdogMask) != 0ul) ||
|
|
authorizedHilReset;
|
|
hilWatchdogRecovered = authorizedHilReset;
|
|
if((resetReason & CY_SYSRESET_PORVDDD) != 0ul)
|
|
{
|
|
markerOk = SafetyManager_WriteDebugMarker(0u);
|
|
marker = SafetyManager_ReadDebugMarker();
|
|
SafetyManager_ClearHilWatchdogCredential();
|
|
hilWatchdogRecovered = false;
|
|
}
|
|
else if(hilWatchdogRecovered)
|
|
{
|
|
/* Consume before CAN comes online: transport retries cannot turn one
|
|
* authenticated reset into a second physical reset. */
|
|
SafetyManager_ClearHilWatchdogCredential();
|
|
}
|
|
else if(hilMagic == SEAT_MOVE_HIL_WDT_MARKER)
|
|
{
|
|
/* XRES/programming without a valid reset reason is not completion. */
|
|
SafetyManager_ClearHilWatchdogCredential();
|
|
}
|
|
#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
|
|
debuggerWasAttached = debuggerAttached;
|
|
#endif
|
|
|
|
if(!markerOk || (marker == SEAT_MOVE_DEBUG_EXIT_MAGIC) ||
|
|
((marker == SEAT_MOVE_DEBUG_ACTIVE_MAGIC) && !debuggerAttached))
|
|
{
|
|
latchedFault = SAFETY_FAULT_DEBUG_DETACH;
|
|
}
|
|
else if(watchdogReset && !hilWatchdogRecovered)
|
|
{
|
|
latchedFault = SAFETY_FAULT_BOOT_RECOVERY;
|
|
}
|
|
else if(debuggerAttached &&
|
|
!SafetyManager_WriteDebugMarker(SEAT_MOVE_DEBUG_ACTIVE_MAGIC))
|
|
{
|
|
latchedFault = SAFETY_FAULT_DEBUG_DETACH;
|
|
}
|
|
|
|
/* The system-WDT configuration register is retained across some reset
|
|
* paths and AUTO_SERVICE is not reset by Cy_WDT_Init(). If that bit is
|
|
* left enabled, stopping the two software service paths can never prove a
|
|
* real watchdog reset because hardware services the counter at WARN_LIMIT.
|
|
* Configure only while stopped, and make software the sole WDT owner. */
|
|
if(!watchdogStopped)
|
|
{
|
|
latchedFault = SAFETY_FAULT_DRIVER;
|
|
return;
|
|
}
|
|
Cy_WDT_Init();
|
|
Cy_WDT_Unlock();
|
|
Cy_WDT_SetUpperLimit(SEAT_MOVE_WDT_TIMEOUT_TICKS);
|
|
Cy_WDT_SetUpperAction(CY_WDT_LOW_UPP_ACTION_RESET);
|
|
Cy_WDT_SetWarnAction(CY_WDT_WARN_ACTION_NONE);
|
|
Cy_WDT_SetAutoService(CY_WDT_DISABLE);
|
|
Cy_WDT_SetDebugRun(CY_WDT_ENABLE);
|
|
Cy_WDT_Lock();
|
|
if((WDT->unUPPER_LIMIT.stcField.u32UPPER_LIMIT !=
|
|
SEAT_MOVE_WDT_TIMEOUT_TICKS) ||
|
|
(WDT->unCONFIG.stcField.u1UPPER_ACTION !=
|
|
(uint32_t)CY_WDT_LOW_UPP_ACTION_RESET) ||
|
|
(WDT->unCONFIG.stcField.u1AUTO_SERVICE !=
|
|
(uint32_t)CY_WDT_DISABLE))
|
|
{
|
|
latchedFault = SAFETY_FAULT_DRIVER;
|
|
return;
|
|
}
|
|
Cy_WDT_Enable();
|
|
if(!SafetyManager_WaitForWatchdogState(true))
|
|
{
|
|
latchedFault = SAFETY_FAULT_DRIVER;
|
|
return;
|
|
}
|
|
Cy_WDT_ClearWatchdog();
|
|
Cy_SysTick_DelayInUs(1000u);
|
|
if(Cy_WDT_GetCount() == 0ul)
|
|
{
|
|
latchedFault = SAFETY_FAULT_DRIVER;
|
|
return;
|
|
}
|
|
watchdogInitialized = true;
|
|
SafetyManager_KickWatchdog();
|
|
}
|
|
|
|
void SafetyManager_KickWatchdog(void)
|
|
{
|
|
if((false != watchdogInitialized) && !hilWatchdogResetArmed)
|
|
{
|
|
Cy_WDT_ClearWatchdog();
|
|
}
|
|
}
|
|
|
|
bool SafetyManager_IsWatchdogInitialized(void)
|
|
{
|
|
return watchdogInitialized;
|
|
}
|
|
|
|
void SafetyManager_LatchFault(safety_fault_t fault)
|
|
{
|
|
if((fault != SAFETY_FAULT_NONE) &&
|
|
(latchedFault == SAFETY_FAULT_NONE))
|
|
{
|
|
latchedFault = fault;
|
|
Motor_LatchSafetyInhibitFromIsr(0u);
|
|
Motor_StopAll();
|
|
SeatMotorManager_StopAll(0ul);
|
|
}
|
|
}
|
|
|
|
void SafetyManager_Service(void)
|
|
{
|
|
bool debuggerAttached = SafetyManager_DebuggerAttached();
|
|
uint32_t marker = SafetyManager_ReadDebugMarker();
|
|
|
|
if(marker == SEAT_MOVE_DEBUG_EXIT_MAGIC)
|
|
{
|
|
SafetyManager_LatchFault(SAFETY_FAULT_DEBUG_DETACH);
|
|
}
|
|
|
|
if(Motor_IsDriverFaultLatched())
|
|
{
|
|
SafetyManager_LatchFault(SAFETY_FAULT_DRIVER);
|
|
}
|
|
else
|
|
{
|
|
/* No newly observed driver fault. */
|
|
}
|
|
|
|
#if (SAFETY_DEBUG_DETACH_SUPPORTED == 1u)
|
|
if(debuggerAttached)
|
|
{
|
|
if((marker != SEAT_MOVE_DEBUG_ACTIVE_MAGIC) &&
|
|
!SafetyManager_WriteDebugMarker(SEAT_MOVE_DEBUG_ACTIVE_MAGIC))
|
|
{
|
|
SafetyManager_LatchFault(SAFETY_FAULT_DEBUG_DETACH);
|
|
}
|
|
debuggerWasAttached = true;
|
|
}
|
|
else if(debuggerWasAttached)
|
|
{
|
|
SafetyManager_LatchFault(SAFETY_FAULT_DEBUG_DETACH);
|
|
debuggerWasAttached = false;
|
|
}
|
|
#else
|
|
(void)debuggerAttached;
|
|
#endif
|
|
|
|
SafetyManager_KickWatchdog();
|
|
}
|
|
|
|
bool SafetyManager_IsInhibited(void)
|
|
{
|
|
return (latchedFault != SAFETY_FAULT_NONE);
|
|
}
|
|
|
|
safety_fault_t SafetyManager_GetFault(void)
|
|
{
|
|
return latchedFault;
|
|
}
|
|
|
|
bool SafetyManager_WasWatchdogReset(void)
|
|
{
|
|
return watchdogReset;
|
|
}
|
|
|
|
bool SafetyManager_TriggerHilWatchdogReset(uint16_t token)
|
|
{
|
|
uint32_t check;
|
|
|
|
if(!watchdogInitialized || hilWatchdogResetArmed)
|
|
{
|
|
return false;
|
|
}
|
|
BACKUP->unBREG[SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register =
|
|
SEAT_MOVE_HIL_WDT_MARKER;
|
|
BACKUP->unBREG[SEAT_MOVE_HIL_WDT_TOKEN_BREG_INDEX].u32Register = token;
|
|
check = SEAT_MOVE_HIL_WDT_MARKER ^ (uint32_t)token ^ 0xFFFFFFFFu;
|
|
BACKUP->unBREG[SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register = check;
|
|
__DSB();
|
|
if((BACKUP->unBREG[
|
|
SEAT_MOVE_HIL_WDT_MAGIC_BREG_INDEX].u32Register !=
|
|
SEAT_MOVE_HIL_WDT_MARKER) ||
|
|
(BACKUP->unBREG[
|
|
SEAT_MOVE_HIL_WDT_CHECK_BREG_INDEX].u32Register != check))
|
|
{
|
|
SafetyManager_ClearHilWatchdogCredential();
|
|
return false;
|
|
}
|
|
/* Long-running motor calibration/coast loops normally service WDT
|
|
* directly so they remain safe even while the application loop is busy.
|
|
* The armed HIL case must suspend both service paths or a real watchdog
|
|
* reset can never occur. Startup re-enables this after the reset. */
|
|
Motor_SetWatchdogServiceEnabled(false);
|
|
Cy_WDT_ClearWatchdog();
|
|
hilWatchdogResetArmed = true;
|
|
return true;
|
|
}
|
|
|
|
void SafetyManager_CommitHilWatchdogReset(void)
|
|
{
|
|
if(!hilWatchdogResetArmed)
|
|
{
|
|
return;
|
|
}
|
|
|
|
/* This bench's connected CMSIS-DAP can suppress the WDT reset output even
|
|
* after C-SPY detaches. First prove physical WDT expiry, then use the
|
|
* reference project's authenticated processor-reset fallback. */
|
|
while(Cy_WDT_GetCount() < SEAT_MOVE_WDT_TIMEOUT_TICKS)
|
|
{
|
|
__NOP();
|
|
}
|
|
__DSB();
|
|
NVIC_SystemReset();
|
|
for(;;)
|
|
{
|
|
__NOP();
|
|
}
|
|
}
|
|
|
|
bool SafetyManager_WasHilWatchdogRecovered(void)
|
|
{
|
|
return hilWatchdogRecovered;
|
|
}
|