// © 2020 - 2021 Raptor Engineering, LLC
//
// Released under the terms of the GPL v3
// See the LICENSE file for full details

#define WITH_SPI 1

#include "fsi.h"
#include "utility.h"

#include <console.h>
#include <crc.h>
#include <endian.h>
#include <generated/csr.h>
#include <generated/mem.h>
#include <irq.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <uart.h>
#if (WITH_SPI)
#include "micron_n25q_flash.h"
#include "tercel_spi.h"
#endif

#include "aquila.h"
#include "ipmi_bt.h"
#include "opencores_i2c.h"

// Performance controls
#define ENABLE_LPC_FW_CYCLE_IRQ_HANDLER 1 // Set to 1 to enable LPC master transfer interrupts to the BMC soft core
#define ENABLE_LPC_FW_CYCLE_DMA         1 // Set to 1 to allow the LPC master to DMA data to/from the Wishbone bus
#define ALLOW_SPI_QUAD_MODE             1 // Set to 1 to allow quad-mode SPI transfers if the hardware supports them

// Debug knobs
#define DEBUG_HOST_SPI_FLASH_READ 0 // Set to 1 to enable verbose logging of SPI flash read process
#define SPI_FLASH_TRIPLE_READ     0 // Set to 1 to enable triple-read data checks (slow)

// General RCS platform registers
#define HOST_PLATFORM_FPGA_I2C_REG_STATUS  0x7
#define HOST_PLATFORM_FPGA_I2C_REG_MFR_OVR 0x33

// Host platform configuration
#define HOST_PLATFORM_FPGA_I2C_ADDRESS 0x31

extern uint32_t irq_unhandled_vector;
extern uint32_t irq_unhandled_source;
extern uint8_t irq_unhandled_vector_valid;
extern uint8_t irq_unhandled_source_valid;

#define VUART_INTERRUPT_TRANSIENT_BUFFER_SIZE 32

// Interrupt transient VUART1 buffer
static uint8_t vuart1_incoming_interrupt_transient_buffer[VUART_INTERRUPT_TRANSIENT_BUFFER_SIZE];
static int vuart1_incoming_interrupt_transient_buffer_pos = 0;
static uint8_t vuart1_incoming_interrupt_transient_buffer_overflow = 0;

// BMC to host VUART1 buffer
static uint8_t vuart1_outgoing_buffer[512];
static int vuart1_outgoing_buffer_read_pos = 0;
static int vuart1_outgoing_buffer_write_pos = 0;

// Host to BMC VUART1 buffer
static uint8_t vuart1_incoming_buffer[512];
static int vuart1_incoming_buffer_read_pos = 0;
static int vuart1_incoming_buffer_write_pos = 0;

// Interrupt transient VUART2 buffer
static uint8_t vuart2_incoming_interrupt_transient_buffer[VUART_INTERRUPT_TRANSIENT_BUFFER_SIZE];
static int vuart2_incoming_interrupt_transient_buffer_pos = 0;
static uint8_t vuart2_incoming_interrupt_transient_buffer_overflow = 0;

// // BMC to host VUART2 buffer
// static uint8_t vuart2_outgoing_buffer[512];
// static int vuart2_outgoing_buffer_read_pos = 0;
// static int vuart2_outgoing_buffer_write_pos = 0;

// Host to BMC VUART2 buffer
static uint8_t vuart2_incoming_buffer[512];
// static int vuart2_incoming_buffer_read_pos = 0;
static int vuart2_incoming_buffer_write_pos = 0;

// IPMI BT buffer
static ipmi_request_message_t ipmi_bt_interrupt_transient_request;
static uint8_t ipmi_bt_interrupt_transient_request_valid = 0;
static ipmi_request_message_t ipmi_bt_current_request;

// HIOMAP
static uint8_t *host_flash_buffer;
static hiomap_configuration_data_t hiomap_config;

// Background service tasks
static uint8_t host_background_service_task_active = 0;
static uint8_t host_console_service_task_active = 0;
static int configured_cpu_count = 1;

// POST codes
uint8_t post_code_high = 0;
uint8_t post_code_low = 0;

// Global configuration
static uint8_t allow_flash_write = 0;
static uint8_t enable_post_code_console_output = 0;

typedef struct
{
    int8_t index;
    uint8_t *i2c_master;
    uint32_t i2c_frequency;
    uint8_t vdd_regulator_addr;
    uint8_t vdd_regulator_page;
    uint8_t vcs_regulator_addr;
    uint8_t vcs_regulator_page;
    uint8_t vdn_regulator_addr;
    uint8_t vdn_regulator_page;
    uint8_t vdd_smbus_addr;
    uint8_t vdn_smbus_addr;
} cpu_info_t;
static const cpu_info_t g_cpu_info[] = {
    {
        .index = 0,
        .i2c_master = (uint8_t *)I2CMASTER1_BASE,
        .i2c_frequency = 100000,
        .vdd_regulator_addr = 0x70,
        .vdd_regulator_page = 0x00,
        .vcs_regulator_addr = 0x70,
        .vcs_regulator_page = 0x01,
        .vdn_regulator_addr = 0x73,
        .vdn_regulator_page = 0x00,
        .vdd_smbus_addr = 0x28,
        .vdn_smbus_addr = 0x2b,

    },
#ifdef I2CMASTER2_BASE
    {
        .index = 1,
        .i2c_master = (uint8_t *)I2CMASTER2_BASE,
        .i2c_frequency = 100000,
        .vdd_regulator_addr = 0x70,
        .vdd_regulator_page = 0x00,
        .vcs_regulator_addr = 0x70,
        .vcs_regulator_page = 0x01,
        .vdn_regulator_addr = 0x73,
        .vdn_regulator_page = 0x00,
        .vdd_smbus_addr = 0x28,
        .vdn_smbus_addr = 0x2b,
    },
#endif
};
#define MAX_CPUS_SUPPORTED (sizeof(g_cpu_info) / sizeof(g_cpu_info[0]))

static const struct power_limit_data_desc board_power_limits[] = {
    [PowerLimitDataGeneric] =
    {
        .packet =
        {
            .fail_response = POWERLIMIT_EXECTPION_ACT_HARD_SHUTDOWN,
            .max_watts = 0,
        },
        .completion_code = DCMI_CC_NO_POWER_LIMIT,
    },
};

void primary_service_event_loop(void);

static char *readstr(void)
{
    char c[2];
    static char s[64];
    static int ptr = 0;

    if (readchar_nonblock())
    {
        c[0] = readchar();
        c[1] = 0;
        switch (c[0])
        {
            case 0x7f:
            case 0x08:
                if (ptr > 0)
                {
                    ptr--;
                    putsnonl("\x08 \x08");
                }
                break;
            case 0x07:
                break;
            case '\r':
            case '\n':
                s[ptr] = 0x00;
                putsnonl("\n");
                ptr = 0;
                return s;
            default:
                if (ptr >= (sizeof(s) - 1))
                {
                    break;
                }
                putsnonl(c);
                s[ptr] = c[0];
                ptr++;
                break;
        }
    }

    primary_service_event_loop();

    return NULL;
}

static char *get_token(char **str)
{
    char *c, *d;

    c = (char *)strchr(*str, ' ');
    if (c == NULL)
    {
        d = *str;
        *str = *str + strlen(*str);
        return d;
    }
    *c = 0;
    d = *str;
    *str = c + 1;
    return d;
}

static void prompt(void)
{
    printf("FSP0>");
}

static void help(void)
{
    puts("Available commands:");
    puts("help                            - this command");
    puts("reboot                          - reboot BMC CPU");
    puts("poweron                         - Turn chassis power on, start IPL, "
         "and attach to host console");
    puts("console                         - Attach to host console");
    puts("status                          - Print system status");
    puts("ipl                             - Start IPL sequence");
    puts("chassison                       - Turn chassis power on and prepare "
         "for IPL");
    puts("chassisoff                      - Turn chassis power off");
    puts("sbe_status                      - Get SBE status register");
    puts("post_codes                      - Enable or disable output of POST "
         "codes on console");
    puts("mr <address> <length>           - Read data from BMC internal address "
         "in 32-bit words");
    puts("mw <address> <length> <data>    - Write data from BMC internal address "
         "in 32-bit words");
}

static void reboot(void)
{
    ctrl_reset_write(1);
}

static void display_character(char character, int dp)
{
    uint16_t value;

    // FIXME Only supports numbers for now
    switch (character)
    {
        case '0':
            value = 0x003f;
            break;
        case '1':
            value = 0x0006;
            break;
        case '2':
            value = 0x221b;
            break;
        case '3':
            value = 0x220f;
            break;
        case '4':
            value = 0x2226;
            break;
        case '5':
            value = 0x222d;
            break;
        case '6':
            value = 0x223d;
            break;
        case '7':
            value = 0x0007;
            break;
        case '8':
            value = 0x223f;
            break;
        case '9':
            value = 0x222f;
            break;
        default:
            value = 0x0000;
            break; // OFF
    }

    gpio3_out_write(~(value | ((dp == 0) ? 0x0000 : 0x4000)));
}

static void set_led_bank_display(uint8_t bitfield)
{
    gpio1_out_write(~bitfield);
}

static void gpio_init(void)
{
    // Set up discrete LED bank
    set_led_bank_display(0x00);
    gpio1_oe_write(0xff);

    // Set up alphanumeric display
    gpio3_out_write(0xefff);
    gpio3_oe_write(0xefff);
}

static void set_lpc_slave_irq_enable(uint8_t enabled)
{
    if (!enabled)
    {
        hostlpcslave_ev_enable_write(0);
        irq_setmask(irq_getmask() & ~(1 << HOSTLPCSLAVE_INTERRUPT));
    }

    // Clear pending interrupts
    hostlpcslave_ev_pending_write(hostlpcslave_ev_pending_read());

    if (enabled)
    {
        hostlpcslave_ev_enable_write(AQUILA_EV_MASTER_IRQ);
        irq_setmask(irq_getmask() | (1 << HOSTLPCSLAVE_INTERRUPT));
    }
}

