/************************************************************************************
 * rodConfiguration.c                                                        
 *
 *  synopsis: Interface to the rodConfig data structure.
 *
 *  in this file: RodConfig, rodModeCfg, maskConfigData, and moduleMaskData structs,
 *                initRodConfig, boardClockInMHz, DSPClockInMHz, atlasRunMode,
 *                numSlvsCommOn, getRodRev, slvPresent, setSlvClk, setSlaveConfig,
 *                numSlaves, slaveIsOn, setLemoLink, rodMode, setMaskConfig,
 *                switchLinkMasks, setLinkMasks.
 *
 *  Damon Fasching, UW Madison                            fasching@wisconsin.cern.ch
 *  Douglas Ferguson, UW Madison   (510) 486-5230         dpferguson@lbl.gov
 ************************************************************************************/
#include <std.h>      /* must be included 1st */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "resources.h"
#include "primParams.h"
#include "registerIndices.h"
#include "rodConfiguration.h"
#include "memoryPartitions.h"
#include "comRegDfns.h"
#include "accessRegister.h"

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

#pragma CODE_SECTION(initRodConfig,   "xcode");
#pragma CODE_SECTION(boardClockInMHz, "xcode");
#pragma CODE_SECTION(DSPClockInMHz,   "xcode");
#pragma CODE_SECTION(atlasRunMode,    "xcode");
#pragma CODE_SECTION(numSlvsCommOn,   "xcode");
#ifdef I_AM_MASTER_DSP
	#pragma CODE_SECTION(getRodRev,       "xcode");
	#pragma CODE_SECTION(slvPresent,      "xcode");
	#pragma CODE_SECTION(setSlvClk,       "xcode");
	#pragma CODE_SECTION(setSlaveConfig,  "xcode");
	#pragma CODE_SECTION(numSlaves,       "xcode");
	#pragma CODE_SECTION(slaveIsOn,       "xcode");

	#pragma CODE_SECTION(setLemoLink,     "xcode");
	#pragma CODE_SECTION(rodMode,         "xcode");
	#pragma CODE_SECTION(setMaskConfig,   "xcode");
	#pragma CODE_SECTION(switchLinkMasks, "icode");
	#pragma CODE_SECTION(setLinkMasks,    "xcode");
#endif

/* rodConfig.detector options */
#define D_SCT                        0
#define D_PXL              (D_SCT + 1)
#define D_PXL_BRL          (D_PXL + 1)
#define D_PXL_FRD      (D_PXL_BLR + 1)
#define D_BLYR         (D_PXL_FRD + 1)

#define DETECTOR_TYPE            D_SCT

/* clock speeds */
#define BOARD_CLOCK_IN_MHZ  40
#define DSP_CLOCK_IN_MHZ   (4*(BOARD_CLOCK_IN_MHZ))

RodConfig rodConfig;

#pragma DATA_SECTION(rodModeCfg, "xpdata")
#pragma DATA_ALIGN (rodModeCfg , 0x20);
RodModeCfg rodModeCfg[2];

#if defined (I_AM_MASTER_DSP)
	#pragma DATA_SECTION(maskConfigData, "xpdata")
	MaskConfigData maskConfigData[N_MASK_SETS];
#endif

#pragma DATA_SECTION(moduleMaskData, "xpdata")
#pragma DATA_SECTION(maskDataFlag,   "xpdata")
ModuleMaskData moduleMaskData[N_TOTMODULES];

//Flags which are set to true when the corresponding data has been loaded.
UINT32 maskDataFlag= FALSE;

#pragma DATA_SECTION(inmemTrap, "idata")
InmemTrap inmemTrap;

/************************************************************************************ 
 * initRodConfig
 *
 *  synopsis: Initialization routine for the rodConfig data structure.
 *
 *  modifications/bugs    
 *   - The slvDspConfig array members must be set to 'off' before configuring
 *     using setSlaveConfig. Otherwise upon resets, the structure remembers the
 *     old values (memory is not cleared), and setSlaveConfig decrements
 *     rodConfif.numSlvsCommOn if the slave DSPs had been turned on.  15.04.02 dpsf
 *   - Clear the modebits lookup-table so that upon reset all links will play
 *     out data arriving in them (the MB LUT is *not* cleared by a reset of the
 *     ROD).                                                          31.03.03 dpsf
 *   - Initialize the mask configuration storage sets, and the rod mode
 *     configuration structure.                                       12.04.03 dpsf
 ************************************************************************************/
void initRodConfig(void) {
	UINT8  slv;
//	UINT32 mt;
	
	setMem((UINT32 *) &rodConfig, SIZEOF(rodConfig), 0);
	rodConfig.detector = DETECTOR_TYPE;
	rodConfig.boardClockInMHz = BOARD_CLOCK_IN_MHZ;
	rodConfig.DSPClockInMHz = DSP_CLOCK_IN_MHZ;

	rodConfig.numSlaves = N_SDSP;
	#if defined(I_AM_MASTER_DSP)
		rodConfig.numSlvsCommOn= 0;
		for (slv= 0; slv < N_SDSP; ++slv) {
			rodConfig.slvDspConfig[slv].present=  slvPresent(slv);
			rodConfig.slvDspConfig[slv].commOnOff= SLV_DSP_COMM_OFF;
			setSlaveConfig(slv, SLV_DSP_COMM_OFF, SLV_DSP_UNCONFIGURED);
		}

		/* The 2nd modeCfg structure ([1]) is used for storage and is zeroed out here. */
		setMem((UINT32 *) &rodModeCfg, SIZEOF(rodModeCfg), 0);
		rodMode(ROD_INIT_MODE, SET_MODE, 0, 0, 0, 0, TRUE);

		setMem((UINT32 *) &maskConfigData, SIZEOF(maskConfigData), 0);
		/* Initialize the mask configuration storage sets */
		//mt= (DATA_LINK) +(COMMAND_LINK) +(LINK_CFG);
		//for (idx= 0; idx < N_MASK_SETS; ++idx)
		//	setLinkMasks(0, SP_BOTH, mt, INIT_MASK, idx, 0);

		//Turn on the VHDL global IDSP signal capability:
		writeRegister(RRIF_CMND_0, 1, SDSP_INT_ENABLE, 1);


	#elif defined(I_AM_SLAVE_DSP)
	#endif

	/* Initialize the data & cmd lines in the module mask structure to
	   0xff (data line off, off-ROD command line). These will be set
	   later in the setMaskConfig routine below. Also clear the calibration
	   coefficient structure. */
	setMem((UINT32 *) &moduleMaskData, SIZEOF(moduleMaskData), 0);
	return;
}

/************************************************************************************ 
 * boardClockInMHz
 *
 *  synopsis: Returns the board clock in MHz.
 ************************************************************************************/
UINT32 boardClockInMHz(void) {
	return rodConfig.boardClockInMHz;
}

/************************************************************************************ 
 * DSPClockInMHz
 *
 *  synopsis: Returns the DSP clock in MHz.
 ************************************************************************************/
UINT32 DSPClockInMHz(void) {
	return rodConfig.DSPClockInMHz;
}

/************************************************************************************ 
 * atlasRunMode:  returns 1 if the ROD is in ATLAS run mode.
 ************************************************************************************/
UINT32 atlasRunMode(void) {
	return (rodModeCfg[0].rodMode == DATA_TAKING_MODE);
}

/************************************************************************************
 * numSlvsCommOn
 *
 *  synopsis: Returns the number of slave DSPs configured to ON.
 ************************************************************************************/
UINT32 numSlvsCommOn() {
	return rodConfig.numSlvsCommOn;
}

#ifdef I_AM_MASTER_DSP

/************************************************************************************ 
 * getRodRev:     returns the ROD's board revision number.
 ************************************************************************************/
UINT8 getRodRev(void) {
	static UINT32 ctrlVer;
	static UINT8  rodRev, firstRevQuery= TRUE;

	if (firstRevQuery) {
		firstRevQuery= FALSE;
		readRegister(RRIF_CODE_VERSION, 16, 0, &ctrlVer);
		ctrlVer>>= 8;
		if (ctrlVer > 0xe) ctrlVer= 0xe;  //dpsf Rev. F patch: mimic Rev E.
		rodConfig.rodType= rodRev= ctrlVer;
	}

	return rodRev;
}

/************************************************************************************ 
 * setSlvClk:  Sets the SDSP's internal clock to run at the input speed (max 220 MHz)
 *             by adjusting the PLLCSR (Phase Lock Loop Control Status Register), 
 *   PLLM (PLL Multiplier control register), and PLLDIV0 (PLL controller divider
 *   register 0). Note that these can only be set in this way (via the HPI) while
 *   the SDSP is *not* running a program. Only the 6713 DSPs have these registers.
 *   Note that the PLLM & PLLDIV registers have different formats: the PLLM contains
 *   a direct multiplier value, while the DIV contains (divisor+1). 
 ************************************************************************************/
