/*
 * sharedmem.c
 *
 *  Created on: 02.06.2015
 *      Author: AllanTengg, allan.tengg@v2c2.at
 */

#include <string.h>
#include <machine/intrinsics.h>
#include <machine/wdtcon.h>
#include "tc_inc_path.h"
#include TC_INCLUDE(TCPATH/Ifx_reg.h)
#include "interrupts.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "semphr.h"
#include "task.h"
#include "sharedmem.h"

//***************************************************************************************************
// SHAREDFIFO is used to describe a FIFO ring-buffer for incoming messages.

typedef struct
{
	volatile unsigned short fifoInitialized;   //0xcafe indicates an initialized FIFO
	volatile unsigned short readIdx;           //the current position of the read pointer
	volatile unsigned short writeIdx;          //the current position of the write pointer
	volatile unsigned short spinlock;          //a spinlock telling what core is accessing the FIFO
	unsigned int fifomem[SHAREDFIFOWORDSIZE];  //the shared memory block
} SHAREDFIFO;

//***************************************************************************************************
// The FIFO buffers for the shared memory communication must reside best in the LMI RAM (0x90000000)
// of the AURIX. Every core gets its own FIFO for incoming messages.

#pragma section .shared
SHAREDFIFO g_sharedfifo[NUMCORES];
#pragma section

//***************************************************************************************************
// The interrupts are generated by using the general purpose interrupt service request nodes.
static const unsigned short GPSRn[6] = {SRC_ID_GPSR01, SRC_ID_GPSR11, SRC_ID_GPSR21, SRC_ID_GPSR31, SRC_ID_GPSR41, SRC_ID_GPSR51};
static volatile Ifx_INT_SRB* SRBn[6] = {&INT_SRB0, &INT_SRB1, &INT_SRB2, &INT_SRB3, &INT_SRB4, &INT_SRB5};

//***************************************************************************************************
// Besides the FIFO, every core has a queue used for synchronization between interrupt & task
static xQueueHandle Rx_Queue;

//***************************************************************************************************
// 'SharedMemIntHandler' is the interrupt handler for the shared memory communication.
//  The procedure is the following:
//  * the sender core puts the data into the FIFO buffer
//  * the sender core triggers the corresponding interrupt in the receiver core
//  * the receiver core branches to the interrupt handler
//  * the interrupt handler wakes any waiting task in the receiver core (by using a queue)

void SharedMemIntHandler(int iArg)
{
    unsigned int val;
    portBASE_TYPE taskWoken = pdFALSE;
	unsigned short cpu_id = _mfcr(CPU_CORE_ID) & 0x0f;

	// The value is actually never used (except debugging)
    val = g_sharedfifo[cpu_id].writeIdx;

    //Wake up any waiting task
	xQueueSendFromISR(Rx_Queue, &val, &taskWoken);

	//and force a task switch if needed
    if (taskWoken)
      taskYIELD();
}

//***************************************************************************************************
// 'SharedMemInit' is needed to set up the shared memory FIFO structures and
// register the interrupt request node and interrupt handler

void SharedMemInit()
{
	unsigned short cpu_id = _mfcr(CPU_CORE_ID) & 0x0f;

	//Initialize the FIFO for this CPU core
	memset(g_sharedfifo[cpu_id].fifomem, 0, SHAREDFIFOWORDSIZE*sizeof(unsigned int));
	g_sharedfifo[cpu_id].readIdx = 0;
	g_sharedfifo[cpu_id].writeIdx = 0;
	g_sharedfifo[cpu_id].spinlock = 0;
	g_sharedfifo[cpu_id].fifoInitialized = 0xcafe;

	//Setup a queue for incoming messages
	Rx_Queue = xQueueCreate(SHAREDFIFOMESSAGECOUNT, 4);

	//Prepare the  General Purpose Service Request <cpuid, 1>
	InterruptInstall(GPSRn[cpu_id], SharedMemIntHandler, SHAREDMEMINTPRIO, cpu_id);
}

//***************************************************************************************************
// 'SharedMemTriggerInt' can be used to generate an interrupt on the
// selected core 'dest_cpu_id'

inline void SharedMemTriggerInt(unsigned short dest_cpu_id)
{
	if (dest_cpu_id < 3)
	{
		SRBn[dest_cpu_id]->B.TRIG1 = 1; _isync();
	}
}

//***************************************************************************************************
// 'SharedMemSend' initiates a shared memory inter core message transfer by writing the
// given message ('data', 'len') into the shared FIFO memory and triggering an interrupt in the
// receiving core ('dest_cpu'id').
// A negative return value indicates a problem. A positive return value tells the number of
// data words written in the receiving core's FIFO.