void lpc_slave_isr(void)
{
#if (ENABLE_LPC_FW_CYCLE_IRQ_HANDLER)
    int byte;
    int word;
#endif
    uint32_t dword;
    uint32_t ev_status;
    uint32_t address;
    uint32_t physical_flash_address;
    uint8_t write_not_read;
    uint32_t status1_reg;
    uint32_t status2_reg;
    uint32_t status4_reg;
    uint32_t vuart_status;
    volatile ipmi_request_message_t *ipmi_bt_request_ptr;

    ev_status = hostlpcslave_ev_pending_read();
    if (ev_status & AQUILA_EV_MASTER_IRQ)
    {
        // Master IRQ asserted
        // Determine source within the LPC slave core
        status4_reg = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_STATUS4);
#if (ENABLE_LPC_FW_CYCLE_IRQ_HANDLER)
        if (status4_reg & AQUILA_LPC_FW_CYCLE_IRQ_ASSERTED)
        {
            // Firmware cycle request has caused IRQ assert
            // This should remain at the beginning of the ISR for maximum transfer
            // performance
            status1_reg = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_STATUS1);
            status2_reg = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_STATUS2);
            address = (status2_reg >> AQUILA_LPC_STATUS_ACT_ADDR_SHIFT) & AQUILA_LPC_STATUS_ACT_ADDR_MASK;
            write_not_read = (status1_reg >> AQUILA_LPC_STATUS_CYC_WNR_SHIFT) & AQUILA_LPC_STATUS_CYC_WNR_MASK;

            if (((status1_reg >> AQUILA_LPC_STATUS_CYCLE_TYPE_SHIFT) & AQUILA_LPC_STATUS_CYCLE_TYPE_MASK) == AQUILA_LPC_STATUS_CYCLE_TYPE_FW)
            {
                uint8_t fw_cycle_idsel = (status1_reg >> AQUILA_LPC_STATUS_FW_CYCLE_IDSEL_SHIFT) & AQUILA_LPC_STATUS_FW_CYCLE_IDSEL_MASK;
                uint8_t fw_cycle_msize = (status1_reg >> AQUILA_LPC_STATUS_FW_CYCLE_MSIZE_SHIFT) & AQUILA_LPC_STATUS_FW_CYCLE_MSIZE_MASK;

                if (fw_cycle_idsel == 0)
                {
                    // Limit firmware address to 64MB (wrap around)
                    address &= 0x3ffffff;

                    physical_flash_address = address;
                    if ((address >= hiomap_config.window_start_address) && ((address - hiomap_config.window_start_address) < hiomap_config.window_length_bytes))
                    {
                        if (!write_not_read &&
                            ((hiomap_config.window_type == HIOMAP_WINDOW_TYPE_READ) || (hiomap_config.window_type == HIOMAP_WINDOW_TYPE_WRITE)))
                        {
                            if (lpc_fw_msize_to_bytes(fw_cycle_msize) >= 4)
                            {
                                for (word = 0; word < (lpc_fw_msize_to_bytes(fw_cycle_msize) / 4); word++)
                                {
                                    *((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + (word * 4))) =
                                        *((uint32_t *)(host_flash_buffer + physical_flash_address + (word * 4)));
                                }
                            }
                            else
                            {
                                for (byte = 0; byte < lpc_fw_msize_to_bytes(fw_cycle_msize); byte++)
                                {
                                    *((volatile uint8_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + byte)) =
                                        *((uint8_t *)(host_flash_buffer + physical_flash_address + byte));
                                }
                            }

                            // Transfer success -- do not send error
                            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                            dword &= ~((AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                        }
                        else if (write_not_read && (hiomap_config.window_type == HIOMAP_WINDOW_TYPE_WRITE))
                        {
                            if (lpc_fw_msize_to_bytes(fw_cycle_msize) >= 4)
                            {
                                for (word = 0; word < (lpc_fw_msize_to_bytes(fw_cycle_msize) / 4); word++)
                                {
                                    *((uint32_t *)(host_flash_buffer + physical_flash_address + (word * 4))) =
                                        *((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + (word * 4)));
                                }
                            }
                            else
                            {
                                for (byte = 0; byte < lpc_fw_msize_to_bytes(fw_cycle_msize); byte++)
                                {
                                    *((uint8_t *)(host_flash_buffer + physical_flash_address + byte)) =
                                        *((volatile uint8_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + byte));
                                }
                            }

                            // Transfer success -- do not send error
                            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                            dword &= ~((AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                        }
                        else
                        {
                            // Invalid access -- send error
                            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                            dword |= ((1 & AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                        }
                    }
                    else
                    {
                        // Invalid access -- send error
                        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                        dword |= ((1 & AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                    }
                }
                else
                {
                    // Received firmware cycle request for unknown IDSEL!  Dazed and
                    // confused, but trying to continue... Do not send error
                    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                    dword &= ~((AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                }

                // Acknowledge data transfer
                dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                dword |= ((1 & AQUILA_LPC_CTL_XFER_CONT_MASK) << AQUILA_LPC_CTL_XFER_CONT_SHIFT);
                write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
            }
        }
#endif
        if ((status4_reg & AQUILA_LPC_VUART1_IRQ_ASSERTED) || (status4_reg & AQUILA_LPC_VUART2_IRQ_ASSERTED))
        {
            // VUART1 or VUART2 has asserted its IRQ
            // Copy received characters to IRQ receive buffer
            do
            {
                vuart_status = *((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + 0x0));
                if (!(vuart_status & AQUILA_LPC_VUART1_FIFO_EMPTY))
                {
                    vuart1_incoming_interrupt_transient_buffer[vuart1_incoming_interrupt_transient_buffer_pos] =
                        (vuart_status >> AQUILA_LPC_VUART1_FIFO_READ_SHIFT) & AQUILA_LPC_VUART1_FIFO_READ_MASK;
                    vuart1_incoming_interrupt_transient_buffer_pos++;
                    if (vuart1_incoming_interrupt_transient_buffer_pos >= VUART_INTERRUPT_TRANSIENT_BUFFER_SIZE)
                    {
                        // Transient buffer is full
                        // Disable VUART1 interrupts, since we are no longer able to service
                        // them, then exit the copy routine
                        dword = (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_CONTROL_REG)));
                        dword &= ~((1 & AQUILA_LPC_VUART_IRQ_EN_MASK) << AQUILA_LPC_VUART_IRQ_EN_SHIFT);
                        (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_CONTROL_REG))) = dword;
                        vuart1_incoming_interrupt_transient_buffer_overflow = 1;
                    }
                }
                if (!(vuart_status & AQUILA_LPC_VUART2_FIFO_EMPTY))
                {
                    vuart2_incoming_interrupt_transient_buffer[vuart2_incoming_interrupt_transient_buffer_pos] =
                        (vuart_status >> AQUILA_LPC_VUART2_FIFO_READ_SHIFT) & AQUILA_LPC_VUART2_FIFO_READ_MASK;
                    vuart2_incoming_interrupt_transient_buffer_pos++;
                    if (vuart2_incoming_interrupt_transient_buffer_pos >= VUART_INTERRUPT_TRANSIENT_BUFFER_SIZE)
                    {
                        // Transient buffer is full
                        // Disable VUART2 interrupts, since we are no longer able to service
                        // them, then exit the copy routine
                        dword = (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART2_CONTROL_REG)));
                        dword &= ~((1 & AQUILA_LPC_VUART_IRQ_EN_MASK) << AQUILA_LPC_VUART_IRQ_EN_SHIFT);
                        (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART2_CONTROL_REG))) = dword;
                        vuart2_incoming_interrupt_transient_buffer_overflow = 1;
                    }
                }
            } while (((!(vuart_status & AQUILA_LPC_VUART1_FIFO_EMPTY)) && (!vuart1_incoming_interrupt_transient_buffer_overflow)) ||
                     ((!(vuart_status & AQUILA_LPC_VUART2_FIFO_EMPTY)) && (!vuart2_incoming_interrupt_transient_buffer_overflow)));
        }
        if (status4_reg & AQUILA_LPC_IPMI_BT_IRQ_ASSERTED)
        {
            // The IPMI BT module has asserted its IRQ
            // Copy IPMI BT request to IRQ receive buffer

            // Signal BMC read starting
            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS) & (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
            if (!(dword & (1 << IPMI_BT_CTL_B_BUSY_SHIFT)))
            {
                // Set B_BUSY
                dword |= (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
            }
            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS, dword);

            // Clear H2B_ATN
            dword = 0;
            dword |= (1 << IPMI_BT_CTL_H2B_ATN_SHIFT);
            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS, dword);

            ipmi_bt_request_ptr = (ipmi_request_message_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_IPMI_BT_DATA_BLOCK_OFFSET);
            ipmi_bt_interrupt_transient_request = *ipmi_bt_request_ptr;

            // Signal BMC read complete
            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS) & (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
            if (dword & (1 << IPMI_BT_CTL_B_BUSY_SHIFT))
            {
                // Clear B_BUSY
                dword |= (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
            }
            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS, dword);

            ipmi_bt_interrupt_transient_request_valid = 1;
        }
    }

    hostlpcslave_ev_pending_write(AQUILA_EV_MASTER_IRQ);
}

uint8_t uart_register_bank[8];

static uint8_t ipmi_bt_transaction_state;

static void configure_flash_write_enable(uint8_t enable_writes)
{
    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    if (enable_writes)
    {
        // Send write enable command
        *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x06;
    }
    else
    {
        // Send write disable command
        *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x04;
    }

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));
}

static uint8_t read_flash_flag_status_register(void)
{
    uint8_t flag_status = 0;

    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Send Read Flag Status Register command
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x70;

    // Read response
    flag_status = *((volatile uint8_t *)HOSTSPIFLASH_BASE);

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    return flag_status;
}

static void reset_flash_device(void)
{
    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Issue RESET ENABLE command
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x66;

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Issue RESET MEMORY command
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x99;

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));
}

static void configure_flash_device(void)
{
    uint8_t config_byte;

    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Enable 4 byte addressing mode
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0xb7;

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    configure_flash_write_enable(1);

    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Initialize volatile configuration register
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x81;

    config_byte = 0;
    config_byte |= (MICRON_N25Q_SPI_FAST_READ_DUMMY_CLOCK_CYCLES & 0xf) << 4;
    config_byte |= (1 & 0x1) << 3;
    config_byte |= (0 & 0x1) << 2;
    config_byte |= (3 & 0x3) << 0;

    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = config_byte;

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    configure_flash_write_enable(0);
}

static void erase_flash_subsector(uint32_t address)
{
    // Limit Flash address to active memory
    address = address & 0x0fffffff;

    while (!(read_flash_flag_status_register() & 0x80))
    {
        // Wait for pending operation to complete
    }

    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Send subsector erase command
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x21;

    // Send address
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = (address >> 24) & 0xff;
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = (address >> 16) & 0xff;
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = (address >> 8) & 0xff;
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = address & 0xff;

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    while (!(read_flash_flag_status_register() & 0x80))
    {
        // Wait for pending operation to complete
    }
}

static int write_data_to_flash(uint8_t *write_buffer, uint32_t bytes, uint32_t flash_offset, uint8_t erase_before_write)
{
    uint32_t flash_address;
    uint32_t bytes_remaining;

    // Limit Flash address to active memory
    flash_offset = flash_offset & 0x0fffffff;

    if (allow_flash_write)
    {
        // Flash erase if needed, then write data
        if (erase_before_write)
        {
            for (flash_address = flash_offset; (flash_address - flash_offset) < bytes; flash_address = flash_address + FLASH_ERASE_GRAN_BYTES)
            {
                configure_flash_write_enable(1);
                erase_flash_subsector(flash_address);
            }

            configure_flash_write_enable(0);
        }

        for (flash_address = flash_offset; (flash_address - flash_offset) < bytes; flash_address = flash_address + FLASH_PAGE_SIZE_BYTES)
        {
            bytes_remaining = bytes - (flash_address - flash_offset);
            configure_flash_write_enable(1);
            while (!(read_flash_flag_status_register() & 0x80))
            {
                // Wait for pending operation to complete
            }
            memcpy((uint8_t *)(HOSTSPIFLASH_BASE + flash_address), write_buffer + (flash_address - flash_offset),
                   (bytes_remaining > 256) ? 256 : bytes_remaining);
            while (!(read_flash_flag_status_register() & 0x80))
            {
                // Wait for pending operation to complete
            }
        }

        configure_flash_write_enable(0);

        return -1;
    }
    else
    {
        return 0;
    }
}

