midware_clock_manager_diag.c

Go to the documentation of this file.

 
// Standard Library Includes
#include <stdbool.h>
#include <stdint.h>
 
// Framework Includes
#include <driver_clkctrl.h>
#include <driver_errctrl.h>
#include <midware_clock_manager_diag.h>
#include <midware_shared_diag_private.h>
 
// Device-specific Includes
#include <xc.h>

#define CRITICAL_CH_CONFIG ((uint8_t)ERRCTRL_FLOAT_bm | (uint8_t)ERRCTRL_ERRLVL_CRITICAL_gc)

#define CFD_RESET_CONFIG 0U

typedef struct
{
    uint16_t windowLow;
    uint16_t windowHigh;
    uint16_t windowRef;
    uint8_t ctrlA;
    uint8_t ctrlB;
    uint8_t errChBackup;
} cfmConfig_t;

typedef struct
{
    bool isDiagOngoing;
    bool wasIntEnabled;
    uint8_t errChBackup;
    uint8_t ctrlABackup;
} cfdState_t;
 
// Error Controller bitmasks used when reading the error channel register
static const uint32_t cfd0ChMask = 1UL << ERRCTRL_ESF_9_bp;
static const uint32_t cfd1ChMask = 1UL << ERRCTRL_ESF_10_bp;
static const uint32_t cfm0ChMask = 1UL << ERRCTRL_ESF_11_bp;
static const uint32_t cfm1ChMask = 1UL << ERRCTRL_ESF_12_bp;
 
/*
 * Variables used for tracking internal state and restoring error channel configs after diagnostics
 * are complete. Must be written to atomically to avoid any race conditions.
 */
static volatile cfdState_t cfd0State = {
    .isDiagOngoing = false,
    .wasIntEnabled = false,
    .errChBackup   = CRITICAL_CH_CONFIG,
    .ctrlABackup   = CFD_RESET_CONFIG,
};
 
static volatile cfdState_t cfd1State = {
    .isDiagOngoing = false,
    .wasIntEnabled = false,
    .errChBackup   = CRITICAL_CH_CONFIG,
    .ctrlABackup   = CFD_RESET_CONFIG,
};
 
// Private function prototypes
static void SetupCfd(bool isCfd0Diag);
static void BackupDisableCfdInterrupts(bool isCfd0Diag);
static void BackupDisableErrorChConfig(bool isCfd0Diag);
static void BackupConfigureCfd(bool isCfd0Diag);
static void DisableCfdChannel(bool isCfd0Diag);
 
static bool IsCfdDiagPassed(uint8_t cfmMask, uint32_t cfmChMask);
 
static void RestoreCfd(bool isCfd0Diag);
static void RestoreCfdChannel(bool isCfd0Diag);
static void RestoreCfdInterrupts(bool isCfd0Diag);
 
static cfmConfig_t SetupCfm(bool isCfm0Diag);
static cfmConfig_t BackupCfmConfig(bool isCfm0Diag);
static cfmConfig_t BackupCfm0Config(void);
static cfmConfig_t BackupCfm1Config(void);
static void DisableCfmChannel(bool isCfm0Diag);
static void ConfigureCfmRefOsc(bool isCfm0Diag);
 
static bool IsCfmDiagPassed(bool isCfm0Diag);
static bool IsCfm0TestPassed(uint16_t windowLow, uint16_t windowHigh, bool expectErr);
static bool IsCfm1TestPassed(uint16_t windowLow, uint16_t windowHigh, bool expectErr);
static bool CheckCfmTest(uint8_t cfmMask, uint8_t cfmDoneMask, uint32_t cfmChMask, bool expectErr);
static bool IsCfmFlagSet(uint8_t cfmMask, uint8_t cfmDoneMask);
 
static void RestoreCfmConfig(bool isCfm0Diag, cfmConfig_t config);
static void RestoreCfm0Config(cfmConfig_t config);
static void RestoreCfm1Config(cfmConfig_t config);
static void RestoreCfmChannel(bool isCfm0Diag, uint8_t chConfig);
 
