#include "fpga.h"
#include "fpga_bin.h"
#include "string.h"

// A public method to flash iCE40 FPGA on Humpback
void flash_fpga()
{
    // Data buffer setup
    uint8_t dummy_bytes[13];
    memset(dummy_bytes, 0, 13);

    // Drive CRESET_B low
    HAL_GPIO_WritePin(FPGA_CRESET_GPIO_Port, FPGA_CRESET_Pin, GPIO_PIN_RESET);

    // Drive SPI_SS_B low
    HAL_GPIO_WritePin(SPI_FPGA_CS_GPIO_Port, SPI_FPGA_CS_Pin, GPIO_PIN_RESET);

    // Wait at least 200ns
    HAL_Delay(5);

    // Drive CRESET_B high
    HAL_GPIO_WritePin(FPGA_CRESET_GPIO_Port, FPGA_CRESET_Pin, GPIO_PIN_SET);

    // Wait at least another 1200us to clear internal config memory
    HAL_Delay(5);

    // Before data transmission starts, check if C_DONE is truly low
    // If C_DONE is high, the FPGA reset procedure is unsuccessful
    while (HAL_GPIO_ReadPin(FPGA_CDONE_GPIO_Port, FPGA_CDONE_Pin) == GPIO_PIN_SET);

    // Set SPI_SS_B high
    HAL_GPIO_WritePin(SPI_FPGA_CS_GPIO_Port, SPI_FPGA_CS_Pin, GPIO_PIN_SET);

    // Send 8 dummy clock, effectively 1 byte of 0x00
    HAL_SPI_Transmit(&hspi1, dummy_bytes, 1, 1000);

    // Drive SPI_SS_B low
    HAL_GPIO_WritePin(SPI_FPGA_CS_GPIO_Port, SPI_FPGA_CS_Pin, GPIO_PIN_RESET);

    // Send the whole image without interruption
    for (uint16_t i = 0; i < 1351U; i++)
    {
        HAL_SPI_Transmit(&hspi1, (uint8_t *)__build_syrostan_fpga_bin+100*i, 100, HAL_MAX_DELAY-1);
    }
    // HAL_SPI_Transmit(&hspi1, (uint8_t *)__build_syrostan_fpga_bin, 65535U, HAL_MAX_DELAY-1);
    // HAL_SPI_Transmit(&hspi1, (uint8_t *)&__build_syrostan_fpga_bin[65535U], 65535U, HAL_MAX_DELAY-1);
    // HAL_SPI_Transmit(&hspi1, (uint8_t *)&__build_syrostan_fpga_bin[131070UL], 4030U, HAL_MAX_DELAY-1);
    

    // Drive SPI_SS_B high
    HAL_GPIO_WritePin(SPI_FPGA_CS_GPIO_Port, SPI_FPGA_CS_Pin, GPIO_PIN_SET);

    // Send at another 100 dummy clocks (choosing 13 bytes)
    HAL_SPI_Transmit(&hspi1, dummy_bytes, 13, 1000);

    // Check the CDONE output from FPGA
    // CDONE needs to be high
    while (HAL_GPIO_ReadPin(FPGA_CDONE_GPIO_Port, FPGA_CDONE_Pin) == GPIO_PIN_RESET);

    // Send at least another 49 clock cycles to activate IO pins (choosing same 13 bytes)
    HAL_SPI_Transmit(&hspi1, dummy_bytes, 13, 1000);
}

void fsmc_read(uint16_t address, uint8_t *dat)
{
    HAL_GPIO_WritePin(FSMC_NE1_GPIO_Port, FSMC_NE1_Pin, GPIO_PIN_RESET);
    // uint32_t fsmc_add = 0x60000000 | (address << 8);
    // *dat = (*(__IO uint32_t *)(fsmc_add));
    HAL_GPIO_WritePin(FSMC_A0_GPIO_Port, FSMC_A0_Pin, (address >> 0) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A1_GPIO_Port, FSMC_A1_Pin, (address >> 1) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A2_GPIO_Port, FSMC_A2_Pin, (address >> 2) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A3_GPIO_Port, FSMC_A3_Pin, (address >> 3) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A4_GPIO_Port, FSMC_A4_Pin, (address >> 4) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A5_GPIO_Port, FSMC_A5_Pin, (address >> 5) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A6_GPIO_Port, FSMC_A6_Pin, (address >> 6) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A7_GPIO_Port, FSMC_A7_Pin, (address >> 7) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A8_GPIO_Port, FSMC_A8_Pin, (address >> 8) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A9_GPIO_Port, FSMC_A9_Pin, (address >> 9) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A10_GPIO_Port, FSMC_A10_Pin, (address >> 10) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A11_GPIO_Port, FSMC_A11_Pin, (address >> 11) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A12_GPIO_Port, FSMC_A12_Pin, (address >> 12) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A13_GPIO_Port, FSMC_A13_Pin, (address >> 13) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A14_GPIO_Port, FSMC_A14_Pin, (address >> 14) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A15_GPIO_Port, FSMC_A15_Pin, (address >> 15) & 0x0001);

    GPIO_InitTypeDef GPIO_InitStruct = {0};

    GPIO_InitStruct.Pin = FSMC_D0_Pin|FSMC_D1_Pin|FSMC_D2_Pin|FSMC_D3_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = FSMC_D4_Pin|FSMC_D5_Pin|FSMC_D6_Pin|FSMC_D7_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);

    *dat = 0x00;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D0_GPIO_Port, FSMC_D0_Pin) << 0;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D1_GPIO_Port, FSMC_D1_Pin) << 1;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D2_GPIO_Port, FSMC_D2_Pin) << 2;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D3_GPIO_Port, FSMC_D3_Pin) << 3;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D4_GPIO_Port, FSMC_D4_Pin) << 4;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D5_GPIO_Port, FSMC_D5_Pin) << 5;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D6_GPIO_Port, FSMC_D6_Pin) << 6;
    *dat |= (uint8_t)HAL_GPIO_ReadPin(FSMC_D7_GPIO_Port, FSMC_D7_Pin) << 7;

    HAL_GPIO_WritePin(FSMC_NOE_GPIO_Port, FSMC_NOE_Pin, GPIO_PIN_RESET);
    HAL_Delay(1);
    HAL_GPIO_WritePin(FSMC_NOE_GPIO_Port, FSMC_NOE_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(FSMC_NE1_GPIO_Port, FSMC_NE1_Pin, GPIO_PIN_SET);
}

