#include "LocalLinkInterface.h"
#include "HardwareMMappingDefs.h"
#include "logger.h"

#include <unistd.h>
#include <sys/mman.h>
#include <fcntl.h>


void Local_LocalLinkInterface1(struct LocalLinkInterface* ll,unsigned int ll_fifo_badr) {
	FILE_LOG(logDEBUG1, ("Initialize PLB LL FIFOs\n"));
	ll->ll_fifo_base=0;
	ll->ll_fifo_ctrl_reg=0;
	if (Local_Init(ll,ll_fifo_badr)) {
		Local_Reset(ll);
		FILE_LOG(logDEBUG1, ("\tFIFO Status : 0x%08x\n\n\n", Local_StatusVector(ll)));
	} else FILE_LOG(logERROR, ("\tCould not map LocalLink : 0x%08x\n\n\n", ll_fifo_badr));
}


void Local_LocalLinkInterface(struct LocalLinkInterface* ll) {
	FILE_LOG(logDEBUG1, ("Initializing new memory\n"));
}



int Local_Init(struct LocalLinkInterface* ll,unsigned int ll_fifo_badr) {
	int fd;
	void *plb_ll_fifo_ptr;

	if ((fd=open("/dev/mem", O_RDWR)) < 0) {
		fprintf(stderr, "Could not open /dev/mem\n");
		return 0;
	}

	plb_ll_fifo_ptr =  mmap(0, getpagesize(), PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, fd, ll_fifo_badr);
	close(fd);

	if (plb_ll_fifo_ptr == MAP_FAILED) {
		perror ("mmap");
		return 0;
	}

	ll->ll_fifo_base = (xfs_u32) plb_ll_fifo_ptr;
	ll->ll_fifo_ctrl_reg = 0;

	return 1;
}



int Local_Reset(struct LocalLinkInterface* ll) {
	return Local_Reset1(ll,PLB_LL_FIFO_CTRL_RESET_STD);
}

int Local_Reset1(struct LocalLinkInterface* ll,unsigned int rst_mask) {
	ll->ll_fifo_ctrl_reg |= rst_mask;
	FILE_LOG(logDEBUG1, ("\tCTRL Register bits: 0x%08x\n",ll->ll_fifo_ctrl_reg));

	HWIO_xfs_out32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_CTRL,ll->ll_fifo_ctrl_reg);
	HWIO_xfs_out32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_CTRL,ll->ll_fifo_ctrl_reg);
	HWIO_xfs_out32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_CTRL,ll->ll_fifo_ctrl_reg);
	HWIO_xfs_out32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_CTRL,ll->ll_fifo_ctrl_reg);

	ll->ll_fifo_ctrl_reg &= (~rst_mask);

	HWIO_xfs_out32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_CTRL,ll->ll_fifo_ctrl_reg);
	return 1;
}



unsigned int Local_StatusVector(struct LocalLinkInterface* ll) {
	return HWIO_xfs_in32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_STATUS);
}

int Local_Write(struct LocalLinkInterface* ll,unsigned int buffer_len, void *buffer) {
	// note: buffer must be word (4 byte) aligned
	// frame_len in byte
	int vacancy=0;
	int i;
	int words_send = 0;
	int last_word;
	unsigned int *word_ptr;
	unsigned int fifo_ctrl;
	xfs_u32 status;

	if (buffer_len < 1) return -1;

	last_word = (buffer_len-1)/4;
	word_ptr = (unsigned int *)buffer;

	FILE_LOG(logDEBUG1, ("LL Write - Len: %2d - If: %X - Data: ",buffer_len, ll->ll_fifo_base));
	for (i=0; i < buffer_len/4; i++)
		FILE_LOG(logDEBUG1, ("%.8X ",*(((unsigned *) buffer)+i)));

	while (words_send <= last_word)
	{
		while (!vacancy)//wait for Fifo to be empty again
		{
			status = HWIO_xfs_in32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_STATUS);
			if ((status & PLB_LL_FIFO_STATUS_ALMOSTFULL) == 0) vacancy = 1;
			if (vacancy == 0) {
				FILE_LOG(logERROR, ("Fifo full!\n"));
			}
		}

		//Just to know: #define PLB_LL_FIFO_ALMOST_FULL_THRESHOLD_WORDS    100
		for (i=0; ((i<PLB_LL_FIFO_ALMOST_FULL_THRESHOLD_WORDS) && (words_send <= last_word)); i++)
		{
			fifo_ctrl = 0;
			if (words_send == 0)
			{
				fifo_ctrl = PLB_LL_FIFO_CTRL_LL_SOF;//announce the start of file
			}

			if (words_send == last_word)
			{
				fifo_ctrl |= (PLB_LL_FIFO_CTRL_LL_EOF | (( (buffer_len-1)<<PLB_LL_FIFO_CTRL_LL_REM_SHIFT) & PLB_LL_FIFO_CTRL_LL_REM)  );
			}
			Local_ctrl_reg_write_mask(ll,PLB_LL_FIFO_CTRL_LL_MASK,fifo_ctrl);
			HWIO_xfs_out32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_FIFO,word_ptr[words_send++]);
		}
	}
	return buffer_len;
}


