/*
 *  Copyright (C) 2018-2022 Texas Instruments Incorporated
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *    Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 *    Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 *    Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */


#include <stdlib.h>
#include <string.h>
#include "ti_drivers_config.h"
#include "ti_drivers_open_close.h"
#include "ti_board_open_close.h"
#include <drivers/sciclient.h>
#include <drivers/bootloader/soc/bootloader_soc.h>
#include <drivers/bootloader.h>
#include <board/psram.h>
#include "psram_ospi.h"
#include "ti_board_psram_open_close.h"
#include <drivers/bootloader/bootloader_priv.h>
#include "fss_config.h"

#define BOOTLOADER_SD_APPIMAGE_FILENAME ("/sd0/app")
#define BOOTLOADER_SD_APPIMAGE_XIP_FILENAME ("/sd0/app_xip")

#define BOOTLOADER_APPIMAGE_MAX_FILE_SIZE (0x30000) /* Size of section MSRAM_2 specified in linker.cmd */
#define BOOTLOADER_APPIMAGE_XIP_MAX_FILE_SIZE (0x30000) /* Size for XIP image */

uint8_t gAppImageBuf[BOOTLOADER_APPIMAGE_MAX_FILE_SIZE] __attribute__((aligned(128), section(".bss.filebuf")));
uint8_t gAppImageXipBuf[BOOTLOADER_APPIMAGE_XIP_MAX_FILE_SIZE] __attribute__((aligned(128), section(".bss.filebuf")));

extern Psram_Config gPsramConfig[];

/* call this API to stop the booting process and spin, do that you can connect
 * debugger, load symbols and then make the 'loop' variable as 0 to continue execution
 * with debugger connected.
 */
void loop_forever(void)
{
    volatile uint32_t loop = 1;
    while(loop)
        ;
}

int32_t Bootloader_programXipImage(uint8_t *xipImageBuf, uint32_t xipImageSize)
{
    int32_t status = SystemP_SUCCESS;
    uint8_t *ptr = xipImageBuf;

    Bootloader_MetaHeaderStart mHdrStr;
    Bootloader_MetaHeaderCore mHdrCore[BOOTLOADER_MAX_INPUT_FILES];

    Psram_OspiObject *obj = NULL;
    Psram_Config *config;
    config = &gPsramConfig[CONFIG_PSRAM0];
    obj = (Psram_OspiObject*)(config->object);
    obj->ospiHandle = OSPI_getHandle(CONFIG_OSPI0);
    obj->syncModeEnable = 0x0;

    obj->dacEnable = TRUE;

    /* Step 1: Parse multicore meta header */
    memset(&mHdrCore[0], 0xFF, BOOTLOADER_MAX_INPUT_FILES * sizeof(Bootloader_MetaHeaderCore));
    memcpy(&mHdrStr, ptr, sizeof(Bootloader_MetaHeaderStart));
    ptr += sizeof(Bootloader_MetaHeaderStart);

    /* Verify magic number */
    if(mHdrStr.magicStr != BOOTLOADER_META_HDR_MAGIC_STR)
    {
        DebugP_log("ERROR: Invalid multicore image magic: 0x%08X\r\n", mHdrStr.magicStr);
        return SystemP_FAILURE;
    }

    /* Step 2: Read all core offset addresses */
    for(uint32_t i = 0; i < mHdrStr.numFiles; i++)
    {
        memcpy(&mHdrCore[i], ptr, sizeof(Bootloader_MetaHeaderCore));
        ptr += sizeof(Bootloader_MetaHeaderCore);
    }

    /* Step 3: Parse and program each core's RPRC file */
    for(uint32_t i = 0; i < mHdrStr.numFiles; i++)
    {
        if(mHdrCore[i].coreId != 0xFFFFFFFFU)
        {
            Bootloader_RprcFileHeader rprcHeader;
            Bootloader_RprcSectionHeader sectionHeader;

            /* Position pointer at RPRC file offset */
            uint8_t *rprcPtr = xipImageBuf + mHdrCore[i].imageOffset;

            /* Step 3a: Parse RPRC header */
            memcpy(&rprcHeader, rprcPtr, sizeof(Bootloader_RprcFileHeader));
            rprcPtr += sizeof(Bootloader_RprcFileHeader);

            /* Verify RPRC magic */
            if(rprcHeader.magic != BOOTLOADER_RPRC_MAGIC_NUMBER)
            {
                DebugP_log("ERROR: Invalid RPRC magic for core %d: 0x%08X\r\n",
                            mHdrCore[i].coreId, rprcHeader.magic);
                status = SystemP_FAILURE;
                break;
            }

            /* Step 3b: Loop through each section */
            for(uint32_t j = 0; j < rprcHeader.sectionCount; j++)
            {
                /* Parse section header */
                memcpy(&sectionHeader, rprcPtr, sizeof(Bootloader_RprcSectionHeader));
                rprcPtr += sizeof(Bootloader_RprcSectionHeader);

                /*
                * CRITICAL: sectionHeader.addr contains PSRAM OFFSET, not CPU address!
                * xipGen already converted CPU address (e.g., 0x60100000) to
                * PSRAM offset (e.g., 0x100000)
                */
                uint32_t psramOffset = sectionHeader.addr;
                uint32_t sectionSize = sectionHeader.size;

                /* Step 3c: Write ONLY section data to PSRAM (headers are skipped!) */
                status = Psram_write(gPsramHandle[CONFIG_PSRAM0],
                                      psramOffset,
                                      rprcPtr,      // Pointer to section data (no headers)
                                      sectionSize);

                if(status != SystemP_SUCCESS)
                {
                    DebugP_log("ERROR: Failed to write section %d to PSRAM!\r\n", j);
                    break;
                }

                /* Move pointer to next section's data */
                rprcPtr += sectionSize;
            }

            if(status != SystemP_SUCCESS)
            {
                break;
            }
        }
    }

    if(status != SystemP_SUCCESS)
    {
        DebugP_log("ERROR: XIP image programming to PSRAM failed!\r\n");
    }

    return status;
}