INT32 setSlvClk(UINT8 slv, UINT8 MHzDiv20) {
	INT32 returnCode= SUCCESS;
	UINT32 pllCsr, t0;
	UINT8  rodRev;

	rodRev= getRodRev();
	if (rodRev < 0xe) return returnCode;

	setSlvReg(slv, SDSP6713_PLLM,  MHzDiv20);
	sprintf(genStr, "%s%d%s%d%s",
		"SDSP #", slv, " clock set to ",MHzDiv20*20," MHz.\n");
	newInformation(__FILE__, __LINE__, genStr);

	setSlvReg(slv, SDSP6713_PLLDIV0, 0x8001); //divide by 2. Bit 15 enables it.
	setSlvReg(slv, SDSP6713_DEVCFG,  0x10);   //SDRAM uses external clock.
	setSlvReg(slv, SDSP6713_PLLCSR,  0x0);    //Clear PLL Reset bit.

	//Wait a millisecond to be absolutely safe, then while the clock stabilizes,
	//with a timeout:
	delay(10000);
	t0= TIMER_getCount(timer1);
	while (  (!((pllCsr= getSlvReg(slv, SDSP6713_PLLCSR)) & (1<<6)) )
	       &&(delta_t(t0) < PLL_TIMEOUT)
	      );

	//Now read back the PLLCSR, checking bit 6 to ensure that the clock is stable.
	pllCsr= getSlvReg(slv, SDSP6713_PLLCSR);
	if (!(pllCsr & (1<<6))) {
		sprintf(genStr, "%s%d%s", "SDSP #", slv, " has an unstable clock!\n");
		newError(&returnCode, CONFIG_ERROR, FATAL_ERR, "setSlvClk", 
		         genStr, __FILE__, __LINE__);
		return returnCode;
	}

	setSlvReg(slv, SDSP6713_PLLCSR,  0x1);    //Enable the PLL.

	//Wait a millisecond to be absolutely safe, then while the clock stabilizes,
	//with a timeout:
	delay(10000);
	t0= TIMER_getCount(timer1);
	while (  (!((pllCsr= getSlvReg(slv, SDSP6713_PLLCSR)) & (1<<6)) )
	       &&(delta_t(t0) < PLL_TIMEOUT)
	      );

	if (!(pllCsr & (1<<6))) {
		sprintf(genStr, "%s%d%s%s", "SDSP #", slv, " has an unstable clock ",
		                "after PLL enable!\n");
		newError(&returnCode, CONFIG_ERROR, FATAL_ERR, "setSlvClk", 
		         genStr, __FILE__, __LINE__);
	}
	else if (!(pllCsr & 1)) {
		sprintf(genStr, "%s%d%s",
		   "Unable to enable the Phase Lock Loop CSR on SDSP #", slv, "!\n");
		newError(&returnCode, CONFIG_ERROR, FATAL_ERR, "setSlvClk", 
		         genStr, __FILE__, __LINE__);
	}

	return returnCode;
}

/************************************************************************************ 
 * slvPresent:    returns TRUE if the slave is physically present on the ROD (for
 *                rev. E RODs -- for rev. B & C always returns TRUE).
 ************************************************************************************/
UINT8 slvPresent(UINT8 slv) {
	UINT8  rodRev;
	UINT32 rcfStat0;

	if (slv >= 4) return FALSE;

	rodRev= getRodRev();
	if ((rodRev == 0xb)||(rodRev == 0xc)) return TRUE;
	else if (rodRev == 0xe) {
		readRegister(RRIF_STATUS_0, 32, 0, &rcfStat0);
		return (rcfStat0 & (1<<RS0_DSP_PRESENT_O(slv)))? TRUE:FALSE;
	}
	else return FALSE;
}

/************************************************************************************ 
 * setSlaveConfig
 *
 *  synopsis: Sets the parameters of the slvDspConfig structure.
 *
 *  NOTE: The caller is responsible to make sure the the slave number is valid.  The
 *        code is protected to avoid a segmentation fault. The symptom of an illegal
 *        slave number is that all slaves will be shut off.  
 *
 *  arguments:
 *     IN:  slaveNumber = number of the slave DSP
 *          commOnOff = indicates whether the slave is present, booted and being used
 *          slaveType = identifies the slave executable, e.g. error checking...
 *
 *  modifications/bugs    
 *   - Added a field in the master DSP's status register #2 which indicates the
 *     current state of slave communications. This is used both to speed up the
 *     main loop (replaces the calls to numSlvsCommOn), and handle host slave DMA
 *     requests-- the state is still preserved in the rodConfig structure and is
 *     restored when the DMA is finished).                             14.11.02 dpsf
 ************************************************************************************/
void setSlaveConfig(UINT32 slvDspNum, UINT32 commOnOff, UINT32 slaveType) {
	UINT32 slvDspIdx;
	UINT32 oldCommOnOff;
	
	if (slvDspNum < N_SDSP) {
		oldCommOnOff = rodConfig.slvDspConfig[slvDspNum].commOnOff;
		
		if ((oldCommOnOff==SLV_DSP_COMM_ON) && (commOnOff==SLV_DSP_COMM_OFF)) {
			--rodConfig.numSlvsCommOn;
		}
		else if ((oldCommOnOff==SLV_DSP_COMM_OFF) && (commOnOff==SLV_DSP_COMM_ON)) {
			++rodConfig.numSlvsCommOn;
		} 
		
		rodConfig.slvDspConfig[slvDspNum].commOnOff = commOnOff;
		rodConfig.slvDspConfig[slvDspNum].type      = slaveType;

		if (commOnOff == SLV_DSP_COMM_ON) SET_RBIT(STATUS_REG_2, SR_SLVCOMM(slvDspNum));		
		else                              RST_RBIT(STATUS_REG_2, SR_SLVCOMM(slvDspNum));		
	}
	
	else {
		for (slvDspIdx = 0; slvDspIdx < N_SDSP; ++slvDspIdx) {
			rodConfig.slvDspConfig[slvDspNum].commOnOff = SLV_DSP_COMM_OFF;
			rodConfig.slvDspConfig[slvDspNum].type      = SLV_DSP_UNCONFIGURED;
			RST_RBIT(STATUS_REG_2, SR_SLVCOMM(slvDspNum));
		}
		rodConfig.numSlvsCommOn = 0;
	}
	
	return;
}

/************************************************************************************
 * numSlaves
 *
 *  synopsis: Returns the number of slave DSPs on the ROD.
 ************************************************************************************/
UINT32 numSlaves() {
	return rodConfig.numSlaves;
}

/************************************************************************************
 * slaveIsOn
 *
 *  synopsis: Returns true if the slave DSP slaveNumber is present (for Rev. E) &
 *            configured on.
 ************************************************************************************/
INT32 slaveIsOn(UINT32 slaveNumber) {
	if (slvPresent(slaveNumber)) {
		if (rodConfig.slvDspConfig[slaveNumber].commOnOff == SLV_DSP_COMM_ON) return 1;
		else                                                                  return 0;
	}
	else return -1;
}

/************************************************************************************
 * setLemoLink: Selects which input data link is sent to the front panel's LEMO
 *              connector.
 ************************************************************************************/
void setLemoLink(UINT8 fmtLink) {
	UINT32 fmt, hwLink;
	#if   (defined(SCT_ROD))
		fmt= fmtLink>>4, hwLink= (fmtLink & 0xf);
	#elif (defined(PIXEL_ROD))
		fmt= fmtLink>>4, hwLink= (fmtLink & 0xf) +2;
	#endif
	writeRegister(EFB_CMND_0, EFB_DATA_LINK_SEL_W, EFB_DATA_LINK_SEL_O, fmt);
	writeRegister(FMT_LINK_DATA_TEST_MUX(fmt), FMT_LINK_DATA_TEST_MUX_W,
	              FMT_LINK_DATA_TEST_MUX_O, hwLink);
}


//dpsf: when VHDL implemented, add pxl MHz switching functionality to rodMode
/************************************************************************************
 *  rodMode
 *
 *   synopsis: Sets RRIF CMD 1, RRIF CMD 0, the router's CMD_STAT register, and
 *             ROD communication registers appropriately to allow the ROD to operate
 *     in a standard running mode. See the ROD manual, appendix A for a dsecription of
 *     the individual bits in the registers set here.
 *     
 *     There are three modes of operation: initialization, data-taking and calibration.
 *     Initialization mode does just that (initialize the aspects of the ROD under MDSP
 *     control). The difference between the data taking and calibration modes lies in
 *     which piece of electronics controls the trigger: the TIM (data taking) or the
 *     ROD (calibration). Several sub-modes are available in the data-taking and
 *     calibration modes: simulation mode allows the ROD to use simulated data placed
 *     in the input memories ("inmems") in liu of real modules, configuration readback
 *     allows the ROD to capture (typically configuration) data sent back over the
 *     data lines, event capture allows an event to be simultaneously captured into the
 *     inmems while it plays through the formatters, and s-link overriding allows the
 *     ROD to flow data through the router when the S-Link is inactive (normally this
 *     is prohibited). In the configuration readback & event capture sub-modes, the
 *     inmems can be automatically read by the MDSP after the serial stream has been
 *     sent out. Once the ROD has been put in a mode, MODIFY_MODE can place the ROD into
 *     a sub-mode, or remove it from one (with NORMAL_MODE). 
 *
 *     An input nBits= 0 (fifo playback/capture length) sets the default length for
 *     that ROD revision. The delay is the # of delay slots before the capture begins.
 *     An input of 0 sets it to the minimum value (1). These inputs only have meaning
 *     if the appropriate mode-modifier is set.
 *
 *     Pixel modules can send up to 16 events (time-slices back per L1A). The evtsPerL1A
 *     input selects this; an input of 0 or mode modification will leave the formatter
 *     registers alone. 
 *             
 *   author: Douglas Ferguson 
 ************************************************************************************/