int Local_Read(struct LocalLinkInterface* ll,unsigned int buffer_len, void *buffer) {
	static unsigned int buffer_ptr = 0;
	// note: buffer must be word (4 byte) aligned
	// frame_len in byte
	int len;
	unsigned int *word_ptr;
	unsigned int status;
	volatile unsigned int fifo_val;
	int sof = 0;

	FILE_LOG(logDEBUG1, ("LL Read - If: %X - Data: ",ll->ll_fifo_base));

	word_ptr = (unsigned int *)buffer;
	do
	{
		status = HWIO_xfs_in32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_STATUS);

		if (!(status & PLB_LL_FIFO_STATUS_EMPTY))
		{
			if (status & PLB_LL_FIFO_STATUS_LL_SOF)
			{
				if (buffer_ptr)
				{
					buffer_ptr = 0;
					return -1; // buffer overflow
				}
				buffer_ptr = 0;
				sof = 1;
			}

			fifo_val = HWIO_xfs_in32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_FIFO);  //read from fifo

			if ((buffer_ptr > 0) || sof)
			{
				if ( (buffer_len >> 2) > buffer_ptr)
				{
					FILE_LOG(logDEBUG1, ("%.8X ", fifo_val));
					word_ptr[buffer_ptr++] = fifo_val; //write to buffer
				}
				else
				{
					buffer_ptr = 0;
					return -2; // buffer overflow
				}

				if (status & PLB_LL_FIFO_STATUS_LL_EOF)
				{
					len = (buffer_ptr << 2) -3 + ( (status & PLB_LL_FIFO_STATUS_LL_REM)>>PLB_LL_FIFO_STATUS_LL_REM_SHIFT );
					FILE_LOG(logDEBUG1, ("Len: %d\n",len));
					buffer_ptr = 0;
					return len;
				}

			}
		}
	}
	while(!(status & PLB_LL_FIFO_STATUS_EMPTY));

	return 0;
}

int Local_ctrl_reg_write_mask(struct LocalLinkInterface* ll,unsigned int mask, unsigned int val) {
	ll->ll_fifo_ctrl_reg &= (~mask);
	ll->ll_fifo_ctrl_reg |= ( mask & val);
	HWIO_xfs_out32(ll->ll_fifo_base+4*PLB_LL_FIFO_REG_CTRL,ll->ll_fifo_ctrl_reg);
	return 1;
}


int Local_Test(struct LocalLinkInterface* ll,unsigned int buffer_len, void *buffer) {

	int len;
	unsigned int rec_buff_len = 4096;
	unsigned int rec_buffer[4097];


	Local_Write(ll,buffer_len,buffer);
	usleep(10000);

	do{
		len = Local_Read(ll,rec_buff_len,rec_buffer);
		FILE_LOG(logDEBUG1, ("receive length: %i\n",len));

		if (len > 0) {
			rec_buffer[len]=0;
			FILE_LOG(logINFO, ("%s\n", (char*) rec_buffer));
		}
	} while(len > 0);

	return 1;
}