// NOTE
// The POWER9 host uses true multitasking (kernel preemptive), so it is entirely
// possible to receive various LPC commands during processing of others.  As a
// result, we need at least a primitive multitasking system for the BMC. For
// now, use cooperative multitasking in this basic bare metal firmware... All
// functions called from the main TX/RX loop should return within some
// timeframe, e.g. 10ms
static void process_host_to_bmc_ipmi_bt_transactions(void)
{
    uint32_t dword;

    static uint8_t unhandled_ipmi_command;
    volatile ipmi_response_message_t *response_ptr;
    static ipmi_response_message_t response;
    static uint8_t request_netfn;
    static uint8_t request_lun;

    uint32_t offset_bytes = 0;
    uint32_t length_bytes = 0;
    uint8_t flags = 0;

    int i;

    switch (ipmi_bt_transaction_state)
    {
        case 0:
            // Idle
            break;
        case 1:
            // Extract NETFN/LUN from request
            request_netfn = ipmi_bt_current_request.netfn_lun >> 2;
            request_lun = ipmi_bt_current_request.netfn_lun & 0x3;

            // Set up basic response parameters
            response.netfn_lun = (((request_netfn + 1) & 0x3f) << 2) | (request_lun & 0x3);
            response.sequence = ipmi_bt_current_request.sequence;
            response.command = ipmi_bt_current_request.command;
            response.length = BASE_IPMI_RESPONSE_LENGTH;
            response.completion_code = IPMI_CC_INVALID_COMMAND;
            memset(response.data, 0, sizeof(response.data));

            unhandled_ipmi_command = 0;
            switch (request_netfn)
            {
                case IPMI_NETFN_SENS_ET_REQ:
                    unhandled_ipmi_command = 1;
                    break;
                case IPMI_NETFN_APP_REQUEST:
                    switch (ipmi_bt_current_request.command)
                    {
                        case IPMI_CMD_GET_DEVICE_ID:
                            response.data[0] = 0x00;
                            response.data[1] = 0x00;
                            response.data[2] = 0x00;
                            response.data[3] = 0x00;
                            response.data[4] = 0x02;
                            response.data[5] = 0x00;
                            response.data[6] = 0x05;
                            response.data[7] = 0xcb;
                            response.data[8] = 0x00;
                            response.data[9] = 0x01;
                            response.data[10] = 0x00;
                            response.data[11] = 0x00;
                            response.data[12] = 0x00;
                            response.data[13] = 0x00;
                            response.data[14] = 0x00;
                            response.length = BASE_IPMI_RESPONSE_LENGTH + 15;
                            response.completion_code = IPMI_CC_NO_ERROR;
                            break;
                        case IPMI_CMD_GET_BT_INT_CAP:
                            response.data[0] = 0x01;
                            response.data[1] = 0x3f;
                            response.data[2] = 0x3f;
                            response.data[3] = 0x01;
                            response.data[4] = 0x01;
                            response.length = BASE_IPMI_RESPONSE_LENGTH + 5;
                            response.completion_code = IPMI_CC_NO_ERROR;
                            break;
                        default:
                            unhandled_ipmi_command = 1;
                            break;
                    }
                    break;
                case IPMI_NETFN_STORAGE_REQ:
                    unhandled_ipmi_command = 1;
                    break;
                case IPMI_NETFN_DCMI_GP_REQ:
                    switch (ipmi_bt_current_request.command)
                    {
                        case DCMI_CMD_GET_POWER_CAP:
                        {
                            /* Only a generic P9 profile with no power
                             * limits is included at the moment.*/
                            uint32_t limit_index = PowerLimitDataGeneric;
                            memcpy(response.data, &board_power_limits[limit_index].packet, sizeof(board_power_limits[0].packet));

                            response.completion_code = board_power_limits[limit_index].completion_code;
                            response.length = BASE_DCMI_RESPONSE_LENGTH + sizeof(board_power_limits[0].packet);
                        }
                        break;
                        default:
                            unhandled_ipmi_command = 1;
                            break;
                    }
                    break;
                case IPMI_NETFN_OEM_IBM_REQ:
                    switch (ipmi_bt_current_request.command)
                    {
                        case IPMI_CMD_IBM_HIOMAP_REQ:
                            switch (ipmi_bt_current_request.data[0])
                            {
                                case HIOMAP_CMD_GET_INFO:
                                    if (ipmi_bt_current_request.data[2] > 3)
                                    {
                                        // We only support up to the HIOMAP v3 protocol
                                        hiomap_config.protocol_version = 3;
                                    }
                                    else
                                    {
                                        hiomap_config.protocol_version = ipmi_bt_current_request.data[2];
                                    }
                                    switch (hiomap_config.protocol_version)
                                    {
                                        case 1:
                                            response.data[2] = hiomap_config.protocol_version;
                                            response.data[3] = FLASH_SIZE_BLOCKS & 0xff;
                                            response.data[4] = (FLASH_SIZE_BLOCKS >> 8) & 0xff;
                                            response.data[5] = FLASH_SIZE_BLOCKS & 0xff;
                                            response.data[6] = (FLASH_SIZE_BLOCKS >> 8) & 0xff;
                                            response.length = BASE_HIOMAP_RESPONSE_LENGTH + 5;
                                            break;
                                        case 2:
                                            response.data[2] = hiomap_config.protocol_version;
                                            response.data[3] = FLASH_BLOCK_SIZE_SHIFT;
                                            response.data[4] = HIOMAP_SUGGESTED_TIMEOUT_S & 0xff;
                                            response.data[5] = (HIOMAP_SUGGESTED_TIMEOUT_S >> 8) & 0xff;
                                            response.length = BASE_HIOMAP_RESPONSE_LENGTH + 4;
                                            break;
                                        case 3:
                                            response.data[2] = hiomap_config.protocol_version;
                                            response.data[3] = FLASH_BLOCK_SIZE_SHIFT;
                                            response.data[4] = HIOMAP_SUGGESTED_TIMEOUT_S & 0xff;
                                            response.data[5] = (HIOMAP_SUGGESTED_TIMEOUT_S >> 8) & 0xff;
                                            response.data[6] = HIOMAP_PNOR_DEVICE_COUNT;
                                            response.length = BASE_HIOMAP_RESPONSE_LENGTH + 5;
                                            break;
                                    }
                                    response.data[0] = ipmi_bt_current_request.data[0];
                                    response.data[1] = ipmi_bt_current_request.data[1];
                                    response.completion_code = IPMI_CC_NO_ERROR;
                                    break;
                                case HIOMAP_CMD_GET_FLASH_INFO:
                                    switch (hiomap_config.protocol_version)
                                    {
                                        case 1:
                                            response.data[2] = FLASH_SIZE_BYTES & 0xff;
                                            response.data[3] = (FLASH_SIZE_BYTES >> 8) & 0xff;
                                            response.data[4] = (FLASH_SIZE_BYTES >> 16) & 0xff;
                                            response.data[5] = (FLASH_SIZE_BYTES >> 24) & 0xff;
                                            response.data[6] = FLASH_ERASE_GRAN_BYTES & 0xff;
                                            response.data[7] = (FLASH_ERASE_GRAN_BYTES >> 8) & 0xff;
                                            response.data[8] = (FLASH_ERASE_GRAN_BYTES >> 16) & 0xff;
                                            response.data[9] = (FLASH_ERASE_GRAN_BYTES >> 24) & 0xff;
                                            response.length = BASE_HIOMAP_RESPONSE_LENGTH + 8;
                                            break;
                                        case 2:
                                            // Fall through, same format as protocol version 3
                                        case 3:
                                            response.data[2] = FLASH_SIZE_BLOCKS & 0xff;
                                            response.data[3] = (FLASH_SIZE_BLOCKS >> 8) & 0xff;
                                            response.data[4] = FLASH_ERASE_GRAN_BLOCKS & 0xff;
                                            response.data[5] = (FLASH_ERASE_GRAN_BLOCKS >> 8) & 0xff;
                                            response.length = BASE_HIOMAP_RESPONSE_LENGTH + 4;
                                            break;
                                    }
                                    response.data[0] = ipmi_bt_current_request.data[0];
                                    response.data[1] = ipmi_bt_current_request.data[1];
                                    response.completion_code = IPMI_CC_NO_ERROR;
                                    break;
                                case HIOMAP_CMD_CREATE_RD_WIN:
                                case HIOMAP_CMD_CREATE_WR_WIN:
                                    // Parse request data
                                    hiomap_config.window_start_address =
                                        (((((uint32_t)ipmi_bt_current_request.data[3]) << 8) | ipmi_bt_current_request.data[2]) << FLASH_BLOCK_SIZE_SHIFT) &
                                        ((1 << LPC_ADDRESS_BITS) - 1);
                                    hiomap_config.window_length_bytes =
                                        (((((uint32_t)ipmi_bt_current_request.data[5]) << 8) | ipmi_bt_current_request.data[4]) << FLASH_BLOCK_SIZE_SHIFT) &
                                        ((1 << LPC_ADDRESS_BITS) - 1);
                                    hiomap_config.active_device_id = ipmi_bt_current_request.data[6];
                                    if (ipmi_bt_current_request.data[0] == HIOMAP_CMD_CREATE_RD_WIN)
                                    {
                                        hiomap_config.window_type = HIOMAP_WINDOW_TYPE_READ;
                                    }
                                    else if (ipmi_bt_current_request.data[0] == HIOMAP_CMD_CREATE_WR_WIN)
                                    {
                                        hiomap_config.window_type = HIOMAP_WINDOW_TYPE_WRITE;
                                    }
                                    else
                                    {
                                        hiomap_config.window_type = HIOMAP_WINDOW_INACTIVE;
                                    }

                                    // Sanitize input
                                    switch (hiomap_config.protocol_version)
                                    {
                                        case 1:
                                            if (ipmi_bt_current_request.data[0] == HIOMAP_CMD_CREATE_RD_WIN)
                                            {
                                                // Size unspecified, use one block as the size
                                                hiomap_config.window_length_bytes = 1 << FLASH_BLOCK_SIZE_SHIFT;
                                            }
                                            if (ipmi_bt_current_request.data[0] == HIOMAP_CMD_CREATE_WR_WIN)
                                            {
                                                // Size unspecified, use one block or the maximum write
                                                // cache size as the returned size, whichever is smaller...
                                                if (FLASH_MAX_WR_WINDOW_BYTES < (1 << FLASH_BLOCK_SIZE_SHIFT))
                                                {
                                                    hiomap_config.window_length_bytes = FLASH_MAX_WR_WINDOW_BYTES;
                                                }
                                                else
                                                {
                                                    hiomap_config.window_length_bytes = 1 << FLASH_BLOCK_SIZE_SHIFT;
                                                }
                                            }
                                            break;
                                        case 2:
                                        case 3:
                                            if (ipmi_bt_current_request.data[0] == HIOMAP_CMD_CREATE_RD_WIN)
                                            {
                                                // Zero sized window indicates undefined size, but must be at
                                                // least one block Just use one block as the size in this corner
                                                // case...
                                                if (hiomap_config.window_length_bytes == 0)
                                                {
                                                    hiomap_config.window_length_bytes = 1 << FLASH_BLOCK_SIZE_SHIFT;
                                                }
                                            }
                                            if (ipmi_bt_current_request.data[0] == HIOMAP_CMD_CREATE_WR_WIN)
                                            {
                                                // Zero sized window indicates undefined size, but must be at
                                                // least one block Just use one block as the size in this corner
                                                // case...
                                                if (hiomap_config.window_length_bytes == 0)
                                                {
                                                    hiomap_config.window_length_bytes = 1 << FLASH_BLOCK_SIZE_SHIFT;
                                                }
                                                else
                                                {
                                                    // The host can only request a window size, not demand one
                                                    // If the request is larger than our write cache size, limit
                                                    // the returned window to the write cache size...
                                                    if (hiomap_config.window_length_bytes > FLASH_MAX_WR_WINDOW_BYTES)
                                                    {
                                                        hiomap_config.window_length_bytes = FLASH_MAX_WR_WINDOW_BYTES;
                                                    }
                                                }
                                            }
                                            break;
                                    }

#if (ENABLE_LPC_FW_CYCLE_DMA)
                                    // Disable DMA engine
                                    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1);
                                    dword &= ~((1 & AQUILA_LPC_CTL_EN_FW_DMA_R_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_R_SHIFT);
                                    dword &= ~((1 & AQUILA_LPC_CTL_EN_FW_DMA_W_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_W_SHIFT);
                                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1, dword);

                                    // Reconfigure LPC firmware cycle DMA ranges
                                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG2, (uintptr_t)host_flash_buffer);
                                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG3, FLASH_SIZE_BYTES);
                                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG4, hiomap_config.window_start_address);
                                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG5,
                                                          hiomap_config.window_start_address + hiomap_config.window_length_bytes);
                                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG6, FLASH_SIZE_BYTES - 1);

                                    // Enable DMA engine
                                    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1);
                                    dword |= ((1 & AQUILA_LPC_CTL_EN_FW_DMA_R_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_R_SHIFT);
                                    if (hiomap_config.window_type == HIOMAP_WINDOW_TYPE_WRITE)
                                    {
                                        dword |= ((1 & AQUILA_LPC_CTL_EN_FW_DMA_W_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_W_SHIFT);
                                    }
                                    else
                                    {
                                        dword &= ~((1 & AQUILA_LPC_CTL_EN_FW_DMA_W_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_W_SHIFT);
                                    }
                                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1, dword);
#endif

                                    // Generate response
                                    switch (hiomap_config.protocol_version)
                                    {
                                        case 1:
                                            // Use 1:1 mapping between LPC firmware address and SPI Flash
                                            // address
                                            response.data[2] = (hiomap_config.window_start_address >> FLASH_BLOCK_SIZE_SHIFT) & 0xff;
                                            response.data[3] = ((hiomap_config.window_start_address >> FLASH_BLOCK_SIZE_SHIFT) >> 8) & 0xff;
                                            response.length = BASE_HIOMAP_RESPONSE_LENGTH + 2;
                                            break;
                                        case 2:
                                        case 3:
                                            // Use 1:1 mapping between LPC firmware address and SPI Flash
                                            // address
                                            response.data[2] = (hiomap_config.window_start_address >> FLASH_BLOCK_SIZE_SHIFT) & 0xff;
                                            response.data[3] = ((hiomap_config.window_start_address >> FLASH_BLOCK_SIZE_SHIFT) >> 8) & 0xff;
                                            // Echo configured Flash window start / length
                                            response.data[4] = (hiomap_config.window_length_bytes >> FLASH_BLOCK_SIZE_SHIFT) & 0xff;
                                            response.data[5] = ((hiomap_config.window_length_bytes >> FLASH_BLOCK_SIZE_SHIFT) >> 8) & 0xff;
                                            response.data[6] = (hiomap_config.window_start_address >> FLASH_BLOCK_SIZE_SHIFT) & 0xff;
                                            response.data[7] = ((hiomap_config.window_start_address >> FLASH_BLOCK_SIZE_SHIFT) >> 8) & 0xff;
                                            response.length = BASE_HIOMAP_RESPONSE_LENGTH + 6;
                                            break;
                                    }

                                    response.data[0] = ipmi_bt_current_request.data[0];
                                    response.data[1] = ipmi_bt_current_request.data[1];
                                    response.completion_code = IPMI_CC_NO_ERROR;
                                    break;
                                case HIOMAP_CMD_MARK_DIRTY:
                                    flags = 0;
                                    switch (hiomap_config.protocol_version)
                                    {
                                        case 1:
                                            offset_bytes = (((((uint32_t)ipmi_bt_current_request.data[3]) << 8) | ipmi_bt_current_request.data[2])
                                                            << FLASH_BLOCK_SIZE_SHIFT) &
                                                           ((1 << LPC_ADDRESS_BITS) - 1);
                                            length_bytes =
                                                ((((uint32_t)ipmi_bt_current_request.data[7]) << 24) | (((uint32_t)ipmi_bt_current_request.data[6]) << 16) |
                                                 (((uint32_t)ipmi_bt_current_request.data[5]) << 8) | ipmi_bt_current_request.data[4]);
                                            break;
                                        case 2:
                                        case 3:
                                            offset_bytes = hiomap_config.window_start_address +
                                                           ((((((uint32_t)ipmi_bt_current_request.data[3]) << 8) | ipmi_bt_current_request.data[2])
                                                             << FLASH_BLOCK_SIZE_SHIFT) &
                                                            ((1 << LPC_ADDRESS_BITS) - 1));
                                            length_bytes = (((((uint32_t)ipmi_bt_current_request.data[5]) << 8) | ipmi_bt_current_request.data[4])
                                                            << FLASH_BLOCK_SIZE_SHIFT) &
                                                           ((1 << LPC_ADDRESS_BITS) - 1);
                                            if (hiomap_config.protocol_version == 3)
                                            {
                                                flags = ipmi_bt_current_request.data[6];
                                            }
                                            break;
                                    }

                                    // Record dirty page
                                    hiomap_config.dirty_ranges[hiomap_config.dirty_range_count].start_address = offset_bytes;
                                    hiomap_config.dirty_ranges[hiomap_config.dirty_range_count].bytes = length_bytes;
                                    hiomap_config.dirty_ranges[hiomap_config.dirty_range_count].erased = flags & 0x1;
                                    hiomap_config.dirty_range_count++;

                                    response.data[0] = ipmi_bt_current_request.data[0];
                                    response.data[1] = ipmi_bt_current_request.data[1];
                                    response.length = BASE_HIOMAP_RESPONSE_LENGTH;
                                    response.completion_code = IPMI_CC_NO_ERROR;
                                    break;
                                case HIOMAP_CMD_FLUSH:
                                    if (hiomap_config.protocol_version == 1)
                                    {
                                        // Only HIOMAP protocol v1 has the ability to mark a page dirty in
                                        // the FLUSH command
                                        offset_bytes =
                                            (((((uint32_t)ipmi_bt_current_request.data[3]) << 8) | ipmi_bt_current_request.data[2]) << FLASH_BLOCK_SIZE_SHIFT) &
                                            ((1 << LPC_ADDRESS_BITS) - 1);
                                        length_bytes =
                                            ((((uint32_t)ipmi_bt_current_request.data[7]) << 24) | (((uint32_t)ipmi_bt_current_request.data[6]) << 16) |
                                             (((uint32_t)ipmi_bt_current_request.data[5]) << 8) | ipmi_bt_current_request.data[4]);

                                        // Record dirty page
                                        hiomap_config.dirty_ranges[hiomap_config.dirty_range_count].start_address = offset_bytes;
                                        hiomap_config.dirty_ranges[hiomap_config.dirty_range_count].bytes = length_bytes;
                                        hiomap_config.dirty_ranges[hiomap_config.dirty_range_count].erased = 0;
                                        hiomap_config.dirty_range_count++;
                                    }

                                    for (i = 0; i < hiomap_config.dirty_range_count; i++)
                                    {
                                        write_data_to_flash(((uint8_t *)(host_flash_buffer + hiomap_config.dirty_ranges[i].start_address)),
                                                            hiomap_config.dirty_ranges[i].bytes, hiomap_config.dirty_ranges[i].start_address,
                                                            !hiomap_config.dirty_ranges[i].erased);
                                    }
                                    hiomap_config.dirty_range_count = 0;

                                    response.data[0] = ipmi_bt_current_request.data[0];
                                    response.data[1] = ipmi_bt_current_request.data[1];
                                    response.length = BASE_HIOMAP_RESPONSE_LENGTH;
                                    response.completion_code = IPMI_CC_NO_ERROR;
                                case HIOMAP_CMD_ACK:
                                    // Mask is in ipmi_bt_current_request.data[2]
                                    // For now just ignore and claim sucess
                                    response.data[0] = ipmi_bt_current_request.data[0];
                                    response.data[1] = ipmi_bt_current_request.data[1];
                                    response.length = BASE_HIOMAP_RESPONSE_LENGTH;
                                    response.completion_code = IPMI_CC_NO_ERROR;
                                    break;
                                default:
                                    unhandled_ipmi_command = 1;
                                    break;
                            }
                            break;
                        default:
                            unhandled_ipmi_command = 1;
                            break;
                    }
                    break;
                default:
                    unhandled_ipmi_command = 1;
                    break;
            }

            if (unhandled_ipmi_command)
            {
                response.length = BASE_IPMI_RESPONSE_LENGTH;
                response.completion_code = IPMI_CC_INVALID_COMMAND;
            }

            ipmi_bt_transaction_state = 2;
            break;
        case 2:
            // Wait for H_BUSY clear
            if (!(read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS) & (1 << IPMI_BT_CTL_H_BUSY_SHIFT)))
            {
                ipmi_bt_transaction_state = 3;
            }
            break;
        case 3:
            // Initialize pointer
            response_ptr = (ipmi_response_message_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_IPMI_BT_DATA_BLOCK_OFFSET);

            // Send response
            // A full copy is done so as to ensure any potentially sensitive data stored
            // in the IPMI BT buffer from a previous request is overwritten
            *response_ptr = response;

            // Signal BMC data ready
            dword = 0;
            dword |= (1 << IPMI_BT_CTL_B2H_ATN_SHIFT);
            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS, dword);

            ipmi_bt_transaction_state = 4;
            break;
        case 4:
            // Wait for processing to complete
            // If B2H_ATN, and H_BUSY are both clear, processing has been completed
            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS);
            if ((!(dword & (1 << IPMI_BT_CTL_B2H_ATN_SHIFT))) && (!(dword & (1 << IPMI_BT_CTL_H_BUSY_SHIFT))))
            {
                ipmi_bt_transaction_state = 0;
            }
            break;
        default:
            ipmi_bt_transaction_state = 0;
            break;
    }
}

