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

/* Xilinx includes */
#include <stdio.h>
#include "xparameters.h"
#include "xil_cache.h"

/* Kernel includes. */
#include "FreeRTOS.h"
#include "task.h"
#include "porthw.h"
#include "multicore.h"

#define APP_MUTEX     5
#define STDOUT_MUTEX  6

/* Task function prototypes */
void vSyncTestMonitor(void *pvParameters);
void vSyncTest(void *pvParameters);

/* Mutex names used by the tasks */
static const signed portBASE_TYPE SYNC_TEST_MUTEX = 1;

/* Shared variables for the sync test tasks */
static volatile portBASE_TYPE usSyncCheck[4];
static volatile portBASE_TYPE usRunCount[4];
static volatile portBASE_TYPE sharedVar = 0;
static volatile portBASE_TYPE badSync = 0;

/* Constants for the sync test tasks */
static const portBASE_TYPE SYNC_RUNS = 10000;
static const portBASE_TYPE SYNCHRONISER_UNKNOWN = 0;
static const portBASE_TYPE SYNCHRONISER_ALIVE = 1;
static const portBASE_TYPE SYNCHRONISER_FINISHED = 2;
static const portBASE_TYPE CHECK_TASK_DELAY_PERIOD = 100;
static const portBASE_TYPE NO_CHECKIN_THRESHOLD = 5000;

signed char *tname[] =
{
    (signed char *) "T1", (signed char *) "T2", (signed char *) "T3",
    (signed char *) "T4", (signed char *) "T5"
};

portBASE_TYPE task_cpu_id[4] = { 0, 0, 0, 0};

int main()
{
    int idx;

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

    /* Initialization of FreeRTOS HAL */
    xPortSetupHardware();

    /* Create all initial tasks for the SMP system */
    xTaskCreate(portNO_SPECIFIC_PROCESSOR, vSyncTestMonitor, tname[0],
        configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, NULL);

    /* Kick off four tasks that does exactly the same thing */
    for (idx = 0; idx < 4; idx++)
    {
        usSyncCheck[idx] = idx;
        xTaskCreate(portNO_SPECIFIC_PROCESSOR, vSyncTest, tname[idx+1],
            configMINIMAL_STACK_SIZE, (void *) &(usSyncCheck[idx]),
            tskIDLE_PRIORITY + 1, NULL);
    }

    /* Start the FreeRTOS scheduler itself. */
    vTaskStartScheduler();

    /* Should never get here unless there was not enough heap space to create
     * the idle and other system tasks. */

    return 0;
}

/*-----------------------------------------------------------*/

void vApplicationIdleHook( void )
{
	/* Background functions go here */
}

void vApplicationMallocFailedHook( void )
{
	/* Can be implemented if required, but not required in this
	 * environment and running this demo. */
}

void vApplicationStackOverflowHook( void )
{
	/* Can be implemented if required, but not required in this
	 * environment and running this demo. */
}

/*-----------------------------------------------------------*/