errFlag_t MW_DiagClockCfd0Begin(void)
{
    // Check if there are any unhandled errors and ensure the Error Controller is in NORMAL state
    const bool isConfigurable = IsErrChConfigurable(cfd0ChMask);
    if (cfd0State.isDiagOngoing || !isConfigurable)
    {
        /* cppcheck-suppress misra-c2012-15.5 */
        return ERROR;
    }
 
    AtomicSectionStart(); // Disable and backup global interrupts configuration
 
    bool isCfd0Diag = true; // CFD0 diagnostics
    // Backup, disable and configure hardware for diagnostic
    SetupCfd(isCfd0Diag);
 
    // Begin diagnostics
    const uint8_t cfd0Mask = (uint8_t)CLKCTRL_CFD0_bm;
    CLKCTRL_SetMainControlC(cfd0Mask);
 
    cfd0State.isDiagOngoing = true; // Set CFD0 diag to in progress
 
    AtomicSectionEnd(); // Restore global interrupts configuration
 
    return NO_ERROR;
}
 
errFlag_t MW_DiagClockCfd0End(void)
{
    if (cfd0State.isDiagOngoing == false)
    {
        // cppcheck-suppress misra-c2012-15.5
        return ERROR;
    }
 
    AtomicSectionStart(); // Disable and backup global interrupts configuration
 
    // End diagnostics
    const uint8_t cfd0Mask  = (uint8_t)CLKCTRL_CFD0_bm;
    const bool isDiagPassed = IsCfdDiagPassed(cfd0Mask, cfd0ChMask);
 
    cfd0State.isDiagOngoing = false; // Set CFD0 diag to done
 
    // Clear interrupt flags and error channels
    CLKCTRL_WriteIntFlags(cfd0Mask);
    ERRCTRL_WriteChannelStatus(cfd0ChMask);
 
    bool isCfd0Diag = true; // CFD0 diagnostics
    // Restore registers used in diagnostic
    RestoreCfd(isCfd0Diag);
 
    AtomicSectionEnd(); // Restore global interrupts configuration
 
    errFlag_t diagResults = ERROR;
    if (isDiagPassed)
    {
        diagResults = NO_ERROR;
    }
    return diagResults;
}
 
bool MW_IsDiagCfd0Ongoing(void)
{
    return cfd0State.isDiagOngoing;
}
 
errFlag_t MW_DiagClockCfd1Begin(void)
{
    // Check if there are any unhandled errors and ensure the Error Controller is in NORMAL state
    const bool isConfigurable = IsErrChConfigurable(cfd1ChMask);
    if (cfd1State.isDiagOngoing || !isConfigurable)
    {
        /* cppcheck-suppress misra-c2012-15.5 */
        return ERROR;
    }
 
    AtomicSectionStart(); // Disable and backup global interrupts configuration
 
    bool isCfd0Diag = false; // CFD1 diagnostics
    // Backup, disable and configure hardware for diagnostic
    SetupCfd(isCfd0Diag);
 
    // Begin diagnostics
    const uint8_t cfd1Mask = (uint8_t)CLKCTRL_CFD1_bm;
    CLKCTRL_SetMainControlC(cfd1Mask);
    cfd1State.isDiagOngoing = true; // Set CFD1 diag to in progress
 
    AtomicSectionEnd(); // Restore global interrupts configuration
 
    return NO_ERROR;
}
 
errFlag_t MW_DiagClockCfd1End(void)
{
    if (cfd1State.isDiagOngoing == false)
    {
        // cppcheck-suppress misra-c2012-15.5
        return ERROR;
    }
 
    AtomicSectionStart(); // Disable and backup global interrupts configuration
 
    // End diagnostics
    const uint8_t cfd1Mask  = (uint8_t)CLKCTRL_CFD1_bm;
    const bool isDiagPassed = IsCfdDiagPassed(cfd1Mask, cfd1ChMask);
 
    cfd1State.isDiagOngoing = false; // Set CFD1 diag to done
 
    // Clear interrupt flags and error channels
    CLKCTRL_WriteIntFlags(cfd1Mask);
    ERRCTRL_WriteChannelStatus(cfd1ChMask);
 
    // Restore registers used in diagnostic
    bool isCfd0Diag = false; // CFD1 diagnostics
    RestoreCfd(isCfd0Diag);
 
    AtomicSectionEnd(); // Restore global interrupts configuration
 
    errFlag_t diagResults = ERROR;
    if (isDiagPassed)
    {
        diagResults = NO_ERROR;
    }
    return diagResults;
}
 