#if !(ENABLE_LPC_FW_CYCLE_IRQ_HANDLER)
static uint32_t previous_fw_read_address;
#endif

static void process_interrupts_stage2(void)
{
    uint32_t dword;
    int read_position;

    // Deactivate interrupts on entering critical section
    irq_setie(0);

    // CRITICAL SECTION
    // No interrupts can fire here!
    // All code in this section must be able to run in bounded time -- do NOT wait
    // on external events etc. here, just move and enqueue data as needed for
    // further processing at a later time

    // Process incoming VUART data
    if (vuart1_incoming_interrupt_transient_buffer_pos > 0)
    {
        read_position = 0;
        while (read_position < vuart1_incoming_interrupt_transient_buffer_pos)
        {
            vuart1_incoming_buffer[vuart1_incoming_buffer_write_pos] = vuart1_incoming_interrupt_transient_buffer[read_position];
            vuart1_incoming_buffer_write_pos++;
            if (vuart1_incoming_buffer_write_pos >= 512)
            {
                vuart1_incoming_buffer_write_pos = 0;
            }
            read_position++;
            if (read_position >= 512)
            {
                break;
            }
        }
        vuart1_incoming_interrupt_transient_buffer_pos = 0;
        if (vuart1_incoming_interrupt_transient_buffer_overflow)
        {
            // Reenable VUART1 interrupts
            dword = (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_CONTROL_REG)));
            dword |= (1 & AQUILA_LPC_VUART_IRQ_EN_MASK) << AQUILA_LPC_VUART_IRQ_EN_SHIFT;
            (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_CONTROL_REG))) = dword;
            vuart1_incoming_interrupt_transient_buffer_overflow = 0;
        }
    }
    if (vuart2_incoming_interrupt_transient_buffer_pos > 0)
    {
        read_position = 0;
        while (read_position < vuart2_incoming_interrupt_transient_buffer_pos)
        {
            vuart2_incoming_buffer[vuart2_incoming_buffer_write_pos] = vuart2_incoming_interrupt_transient_buffer[read_position];
            vuart2_incoming_buffer_write_pos++;
            if (vuart2_incoming_buffer_write_pos >= 512)
            {
                vuart2_incoming_buffer_write_pos = 0;
            }
            read_position++;
            if (read_position >= 512)
            {
                break;
            }
        }
        vuart2_incoming_interrupt_transient_buffer_pos = 0;
        if (vuart2_incoming_interrupt_transient_buffer_overflow)
        {
            // Reenable VUART1 interrupts
            dword = (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_CONTROL_REG)));
            dword |= (1 & AQUILA_LPC_VUART_IRQ_EN_MASK) << AQUILA_LPC_VUART_IRQ_EN_SHIFT;
            (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_CONTROL_REG))) = dword;
            vuart2_incoming_interrupt_transient_buffer_overflow = 0;
        }
    }

    // Process incoming IPMI BT request data
    if (ipmi_bt_interrupt_transient_request_valid)
    {
        if (ipmi_bt_transaction_state == 0)
        {
            ipmi_bt_current_request = ipmi_bt_interrupt_transient_request;
            ipmi_bt_interrupt_transient_request_valid = 0;
            ipmi_bt_transaction_state = 1;
        }
    }

    // Re-activate interupts on exiting critical section
    irq_setie(1);
}

static void run_pre_ipl_bmc_peripheral_setup(void)
{
    uint32_t dword;

    // Reset POST codes and display
    post_code_high = 0;
    post_code_low = 0;
    set_led_bank_display(0x00);

    // Deactivate interrupts on entering critical section
    irq_setie(0);

    // Reset VUART1 FIFO pointers
    vuart1_incoming_interrupt_transient_buffer_pos = 0;
    vuart1_incoming_interrupt_transient_buffer_overflow = 0;
    vuart1_outgoing_buffer_read_pos = 0;
    vuart1_outgoing_buffer_write_pos = 0;
    vuart1_incoming_buffer_read_pos = 0;
    vuart1_incoming_buffer_write_pos = 0;

    // Re-activate interupts on exiting critical section
    irq_setie(1);

    // Configure VUART1
    dword = 0;
    dword |= (1 & AQUILA_LPC_VUART_FIFO_TRIG_LVL_MASK) << AQUILA_LPC_VUART_FIFO_TRIG_LVL_SHIFT;
    dword |= (1 & AQUILA_LPC_VUART_IRQ_EN_MASK) << AQUILA_LPC_VUART_IRQ_EN_SHIFT;
    dword |= (1 & AQUILA_LPC_VUART_FIFO_IRQ_EN_MASK) << AQUILA_LPC_VUART_FIFO_IRQ_EN_SHIFT;
    (*((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_CONTROL_REG))) = dword;

    // Enable LPC slave IRQs
    set_lpc_slave_irq_enable(1);

    // Clear IPMI BT B_BUSY flag
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS) & (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
    if (dword & (1 << IPMI_BT_CTL_B_BUSY_SHIFT))
    {
        dword |= (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
    }
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS, dword);

    // Enable IPMI BT IRQ
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
    dword |= ((1 & AQUILA_LPC_CTL_EN_IPMI_BT_IRQ_MASK) << AQUILA_LPC_CTL_EN_IPMI_BT_IRQ_SHIFT);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);

#if (ENABLE_LPC_FW_CYCLE_IRQ_HANDLER)
    // Enable LPC firmware cycle IRQ
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
    dword |= ((1 & AQUILA_LPC_CTL_EN_FW_CYCLE_IRQ_MASK) << AQUILA_LPC_CTL_EN_FW_CYCLE_IRQ_SHIFT);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);
#endif

    // Reset HIOMAP windows
    hiomap_config.protocol_version = 0;
    hiomap_config.window_start_address = 0;
    hiomap_config.window_length_bytes = FLASH_SIZE_BYTES;
    hiomap_config.active_device_id = 0;
    hiomap_config.window_type = HIOMAP_WINDOW_TYPE_READ;
    hiomap_config.dirty_range_count = 0;

