/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

/* FreeRTOS kernel includes. */

#include "bootloader_types.h"
#include "c4_error_codes.h"
#include "c4_qspi.h"
#include "c4_parser.h"
#include "c4_tftp.h"

#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "timers.h"

//#include <stdio.h>
//#include <stdlib.h>
//#include <stdarg.h>
//#include <ctype.h>

// LWIP includes
//#include "lwip/netif.h"
//#include "lwip/sys.h"
//#include "lwip/arch.h"
//#include "lwip/api.h"
#include "lwip/tcpip.h"
//#include "lwip/ip.h"
//#include "lwip/sockets.h"
//#include "lwip/igmp.h"
//#include "netif/etharp.h"
#if LWIP_DHCP
#include "lwip/dhcp.h"
#endif
#include "enet_ethernetif.h"

// LWIP-Contrib includes
#include "httpserver-netconn.h"

// NXP includes
#include "board.h"
//#include "peripherals.h"
#include "pin_mux.h"	// for BOARD_InitPins()
#include "clock_config.h"
//#include "MIMXRT1062.h"

/* Freescale includes. */
#include "fsl_device_registers.h"
//#include "fsl_debug_console.h"
#include "fsl_iomuxc.h"
#include "fsl_lpi2c.h"

/* Other driver headers */
//#include "at24c01d.h"
#include "sja1105.h"
#include "remsdaq_uart.h"

// CoAP includes
//#include "coap_io.h"    // for CreateCoapTask()

// sample value includes
//#include "sav_init.h"

// Includes for PTP task
//#include "ptpd.h"

/* PlatformAPI includes */
#include "platform-eeprom-header.h"
#include "platform-basetypes.h"
#include "platform-memory-map.h"

/* Platform includes */
//#include "platform_startup.h"
//#include "platform_main_freertos.h"
//#include "platform_coap.h"
//#include "platform_sampler_lpspi3_ad8688.h"
//#include "ip-macros.h"

// Fake externs used to force linker to include a QSPI header
// Once this code is running inside a bootloader framework, remove these!
extern char qspiflash_config;	// wrong type!
extern char image_vector_table;	// wrong type!
extern char boot_data;			// wrong type!

/*******************************************************************************
 * Device configuration
 ******************************************************************************/

// Set this to 1 to force the platform to write your settings to EEPROM header
#define FORCE_WRITE_TO_EEPROM	0

#define PTP_APP_MASTER_TEST     0

#define STACK_INIT_USE_TASK		0

// Your local settings (these get used if the device has no valid EEPROM header)

/// @brief Manufacturer ID (1 character)
///
/// - D - Danton
/// - S - SRX
/// - F - DFM
/// - N - Nyquist
#define configSUPPLIER_CODE 'D'
/// @brief Device code (5-digit number)
#define configDEVICE		12369
/// @brief Option code (2-digit number)
#define configOPTION        1
/// @brief Revision code (1 character)
#define configREVISION      'A'
/// @brief Year (2-digit number)
#define configYEAR          20
/// @brief week (2-digit number)
#define configWEEK          9
/// @brief Serial number (3-digit number)
#define configSERIAL        1

/// Device serial number (16-character string)
///
/// Construct the serial string from the above information.
///
/// @param[out] str Array to hold serial string.
/// @return number of characters written to string
#define configSERIAL_NUM(str)                \
    snprintf(str, PLATFORM_SERIAL_LEN+1, "%c%05d%02d%c%02d%02d%03d", \
            configSUPPLIER_CODE,             \
            configDEVICE,                    \
            configOPTION,                    \
            configREVISION,                  \
            configYEAR,                      \
            configWEEK,                      \
            configSERIAL)

#define configHOSTNAME		"Module-57920"
#define configMAC_ADDR(X)	X(0x00,0x0e,0xbf,0x33,0x00,0x00)	// Remsdaq OUI = 00:0e:bf:xx:xx:xx
#define configIP_ADDR(X)	X(169,254,154,150)	// Set this if you need a static IP
#define configNET_MASK(X)	X(255,255,255,0)	// Set this if you need a netmask
#define configGW_ADDR(X)	X(0,0,0,0)			// Set this if you need a gateway
#define configCOAP_ADDR(X)	X(224,0,1,187)		// I'm not sure how this works yet :-(
#define configUSE_DHCP		0					// 0 => false, 1 => true

#define COAP_TASK_STACK_SIZE	(30000 / 4)		// Size in 32-bit words, hence "/ 4"
#define COAP_TASK_PRIO			4


/*******************************************************************************
 * Definitions
 ******************************************************************************/

#define C4_PRINT(...) printf(__VA_ARGS__)

/** @brief Major version number. */
static const uint32_t VER_MAJ = 0;
/** @brief Minor version number. */
static const uint32_t VER_MIN = 1;
/** @brief Compilation date */
static const char * const VER_DATE = __DATE__;
/** @brief Compilation time */
static const char * const VER_TIME = "time"; // __TIME__;

/** @brief fixed offset of IVT in program image region. */
 static const ptrdiff_t IVT_offset = 0x1000L;

