/*************************************************************************************
 * accessFifo.c                                                   
 *
 *  synopsis: Reads/writes some number of elements from/to a FIFO on the ROD.
 *
 *   There are 3 FIFOs on the ROD: the input memory (INPUT_MEM) which feeds data to
 *   the formatter FPGAs, the debug memory (DEBUG_MEM) which traps output data from
 *   the formatter FPGAs and the event memory (EVENT_MEM) which is a derandomizing
 *   FIFO at the output of the Event Fragment Builder (EFB).  Each of these memories
 *   is also on the ROD bus and can therefore be read or written by the master DSP
 *   via the ROD Resources Interface FPGA (RRIF).
 *
 *   All 3 FIFOs have 2 48b wide banks (BANK_A and BANK_B).  In addition, EVENT_MEM
 *   has a 32b wide bank, BANK_C.  Each memory has a dedicated 32b wide bus off of
 *   the ROD bus.  Reads and writes of INPUT_MEM and DEBUG_MEM and reads from the
 *   EVENT_MEM are carried out over the corresponding bus.  Writes to EVENT_MEM are
 *   carried out over the 16b wide EFB bus.
 *
 *          INPUT_MEM and DEBUG_MEM reads and writes; EVENT_MEM reads
 *
 *   In cases where the memory bank is wider than the bus by which it is accessed
 *   (all cases except EVENT_MEM BANK_C) multiple accesses are required per data
 *   element.  The addresses by which the memories are accessed are determined by
 *     1) which memory is being accessed
 *     2) which bank of that memory is being accessed
 *     3) whether the access is a a read or write
 *     4) if multiple accesses are required per FIFO entry, whether the current
 *        access is of the high order or the low order bit
 *   To write a single entry to a 48b wide FIFO 3 steps are required.
 *     1) write the low order bits (to latches on the memory bus)
 *     2) write the high order bits (to latches on the memory bus)
 *     3) strobe the contents of the latches into the FIFO
 *   To read a single entry from a 48b wide FIFO 3 steps are required
 *     1) read from the FIFO into latches on the memory bus
 *     2) read the low order bits (from the latches)
 *     3) read the high order bits (from the latches)
 *   For the 32b wide EVENT_MEM BANK_C there are no high order bits.
 *
 *                             EVENT_MEM writes
 *
 *   Because the EFB bus is only 16b wide, data is moved into BANK_A or BANK_B in 3
 *   parts and BANK_C in 2 parts.  In this case the bank number is not determined by
 *   the address.  Rather, prior to writing any data to one of the EVENT_MEM banks it
 *   is necessary to
 *     1) identify the bank by writing to an internal register in the EFB
 *   Then for each entry
 *     2) write the low order bits (to an internal EFB register)
 *     3) write the middle order bits (to an internal EFB register, BANK_A and B only)
 *     4) write the high order bits (to an internal EFB register)
 *     5) instruct the EFB to strobe the data into the selected FIFO by writing to an
 *        EFB control register
 *
 *                               TIM_MEM reads and writes
 *
 *   This FIFO is a byte wide FIFO in the RRIF.  The data read or to write is packed.
 *
 *  arguments:
 *    IN
 *      fifoID: INPUT_MEM, DEBUG_MEM, EVENT_MEM or TIM_MEM
 *      bank: BANK_A, BANK_B or (EVENT_MEM only) BANK_C
 *      readNotWrite: READ_OP for read, WRITE_OP for write
 *      numElements: number of fifo entries to read or write; NOTE: no packing, eg
 *                to write 20 elements to INPUT_MEM BANK_A requires 30 32b data words
 *                to write 20 elements to EVENT_MEM BANK_C requires 20 32b data words
 *                20 elements read from EVENT_MEM BANK_C will occupy 20 32b data words
 *      *dataBaseAdr: for readNotWrite = 0 source address of data for the FIFO
 *                                     = 1 destination address for data from the FIFO
 *      bytesXfrd: (reply data) Number of bytes read or written.
 *
 *  Damon Fasching, UW Madison                           fasching@wisconsin.cern.ch
 *
 *  modifications/bugs:    
 *   - Store the old value inside the ROD mux control field before setting to
 *     1 (necessary for non TIM-FIFO access) & then restore when operation
 *     is complete.                                                    24.04.03 dpsf
 *   - Turn off the formatters before reading the FIFO, and restore them
 *     afterwards.                                                     03.10.03 dpsf
 ************************************************************************************/
