/*
 * core_1_boot.c: the boot code for processor core #1.
 */

/* Xilinx includes */
#include "pvr.h"
#include "xparameters.h"
#include "mb_interface.h"

/* Kernel defines. */
#define NUM_CORES 4
#define MASTER_CPU_SYNC_MAGIC_CODE 	31628
#define SLAVE_CPU_SYNC_MAGIC_CODE 	54766
#define KERNEL_TABLE_ADDR (XPAR_KERNEL_BRAM_IF_CNTLR_BASEADDR)



typedef struct
{
    volatile long bootSync;
    volatile void *applicationThreadPtr[NUM_CORES];
} kernelTable;

volatile kernelTable *KTBL = (kernelTable *) (KERNEL_TABLE_ADDR);

unsigned int getCurrentCoreID()
{
    /* Query the USER1 bits in the PVR to determine the current core's ID. */
    pvr_t pvr_data;
    microblaze_get_pvr(&pvr_data);
    unsigned int core_id = (unsigned int) (pvr_data.pvr[0] & 0xFF);
    return core_id;
}

void enableCaches()
{
#if XPAR_MICROBLAZE_USE_ICACHE
	microblaze_enable_icache();
#endif
#if XPAR_MICROBLAZE_USE_DCACHE
	microblaze_enable_dcache();
#endif
}

void disableCaches()
{
#if XPAR_MICROBLAZE_USE_ICACHE
	microblaze_disable_icache();
#endif
#if XPAR_MICROBLAZE_USE_DCACHE
	microblaze_disable_dcache();
#endif
}

int main()
{
	void (*threadLauncher) (void);
    unsigned int core_id = getCurrentCoreID();

    enableCaches();

    //microblaze_flush_dcache();


	/* Spin until the master CPU has finished writing the */
    /* application thread pointer to the kernel table.    */
    KTBL->bootSync = SLAVE_CPU_SYNC_MAGIC_CODE;
	while (KTBL->bootSync != MASTER_CPU_SYNC_MAGIC_CODE) {};

	threadLauncher = KTBL->applicationThreadPtr[core_id];

    threadLauncher();         /* Never returns */

    /* We shall never reach here. */
    disableCaches();

    return 0;
}
