/*
 * This is a simple program to test multi-core microblaze SoC
 */

/* Xilinx includes */
#include <stdio.h>
#include "xintc.h"
#include "xtmrctr.h"
#include "pvr.h"
#include "xparameters.h"
#include "mb_interface.h"

#define KERNEL_TABLE_ADDR XPAR_KERNEL_BRAM_IF_CNTLR_BASEADDR

#include "multicore.h"

void print(char *str);

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 enable_caches()
{
#if XPAR_MICROBLAZE_USE_ICACHE
	microblaze_enable_icache();
#endif
#if XPAR_MICROBLAZE_USE_DCACHE
	microblaze_enable_dcache();
#endif
}

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

int main()
{

    /* EXTERNAL RAM INTERRUPT HANDLER BUG WORKAROUND
     * See: http://forums.xilinx.com/t5/EDK-and-Platform-Studio/
     *            MIcroblaze-running-from-external-SDRAM-and-interrupts/td-p/26685
     * */
    int x = (int)&_interrupt_handler;
    *(int*)(0x10) = 0xb0000000 | (((x) & 0xFFFF0000) >> 16);
    *(int*)(0x14) = 0xb8080000 | (((x) & 0xFFFF));

	enable_caches();

    print("Core 0: Hello World!\n\r");

    disable_caches();

    return 0;
}

void core_1_thread(void *parameter)
{
    unsigned int core_id = getCurrentCoreID();

    printf("Core %d: Hello World!\r\n", core_id);

    while (1) { /* thread never returns */ };
}

void core_2_thread(void *parameter)
{
    unsigned int core_id = getCurrentCoreID();

    printf("Core %d: Hello World!\r\n", core_id);

    while (1) { /* thread never returns */ };
}

void core_3_thread(void *parameter)
{
    unsigned int core_id = getCurrentCoreID();

    printf("Core %d: Hello World!\r\n", core_id);

    while (1) { /* thread never returns */ };
}