#if (ENABLE_LPC_FW_CYCLE_DMA)
    // Configure and enable LPC firmware cycle DMA
    // Set up default window with address masking based on physical ROM size
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG2, (uintptr_t)host_flash_buffer);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG3, FLASH_SIZE_BYTES);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG4, 0x0);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG5, FLASH_SIZE_BYTES);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG6, FLASH_SIZE_BYTES - 1);

    // Enable DMA engine
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1);
    dword |= ((1 & AQUILA_LPC_CTL_EN_FW_DMA_R_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_R_SHIFT);
    dword &= ~((1 & AQUILA_LPC_CTL_EN_FW_DMA_W_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_W_SHIFT);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1, dword);
#endif

    // Enable host background service task
    host_background_service_task_active = 1;

    // Assume console service task is inactive at startup
    host_console_service_task_active = 0;
}

static void run_post_shutdown_bmc_peripheral_teardown(void)
{
    uint32_t dword;

    // Disable host and console background service tasks
    host_background_service_task_active = 0;
    host_console_service_task_active = 0;

    // Reset internal state variables
    ipmi_bt_transaction_state = 0;

    // Set IPMI BT B_BUSY flag
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS) & (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
    if (!(dword & (1 << IPMI_BT_CTL_B_BUSY_SHIFT)))
    {
        // Set B_BUSY
        dword |= (1 << IPMI_BT_CTL_B_BUSY_SHIFT);
    }
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_IPMI_BT_STATUS, dword);

#if (ENABLE_LPC_FW_CYCLE_DMA)
    // Disable DMA engine
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1);
    dword &= ~((1 & AQUILA_LPC_CTL_EN_FW_DMA_R_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_R_SHIFT);
    dword &= ~((1 & AQUILA_LPC_CTL_EN_FW_DMA_W_MASK) << AQUILA_LPC_CTL_EN_FW_DMA_W_SHIFT);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DMA_CONFIG1, dword);
#endif

#if (ENABLE_LPC_FW_CYCLE_IRQ_HANDLER)
    // Disable LPC firmware cycle IRQ
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
    dword &= ~((1 & AQUILA_LPC_CTL_EN_FW_CYCLE_IRQ_MASK) << AQUILA_LPC_CTL_EN_FW_CYCLE_IRQ_SHIFT);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);
#endif

    // Disable IPMI BT IRQ
    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
    dword &= ~((1 & AQUILA_LPC_CTL_EN_IPMI_BT_IRQ_MASK) << AQUILA_LPC_CTL_EN_IPMI_BT_IRQ_SHIFT);
    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);

    // Disable LPC slave IRQs
    set_lpc_slave_irq_enable(0);

    // Reset HIOMAP windows
    hiomap_config.protocol_version = 0;
    hiomap_config.window_start_address = 0;
    hiomap_config.window_length_bytes = FLASH_SIZE_BYTES;
    hiomap_config.active_device_id = 0;
    hiomap_config.window_type = HIOMAP_WINDOW_TYPE_READ;
    hiomap_config.dirty_range_count = 0;

    // Reset POST codes and display
    post_code_high = 0;
    post_code_low = 0;
    set_led_bank_display(0x00);
}

static int apply_avsbus_workarounds_cpu(const cpu_info_t *cpu)
{
    printf("\tVDD/VCS %d: Enabling AVSBus CLK/MDAT pullups and selecting "
           "VIH/VIL 0x2 (0.65V/0.55V)\n",
           cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdd_smbus_addr, 0x2e, 0x23))
    {
        return -1;
    }

    printf("\tVDN %d: Enabling AVSBus CLK/MDAT pullups and selecting VIH/VIL "
           "0x2 (0.65V/0.55V)\n",
           cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdn_smbus_addr, 0x2e, 0x23))
    {
        return -1;
    }

    return 0;
}

static int apply_avsbus_workarounds(const cpu_info_t *cpu_info, int cpu_count)
{
    printf("Applying AVSBus workarounds...\n");

    for (int i = 0; i < cpu_count; i++)
    {
        if (apply_avsbus_workarounds_cpu(&cpu_info[i]))
        {
            return -1;
        }
    }

    printf("\tAVSBus workaround application complete!\n");
    return 0;
}

static int enable_avsbus_pmbus_cpu(const cpu_info_t *cpu)
{
    printf("\tVDD %d: Placing device in AVSBus voltage command mode\n", cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdd_regulator_addr, 0x00, cpu->vdd_regulator_page))
    {
        return -1;
    }
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdd_regulator_addr, 0x01, 0xb0))
    {
        return -1;
    }

    printf("\tVCS %d: Placing device in AVSBus voltage command mode\n", cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vcs_regulator_addr, 0x00, cpu->vcs_regulator_page))
    {
        return -1;
    }
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vcs_regulator_addr, 0x01, 0xb0))
    {
        return -1;
    }

    printf("\tVDN %d: Placing device in AVSBus voltage command mode\n", cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdn_regulator_addr, 0x00, cpu->vdn_regulator_page))
    {
        return -1;
    }
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdn_regulator_addr, 0x01, 0xb0))
    {
        return -1;
    }

    return 0;
}

static int enable_avsbus_pmbus(const cpu_info_t *cpu_info, int cpu_count)
{
    printf("Enabling AVSbus PMBUS functionality...\n");

    for (int i = 0; i < cpu_count; i++)
    {
        if (enable_avsbus_pmbus_cpu(&cpu_info[i]))
        {
            return -1;
        }
    }

    printf("\tAVSBus PMBUS functionality enabled!\n");
    return 0;
}

static int disable_avsbus_pmbus_cpu(const cpu_info_t *cpu)
{
    printf("\tVDD %d: Placing device in immediate off mode\n", cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdd_regulator_addr, 0x00, cpu->vdd_regulator_page))
    {
        return -1;
    }
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdd_regulator_addr, 0x01, 0x80))
    {
        return -1;
    }

    printf("\tVCS %d: Placing device in immediate off mode\n", cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vcs_regulator_addr, 0x00, cpu->vcs_regulator_page))
    {
        return -1;
    }
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vcs_regulator_addr, 0x01, 0x80))
    {
        return -1;
    }

    printf("\tVDN %d: Placing device in immediate off mode\n", cpu->index);
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdn_regulator_addr, 0x00, cpu->vdn_regulator_page))
    {
        return -1;
    }
    if (i2c_write_register_byte(cpu->i2c_master, cpu->vdn_regulator_addr, 0x01, 0x80))
    {
        return -1;
    }

    return 0;
}
static int disable_avsbus_pmbus(const cpu_info_t *cpu_info, int cpu_count)
{
    int status = 0;
    printf("Disabling AVSbus PMBUS functionality...\n");

    for (int i = 0; i < cpu_count; i++)
    {
        // Attempt to turn of power on all CPUs, even if one isn't responding.
        if (disable_avsbus_pmbus_cpu(&cpu_info[i]))
        {
            status = -1;
        }
    }

    printf("\tAVSBus PMBUS functionality disabled!\n");
    return status;
}

static void power_off_chassis(void)
{
    // Disable PMBUS
    if (disable_avsbus_pmbus(g_cpu_info, configured_cpu_count))
    {
        printf("PMBUS disable failed!\n");
    }

    // Power off host via platform FPGA commands
    i2c_write_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, HOST_PLATFORM_FPGA_I2C_REG_MFR_OVR, 0x00);

    run_post_shutdown_bmc_peripheral_teardown();
}

static int power_on_chassis(void)
{
    uint8_t platform_fpga_identifier[4];
    int platform_power_on_timeout_counter;
    int cpu_count = 1;
    int i2c_read_retcode;
    uint8_t byte;

    // Verify communication with platform control FPGA
    platform_fpga_identifier[0] = i2c_read_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, 0x0c, NULL);
    if (platform_fpga_identifier[0] == 0xff)
    {
        return -1;
    }
    platform_fpga_identifier[1] = i2c_read_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, 0x0d, NULL);
    platform_fpga_identifier[2] = i2c_read_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, 0x0e, NULL);
    platform_fpga_identifier[3] = i2c_read_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, 0x0f, NULL);
    if ((platform_fpga_identifier[0] != 0x52) || (platform_fpga_identifier[1] != 0x43) || (platform_fpga_identifier[2] != 0x53) ||
        (platform_fpga_identifier[3] != 0x20))
    {
        return -1;
    }
    printf("Platform FPGA communication verified\n");

    // Enable BMC runtime support tasks
    run_pre_ipl_bmc_peripheral_setup();

    // Power on host via platform FPGA commands
    printf("Commanding chassis power ON...\n");
    i2c_write_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, HOST_PLATFORM_FPGA_I2C_REG_MFR_OVR, 0x01);
    platform_power_on_timeout_counter = 0;
    byte = i2c_read_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, HOST_PLATFORM_FPGA_I2C_REG_STATUS, &i2c_read_retcode);
    while (i2c_read_retcode || (((byte)&0x03) != 0x03))
    {
        if (platform_power_on_timeout_counter > 20000)
        {
            printf("Chassis poweron timeout!\n");
            power_off_chassis();
            return -2;
        }
        usleep(100);
        platform_power_on_timeout_counter++;
        byte = i2c_read_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, HOST_PLATFORM_FPGA_I2C_REG_STATUS, &i2c_read_retcode);
    }
    if (i2c_read_retcode)
    {
        printf("FPGA communication failure during poweron!\n");
        return -3;
    }
    if (byte & 0x20)
    {
        cpu_count = 2;
    }
    printf("Chassis power verified active\n");
    configured_cpu_count = cpu_count;
    if (cpu_count > MAX_CPUS_SUPPORTED)
    {
        configured_cpu_count = cpu_count = MAX_CPUS_SUPPORTED;
        printf("Limiting number of CPUs to %d\n", MAX_CPUS_SUPPORTED);
    }
    printf("%d CPU(s) installed\n", cpu_count);

    // Apply AVSBus workarounds
    if (apply_avsbus_workarounds(g_cpu_info, cpu_count))
    {
        printf("AVSBus setup failed!\n");
        power_off_chassis();
        return -4;
    }

    // Enable PMBUS
    if (enable_avsbus_pmbus(g_cpu_info, cpu_count))
    {
        printf("PMBUS enable failed!\n");
        power_off_chassis();
        return -5;
    }

    return 0;
}

static int power_on_host(void)
{
    if (power_on_chassis())
    {
        return -1;
    }

    if (start_ipl(0))
    {
        return -1;
    }

    return 0;
}

static void print_chassis_status(void)
{
    int i2c_read_retcode;
    uint8_t byte;

    byte = i2c_read_register_byte((uint8_t *)I2CMASTER4_BASE, HOST_PLATFORM_FPGA_I2C_ADDRESS, HOST_PLATFORM_FPGA_I2C_REG_STATUS, &i2c_read_retcode);
    if (i2c_read_retcode)
    {
        printf("Unable to communicate with platform control FPGA!\n");
    }

    printf("Platform FPGA status:\n");
    if (byte & 0x1)
    {
        printf("\tPSU commanded ON\n");
    }
    else
    {
        printf("\tPSU commanded OFF\n");
    }
    if (byte & 0x2)
    {
        printf("\tPSU PGOOD asserted\n");
    }
    else
    {
        printf("\tPSU PGOOD deasserted\n");
    }
    printf("Platform overall status:\n");
    if (byte & 0x20)
    {
        printf("\t2 CPUs installed\n");
    }
    else
    {
        printf("\t1 CPU installed\n");
    }
    printf("BMC status:\n");
    if (host_background_service_task_active)
    {
        printf("\tBackground host services active\n");
    }
    else
    {
        printf("\tBackground host services inactive\n");
    }
}

static void host_background_service_task_event_loop(void)
{
    uint32_t address;
    uint8_t write_not_read;
    uint32_t dword;
#if !(ENABLE_LPC_FW_CYCLE_IRQ_HANDLER)
    int byte;
    int word;
    uint32_t physical_flash_address;
#endif

    // IRQ debugging routines
    if (irq_unhandled_source_valid)
    {
        printf("[WARNING] Interrupt triggered without external IRQ set!  source: %d\n", irq_unhandled_source);
        irq_unhandled_source_valid = 0;
    }
    if (irq_unhandled_vector_valid)
    {
        printf("[ERROR] Unhandled exception 0x%03d\n", irq_unhandled_vector);
        irq_unhandled_vector_valid = 0;
    }

    if (read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_STATUS1) & (AQUILA_LPC_STATUS_ATTN_REQ_MASK << AQUILA_LPC_STATUS_ATTN_REQ_SHIFT))
    {
        // Store / retrieve data
        uint32_t status1_reg = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_STATUS1);
        uint32_t status2_reg = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_STATUS2);
        address = (status2_reg >> AQUILA_LPC_STATUS_ACT_ADDR_SHIFT) & AQUILA_LPC_STATUS_ACT_ADDR_MASK;
        write_not_read = (status1_reg >> AQUILA_LPC_STATUS_CYC_WNR_SHIFT) & AQUILA_LPC_STATUS_CYC_WNR_MASK;