void fsmc_write(uint16_t address, uint8_t dat)
{
    HAL_GPIO_WritePin(FSMC_NE1_GPIO_Port, FSMC_NE1_Pin, GPIO_PIN_RESET);
    // uint32_t fsmc_add = 0x60000000 | (address << 8);
    // (*(__IO uint32_t *)(fsmc_add)) = dat;
    HAL_GPIO_WritePin(FSMC_A0_GPIO_Port, FSMC_A0_Pin, (address >> 0) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A1_GPIO_Port, FSMC_A1_Pin, (address >> 1) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A2_GPIO_Port, FSMC_A2_Pin, (address >> 2) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A3_GPIO_Port, FSMC_A3_Pin, (address >> 3) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A4_GPIO_Port, FSMC_A4_Pin, (address >> 4) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A5_GPIO_Port, FSMC_A5_Pin, (address >> 5) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A6_GPIO_Port, FSMC_A6_Pin, (address >> 6) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A7_GPIO_Port, FSMC_A7_Pin, (address >> 7) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A8_GPIO_Port, FSMC_A8_Pin, (address >> 8) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A9_GPIO_Port, FSMC_A9_Pin, (address >> 9) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A10_GPIO_Port, FSMC_A10_Pin, (address >> 10) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A11_GPIO_Port, FSMC_A11_Pin, (address >> 11) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A12_GPIO_Port, FSMC_A12_Pin, (address >> 12) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A13_GPIO_Port, FSMC_A13_Pin, (address >> 13) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A14_GPIO_Port, FSMC_A14_Pin, (address >> 14) & 0x0001);
    HAL_GPIO_WritePin(FSMC_A15_GPIO_Port, FSMC_A15_Pin, (address >> 15) & 0x0001);

    GPIO_InitTypeDef GPIO_InitStruct = {0};

    GPIO_InitStruct.Pin = FSMC_D0_Pin|FSMC_D1_Pin|FSMC_D2_Pin|FSMC_D3_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = FSMC_D4_Pin|FSMC_D5_Pin|FSMC_D6_Pin|FSMC_D7_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);

    HAL_GPIO_WritePin(FSMC_D0_GPIO_Port, FSMC_D0_Pin, (dat >> 0) & 0x01);
    HAL_GPIO_WritePin(FSMC_D1_GPIO_Port, FSMC_D1_Pin, (dat >> 1) & 0x01);
    HAL_GPIO_WritePin(FSMC_D2_GPIO_Port, FSMC_D2_Pin, (dat >> 2) & 0x01);
    HAL_GPIO_WritePin(FSMC_D3_GPIO_Port, FSMC_D3_Pin, (dat >> 3) & 0x01);
    HAL_GPIO_WritePin(FSMC_D4_GPIO_Port, FSMC_D4_Pin, (dat >> 4) & 0x01);
    HAL_GPIO_WritePin(FSMC_D5_GPIO_Port, FSMC_D5_Pin, (dat >> 5) & 0x01);
    HAL_GPIO_WritePin(FSMC_D6_GPIO_Port, FSMC_D6_Pin, (dat >> 6) & 0x01);
    HAL_GPIO_WritePin(FSMC_D7_GPIO_Port, FSMC_D7_Pin, (dat >> 7) & 0x01);

    HAL_GPIO_WritePin(FSMC_NWE_GPIO_Port, FSMC_NWE_Pin, GPIO_PIN_RESET);
    HAL_Delay(1);
    HAL_GPIO_WritePin(FSMC_NWE_GPIO_Port, FSMC_NWE_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(FSMC_NE1_GPIO_Port, FSMC_NE1_Pin, GPIO_PIN_SET);
}