/*
 * 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 startApplicationThreads(void);

int main()
{
	enableCaches();

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

    disableCaches();

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

void startApplicationThreads(void)
{
	unsigned int core_id;

    for (core_id = 0; core_id < NUM_CORES; core_id++)
    {
        timer_tick[core_id] = 0;
    }

    /* Tell the slave cores to start running its thread */
    KTBL->applicationThreadPtr[1] = core_1_thread;
    KTBL->applicationThreadPtr[2] = core_2_thread;
    KTBL->applicationThreadPtr[3] = core_3_thread;
    KTBL->bootSync = MASTER_CPU_SYNC_MAGIC_CODE;
    printf("Master: %d\r\n", KTBL->bootSync);
}