INT32 rodMode(UINT32 mode, UINT8 flag, UINT8 fifoSetup, UINT32 nBits, UINT32 delay,
              UINT32 evtsPerL1A, UINT8 message) {
 	INT32 returnCode= SUCCESS;
	UINT32 mt, bocBusyStat, mux, fifoBits;
	UINT8  mm, i, j, k;

	if (flag == STORE_MODE) {
		rodModeCfg[1]= rodModeCfg[0];
		//copyMem((UINT32 *) &rodModeCfg[0], (UINT32 *) &rodModeCfg[1],
		//        SIZEOF(RodModeCfg));
		readRegister(RRIF_CMND_0, 32, 0, &rodModeCfg[1].rcfCmd[0]);
		readRegister(RRIF_CMND_1, 32, 0, &rodModeCfg[1].rcfCmd[1]);
		readRegister(RTR_CMND_STAT, 32, 0, &rodModeCfg[1].rtrCmdStat);
		readRegister(FE_MASK_LUT_SELECT, 32, 0, &rodModeCfg[1].lutSelect);

		/* Note there are command masks in the mask LUTs. However, if a module needs
		   re-configuring, it is likely that the module is temporarily removed from
		   readout by setting its formatters off & setting the actual command masks
		   (without touching the LUTs) to reflect this. So they are stored here. */
		readRegister(FE_CMND_MASK_0_LO, 32, 0, &rodModeCfg[1].cmdMask[0][0]);
		readRegister(FE_CMND_MASK_0_HI, 32, 0, &rodModeCfg[1].cmdMask[0][1]);
		readRegister(FE_CMND_MASK_1_LO, 32, 0, &rodModeCfg[1].cmdMask[1][0]);
		readRegister(FE_CMND_MASK_1_HI, 32, 0, &rodModeCfg[1].cmdMask[1][1]);
		for (i= 0; i < 8; ++i) {   //j ==> formatter or serial port
			readRegister(FMT_LINK_EN(i), 12, 0, &rodModeCfg[1].fmtCfg[i]);
			#ifdef PIXEL_ROD
			readRegister(FMT_PXL_LINK_L1A_CNT(i), 16, 0,
			             &rodModeCfg[1].fmt_evtsPerL1A[i]);
			#endif
		}
	}

	else if (flag == RESTORE_MODE) {
		rodModeCfg[0]= rodModeCfg[1];
		//copyMem((UINT32 *) &rodModeCfg[1], (UINT32 *) &rodModeCfg[0],
		//        SIZEOF(RodModeCfg));
		writeRegisterDirect(RRIF_CMND_0, 32, 0, rodModeCfg[0].rcfCmd[0]);
		writeRegisterDirect(RRIF_CMND_1, 32, 0, rodModeCfg[0].rcfCmd[1]);
		writeRegisterDirect(RTR_CMND_STAT, 32, 0, rodModeCfg[0].rtrCmdStat);
		writeRegisterDirect(FE_MASK_LUT_SELECT, 32, 0, rodModeCfg[0].lutSelect);

		writeRegisterDirect(FE_CMND_MASK_0_LO, 32, 0, rodModeCfg[0].cmdMask[0][0]);
		writeRegisterDirect(FE_CMND_MASK_0_HI, 32, 0, rodModeCfg[0].cmdMask[0][1]);
		writeRegisterDirect(FE_CMND_MASK_1_LO, 32, 0, rodModeCfg[0].cmdMask[1][0]);
		writeRegisterDirect(FE_CMND_MASK_1_HI, 32, 0, rodModeCfg[0].cmdMask[1][1]);
		for (i= 0; i < 8; ++i) {   //j ==> formatter or serial port
			writeRegisterDirect(FMT_LINK_EN(i), 12, 0, rodModeCfg[0].fmtCfg[i]);
			#ifdef PIXEL_ROD
			writeRegisterDirect(FMT_PXL_LINK_L1A_CNT(i), 16, 0,
			                    rodModeCfg[0].fmt_evtsPerL1A[i]);
			#endif
		}

		return returnCode;
	}

	/* If modifying the current mode, mask off the major modes so that the routine
	   skips straight to the modification section. If not modifying the mode (i.e.
	   setting a new mode), clear the mode structure in memory: */
	if (flag == MODIFY_MODE)
		mode &= ~((ROD_INIT_MODE)+(DATA_TAKING_MODE)+(CALIBRATION_MODE));
	else
		setMem((UINT32 *) &rodModeCfg[0], SIZEOF(RodModeCfg), 0);

	/* Find any mode modifiers & set the appropriate flags. Note that the simulation,
	   evt capture & cfg. readback modifiers are mutually incompatible; all use the
	   input-memory FIFOs for different purposes. */
	mm= 0;
	if (mode & SIMULATION_MODE)        ++mm;
	if (mode & CONFIG_READBACK_MODE)   ++mm;
	if (mode & INMEM_EVT_CAPTURE_MODE) ++mm;

	if (mm > 1) {
		newError(&returnCode, PARAM_ERROR, FATAL_ERR, "rodMode", 
		"Error: Cfg readback, evt. capture, & simulation all use input memories!\n",
		         __FILE__, __LINE__);
		return returnCode;
	}  

	rodModeCfg[0].sim=             FALSE;
	rodModeCfg[0].cfgReadback=     FALSE;
	rodModeCfg[0].inmemEvtCapture= FALSE;
	if      (mode & SIMULATION_MODE) {rodModeCfg[0].sim= TRUE; }
	else if (mode & CONFIG_READBACK_MODE) {rodModeCfg[0].cfgReadback= TRUE; }
	else if (mode & INMEM_EVT_CAPTURE_MODE) {rodModeCfg[0].inmemEvtCapture= TRUE; }

	/* Automatic inmem readout is triggered in serialStreamOut if the rod mode 
	   flag below is set & either of the other capture flags is set. */
	if (  (mode & INMEM_AUTO_READOUT_MODE) 
	    &&((rodModeCfg[0].inmemEvtCapture)||(rodModeCfg[0].cfgReadback)) ) {
		rodModeCfg[0].autoInmemReadout= TRUE;
	}
	else {rodModeCfg[0].autoInmemReadout= FALSE; }

/************************************************************************************
 *  INIT mode:  set the ROD to a default (everything off) state.
 ************************************************************************************/
	if (mode & ROD_INIT_MODE) {
		rodModeCfg[0].rodMode= ROD_INIT_MODE;

		/* If the ROD is being initialized (is not yet running), check the BOC
		   status. If there isn't BOC attached, use simulation mode. */
		if (!GET_RBIT(STATUS_REG_0, SR_RUNNING)) {
			readRegister(RRIF_STATUS_1, 1, BOC_BUSY_O, &bocBusyStat);
			//if (bocBusyStat) rodModeCfg[0].sim= TRUE;
			//else             rodModeCfg[0].sim= FALSE;

			for (i= 0; i < 8; ++i) {   //i ==> look-up table
				for (j= 0; j < 8; ++j) {   //j ==> formatter or serial port
					for (k= 0; k < 2; ++k) {  //k ==> mode bit or low/high
						//dpsf: analyze & fix.
						writeRegister(RMB_DFLT_LUT(i, j, k), 12, 0,  0);
						writeRegister(RMB_CRTV_LUT(i, j, k), 12, 0,  0);

						writeRegister(RMB_CRTV_LUT(i, j, k), 12, 0,  0);
						writeRegister(RMB_CRTV_LUT(i, j, k), 12, 0,  0);

						if (j < 2)
							writeRegister(CMND_MASK_LUT(i, j, k), 12, 0,  0);
					}
				}
			}
		} //end: ROD is still being initialized

		else {
			/* Use setLinkMasks to set the dynamic modebits for both SP to skip/mask
			   all events, and turn off the command and data links.  Also clear the
			   EFB dynamic mask bits (no L1-ID offsets). Note that the init mode does
			   not care about whether there is simulated data or event capture, sofar
			   as the RRIF CMD registers are concerned: the intent is simply to put
			   the ROD in a known default state. */
			mt= (DATA_LINK) +(COMMAND_LINK) +(LINK_CFG);
			for (i=0; i<N_MASK_SETS; ++i) {
				if (i == MASK_SET_INIT) continue;  //Init set is left alone.
				setLinkMasks(0, SP_BOTH, mt, INIT_MASK, i);
			}
		}

		//Set the ROD's command mask & mode bits to the initial settings:
		setLinkMasks(0, SP_BOTH, mt, SET_MASK, MASK_SET_INIT);

		for (i= 0; i < 12; ++i) { //clear Dyn. Mask L.U.T.
			writeRegister(DM_DFLT_LUT(i), 16, 0, 0);
			writeRegister(DM_CRTV_LUT(i), 16, 0, 0);
		}

		/* Place all FIFOs in reset. */
		writeRegister(RRIF_CMND_0, 8, 0, 0xff);

		/* Place all VHDL fxn blocks in reset. */
		writeRegister(RRIF_CMND_1, 32, 0, 0x8a90);

		/* Set all formatters: off, accepting 1 event per L1A */
		for (i= 0; i < 8; ++i) {   //j ==> formatter or serial port
			writeRegister(FMT_LINK_EN(i), 12, 0,  0);
			#ifdef PIXEL_ROD
				writeRegister(FMT_PXL_LINK_L1A_CNT(i), 16, 0,  0x0);
			#endif
		}

		/* Set the router cmd-stat register's command bits to default turn on value: */
		writeRegister(RTR_CMND_STAT, 8, 0, 0);
	}

/************************************************************************************
 *  data-taking mode:  set up the ROD to await TIM triggers, or if in simulation
 *                     mode, to respond to "run the test bench" signal on the main
 *    controller FPGA by piping data out of the input-memories.
 ************************************************************************************/
	else if (mode & DATA_TAKING_MODE) {
		rodModeCfg[0].rodMode= DATA_TAKING_MODE;

		/* Set the RRIF command register to give the TIM control over all all
		   aspects of sending triggers (at least over SP0). */
		writeRegister(RRIF_CMND_1, 23, 0, 0x009523);

		/* Set the router cmd-status register's command bits to keep the data
		   in the EFB if the S-Link goes down, and dis-allow back-pressure from
		   the router to the EFBs : */
		writeRegister(RTR_CMND_STAT, 1, RTR_SLINK_DOWN_OVERRIDE_O, 0);
		writeRegister(RTR_CMND_STAT, 1, RTR_CALIB_BACK_PRES_EFB_O, 0);
	}


/************************************************************************************
 *  calibration mode:  set up the ROD for ROD-based triggers, and if in simulation
 *                     mode, to play back the input-memory FIFOs when the ROD detects
 *     an L1A on either of the serial ports (and the playback-inhibit bit is not set
 *     in the main RCF command register).
 ************************************************************************************/
	else if (mode & CALIBRATION_MODE) {
		rodModeCfg[0].rodMode= CALIBRATION_MODE;

		/* Set the RRIF command register to give the ROD control over all all
		   aspects of sending triggers.  */
		writeRegister(RRIF_CMND_1, 23, 0, 0x005494a1);

		/* Set the router cmd-status register's command bits to allow back-pressure
		   from the router to the EFBs : */
		writeRegister(RTR_CMND_STAT, 1, RTR_CALIB_BACK_PRES_EFB_O, 1);
	}


/************************************************************************************
 * MODIFY_MODE is used for the modifiable parts of the modes above (such as cfg
 * readback, evt. capture & simulation. This section is also used for the sections
 * which are common to both modes. Note: INIT mode has no modifications.
 ************************************************************************************/
	if (  ((flag == MODIFY_MODE) && (rodModeCfg[0].rodMode != ROD_INIT_MODE))
	    ||(mode & CALIBRATION_MODE)
	    ||(mode & DATA_TAKING_MODE)
	   ) {

		/* If continuing a current mode, only want to change the aspects of the mode
		   which relate to the mode modifiers. Other registers are left untouched since
		   these may have been changed since the mode was initiated. The formatter &
		   mask setting code is common to both data-taking & calibration modes and is
		   thus placed here-- they are left untouched. */
		if (flag != MODIFY_MODE) {
			#ifdef PIXEL_ROD
			if (evtsPerL1A>0) {
				for (i= 0; i < 8; ++i) {   //i ==> formatter
					for (j= 0; j < 4; ++j) {   //i ==> formatter
						writeRegister(FMT_PXL_LINK_L1A_CNT(i), 4, j*4, evtsPerL1A-1);
					}
				}
			}
			#endif
	
			mt= (DATA_LINK) +(COMMAND_LINK) +(LINK_CFG);
			setLinkMasks(0, SP0, mt, SET_MASK, MASK_SET_INIT);
		}


		/* Set the router cmd-status register's command bits to keep the data
		   flowing even if the S-Link goes down, if requested: */
		if (mode & CALIBRATION_SLINK_OVERRIDE_MODE)
			writeRegister(RTR_CMND_STAT, 1, RTR_SLINK_DOWN_OVERRIDE_O, 1);

		//Set mode modifiers:
		if (mode & SIMULATION_MODE) {
			if (rodModeCfg[0].rodMode == DATA_TAKING_MODE) {
				/* Enable FIFO -> formatters: */
				writeRegister(RRIF_CMND_1, 3, FIFO_CTRL_MUX_O, 0x2);
	
				/* Enable the test bench: */
				writeRegister(RRIF_CMND_1, 1, TEST_BENCH_RESET_O,  0x0);
				writeRegister(RRIF_CMND_1, 1, TEST_BENCH_ENABLE_O, 0x1);
	
				/* Set the test mode: */
				writeRegister(IDE_MEM_CTRL, TEST_FIXTURE_MODE_W,
				              TEST_FIXTURE_MODE_O, 0x20);
	
				/* Default playback length is ~819 micro-seconds 32K bits): */
				if (nBits == 0) writeRegister(INP_MEM_CTRL, 32, 0, 0x8000);
				else            writeRegister(INP_MEM_CTRL, 32, 0, nBits);
			}
			else if (rodModeCfg[0].rodMode == CALIBRATION_MODE) {
				writeRegister(RRIF_CMND_1, 1, STATIC_BCID_ENABLE_O, 1);
				/* Calibration using input memories is not in test-bench, so it
				   is left in reset. Enable FIFO -> formatters on L1A: */
				writeRegister(RRIF_CMND_1, 3, FIFO_CTRL_MUX_O, 0x6);
			}
		}
		else if ((mode & INMEM_EVT_CAPTURE_MODE)||(mode & CONFIG_READBACK_MODE)) {
			if      (mode & INMEM_EVT_CAPTURE_MODE) mux= 0x7;
			else if (mode & CONFIG_READBACK_MODE)   mux= 0x5;
			writeRegister(RRIF_CMND_1, 3, FIFO_CTRL_MUX_O, mux);

			if (delay == 0) delay= 1;
			if (nBits == 0) {
				if (getRodRev() == 0xb) nBits= 0x1000;
				else                    nBits= 0x8000;
			}

			writeRegister(CFG_READBACK_CNT, FIFO_WRITE_CNT_W, FIFO_WRITE_CNT_O, nBits);
			writeRegister(CFG_READBACK_CNT, 1, DELAY_FIFO_WRITE_O, delay);

			/* If the rod is in calibration mode and we want configuration readback,
			   turn off the calibration L1A sensing engine so that and L1A-like sequences
			   in the serial data don't confuse the ROD. */
			if (  (mode & CONFIG_READBACK_MODE)
			    &&(rodModeCfg[0].rodMode == CALIBRATION_MODE) )
				writeRegister(RRIF_CMND_1, 1, SP_TRIGGER_SIGNAL_DECODER_EN_O, 0);
		}
		else { //normal data
			/* Set normal data path: */
			writeRegister(RRIF_CMND_1, 3, FIFO_CTRL_MUX_O, 0x4);

			if (rodModeCfg[0].rodMode == DATA_TAKING_MODE) {
	
				/* Disable the test bench: */
				writeRegister(RRIF_CMND_1, 1, TEST_BENCH_RESET_O,  0x1);
				writeRegister(RRIF_CMND_1, 1, TEST_BENCH_ENABLE_O, 0x0);
			}
			else if (rodModeCfg[0].rodMode == CALIBRATION_MODE) {
				writeRegister(RRIF_CMND_1, 1, STATIC_BCID_ENABLE_O, 0);
				writeRegister(RRIF_CMND_1, 1, SP_TRIGGER_SIGNAL_DECODER_EN_O, 1);
			}
		}

		if (fifoSetup) {
			/* If requested, reset all FIFOs and then enable the FIFOs
			   which matter for that mode, leaving the others in reset */
			if      (mode & SIMULATION_MODE)        fifoBits= 0x30;
			else if (mode & INMEM_EVT_CAPTURE_MODE) fifoBits= 0x70;
			else if (mode & CONFIG_READBACK_MODE)   fifoBits= 0x70;
			else                                    fifoBits= 0x78;

			writeRegister(RRIF_CMND_0, 8, 0, 0xff);
			writeRegister(RRIF_CMND_0, 8, 0, fifoBits);
		}
	} //End: common mode configurations & modifications.

	//Read in the new values of the ROD registers into the MDSP structure: 
	readRegister(RRIF_CMND_0, 32, 0, &rodModeCfg[0].rcfCmd[0]);
	readRegister(RRIF_CMND_1, 32, 0, &rodModeCfg[0].rcfCmd[1]);
	readRegister(RTR_CMND_STAT, 32, 0, &rodModeCfg[0].rtrCmdStat);
	readRegister(FE_MASK_LUT_SELECT, 32, 0, &rodModeCfg[0].lutSelect);

	/* Note there are command masks in the mask LUTs. However, if a module needs
	   re-configuring, it is likely that the module is temporarily removed from
	   readout by setting its formatters off & setting the actual command masks
	   (without touching the LUTs) to reflect this. So they are stored here. */
	readRegister(FE_CMND_MASK_0_LO, 32, 0, &rodModeCfg[0].cmdMask[0][0]);
	readRegister(FE_CMND_MASK_0_HI, 32, 0, &rodModeCfg[0].cmdMask[0][1]);
	readRegister(FE_CMND_MASK_1_LO, 32, 0, &rodModeCfg[0].cmdMask[1][0]);
	readRegister(FE_CMND_MASK_1_HI, 32, 0, &rodModeCfg[0].cmdMask[1][1]);
	for (i= 0; i < 8; ++i) {   //j ==> formatter or serial port
		readRegister(FMT_LINK_EN(i), 12, 0, &rodModeCfg[0].fmtCfg[i]);
		#ifdef PIXEL_ROD
		readRegister(FMT_PXL_LINK_L1A_CNT(i), 16, 0,
		             &rodModeCfg[0].fmt_evtsPerL1A[i]);
		#endif
	}

	if (message) {
		strcpy(genStr, "Configuring ROD: ");
		if (mode & (ROD_INIT_MODE))    strcat(genStr, "initialization mode");
		if (mode & (DATA_TAKING_MODE)) strcat(genStr, "physics mode");
		if (mode & (CALIBRATION_MODE)) strcat(genStr, "calibration mode");
	
		if (mode & SIMULATION_MODE)         strcat(genStr, " (simulation data)");
		if (mode & CONFIG_READBACK_MODE)    strcat(genStr, " (configuration readback)");
		if (mode & INMEM_EVT_CAPTURE_MODE)  strcat(genStr, " (event capture)");
		if (rodModeCfg[0].autoInmemReadout) strcat(genStr, " (automatic readout)");

		strcat(genStr, ".\n");
		newInformation(__FILE__, __LINE__, genStr);
	}

	return returnCode;
}


