
#include "fsl_spifi.h"
#include "fsl_clock.h"
#include "fsl_debug_console.h"



/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define EXAMPLE_SPIFI        SPIFI0
#define PAGE_SIZE            (256)
#define SECTOR_SIZE          (4096)
#define EXAMPLE_SPI_BAUDRATE (1000000)
#define FLASH_W25Q
#define COMMAND_NUM          (6)
#define READ                 (0)
#define PROGRAM_PAGE         (1)
#define GET_STATUS           (2)
#define ERASE_SECTOR         (3)
#define WRITE_ENABLE         (4)
#define WRITE_REGISTER       (5)

#define TARGET_SECTOR_ADDR   (10)

/*******************************************************************************
 * Variables
 ******************************************************************************/
 
uint8_t g_buffer[PAGE_SIZE];    //array to hold input data

#if defined FLASH_W25Q
spifi_command_t command[COMMAND_NUM] = {
    {PAGE_SIZE, false, kSPIFI_DataInput, 1, kSPIFI_CommandDataQuad, kSPIFI_CommandOpcodeAddrThreeBytes, 0x6B},
    {PAGE_SIZE, false, kSPIFI_DataOutput, 0, kSPIFI_CommandDataQuad, kSPIFI_CommandOpcodeAddrThreeBytes, 0x32},
    {1, false, kSPIFI_DataInput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x05},
    {0, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeAddrThreeBytes, 0x20},
    {0, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x06},
    {1, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x31}};
#define QUAD_MODE_VAL 0x02
#elif defined FLASH_MX25R
spifi_command_t command[COMMAND_NUM] = {
    {PAGE_SIZE, false, kSPIFI_DataInput, 1, kSPIFI_CommandDataQuad, kSPIFI_CommandOpcodeAddrThreeBytes, 0x6B},
    {PAGE_SIZE, false, kSPIFI_DataOutput, 0, kSPIFI_CommandOpcodeSerial, kSPIFI_CommandOpcodeAddrThreeBytes, 0x38},
    {1, false, kSPIFI_DataInput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x05},
    {0, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeAddrThreeBytes, 0x20},
    {0, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x06},
    {1, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x01}};
#define QUAD_MODE_VAL 0x40
#else /* Use MT25Q */
spifi_command_t command[COMMAND_NUM] = {
    {PAGE_SIZE, false, kSPIFI_DataInput, 1, kSPIFI_CommandDataQuad, kSPIFI_CommandOpcodeAddrThreeBytes, 0x6B},
    {PAGE_SIZE, false, kSPIFI_DataOutput, 0, kSPIFI_CommandOpcodeSerial, kSPIFI_CommandOpcodeAddrThreeBytes, 0x38},
    {1, false, kSPIFI_DataInput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x05},
    {0, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeAddrThreeBytes, 0x20},
    {0, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x06},
    {1, false, kSPIFI_DataOutput, 0, kSPIFI_CommandAllSerial, kSPIFI_CommandOpcodeOnly, 0x61}};
#endif

/*******************************************************************************
 * Code
 ******************************************************************************/
void check_if_finish()
{
    uint8_t val = 0;
    /* Check WIP bit */
    do
    {
        SPIFI_SetCommand(EXAMPLE_SPIFI, &command[GET_STATUS]);
        while ((EXAMPLE_SPIFI->STAT & SPIFI_STAT_INTRQ_MASK) == 0U)
        {
        }
        val = SPIFI_ReadDataByte(EXAMPLE_SPIFI);
    } while (val & 0x1);
}

#if defined QUAD_MODE_VAL
void enable_quad_mode()
{
    /* Write enable */
    SPIFI_SetCommand(EXAMPLE_SPIFI, &command[WRITE_ENABLE]);

    /* Set write register command */
    SPIFI_SetCommand(EXAMPLE_SPIFI, &command[WRITE_REGISTER]);

    SPIFI_WriteDataByte(EXAMPLE_SPIFI, QUAD_MODE_VAL);

    check_if_finish();
}
#endif

void spifi_flash_init(void)
{
    spifi_config_t config = {0};
    uint32_t sourceClockFreq;
    /* Set SPIFI clock source */
    CLOCK_AttachClk(kFRO_HF_to_SPIFI_CLK);
    sourceClockFreq = CLOCK_GetFroHfFreq();

    /* Set the clock divider */
    CLOCK_SetClkDiv(kCLOCK_DivSpifiClk, sourceClockFreq / EXAMPLE_SPI_BAUDRATE, false);

    /* Initialize SPIFI */
    SPIFI_GetDefaultConfig(&config);
    SPIFI_Init(EXAMPLE_SPIFI, &config);

#if defined QUAD_MODE_VAL
    /* Enable Quad mode */
    enable_quad_mode();
#endif
}

void spifi_set_memory_mode(void)
{
    /* Reset to memory command mode */
    SPIFI_ResetCommand(EXAMPLE_SPIFI);
    /* Setup memory command */
    SPIFI_SetMemoryCommand(EXAMPLE_SPIFI, &command[READ]);
}


void spifi_set_command_mode(void)
{
    /* Reset to memory command mode */
    SPIFI_ResetCommand(EXAMPLE_SPIFI);
}

 
__attribute__((section("RAM_LOCATION"))) void spifi_sector_program_test(void)
{
    uint32_t page = 0;
    uint32_t i = 0, j = 0, data = 0;
    
    /* input data initialize */
    for(i = 0; i < PAGE_SIZE; i++)
    {
        g_buffer[i] = i;
    }
    
    
    
    /* Erase the target sector of SPIFI flash */
    SPIFI_SetCommand(EXAMPLE_SPIFI, &command[WRITE_ENABLE]);    /* Write enable */
    SPIFI_SetCommandAddress(EXAMPLE_SPIFI, TARGET_SECTOR_ADDR*SECTOR_SIZE); /* Set address */
    SPIFI_SetCommand(EXAMPLE_SPIFI, &command[ERASE_SECTOR]);    /* Erase sector */
    check_if_finish();                                          /* Check if finished */
    
    /* Write data to target sector page by page */
    while (page < (SECTOR_SIZE / PAGE_SIZE))
    {
        SPIFI_SetCommand(EXAMPLE_SPIFI, &command[WRITE_ENABLE]); /* Write enable must be set prior to every Page Program */
        SPIFI_SetCommandAddress(EXAMPLE_SPIFI, TARGET_SECTOR_ADDR * SECTOR_SIZE + page * PAGE_SIZE);
        SPIFI_SetCommand(EXAMPLE_SPIFI, &command[PROGRAM_PAGE]);
        for (i = 0; i < PAGE_SIZE; i += 4)
        {
            for (j = 0; j < 4; j++)
            {
                data |= ((uint32_t)(g_buffer[i + j])) << (j * 8);
            }
            SPIFI_WriteData(EXAMPLE_SPIFI, data);
            data = 0;
        }
        page++;
        check_if_finish();
    }
    
}


void spifi_sector_read_test(void)
{
    uint8_t *pVal = NULL;
    uint32_t i = 0, err = 0;
    
    
    
    for (i = 0; i < SECTOR_SIZE; i++)
    {
        pVal = (uint8_t *)(FSL_FEATURE_SPIFI_START_ADDR + TARGET_SECTOR_ADDR * SECTOR_SIZE + i);
        if (*pVal != g_buffer[i % PAGE_SIZE])
        {
            PRINTF("Data error in address 0x%x, the value in memory is 0x%x\r\n", i, *pVal);
            err++;
        }
    }
    if (err == 0)
    {
        PRINTF("All data written is correct!\r\n");
    }
}