#if !(ENABLE_LPC_FW_CYCLE_IRQ_HANDLER)
        if (((status1_reg >> AQUILA_LPC_STATUS_CYCLE_TYPE_SHIFT) & AQUILA_LPC_STATUS_CYCLE_TYPE_MASK) == AQUILA_LPC_STATUS_CYCLE_TYPE_FW)
        {
            uint8_t fw_cycle_idsel = (status1_reg >> AQUILA_LPC_STATUS_FW_CYCLE_IDSEL_SHIFT) & AQUILA_LPC_STATUS_FW_CYCLE_IDSEL_MASK;
            uint8_t fw_cycle_msize = (status1_reg >> AQUILA_LPC_STATUS_FW_CYCLE_MSIZE_SHIFT) & AQUILA_LPC_STATUS_FW_CYCLE_MSIZE_MASK;

            if (fw_cycle_idsel == 0)
            {
                // Limit firmware address to 64MB (wrap around)
                address &= 0x3ffffff;

                previous_fw_read_address = address;
                physical_flash_address = address;
                if ((address >= hiomap_config.window_start_address) && ((address - hiomap_config.window_start_address) < hiomap_config.window_length_bytes))
                {
                    if (!write_not_read && ((hiomap_config.window_type == HIOMAP_WINDOW_TYPE_READ) || (hiomap_config.window_type == HIOMAP_WINDOW_TYPE_WRITE)))
                    {
                        if (lpc_fw_msize_to_bytes(fw_cycle_msize) >= 4)
                        {
                            for (word = 0; word < (lpc_fw_msize_to_bytes(fw_cycle_msize) / 4); word++)
                            {
                                *((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + (word * 4))) =
                                    *((uint32_t *)(host_flash_buffer + physical_flash_address + (word * 4)));
                            }
                        }
                        else
                        {
                            for (byte = 0; byte < lpc_fw_msize_to_bytes(fw_cycle_msize); byte++)
                            {
                                *((volatile uint8_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + byte)) =
                                    *((uint8_t *)(host_flash_buffer + physical_flash_address + byte));
                            }
                        }

                        // Transfer success -- do not send error
                        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                        dword &= ~((AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                    }
                    else if (write_not_read && (hiomap_config.window_type == HIOMAP_WINDOW_TYPE_WRITE))
                    {
                        if (lpc_fw_msize_to_bytes(fw_cycle_msize) >= 4)
                        {
                            for (word = 0; word < (lpc_fw_msize_to_bytes(fw_cycle_msize) / 4); word++)
                            {
                                *((uint32_t *)(host_flash_buffer + physical_flash_address + (word * 4))) =
                                    *((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + (word * 4)));
                            }
                        }
                        else
                        {
                            for (byte = 0; byte < lpc_fw_msize_to_bytes(fw_cycle_msize); byte++)
                            {
                                *((uint8_t *)(host_flash_buffer + physical_flash_address + byte)) =
                                    *((volatile uint8_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_FW_DATA_BLOCK_OFFSET + byte));
                            }
                        }

                        // Transfer success -- do not send error
                        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                        dword &= ~((AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                    }
                    else
                    {
                        printf("[WARNING] Data transfer attempted without active HIOMAP "
                               "window!  Returning error to host...\n");

                        // Invalid access -- send error
                        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                        dword |= ((1 & AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                    }
                }
                else
                {
                    printf("[WARNING] Data transfer attempted outside configured HIOMAP "
                           "window!  Returning error to host...\n");

                    // Invalid access -- send error
                    dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                    dword |= ((1 & AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                    write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
                }
            }
            else
            {
                printf("[WARNING] Received firmware cycle request for IDSEL 0x%02x "
                       "(address 0x%08x)!  Dazed and confused, but trying to "
                       "continue...\n",
                       fw_cycle_idsel, address);

                // Do not send error
                dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                dword &= ~((AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
            }

            // Acknowledge data transfer
            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
            dword |= ((1 & AQUILA_LPC_CTL_XFER_CONT_MASK) << AQUILA_LPC_CTL_XFER_CONT_SHIFT);
            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
        }
        else
#endif
            if (((status1_reg >> AQUILA_LPC_STATUS_CYCLE_TYPE_SHIFT) & AQUILA_LPC_STATUS_CYCLE_TYPE_MASK) == AQUILA_LPC_STATUS_CYCLE_TYPE_IO)
        {
            if ((address >= 0x80) && (address <= 0x82))
            {
                if (write_not_read)
                {
                    uint8_t post_code = (read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_STATUS3) >> AQUILA_LPC_STATUS_ACT_WDATA_SHIFT) &
                                        AQUILA_LPC_STATUS_ACT_WDATA_MASK;
                    if (address == 0x81)
                    {
                        post_code_high = post_code;
                    }
                    else if (address == 0x82)
                    {
                        post_code_low = post_code;
                        set_led_bank_display(((post_code_high & 0xf) << 4) | (post_code_low & 0xf));

                        if (enable_post_code_console_output)
                        {
                            printf("[POST CODE] %d.%d\n", post_code_high, post_code_low);
                        }
                    }
                }

                // Transfer success -- do not send error
                dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                dword &= ~((AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);

                // Acknowledge data transfer
                dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                dword |= ((1 & AQUILA_LPC_CTL_XFER_CONT_MASK) << AQUILA_LPC_CTL_XFER_CONT_SHIFT);
                write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
            }
            else
            {
                printf("[WARNING] LPC I/O transfer attempted to invalid address 0x%04x\n", address);

                // Transfer failed -- send error
                dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
                dword |= ((1 & AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
                write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
            }
        }
        else
        {
            // Transfer failed -- send error
            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
            dword |= ((1 & AQUILA_LPC_CTL_XFER_ERR_MASK) << AQUILA_LPC_CTL_XFER_ERR_SHIFT);
            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);

            // Acknowledge data transfer
            dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2);
            dword |= ((1 & AQUILA_LPC_CTL_XFER_CONT_MASK) << AQUILA_LPC_CTL_XFER_CONT_SHIFT);
            write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL2, dword);
        }
    }
}

void primary_service_event_loop(void)
{
    // ===================================================================================
    // Main service loop
    // ===================================================================================
    // This loop is called as frequently as practical to keep response times low
    // All background tasks, from LPC I/O transfers to IPMI requests, are handled
    // here
    // ===================================================================================

    if (host_background_service_task_active)
    {
        // Run background service task event loop
        host_background_service_task_event_loop();

        // Process queued interrupt tasks
        process_interrupts_stage2();

        // Process cooperative multitasking threads
        process_host_to_bmc_ipmi_bt_transactions();
    }
}

static void attach_to_host_console(void)
{
    uint8_t escape_sequence_state;
    uint8_t character;
    uint8_t character_read;

    // Deactivate interrupts on entering critical section
    irq_setie(0);

    // Reset VUART1 FIFO pointers
    vuart1_incoming_interrupt_transient_buffer_pos = 0;
    vuart1_incoming_interrupt_transient_buffer_overflow = 0;
    vuart1_outgoing_buffer_read_pos = 0;
    vuart1_outgoing_buffer_write_pos = 0;
    vuart1_incoming_buffer_read_pos = 0;
    vuart1_incoming_buffer_write_pos = 0;

    // Re-activate interupts on exiting critical section
    irq_setie(1);

    // Signal host console service is now active
    host_console_service_task_active = 1;

    // Enter polling loop
    escape_sequence_state = 0;

    while (1)
    {
        // Escape sequence handler
        character_read = 0;
        if (readchar_nonblock())
        {
            character = readchar();
            character_read = 1;
            switch (escape_sequence_state)
            {
                case 0:
                    if (character == '\n')
                    {
                        escape_sequence_state = 1;
                    }
                    break;
                case 1:
                    if (character == '~')
                    {
                        escape_sequence_state = 2;
                    }
                    else
                    {
                        escape_sequence_state = 0;
                    }
                    break;
                case 2:
                    if (character == '.')
                    {
                        escape_sequence_state = 3;
                    }
                    else
                    {
                        escape_sequence_state = 0;
                    }
                    break;
                default:
                    escape_sequence_state = 0;
            }
        }
        if (escape_sequence_state == 3)
        {
            break;
        }

        while (vuart1_outgoing_buffer_write_pos != vuart1_outgoing_buffer_read_pos)
        {
            uint32_t vuart1_status_register = *((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_STATUS_REG));
            if (!((vuart1_status_register >> AQUILA_LPC_VUART_WFIFO_FULL_SHIFT) & AQUILA_LPC_VUART_WFIFO_FULL_MASK))
            {
                // VUART FIFO now has room, send queued character to VUART hardware
                *((volatile uint8_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + 0x0)) = vuart1_outgoing_buffer[vuart1_outgoing_buffer_read_pos];
                vuart1_outgoing_buffer_read_pos++;
                if (vuart1_outgoing_buffer_read_pos >= 512)
                {
                    vuart1_outgoing_buffer_read_pos = 0;
                }
            }
            else
            {
                break;
            }
        }

        if (character_read)
        {
            // Attempt to send character to host
            uint32_t vuart1_status_register = *((volatile uint32_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + AQUILA_LPC_VUART1_STATUS_REG));
            if ((vuart1_status_register >> AQUILA_LPC_VUART_WFIFO_FULL_SHIFT) & AQUILA_LPC_VUART_WFIFO_FULL_MASK)
            {
                // VUART FIFO full, add to soft buffer
                vuart1_outgoing_buffer[vuart1_outgoing_buffer_write_pos] = character;
                vuart1_outgoing_buffer_write_pos++;
                if (vuart1_outgoing_buffer_write_pos >= 512)
                {
                    vuart1_outgoing_buffer_write_pos = 0;
                }
            }
            else
            {
                // VUART FIFO still has room, send character to VUART hardware
                *((volatile uint8_t *)(HOSTLPCSLAVE_BASE + AQUILA_LPC_VUART_BLOCK_OFFSET + 0x0)) = character;
            }
        }

        // Send any queued VUART output from host to BMC console
        while (vuart1_incoming_buffer_write_pos != vuart1_incoming_buffer_read_pos)
        {
            printf("%c", vuart1_incoming_buffer[vuart1_incoming_buffer_read_pos]);
            vuart1_incoming_buffer_read_pos++;
            if (vuart1_incoming_buffer_read_pos >= 512)
            {
                vuart1_incoming_buffer_read_pos = 0;
            }
        }

        primary_service_event_loop();
    }

    // Signal host console service is now inactive
    host_console_service_task_active = 0;
}

static uint64_t parse_user_provided_number(const char *string)
{
    if (((*(string + 0)) == '0') && (((*(string + 1)) == 'x') || ((*(string + 1)) == 'X')))
    {
        return strtoul(string, NULL, 16);
    }
    return strtoul(string, NULL, 10);
}

static uint8_t sanitize_ascii(uint8_t char_in)
{
    if ((char_in >= 32) && (char_in <= 126))
    {
        return char_in;
    }
    return '.';
}

static void console_service(void)
{
    char *str;
    char *token;
    uint64_t address;
    uint32_t data;
    unsigned int i;
    unsigned int length;

    str = readstr();
    if (str == NULL)
    {
        return;
    }
    token = get_token(&str);
    if (strcmp(token, "help") == 0)
    {
        help();
    }
    else if (strcmp(token, "reboot") == 0)
    {
        reboot();
    }
    else if (strcmp(token, "ipl") == 0)
    {
        start_ipl(0);
    }
    else if (strcmp(token, "sbe_status") == 0)
    {
        get_sbe_status();
    }
    else if (strcmp(token, "mr") == 0)
    {
        if (*str)
        {
            token = get_token(&str);
            address = parse_user_provided_number(token);
            if (*str)
            {
                token = get_token(&str);
                length = parse_user_provided_number(token);
            }
            else
            {
                length = 1;
            }
            for (i = 0; i < length; i++)
            {
                printf("0x%08x: 0x%08x\t%02x%02x%02x%02x\t%c%c%c%c\n", address + (i * 4), *((volatile uint32_t *)(address + (i * 4))),
                       *((volatile uint8_t *)(address + (i * 4) + 0)), *((volatile uint8_t *)(address + (i * 4) + 1)),
                       *((volatile uint8_t *)(address + (i * 4) + 2)), *((volatile uint8_t *)(address + (i * 4) + 3)),
                       sanitize_ascii(*((volatile uint8_t *)(address + (i * 4) + 0))), sanitize_ascii(*((volatile uint8_t *)(address + (i * 4) + 1))),
                       sanitize_ascii(*((volatile uint8_t *)(address + (i * 4) + 2))), sanitize_ascii(*((volatile uint8_t *)(address + (i * 4) + 3))));
            }
        }
        else
        {
            printf("USAGE: mr <memory address>\n");
        }
    }
    else if (strcmp(token, "mw") == 0)
    {
        if (*str)
        {
            token = get_token(&str);
            address = parse_user_provided_number(token);
            if (*str)
            {
                token = get_token(&str);
                length = parse_user_provided_number(token);
            }
            else
            {
                length = 1;
            }
            i = 0;
            while (*str)
            {
                token = get_token(&str);
                data = parse_user_provided_number(token);
                *((volatile uint32_t *)(address)) = data;
                i++;
                if (i >= length)
                {
                    break;
                }
            }
        }
        else
        {
            printf("USAGE: mr <memory address>\n");
        }
    }
    else if (strcmp(token, "flash_write") == 0)
    {
        if (*str)
        {
            token = get_token(&str);
            if (strcmp(token, "enable") == 0)
            {
                allow_flash_write = 1;
                printf("Flash write ENABLED\n");
            }
            else if (strcmp(token, "disable") == 0)
            {
                allow_flash_write = 0;
                printf("Flash write DISABLED\n");
            }
            else
            {
                printf("USAGE: flash_write <enable|disable>\n");
            }
        }
        else
        {
            printf("USAGE: flash_write <enable|disable>\n");
        }
    }
    else if (strcmp(token, "chassison") == 0)
    {
        power_on_chassis();
    }
    else if (strcmp(token, "chassisoff") == 0)
    {
        power_off_chassis();
        printf("Chassis power commanded OFF\n");
    }
    else if (strcmp(token, "status") == 0)
    {
        print_chassis_status();
    }
    else if (strcmp(token, "poweron") == 0)
    {
        if (power_on_host() == 0)
        {
            attach_to_host_console();
        }
        else
        {
            printf("Host poweron procedure FAILED\n");
        }
    }
    else if (strcmp(token, "post_codes") == 0)
    {
        if (*str)
        {
            token = get_token(&str);
            if (strcmp(token, "enable") == 0)
            {
                enable_post_code_console_output = 1;
                printf("POST code console output ENABLED\n");
            }
            else if (strcmp(token, "disable") == 0)
            {
                enable_post_code_console_output = 0;
                printf("POST code console output DISABLED\n");
            }
            else
            {
                printf("USAGE: post_codes <enable|disable>\n");
            }
        }
        else
        {
            printf("USAGE: post_codes <enable|disable>\n");
        }
    }
    else if (strcmp(token, "console") == 0)
    {
        attach_to_host_console();
    }
    else if (strcmp(token, "") != 0)
    {
        printf("%s: command not found\n", token);
    }
    prompt();
}

static void memcpy32(uint32_t *destination, uint32_t *source, int words)
{
    int word;
    for (word = 0; word < words; word++)
    {
        *destination = *source;
        destination++;
        source++;
    }
}

static void memset32(uint32_t *destination, uint32_t value, int words)
{
    int word;
    for (word = 0; word < words; word++)
    {
        *destination = value;
        destination++;
    }
}

#if (WITH_SPI)
static uint32_t read_host_spi_flash_id(void)
{
    uint32_t flash_id = 0;

    // Set user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) |
                              (TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Send Flash ID command
    *((volatile uint8_t *)HOSTSPIFLASH_BASE) = 0x9e;

    // Read response
    flash_id = (flash_id << 8) | (*((volatile uint8_t *)HOSTSPIFLASH_BASE) & 0xff);
    flash_id = (flash_id << 8) | (*((volatile uint8_t *)HOSTSPIFLASH_BASE) & 0xff);
    flash_id = (flash_id << 8) | (*((volatile uint8_t *)HOSTSPIFLASH_BASE) & 0xff);
    flash_id = (flash_id << 8) | (*((volatile uint8_t *)HOSTSPIFLASH_BASE) & 0xff);

    // Clear user mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    return flash_id;
}

static int host_spi_flash_init(void)
{
    int i;
    uint32_t dword;
    uint32_t flash_device_id;

    if ((read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_DEVICE_ID_HIGH) != TERCEL_SPI_DEVICE_ID_HIGH) ||
        (read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_DEVICE_ID_LOW) != TERCEL_SPI_DEVICE_ID_LOW))
    {
        return -1;
    }

    uint32_t tercel_version = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_DEVICE_VERSION);
    printf("Raptor Tercel SPI master found, device version %0d.%0d.%d\n", (tercel_version >> TERCEL_SPI_VERSION_MAJOR_SHIFT) & TERCEL_SPI_VERSION_MAJOR_MASK,
           (tercel_version >> TERCEL_SPI_VERSION_MINOR_SHIFT) & TERCEL_SPI_VERSION_MINOR_MASK,
           (tercel_version >> TERCEL_SPI_VERSION_PATCH_SHIFT) & TERCEL_SPI_VERSION_PATCH_MASK);

    flash_device_id = read_host_spi_flash_id();
    for (i = 0; i < (sizeof(micron_n25q_spi_device_ids) / sizeof(micron_n25q_spi_device_ids[0])); i++)
    {
        if (flash_device_id == micron_n25q_spi_device_ids[i])
        {
            printf("%s Flash device detected, configuring\n", micron_n25q_spi_device_names[i]);

            // Set up Flash-specific commands
            dword = 0;
            dword |= (MICRON_N25Q_SPI_4BA_QSPI_READ_CMD & TERCEL_SPI_4BA_QSPI_CMD_MASK) << TERCEL_SPI_4BA_QSPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_3BA_QSPI_READ_CMD & TERCEL_SPI_3BA_QSPI_CMD_MASK) << TERCEL_SPI_3BA_QSPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_4BA_SPI_READ_CMD & TERCEL_SPI_4BA_SPI_CMD_MASK) << TERCEL_SPI_4BA_SPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_3BA_SPI_READ_CMD & TERCEL_SPI_3BA_SPI_CMD_MASK) << TERCEL_SPI_3BA_SPI_CMD_SHIFT;
            write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG1, dword);

            dword = 0;
            dword |= (MICRON_N25Q_SPI_4BA_QSPI_FAST_READ_CMD & TERCEL_SPI_4BA_QSPI_CMD_MASK) << TERCEL_SPI_4BA_QSPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_3BA_QSPI_FAST_READ_CMD & TERCEL_SPI_3BA_QSPI_CMD_MASK) << TERCEL_SPI_3BA_QSPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_4BA_SPI_FAST_READ_CMD & TERCEL_SPI_4BA_SPI_CMD_MASK) << TERCEL_SPI_4BA_SPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_3BA_SPI_FAST_READ_CMD & TERCEL_SPI_3BA_SPI_CMD_MASK) << TERCEL_SPI_3BA_SPI_CMD_SHIFT;
            write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG2, dword);

            dword = 0;
            dword |= (MICRON_N25Q_SPI_4BA_QSPI_PAGE_PROGRAM_CMD & TERCEL_SPI_4BA_QSPI_CMD_MASK) << TERCEL_SPI_4BA_QSPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_3BA_QSPI_PAGE_PROGRAM_CMD & TERCEL_SPI_3BA_QSPI_CMD_MASK) << TERCEL_SPI_3BA_QSPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_4BA_SPI_PAGE_PROGRAM_CMD & TERCEL_SPI_4BA_SPI_CMD_MASK) << TERCEL_SPI_4BA_SPI_CMD_SHIFT;
            dword |= (MICRON_N25Q_SPI_3BA_SPI_PAGE_PROGRAM_CMD & TERCEL_SPI_3BA_SPI_CMD_MASK) << TERCEL_SPI_3BA_SPI_CMD_SHIFT;
            write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG3, dword);

            // Enable extended QSPI read/write operations
            dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1);
            dword |= TERCEL_SPI_PHY_QSPI_EXT_READ_EN_MASK << TERCEL_SPI_PHY_QSPI_EXT_READ_EN_SHIFT;
            dword |= TERCEL_SPI_PHY_QSPI_EXT_WRITE_EN_MASK << TERCEL_SPI_PHY_QSPI_EXT_WRITE_EN_SHIFT;
            write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1, dword);

            break;
        }
    }

    // Set SPI core to automatic mode
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CORE_CTL1) &
                              ~(TERCEL_SPI_ENABLE_USER_MODE_MASK << TERCEL_SPI_ENABLE_USER_MODE_SHIFT));

    // Set extra CS delay cycle count to 0
    dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1);
    dword &= ~(TERCEL_SPI_PHY_CS_EXTRA_IDLE_CYC_MASK << TERCEL_SPI_PHY_CS_EXTRA_IDLE_CYC_SHIFT);
    dword |= ((0 & TERCEL_SPI_PHY_CS_EXTRA_IDLE_CYC_MASK) << TERCEL_SPI_PHY_CS_EXTRA_IDLE_CYC_SHIFT);
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1, dword);

    // Set maximum CS assert cycle count to 10000
    dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG4);
    dword &= ~(TERCEL_SPI_FLASH_CS_EN_LIMIT_CYC_MASK << TERCEL_SPI_FLASH_CS_EN_LIMIT_CYC_SHIFT);
    dword |= ((10000 & TERCEL_SPI_FLASH_CS_EN_LIMIT_CYC_MASK) << TERCEL_SPI_FLASH_CS_EN_LIMIT_CYC_SHIFT);
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG4, dword);

    // Set SPI fast read dummy cycles to
    // MICRON_N25Q_SPI_FAST_READ_DUMMY_CLOCK_CYCLES
    dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1);
    dword &= ~(TERCEL_SPI_PHY_DUMMY_CYCLES_MASK << TERCEL_SPI_PHY_DUMMY_CYCLES_SHIFT);
    dword |= ((MICRON_N25Q_SPI_FAST_READ_DUMMY_CLOCK_CYCLES & TERCEL_SPI_PHY_DUMMY_CYCLES_MASK) << TERCEL_SPI_PHY_DUMMY_CYCLES_SHIFT);
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1, dword);

    // Enable SPI fast read functionality
    dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1);
    dword &= ~(TERCEL_SPI_PHY_FAST_READ_ENABLE_MASK << TERCEL_SPI_PHY_FAST_READ_ENABLE_SHIFT);
    dword |= ((1 & TERCEL_SPI_PHY_FAST_READ_ENABLE_MASK) << TERCEL_SPI_PHY_FAST_READ_ENABLE_SHIFT);
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1, dword);

    // Set SPI controller to 4BA mode
    dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1);
    dword &= ~(TERCEL_SPI_PHY_4BA_ENABLE_MASK << TERCEL_SPI_PHY_4BA_ENABLE_SHIFT);
    dword |= ((TERCEL_SPI_PHY_4BA_MODE & TERCEL_SPI_PHY_4BA_ENABLE_MASK) << TERCEL_SPI_PHY_4BA_ENABLE_SHIFT);
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1, dword);

