/** @brief Implements read, write and erase functionality of QSPI
 * @copyright 2020 Remsdaq Ltd.
 *
 * @file c4_qspi.c
 *
 *  @date:
 *  @author: Dan Smithers <dwhs@remsdaq.com>
 */

//************************************************************************
//
// MODULES USED
//
//************************************************************************

#include "c4_qspi_types.h"
#include "c4_privilege_modes.h"
#include "c4_qspi.h"   // include this first. It must not depend on any other header.
#include "bl_api.h"

#include "fsl_debug_console.h"
#include <stdbool.h>
#include <stddef.h>


extern void SysTick_DelayTicks(uint32_t n);

//************************************************************************
//
// DEFINITIONS AND MACROS
//
//************************************************************************

//************************************************************************
//
// TYPEDEFS, ENUMS, UNIONS AND STRUCTURES
//
//************************************************************************

//************************************************************************
//
// LOCAL PROTOTYPES
//
//************************************************************************

/** @brief Write a whole sector.
 *
 * The interface provides for sector erase but only page write. This
 * function writes the provided data into the sector page by page.
 *
 * @param pSect [in] Pointer to write relative to the beginning of the flash memory.
 * @param pData [in] Data to write.
 *
 * @return Number of bytes written.
 */
ptrdiff_t writeSector(uintptr_t pSect, const uint8_t *pData);

//************************************************************************
//
// LOCAL VARIABLES AND CONSTS
//
//************************************************************************

flexspi_nor_config_t sFlashConfig;
static serial_nor_config_option_t sFlashConfigOption;
bool FLASH_INITIALISED = false;

//************************************************************************
//
// EXPORTED FUNCTIONS
//
//************************************************************************

exit_code_t C4_QSPI_init(void)
{
    exit_code_t rc = C4_ERR_OK;

    if (!FLASH_INITIALISED) // this hasn't been done before.
    {
        bl_api_init();
        sFlashConfigOption.option0.U = 0xc0000007;
        C4_QSPI_getConfig(&sFlashConfig);
        status_t status = flexspi_nor_flash_init(C4_FLASH_INSTANCE, &sFlashConfig);
        if (kStatus_Success == status)
            FLASH_INITIALISED = true;
        else
        {
            rc = C4_ERR_HARDWARE;
        }
    }

    return rc;
}
/*
 * ***********************************************************************
 */
exit_code_t C4_QSPI_getConfig(flexspi_nor_config_t *pConfig)
{
    exit_code_t rc = C4_ERR_OK;

    status_t status = flexspi_nor_get_config(C4_FLASH_INSTANCE, pConfig, &sFlashConfigOption);
    if (status != kStatus_Success)
    {
        C4_PRINT("flexspi_nor_get_config returns %ld\n", status);
        rc = C4_ERR_HARDWARE;
    }
    else
    {
        C4_PRINT("Flash get configuration passed\n");
        C4_PRINT("Got NOR Flash configuration\n");
        C4_PRINT("\tpConfig->memConfig.tag 0x%08lx\n", pConfig->memConfig.tag);               //!< [0x000-0x003] Tag, fixed value 0x42464346UL
        C4_PRINT("\tpConfig->memConfig.version 0x%08lx\n", pConfig->memConfig.version);           //!< [0x004-0x007] Version,[31:24] -'V', [23:16] - Major, [15:8] - Minor, [7:0] - bugfix

        /* The fields available in flexspi_nor_config_t are in drivers/flexspi_nor_flash.h */

    }
    return rc;
}
/*
 * ***********************************************************************
 */