/// @brief Index in jump appLocation array for currant image.
enum
{
    E_CURRENT,    ///< @brief Pointer to current application binary
    E_RECOVERY,   ///< @brief pointer to recovery application binary
    E_UPDATE,     ///< @brief Pointer to update region for application binary
    E_NUM_REGIONS ///< @brief Number of assigned regions
};

static const uint32_t BUF_LEN = 1024;

/* System clock name. */
#define EXAMPLE_CLOCK_NAME		kCLOCK_CoreSysClk

#define MAIN_THREAD_NAME		"Main Thread"
#define MAIN_THREAD_STACKSIZE_BYTES		8192
#define MAIN_THREAD_PRIO		DEFAULT_THREAD_PRIO		// FIXME

/*******************************************************************************
 * Structures
 ******************************************************************************/

/** @brief structure for CPU ID base register in System Control Block (SCB). */
typedef union {
    struct {
        uint32_t revision:4;     ///< @brief CPU revision
        uint32_t partNo:12;      ///< @brief CPU part number
        uint32_t architecture:4; ///< @brief Reads as 0xF
        uint32_t variant:4;      ///< @brief Variant number
        uint32_t implementer:8;  ///< @brief Implementer code reads as 0x41 for processor implemented by ARM
    } B;
    uint32_t R;
} CPUID_t;

/*******************************************************************************
 * Prototypes
 ******************************************************************************/

#if STACK_INIT_USE_TASK
static void stack_init(void * pvParams);
#else
void stack_init(stEEPROM_Header_t *stEEPROM_Header);
#endif

void ipaddress_show(stEEPROM_Header_t *stEEPROM_Header);
void coap_join_group(stEEPROM_Header_t *stEEPROM_Header);
void main_task(void *pvParam);

void BOARD_InitLPSPI3(void);
void BOARD_InitModuleClock(void);

//static void tasks_debug(void);
void RegisterFactoryConfig(void);

//static bool ConfigHasBeenEdited( const stPLATFORM_ObjectData_t * pstObjectData );

/**
 * @brief Set the local defaults to use if the EEPROM header couldn't be read in
 *
 * @param[out] pstHeader Header to fill with defaults
 */
 void eeprom_header_setdefaults(stEEPROM_Header_t * pstHeader);
 /** @brief Set up jump and arguments.
  *
  * This determines the  stack and program counter from the values contained in the
  * application flash.
  *
  * @param[in] app_start  Beginning of application flash location.
  * @param[in] args  Aguments to be passed to the application.
  *
  */

 static void C4_jumpToUserApplication(uint32_t app_start, const char *args);

 /** @brief Jump to application
  *
  * Sets the stack pointer and program counter.
  *
  * This function does not return in normal situations.
  *
  * @param[in] user_sp      Location to set stack pointer.
  * @param[in] user_startup Location to set program counter.
  * @param[in] argv         Arguments to be passed to program.
  *
  */
 static void C4_jump(uint32_t user_sp, uint32_t user_startup, const char *argv);

 /** @brief Check whether an executable exists at the location and is valid.
  *
  * This performs a simple check that the first word is not un set (0xFFFF_FFFF
  * in NOR Flash) and performs other tests such as checking CRC.
  *
  * @param[in] image  Start of image region.
  *
  * @return Exit code
  * @retval C4_ERR_OK image passes tests.
  * @retval C4_ERR_INCOMPATIBLE_IMAGE image is not compatible.
  * @retval C4_ERR_CORRUPT_IMAGE image is corrupt or not present
  */
 static exit_code_t C4_checkExecutable(const serialNOR_t *image);

 /** @brief Print out vector table.
  *
  * @param[in] identifier  Descriptive text to help reader identify which image vector
  * table applies to.
  * @param[in] table  Vector table to diplay.
  */
 static void C4_displayVectorTable(const char *identifier, const serialNOR_t *table);

 static void C4_BootFunc(void *pParam);

 // ----------------------------------
// Global variables
// ----------------------------------

volatile char qspifake;

/*******************************************************************************
 * Variables
 ******************************************************************************/

extern uint32_t __Vectors[];
extern uint32_t __boot_hdr_start__[];
extern uint32_t __boot_hdr_end__[];
extern uint32_t *__vectors_start__;
extern uint32_t *__section_table_start;
extern unsigned int __data_section_table;
extern unsigned int __data_section_table_end;
extern unsigned int __bss_section_table;
extern unsigned int __bss_section_table_end;
extern uint32_t *__section_table_end;

// Settings struct loaded from the start of the AT24C01D I2C 1K EEPROM
//stEEPROM_Header_t stEEPROM_Header;

stPlatformCalls_t stPlatformCalls;

static struct netif fsl_netif1;
static ip4_addr_t fsl_netif1_ipaddr;
static ip4_addr_t fsl_netif1_netmask;
static ip4_addr_t fsl_netif1_gw;

typedef char tHostName[PLATFORM_HOSTNAME_MAXLEN];
MAKE_STRING(tHostName);
typedef char tSerialNo[PLATFORM_SERIAL_LEN];
MAKE_STRING(tSerialNo);