bool MW_IsDiagCfd1Ongoing(void)
{
    return cfd1State.isDiagOngoing;
}
 
errFlag_t MW_DiagClockCfm0(void)
{
    // Check if there are any unhandled errors and ensure the Error Controller is in NORMAL state
    const bool isConfigurable = IsErrChConfigurable(cfm0ChMask);
    if (!isConfigurable)
    {
        /* cppcheck-suppress misra-c2012-15.5 */
        return ERROR;
    }
 
    AtomicSectionStart(); // Disable and backup global interrupts configuration
 
    bool isCfm0Diag        = true; // CFM0 diagnostics
    // Backup registers used in diagnostic
    cfmConfig_t cfm0Backup = SetupCfm(isCfm0Diag);
 
    // Run tests for measurement within, above and below window
    const bool isDiagPassed = IsCfmDiagPassed(isCfm0Diag);
 
    uint8_t monitorEnableMask = (uint8_t)CLKCTRL_MEN_bm;
    CLKCTRL_ClearCfm0ControlA(monitorEnableMask); // Disable CFM0 to do config
 
    RestoreCfmConfig(isCfm0Diag, cfm0Backup); // Restore CFM configuration
 
    AtomicSectionEnd(); // Restore global interrupts configuration
 
    errFlag_t diagResults = ERROR;
    if (isDiagPassed)
    {
        diagResults = NO_ERROR;
    }
    return diagResults;
}
 
errFlag_t MW_DiagClockCfm1(void)
{
    // Check if there are any unhandled errors and ensure the Error Controller is in NORMAL state
    const bool isConfigurable = IsErrChConfigurable(cfm1ChMask);
    if (!isConfigurable)
    {
        /* cppcheck-suppress misra-c2012-15.5 */
        return ERROR;
    }
 
    AtomicSectionStart(); // Disable and backup global interrupts configuration
 
    bool isCfm0Diag        = false; // CFM1 diagnostics
    // Backup registers used in diagnostic
    cfmConfig_t cfm1Backup = SetupCfm(isCfm0Diag);
 
    // Run tests for measurement within, above and below window
    const bool isDiagPassed = IsCfmDiagPassed(isCfm0Diag);
 
    uint8_t monitorEnableMask = (uint8_t)CLKCTRL_MEN_bm;
    CLKCTRL_ClearCfm1ControlA(monitorEnableMask); // Disable CFM1 to do config
 
    // Restore registers used in diagnostic
    RestoreCfmConfig(isCfm0Diag, cfm1Backup); // Restore CFM configuration
 
    AtomicSectionEnd(); // Restore global interrupts configuration
 
    errFlag_t diagResults = ERROR;
    if (isDiagPassed)
    {
        diagResults = NO_ERROR;
    }
    return diagResults;
}
 
// Backup, disable and configure hardware for diagnostic
static void SetupCfd(bool isCfd0Diag)
{
    // Store and disable CFD interrupts between begin and end diagnostic
    BackupDisableCfdInterrupts(isCfd0Diag);
 
    // Store error channel config to global state variable and set to NOTIFICATION
    BackupDisableErrorChConfig(isCfd0Diag);
 
    // Store current CFD config and set it to diagnostic config
    BackupConfigureCfd(isCfd0Diag);
}
 
// Backup and disable CFD interrupts, stays of during diag
static void BackupDisableCfdInterrupts(bool isCfd0Diag)
{
    const uint8_t intControl = CLKCTRL_ReadIntControl();
    uint8_t intMask;
    if (isCfd0Diag)
    {
        intMask                 = (uint8_t)CLKCTRL_CFD0_bm;
        cfd0State.wasIntEnabled = ((intControl & intMask) == intMask);
    }
    else
    {
        intMask                 = (uint8_t)CLKCTRL_CFD1_bm;
        cfd1State.wasIntEnabled = ((intControl & intMask) == intMask);
    }
    CLKCTRL_ClearIntControl(intMask);
}
 
