#include "gpio.h"
#include "ssi_reg.h"

void __attribute__((section(".text.boot2_pre"))) boot2_copy_self(void)
{
    #define BOOT2_FLASHCMD_READ_DATA (0x03)
    #define BOOT2_FLASH_OFFSET       (256)
    extern uint32_t _boot2_copy_self_start_addr;
    extern uint32_t _boot2_copy_self_end_addr;

    io_rw_32 *reg;
    uint32_t cmd;
    uint32_t length, tx_count, rx_count, rx_skip;
    uint8_t *copy_to_addr;
    uint8_t rxbyte;

    /* force QSPI_CS low level as selected */
    reg = (io_rw_32 *)(IO_QSPI_BASE + GPIO_QSPI_CS_CTRL_OFFSET);
    *reg = GPIO_OVER_OUT_FORCE_LOW << GPIO_OVER_OUT_POS;

    /* send command and length */
    cmd = (BOOT2_FLASHCMD_READ_DATA << 24) | BOOT2_FLASH_OFFSET;
    for (int i = 0; i < 4; ++i) {
        ssi_hw->dr0 = cmd >> 24;
        cmd <<= 8;
    }
    /* init parameter */
    length = (uint32_t)&_boot2_copy_self_end_addr - (uint32_t)&_boot2_copy_self_start_addr;
    tx_count = length;
    rx_count = length;
    rx_skip = 4;
    copy_to_addr = (uint8_t *)&_boot2_copy_self_start_addr;
    /* copy data from flash to ram */
    while (tx_count || rx_skip || rx_count) {
        uint32_t tx_level = ssi_hw->txflr;
        uint32_t rx_level = ssi_hw->rxflr;
        if (tx_count && tx_level + rx_level < 14) {
            ssi_hw->dr0 = 0;
            tx_count--;
        }
        if (rx_level) {
            rxbyte = ssi_hw->dr0;
            if (rx_skip) {
                rx_skip--;
            } else {
                *copy_to_addr++ = rxbyte;
                rx_count--;
            }
        }
    }

    /* force QSPI_CS high level as disselected */
    *reg = GPIO_OVER_OUT_FORCE_HIGH << GPIO_OVER_OUT_POS;
}

#include "resets.h"
#include "clock.h"
#include "uart.h"
#include "flash.h"
#include "timer.h"
#include "stdio.h"

uint8_t uart_tx_buffer[512];
uint8_t uart_rx_buffer[512];
uint8_t flash_tx_buffer[512];
uint8_t flash_rx_buffer[512];

struct uart_cfg_s uart_cfg = {
    .baudrate = 2 * 1000 * 1000,
    .mode = UART_MODE_TX_RX,
    .data_bits = UART_DATABITS_8,
    .parity = UART_PARITY_NONE,
    .stop_bits = UART_STOPBITS_1,
    .fifo_enable = ENABLE,
    .tx_fifo_level = UART_FIFO_LEVEL_1_2,
    .rx_fifo_level = UART_FIFO_LEVEL_1_2,
};

int main(void)
{
    clock_ref_set_src(CLOCK_REF_SRC_XOSC_GLITCHLESS);
    clock_sys_set_src(CLOCK_SYS_SRC_REF_GLITCHLESS);
    /* refdiv >= 5MHz, VCO=[750:1600]MHz, fbdiv=[16:320], postdiv=[1:7] */
    clock_pll_init(CLOCK_PLL_SYSPLL, 1, 100, 5, 2); /* 12MHz / 1 * 100 / 5 / 2 = 120MHz */
    clock_sys_set_div(2 << 8);                      /* 120MHz / 2 = 60MHz */
    clock_sys_set_src(CLOCK_SYS_SRC_SYSPLL);
    clock_peri_set(ENABLE, CLOCK_PERI_SRC_SYSPLL);

    reset_unreset_blocks_wait(RESETS_BLOCK_IO_BANK0 | RESETS_BLOCK_PADS_BANK0 | RESETS_BLOCK_UART0 | RESETS_BLOCK_TIMER);
    uart_init(UART_ID_0, &uart_cfg);
    gpio_init_simple(0, GPIO_FUNC_UART, DISABLE, ENABLE);
    timer_start();

    printf("boot2 system clock = 60MHz\r\n");
    printf("boot2 peripheral clock = 120MHz\r\n");

    // flash_erase(addr);
    flash_read(0x1200, flash_rx_buffer, FLASH_WRITE_SIZE);
    // flash_write(addr, data, FLASH_WRITE_SIZE);
    return 0;
}