Go through QNX Startup Code for Raspberry Pi 4 (BCM2711)

void cpu_tweak_cmdline(struct bootargs_entry *bap, const char *name) {
    const static char *next_arg;

    if (!next_arg) { // only do this once
        char *str = mbox_get_cmdline();

        if (str == NULL) {
            return;
        }

        // skip vpu-inserted arguments up to dash or double-space
        do {
            next_arg = find_next_arg(str, &str, 0);
            if (next_arg[0] == '\0') break;
            if (next_arg[0] == '-') break;
            if ((next_arg[0] == ' ') && (next_arg[1] == ' ')) break;
        } while (1);

        for (;;) {
            if (next_arg[0] == '\0') break;
            if ((next_arg[0] == '-') && (next_arg[1] == '-')) break;
            bootstrap_arg_adjust(bap, NULL, next_arg);
            next_arg = find_next_arg(str, &str, 1);
        }
    }
}

const struct debug_device debug_devices[] = {
    {   .name = "miniuart",
        .defaults[DEBUG_DEV_CONSOLE] = "fe215000^0.115200",  /* uart1 */
        .init = init_miniuart,
        .put = put_miniuart,
        .callouts[DEBUG_DISPLAY_CHAR] = &display_char_miniuart,
        .callouts[DEBUG_POLL_KEY] = &poll_key_miniuart,
        .callouts[DEBUG_BREAK_DETECT] = &break_detect_miniuart,
    },
    {   .name = "pl011-3",
        .defaults[DEBUG_DEV_CONSOLE] = "fe201600^0.115200.48000000", /* uart 3*/
        .init = init_pl011,
        .put = put_pl011,
        .callouts[DEBUG_DISPLAY_CHAR] = &display_char_pl011,
        .callouts[DEBUG_POLL_KEY] = &poll_key_pl011,
        .callouts[DEBUG_BREAK_DETECT] = &break_detect_pl011,
    },
};

int main(const int argc, char ** const argv)
{
    int opt, options = 0;
    int wdt_timeout = BCM2711_WDT_USE_DEFAULT;
    uint32_t board_rev;

    // 1. Add callout array (reboot mechanism)
    const struct callout_slot callouts[] = {
        { CALLOUT_SLOT( reboot, _bcm2711 ) },
    };
    add_callout_array(callouts, sizeof callouts);

    // 2. Initialize board (GPIO/pinmux)
    init_board();

    // 3. Select debug device
    select_debug(debug_devices, sizeof(debug_devices));

    // 4. Parse command line options
    while ((opt = getopt(argc, argv, COMMON_OPTIONS_STRING "W:")) != -1) {
        switch (opt) {
            case 'W':
                options |= BCM2711_WDT_ENABLE;
                if (optarg) {
                    wdt_timeout = (int) strtol(optarg, &optarg, 0);
                }
                break;
            default:
                handle_common_option(opt);
                break;
        }
    }

    // 5. Enable watchdog if requested
    if (options & BCM2711_WDT_ENABLE) {
        bcm2711_wdt_enable(wdt_timeout);
    }

    // 6. Re-initialize debug device if "-D" option specified
    select_debug(debug_devices, sizeof(debug_devices));

    // 7. Get CPU frequency from VideoCore
    cpu_freq = mbox_get_clock_rate(MBOX_CLK_ARM);
    cycles_freq = cpu_freq;
    timer_freq = 0;

    // 8. Setup RAM
    bcm2711_init_raminfo();
    alloc_ram(shdr->ram_paddr, shdr->ram_size, 1);

    // 9. Enable Hypervisor if requested
    hypervisor_init(0);

    // 10. Initialize SMP
    init_smp();

    // 11. Debug info (if verbose)
    if (debug_flag > 3) {
        kprintf("%u CPUs @ %uMHz\n", lsp.syspage.p->num_cpu, cpu_freq / 1000000);
        kprintf("Temps %d\n", mbox_get_temperature(0));
        // ... more debug output
    }

    // 12. Initialize MMU if virtual mode
    if (shdr->flags1 & STARTUP_HDR_FLAGS1_VIRTUAL) {
        init_mmu();
    }

    // 13. Initialize interrupt system
    init_intrinfo();

    // 14. Initialize system timer
    init_qtime();

    // 15. Initialize cache attributes
    init_cacheattr();

    // 16. Initialize CPU info
    init_cpuinfo();

    // 17. Initialize hardware database
    init_hwinfo();

    // 18. Add board identification strings
    board_rev = mbox_get_board_revision();
    add_typed_string(_CS_MACHINE, get_board_name(board_rev));
    add_typed_string(_CS_HW_PROVIDER, get_mfr_name(board_rev));
    add_typed_string(_CS_ARCHITECTURE, get_soc_name(board_rev));
    add_typed_string(_CS_HW_SERIAL, get_board_serial());

    // 19. Load IFS and transfer control
    init_system_private();

    print_syspage();
    return 0;
}