typedef stEEPROM_Header_t tFactoryConfig;

/*******************************************************************************
 * GLOBAL FUNCTIONS
 ******************************************************************************/

/** @brief Sets a delay.
 *
 * @param[in] n Delay time in ms
 */
void BL_SysTick_DelayTicks(uint32_t n)
{
    TickType_t delay = pdMS_TO_TICKS( n );
    vTaskDelay(delay);
/*    g_systickCounter = n;
    while (g_systickCounter != 0U)
    {
    } */
}

/**
 * @brief Application entry point.
 *
 * @return Doesn't actually return
 *
 * Initialises board hardware, reads EEPROM, starts the platform code etc.
 */
int main(void)
{

    /* Init board hardware. */
    BOARD_ConfigMPU();
    BOARD_InitBootPins();	// calls both BOARD_InitPins() and BOARD_InitDEBUG_UART()
    BOARD_InitBootClocks();	// calls BOARD_BootClockRUN()
    BOARD_InitModuleClock();

    (void) REMSDAQ_UART_Init(eUartIndex_UART1, 80 * 1000000, 115200);

    REMSDAQ_UART_Init(eUartIndex_UART1, 80 * 1000000, 115200);
    printf("Remsdaq bootloader V%lu.%lu %s %s\n", VER_MAJ, VER_MIN, VER_DATE, VER_TIME);
    // Update the core clock
    SystemCoreClockUpdate();

    /* Set systick reload value to generate 1ms interrupt */
    if (SysTick_Config(SystemCoreClock / 1000U))
    {
        while (1)
        {
        }
    }
    printf("main() function @ %p\n", &main);

    const CPUID_t *cpuid = (const CPUID_t *)&(SCB->CPUID);
    printf("chip CPUID 0x%08lX @ %p\n", cpuid->R, cpuid);
    printf("\tmanufacturer 0x%02X\n\tarchitecture 0x%X\n\tpart 0x%03X\n\tr%dp%d\n",
            cpuid->B.implementer,
            cpuid->B.architecture,
            cpuid->B.partNo,
            cpuid->B.variant,
            cpuid->B.revision);
    printf("Exception vector 0x%08lX\n", SCB->VTOR);


    printf("__Vectors %p\n", __Vectors);
    printf("__boot_hdr_start__ %p __boot_hdr_end__ %p\n", __boot_hdr_start__, __boot_hdr_end__);
    printf("__vectors_start__ %p\n", __vectors_start__);
    printf("__section_table_start %p __data_section_table 0x%08x __data_section_table_end 0x%08x\n",
            __section_table_start, __data_section_table, __data_section_table_end);
    printf("__bss_section_table 0x%08x __bss_section_table_end 0x%08x __section_table_end %p\n",
            __bss_section_table, __bss_section_table_end, __section_table_end);

    // Enable any other pins or muxes
    IOMUXC_EnableMode(IOMUXC_GPR, kIOMUXC_GPR_ENET1TxClkOutputDir, true);   // enet1
    IOMUXC_EnableMode(IOMUXC_GPR, kIOMUXC_GPR_ENET2TxClkOutputDir, true);   // lwIP stack

	// Initialise LPSPI1 and the SJA1105 attached to it (and the TJA1102 as well)
    initSJA1105();

    InitialiseI2CMasterClock();
    InitialiseI2C2();

    // Get module config from EEPROM
    // Settings struct loaded from the start of the AT24C01D I2C 1K EEPROM
    stEEPROM_Header_t stEEPROM_Header;
    if (eeprom_header_read(&stEEPROM_Header) == false)
    {
        printf("Note: no EEPROM config header found, so using defaults instead.\r\n");
        C4_displayError("EEPROM_Header not found.", C4_ERR_FILE_NOT_FOUND, true);
    }
    else
    {
#ifdef DEBUG
        printf("Note: EEPROM header read OK\r\n");
#endif
    }
    // need to print serial number which is an unterminated string of fixed length.
    // The %*s or %.*s printf constructs take the length from the parameter _before_ the string parameter.
    printf(" Remsdaq %d module \"%*s\", S/N = \"%.*s\"\r\n",
                (int) stEEPROM_Header.u16DeviceIndex,
                PLATFORM_HOSTNAME_MAXLEN,
                stEEPROM_Header.achHostName,
                PLATFORM_SERIAL_LEN,
                stEEPROM_Header.achSerialNumber );

    stack_init( &stEEPROM_Header);

    xTaskCreate(C4_BootFunc, "Callisto4 Bootloader", 2048, (void *)&stEEPROM_Header, 10, NULL );

    if (sys_thread_new(MAIN_THREAD_NAME, main_task, &stEEPROM_Header, MAIN_THREAD_STACKSIZE_BYTES / 4, MAIN_THREAD_PRIO) == NULL)
    {
    	printf( "Error: unable to start main thread\r\n" );
    }

    qspifake = qspiflash_config;	// Force qspiflash_config   into the binary!
    qspifake += image_vector_table;	// Force image_vector_table into the binary!
    qspifake += boot_data;			// Force boot_data          into the binary!

    vTaskStartScheduler();
    for (;;)
        ;
}