// Backup and disable CFD error channel
static void BackupDisableErrorChConfig(bool isCfd0Diag)
{
    if (isCfd0Diag)
    {
        cfd0State.errChBackup = ERRCTRL_ReadConfigCfd0();
        DisableCfdChannel(isCfd0Diag); // Disables float and sets channel to NOTIFICATION
    }
    else
    {
        cfd1State.errChBackup = ERRCTRL_ReadConfigCfd1();
        DisableCfdChannel(isCfd0Diag); // Disables float and sets channel to NOTIFICATION
    }
}
 
// Backup and configure CFD for diagnostic
static void BackupConfigureCfd(bool isCfd0Diag)
{
    const uint8_t cfdEnableMask   = (uint8_t)CLKCTRL_CFDEN_bm;
    const uint8_t srcOscHfConfig  = (uint8_t)CLKCTRL_CFDSRC_OSCHF_gc;
    const uint8_t refOsc32kConfig = (uint8_t)CLKCTRL_CFDREF_OSC32K_gc;
 
    if (isCfd0Diag)
    {
        cfd0State.ctrlABackup = CLKCTRL_ReadCfd0ControlA();
        CLKCTRL_WriteCfd0ControlA(srcOscHfConfig | refOsc32kConfig | cfdEnableMask);
    }
    else
    {
        cfd1State.ctrlABackup = CLKCTRL_ReadCfd1ControlA();
        CLKCTRL_WriteCfd1ControlA(srcOscHfConfig | refOsc32kConfig | cfdEnableMask);
    }
}
 
// Sets the specified CFD Error Channel to NOTIFICATION severity and disables I/O float
static void DisableCfdChannel(bool isCfd0Diag)
{
    const uint8_t stateMask        = (uint8_t)ERRCTRL_STATE_gm;
    const uint8_t floatConfig      = 0U << ERRCTRL_FLOAT_bp; // Disable I/O float
    const uint8_t severityConfig   = (uint8_t)(ERRCTRL_ERRLVL_NOTIFICATION_gc);
    const uint8_t disabledChConfig = (floatConfig | severityConfig);
 
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_CONFIG_gc);
    if (isCfd0Diag)
    {
        ERRCTRL_WriteConfigCfd0(disabledChConfig);
    }
    else
    {
        ERRCTRL_WriteConfigCfd1(disabledChConfig);
    }
 
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_NORMAL_gc);
}
 
// Read interrupt flag and error controller to determine if diag passed
static bool IsCfdDiagPassed(uint8_t cfmMask, uint32_t cfmChMask)
{
    const uint8_t intFlags  = CLKCTRL_ReadIntFlags();
    const bool isCfdFlagSet = ((intFlags & cfmMask) == cfmMask);
 
    const uint32_t chStatus = ERRCTRL_ReadChannelStatus();
    const bool isErrorChSet = ((chStatus & cfmChMask) == cfmChMask);
 
    return (isCfdFlagSet && isErrorChSet);
}
 
// Restore CFD config from backup
static void RestoreCfd(bool isCfd0Diag)
{
    // Restore CFD CTRLA register
    if (isCfd0Diag)
    {
        CLKCTRL_WriteCfd0ControlA(cfd0State.ctrlABackup);
    }
    else
    {
        CLKCTRL_WriteCfd1ControlA(cfd1State.ctrlABackup);
    }
 
    // Restore error channel config
    RestoreCfdChannel(isCfd0Diag);
 
    // Restore global and CFD0 interrupts configuration
    RestoreCfdInterrupts(isCfd0Diag);
}
 
// Restores the specified CFD Error Channel configs to their backed up configurations
static void RestoreCfdChannel(bool isCfd0Diag)
{
    const uint8_t stateMask = (uint8_t)ERRCTRL_STATE_gm;
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_CONFIG_gc);
    if (isCfd0Diag)
    {
        ERRCTRL_WriteConfigCfd0(cfd0State.errChBackup);
    }
    else
    {
        ERRCTRL_WriteConfigCfd1(cfd1State.errChBackup);
    }
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_NORMAL_gc);
}
 
// Restore CFD interrupts
static void RestoreCfdInterrupts(bool isCfd0Diag)
{
    if (isCfd0Diag)
    {
        if (cfd0State.wasIntEnabled)
        {
            uint8_t intMask = (uint8_t)CLKCTRL_CFD0_bm;
            CLKCTRL_SetIntControl((uint8_t)intMask);
        }
    }
    else
    {
        if (cfd1State.wasIntEnabled)
        {
            uint8_t intMask = (uint8_t)CLKCTRL_CFD1_bm;
            CLKCTRL_SetIntControl((uint8_t)intMask);
        }
    }
}
 