#define FUNC_IP     0  // Input
#define FUNC_OP     1  // Output
#define FUNC_A0     4  // Alternate Function 0
#define FUNC_A1     5  // Alternate Function 1
#define FUNC_A2     6  // Alternate Function 2
#define FUNC_A3     7  // Alternate Function 3
#define FUNC_A4     3  // Alternate Function 4
#define FUNC_A5     2  // Alternate Function 5

typedef struct _bcm2711_pinmux {
    uint8_t pin_num;    // GPIO pin number (0-53)
    uint8_t fsel;       // Function select
    uint8_t pull;       // Pull-up/pull-down/none
    uint8_t gpio_lvl;   // Output level (if output)
    uint8_t is_last;    // Array terminator
} bcm2711_pinmux_t;

const bcm2711_config_t rpi_board_config[] = {
    {   // UART Configuration
        .pinmux = (bcm2711_pinmux_t[])
        {
            { .pin_num = 14, .fsel = FUNC_A5, .pull = PULL_UP, .gpio_lvl = 0 }, /* TXD0 */
            { .pin_num = 15, .fsel = FUNC_A5, .pull = PULL_UP, .gpio_lvl = 0 }, /* RXD0 */
            { .pin_num =  4, .fsel = FUNC_A4, .pull = PULL_UP, .gpio_lvl = 0 }, /* TXD3 */
            { .pin_num =  5, .fsel = FUNC_A4, .pull = PULL_UP, .gpio_lvl = 0 }, /* RXD3 */
            { .is_last = 1 }
        },
    },
    {   // SPI Configuration
        .pinmux = (bcm2711_pinmux_t[])
        {
            { .pin_num =  7, .fsel = FUNC_A0, .pull = PULL_UP,   .gpio_lvl = 0 }, /* SPI0_CE1_N */
            { .pin_num =  8, .fsel = FUNC_A0, .pull = PULL_UP,   .gpio_lvl = 0 }, /* SPI0_CE0_N */
            { .pin_num =  9, .fsel = FUNC_A0, .pull = PULL_NONE, .gpio_lvl = 0 }, /* SPI0_MISO */
            { .pin_num = 10, .fsel = FUNC_A0, .pull = PULL_NONE, .gpio_lvl = 0 }, /* SPI0_MOSI */
            { .pin_num = 11, .fsel = FUNC_A0, .pull = PULL_NONE, .gpio_lvl = 0 }, /* SPI0_SCLK */
            { .is_last = 1 }
        },
    },
    {   // I2C Configuration
        .pinmux = (bcm2711_pinmux_t[])
        {
            { .pin_num = 0, .fsel = FUNC_A0, .pull = PULL_UP, .gpio_lvl = 0 }, /* SDA0 */
            { .pin_num = 1, .fsel = FUNC_A0, .pull = PULL_UP, .gpio_lvl = 0 }, /* SCL0 */
            { .pin_num = 2, .fsel = FUNC_A0, .pull = PULL_UP, .gpio_lvl = 0 }, /* SDA1 */
            { .pin_num = 3, .fsel = FUNC_A0, .pull = PULL_UP, .gpio_lvl = 0 }, /* SCL1 */
            { .is_last = 1 }
        },
    },
};