#include <stdio.h> 

#include <csl.h> 
#include "resources.h"
#include "registerIndices.h"

#pragma CODE_SECTION(accessFifo,                 "xcode");
#pragma CODE_SECTION(waitSerialCaptureComplete,  "xcode");
#pragma CODE_SECTION(fmtLinkToHwLink,            "xcode");
#pragma CODE_SECTION(extractRawLink,             "xcode");
#pragma CODE_SECTION(resetCaptureBuffer,         "xcode");
#pragma CODE_SECTION(inmemCaptureNext,           "xcode");

extern char genStr[];
extern TIMER_Handle timer1;  /* general purpose timer, started in main() */

/* read and write input parameters */
#define READING  readNotWrite
#define WRITING (!readNotWrite)

/* FIFO id and bank input parameters */
#define FIFO_IS_INPUT (fifoId == INPUT_MEM)
#define FIFO_IS_DEBUG (fifoId == DEBUG_MEM)
#define FIFO_IS_EVENT (fifoId == EVENT_MEM)
#define FIFO_IS_TIM   (fifoId == TIM_MEM)
#define BANK_IS_A (bank == BANK_A)
#define BANK_IS_B (bank == BANK_B)
#define BANK_IS_C (bank == BANK_C)

#define INVALID_FIFO_ID ( ! (FIFO_IS_INPUT || FIFO_IS_DEBUG ||  \
                             FIFO_IS_EVENT || FIFO_IS_TIM))

#define INVALID_FIFO_BANK (!(BANK_IS_A || BANK_IS_B || BANK_IS_C) || \
                           (BANK_IS_C && (FIFO_IS_INPUT || FIFO_IS_DEBUG)))

/* INPUT FIFO r/w, DEBUG FIFO r/w and EVENT FIFO read addresses */
#define FIFOS_ADR        ( REG_BASE + (FIFO_BASE << 2) )
#define FIFO_BITS(fifo)  (0x20*fifo)
#define BANK_BITS(bank)  (0x02*bank)
#define OP_BITS(op)      (0x08*op)
#define WRITE_OP  0
#define READ_OP   1
#define STROBE    2
#define ADR_OFFSET(fifo,bank,op)  \
              ( (FIFO_BITS(fifo) + BANK_BITS(bank) + OP_BITS(op)) << 2 )

/* 'fake' TIM FIFO address (internal RRIF FIFO which can supply TIM signals) */

#define TIM_FIFO_ADR          ( REG_BASE + ((RCF_BASE + 0x120) << 2) )

/* EVENT FIFO write addresses: data is written to registers in the EFB, then strobed
 * into the FIFO by writing to another EFB register. */
#define EVT_MEM_DATA_ADR  ( REG_BASE + ((EFB_BASE + 0x94) << 2) )
#define EVT_MEM_STR_ADR   ( REG_BASE + ((EFB_BASE + 0x97) << 2) )
#define EVT_MEM_STROBE    0x1


#define MAX_XFRS_PER_ELEMENT 3  /*  The worst case is writing to EVENT_MEM A or B, */
                                /* where a 6 byte FIFO accessed over 2 byte bus    */
                                
extern far InmemTrap inmemTrap;

/* macros to check for errors and call error handler routines */
#define NEW_ERR_X(err, level, routine, msg)                               \
  {                                                                       \
    newError(&returnCode, err, level, routine, msg, __FILE__, __LINE__);  \
    if (FATAL(returnCode)) { return returnCode; }                         \
  }

#define ADD_ERR_X(err, routine, msg)                                 \
  {                                                                  \
    if (err != SUCCESS) {                                            \
      addError(&returnCode, err, routine, msg, __FILE__, __LINE__);  \
      if (FATAL(returnCode)) { return returnCode; }                  \
    }                                                                \
  }

/* Global variables & definitions: */
#pragma DATA_SECTION(inmemBuff, "xdata");
UINT16 inmemBuff[3 * INMEM_DEPTH];

#define NCAPTBUFF    100
#define CAPTBUFFSIZE 0x400
static UINT32 inmemCaptureBuffer[CAPTBUFFSIZE * NCAPTBUFF];
#pragma DATA_SECTION(inmemCaptureBuffer, "xdata");