exit_code_t C4_QSPI_erase(uintptr_t addr, size_t len)
{
    exit_code_t rc = C4_ERR_OK;

//    C4_PRINT("%s(0x%p, %d, 0x%p)\n", __FUNCTION__, addr, len);

    if (0 == len)
    {
        // nothing to do
        return C4_ERR_OK;
    }

    if (!FLASH_INITIALISED)
    {
        C4_PRINT("%s QSPI uninitialised\n", __FUNCTION__);
        return C4_ERR_UNINITIALISED;
    }

    // test in range
    if ((FlexSPI_AMBA_BASE > addr) ||
        (FlexSPI_AMBA_BASE + sFlashConfig.memConfig.sflashA1Size <= addr + len))
    {
        C4_PRINT("%s target range (0x%p - 0x%p) outside flash range (0x%p - 0x%p)\n",
                __FUNCTION__,
                (void *)(addr),
                (void *)(addr + len),
                (void *)(FlexSPI_AMBA_BASE),
                (void *)(FlexSPI_AMBA_BASE + sFlashConfig.memConfig.sflashA1Size -1));
        return C4_ERR_RANGE;
    }

    // minimum erase is one sector so if we don't cover an exact number of sectors
    // call C4_QSPI_write with FF data.


    if ( (((addr - FlexSPI_AMBA_BASE) % C4_FLASH_SECTOR_SIZE) == 0) &&
         ((len % C4_FLASH_SECTOR_SIZE) == 0))
    {
        C4_PRINT("Erasing whole sector(s)\n");
        status_t status = flexspi_nor_flash_erase(C4_FLASH_INSTANCE, &sFlashConfig, addr, len);

        if (kStatus_Success != status)
        {
            C4_PRINT("flexspi_nor_flash_erase returns %ld\n", status);
            rc = C4_ERR_HARDWARE;
        }
    }
    else
    {
        C4_PRINT("Erasing partial sector(s)\n");
        uint8_t buf[len+1];
        memset(buf, 0xFF, len);
        buf[len] = '\0';
        C4_QSPI_write(addr, len, buf);
    }
    return rc;
}
/*
 * ***********************************************************************
 */

exit_code_t C4_QSPI_read(uintptr_t addr, size_t len, uint8_t *pData)
{
    exit_code_t rc = C4_ERR_OK;

//    C4_PRINT("%s(0x%p, %d, 0x%p)\n", __FUNCTION__, addr, len, pData);

    if (0 == len)
    {
        // nothing to do
        return C4_ERR_OK;
    }

    if (!FLASH_INITIALISED)
    {
        C4_PRINT("%s QSPI uninitialised\n", __FUNCTION__);
        return C4_ERR_UNINITIALISED;
    }

    // test in range
    if ((FlexSPI_AMBA_BASE > addr) ||
        (FlexSPI_AMBA_BASE + sFlashConfig.memConfig.sflashA1Size <= addr + len))
    {
        C4_PRINT("%s target range (0x%p - 0x%p) outside flash range (0x%p - 0x%p)\n",
                __FUNCTION__,
                (void *)(addr),
                (void *)(addr + len),
                (void *)(FlexSPI_AMBA_BASE),
                (void *)(FlexSPI_AMBA_BASE + sFlashConfig.memConfig.sflashA1Size -1));
        return C4_ERR_RANGE;
    }

    //!@brief Read data from Serial NOR
    status_t status = flexspi_nor_flash_read(C4_FLASH_INSTANCE, &sFlashConfig, (uint32_t *)pData, addr, len);
    if (kStatus_Success != status)
    {
        C4_PRINT("flexspi_nor_flash_read returns %ld\n", status);
        rc = C4_ERR_HARDWARE;
    }

    return rc;
}

/*
 * ***********************************************************************
 */