static void bcm2711_set_gpio_fsel(const uint32_t gpio, const uint32_t fsel)
{
    const uint32_t reg = (gpio / 10) * 4;  // GPFSEL0-5 (10 pins per register)
    const uint32_t sel = gpio % 10;
    uint32_t val;

    if ((gpio < GPIO_MIN) || (gpio > GPIO_MAX)) {
        return;
    }

    val = in32(BCM2711_GPIO_BASE + reg);
    val &= ~(0x7 << (3 * sel));  // Clear 3-bit field
    val |=  (fsel & 0x7) << (3 * sel);  // Set new function
    out32(BCM2711_GPIO_BASE + reg, val);
}

static void bcm2711_gpio_set_pull(const uint32_t gpio, const uint32_t type)
{
    const uint32_t reg = GPPUPPDN0 + (gpio >> 4) * 4;  // 16 pins per register
    const uint32_t shift = (gpio & 0xf) << 1;  // 2 bits per pin
    uint32_t val;

    if ((gpio < GPIO_MIN) || (gpio > GPIO_MAX)) {
        return;
    }

    if ((type < 0) || (type > 2)) {
        return;
    }

    val = in32(BCM2711_GPIO_BASE + reg);
    val &= ~(3 << shift);  // Clear 2-bit field
    val |= (type << shift);  // Set new pull type
    out32(BCM2711_GPIO_BASE + reg, val);
}

const char *get_board_name(const uint32_t board_rev)
{
    const char * const types[] = {
        [0]  = "RaspberryPi1A",
        [1]  = "RaspberryPi1B",
        [2]  = "RaspberryPi1A+",
        [3]  = "RaspberryPi1B+",
        [4]  = "RaspberryPi2B",
        [5]  = "RaspberryPiAlpha",
        [6]  = "RaspberryPiCM1",
        [8]  = "RaspberryPi3B",
        [9]  = "RaspberryPi0",
        [10] = "RaspberryPiCM3",
        [12] = "RaspberryPi0W",
        [13] = "RaspberryPi3B+",
        [14] = "RaspberryPi3A+",
        [16] = "RaspberryPiCM3+",
        [17] = "RaspberryPi4B",      // ← Our target
        [18] = "RaspberryPi02W",
        [19] = "RaspberryPi400",
        [20] = "RaspberryPiCM4",
    };

    const uint32_t idx = (board_rev >> BOARD_REV_TYPE_SHIFT) & BOARD_REV_TYPE_MASK;
    if (idx < NUM_ELTS(types)) {
        return types[idx];
    }
    return "Unknown";
}

void bcm2711_init_raminfo(void) {
    const uint32_t vcram_size = mbox_get_vc_memory();
    const paddr_t vcram_start = GIG(1) - vcram_size;
    const uint32_t rev = mbox_get_board_revision();
    uint64_t ramsize = 0;

    /* Avoid touching the stub in startup */
    avoid_ram(BCM2711_DRAM_BASE, BCM2711_DRAM_STUB_SIZE);
    
    /* Avoid touching the VideoCore memory in startup */
    avoid_ram(vcram_start, vcram_size);

    ramsize = MEG(get_memory_size(rev));
    if (ramsize == 0) {
        ramsize = MEG(256);
        kprintf("Warning: Failed to detect RAM size. Defaulting to %u MB.\n",
            ramsize);
    }

    if (ramsize >= GIG(4)) {
        add_ram(BCM2711_DRAM_BASE, GIG(4) - MEG(64)); // periphs are mapped to 0xfc000000
        if (ramsize >= GIG(8)) {
            add_ram(BCM2711_DRAM_BASE_1, GIG(4));    // for rpi4 8GB target
        }
    } else {
        add_ram(BCM2711_DRAM_BASE, ramsize); // 256M, 512M, 1G, 2G
    }

    /* Reserve VideoCore memory */
    alloc_ram(vcram_start, vcram_size, 0);
    as_add_containing(vcram_start, vcram_start + vcram_size - 1, AS_ATTR_RAM, "vcram", "ram");

    /* Add a below1G tag for legacy peripherals */
    if (ramsize > GIG(1)) {
        as_add_containing(BCM2711_DRAM_BASE,  vcram_start - 1, AS_ATTR_RAM, "below1G", "ram");
    }

    if (debug_flag > 3) {
        kprintf("vcram start-size: %P-%P, memory size: %l\n", vcram_start, vcram_size, ramsize);
    }
}