/*******************************************************************************
 * LOCAL FUNCTIONS
 ******************************************************************************/

/**
 * @brief Initialise the IP stack
 *
 * Starts LWIP, TCPIP, IGMP, etc
 */

void stack_init(stEEPROM_Header_t *stEEPROM_Header)
{
    ethernetif_config_t fsl_enet_config1 = {
        .clockName  = EXAMPLE_CLOCK_NAME,
        .macAddress =
        {
        	stEEPROM_Header->au8MACAddress[0],
        	stEEPROM_Header->au8MACAddress[1],
        	stEEPROM_Header->au8MACAddress[2],
        	stEEPROM_Header->au8MACAddress[3],
        	stEEPROM_Header->au8MACAddress[4],
        	stEEPROM_Header->au8MACAddress[5]
    	}
    };
#if LWIP_DHCP
    int err;
#endif

    // Start tcpip_thread (lwIP) task. (Also calls lwip_init() etc.)
    tcpip_init(NULL, NULL);

    if (stEEPROM_Header->u8UseDHCP == 0)
    {
		IP4_ADDR(&fsl_netif1_ipaddr,
				stEEPROM_Header->au8StaticIPAddress[0],
				stEEPROM_Header->au8StaticIPAddress[1],
				stEEPROM_Header->au8StaticIPAddress[2],
				stEEPROM_Header->au8StaticIPAddress[3]);
		IP4_ADDR(&fsl_netif1_netmask,
				stEEPROM_Header->au8StaticNetMask[0],
				stEEPROM_Header->au8StaticNetMask[1],
				stEEPROM_Header->au8StaticNetMask[2],
				stEEPROM_Header->au8StaticNetMask[3]);
		IP4_ADDR(&fsl_netif1_gw,
				stEEPROM_Header->au8StaticGateway[0],
				stEEPROM_Header->au8StaticGateway[1],
				stEEPROM_Header->au8StaticGateway[2],
				stEEPROM_Header->au8StaticGateway[3]);
    }

    netif_add(&fsl_netif1, &fsl_netif1_ipaddr, &fsl_netif1_netmask, &fsl_netif1_gw,
              &fsl_enet_config1, ethernetif1_init, tcpip_input);

#if LWIP_NETIF_HOSTNAME
    netif_set_hostname(&fsl_netif1, (const char *) stEEPROM_Header->achHostName);
#endif

#if LWIP_IPV6
    netif_create_ip6_linklocal_address(&fsl_netif1, 1);    // LWIP_IPV6
//    netif_ip6_addr_set_state((&EMAC_if), 0, IP6_ADDR_TENTATIVE);    LWIP_IPV6 Don't think I need to do this...
#endif

    netif_set_default(&fsl_netif1);

// Note: according to https://www.nongnu.org/lwip/2_1_x/sys_init.html ...
// ...we should add the following call in here: (NickP)
//    netif_set_link_up(&fsl_netif1);

    netif_set_up(&fsl_netif1);

    if (stEEPROM_Header->u8UseDHCP != 0)
    {
#if LWIP_DHCP
    	err = dhcp_start(&fsl_netif1);
    	LWIP_PLATFORM_DIAG(( "dhcp_start...........[result=%d]\r\n", err ));
#else
    	LWIP_PLATFORM_DIAG(( "Error: DHCP enabled but not compiled in\r\n" ));
#endif
    }

    printf("\r\n************************************************\r\n");
    printf(" Remsdaq %d module, S/N = %s\r\n",
				(int) stEEPROM_Header->u16DeviceIndex,
				stEEPROM_Header->achSerialNumber );
	printf(" MAC Address      : %02X:%02X:%02X:%02X:%02X:%02X\r\n",
				(int) fsl_enet_config1.macAddress[0],
				(int) fsl_enet_config1.macAddress[1],
				(int) fsl_enet_config1.macAddress[2],
				(int) fsl_enet_config1.macAddress[3],
				(int) fsl_enet_config1.macAddress[4],
				(int) fsl_enet_config1.macAddress[5]);

    if (stEEPROM_Header->u8UseDHCP == 0)
    {
    	// No DHCP, so display static IP address
    	ipaddress_show(stEEPROM_Header);
    }

#if LWIP_IPV6
    printf(" IPv6 Link Local Address : %s\r\n", ip6addr_ntoa(&(fsl_netif1.ip6_addr[0].u_addr.ip6)) );
#endif
    printf("************************************************\r\n");

#if STACK_INIT_USE_TASK
	vTaskDelete(NULL);
#endif
}

/**
 * Display the current IP address, subnet mask, and gateway address
 */