#if (ALLOW_SPI_QUAD_MODE)
    // Set SPI controller to QSPI mode
    dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1);
    dword &= ~(TERCEL_SPI_PHY_IO_TYPE_MASK << TERCEL_SPI_PHY_IO_TYPE_SHIFT);
    dword |= ((TERCEL_SPI_PHY_IO_TYPE_QUAD & TERCEL_SPI_PHY_IO_TYPE_MASK) << TERCEL_SPI_PHY_IO_TYPE_SHIFT);
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1, dword);
#endif

    // Set SPI clock cycle divider to 5
    dword = read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1);
    dword &= ~(TERCEL_SPI_PHY_CLOCK_DIVISOR_MASK << TERCEL_SPI_PHY_CLOCK_DIVISOR_SHIFT);
    dword |= ((5 & TERCEL_SPI_PHY_CLOCK_DIVISOR_MASK) << TERCEL_SPI_PHY_CLOCK_DIVISOR_SHIFT);
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1, dword);

    // Calculate and dump configured SPI clock speed
    uint8_t spi_divisor =
        (read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1) >> TERCEL_SPI_PHY_CLOCK_DIVISOR_SHIFT) & TERCEL_SPI_PHY_CLOCK_DIVISOR_MASK;
    spi_divisor = (spi_divisor + 1) * 2;
    uint8_t spi_dummy_cycles =
        (read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_PHY_CFG1) >> TERCEL_SPI_PHY_DUMMY_CYCLES_SHIFT) & TERCEL_SPI_PHY_DUMMY_CYCLES_MASK;
    printf("Flash controller frequency configured to %d MHz (bus frequency %d MHz, "
           "dummy cycles %d)\n",
           (read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CLK_FREQ) / spi_divisor) / 1000000,
           read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_CLK_FREQ) / 1000000, spi_dummy_cycles);

    // Enable read merging
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG5,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG5) |
                              (TERCEL_SPI_FLASH_EN_MULTCYC_READ_MASK << TERCEL_SPI_FLASH_EN_MULTCYC_READ_SHIFT));

    // Enable write merging
    write_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG5,
                          read_tercel_register(HOSTSPIFLASHCFG_BASE, TERCEL_SPI_REG_SYS_FLASH_CFG5) |
                              (TERCEL_SPI_FLASH_EN_MULTCYC_WRITE_MASK << TERCEL_SPI_FLASH_EN_MULTCYC_WRITE_SHIFT));

    return 0;
}
#endif