// Configures the CFM and Error Controller for diagnostics
static cfmConfig_t SetupCfm(bool isCfm0Diag)
{
    // Backup registers used in diagnostic
    cfmConfig_t cfmBackup = BackupCfmConfig(isCfm0Diag);
 
    DisableCfmChannel(isCfm0Diag); // Disables float and sets channel to NOTIFICATION
 
    // Set reference clock to fastest internal clock and enable
    ConfigureCfmRefOsc(isCfm0Diag);
 
    return cfmBackup;
}
 
// Backs up CFM configuration before diagnostic
static cfmConfig_t BackupCfmConfig(bool isCfm0Diag)
{
    cfmConfig_t cfmConfig;
    if (isCfm0Diag)
    {
        cfmConfig = BackupCfm0Config();
    }
    else
    {
        cfmConfig = BackupCfm1Config();
    }
    return cfmConfig;
}
 
// Backs up CFM0 configuration before diagnostic
static cfmConfig_t BackupCfm0Config(void)
{
    cfmConfig_t cfmConfig;
    cfmConfig.errChBackup = ERRCTRL_ReadConfigCfm0();
    cfmConfig.windowLow   = CLKCTRL_ReadCfm0WindowLow();
    cfmConfig.windowHigh  = CLKCTRL_ReadCfm0WindowHigh();
    cfmConfig.windowRef   = CLKCTRL_ReadCfm0RefNum();
    cfmConfig.ctrlA       = CLKCTRL_ReadCfm0ControlA();
    cfmConfig.ctrlB       = CLKCTRL_ReadCfm0ControlB();
    return cfmConfig;
}
 
// Backs up CFM1 configuration before diagnostic
static cfmConfig_t BackupCfm1Config(void)
{
    cfmConfig_t cfmConfig;
    cfmConfig.errChBackup = ERRCTRL_ReadConfigCfm1();
    cfmConfig.windowLow   = CLKCTRL_ReadCfm1WindowLow();
    cfmConfig.windowHigh  = CLKCTRL_ReadCfm1WindowHigh();
    cfmConfig.windowRef   = CLKCTRL_ReadCfm1RefNum();
    cfmConfig.ctrlA       = CLKCTRL_ReadCfm1ControlA();
    cfmConfig.ctrlB       = CLKCTRL_ReadCfm1ControlB();
    return cfmConfig;
}
 
// Sets the specified CFM Error Channel to NOTIFICATION severity and disables I/O float
static void DisableCfmChannel(bool isCfm0Diag)
{
    const uint8_t stateMask        = (uint8_t)ERRCTRL_STATE_gm;
    const uint8_t floatConfig      = 0U << ERRCTRL_FLOAT_bp; // Disable I/O float
    const uint8_t severityConfig   = (uint8_t)(ERRCTRL_ERRLVL_NOTIFICATION_gc);
    const uint8_t disabledChConfig = (floatConfig | severityConfig);
 
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_CONFIG_gc);
    if (isCfm0Diag)
    {
        ERRCTRL_WriteConfigCfm0(disabledChConfig);
    }
    else
    {
        ERRCTRL_WriteConfigCfm1(disabledChConfig);
    }
 
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_NORMAL_gc);
}
 
// Configures the CFM reference clock and the number of cycles to monitor
static void ConfigureCfmRefOsc(bool isCfm0Diag)
{
    uint8_t monitorEnableMask = (uint8_t)CLKCTRL_MEN_bm;
    // Set high frequency oscillator as reference and enable ref
    uint8_t cfmCtrlBConfig    = (uint8_t)CLKCTRL_CFMREF_OSCHF_gc | (uint8_t)CLKCTRL_CFMREF_gm;
 
    if (isCfm0Diag)
    {
        CLKCTRL_ClearCfm0ControlA(monitorEnableMask); // Disable CFM0
        CLKCTRL_WriteCfm0RefNum(8U); // Set number of reference clock cycles to monitor
        CLKCTRL_WriteCfm0ControlB(cfmCtrlBConfig);
    }
    else
    {
        CLKCTRL_ClearCfm1ControlA(monitorEnableMask); // Disable CFM1
        CLKCTRL_WriteCfm1RefNum(8U); // Set number of reference clock cycles to monitor
        CLKCTRL_WriteCfm1ControlB(cfmCtrlBConfig);
    }
}
 