void init_intrinfo(void)
{
    /*
     * Initialise GIC
     */
#ifndef BCM2711_GIC_V3
#define GICD_PADDR      0xff841000  // Distributor
#define GICC_PADDR      0xff842000  // CPU Interface
    gic_v2_init(GICD_PADDR, GICC_PADDR);
#else
#define GIC_GICD_BASE   0x4c0040000     // CPU distributor BASE address
#define GIC_GICR_BASE   0x4c0041000     // CPU redistributor BASE address
#define GIC_GICC_BASE   0x4c0042000     // CPU interface, CBAR register value
    gic_v3_init(GIC_GICD_BASE, GIC_GICR_BASE, GIC_GICC_BASE, 0, 1);
#endif

    // ... MSI interrupt setup follows
}

static const paddr_t bcm2711_msi_base = BCM2711_PCIE_BASE + 0x4000;

void init_intrinfo(void)
{
    const struct startup_intrinfo interrupts[] =
    {
        /* MSI interrupts (32) starting at 0x200 */
        {   .vector_base = 0x200,           // MSI vector base (MSI 0x200 - 0x21F)
            .num_vectors = 32,              // 32 MSI vectors
            .cascade_vector = BCM2711_PCIE_MSI, // Parent SPI interrupt
            .cpu_intr_base = 0,             // CPU-relative base (0-31)
            .cpu_intr_stride = 0,
            .flags = INTR_FLAG_MSI,
            .id = { .genflags = INTR_GENFLAG_NOGLITCH, .size = 0, 
                    .rtn = &interrupt_id_bcm2711_msi },
            .eoi = { .genflags = INTR_GENFLAG_LOAD_INTRMASK, .size = 0, 
                     .rtn = &interrupt_eoi_bcm2711_msi },
            .mask = &interrupt_mask_bcm2711_msi,
            .unmask = &interrupt_unmask_bcm2711_msi,
            .config = 0,
            .patch_data = (paddr_t*)&bcm2711_msi_base,
        },
    };

    add_interrupt_array(interrupts, sizeof interrupts);
}

void init_hwinfo(void)
{
    const unsigned hwi_bus_internal = 0;
    
    /* Add ENET (Ethernet) */
    {
        unsigned i, hwi_off;
        uint8_t mac[6];
        hwiattr_enet_t attr = HWIATTR_ENET_T_INITIALIZER;
        hwiattr_common_t common_attr = HWIATTR_COMMON_INITIALIZER;
        HWIATTR_ENET_SET_NUM_IRQ(&attr, 1);

        uint32_t irqs[] = {
            BCM2711_ENET_IRQA,  // Queue 0 interrupt
            BCM2711_ENET_IRQB   // Queue 16 interrupt
        };

        HWIATTR_SET_NUM_IRQ(&common_attr, NUM_ELTS(irqs));

        /* Create GENET device */
        HWIATTR_ENET_SET_LOCATION(&attr, BCM2711_ENET_BASE,
                BCM2711_ENET_SIZE, 0, hwi_find_as(BCM2711_ENET_BASE, 1));
        hwi_off = hwidev_add_enet(BCM2711_HWI_GENET, &attr, hwi_bus_internal);
        hwitag_add_common(hwi_off, &attr);
        ASSERT(hwi_find_unit(hwi_off) == 0);

        /* Add IRQ numbers */
        for(i = 0; i < NUM_ELTS(irqs); i++) {
            hwitag_set_ivec(hwi_off, i, irqs[i]);
        }

        /* Get MAC address from VideoCore mailbox */
        mbox_get_board_mac_address(mac);
        hwitag_add_nicaddr(hwi_off, mac, sizeof(mac));
    }

    /* add the WATCHDOG device */
    {
        unsigned hwi_off;
        hwiattr_timer_t attr = HWIATTR_TIMER_T_INITIALIZER;
        const struct hwi_inputclk clksrc_kick = {.clk = 500, .div = 1};
        
        HWIATTR_TIMER_SET_NUM_CLK(&attr, 1);
        HWIATTR_TIMER_SET_LOCATION(&attr, BCM2711_PM_BASE, BCM2711_PM_SIZE, 0, 
                                   hwi_find_as(BCM2711_PM_BASE, 1));
        hwi_off = hwidev_add_timer("wdog", &attr, HWI_NULL_OFF);
        ASSERT(hwi_off != HWI_NULL_OFF);
        hwitag_set_inputclk(hwi_off, 0, (struct hwi_inputclk *)&clksrc_kick);

        hwi_off = hwidev_add("wdt,options", 0, HWI_NULL_OFF);
        hwitag_add_regname(hwi_off, "write_width", 32);
        hwitag_add_regname(hwi_off, "enable_width", 32);
    }
}