exit_code_t C4_QSPI_write(uintptr_t dstAddr, size_t len, const uint8_t *pData)
{
    exit_code_t rc = C4_ERR_OK;
//    C4_PRINT("%s(0x%p, %d, 0x%p)\n", __FUNCTION__, dstAddr, len, pData);

    if (0 == len)
    {
        // nothing to do
        return C4_ERR_OK;
    }

    if (!FLASH_INITIALISED)
    {
        C4_PRINT("%s QSPI uninitialised\n", __FUNCTION__);
        return C4_ERR_UNINITIALISED;
    }

    if ((FlexSPI_AMBA_BASE > dstAddr) ||
        (FlexSPI_AMBA_BASE + sFlashConfig.memConfig.sflashA1Size <= dstAddr + len))
    {
        C4_PRINT("%s target range (0x%p - 0x%p) outside flash range (0x%p - 0x%p)\n",
                __FUNCTION__,
                (void *)(dstAddr),
                (void *)(dstAddr + len),
                (void *)(FlexSPI_AMBA_BASE),
                (void *)(FlexSPI_AMBA_BASE + sFlashConfig.memConfig.sflashA1Size -1));
        return C4_ERR_RANGE;
    }

    uintptr_t lastAddr = dstAddr + len - 1;

    // find first sector
    const uint32_t firstSector = (dstAddr - FlexSPI_AMBA_BASE) / C4_FLASH_SECTOR_SIZE;

    // and last sector
    const uint32_t lastSector = (lastAddr - FlexSPI_AMBA_BASE) / C4_FLASH_SECTOR_SIZE;

//    C4_PRINT("start address 0x%08x first Sector %d\nend address  0x%08x last Sector %d\n",
//            dstAddr, firstSector, lastAddr, lastSector);

    // calculate the start address within the first sector
    uintptr_t sectAddr = dstAddr - FlexSPI_AMBA_BASE - firstSector * C4_FLASH_SECTOR_SIZE;
//    C4_PRINT("sectAddr = %d\n", sectAddr);

    uint32_t sector = firstSector;
    do {

        // base address of sector in flash
        uintptr_t sectorBase = sector * C4_FLASH_SECTOR_SIZE;

        // number of bytes in the sector
        size_t bytes = C4_FLASH_SECTOR_SIZE;

        // Is this a complete sector?
        if ((sectAddr == 0) || (len >= C4_FLASH_SECTOR_SIZE))
        {
            // just write sector
//            C4_PRINT("wrC4_PRINTwhole sector %d\n", sector);
            if (C4_FLASH_SECTOR_SIZE != writeSector(sectorBase, pData))
                rc = C4_ERR_FAILED;
        }
        else
        {
            // Not a complete sector, which will happen for the first and last sectors only.
            // The first sector may have an sectAddr > 0 and the last sectAddr + len < sector length.
            // They may be the same sector, consecutive sectors or have intervening complete sectors.

            // read-modify-erase-write sector
            uint8_t buf[C4_FLASH_SECTOR_SIZE];

//            C4_PRINT("reading sector %d (0x%p - 0x%p)\n",
//                      sector, sectorBase, sectorBase+C4_FLASH_SECTOR_SIZE-1);
               // read the contents of the sector.
           status_t status =  flexspi_nor_flash_read(C4_FLASH_INSTANCE, &sFlashConfig, (uint32_t *)buf, sectorBase, C4_FLASH_SECTOR_SIZE);
           if (kStatus_Success != status)
           {
               C4_PRINT("flexspi_nor_flash_read returns %ld\n", status);
               rc = C4_ERR_HARDWARE;
           }

           // calculate the number of bytes remaining in the sector
            bytes -= sectAddr;

            // If the number of bytes remaining is more than the number we want to write then
            // reduce the number of bytes.
            if (bytes > len)
                bytes = len;

//          C4_PRINT("Copying %d bytes from 0x%p \"%s\" into sector @ 0x%p\n", bytes, pData, pData, buf+sectAddr);
            // copy the new data into the sector
            memcpy(buf+sectAddr, pData, bytes);

            // write sector
//            C4_PRINT("writing sector %d\n", sector);
//            C4_PRINT("writing sector %d (0x%p - 0x%p) from 0x%p via 0x%p\n",
//                    sector, sectorBase, sectorBase+C4_FLASH_SECTOR_SIZE-1, pData, buf);
            if (C4_FLASH_SECTOR_SIZE != writeSector(sectorBase, buf))
                rc = C4_ERR_FAILED;
        }

        // reduce number of bytes remaining
        len -= bytes;
        // move data pointer forward
        pData += bytes;

        sectAddr = 0; // always start at the beginning of subsequent sectors.

        ++sector;
    } while (lastSector >= sector);
    return rc;
}

/*************************************************************************
 * Local Functions
 ************************************************************************/

ptrdiff_t writeSector(uintptr_t pSect, const uint8_t *pData)
{
    ptrdiff_t len = 0;

    // erase sector
//    C4_PRINT("LINE %d erasing sector 0x%p - 0x%p\n", __LINE__,
//            pSect, pSect+C4_FLASH_SECTOR_SIZE-1);
    status_t status = flexspi_nor_flash_erase(C4_FLASH_INSTANCE, &sFlashConfig, pSect, C4_FLASH_SECTOR_SIZE);
    if (kStatus_Success != status)
    {
        C4_PRINT("flexspi_nor_flash_erase returns %ld\n", status);
        return 0;
    }

    while (len < C4_FLASH_SECTOR_SIZE)
    {
//        C4_PRINT("LINE %d writing \"%c\" @ 0x%p len %d\n", __LINE__,
//                *pData, pSect, len);
        status = flexspi_nor_flash_page_program(C4_FLASH_INSTANCE, &sFlashConfig, pSect, (const uint32_t *)pData);
        if (kStatus_Success != status)
        {
            C4_PRINT("flexspi_nor_flash_page_program returns %ld\n", status);
            return len;
        }
        pSect += C4_FLASH_PAGE_SIZE;
        pData += C4_FLASH_PAGE_SIZE;
        len += C4_FLASH_PAGE_SIZE;
    }
   return len;
}
// EOF