void vSyncTestMonitor(void *pvParameters)
{
    portBASE_TYPE noCheckinSince = 0;
    portBASE_TYPE currentRunCount[4] = { 0, 0, 0, 0 };
    portBASE_TYPE lastRunCount[4] = { 0, 0, 0, 0 };
    int idx = 0;

    for (;;)
    {
        currentRunCount[0] = usRunCount[0];
        currentRunCount[1] = usRunCount[1];
        currentRunCount[2] = usRunCount[2];
        currentRunCount[3] = usRunCount[3];

        for (idx = 0; idx < 4; idx++)
        {
            if ((currentRunCount[idx] - lastRunCount[idx]) >= 100)
            {
                mutex_lock(STDOUT_MUTEX, 10);
                printf("Task %d[%ld]: Still alive", idx, task_cpu_id[idx]);
                printf(" (%li runs complete).\r\n", usRunCount[idx]);
                mutex_unlock(STDOUT_MUTEX, 10);
                lastRunCount[idx] = currentRunCount[idx];
            }
        }

        if ((usSyncCheck[0] == SYNCHRONISER_UNKNOWN) ||
            (usSyncCheck[1] == SYNCHRONISER_UNKNOWN) ||
            (usSyncCheck[2] == SYNCHRONISER_UNKNOWN) ||
            (usSyncCheck[3] == SYNCHRONISER_UNKNOWN))
        {
            noCheckinSince++;
        }
        else if ((usSyncCheck[0] == SYNCHRONISER_FINISHED) &&
                 (usSyncCheck[1] == SYNCHRONISER_FINISHED) &&
                 (usSyncCheck[2] == SYNCHRONISER_FINISHED) &&
                 (usSyncCheck[3] == SYNCHRONISER_FINISHED))
        {
            noCheckinSince = 0;
            if (badSync == 0)
            {
                mutex_lock(STDOUT_MUTEX, 10);
                print("Tasks finished without deadlock ");
                print("or synchronization failure.\r\n");
                mutex_unlock(STDOUT_MUTEX, 10);
            }
            else if (badSync == 1)
            {
                mutex_lock(STDOUT_MUTEX, 10);
                print("!!! Tasks finished without deadlock, but ");
                print("there was a synchronization failure !!!\r\n");
                mutex_unlock(STDOUT_MUTEX, 10);
            }
            for (;;)
            {
                /* Sync monitor task ends here! */
            }
        }
        else if ((((usSyncCheck[0] == SYNCHRONISER_ALIVE)
                    || (usSyncCheck[0] == SYNCHRONISER_FINISHED))
                && ((usSyncCheck[1] == SYNCHRONISER_ALIVE)
                    || (usSyncCheck[0] == SYNCHRONISER_FINISHED))
                && ((usSyncCheck[2] == SYNCHRONISER_ALIVE)
                    || (usSyncCheck[0] == SYNCHRONISER_FINISHED))
                && ((usSyncCheck[3] == SYNCHRONISER_ALIVE)
                    || (usSyncCheck[0] == SYNCHRONISER_FINISHED))))
        {
            noCheckinSince = 0;
            usSyncCheck[0] = SYNCHRONISER_UNKNOWN;
            usSyncCheck[1] = SYNCHRONISER_UNKNOWN;
        }

        if (noCheckinSince >= NO_CHECKIN_THRESHOLD)
        {
            mutex_lock(STDOUT_MUTEX, 10);
            print("!!! Possible deadlock detected !!!\r\n");
            mutex_unlock(STDOUT_MUTEX, 10);
        }

        if (badSync != 0)
        {
            mutex_lock(STDOUT_MUTEX, 10);
            print("!!! Synchronization failure detected !!!\r\n");
            mutex_unlock(STDOUT_MUTEX, 10);
        }
    }
}

void vSyncTest(void *pvParameters)
{
    portBASE_TYPE counter;
    volatile portBASE_TYPE delay;
    portBASE_TYPE *checkIn = (portBASE_TYPE *)
        (((systemTaskParameters *) (pvParameters))->applicationParameters);
    int task_id = *checkIn;     // this parameter passes in the task ID

    usRunCount[task_id] = 0;    // Initialize the run count
    *checkIn = 0;               // Initialize the check-in flag

    for (;;)
    {
        if (usRunCount[task_id] == SYNC_RUNS)
        {
            for (;;)
            {
                /* Sync task ends here! */
                *checkIn = SYNCHRONISER_FINISHED;
            }
        }
        else
        {
            *checkIn = SYNCHRONISER_ALIVE;
        }

        /* ============================================================ */
        /* Begin of critical section                                    */
        /* ============================================================ */
        task_cpu_id[task_id] = xPortGetCurrentCPU();
        mutex_lock(APP_MUTEX, task_id+10);

        /* If sharedVar is not a multiple of 1000, synchronization has failed. */
        if (sharedVar % 1000 != 0)
        {
            badSync = 1;
        }

        *checkIn = SYNCHRONISER_ALIVE;

        for (counter = 0; counter < 1000; counter++)
        {
            sharedVar++;
        }

        mutex_unlock(APP_MUTEX, task_id+10);
        /* ============================================================ */
        /* End of critical section                                      */
        /* ============================================================ */

        usRunCount[task_id]++;
    }
}