void ipaddress_show(stEEPROM_Header_t *stEEPROM_Header)
{
    printf(" IPv4 Address     : %u.%u.%u.%u\r\n",
				(unsigned int)((u8_t *)&fsl_netif1_ipaddr)[0],
				(unsigned int)((u8_t *)&fsl_netif1_ipaddr)[1],
				(unsigned int)((u8_t *)&fsl_netif1_ipaddr)[2],
				(unsigned int)((u8_t *)&fsl_netif1_ipaddr)[3]);
	printf(" IPv4 Subnet mask : %u.%u.%u.%u\r\n",
				(unsigned int)((u8_t *)&fsl_netif1_netmask)[0],
				(unsigned int)((u8_t *)&fsl_netif1_netmask)[1],
				(unsigned int)((u8_t *)&fsl_netif1_netmask)[2],
				(unsigned int)((u8_t *)&fsl_netif1_netmask)[3]);
    printf(" IPv4 Gateway     : %u.%u.%u.%u\r\n",
				(unsigned int)((u8_t *)&fsl_netif1_gw)[0],
				(unsigned int)((u8_t *)&fsl_netif1_gw)[1],
				(unsigned int)((u8_t *)&fsl_netif1_gw)[2],
				(unsigned int)((u8_t *)&fsl_netif1_gw)[3]);
}

/**
 * @param[in] pvParam Task parameter pointer (not used)
 *
 * Low priority (i.e. unimportant) background task.
 */
void main_task(void *pvParam)
{
    stEEPROM_Header_t *stEEPROM_Header = (stEEPROM_Header_t *)pvParam;

	bool bHasIPAddr = (stEEPROM_Header->u8UseDHCP == 0);
	int iCount;

	if (pvParam) { }			// Unused parameter

	vTaskDelay( 250 );			// Sleep for 250 msec

	for (iCount = 0; ; iCount++)
	{
		vTaskDelay( 250 );			// Sleep for 250 msec
#if 0
		// Note: according to https://www.nongnu.org/lwip/2_1_x/sys_init.html ...
		// ...we should periodically call sys_check_timeouts() to keep TCPIP/LWIP happy
		// However, Jeremy says the tcpip task already calls this - CHECK THIS!
		sys_check_timeouts();
#endif

//		tasks_debug();

		if (bHasIPAddr == false)
		{
#if LWIP_DHCP
			// Has DHCP supplied us an address yet?
			if (dhcp_supplied_address(&fsl_netif1))
			{
				bHasIPAddr = true;
		    	// Yes, DHCP has supplied an IP address etc, so display dynamic IP address
				ipaddress_show(stEEPROM_Header);
			}
#else
			bHasIPAddr = true;
#endif
		}
	}
}

/**
 * Set up the Ethernet PLL
 *
 * FIXME: shouldn't this live in the NXP clock config setup instead?
 */
void BOARD_InitModuleClock(void)
{
    const clock_enet_pll_config_t config = {.enableClkOutput = true, .enableClkOutput25M = true, .loopDivider = 1, .enableClkOutput1 = true, .loopDivider1 = 1};
    CLOCK_InitEnetPll(&config);
}