/************************************************************************************
 *  setMaskConfig
 *
 *   synopsis: Sets the mask data for a module and tests the added module's links
 *             against existing modules' links for conflicts (warns if found).  
 *
 *   author: Douglas Ferguson 
 ************************************************************************************/
INT32 setMaskConfig(UINT8 moduleNum, UINT8 cmdLine, UINT8 fmtLink[2]) {
	INT32  returnCode= SUCCESS;
	UINT8  mod, i, j, lMax;

	/* Check input validity. */
	if (moduleNum >= N_TOTMODULES) {
		newError(&returnCode, PARAM_ERROR, FATAL_ERR, "setMaskConfig", 
		         "Invalid module number!\n", __FILE__, __LINE__);
	}

#if defined(SCT_ROD)
	else if ((cmdLine > 47)&&(cmdLine != DEFAULT_TTC)&&(cmdLine != OFF_ROD_TTC)) {
		newError(&returnCode, PARAM_ERROR, FATAL_ERR, "setMaskConfig", 
		         "Invalid command line!\n", __FILE__, __LINE__);
	}
	else if (  ((fmtLink[0] > 0x7b) && (fmtLink[0] != DATA_LINK_OFF))
	         ||((fmtLink[1] > 0x7b) && (fmtLink[1] != DATA_LINK_OFF))

	         ||(((fmtLink[0]&0x0f) > 0x0b) && (fmtLink[0] != DATA_LINK_OFF))
	         ||(((fmtLink[1]&0x0f) > 0x0b) && (fmtLink[1] != DATA_LINK_OFF)) ) {
		newError(&returnCode, PARAM_ERROR, FATAL_ERR, "setMaskConfig", 
		         "Invalid data line!\n", __FILE__, __LINE__);
	}

	lMax= 2;   //Up to two data lines for SCT modules.
#elif defined(PIXEL_ROD)
	else if ((cmdLine > 47)&&(cmdLine != DEFAULT_TTC)) {
		newError(&returnCode, PARAM_ERROR, FATAL_ERR, "setMaskConfig", 
		         "Invalid command line!\n", __FILE__, __LINE__);
	}
	else if (  ((fmtLink[0] > 0x73) && (fmtLink[0] != DATA_LINK_OFF))
	         ||(((fmtLink[0]&0x0f) > 0x03) && (fmtLink[0] != DATA_LINK_OFF)) ) {
		newError(&returnCode, PARAM_ERROR, FATAL_ERR, "setMaskConfig", 
		         "Invalid data line!\n", __FILE__, __LINE__);
	}

	lMax= 1;   //Only one data line defined for Pixel modules.
#endif

	if (FATAL(returnCode)) return returnCode;

	if (cmdLine == DEFAULT_TTC) cmdLine= moduleNum;

#ifndef SIM
	/* Modules cannot share their primary TTC or data links with another module. */
	for (mod= 0; mod<N_TOTMODULES; ++mod) {
	if ((mod != moduleNum) && (moduleMaskData[mod].defined)) {

		//SCT modules have a redundant command line which is used if the primary
		//is broken (different chip addresses distinguish the 2 modules sharing that
		//command line, in that case. So for SCT modules, no check is made on the
		//command line.
		#ifdef PIXEL_ROD
			if (cmdLine == moduleMaskData[mod].cmdLine) {
				newError(&returnCode, PARAM_ERROR, FATAL_ERR, "setMaskConfig", 
				         "Command line cannot be shared!\n", __FILE__, __LINE__);
				return returnCode;
			}
		#endif

		for (i=0; i<lMax; ++i) {
		if (fmtLink[i] != DATA_LINK_OFF) {
			for (j=0; j<lMax; ++j) {
				if (fmtLink[i] == moduleMaskData[mod].fmtLink[j]) {
					newError(&returnCode, PARAM_ERROR, FATAL_ERR, "setMaskConfig", 
					         "Data lines cannot be shared!\n", __FILE__, __LINE__);
					return returnCode;
				}
			}
		}}  //loop over active data lines.
	}}    //loop over all other defined modules.
#endif

	/* Store the module's link information inside the structure. */
	moduleMaskData[moduleNum].defined= TRUE;
	moduleMaskData[moduleNum].cmdLine= cmdLine;
	for (i=0; i<lMax; ++i) moduleMaskData[moduleNum].fmtLink[i]= fmtLink[i];

	return returnCode;
}