// Example: Ethernet driver queries its configuration
#include <hw/hwinfo.h>

void enet_driver_init(void) {
    unsigned hwi_off = hwi_find_device("genet", 0);  // Find first GENET
    
    if (hwi_off != HWI_NULL_OFF) {
        uint64_t base;
        unsigned size;
        unsigned irq;
        uint8_t mac[6];
        
        // Get base address and size
        hwitag_find_ivaddr(hwi_off, NULL, &base, &size);
        
        // Get interrupt vector
        hwitag_find_ivec(hwi_off, 0, &irq);
        
        // Get MAC address
        hwitag_find_nicaddr(hwi_off, mac, sizeof(mac));
        
        kprintf("GENET at 0x%llx, IRQ %d, MAC %02x:%02x:%02x:%02x:%02x:%02x\n",
                base, irq, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
    }
}

unsigned board_smp_num_cpu(void) {
    return 4;  // BCM2711 has 4 cores
}

void board_smp_init(struct smp_entry *smp, const unsigned num_cpus)
{
#ifndef BCM2711_GIC_V3
    smp->send_ipi = (void *)&sendipi_gic_v2;
#else
    smp->send_ipi = (void *)&sendipi_gic_v3_sr;
#endif

    /* Need to turn on the IPI interrupt for mailbox 0 on all cores */
    uint32_t cpu;
    for (cpu = 0; cpu < num_cpus; ++cpu) {
        const uint32_t mbic_addr = 0x40000050 + 0x4 * cpu;
        uint32_t mbic = in32(mbic_addr);
        mbic |= 0x1;  // Enable mailbox 0 interrupt
        out32(mbic_addr, mbic);
    }
}

static void wake_aux_cpu(void)
{
    // Same assembly works in 32 and 64 bit
    __asm__ __volatile__(
        "dsb sy\n"      // Data Synchronization Barrier
        "sev\n"         // Send Event (wakes cores from WFE)
        : : : "memory"
    );
}

int board_smp_start(unsigned cpu, void (*start)(void))
{
    typedef void (*start_t)(void);
    volatile start_t *cpu_start = (start_t *)(0xd8L + cpu * 8);
    
    *cpu_start = start;  // Write start address to spin table
    wake_aux_cpu();      // Wake the core
    
    return 1;
}

// IPI mechanism (simplified)
void sendipi_gic_v2(struct syspage_entry *syspage, unsigned cpu, int cmd, void *arg)
{
    // GIC uses SGI (Software Generated Interrupt) for IPI
    // Write to GICD_SGIR register:
    // - Target CPU in bits [16:23]
    // - SGI number (0-15) in bits [0:3]
    
    const unsigned sgi_num = 0;  // Use SGI 0 for IPI
    const uint32_t gicd_sgir = GICD_BASE + 0xF00;
    
    // Send SGI to specific CPU
    out32(gicd_sgir, (1 << (16 + cpu)) | sgi_num);
}

#define MBOX_REG_BASE               0xfe00b880
#define MBOX_BUFFER_ADDR            0x100      // Fixed buffer at 0x100
#define MBOX_BUFFER_SIZE            128
#define MBOX_SEND_CHANNEL           8

// Mailbox registers
#define MBOX0_READ                  0x00       // Read from VC
#define MBOX0_STATUS                0x18       // Status flags
#define MBOX1_WRITE                 0x20       // Write to VC
#define MBOX1_STATUS                0x38       // Status flags

#define MBOX_STATUS_FULL            0x80000000
#define MBOX_STATUS_EMPTY           0x40000000

struct mbox_msg_header {
    uint32_t size;      // Total message size (including header)
    uint32_t code;      // Request: 0, Response: 0x80000000
};

struct mbox_msg_tag {
    uint32_t tag;       // Property tag ID
    uint32_t size;      // Buffer size
    uint32_t code;      // Request: 0, Response: 0x80000000 | value_length
};

// Example message format
struct mbox_msg_get_clock_rate {
    struct mbox_msg_header header;
    struct mbox_msg_tag tag;
    struct {
        uint32_t clock_id;  // Input: which clock
        uint32_t rate;      // Output: clock rate in Hz
    } body;
    uint32_t end;           // End tag: 0x00000000
};

static uint32_t mbox_send_message(void) {
    uint32_t rdata;

    // Wait for mailbox ready (not full)
    for (;;) {
        rdata = *((volatile uint32_t *) (MBOX_REG_BASE + MBOX0_STATUS));
        if (!(rdata & MBOX_STATUS_FULL)) {
            break;
        }
    }

    // Send message address | channel number
    rdata = (MBOX_BUFFER_ADDR & ~0xf) | MBOX_SEND_CHANNEL;
    *((volatile uint32_t *) (MBOX_REG_BASE + MBOX1_WRITE)) = rdata;

    // Wait for response
    for (;;) {
        // Wait for mailbox not empty
        rdata = *((volatile uint32_t *) (MBOX_REG_BASE + MBOX0_STATUS));
        if (rdata & MBOX_STATUS_EMPTY) {
            continue;
        }

        // Read response
        rdata = *((volatile uint32_t *) (MBOX_REG_BASE + MBOX0_READ));
        
        // Make sure it's for our channel
        if ((rdata & 0xf) == MBOX_SEND_CHANNEL) {
            return rdata & ~0xf;
        }
    }

    return 0;
}

uint32_t mbox_get_board_revision(void) {
    volatile struct mbox_msg_get_board_revision {
        struct mbox_msg_header header;
        struct mbox_msg_tag tag;
        struct {
            uint32_t revision;
        } body;
        uint32_t end;
    } *msg = (volatile struct mbox_msg_get_board_revision *) MBOX_BUFFER_ADDR;

    MSG_INIT(msg, MBOX_TAG_GET_BOARDREVISION);
    mbox_send_message();
    return msg->body.revision;
}

void mbox_get_board_mac_address(uint8_t* const address) {
    volatile struct mbox_msg_get_board_mac_address {
        struct mbox_msg_header header;
        struct mbox_msg_tag tag;
        struct {
            uint8_t address[6];
            uint8_t _pad[2];
        } body;
        uint32_t end;
    } *msg = (volatile struct mbox_msg_get_board_mac_address *) MBOX_BUFFER_ADDR;

    MSG_INIT(msg, MBOX_TAG_GET_MACADDRESS);
    mbox_send_message();
    memcpy(address, (uint8_t*) msg->body.address, 6);
}

uint32_t mbox_get_clock_rate(const uint32_t id) {
    volatile struct mbox_msg_get_clock_rate {
        struct mbox_msg_header header;
        struct mbox_msg_tag tag;
        struct {
            uint32_t clock_id;
            uint32_t rate;
        } body;
        uint32_t end;
    } *msg = (volatile struct mbox_msg_get_clock_rate *) MBOX_BUFFER_ADDR;

    MSG_INIT(msg, MBOX_TAG_GET_CLOCKRATE);
    msg->body.clock_id = id;
    mbox_send_message();
    return msg->body.rate;
}

int32_t mbox_get_temperature(const uint32_t id) {
    volatile struct {
        struct mbox_msg_header header;
        struct mbox_msg_tag tag;
        struct {
            uint32_t temperature_id;
            int32_t millidegrees;
        } body;
        uint32_t end;
    } *msg = (volatile void *) MBOX_BUFFER_ADDR;

    MSG_INIT(msg, MBOX_TAG_GET_TEMPERATURE);
    msg->body.temperature_id = id;
    mbox_send_message();
    return msg->body.millidegrees;  // Returns temperature in millidegrees C
}

uint32_t mbox_get_expgpio(const uint32_t gpio) {
    volatile struct {
        struct mbox_msg_header header;
        struct mbox_msg_tag tag;
        struct {
            uint32_t gpio;
            uint32_t state;
        } body;
        uint32_t end;
    } *msg = (volatile void *) MBOX_BUFFER_ADDR;

    MSG_INIT(msg, MBOX_TAG_GET_GPIO_STATE);
    msg->body.gpio = gpio;
    msg->body.state = 0;
    mbox_send_message();
    return msg->body.state;
}

// Get CPU frequency
cpu_freq = mbox_get_clock_rate(MBOX_CLK_ARM);

// Get memory configuration
const uint32_t vcram_size = mbox_get_vc_memory();

// Get board info
board_rev = mbox_get_board_revision();
uint64_t serial = mbox_get_board_serial();

// Debug output
if (debug_flag > 3) {
    kprintf("Temps %d\n", mbox_get_temperature(0));
    kprintf("Volts");
    for (unsigned i = 1; i <= 4; i++) {
        kprintf(" %u=%u", i, mbox_get_voltage(i));
    }
    kprintf("\n");
}

#define PM_PASSWORD                 0x5a000000  // Required for all PM writes
#define PM_WDOG_TIME_SET            0x000fffff  // Max watchdog value
#define PM_RSTC_WRCFG_CLR           0xffffffcf  // Clear reset config
#define PM_RSTS_HADWRH_SET          0x00000040  // Hardware reset flag
#define PM_RSTC_WRCFG_SET           0x00000030  // Full reset config
#define PM_RSTC_WRCFG_FULL_RESET    0x00000020  // Full reset mode
#define PM_RSTC_STOP                0x00000102  // Stop watchdog
#define PM_WDOG_MS_TO_TICKS         67UL        // Conversion factor
#define WDT_MAX_TICKS               0x000fffff  // Maximum ticks
#define WDT_DEFAULT_TICKS           0x00010000  // Default timeout

void bcm2711_wdt_enable(const int timeout_value)
{
    const uintptr_t base = (uintptr_t) BCM2711_PM_BASE;
    uint32_t val;
    uint64_t ticks;

    /* Disable watch dog first */
    out32(base + BCM2711_PM_RSTC, PM_PASSWORD | PM_RSTC_STOP);

    // Calculate timeout in ticks
    if (timeout_value <= 1000) {
        val = WDT_DEFAULT_TICKS;  // ~4.2 seconds
    } else {
        ticks = timeout_value * PM_WDOG_MS_TO_TICKS;
        if (ticks > WDT_MAX_TICKS) {
            ticks = WDT_MAX_TICKS;  // Cap at maximum (~262 seconds)
        }
        val = (uint32_t) ticks;
    }
    
    /* Reload the timer value */
    out32(base + BCM2711_PM_WDOG, PM_PASSWORD | val);

    /* Enable the watchdog timer in full reset mode */
    val = in32(base + BCM2711_PM_RSTC) & PM_RSTC_WRCFG_CLR;
    val |= (PM_PASSWORD | PM_RSTC_WRCFG_FULL_RESET);
    out32(base + BCM2711_PM_RSTC, val);
}

// Typical wdtkick implementation
while (1) {
    out32(BCM2711_PM_BASE + BCM2711_PM_WDOG, PM_PASSWORD | current_value);
    sleep(timeout / 2);  // Kick before timeout expires
}