Teensy 4 RTWDOG implementation

Teensy4_RTWDOG.cpp

#include "Teensy4_RTWDOG.h"

static void unlock() {
    WDOG3_CNT = 0xD928C520; // unlock watchdog
    while ((WDOG3_CS & WDOG_CS_ULK) == 0); // wait until registers are unlocked    
}

void RTWDOG::disable() {
    __disable_irq();
    unlock();
    WDOG3_CS &= ~WDOG_CS_EN; // disable watchdog
    NVIC_DISABLE_IRQ(IRQ_RTWDOG); // disable interrupt
    __enable_irq();
}

uint16_t RTWDOG::ms_timeout(float ms) {
    return (uint16_t)round(32.768f*ms); // LPO is given by XTAL Y2 @ 32.768 KHz
}

void RTWDOG::enable(uint16_t timeout, uint16_t window, void (*isr)(void)) {
    __disable_irq();

    // Have we got an interrupt service routine?
    if (isr != NULL) {
        attachInterruptVector(IRQ_RTWDOG, isr);
        NVIC_SET_PRIORITY(IRQ_RTWDOG, 0);
        NVIC_ENABLE_IRQ(IRQ_RTWDOG);
    }
    else {
        NVIC_DISABLE_IRQ(IRQ_RTWDOG);
    }

    // Unlock watchdog:
    unlock();

    // Set timeout:
    WDOG3_TOVAL = timeout;

    // Set window:
    WDOG3_WIN = window;

    // Set control bits:
    uint16_t CS = WDOG_CS_EN | WDOG_CS_CLK(1) | WDOG_CS_UPDATE | WDOG_CS_CMD32EN;
    if (isr != NULL) CS |= WDOG_CS_INT;
    if (window > 0) CS |= WDOG_CS_WIN;
    WDOG3_CS = CS;
    
    while ((WDOG3_CS & WDOG_CS_RCS) == 0); // wait until new configuration takes effect 

    __enable_irq(); //enable global interrupt
}

void RTWDOG::feed() {
    __disable_irq();
    WDOG3_CNT = 0xB480A602; // refresh watchdog
    __enable_irq();
}

void RTWDOG::bark() {
    __disable_irq();
    WDOG3_CNT = 0x00000000; // illegal write will trigger rtwdog
    __enable_irq();
}