Cognitive_Agent/firmware/t2g/Seat_Controller_demo/App/safety_manager.c

#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;
#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);
}

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 debuggerAttached = SafetyManager_DebuggerAttached();
    uint32_t marker = SafetyManager_ReadDebugMarker();
    bool markerOk = true;

    latchedFault = SAFETY_FAULT_NONE;
    watchdogInitialized = false;
    watchdogReset = ((resetReason & watchdogMask) != 0ul);
    if((resetReason & CY_SYSRESET_PORVDDD) != 0ul)
    {
        markerOk = SafetyManager_WriteDebugMarker(0u);
        marker = SafetyManager_ReadDebugMarker();
    }
#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)
    {
        latchedFault = SAFETY_FAULT_BOOT_RECOVERY;
    }
    else if(debuggerAttached &&
            !SafetyManager_WriteDebugMarker(SEAT_MOVE_DEBUG_ACTIVE_MAGIC))
    {
        latchedFault = SAFETY_FAULT_DEBUG_DETACH;
    }

    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_SetDebugRun(CY_WDT_ENABLE);
    Cy_WDT_Lock();
    Cy_WDT_Enable();
    watchdogInitialized = true;
    SafetyManager_KickWatchdog();
}

void SafetyManager_KickWatchdog(void)
{
    if(false != watchdogInitialized)
    {
        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;
}