static void C4_BootFunc(void *pParams)
{
    stEEPROM_Header_t *stEEPROM_Header = (stEEPROM_Header_t *)pParams;

    exit_code_t rc = C4_ERR_OK;

#ifdef TEST_ASM
    asm_test();
#endif


#ifdef TEST_EXCEP
    excep_test();
#endif

#ifdef TEST_QSPI
    // call test before C4_QSPI_init().
    rc = QSPI_test();
    C4_displayError("QSPI test", rc, true);
#endif

#ifdef TEST_WATCHDOG
    bl_api_init();
    rc = watchdog_test();
    C4_displayError("Watchdog test", rc, true);
#else
//    c4_initWatchdog(C4_WDOG1, 1275, 20);
 //   c4_getResetHistory();
# ifdef TEST_WATCHDOG_IF
    rc = watchdog_if_test(C4_WDOG1);
    C4_displayError("Watchdog interface test", rc, true);
# endif
#endif

#ifdef TEST_SVC
    rc = SVC_test();
    C4_displayError("SVC test", rc, true);
#endif

#ifdef TEST_PRIVILEGE
    rc = TestUnprivileged();
    C4_displayError("", rc, false);
#endif

    // Make sure that the QSPI interface is initialised
    rc = C4_QSPI_init();
    C4_displayError("", rc, true);

    const char *const config_addr = (const char *const) QSPI_BOOTLOADERx_CONFIG;

    char args[BUF_LEN];
    const serialNOR_t *appLocation[E_NUM_REGIONS] = {0, 0, 0};

#if 1

#define C4_DO_PRINT(x) printf("%s = 0X%08X\n", #x, x)

    C4_DO_PRINT(QSPI_BOOTLOADERx_BINARY);
    C4_DO_PRINT(QSPI_BOOTLOADERx_CONFIG);
    C4_DO_PRINT(QSPI_BOOTLOADERy_BINARY);
    C4_DO_PRINT(QSPI_BOOTLOADERy_CONFIG);

   C4_DO_PRINT(QSPI_PLATFORMx_BINARY);
   C4_DO_PRINT(QSPI_PLATFORMx_CONFIG);
   C4_DO_PRINT(QSPI_PLATFORMy_BINARY);
   C4_DO_PRINT(QSPI_PLATFORMy_CONFIG);
   C4_DO_PRINT(QSPI_PLATFORMz_BINARY);
   C4_DO_PRINT(QSPI_PLATFORMz_CONFIG);

   C4_DO_PRINT(QSPI_PLUGIN1x_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN1x_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN1y_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN1y_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN1z_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN1z_CONFIG);

   C4_DO_PRINT(QSPI_PLUGIN2x_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN2x_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN2y_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN2y_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN2z_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN2z_CONFIG);

   C4_DO_PRINT(QSPI_PLUGIN3x_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN3x_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN3y_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN3y_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN3z_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN3z_CONFIG);

   C4_DO_PRINT(QSPI_PLUGIN4x_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN4x_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN4y_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN4y_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN4z_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN4z_CONFIG);

   C4_DO_PRINT(QSPI_PLUGIN5x_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN5x_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN5y_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN5y_CONFIG);
   C4_DO_PRINT(QSPI_PLUGIN5z_BINARY);
   C4_DO_PRINT(QSPI_PLUGIN5z_CONFIG);
#undef C4_DO_PRINT
#endif

    do
    {
        if (C4_ERR_OK == (rc = GetConfig(config_addr, BUF_LEN, C4_BOOT_CONFIG_PARSER_CURRENT, args)))

        {
            sscanf(args, "%p", &appLocation[E_CURRENT]);
            C4_PRINT("Got current location %p\n", appLocation[E_CURRENT]);
        }
        else if (C4_ERR_FILE_NOT_FOUND == rc)
        {
            C4_PRINT("Missing config @%p %d\n", config_addr, rc);
            char file[1024];
            rc = c4_tftpDownload(stEEPROM_Header->au8StaticIPAddress,
                                 "CallistoBootloader_data.txt",
                                 (char *)config_addr,
                                 QSPI_BOOTLOADER_CONFIG_SIZE,
                                 E_TFTP_MODE_TEXT);
            /* Need to force loop to use downloaded config second time. */
        }
        else
        {
            C4_PRINT("Bad config @%p %d\n", config_addr, rc);
        }
        C4_displayError("", rc, true);
    } while (C4_ERR_OK != rc);

    if (C4_ERR_OK == (rc = GetConfig(config_addr, BUF_LEN, C4_BOOT_CONFIG_PARSER_RECOVERY, args)))
    {
        sscanf(args, "%p", &appLocation[E_RECOVERY]);
        C4_PRINT("Got recovery location %p\n", appLocation[E_RECOVERY]);
    }
    else
    {
        C4_PRINT("Bad config @%p %d\n", config_addr, rc);
    }

    C4_displayError("", rc, true);

    if (C4_ERR_OK == (rc = GetConfig(config_addr, BUF_LEN, C4_BOOT_CONFIG_PARSER_UPDATE, args)))
    {
        sscanf(args, "%p", &appLocation[E_UPDATE]);
        C4_PRINT("Got recovery appLocation %p\n", appLocation[E_UPDATE]);
    }
    else
    {
        C4_PRINT("Bad config @%p %d\n", config_addr, rc);
    }

    C4_displayError("", rc, true);


    if (C4_ERR_OK == (rc = GetConfig(config_addr, BUF_LEN, C4_BOOT_CONFIG_PARSER_ARGS, args)))
    {
        C4_PRINT("Got args \"%s\"\n", args);
    }
    else
    {
        C4_PRINT("Bad config @%p %d\n", config_addr, rc);
    }

    C4_displayError("", rc, true);

    if (C4_ERR_OK == (rc = C4_checkExecutable(appLocation[E_CURRENT])))
    {
        char newval[11] = {'\0'};
        sprintf(newval,"%p", appLocation[E_CURRENT]);
        rc = SetConfig(config_addr, BUF_LEN, C4_BOOT_CONFIG_PARSER_FAULTY, newval, E_REMOVE);

       // For test purposes toggle locations:
        if (appLocation[E_RECOVERY]){
            sprintf(newval,"%p", appLocation[E_RECOVERY]);
            rc = SetConfig(config_addr,
                           BUF_LEN,
                           C4_BOOT_CONFIG_PARSER_CURRENT,
                           newval,
                           E_REPLACE);
            sprintf(newval,"%p", appLocation[E_CURRENT]);
            rc = SetConfig(config_addr,
                           BUF_LEN,
                           C4_BOOT_CONFIG_PARSER_RECOVERY,
                           newval,
                           E_REPLACE);
        }

        C4_displayVectorTable("Current", *(appLocation + E_CURRENT));
        C4_PRINT("Jumping to %p\n", (uint8_t *)appLocation[E_CURRENT]);

        C4_jumpToUserApplication((uint32_t)appLocation[E_CURRENT], args);
    }
    else
    {
        C4_displayError("Current image bad", rc, false);
        char newval[11] = {'\0'};
        sprintf(newval,"%p", appLocation[E_CURRENT]);
        rc = SetConfig(config_addr,
                BUF_LEN,
                C4_BOOT_CONFIG_PARSER_FAULTY,
                newval,
                E_EXTEND);
   }

    if (C4_ERR_OK == (rc = C4_checkExecutable(appLocation[E_RECOVERY])))
    {
        // Set recovery as current image until we decide on a mechanism
        // to mark the current faulty.
        {
            char newval[11] = {'\0'};
            sprintf(newval,"%p", appLocation[E_RECOVERY]);
            rc = SetConfig(config_addr,
                    BUF_LEN,
                    C4_BOOT_CONFIG_PARSER_CURRENT, newval, E_REPLACE);
            sprintf(newval,"%p", appLocation[E_CURRENT]);
            rc = SetConfig(config_addr, BUF_LEN, C4_BOOT_CONFIG_PARSER_RECOVERY, newval, E_REPLACE);
        }
        C4_displayVectorTable("Recovery", *(appLocation + E_RECOVERY));

        C4_PRINT("Jumping to %p\n", (uint8_t *)appLocation[E_RECOVERY]);

        C4_jumpToUserApplication((uint32_t)appLocation[E_RECOVERY], args);
    }
    else
    {
        C4_displayError("Recovery image bad", rc, false);
        char newval[11] = {'\0'};
        sprintf(newval,"%p", appLocation[E_RECOVERY]);
        rc = SetConfig(config_addr, BUF_LEN, C4_BOOT_CONFIG_PARSER_FAULTY, newval, E_EXTEND);

        rc = c4_tftpDownload(stEEPROM_Header->au8StaticIPAddress,
                "CallistoBootloader_data.txt",
                (char *)appLocation[E_UPDATE],
                QSPI_BOOTLOADER_CONFIG_SIZE,
                E_TFTP_MODE_BINARY);

    }

    while (1)
    {
        FlashLed(E_RED, 100U, 100U);
        FlashLed(E_YELLOW, 50U, 50U);
        FlashLed(E_GREEN, 50U, 50U);
    }

    // Task shouldn't return, but should't get here either.
    vTaskDelete( NULL );
}