/************************************************************************************/
INT32 accessFifo(UINT32 fifoId, UINT32 bank, UINT32 readNotWrite, UINT32 numElements,
                  UINT32 *dataBaseAdr, UINT32 *bytesXfrd) {
	INT32 returnCode = SUCCESS, error;
	UINT32 fifoWidInBytes, busWidInBytes;
	UINT32 xfrSizeInBytes[MAX_XFRS_PER_ELEMENT];
	UINT32 *latchAdr, *strobeAdr;
	UINT32 elementIdx, xfrsPerElement, xfrIdx, byteIdx;
	UINT8  *dataAdr;
	UINT32 loadData, muxField;
	UINT32 *bankPtr[]= {(UINT32 *) 0x02e80000, (UINT32 *) 0x02ec0000, (UINT32 *) 0x02f00000};
	
	/* Check validity of input parameters */
	if (INVALID_FIFO_ID) {
		NEW_ERR_X(FIFO_DNE, FATAL_ERR, "accessFifo\0", "invalid fifo Id\0");
	}
	
	if (INVALID_FIFO_BANK) {
		NEW_ERR_X(FIFO_BANK_DNE, FATAL_ERR, "accessFifo\0", "invalid fifo bank\0");
	}
	
	/* Set up the EFB control registers: required only for EVENT_MEM writes */
	if (FIFO_IS_EVENT && WRITING)  {
		
		/* 1st set mode: allow FIFO access via the RODBUS */
		error = writeRegister(EVT_MEM_MODE, EVT_MEM_MODE_W,
		                      EVT_MEM_MODE_O, RODBUS_ACCESS);
		ADD_ERR_X(error, "accessFifo\0", "writeRegister\0");
		
		/* Then select the bank (A, B or C) (only needed on writes) */
		if      (BANK_IS_A) {
			error = writeRegister(EVT_MEM_CMND_STAT, EVT_MEM_SEL_W,
			                      EVT_MEM_SEL_O, A_SELECT);
		}
		else if (BANK_IS_B) {
			error = writeRegister(EVT_MEM_CMND_STAT, EVT_MEM_SEL_W,
			                      EVT_MEM_SEL_O, B_SELECT);
		}
		else {
			error = writeRegister(EVT_MEM_CMND_STAT, EVT_MEM_SEL_W,
			                      EVT_MEM_SEL_O, C_SELECT);
		}
		ADD_ERR_X(error, "accessFifo\0", "writeRegister\0");
	}
	
	/* Set up RRIF control registers: required for EVENT FIFO reads and any INPUT or
	 * DEBUG FIFO access. */
	else if ( !(FIFO_IS_TIM) ) {
		/* store the old value of the ROD mux mode field */
		error = readRegister(RRIF_CMND_1, 3, FIFO_CTRL_MUX_O, &muxField);
		ADD_ERR_X(error, "accessFifo\0", "readRegister\0");

		/* set MUX on FIFO control signals to select signal set for ROD bus access */
		error = writeRegister(RRIF_CMND_1, 3, FIFO_CTRL_MUX_O, ROD_BUS_FIFO_ACCESS);
		ADD_ERR_X(error, "accessFifo\0", "writeRegister\0");

		/* Store the old formatter configuration & turn all formatters off */
		setLinkMasks(0, SP_BOTH, LINK_CFG, STORE_MASK, MASK_SET_WORK0);
		setLinkMasks(0, SP_BOTH, LINK_CFG, INIT_MASK, MASK_SET_WORK1);
		setLinkMasks(0, SP_BOTH, LINK_CFG, SET_MASK, MASK_SET_WORK1);
	}
	
	/* Set up addr of FIFO input and output latches and strobe command registers. */
	if ( !( FIFO_IS_TIM)) {
		if (FIFO_IS_EVENT && WRITING)  {
			latchAdr  = (UINT32 *)(EVT_MEM_DATA_ADR);
			strobeAdr = (UINT32 *)(EVT_MEM_STR_ADR);
		}
		else {
			if (READING) {
				latchAdr= (UINT32 *)((FIFOS_ADR) +(ADR_OFFSET(fifoId, bank, READ_OP)));
			}
			else {
				latchAdr= (UINT32 *)((FIFOS_ADR) +(ADR_OFFSET(fifoId, bank, WRITE_OP)));
			}
			strobeAdr = (UINT32 *)((FIFOS_ADR) + (ADR_OFFSET(fifoId, bank, STROBE)));
		}
	}
	
	/* Determine the number of transfers per element and their sizes. */
	/* 1st the givens: the fifo width & the width of the bus by which it is accessed */
	if (FIFO_IS_EVENT && BANK_IS_C) fifoWidInBytes = 4;
	else                            fifoWidInBytes = 6;
	
	if (FIFO_IS_EVENT && WRITING) busWidInBytes = 2;
	else                          busWidInBytes = 4;
	
	/* Determine the number of "busWidInBytes" transfers per element and their size. */
	xfrsPerElement = fifoWidInBytes / busWidInBytes;
	
	if (xfrsPerElement <= MAX_XFRS_PER_ELEMENT) {
		for (xfrIdx = 0; xfrIdx < xfrsPerElement; ++ xfrIdx) {
			xfrSizeInBytes[xfrIdx] = busWidInBytes;
		}
	}
	else {
		NEW_ERR_X(ARRAY_INDEX_ERR, FATAL_ERR, "accessFifo\0", "array index error\0");
	}
	
	/* If there is an odd transfer ending each element, count it and set its size */
	if ((fifoWidInBytes % busWidInBytes) != 0) {
		if (xfrsPerElement < MAX_XFRS_PER_ELEMENT) {
			xfrSizeInBytes[xfrsPerElement] =
					fifoWidInBytes -(busWidInBytes*xfrsPerElement);
			++xfrsPerElement;
		}
		else {
			NEW_ERR_X(ARRAY_INDEX_ERR,FATAL_ERR,"accessFifo\0", "array index error\0");
		}
	}
	
	/* Now do the read or the write.
	 * For each FIFO entry read, a 'dummy' read of *strobeAdr strobes the data out.
	 * For each FIFO entry written,
	 *   EVENT_MEM: writing EVT_MEM_STROBE to *strobeAdr strobes the data in.
	 *   INPUT and DEBUG: any write to *strobeAdr strobes the data in.
	 * The TIM FIFO is internal to the RRIF.  It does not require a strobe. */
	*bytesXfrd = 0;
	
	if (READING) {
		if (FIFO_IS_TIM) {
			for (*bytesXfrd = 0; *bytesXfrd < numElements; ++*bytesXfrd) {
				*((UINT8 *)((UINT32)dataBaseAdr + *bytesXfrd)) = 
					*((UINT8 *)(TIM_FIFO_ADR));
			}
			*bytesXfrd = numElements;
		}
	
		else {
			for (elementIdx = 0; elementIdx < numElements; ++elementIdx) {
				loadData = *strobeAdr;            /* dummy read: toggles data strobe */
	
				for (xfrIdx = 0; xfrIdx < xfrsPerElement; ++xfrIdx) {
					loadData = *(latchAdr + xfrIdx);
					*bankPtr[xfrIdx]++= loadData;

					for(byteIdx = 0; byteIdx < xfrSizeInBytes[xfrIdx]; ++byteIdx) {
						dataAdr = (UINT8 *)((UINT32)dataBaseAdr + *bytesXfrd +byteIdx);
						*dataAdr = loadData >> (byteIdx*BYTE_W);
					}
	
					*bytesXfrd = *bytesXfrd + xfrSizeInBytes[xfrIdx];
				}
			}


		}
	} /* reading */
	
	else if (WRITING) {
	
		if (FIFO_IS_TIM) {
			for (*bytesXfrd = 0; *bytesXfrd < numElements; ++*bytesXfrd) { 
				*((UINT8 *)(TIM_FIFO_ADR)) =
					*((UINT8 *)((UINT32)dataBaseAdr + *bytesXfrd));
			}
			*bytesXfrd = numElements;
		}
	
		else {
			for (elementIdx = 0; elementIdx < numElements; ++elementIdx) {
				for (xfrIdx = 0; xfrIdx < xfrsPerElement; ++xfrIdx) {
					loadData = 0;
					for(byteIdx = 0; byteIdx < xfrSizeInBytes[xfrIdx]; ++byteIdx) {
						dataAdr = (UINT8 *)((UINT32)dataBaseAdr + *bytesXfrd +byteIdx);
						loadData += *dataAdr << (byteIdx*BYTE_W);
					}

					*(latchAdr + xfrIdx) = loadData;
					*bytesXfrd = *bytesXfrd + xfrSizeInBytes[xfrIdx];
					*bankPtr[xfrIdx]++= loadData;
				}

				*strobeAdr = EVT_MEM_STROBE;
			}
		}
	}  /* writing */
	
	if ( (!(FIFO_IS_EVENT && WRITING)) && (!(FIFO_IS_TIM)) ) {
		/* restore the old formatter cfg. and value of the ROD mux mode field */
		setLinkMasks(0, SP_BOTH, LINK_CFG, SET_MASK, MASK_SET_WORK0);

		error = writeRegister(RRIF_CMND_1, 3, FIFO_CTRL_MUX_O, muxField);
		ADD_ERR_X(error, "accessFifo\0", "writeRegister\0");
	}

	return returnCode;
}