/************************************************************************************
 *  switch/setLinkMasks
 *
 *   synopsis: Configures the ROD's command and data links (both statically and
 *             dynamically) according to the moduleMaskData array's settings. Note
 *     that for some settings (SWITCH_MASK) of the storage flag of the routines, the
 *     routine depends upon internally stored settings to speed its work. *If the
 *     calling routine (the histogram control task) is using these settings, then it
 *     is vital that if it is interrupted that the ROD's masks are not disturbed.* If
 *     the masks are modified, results will be dependent on the modification-- some
 *     modifications may not matter much; the user should have a clear understanding
 *     of the mask configuration sets in use before attempting such modifications. 
 *     The routines are optimized for speed while switching masks (e.g. during
 *     a histogram scan). Functions include:  SET_MASK (sets the mask without regard
 *     to previous mask), UPDATE_MASK (adds a module's links to a mask), STORE_MASK &
 *     RESTORE_MASK (store & recall the ROD register settings), SWITCH_MASK (set the
 *     mask without writing to any ROD registers that do not need changing), and
 *     COMPUTE_MASK_DELTAS (determines which registers will need updating when
 *     SWTITCH_MASK is called). The functions are split in two since setLinkMasks
 *     does not need to reside in fast memory.
 *           
 *   author: Douglas Ferguson 
 *
 *  modifications/bugs    
 *   - Changed over to use the new moduleMaskData structure, and improved
 *     the way the routine handles switching masks by using the pre-computed
 *     delta-sets.                                                     08.04.03 dpsf
 *   - Implemented toggle switch which allows storage of the mask sets (for
 *     command masks & mode bits) either on the MDSP (as before), or using the
 *     ROD FPGA registers.                                             28.05.03 dpsf
 *   - Added functionality for mask-set searching and link suppression, to
 *     allow SCT token & bypass scanning. Searching will be used as a
 *     general mask-setting tool.                                      03.02.04 dpsf
 *   - Added functionality for switching the output of the front panel lemo
 *     (for ex. while scanning) when switching mask sets.              11.02.04 dpsf
 *   - Implemented module removal from a mask set.                     
 ************************************************************************************/
/* Semi-private variables for the two following routines: */
UINT8 lastSet= MASK_SET_INIT, lastCfgSet= MASK_SET_INIT;

void switchLinkMasks(UINT32 maskType, UINT8  maskSet) {
	UINT8  i;
	UINT32 v;

	maskSet&= 7;  //Quick & primitive input protection.
	/* Set SP mask registers if they differ from the last set's values. For command
	   links, RRIF CMD 1 mask load enable must have been pre-set (for loading without
	   an L1A). The masks are loaded, then the self-clearing NMR bit is set. */
	v= maskConfigData[maskSet].deltaMask[lastSet].cmdLine;
	if (v & DELTA_SP0_LO) 
		writeRegisterDirect(FE_CMND_MASK_0_LO, FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O,
	                        maskConfigData[maskSet].cmdMask[0].lowMask);

	if (v & DELTA_SP0_HI) 
		writeRegisterDirect(FE_CMND_MASK_0_HI, FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O,
			                maskConfigData[maskSet].cmdMask[0].highMask);


	if (v & DELTA_SP1_LO) 
		writeRegisterDirect(FE_CMND_MASK_1_LO, FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O,
	                        maskConfigData[maskSet].cmdMask[1].lowMask);

	if (v & DELTA_SP1_HI) 
		writeRegisterDirect(FE_CMND_MASK_1_HI, FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O,
			                maskConfigData[maskSet].cmdMask[1].highMask);

	if (v) writeRegister(RRIF_CMND_1, 1, NEW_MASK_READY_O, 1);

	/* Set the dynamic masks if they differ from the last set's values */
	v= maskConfigData[maskSet].deltaMask[lastSet].modeBits;
	for (i=0; i<FORMATTERS_PER_ROD; ++i) {

//dpsf comment out MB1?

		if (v & DELTA_DFLT_MB0(i)) 
			writeRegisterDirect(RMB0_DFLT_LUT(i), LINKS_PER_FORMATTER, 0,
			          maskConfigData[maskSet].dynMask[0].modeBits[i][0]);

		if (v & DELTA_DFLT_MB1(i)) 
			writeRegisterDirect(RMB1_DFLT_LUT(i), LINKS_PER_FORMATTER, 0,
			          maskConfigData[maskSet].dynMask[0].modeBits[i][1]);


		if (v & DELTA_CRTV_MB0(i)) 
			writeRegisterDirect(RMB0_CRTV_LUT(i), LINKS_PER_FORMATTER, 0,
			          maskConfigData[maskSet].dynMask[1].modeBits[i][0]);

		if (v & DELTA_CRTV_MB1(i)) 
			writeRegisterDirect(RMB1_CRTV_LUT(i), LINKS_PER_FORMATTER, 0,
			          maskConfigData[maskSet].dynMask[1].modeBits[i][1]);
	}

	if (GET_RBIT(DIAGNOSTIC_REG, DR_SCAN_LEMO)) {
		writeRegisterDirect(FMT_LINK_DATA_TEST_MUX(maskConfigData[maskSet].lemoFmt),
		                    FMT_LINK_DATA_TEST_MUX_W, 0, maskConfigData[maskSet].lemoLink);
		writeRegister(EFB_CMND_0, EFB_DATA_LINK_SEL_W, FMT_LINK_DATA_TEST_MUX_O,
		              maskConfigData[maskSet].lemoFmt);
	}

	lastSet= maskSet;

	/* Link re-configuration is not anticipated during most switching, and 
	   to re-configure links it must be explicitly requested. */
	if (maskType & LINK_CFG) {
		v= maskConfigData[maskSet].deltaMask[lastCfgSet].fmtCfg;
		for (i=0; i<FORMATTERS_PER_ROD; ++i) {
			if (v & DELTA_FMT_CFG(i)) 
				writeRegisterDirect(FMT_LINK_EN(i), LINKS_PER_FORMATTER, 0, 
					          maskConfigData[maskSet].fmtMask.fmtCfg[i]);

		}

		v= maskConfigData[maskSet].deltaMask[lastCfgSet].dataLinkMask;
		for (i=0; i<3; ++i) {
			if (v & DELTA_DATA_LINK_MASK(i)) 
				writeRegisterDirect(DATA_LINK_MASK(i), 32, 0, 
					          maskConfigData[maskSet].fmtMask.dataLinkMask[i]);

		}

		lastCfgSet= maskSet;
	}
}