short int SharedMemSend(unsigned short dest_cpu_id, unsigned int *data, unsigned short len)
{
	unsigned short cpu_id = _mfcr(CPU_CORE_ID) & 0x0f;
    short int words_in_fifo;
    short int free_words;
    unsigned short i;

	//Some simple sanity checks
	if (dest_cpu_id > 2)
	  return -1;

	if (dest_cpu_id == cpu_id)
	  return -2;

    if (len > SHAREDFIFOWORDSIZE)
      return -3;

retry:
    //Wait until the spin lock is available
	while (g_sharedfifo[dest_cpu_id].spinlock != 0);

	//Acquire the spin lock and make sure that we've really got it
	g_sharedfifo[dest_cpu_id].spinlock = (cpu_id+1);
	if (g_sharedfifo[dest_cpu_id].spinlock != (cpu_id+1))
	 goto retry;

	//Now it is safe to manipulate the FIFO buffer!

	//First look how many words are used/free
	if (g_sharedfifo[dest_cpu_id].writeIdx >= g_sharedfifo[dest_cpu_id].readIdx)
		words_in_fifo = g_sharedfifo[dest_cpu_id].writeIdx - g_sharedfifo[dest_cpu_id].readIdx;
	else
		words_in_fifo = SHAREDFIFOWORDSIZE - g_sharedfifo[dest_cpu_id].readIdx + g_sharedfifo[dest_cpu_id].writeIdx;

	free_words = SHAREDFIFOWORDSIZE - words_in_fifo;

	//Abort if the message won't fit
	if (len > free_words)
	{
		g_sharedfifo[dest_cpu_id].spinlock = 0;
		return -4;
	}

	//Copy the message in the shared ring-buffer
	//(this code could/should be optimized for performance later on)
	for (i=0; i<len; i++)
	{
		g_sharedfifo[dest_cpu_id].fifomem[g_sharedfifo[dest_cpu_id].writeIdx] = data[i];

		g_sharedfifo[dest_cpu_id].writeIdx++;

		if (g_sharedfifo[dest_cpu_id].writeIdx == SHAREDFIFOWORDSIZE)
			g_sharedfifo[dest_cpu_id].writeIdx = 0;
	}

	//Release the spin lock again
	g_sharedfifo[dest_cpu_id].spinlock = 0;

	//And notify the receiving core that new data is available
	SharedMemTriggerInt(dest_cpu_id);

	return len;
}

//***************************************************************************************************
// 'SharedMemReceive' waits for an incoming shared memory message. If there is no message within
// the specified timeout (in OS ticks), a negative value is returned. Otherwise the number of
// words successfully transferred from the FIFO to the 'data' pointer is reported.

short int SharedMemReceive(unsigned int *data, unsigned short len, unsigned int timeout)
{
    short int words_in_fifo;
    unsigned int val;
    unsigned short i;
	unsigned short cpu_id = _mfcr(CPU_CORE_ID) & 0x0f;

	//Wait & block until new data is available
	if (xQueueReceive(Rx_Queue, &val, timeout) == pdFALSE)
	  return -1;

retry:
    //Wait until the spin lock is available
	while (g_sharedfifo[cpu_id].spinlock != 0);

	//Acquire the spin lock and make sure that we've really got it
	g_sharedfifo[cpu_id].spinlock = (cpu_id+1);
	if (g_sharedfifo[cpu_id].spinlock != (cpu_id+1))
	 goto retry;

	//Now it is safe to read from the FIFO buffer!

	//First look how many words are available
	if (g_sharedfifo[cpu_id].writeIdx >= g_sharedfifo[cpu_id].readIdx)
		words_in_fifo = g_sharedfifo[cpu_id].writeIdx - g_sharedfifo[cpu_id].readIdx;
	else
		words_in_fifo = SHAREDFIFOWORDSIZE - g_sharedfifo[cpu_id].readIdx + g_sharedfifo[cpu_id].writeIdx;

	//Check if we have enough data in FIFO
	if (words_in_fifo < len)
	{
	   g_sharedfifo[cpu_id].spinlock = 0;
	   return -2;
	}

	//Copy the message from the shared ring-buffer to the given data pointer
	for (i=0; i<len; i++)
	{
		data[i] = g_sharedfifo[cpu_id].fifomem[g_sharedfifo[cpu_id].readIdx];

		g_sharedfifo[cpu_id].readIdx++;

		if (g_sharedfifo[cpu_id].readIdx == SHAREDFIFOWORDSIZE)
			g_sharedfifo[cpu_id].readIdx = 0;
	}

	//Release the spin lock again
	g_sharedfifo[cpu_id].spinlock = 0;

	return len;
}

//***************************************************************************************************
// 'SharedMemDataAvailable' tells how many words can be read from the shared memory FIFO before
// the task is blocked.

short int SharedMemDataAvailable()
{
    short int words_in_fifo;
 	unsigned short cpu_id = _mfcr(CPU_CORE_ID) & 0x0f;

retry:
    //Wait until the spin lock is available
	while (g_sharedfifo[cpu_id].spinlock != 0);

	//Acquire the spin lock and make sure that we've really got it
	g_sharedfifo[cpu_id].spinlock = (cpu_id+1);
	if (g_sharedfifo[cpu_id].spinlock != (cpu_id+1))
	 goto retry;

	//Look how many words are available
	if (g_sharedfifo[cpu_id].writeIdx >= g_sharedfifo[cpu_id].readIdx)
		words_in_fifo = g_sharedfifo[cpu_id].writeIdx - g_sharedfifo[cpu_id].readIdx;
	else
		words_in_fifo = SHAREDFIFOWORDSIZE - g_sharedfifo[cpu_id].readIdx + g_sharedfifo[cpu_id].writeIdx;

	//Release the spin lock again
	g_sharedfifo[cpu_id].spinlock = 0;

	return words_in_fifo;
}
