/*
 * 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

/* Kernel includes. */
#define NUM_CORES 4
#define CLOCK_RATE 1000                     /* timer has 1 ms resolution */
#define MASTER_CPU_SYNC_MAGIC_CODE 	31628
#define SLAVE_CPU_SYNC_MAGIC_CODE 	54766

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

static XIntc xIntc;
static XTmrCtr xTickTimer;
volatile kernelTable *KTBL = (kernelTable *) (KERNEL_TABLE_ADDR);
volatile unsigned long *timer_tick = (unsigned long *) (KERNEL_TABLE_ADDR+0x400);

int setupInterruptController()
{
    XStatus status;
    int xReturn = 0;

    /* initialize the interrupt controller */
    status = XIntc_Initialize(&xIntc, XPAR_XPS_INTC_1_DEVICE_ID);

    if (status == XST_SUCCESS)
    {
        /* Set the ISR to service all interrupts when a single interrupt occurs */
        XIntc_SetIntrSvcOption(xIntc.BaseAddress, XIN_SVC_ALL_ISRS_OPTION);

        /* Start the interrupt controller, note that this doesn't actually
         * enable interrupts */
        status = XIntc_Start(&xIntc, XIN_REAL_MODE);
        if (status == XST_SUCCESS)
        {
            xReturn = 1;
        }
    }
    return xReturn;
}

void enableTimerInterrupt()
{
    XIntc_Enable(&xIntc, XPAR_XPS_INTC_1_XPS_TIMER_1_INTERRUPT_INTR);
    microblaze_enable_interrupts();
}

int connectInterruptHandler()
{
    XStatus status;
    int xReturn = 0;

    /* pass the handler information to the interrupt controller */
    status =
        XIntc_Connect(&xIntc, XPAR_XPS_INTC_1_XPS_TIMER_1_INTERRUPT_INTR,
        XTmrCtr_InterruptHandler, &xTickTimer);

    if (status == XST_SUCCESS)
    {
        /* enable the connected interrupt within the interrupt controller */
        enableTimerInterrupt();
        xReturn = 1;
    }

    return xReturn;
}

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 _TimerHandler(void *CallBackRef, u8 TmrCtrNumber)
{
	volatile unsigned long *TCSR0 = (volatile unsigned long *) XPAR_XPS_TIMER_1_BASEADDR;
    unsigned long core_id = getCurrentCoreID();
    timer_tick[core_id]++;
    *TCSR0 = *TCSR0;
}

int setupTickTimer()
{
    XStatus status;
    int xReturn = 0;
    /* initialize the tick timer to periodically generate an interrupt */
    status = XTmrCtr_Initialize(&xTickTimer, XPAR_XPS_TIMER_1_DEVICE_ID);
    if (status == XST_SUCCESS)
    {
        XTmrCtr_SetOptions(&xTickTimer, 0,
            XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION |
            XTC_DOWN_COUNT_OPTION);

        /* setup the timer with a 100 Hz tick rate. */
        XTmrCtr_SetResetValue(&xTickTimer, 0,
            ((unsigned long) XPAR_XPS_TIMER_1_CLOCK_FREQ_HZ)/CLOCK_RATE);

        /* register and enable the timer handler with the interrupt controller,
         * then register our tick handler with the specific timer */
        XTmrCtr_SetHandler(&xTickTimer, &_TimerHandler, &xTickTimer);
        xReturn = connectInterruptHandler();
        if (xReturn == 1)
        {
            /* start the timer counting */
            XTmrCtr_Start(&xTickTimer, 0);
            xReturn = 1;
        }
    }

    return xReturn;
}

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
}

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
}

startApplicationThreads()
{
	unsigned int core_id;

    for (core_id = 0; core_id < NUM_CORES; core_id++)
    {
        ulYieldFlag[core_id] = 0;
        pulMainStack[NUM_CORES] = (portSTACK_TYPE *) pvPortMalloc(stack_size);
    }

    /* The ignition communication block is at the bottom of shared memory */
    cpuComBlockPtr = (cpuComBlock *) (XPAR_DDR2_SDRAM_MPMC_BASEADDR);

    /* Tell the slave cores where to find the master interrupt handler and
     * "start first task" code.
     */
    cpuComBlockPtr->deviceHandlers[0] = &XIntc_DeviceInterruptHandler;
    cpuComBlockPtr->interruptHandlerPtr = &_interrupt_handler;
    cpuComBlockPtr->startFirstTaskPtr = &vPortStartFirstTask;
    cpuComBlockPtr->initMagic = MASTER_CPU_SYNC_MAGIC_CODE;

}

int main()
{
	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 */ };
}