/*
 * ============================================================================
 */


static void C4_jumpToUserApplication(uint32_t app_start, const char *args)
{
    C4_PRINT("%s (user_sp %p, user_start %p)\n", __FUNCTION__,
            (void *)(*(uint32_t *)(app_start)),
            (void *)(*((uint32_t *)(app_start+4))));

    /* Invalidate caches */
    SCB_DisableDCache();
    SCB_DisableICache();
    /* The main application will enable caches */
    __DMB();

    /* Relocate vector table */
    SCB->VTOR = app_start;

    C4_jump(*(uint32_t *)(app_start), *((uint32_t *)(app_start+4)), args);
}

/*
 * ============================================================================
 */
static void C4_jump(uint32_t user_sp, uint32_t user_startup, const char *argv)
{
    int argc = 0;
    const char *ptr = argv;
    while((ptr = strpbrk(ptr, " ")) != NULL)
    {
        ++argc;
        ++ptr;
    }

    const char *const start_log = (const char *const) QSPI_BOOTLOADERy_CONFIG;

    // Read HPRTCMR and HPRTCLR - 15 bits and 32 bits respectively
    // Combined 47 bits max value 140,737,488,355,327
    // The exact maximum time in seconds depends on the value set in HPCR and the tick rate.
    SNVS_Type *pSNVS = SNVS;

    uint32_t seconds = 0, tmp = 0;
    do
    {
        // keep reading until consecutive reads match
        seconds = tmp;
        tmp = ((pSNVS->HPRTCMR << 17) | (pSNVS->HPRTCLR >> 15));
    } while (seconds != tmp);

    char str[32] = "\0";
    sprintf(str, "START @ %ld\n", seconds);
    uintptr_t dst = strlen(start_log);

    C4_QSPI_write((uintptr_t)(start_log+dst), (const uint8_t *)str, strlen(str));

    // Create the function call to the user application.
    // Static variables are needed since changed the stack pointer out from under the compiler
    // we need to ensure the values we are using are not stored on the previous stack
    static uint32_t s_stackPointer = 0;
    s_stackPointer = user_sp;
    static void (*farewell_bootloader)(int, const char *) = 0;
    farewell_bootloader = (void (*)(int, const char *))user_startup;

    // Set stack pointers to the application stack pointer.
    __set_MSP(s_stackPointer);
    __set_PSP(s_stackPointer);

    // Jump to the application.
    farewell_bootloader(argc, argv);
}

/*
 * ============================================================================
 */
static exit_code_t C4_checkExecutable(const serialNOR_t *image)
{
    exit_code_t rc = C4_ERR_OK;

    if (0xFFFFFFFF == image->memCfg.tag)
        rc = C4_ERR_IMAGE_CORRUPT;
    else if (0x42464346 == image->memCfg.tag)
        rc = C4_ERR_IMAGE_BOOTABLE;

    return rc;
}

/*
 * ============================================================================
 */