//dpsf: +mask copy routine.
/************************************************************************************/
void setLinkMasks(UINT8 module,  UINT8 port,    UINT32 maskType,
                  UINT8 storage, UINT8 maskSet) {
	UINT8  i, sp, v, mod, spi, spf, dl, fmt, link, max, ms, mode, opt,
	       setLemo= FALSE, linkMod= 0;
	static UINT8 setModulesOnly= FALSE;
	UINT32 dxBit[2], fmtVal;
	UINT32 mle, mask[2], rm[2], regID[4], cms;
	UINT16 dMask, xMask[2], yMask[2];

	if (  (  (storage == UPDATE_MASK) && (module >= N_TOTMODULES)
	       &&(module != ALL_MODULES)  && (module != ALL_SET_MODULES))
	    ||((maskSet >= N_MASK_SETS)&&(maskSet != MASK_SET_SEARCH)) 
	   ) return;

	if (port < SP_BOTH) {spi= port;  spf= port+1; }
	else                {spi= 0;     spf= SP_BOTH; }

#if defined(SCT_ROD)
	/* For SCT RODs, two special calibration modes (bin variables ST_BYPASS & ST_TOKEN)
	   involve changing the chips' configuration to send data back either on both data
	   links, or all data on only one of the two. If these modes are being used, the
	   setToken routine in ABCDConfig.c will call setLinkMasks with the linkMod input
	   set. In this case, we need to loop over the calibration mask sets and find which
	   one(s) contain the input module, and adjust the modebits for that module. 
	   
	   Automatic link modification is only called through the SCT set-token routine,
	   and must be called (since it uses search) for a single module at a time. The user
	   can perform this outside a scan by sending the r/w module variable primitive. */
	if (storage >= LINK_MOD_LOWER) {
		if (maskSet == MASK_SET_SEARCH) {
			if (module >= N_TOTMODULES) return;
	
			for (maskSet=0; maskSet<MASK_SET_USER; ++maskSet) {
				//Check if module is in the set & recurse if so:
				i= FALSE;  //Borrow i as a temp flag.
				if (module<32) {
					if (maskConfigData[maskSet].validModules[1][0] & (1<<(module))) i= TRUE;
				}
				else { 
					if (maskConfigData[maskSet].validModules[1][1] & (1<<(module-32))) i= TRUE;
				}

				if (i) setLinkMasks(module, port, maskType, storage, maskSet);
			}
		
			return;
		}

		/* When arriving here, the code has found a mask-set to which the module belongs and
		   has recursed to modify the module's links for that mask set. The storage command is
		   internally modified to UPDATE_MASK after transferring it to a local variable: */
		linkMod= storage;
		storage= UPDATE_MASK;
	}
#endif

	//Use the Rod LUT by default:
	opt= (!(GET_RBIT(DIAGNOSTIC_REG, DR_USE_MDSP_MASK_LUT)));

	if (storage == SWITCH_MASK) {
		if (opt) writeRegisterDirect(FE_MASK_LUT_SELECT, 3, 0, maskSet);
		else     switchLinkMasks(maskType, maskSet);
        return;
	}

	/* If ROD storage is being used, get the current mask set from the mask set
	   lookup-table select register. This register selects which serial port command
	   mask set is used to select modules, and which (corresponding) set of mode bits
	   (& also, not used here, which L1-ID and ROD event types) are sent to the EFB
	   and formatters upon the decoding engine's receipt of an L1A. Note that for ROD
	   storage, the mask switching VHDL process on the ROD will switch command
	   and data links together, and so here they must be set together. */ 
	if (opt) {
		readRegister(FE_MASK_LUT_SELECT, 3,0, &cms);
		//if (  ((maskType & COMMAND_LINK) || (maskType & DATA_LINK))
		//    &&(!((maskType & COMMAND_LINK) && (maskType & DATA_LINK)))
		//    &&((storage == SET_MASK) || (storage == UPDATE_MASK))        ) {
		//	newInformation(__FILE__, __LINE__,
		//	    "Warning: command & data links will be set together.\n");
		//}
	}
	
	//If all the modules within the input mask set are being modified, recurse:
	if ((storage == UPDATE_MASK) && (module >= N_TOTMODULES)) {
		if (module == ALL_SET_MODULES) setModulesOnly= TRUE;
		else                           setModulesOnly= FALSE;

		for (mod= 0; mod<N_TOTMODULES; ++mod) {
			setLinkMasks(mod, port, maskType, storage, maskSet);
		}

		setModulesOnly= FALSE;
		return;
	}
	else {
		mod= module;
	}

	if (storage == UPDATE_MASK) {
		if (mod<32) {i= 0; v= mod; }
		else        {i= 1; v= mod -32; }

		/* Turn off the command & data link masks in the maskType variable if the
		   setModulesOnly flag is raised (& the module is not already part of the
		   set); this way the routine simply skips that part of the code. There
		   are two validModules fields in the mask cfg. sets: one for the command
		   links & another for the data links- each is a two word bitfield containing
		   up to 48 modules. */
		if (setModulesOnly) {
			if (!(maskConfigData[maskSet].validModules[0][i] & (1<<(v))))
				maskType &= ~(COMMAND_LINK);

			if (!(maskConfigData[maskSet].validModules[1][i] & (1<<(v))))
				maskType &= ~(DATA_LINK);
		}

		else { //Add to the set.
			if (maskType & COMMAND_LINK)
				maskConfigData[maskSet].validModules[0][i] |= (1<<(v));

			if (maskType & DATA_LINK) {
				maskConfigData[maskSet].validModules[1][i] |= (1<<(v));
				setLemo= TRUE;
			}
		}
	}

	/* Command links: RRIF CMD 1 mask load enable must be set for loading without an
	   L1A. The masks are loaded, then the self-clearing NMR bit is set. */
	if (maskType & COMMAND_LINK) {

		/* Set the mask load enable bit in RRIF CMD 1. */
		if ((storage != STORE_MASK) && (storage != COMPUTE_MASK_DELTAS)) {
			readRegister(RRIF_CMND_1, 1, FE_MASK_LOAD_ENABLE_O, &mle);
			writeRegister(RRIF_CMND_1, 1, FE_MASK_LOAD_ENABLE_O, 1);
		}

		for (sp=spi; sp<spf; ++sp) {	
			/* Get the module HI & LO masks, these variables are re-cycled in the
			   COMPUTE_MASK_DELTAS section and so must be reset if looping twice. */
			mask[0]= mask[1]= 0;
			v= moduleMaskData[mod].cmdLine;
			if      (v < 32) mask[0]|= 1<<v;
			else if (v < 48) mask[1]|= 1<<(v-32);
			if ((storage == UPDATE_MASK)&&(v >= 48)) continue;

			if      (sp == 0) {
				if (!opt) {regID[0]= FE_CMND_MASK_0_LO; regID[1]= FE_CMND_MASK_0_HI; }
				//else      {regID[0]= FE_CMND_MASK_0_LO; regID[1]= FE_CMND_MASK_0_HI;
				//           regID[2]= FE_CMND_MASK_0_LO; regID[3]= FE_CMND_MASK_0_HI;}
				else      {for (i=0; i<2; ++i) regID[i]= CMND_MASK_LUT(maskSet, sp, i);
				           for (i=2; i<4; ++i) regID[i]= CMND_MASK_LUT(cms, sp, i-2); }
				dxBit[0]= DELTA_SP0_LO;      dxBit[1]= DELTA_SP0_HI;
			}
			else if (sp == 1) {
				if (!opt) {regID[0]= FE_CMND_MASK_1_LO; regID[1]= FE_CMND_MASK_1_HI; }
				//else      {regID[0]= FE_CMND_MASK_1_LO; regID[1]= FE_CMND_MASK_1_HI;
				//           regID[2]= FE_CMND_MASK_1_LO; regID[3]= FE_CMND_MASK_1_HI;}
				else      {for (i=0; i<2; ++i) regID[i]= CMND_MASK_LUT(maskSet, sp, i);
				           for (i=2; i<4; ++i) regID[i]= CMND_MASK_LUT(cms, sp, i-2); }
				dxBit[0]= DELTA_SP1_LO;      dxBit[1]= DELTA_SP1_HI;
			}

			if (storage == INIT_MASK) {  // initialize data set.
				if (!opt) {
					setMem((UINT32 *) &maskConfigData[maskSet].cmdMask[sp], 
					        SIZEOF(CmdMask), 0);
				}
				else {
					writeRegisterDirect(regID[0], FE_CMD_MASK_LO_W,FE_CMD_MASK_LO_O, 0);
					writeRegisterDirect(regID[1], FE_CMD_MASK_LO_W,FE_CMD_MASK_LO_O, 0);
				}
			}

			else if (storage == SET_MASK) { // data set => ROD mask
				if (!opt) {
					writeRegisterDirect(regID[0], FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O,
					                    maskConfigData[maskSet].cmdMask[sp].lowMask);
					writeRegisterDirect(regID[1], FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O,
						                maskConfigData[maskSet].cmdMask[sp].highMask);
				}
				else {
					writeRegisterDirect(FE_MASK_LUT_SELECT, 3,0, maskSet);

					//readRegister(regID[0], FE_CMD_MASK_LO_W,FE_CMD_MASK_LO_O, &rm[0]);
					//readRegister(regID[1], FE_CMD_MASK_HI_W,FE_CMD_MASK_HI_O, &rm[1]);
					////dpsf: ask John if setting NMR bit in CMND1 while using the
					////FPGA RAM will have any effect if bit 20 set (no?). 
					//writeRegisterDirect(regID[2], FE_CMD_MASK_LO_W,FE_CMD_MASK_LO_O,
					//                    rm[0]);
					//writeRegisterDirect(regID[3], FE_CMD_MASK_HI_W,FE_CMD_MASK_HI_O,
					//	                rm[1]);
				}
			}

			else if (storage == UPDATE_MASK) { //input => update data set, ROD mask
				if (!opt) {
					if ((maskType & COMMAND_LINK) == COMMAND_LINK_ON) {
						maskConfigData[maskSet].cmdMask[sp].lowMask|=  mask[0];
						maskConfigData[maskSet].cmdMask[sp].highMask|= mask[1];
					}
					else if ((maskType & COMMAND_LINK) == COMMAND_LINK_OFF) {
						maskConfigData[maskSet].cmdMask[sp].lowMask&=  ~mask[0];
						maskConfigData[maskSet].cmdMask[sp].highMask&= ~mask[1];
					}
	
					writeRegisterDirect(regID[0], FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O,
					                    maskConfigData[maskSet].cmdMask[sp].lowMask);
					writeRegisterDirect(regID[1], FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O,
						                maskConfigData[maskSet].cmdMask[sp].highMask);
				}
				else {
					readRegister(regID[0], FE_CMD_MASK_LO_W,FE_CMD_MASK_LO_O, &rm[0]);
					readRegister(regID[1], FE_CMD_MASK_HI_W,FE_CMD_MASK_HI_O, &rm[1]);

					if ((maskType & COMMAND_LINK) == COMMAND_LINK_ON) {
						rm[0]|= mask[0];   rm[1]|= mask[1];
					}
					else if ((maskType & COMMAND_LINK) == COMMAND_LINK_OFF) {
						rm[0]&= ~mask[0];  rm[1]&= ~mask[1];
					}
	
					writeRegisterDirect(regID[0], FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O, rm[0]);
					writeRegisterDirect(regID[1], FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O, rm[1]);
					writeRegisterDirect(FE_MASK_LUT_SELECT, 3,0, maskSet);
				}
			}
	
			else if (storage == STORE_MASK) { //ROD Mask => data set.
				if (!opt) {
					readRegister(regID[0], FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O, 
						&maskConfigData[maskSet].cmdMask[sp].lowMask);
					readRegister(regID[1], FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O,
						&maskConfigData[maskSet].cmdMask[sp].highMask);
				}
				else {
					readRegister(regID[2], FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O, &rm[0]);
					readRegister(regID[3], FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O, &rm[1]);

					writeRegisterDirect(regID[0], FE_CMD_MASK_LO_W, FE_CMD_MASK_LO_O, rm[0]);
					writeRegisterDirect(regID[1], FE_CMD_MASK_HI_W, FE_CMD_MASK_HI_O, rm[1]);
				}
			}
			
			else if ((storage == COMPUTE_MASK_DELTAS)&&(!opt)) {
				for (ms= 0; ms < N_MASK_SETS; ++ms) {
					if (ms == maskSet) continue;

					mask[0]= maskConfigData[ms].cmdMask[sp].lowMask;
					mask[1]= maskConfigData[ms].cmdMask[sp].highMask;

					if (mask[0] != maskConfigData[maskSet].cmdMask[sp].lowMask)
						 maskConfigData[maskSet].deltaMask[ms].cmdLine|=  dxBit[0];
					else maskConfigData[maskSet].deltaMask[ms].cmdLine&= ~dxBit[0];

					if (mask[1] != maskConfigData[maskSet].cmdMask[sp].highMask)
						 maskConfigData[maskSet].deltaMask[ms].cmdLine|=  dxBit[1];
					else maskConfigData[maskSet].deltaMask[ms].cmdLine&= ~dxBit[1];
				}
			}

		} //Loop over serial ports.		

		if (storage == INIT_MASK) {  //All delta-set flags are set high.
			for (ms= 0; ms < N_MASK_SETS; ++ms) {
				if (ms != maskSet) {
					maskConfigData[maskSet].deltaMask[ms].cmdLine=
						 (DELTA_SP0_LO) +(DELTA_SP0_HI)
						+(DELTA_SP1_LO) +(DELTA_SP1_HI);
				}
				else maskConfigData[maskSet].deltaMask[ms].cmdLine= FALSE;
			}
		}


		/* Set the new mask ready bit in RRIF CMD 1, and then restore its
		   former state. */
		if ((storage != STORE_MASK) && (storage != COMPUTE_MASK_DELTAS)) {
			writeRegister(RRIF_CMND_1, 1, NEW_MASK_READY_O, 1);
			delay(30);  //Wait for the new-mask-ready bit to clear.
			writeRegister(RRIF_CMND_1, 1, FE_MASK_LOAD_ENABLE_O, mle);
		}

		lastSet= maskSet;

	} //End of command links

	/* Data Link configuration: The corresponding static link mask is set, and
	   its bit inside the DATA_LINK_MASK register is left off (enabled). Data
	   link numbers (stored in the moduleMaskData structure array) use the standard
	   notation for the ROD's formatters: 0xCL where C= formatter chip (0-7), and
	   L= link number on that chip (0-b). */
	if (maskType & LINK_CFG) {

		/* For updates, simply want to update the current masks for the module's
		   data links. For all other modes, want to loop over all formatters. */
		if (storage == UPDATE_MASK) {
			for (dl= 0; dl < 2; ++dl) {
				v= moduleMaskData[mod].fmtLink[dl];
				if ((v > 0x7b) || ((v&0xf) > 0xb)) continue;
				fmt= v>>4;  link= v&0xf;
				dMask= 1<<link;
				v= (12*fmt +link);
				mask[0]= 1<<(v & 0x1f);  //shift remainder into mask.
				v>>= 5; //divide by 32.

				regID[0]= FMT_LINK_EN(fmt);
				regID[1]= DATA_LINK_MASK(v);
	
				if ((maskType & LINK_CFG) == LINK_CFG_OFF) {
					maskConfigData[maskSet].fmtMask.fmtCfg[fmt]&= ~dMask;
					maskConfigData[maskSet].fmtMask.dataLinkMask[v]|= mask[0];
				}
				else if ((maskType & LINK_CFG) == LINK_CFG_ON) {
					maskConfigData[maskSet].fmtMask.fmtCfg[fmt]|=  dMask;
					maskConfigData[maskSet].fmtMask.dataLinkMask[v]&= ~mask[0];
				}

				writeRegisterDirect(regID[0], LINKS_PER_FORMATTER, 0,
				        maskConfigData[maskSet].fmtMask.fmtCfg[fmt]);
				writeRegisterDirect(regID[1], 32, 0,
					    maskConfigData[maskSet].fmtMask.dataLinkMask[v]);
					
			} //Loop over the data links for this module.
		}

		else {
			for (fmt= 0; fmt < FORMATTERS_PER_ROD; ++fmt) {
				regID[0]= FMT_LINK_EN(fmt);
				dxBit[0]= DELTA_FMT_CFG(fmt);
	
				if (storage == INIT_MASK) { /* modebits => mask link */
					maskConfigData[maskSet].fmtMask.fmtCfg[fmt]= 0x000;
				}
				else if (storage == SET_MASK) {
					writeRegisterDirect(regID[0], LINKS_PER_FORMATTER, 0,
			                            maskConfigData[maskSet].fmtMask.fmtCfg[fmt]);
				}
		
				else if (storage == STORE_MASK) {
					readRegister(regID[0], LINKS_PER_FORMATTER, 0, &mask[0]);
					maskConfigData[maskSet].fmtMask.fmtCfg[fmt]= (UINT16) mask[0];
				}

				else if (storage == COMPUTE_MASK_DELTAS) {
					for (ms= 0; ms < N_MASK_SETS; ++ms) {
						if (ms == maskSet) continue;
	
						xMask[0]= maskConfigData[ms].fmtMask.fmtCfg[fmt];
						yMask[0]= maskConfigData[maskSet].fmtMask.fmtCfg[fmt];

						if (xMask[0] != yMask[0])
							 maskConfigData[maskSet].deltaMask[ms].fmtCfg|=  dxBit[0];
						else maskConfigData[maskSet].deltaMask[ms].fmtCfg&= ~dxBit[0];
					}
				}
			} //Loop over the formatters for this set.


			for (v=0; v<3; ++v) {
				if (storage == INIT_MASK) { /* modebits => mask link */
					maskConfigData[maskSet].fmtMask.dataLinkMask[v]= 0xffffffff;
				}

				else if (storage == SET_MASK) {
					writeRegisterDirect(DATA_LINK_MASK(v), 32, 0,
			                       maskConfigData[maskSet].fmtMask.dataLinkMask[v]);
				}
		
				else if (storage == STORE_MASK) {
					readRegister(DATA_LINK_MASK(v), 32, 0,
			                     &maskConfigData[maskSet].fmtMask.dataLinkMask[v]);
				}
	
				else if (storage == COMPUTE_MASK_DELTAS) {
					dxBit[0]= DELTA_DATA_LINK_MASK(v);
	
					for (ms= 0; ms < N_MASK_SETS; ++ms) {
						if (ms == maskSet) continue;
	
						xMask[0]= maskConfigData[ms].fmtMask.dataLinkMask[v];
						yMask[0]= maskConfigData[maskSet].fmtMask.dataLinkMask[v];
	
						if (xMask[0] != yMask[0])
							 maskConfigData[maskSet].deltaMask[ms].dataLinkMask|=
								 dxBit[0];
						else maskConfigData[maskSet].deltaMask[ms].dataLinkMask&=
								~dxBit[0];
					}
				}
			} //Loop over the 3 DATA_LINK_MASK variables.

			if (storage == INIT_MASK) {
				for (ms= 0; ms < N_MASK_SETS; ++ms) {
					if (ms != maskSet) {
						/* 0xff, 0x7=> all delta-set link cfg flags are high. */
						maskConfigData[maskSet].deltaMask[ms].fmtCfg= 0xff;
						maskConfigData[maskSet].deltaMask[ms].dataLinkMask= 0x7;
					}
					else {
						maskConfigData[maskSet].deltaMask[ms].fmtCfg= FALSE;
						maskConfigData[maskSet].deltaMask[ms].dataLinkMask= FALSE;
					}
				}
			}

		} //End: not updating masks.		

		lastCfgSet= maskSet;

	} //End of data link configuration.


	/* Data Links: The corresponding dynamic data link mask (modebit) is set. 
	   Note that if all the corresponding formatter's links are off (as on startup),
	   and the routine is initializing a mask set, the mode bits are left in PLAY
	   (so that the ROD can accept input from primlists & play out data without
	   having to re-configure the modebits.) */
	if (maskType & DATA_LINK) {

		/* For updates, simply want to update the current masks for the module's
		   data links. For all other modes, want to loop over all formatters. */
		if (storage == UPDATE_MASK) {max= 2; /* max # of data links */}
		else                        {max= FORMATTERS_PER_ROD;}		

		for (dl= 0; dl < max; ++dl) {
			if (storage == UPDATE_MASK) {
				v= moduleMaskData[mod].fmtLink[dl];
				if ((v > 0x7b) || ((v&0xf) > 0xb)) continue;
				fmt= v>>4;  link= v&0xf;
				dMask= 1<<link;
			}
			else {fmt= dl;}

			for (sp=spi; sp<spf; ++sp) {	
				if      (sp == 0) {
					if (!opt) {regID[0]= RMB0_DFLT_LUT(fmt);  regID[1]= RMB1_DFLT_LUT(fmt);}
					//else      {regID[0]= RMB0_DFLT_LUT(fmt);  regID[1]= RMB1_DFLT_LUT(fmt);
					//           regID[2]= RMB0_DFLT_LUT(fmt);  regID[3]= RMB1_DFLT_LUT(fmt);}
					else      {for (i=0; i<2; ++i) regID[i]= RMB_DFLT_LUT(maskSet, fmt, i); 
						       for (i=2; i<4; ++i) regID[i]= RMB_DFLT_LUT(cms, fmt, i-2);  }
					dxBit[0]= DELTA_DFLT_MB0(fmt); dxBit[1]= DELTA_DFLT_MB1(fmt);
				}
				else if (sp == 1) {
					if (!opt) {regID[0]= RMB0_CRTV_LUT(fmt);  regID[1]= RMB1_CRTV_LUT(fmt);}
					//else      {regID[0]= RMB0_CRTV_LUT(fmt);  regID[1]= RMB1_CRTV_LUT(fmt);
					//           regID[2]= RMB0_CRTV_LUT(fmt);  regID[3]= RMB1_CRTV_LUT(fmt);}
					else      {for (i=0; i<2; ++i) regID[i]= RMB_CRTV_LUT(maskSet, fmt, i);
					           for (i=2; i<4; ++i) regID[i]= RMB_CRTV_LUT(cms, fmt, i-2);  }
					dxBit[0]= DELTA_CRTV_MB0(fmt); dxBit[1]= DELTA_CRTV_MB1(fmt);
				}

				if (storage == INIT_MASK) { 
					if (!opt) {
						if (rodModeCfg[0].sim == TRUE) { /* modebits init => mask link */
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]= 0xfff;
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]= 0x000;
						}
						else { /* not sim: modebits init => skip link */
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]= 0x000;
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]= 0xfff;
						}
					}
					else {
						if (rodModeCfg[0].sim == TRUE) { /* modebits init => mask link */
							writeRegisterDirect(regID[0], LINKS_PER_FORMATTER, 0, 0xfff);
							writeRegisterDirect(regID[1], LINKS_PER_FORMATTER, 0, 0x000);
						}
						else { /* not sim: modebits init => skip link */
							writeRegisterDirect(regID[0], LINKS_PER_FORMATTER, 0, 0x000);
							writeRegisterDirect(regID[1], LINKS_PER_FORMATTER, 0, 0xfff);
						}
					}
				}

				else if (storage == SET_MASK) {
					if (!opt) {
						writeRegisterDirect(regID[0], LINKS_PER_FORMATTER, 0,
						        maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]);
						writeRegisterDirect(regID[1], LINKS_PER_FORMATTER, 0,
							    maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]);
					}
					else {
						writeRegisterDirect(FE_MASK_LUT_SELECT, 3,0, maskSet);
					}
				}

				else if (storage == UPDATE_MASK) {
					mode= (maskType & DATA_LINK);
					#ifdef SCT_ROD
					if      ((linkMod == LINK_MOD_LOWER) && (dl == 1)) mode= DATA_LINK_SKIP;
					else if ((linkMod == LINK_MOD_UPPER) && (dl == 0)) mode= DATA_LINK_SKIP;
					#endif

					if (  (setLemo)
					    &&(mode != DATA_LINK_SKIP) && (mode != DATA_LINK_MASKED) )  {
						maskConfigData[maskSet].lemoFmt=  fmt;
						maskConfigData[maskSet].lemoLink= link;
					}

					if (!opt) {
						if (mode == DATA_LINK_PLAY) {
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]&= ~dMask;
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]&= ~dMask;
						}
						else if (mode == DATA_LINK_MASKED) {
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]|=  dMask;
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]&= ~dMask;
						}
						else if (mode == DATA_LINK_SKIP) {
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]&= ~dMask;
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]|=  dMask;
						}
						else if (mode == DATA_LINK_D1P1) {
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]|=  dMask;
							maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]|=  dMask;
						}

						writeRegisterDirect(regID[0], LINKS_PER_FORMATTER, 0,
						        maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]);
						writeRegisterDirect(regID[1], LINKS_PER_FORMATTER, 0,
							    maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]);
					}
					else {
						readRegister(regID[0], LINKS_PER_FORMATTER, 0, &rm[0]);
						readRegister(regID[1], LINKS_PER_FORMATTER, 0, &rm[1]);

						if (mode == DATA_LINK_PLAY) {
							rm[0]&= ~dMask;    rm[1]&= ~dMask;
						}
						else if (mode == DATA_LINK_MASKED) {
							rm[0]|=  dMask;    rm[1]&= ~dMask;
						}
						else if (mode == DATA_LINK_SKIP) {
							rm[0]&= ~dMask;    rm[1]|=  dMask;
						}
						else if (mode == DATA_LINK_D1P1) {
							rm[0]|=  dMask;    rm[1]|=  dMask;
						}
						writeRegisterDirect(regID[0], LINKS_PER_FORMATTER,0, rm[0]);
						writeRegisterDirect(regID[1], LINKS_PER_FORMATTER,0, rm[1]);
						writeRegisterDirect(FE_MASK_LUT_SELECT, 3,0, maskSet);
					}
				}
		
				else if (storage == STORE_MASK) {
					if (!opt) {
						readRegister(regID[0], LINKS_PER_FORMATTER, 0, &mask[0]);
						readRegister(regID[1], LINKS_PER_FORMATTER, 0, &mask[1]);
	
						maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0]= 
							(UINT16) mask[0];
						maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1]= 
							(UINT16) mask[1];
					}
					else {
						readRegister(regID[2], LINKS_PER_FORMATTER, 0, &rm[0]);
						readRegister(regID[3], LINKS_PER_FORMATTER, 0, &rm[1]);
						writeRegisterDirect(regID[0], LINKS_PER_FORMATTER,0, rm[0]);
						writeRegisterDirect(regID[1], LINKS_PER_FORMATTER,0, rm[1]);
					}
				}
				
				else if ((storage == COMPUTE_MASK_DELTAS)&&(!opt)) {
					for (ms= 0; ms < N_MASK_SETS; ++ms) {
						if (ms == maskSet) continue;
	
						xMask[0]= maskConfigData[ms].dynMask[sp].modeBits[fmt][0];
						xMask[1]= maskConfigData[ms].dynMask[sp].modeBits[fmt][1];
	
						yMask[0]= maskConfigData[maskSet].dynMask[sp].modeBits[fmt][0];
						yMask[1]= maskConfigData[maskSet].dynMask[sp].modeBits[fmt][1];

						if (xMask[0] != yMask[0])
							 maskConfigData[maskSet].deltaMask[ms].modeBits|=  dxBit[0];
						else maskConfigData[maskSet].deltaMask[ms].modeBits&= ~dxBit[0];
	
						if (xMask[1] != yMask[1])
						     maskConfigData[maskSet].deltaMask[ms].modeBits|=  dxBit[1];
						else maskConfigData[maskSet].deltaMask[ms].modeBits&= ~dxBit[1];
					}
				}

			} //Loop over serial ports.		
		} //Loop over the data links/formatters for this module/set.

		if (storage == INIT_MASK) {
			for (ms= 0; ms < N_MASK_SETS; ++ms) {
				if (ms != maskSet) {
					/* 0xffffffff=> all delta-set modebit flags are high. */
					maskConfigData[maskSet].deltaMask[ms].modeBits= 0xffffffff;
				}
				else maskConfigData[maskSet].deltaMask[ms].modeBits= FALSE;
			}
		}

		lastSet= maskSet;
	} //End of dynamic data links (modebits)
}

#endif /* master DSP */