int main(void)
{
    int32_t status;

    Bootloader_profileReset();

    Bootloader_socWaitForFWBoot();

#ifndef DISABLE_WARM_REST_WA
    /* Warm Reset Workaround to prevent CPSW register lockup */
    if (!Bootloader_socIsMCUResetIsoEnabled())
    {
        Bootloader_socResetWorkaround();
    }
#endif

    Bootloader_profileAddProfilePoint("SYSFW init");

    if (!Bootloader_socIsMCUResetIsoEnabled())
    {
        /* Update devGrp to ALL to initialize MCU domain when reset isolation is
        not enabled */
        Sciclient_BoardCfgPrms_t boardCfgPrms_pm =
            {
                .boardConfigLow = (uint32_t)0,
                .boardConfigHigh = 0,
                .boardConfigSize = 0,
                .devGrp = DEVGRP_ALL,
            };

        status = Sciclient_boardCfgPm(&boardCfgPrms_pm);

        Sciclient_BoardCfgPrms_t boardCfgPrms_rm =
        {
            .boardConfigLow = (uint32_t)0,
            .boardConfigHigh = 0,
            .boardConfigSize = 0,
            .devGrp = DEVGRP_ALL,
        };

        status = Sciclient_boardCfgRm(&boardCfgPrms_rm);

        /* Enable MCU PLL. MCU PLL will not be enabled by DMSC when devGrp is set
        to Main in boardCfg */
        Bootloader_enableMCUPLL();
    }

    System_init();
    Bootloader_profileAddProfilePoint("System_init");
    
    Bootloader_socOpenFirewalls();

    Bootloader_socNotifyFirewallOpen();

    Drivers_open();

    /* Enable the logic for Switching to PSRAM*/
    status = i2c_io_expander(NULL);

    /* Wait for I/O expander mux and PSRAM to stabilize */
    ClockP_usleep(50000);  // 50ms delay


    Bootloader_profileAddProfilePoint("Drivers_open");

    #if 0
    DebugP_log("\r\n");
    DebugP_log("Starting SD Bootloader ... \r\n");
    #endif

    status = Board_driversOpen();

    status = Board_psramOpen();

    DebugP_assert(status == SystemP_SUCCESS);
    Bootloader_profileAddProfilePoint("Board_driversOpen");

    status = Sciclient_getVersionCheck(1);
    Bootloader_profileAddProfilePoint("Sciclient Get Version");

    /* File I/O - Load regular app image */
    if(SystemP_SUCCESS == status)
    {
        /* Open app file */
        FF_FILE *appFp = ff_fopen(BOOTLOADER_SD_APPIMAGE_FILENAME, "rb");

        /* Check if file open succeeded */
        if(appFp == NULL)
        {
            status =  SystemP_FAILURE;
            DebugP_log("Failed to open app file: %s\r\n", BOOTLOADER_SD_APPIMAGE_FILENAME);
        }
        else
        {
            /* Check file size */
            uint32_t fileSize = ff_filelength(appFp);

            if(fileSize >= BOOTLOADER_APPIMAGE_MAX_FILE_SIZE)
            {
                /* Application size more than buffer size, abort */
                status = SystemP_FAILURE;
                DebugP_log("Appimage size exceeded limit !!\r\n");
            }
            else
            {
                /* Read the file into RAM buffer */
                ff_fread(gAppImageBuf, fileSize, 1, appFp);
            }

            /* Close the file */
            ff_fclose(appFp);
        }
    }
    Bootloader_profileAddProfilePoint("File read from SD card");

    /* File I/O - Load XIP app image */
    uint32_t xipImageSize = 0;
    if(SystemP_SUCCESS == status)
    {
        /* Open XIP app file */
        FF_FILE *appXipFp = ff_fopen(BOOTLOADER_SD_APPIMAGE_XIP_FILENAME, "rb");

        /* Check if file open succeeded */
        if(appXipFp == NULL)
        {
            DebugP_log("XIP app file not found: %s (optional)\r\n", BOOTLOADER_SD_APPIMAGE_XIP_FILENAME);
            /* XIP is optional, don't fail if not present */
        }
        else
        {
            /* Check file size */
            xipImageSize = ff_filelength(appXipFp);

            if(xipImageSize >= BOOTLOADER_APPIMAGE_XIP_MAX_FILE_SIZE)
            {
                /* Application size more than buffer size, abort */
                status = SystemP_FAILURE;
                DebugP_log("XIP appimage size exceeded limit !!\r\n");
            }
            else
            {
                /* Read the file into RAM buffer */
                ff_fread(gAppImageXipBuf, xipImageSize, 1, appXipFp);
            }

            /* Close the file */
            ff_fclose(appXipFp);
        }
    }
    Bootloader_profileAddProfilePoint("XIP file read from SD card");

    /* Program XIP image to OSPI PSRAM */
    if(SystemP_SUCCESS == status && xipImageSize > 0)
    {

        status = Bootloader_programXipImage(gAppImageXipBuf,xipImageSize);

        // status = Bootloader_programXipImage(gAppImageXipBuf, xipImageSize, xipPsramOffset);

        if(status != SystemP_SUCCESS)
        {
            DebugP_log("Failed to program XIP image to PSRAM!\r\n");
        }
        else
        {
            /* CRITICAL: Enable OSPI DAC (Direct Access Controller) mode for memory-mapped access to PSRAM */
            Psram_OspiObject *obj = NULL;
            Psram_Config *config;

            /* Get PSRAM handle - should already be opened by Board_driversOpen() */
            config = &gPsramConfig[CONFIG_PSRAM0];
            obj = (Psram_OspiObject*)(config->object);
            obj->ospiHandle = OSPI_getHandle(CONFIG_OSPI0);
            obj->syncModeEnable = 0x0;

            const OSPI_Attrs *ospiAttrs = ((OSPI_Config *)obj->ospiHandle)->attrs;
            volatile CSL_ospi_flash_cfgRegs *pReg = (volatile CSL_ospi_flash_cfgRegs *)(ospiAttrs->baseAddr);  

            if(obj->ospiHandle != NULL)
            {
                obj->dacEnable = TRUE;
                ospi_dac_en(pReg,obj->dacEnable); //Update the mode of transfer
            }
          }
      }
    
    Bootloader_profileAddProfilePoint("XIP image programmed to PSRAM");

    if(SystemP_SUCCESS == status)
    {
        Bootloader_BootImageInfo bootImageInfo;
        Bootloader_Params bootParams;
        Bootloader_Handle bootHandle;

        Bootloader_Params_init(&bootParams);
        Bootloader_BootImageInfo_init(&bootImageInfo);

        bootParams.memArgsAppImageBaseAddr = (uintptr_t)gAppImageBuf;

        bootHandle = Bootloader_open(CONFIG_BOOTLOADER_MEM, &bootParams);
        if(bootHandle != NULL)
        {
            /* Initialize PRU Cores if applicable */
            Bootloader_Config *cfg = (Bootloader_Config *)bootHandle;
            if(TRUE == cfg->initICSSCores)
            {
                status = Bootloader_socEnableICSSCores(BOOTLOADER_ICSS_CORE_DEFAULT_FREQUENCY);
                DebugP_assert(status == SystemP_SUCCESS);
            }

            status = Bootloader_parseMultiCoreAppImage(bootHandle, &bootImageInfo);
            /* Load CPUs */
            /* Do not load M4 when MCU domain is reset isolated */

            uint32_t coreVariant = Bootloader_socGetCoreVariant();
            /*Checks the core variant(Dual/Quad) */

            if (!Bootloader_socIsMCUResetIsoEnabled())
            {
                if(status == SystemP_SUCCESS && (TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_M4FSS0_0)))
                {
                    bootImageInfo.cpuInfo[CSL_CORE_ID_M4FSS0_0].clkHz = Bootloader_socCpuGetClkDefault(CSL_CORE_ID_M4FSS0_0);
                    Bootloader_profileAddCore(CSL_CORE_ID_M4FSS0_0);
                    status = Bootloader_loadCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_M4FSS0_0]);
                }
            }
            if(status == SystemP_SUCCESS && (TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_R5FSS1_0)))
            {
                bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS1_0].clkHz = Bootloader_socCpuGetClkDefault(CSL_CORE_ID_R5FSS1_0);
                Bootloader_profileAddCore(CSL_CORE_ID_R5FSS1_0);
                status = Bootloader_loadCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS1_0]);
            }
            /*Checks the core variant(Dual/Quad) */
            if((coreVariant == BOOTLOADER_DEVICE_VARIANT_QUAD_CORE) && status == SystemP_SUCCESS && (TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_R5FSS1_1)))
            {
                bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS1_1].clkHz = Bootloader_socCpuGetClkDefault(CSL_CORE_ID_R5FSS1_1);
                Bootloader_profileAddCore(CSL_CORE_ID_R5FSS1_1);
                status = Bootloader_loadCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS1_1]);
            }

            /* Assume self boot for either of the cores of R50 cluster */
            uint32_t isSelfBoot = FALSE;
            if(TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_R5FSS0_0))
            {
                isSelfBoot = TRUE;
                Bootloader_profileAddCore(CSL_CORE_ID_R5FSS0_0);
            }
            /*Checks the core variant(Dual/Quad) */
            if((coreVariant == BOOTLOADER_DEVICE_VARIANT_QUAD_CORE) && TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_R5FSS0_1))
            {
                isSelfBoot = TRUE;
                Bootloader_profileAddCore(CSL_CORE_ID_R5FSS0_1);
            }

            /* Self cores has to be reset together, so check for both */
            if(status == SystemP_SUCCESS && (TRUE == isSelfBoot))
            {
                /* Set clocks for self cluster */
                bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS0_0].clkHz = Bootloader_socCpuGetClkDefault(CSL_CORE_ID_R5FSS0_0);
                if((Bootloader_socIsR5FSSDual(BOOTLOADER_R5FSS0)))
                {
                    bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS0_1].clkHz = Bootloader_socCpuGetClkDefault(CSL_CORE_ID_R5FSS0_1);
                }

                /* Reset self cluster, both Core0 and Core 1. Init RAMs and load the app  */
                /* Skip the image load by passing TRUE, so that image load on self core doesnt corrupt the SBLs IVT. Load the image later before the reset release of the self core  */
                status = Bootloader_loadSelfCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS0_0], TRUE);
                if((status == SystemP_SUCCESS) && (TRUE == Bootloader_socIsR5FSSDual(BOOTLOADER_R5FSS0)))
                {
                    status = Bootloader_loadSelfCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS0_1], FALSE);
                }
            }
            Bootloader_profileAddProfilePoint("CPU load");
            Bootloader_profileUpdateAppimageSize(Bootloader_getMulticoreImageSize(bootHandle));
            Bootloader_profileUpdateMediaAndClk(BOOTLOADER_MEDIA_SD, 0);

            if(status == SystemP_SUCCESS)
            {
                /* Print SBL Profiling logs to UART as other cores may use the UART for logging */
                Bootloader_profileAddProfilePoint("SBL End");
                Bootloader_profilePrintProfileLog();
                DebugP_log("Image loading done, switching to application ...\r\n");
                UART_flushTxFifo(gUartHandle[CONFIG_UART0]);
            }

            /* Run CPUs */
            /* Do not run M4 when MCU domain is reset isolated */
            if (!Bootloader_socIsMCUResetIsoEnabled())
            {
                if(status == SystemP_SUCCESS && (TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_M4FSS0_0)))
                {
                    status = Bootloader_runCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_M4FSS0_0]);
                }
            }
            if(status == SystemP_SUCCESS && (TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_R5FSS1_0)))
            {
                status = Bootloader_runCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS1_0]);
            }
            /*Checks the core variant(Dual/Quad) */
            if((Bootloader_socIsR5FSSDual(BOOTLOADER_R5FSS1)) && status == SystemP_SUCCESS && (TRUE == Bootloader_isCorePresent(bootHandle, CSL_CORE_ID_R5FSS1_1)))
            {
                status = Bootloader_runCpu(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS1_1]);
            }
            if(status == SystemP_SUCCESS)
            {
                /* Load the image on self core now */
                if( bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS0_0].rprcOffset != BOOTLOADER_INVALID_ID)
                {
                    status = Bootloader_rprcImageLoad(bootHandle, &bootImageInfo.cpuInfo[CSL_CORE_ID_R5FSS0_0]);
                }
                /* Reset self cluster, both Core0 and Core 1. Init RAMs and run the app  */
                status = Bootloader_runSelfCpu(bootHandle, &bootImageInfo);
            }

            /* it should not return here, if it does, then there was some error */
            Bootloader_close(bootHandle);
        }
    }
    if(status != SystemP_SUCCESS)
    {
        DebugP_log("Some tests have failed!!\r\n");
    }
    Drivers_close();
    System_deinit();

    return 0;
}