/*
 * Runs all 3 CFM diagnostic tests and returns the result
 *
 * The number of reference clock cycles to measure the source clock is set in ConfigureCfmRefOsc
 * to 8. Due to hardware synchronization the measurement might count between 6 and 10 cycles.
 * The window must then be configures so that the expected measurement of 6-10 is inside, above
 * or below depending on which part is tested.
 */
static bool IsCfmDiagPassed(bool isCfm0Diag)
{
    bool (*cfmTest)(uint16_t windowLow, uint16_t windowHigh, bool expectErr);
    if (isCfm0Diag)
    {
        cfmTest = &IsCfm0TestPassed;
    }
    else
    {
        cfmTest = &IsCfm1TestPassed;
    }
 
    // Reports no error if inside window
    uint16_t windowLow            = 5U;
    uint16_t windowHigh           = 11U;
    bool expectErr                = false;
    const bool isDiagInsidePassed = cfmTest(windowLow, windowHigh, expectErr);
 
    // Reports error if above window
    windowLow                   = 2U;
    windowHigh                  = 5U;
    expectErr                   = true;
    const bool isDiagOverPassed = cfmTest(windowLow, windowHigh, expectErr);
 
    // Reports error if below window
    windowLow                    = 12U;
    windowHigh                   = 15U;
    expectErr                    = true;
    const bool isDiagUnderPassed = cfmTest(windowLow, windowHigh, expectErr);
 
    return (isDiagInsidePassed && isDiagOverPassed && isDiagUnderPassed);
}
 
// Runs one CFM0 test based on input and returns the results
static bool IsCfm0TestPassed(uint16_t windowLow, uint16_t windowHigh, bool expectErr)
{
    // Bitmasks used for this diagnostic
    const uint8_t monitorEnableMask = (uint8_t)CLKCTRL_MEN_bm;
    const uint8_t srcOscHfMask      = (uint8_t)CLKCTRL_CFMSRC_OSCHF_gc;
    const uint8_t oneShotMask       = (uint8_t)CLKCTRL_TYPE_ONESHOT_gc;
    const uint8_t cfmMask           = (uint8_t)CLKCTRL_CFM0_bm;
    const uint8_t cfmDoneMask       = (uint8_t)CLKCTRL_CFMD0_bm;
 
    CLKCTRL_ClearCfm0ControlA(monitorEnableMask); // Disable CFM0
    CLKCTRL_WriteCfm0WindowLow(windowLow);
    CLKCTRL_WriteCfm0WindowHigh(windowHigh);
 
    CLKCTRL_WriteCfm0ControlA(monitorEnableMask | srcOscHfMask | oneShotMask);
 
    const bool isDiagPassed = CheckCfmTest(cfmMask, cfmDoneMask, cfm0ChMask, expectErr);
 
    // Clear interrupt flags and error channels
    CLKCTRL_WriteIntFlags(cfmMask | cfmDoneMask);
    ERRCTRL_WriteChannelStatus(cfm0ChMask);
 
    return isDiagPassed; // Return true if flag set (Diag passed)
}
 
// Runs one CFM1 test based on input and returns the results
static bool IsCfm1TestPassed(uint16_t windowLow, uint16_t windowHigh, bool expectErr)
{
    // Bitmasks used for this diagnostic
    const uint8_t monitorEnableMask = (uint8_t)CLKCTRL_MEN_bm;
    const uint8_t srcOscHfMask      = (uint8_t)CLKCTRL_CFMSRC_OSCHF_gc;
    const uint8_t oneShotMask       = (uint8_t)CLKCTRL_TYPE_ONESHOT_gc;
    const uint8_t cfmMask           = (uint8_t)CLKCTRL_CFM1_bm;
    const uint8_t cfmDoneMask       = (uint8_t)CLKCTRL_CFMD1_bm;
 
    CLKCTRL_ClearCfm1ControlA(monitorEnableMask); // Disable CFM1
 
    CLKCTRL_WriteCfm1WindowLow(windowLow);
    CLKCTRL_WriteCfm1WindowHigh(windowHigh);
 
    CLKCTRL_WriteCfm1ControlA(monitorEnableMask | srcOscHfMask | oneShotMask);
 
    const bool isDiagPassed = CheckCfmTest(cfmMask, cfmDoneMask, cfm1ChMask, expectErr);
 
    // Clear interrupt flags and error channels
    CLKCTRL_WriteIntFlags(cfmMask | cfmDoneMask);
    ERRCTRL_WriteChannelStatus(cfm1ChMask);
 
    return isDiagPassed; // Return true if flag set (Diag passed)
}
 
