#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stddef.h>

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

#include <std.h>
#include <clk.h>
extern char genStr[];

#pragma CODE_SECTION(setupRouter, "xcode");
/*************************************************************************************
 * setupRouter
 *
 *  synopsis: Tests the validity of the trapping parameters, and sets the router's
 *            command and trap registers.  The router has a global command/status
 *    register, along with delay, reset and status registers plus two trapping register
 *    sets per DSP. Each register set has a command, match, and modulus register.
 *    Additionally, the command register for trap 0 (the primary trap) has several
 *    global flags: the data format, slink, exclusion bit and data mode flags. The
 *    function of each register/flag is:
 *    
 *    Global Command register: several bits, the only one we are interested in here
 *                             is the bit which permits the router to exert back-
 *       pressure on the EFB if it fills up. This must not be done during data taking!
 *
 *    Internal trap delay cnt: the delay in 25 ns increments before the router will
 *                             send any accumulated data frames, after the DSP has
 *       signaled that it is ready by lowering the pin 4 mask. This delay gives the
 *       DSP time to exit the DMA ISR-- otherwise the next interrupt could potentially
 *       arrive and get masked out since the ISR is not yet complete (resulting in
 *       data loss).
 *
 *    Reset register: has bits which signal the router to either reset the traps, or
 *                    to load the traps with the data inside the command, match and
 *       modulus registers, and ready the trap.
 *
 *    Status register: not used here; contains the trap status and is monitiored.
 *
 *    Trap command register:
 *
 *  author: Douglas Ferguson
 ************************************************************************************/