#if (WITH_SPI)
#define SPI_READ_TRANSFER_SIZE (1 * 1024 * 1024LL)
int copy_spi_flash_to_internal_buffer(uintptr_t flash_data, uintptr_t flash_ctl, uint8_t *buffer)
{
    int retcode;
    int chunk;
    int redundancy_chunk;
    uintptr_t redundancy_buffer_offset;
#if (SPI_FLASH_TRIPLE_READ)
    uintptr_t buffer_check_offset;
    uint32_t value1;
    uint32_t value2;
    uint32_t value3;
    uint32_t final_value;
#endif

    printf("Copying host Flash ROM (%p) to internal buffer (%p)...\n", flash_data, buffer);
    retcode = 0;

    for (chunk = 0; chunk < FLASH_SIZE_BYTES / SPI_READ_TRANSFER_SIZE; chunk++)
    {
#if (SPI_FLASH_TRIPLE_READ)
        for (redundancy_chunk = 0; redundancy_chunk < 3; redundancy_chunk++)
        {
#else
        for (redundancy_chunk = 0; redundancy_chunk < 1; redundancy_chunk++)
        {
#endif
            redundancy_buffer_offset = redundancy_chunk * SPI_READ_TRANSFER_SIZE;
            memcpy32((uint32_t *)(buffer + redundancy_buffer_offset + (chunk * SPI_READ_TRANSFER_SIZE)),
                     (uint32_t *)(flash_data + (chunk * SPI_READ_TRANSFER_SIZE)), SPI_READ_TRANSFER_SIZE / 4);
            printf("\r[%d/%d]", chunk + 1, FLASH_SIZE_BYTES / SPI_READ_TRANSFER_SIZE);

            // Reset ongoing multibyte access due to die switch requirements on the N25Q
            // Flash devices
            write_tercel_register(flash_ctl, TERCEL_SPI_REG_SYS_FLASH_CFG5,
                                  read_tercel_register(flash_ctl, TERCEL_SPI_REG_SYS_FLASH_CFG5) &
                                      ~(TERCEL_SPI_FLASH_EN_MULTCYC_READ_MASK << TERCEL_SPI_FLASH_EN_MULTCYC_READ_SHIFT));
            write_tercel_register(flash_ctl, TERCEL_SPI_REG_SYS_FLASH_CFG5,
                                  read_tercel_register(flash_ctl, TERCEL_SPI_REG_SYS_FLASH_CFG5) |
                                      (TERCEL_SPI_FLASH_EN_MULTCYC_READ_MASK << TERCEL_SPI_FLASH_EN_MULTCYC_READ_SHIFT));
        }
#if (SPI_FLASH_TRIPLE_READ)
        for (buffer_check_offset = 0; buffer_check_offset < SPI_READ_TRANSFER_SIZE; buffer_check_offset = buffer_check_offset + 4)
        {
            value1 = *((uint32_t *)(buffer + buffer_check_offset + (chunk * SPI_READ_TRANSFER_SIZE)));
            value2 = *((uint32_t *)(buffer + buffer_check_offset + SPI_READ_TRANSFER_SIZE + (chunk * SPI_READ_TRANSFER_SIZE)));
            value3 = *((uint32_t *)(buffer + buffer_check_offset + (2 * SPI_READ_TRANSFER_SIZE) + (chunk * SPI_READ_TRANSFER_SIZE)));
            if (!((value1 == value2) && (value1 == value3)))
            {
                printf("[WARNING] Triple read FAILED integrity check at address 0x%08x!  Values 0x%08x/0x%08x/0x%08x\n",
                       buffer + buffer_check_offset + (chunk * SPI_READ_TRANSFER_SIZE), value1, value2, value3);
                if (value1 == value2)
                {
                    final_value = value1;
                }
                else if (value2 == value3)
                {
                    final_value = value2;
                }
                else if (value1 == value3)
                {
                    final_value = value1;
                }
                else
                {
                    printf("[ERROR] UNCORRECTABLE data read at address 0x%08x!\n", buffer + buffer_check_offset + (chunk * SPI_READ_TRANSFER_SIZE));
                    final_value = 0xdeadbeef;
                    retcode = -1;
                }
                *((uint32_t *)(buffer + buffer_check_offset + (chunk * SPI_READ_TRANSFER_SIZE))) = final_value;
                if (retcode)
                {
                    // Fast abort on fatal error
                    break;
                }
            }
        }
#endif
    }
    printf("\r%dMB copied\n", chunk);

    return retcode;
}
#endif

int main(void)
{
    uint32_t dword;

#ifdef CONFIG_CPU_HAS_INTERRUPT
    // Mask external interrupts / enable global interrupts
    irq_setmask(0);
    irq_setie(1);
#endif
    uart_init();
    gpio_init();

    display_character('0', 0); // STATUS CODE: 0

    for (int i = 0; i < MAX_CPUS_SUPPORTED; i++)
    {
        initialize_i2c_master(g_cpu_info[i].i2c_master, g_cpu_info[i].i2c_frequency);
    }
    // initialize_i2c_master((uint8_t*)I2CMASTER3_BASE, 100000);
    initialize_i2c_master((uint8_t *)I2CMASTER4_BASE, 100000);

    // Check for Aquila core presence
    if ((read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DEVICE_ID_HIGH) == AQUILA_LPC_DEVICE_ID_HIGH) &&
        (read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DEVICE_ID_LOW) == AQUILA_LPC_DEVICE_ID_LOW))
    {
        uint32_t aquila_version = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_DEVICE_VERSION);
        printf("Raptor Aquila LPC slave found, device version %0d.%0d.%d\n", (aquila_version >> AQUILA_LPC_VERSION_MAJOR_SHIFT) & AQUILA_LPC_VERSION_MAJOR_MASK,
               (aquila_version >> AQUILA_LPC_VERSION_MINOR_SHIFT) & AQUILA_LPC_VERSION_MINOR_MASK,
               (aquila_version >> AQUILA_LPC_VERSION_PATCH_SHIFT) & AQUILA_LPC_VERSION_PATCH_MASK);

        // Configure Aquila core to intercept I/O port ranges 0x80-0x82 and
        // 0x3f8-0x3ff 0x80
        dword = 0;
        dword |= ((0x82 & AQUILA_LPC_RANGE_END_ADDR_MASK) << AQUILA_LPC_RANGE_END_ADDR_SHIFT);
        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_RANGE1_END, dword);
        dword = 0;
        dword |= ((0x80 & AQUILA_LPC_RANGE_START_ADDR_MASK) << AQUILA_LPC_RANGE_START_ADDR_SHIFT);
        dword |= ((1 & AQUILA_LPC_RANGE_ALLOW_IO_MASK) << AQUILA_LPC_RANGE_ALLOW_IO_SHIFT);
        dword |= ((1 & AQUILA_LPC_RANGE_ENABLE_MASK) << AQUILA_LPC_RANGE_ENABLE_SHIFT);
        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_RANGE1_CONFIG, dword);

        // Enable I/O cycle intercept
        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
        dword |= ((1 & AQUILA_LPC_CTL_EN_IO_CYCLES_MASK) << AQUILA_LPC_CTL_EN_IO_CYCLES_SHIFT);
        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);

        // Enable VUART1
        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
        dword |= ((1 & AQUILA_LPC_CTL_EN_VUART1_MASK) << AQUILA_LPC_CTL_EN_VUART1_SHIFT);
        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);

        // Enable IPMI BT functionality
        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
        dword |= ((1 & AQUILA_LPC_CTL_EN_IPMI_BT_MASK) << AQUILA_LPC_CTL_EN_IPMI_BT_SHIFT);
        dword &= ~((AQUILA_LPC_CTL_IPMI_BT_ADDR_MASK) << AQUILA_LPC_CTL_IPMI_BT_ADDR_SHIFT);
        dword |= ((0xe4 & AQUILA_LPC_CTL_IPMI_BT_ADDR_MASK) << AQUILA_LPC_CTL_IPMI_BT_ADDR_SHIFT);
        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);

        // Enable firmware cycle intercept
        dword = read_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1);
        dword |= ((1 & AQUILA_LPC_CTL_EN_FW_CYCLES_MASK) << AQUILA_LPC_CTL_EN_FW_CYCLES_SHIFT);
        write_aquila_register(HOSTLPCSLAVE_BASE, AQUILA_LPC_REG_CONTROL1, dword);
    }

    // Allocate internal host SPI Flash ROM buffer
    host_flash_buffer = (uint8_t *)(MAIN_RAM_BASE + 0x3e00000);

    // Clear SPI Flash buffer
    printf("Clearing host ROM internal buffer...");
    memset32((uint32_t *)host_flash_buffer, 0xdeadbeef, 0x4000000 / 4);
    printf("\rInternal host ROM buffer cleared    \n");

    display_character('1', 0); // STATUS CODE: 1

#if (WITH_SPI)
    host_spi_flash_init();

    // Detect and print attached host SPI Flash ID
    printf("Host SPI flash ID: 0x%08x\n", read_host_spi_flash_id());

    reset_flash_device();
    configure_flash_device();

    // Copy external SPI Flash ROM contents to internal host SPI Flash ROM buffer
    copy_spi_flash_to_internal_buffer(HOSTSPIFLASH_BASE, HOSTSPIFLASHCFG_BASE, host_flash_buffer);

#if (DEBUG_HOST_SPI_FLASH_READ)
    printf("host_flash_buffer: %p First 1KB:\n", host_flash_buffer);
    int byte = 0;
    int row = 0;
    for (row = 0; row < 32; row++)
    {
        for (byte = 0; byte < 32; byte++)
        {
            printf("%02x ", host_flash_buffer[byte + (row * 32)]);
        }
        printf("\n");
    }

    printf("[1/5] CRC of first 64B: %08x\n", crc32(host_flash_buffer, 1 * 64LL));
    printf("[2/5] CRC of first 1KB: %08x (next %08x, next %08x)\n", crc32(host_flash_buffer, 1024LL), crc32(host_flash_buffer + 1024LL, 1024LL),
           crc32(host_flash_buffer + (2 * 1024LL), 1024LL));
    printf("[3/5] CRC of first 1KB: %08x (next %08x, next %08x)\n", crc32(host_flash_buffer, 1024LL), crc32(host_flash_buffer + 1024LL, 1024LL),
           crc32(host_flash_buffer + (2 * 1024LL), 1024LL));
    printf("[4/5] CRC of first 1MB: %08x\n", crc32(host_flash_buffer, 1 * 1024 * 1024LL));
    printf("[5/5] CRC of full 64MB: %08x\n", crc32(host_flash_buffer, 64 * 1024 * 1024LL));

    // HBBL on test sytem is from 0x206200 to 0x207388 inclusive
    printf("\nHBBL region:\n");
    for (row = 0; row < 141; row++)
    {
        for (byte = 0; byte < 32; byte++)
        {
            printf("%02x ", *(host_flash_buffer + 0x206200ULL + (byte + (row * 32))));
            // printf("%02x ", *(host_flash_buffer + 0x1000ULL + (byte + (row *
            // 32))));
        }
        printf("\n");
    }
    printf("CRC of HBBL region: %08x\n", crc32(host_flash_buffer + 0x206200ULL, 4488));
#endif
#endif

    display_character('2', 0); // STATUS CODE: 2

    puts("\nRaptor Open FSP\nBuilt "__DATE__
         " "__TIME__
         "\n");
    help();
    prompt();

    while (1)
    {
        console_service();
    }

    return 0;
}