// Reads the CFM interrupt flags and error channel and returns if triggered correctly
static bool CheckCfmTest(uint8_t cfmMask, uint8_t cfmDoneMask, uint32_t cfmChMask, bool expectErr)
{
    bool isIntFlagSet = IsCfmFlagSet(cfmMask, cfmDoneMask);
 
    const uint32_t chStatus = ERRCTRL_ReadChannelStatus();
    bool isErrorChSet       = ((chStatus & cfmChMask) == cfmChMask);
 
    bool isDiagPassed = false;
    if (expectErr)
    {
        // Interrupt flag and error channel set, diagnostic passed
        isDiagPassed = (isIntFlagSet && isErrorChSet);
    }
    else
    {
        // Interrupt flag and/or error channel not set, diagnostic failed
        isDiagPassed = !(isIntFlagSet || isErrorChSet);
    }
 
    return isDiagPassed;
}
 
// Reads CFM flag and returns state
static bool IsCfmFlagSet(uint8_t cfmMask, uint8_t cfmDoneMask)
{
    uint8_t intFlags;
    bool isCfmDone = false;
    while (!isCfmDone) // Wait until measurement is done
    {
        intFlags  = CLKCTRL_ReadIntFlags();
        isCfmDone = ((intFlags & cfmDoneMask) == cfmDoneMask);
    }
 
    return ((intFlags & cfmMask) == cfmMask);
}
 
// Restores the CFM registers and error channel used in the diagnostics
static void RestoreCfmConfig(bool isCfm0Diag, cfmConfig_t config)
{
    if (isCfm0Diag)
    {
        RestoreCfm0Config(config);
    }
    else
    {
        RestoreCfm1Config(config);
    }
    RestoreCfmChannel(isCfm0Diag, config.errChBackup);
}
 
// Restores the CFM0 registers used in the diagnostics
static void RestoreCfm0Config(cfmConfig_t config)
{
    CLKCTRL_WriteCfm0WindowLow(config.windowLow);
    CLKCTRL_WriteCfm0WindowHigh(config.windowHigh);
    CLKCTRL_WriteCfm0RefNum(config.windowRef);
    CLKCTRL_WriteCfm0ControlB(config.ctrlB);
    CLKCTRL_WriteCfm0ControlA(config.ctrlA); // Restored last to avoid monitor enabled in config
}
 
// Restores the CFM1 registers used in the diagnostics
static void RestoreCfm1Config(cfmConfig_t config)
{
    CLKCTRL_WriteCfm1WindowLow(config.windowLow);
    CLKCTRL_WriteCfm1WindowHigh(config.windowHigh);
    CLKCTRL_WriteCfm1RefNum(config.windowRef);
    CLKCTRL_WriteCfm1ControlB(config.ctrlB);
    CLKCTRL_WriteCfm1ControlA(config.ctrlA); // Restored last to avoid monitor enabled in config
}
 
// Restores the specified CFM Error Channel configs to their backed up configurations
static void RestoreCfmChannel(bool isCfm0Diag, uint8_t chConfig)
{
    const uint8_t stateMask = (uint8_t)ERRCTRL_STATE_gm;
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_CONFIG_gc);
    if (isCfm0Diag)
    {
        ERRCTRL_WriteConfigCfm0(chConfig);
    }
    else
    {
        ERRCTRL_WriteConfigCfm1(chConfig);
    }
    ERRCTRL_ModifyControlA(stateMask, (uint8_t)ERRCTRL_STATE_NORMAL_gc);
}