INT32 setupRouter(UINT32 slvNum, UINT32 permitBackPressure, UINT32 dataMode,
                  UINT32 sLink, UINT32 format, RouterTrapParams traps[2]) {
	INT32  returnCode= SUCCESS;
	UINT32 cmdRegWord, modRegWord;

	if ((traps[0].config== TRAP_CFG_IDLE)&&(traps[1].config== TRAP_CFG_IDLE)) {
		sprintf(genStr,"%s%s","At least one trap (trap 0) must be active. ",
		                  "For S-Link override this indicates trap function.\n");

		newError(&returnCode, TRAP_CONFIG_ERROR, FATAL_ERR,
	             "routerSetup", genStr, __FILE__, __LINE__);
	}

	else if ((traps[0].config== TRAP_CFG_IDLE)&&(traps[1].config!= TRAP_CFG_IDLE)) {
		newError(&returnCode, TRAP_CONFIG_ERROR, FATAL_ERR,
		         "eventTrapSetup", "If only one active trap, it must be trap 0.\n",
		         __FILE__, __LINE__);
	}

	if ((sLink)&&(  (traps[0].config!= TRAP_CFG_SLINK_EVT)
	              ||(traps[1].config!= TRAP_CFG_IDLE)     ) ) {
		sprintf(genStr,"%s%s","For S-Link override only trap 0 functions; ",
		                      "it must be set to SLINK_EVT.\n");

		newError(&returnCode, TRAP_CONFIG_ERROR, FATAL_ERR,
	             "routerSetup", genStr, __FILE__, __LINE__);
	}


	if ( (traps[0].match>255) || (traps[0].modulus>255) ||
	     (traps[0].remainder>255) ) {
		newError(&returnCode, TRAP_PARAM_ERROR, FATAL_ERR,
		         "eventTrapSetup", "Trap parameter out of bounds.\n",
		         __FILE__, __LINE__);
	}

	if ( (traps[1].match>255) || (traps[1].modulus>255) ||
	     (traps[1].remainder>255) ) {
		newError(&returnCode, TRAP_PARAM_ERROR, FATAL_ERR,
		         "eventTrapSetup", "Trap parameter out of bounds.\n",
		         __FILE__, __LINE__);
	}


	if (returnCode!= SUCCESS) return returnCode;
	
	/* ROUTER: set the router's command, match, & prescale/modulus regs. This is
	           complicated by the way the router registers are loaded: the values
	     written in are only readable once the trap is loaded, so we must build the
	     register words up & then write them in for the cmd & modulus registers. */

	/* writeRegister(RTR_TRAP_XFR_FRM_SIZE(slvNum),  WORD_W, 0, 0x100); fixed now */

	writeRegister(RTR_TRAP_INT_DELAY_CNT(slvNum),
	          RTR_TRAP_INT_DELAY_CNT_W, RTR_TRAP_INT_DELAY_CNT_O, 0xC);
	
	/* This is a global router register; however since we have no idea which slaves
	   are trapping events beyond the current one, we set it each time. It's the
	   user's responsibility to be sure it is consistent. */
	if (permitBackPressure) {
		if (atlasRunMode()) {
			sprintf(genStr,"%s%s","Router -> EFB back-pressure is NOT allowed ",
			                      "during ATLAS running!\n");
			newError(&returnCode, BACK_PRESSURE_ILLEGAL, FATAL_ERR,
			         "eventTrapSetup", genStr,
			         __FILE__, __LINE__);
			return returnCode;
		}
		else {
			writeRegister(RTR_CMND_STAT, 1, RTR_CALIB_BACK_PRES_EFB_O, 1);
		}
	}
	
	cmdRegWord= 0;
	if (dataMode)  cmdRegWord |= 1<<RTR_TRAP_DATA_MODE_O;
	if (sLink)     cmdRegWord |= 1<<RTR_TRAP_ALL_EVT_SLINK_O;
	
	/* format: TRAP_FMT_NORMAL= 0, TRAP_FMT_ERROR= 1 */
	cmdRegWord |= format<<RTR_TRAP_ERROR_FMT_O;
    
	if (traps[0].config!= TRAP_CFG_IDLE) {  /* trap 0 */
	
		if (!sLink) {

			if      (traps[0].config== TRAP_CFG_ATLAS_EVT)
				cmdRegWord |= 1<<RTR_TRAP_ATLAS_EVT_TYPE_O;
			
			else if (traps[0].config== TRAP_CFG_TIM_EVT)
				cmdRegWord |= 1<<RTR_TRAP_TIM_EVT_TYPE_O;
			
			else if (traps[0].config== TRAP_CFG_ROD_EVT)
				cmdRegWord |= 1<<RTR_TRAP_ROD_EVT_TYPE_O;
			
			if (traps[0].exclusionFlag)
				cmdRegWord |= 1<<RTR_TRAP_EXCLUSION_FLAG_O;
			
			
			writeRegister(RTR_TRAP_MATCH_0(slvNum), RTR_TRAP_MATCH_W,
			              RTR_TRAP_MATCH_O, traps[0].match);
		}

		/* set command & modulus registers & activate the trap */
		writeRegister(RTR_TRAP_CMND_0(slvNum), 7, 0, cmdRegWord);
		modRegWord  = (traps[0].modulus<<RTR_TRAP_MODULUS_O); 
		modRegWord |= (traps[0].remainder<<RTR_TRAP_REMAINDER_O); 
		writeRegister(RTR_TRAP_MOD_0(slvNum), 16, RTR_TRAP_MODULUS_O, modRegWord);

		writeRegister(RTR_TRAP_RESET(slvNum),1, RTR_TRAP0_LOAD_NEW_O, 1);
	}
	
	if ((traps[1].config!= TRAP_CFG_IDLE)&&(!sLink)) {  /* trap 1 */
	
		cmdRegWord= 0;
		if      (traps[1].config== TRAP_CFG_ATLAS_EVT)
			cmdRegWord |= 1<<RTR_TRAP_ATLAS_EVT_TYPE_O;
		
		else if (traps[1].config== TRAP_CFG_TIM_EVT)
			cmdRegWord |= 1<<RTR_TRAP_TIM_EVT_TYPE_O;
		
		else if (traps[1].config== TRAP_CFG_ROD_EVT)
			cmdRegWord |= 1<<RTR_TRAP_ROD_EVT_TYPE_O;
		
		/* for now, exclusion flag is common to both traps
		if (traps[1].exclusionFlag)
			cmdRegWord |= 1<<RTR_TRAP_EXCLUSION_FLAG_O;   */
			
		writeRegister(RTR_TRAP_MATCH_1(slvNum), RTR_TRAP_MATCH_W,
		              RTR_TRAP_MATCH_O, traps[1].match);
		
		/* set command & modulus registers & activate the trap */
		modRegWord  = (traps[1].modulus<<RTR_TRAP_MODULUS_O); 
		modRegWord |= (traps[1].remainder<<RTR_TRAP_REMAINDER_O); 
		writeRegister(RTR_TRAP_MOD_1(slvNum), 16, RTR_TRAP_MODULUS_O, modRegWord);
		writeRegister(RTR_TRAP_CMND_1(slvNum), 3, 0, cmdRegWord);

		writeRegister(RTR_TRAP_RESET(slvNum), 1, RTR_TRAP1_LOAD_NEW_O, 1);
	}
	
	return returnCode;
}