static void C4_displayVectorTable(const char *identifier, const serialNOR_t *table)
{
    C4_PRINT("Using %s image at %p\n", identifier, table);
    C4_PRINT("IVT_offset 0x%08x\n", IVT_offset);
    const IVT_t * const IVT = (const IVT_t *const)((uint8_t *)table + IVT_offset);
    C4_PRINT("Got IVT @%p\n", IVT);
    C4_PRINT("\theader 0x%08lx\n", IVT->header.R);
    C4_PRINT("\tentry 0x%08lx\n", IVT->entry);
    C4_PRINT("\treserved 0x%08lx\n", IVT->reserved1);
    C4_PRINT("\tdcd 0x%08lx\n", IVT->dcd);
    C4_PRINT("\tbootdata 0x%08lx\n", IVT->bootData);
    C4_PRINT("\tself 0x%08lx\n", IVT->self);
    C4_PRINT("\tcsf 0x%08lx\n", IVT->csf);
    C4_PRINT("\treserved 0x%08lx\n", IVT->reserved2);
}


/**
 * Initialise LPSPI3's pin muxes
 *
 * This can't go in the NXP config because many modules don't use LPSPI3
 */
void BOARD_InitLPSPI3(void)
{
    IOMUXC_SetPinMux(
        IOMUXC_GPIO_AD_B1_15_LPSPI3_SCK,        /* GPIO_AD_B1_15 is configured as LPSPI3_SCK */
        0U);                                    /* Software Input On Field: Input Path is determined by functionality */
    IOMUXC_SetPinMux(
        IOMUXC_GPIO_AD_B1_12_LPSPI3_PCS0,       /* GPIO_AD_B1_12 is configured as LPSPI3_PCS0 */
        0U);                                    /* Software Input On Field: Input Path is determined by functionality */
    IOMUXC_SetPinMux(
        IOMUXC_GPIO_AD_B1_14_LPSPI3_SDO,        /* GPIO_AD_B1_14 is configured as LPSPI3_SDO */
        0U);                                    /* Software Input On Field: Input Path is determined by functionality */
    IOMUXC_SetPinMux(
        IOMUXC_GPIO_AD_B1_13_LPSPI3_SDI,        /* GPIO_AD_B1_13 is configured as LPSPI3_SDI */
        0U);                                    /* Software Input On Field: Input Path is determined by functionality */

      IOMUXC_SetPinConfig(
          IOMUXC_GPIO_AD_B1_15_LPSPI3_SCK,        /* GPIO_SD_B0_00 PAD functional properties : */
          0x10B0u);                               /* Slew Rate Field: Slow Slew Rate
                                                     Drive Strength Field: R0/6
                                                     Speed Field: medium(100MHz)
                                                     Open Drain Enable Field: Open Drain Disabled
                                                     Pull / Keep Enable Field: Pull/Keeper Enabled
                                                     Pull / Keep Select Field: Keeper
                                                     Pull Up / Down Config. Field: 100K Ohm Pull Down
                                                     Hyst. Enable Field: Hysteresis Disabled */
      IOMUXC_SetPinConfig(
          IOMUXC_GPIO_AD_B1_12_LPSPI3_PCS0,       /* GPIO_SD_B0_01 PAD functional properties : */
          0x10B0u);                               /* Slew Rate Field: Slow Slew Rate
                                                     Drive Strength Field: R0/6
                                                     Speed Field: medium(100MHz)
                                                     Open Drain Enable Field: Open Drain Disabled
                                                     Pull / Keep Enable Field: Pull/Keeper Enabled
                                                     Pull / Keep Select Field: Keeper
                                                     Pull Up / Down Config. Field: 100K Ohm Pull Down
                                                     Hyst. Enable Field: Hysteresis Disabled */
      IOMUXC_SetPinConfig(
          IOMUXC_GPIO_AD_B1_14_LPSPI3_SDO,        /* GPIO_SD_B0_02 PAD functional properties : */
          0x10B0u);                               /* Slew Rate Field: Slow Slew Rate
                                                     Drive Strength Field: R0/6
                                                     Speed Field: medium(100MHz)
                                                     Open Drain Enable Field: Open Drain Disabled
                                                     Pull / Keep Enable Field: Pull/Keeper Enabled
                                                     Pull / Keep Select Field: Keeper
                                                     Pull Up / Down Config. Field: 100K Ohm Pull Down
                                                     Hyst. Enable Field: Hysteresis Disabled */
      IOMUXC_SetPinConfig(
          IOMUXC_GPIO_AD_B1_13_LPSPI3_SDI,        /* GPIO_SD_B0_03 PAD functional properties : */
          0x10B0u);                               /* Slew Rate Field: Slow Slew Rate
                                                     Drive Strength Field: R0/6
                                                     Speed Field: medium(100MHz)
                                                     Open Drain Enable Field: Open Drain Disabled
                                                     Pull / Keep Enable Field: Pull/Keeper Enabled
                                                     Pull / Keep Select Field: Keeper
                                                     Pull Up / Down Config. Field: 100K Ohm Pull Down
                                                     Hyst. Enable Field: Hysteresis Disabled */
}