/************************************************************************************/
INT32 waitSerialCaptureComplete(void) {
	UINT32 tf, flag;
	INT32 status;

	tf= TIMER_getCount(timer1) + 40000000; //One second timeout

	//RRIF_STATUS_1 contains a bit indicating the readback status, poll it:
	do {
		readRegister(RRIF_STATUS_1, 1, CFG_READBACK_DONE_O, &flag);
	} while(!flag && (status = (TIMER_getCount(timer1) < tf)));

	return status? 0 : -1;
}

/************************************************************************************/
UINT8 fmtLinkToHwLink(UINT8 fmtLink) {
	return (fmtLink >> 4) * 12 + (fmtLink & 0xf) + 2;
}

/************************************************************************************/
void extractRawLink(int hwLink, UINT32 *dst, int extractData) {
	int j, k, nElements= 0x8000, bankId, outerLimit= (INMEM_DEPTH>>5), innerLimit= 32;
	UINT16 *sptr, mask;
	UINT32 word, nBytes;

	if(hwLink < 48) {bankId= BANK_A; }
	else            {bankId= BANK_B;  hwLink-= 48;  }
	
	if (extractData) accessFifo(INPUT_MEM, bankId, 1, nElements, (UINT32 *) inmemBuff, &nBytes);
	sptr= inmemBuff;
	
#if 0	
	sprintf(genStr, "first few words = 0x%04x 0x%04x 0x%04x\n", sptr[0], sptr[1], sptr[2]);
	newInformation(__FILE__, __LINE__, genStr);
#endif
	
	/* Correct the pointer for the link's offset within the FIFO: the left-shift
	   divides by 16 (since the pointer is to a 16 bit unsigned short). The mask
	   will remove all but the desired link's bit: */
	sptr+= (hwLink>>4);
	mask= 1<<(hwLink & 0xf);  //Mask off all but the link in the data word.

	for(k=0; k<outerLimit; ++k) {
		/* The bits within the desired link are added to the output data word. */
		for(word= j= 0; j<innerLimit; ++j, sptr+= 3) {
			word<<= 1;
			if (*sptr & mask) word|= 1;
		}
		*dst++= word;
	}	
}

static int captureIdx= 0;
static int defaultFmtLink= 0x03; // dpsf
/************************************************************************************/
void resetCaptureBuffer() {
	captureIdx= inmemTrap.length= inmemTrap.nEvents= 0;
}

/************************************************************************************/
INT32 inmemCaptureNext(int fmtLink, void *dest, int extractData) {
	UINT32 *dst, hwLink;
	
	if   (dest == (void *) DEFAULT) dst= (UINT32 *) &inmemCaptureBuffer[captureIdx *CAPTBUFFSIZE];
	else                            dst= (UINT32 *) dest;

	if (fmtLink == DEFAULT_CAPTURE_LINK) hwLink= fmtLinkToHwLink(defaultFmtLink);
	else                                 hwLink= fmtLinkToHwLink(fmtLink);

	extractRawLink(hwLink, dst, extractData);
	inmemTrap.buffPtr= inmemCaptureBuffer;
	inmemTrap.nEvents= ++captureIdx;
	inmemTrap.length= inmemTrap.nEvents *CAPTBUFFSIZE;
	return 1;
//	return (inmemCaptureIndex == NCAPTUREBUFFERS) ? 1 : 0;
}