/*************************************************************************************
 * serialStreams_sct.c                                                 
 *
 *  synopsis: Contains SCT-specific routines which handle building and sending of
 *            serial streams.
 *
 *  in this file:  sendConfigSet, addCommand, addCmdDelay, addFastCmd, addSlowCmd,
 *                 addDataCmd, addMaskCmd, maskAllChannels, unmaskAllChannels,
 *    maskChannel, unmaskChannel, getChannelMask, biasDACRegValue, trimDACRegValue,
 *    thresholdRegValue,
 *    addConfigChipMask, addConfigChipBasic, addConfigChipTrim, addConfigChip,
 *    addConfigModuleMask, addConfigModuleBasic, addConfigModuleTrim, addConfigModule.
 *
 *  related files:
 *   serialStreams.c:    General serial-stream building & transmission routines.
 *   masterPrimFuncts.c: transSerialData, buildStream, sendStream, and sendConfig
 *                       primitives 
 *   primFuncts.c:       setTrigger primitive 
 *   ABCDcommands.c:     A separate library which has the prototypes for several of
 *                       the command-building functions here.
 ************************************************************************************/
#include <std.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <time.h>

#include <csl.h>
#include <csl_timer.h>

#include "resources.h"
#include "registerIndices.h"
#include "memoryPartitions.h"
#include "comRegDfns.h"
#include "serialStreams.h"
#include "macros.h"

extern char genStr[];
extern far CmdBuff cmdBuffer[];
extern far ModuleMaskData moduleMaskData[];
extern TIMER_Handle timer1;  /* general purpose timer, started in main() */

#pragma CODE_SECTION(sendConfigSet,     "xcode");

#pragma CODE_SECTION(addCommand,        "icode");
#pragma CODE_SECTION(addCmdDelay,       "icode");
#pragma CODE_SECTION(addFastCmd,        "icode");
#pragma CODE_SECTION(addSlowCmd,        "icode");
#pragma CODE_SECTION(addDataCmd,        "icode");
#pragma CODE_SECTION(addMaskCmd,        "icode");

#pragma CODE_SECTION(maskAllChannels,      "xcode");
#pragma CODE_SECTION(unmaskAllChannels,    "xcode");
#pragma CODE_SECTION(maskChannel,          "xcode");
#pragma CODE_SECTION(unmaskChannel,        "xcode");
#pragma CODE_SECTION(getChannelMask,       "xcode");

#pragma CODE_SECTION(biasDACRegValue,      "xcode");
#pragma CODE_SECTION(trimDACRegValue,      "xcode");
#pragma CODE_SECTION(thresholdRegValue,    "xcode");

#pragma CODE_SECTION(addConfigChipMask,    "icode");
#pragma CODE_SECTION(addConfigChipBasic,   "xcode");
#pragma CODE_SECTION(addConfigChipTrim,    "xcode");
#pragma CODE_SECTION(addConfigChip,        "xcode");

#pragma CODE_SECTION(addConfigModuleMask,  "icode");
#pragma CODE_SECTION(addConfigModuleBasic, "xcode");
#pragma CODE_SECTION(addConfigModuleTrim,  "xcode");
#pragma CODE_SECTION(addConfigModule,      "xcode");

/* file scope variables */
#pragma DATA_SECTION(cfgTiming, "idata");
CfgTiming cfgTiming;

#pragma DATA_SECTION(moduleConfigSet, "moduleData");
Module  moduleConfigSet[N_MODULE_CONFIG_SETS][N_TOTMODULES];

/* file scope variables */
#pragma DATA_SECTION(stageStream,       "stageBfr");
#pragma DATA_SECTION(moduleStageStream, "stageBfr");
UINT32  stageStream[4][8];
UINT32  moduleStageStream[N_TOTMODULES][4][8*N_CHIPS];

#pragma DATA_SECTION(stageIlBuff,       "stageBfr");
#pragma DATA_SECTION(moduleStageIlBuff, "stageBfr");
UINT32  stageIlBuff[4][2*8];
UINT32  moduleStageIlBuff[N_TOTMODULES/2][4][2*8*N_CHIPS];

#pragma DATA_SECTION(stageCmdBuff,       "xpdata");
#pragma DATA_SECTION(moduleStageCmdBuff, "xpdata");
CmdBuff stageCmdBuff[4];
CmdBuff moduleStageCmdBuff[N_TOTMODULES/2][4];


/* command stream transmission modes:  */
#define SERIAL_SEND    0
#define PARALLEL_SEND  1
/************************************************************************************
 *  sendConfigSet
 *
 *   synopsis: Sends out module configuration data from one of the configuration
 *             sets to a pair of modules, optionally looping over the entire set.
 *             It uses both serial ports if they are available.         
 *
 *   author: Douglas Ferguson
 *
 *  modifications/bugs:    
 *   - Altered the way the loop decides which command masks to use-- it will now
 *     look at the pTTC variable inside the module's configuration structure.
 *     Conflicts are avoided by flipping the sending mode for a pair of modules
 *     into serial (usually streams are sent out in parallel) if there is a
 *     conflict between the two command lines (i.e. one of the modules is using a
 *     redundant command line, which is the same as the other module's primary
 *     command line.                                                   25.03.03 dpsf
 *   - All command line & data line configuration is now stored internally by
 *     structures in rodConfiguration.c; the routine now accesses these and set
 *     the masks using the standard setLinkMasks routine.              18.04.03 dpsf
 ************************************************************************************/
INT32 sendConfigSet(UINT8 sp, UINT8 modIn[2], UINT8 chip, UINT8 loop, UINT8 setLinks,
                    UINT8 cfgSet, UINT8 groupId, UINT32 dataType, UINT8 activeOnly,
                    UINT8 enableDataTaking) {
#if defined(SIM)
	return SUCCESS;
}

#else
	INT32 returnCode= SUCCESS, status;
	INT32  (*chipFxn)(CmdBuff *cb, Chip *chip);  

	UINT8  i, j, jMax, mod[2], loopLimit[2]= {0,0}, fcidx, lcidx, cidx,
	       nj, nMod, port, sendMode, transmit[2];
	UINT32 cmd1, *ptr, nSets, *stream[2], streamLen[2], data;
	UINT32 t0, t1, len1;
	Module *modCfg[2];
	CmdBuff *cb;

	UINT8  chipLut[N_CHIPS]= {M0_CHIP,  S1_CHIP,  S2_CHIP,  S3_CHIP,
	                          S4_CHIP,  E5_CHIP,  M8_CHIP,  S9_CHIP,
	                          S10_CHIP, S11_CHIP, S12_CHIP, E13_CHIP};
	yellowLed_on;
	timingMacro_cfg0();

	/* Save the state of the main controller register. */
	readRegister(RRIF_CMND_1, 32, 0, &cmd1);

	/* Switch off the trigger sensing engine's connection to the control link- do
	   not want spurious L1 triggers etc. when configuring chips. The module's
	   command masks must already have been set calls to rwModuleData or moduleMask.
	   Enable loading of new masks, and serial stream output. These are all bits 
	   in RCF CMND 1. */	
	writeRegister(RRIF_CMND_1, 1, SP_TRIGGER_SIGNAL_DECODER_EN_O, 0);
	writeRegister(RRIF_CMND_1, 1, FE_MASK_LOAD_ENABLE_O, 1);
	writeRegister(RRIF_CMND_1, 1, FE_CMND_OUTPUT_ENABLE_O, 1);

	if (chip != ALL_CHIPS) {
		for (i=0; i<N_CHIPS; ++i) if (chip == chipLut[i]) {
			fcidx= i; lcidx= i +1; break;
		}
	}
	else {fcidx= 0; lcidx= N_CHIPS; }

	/* Store the current command link masks if the external caller wants them
	   restored when through; also turn off the formatters: */
	if (setLinks) {
		setLinkMasks(0, SP_BOTH, ((COMMAND_LINK)+(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);
	}

	/* jMax determines how many modules to configure in a single pass through 
	   the loop; normally jMax is 2, but it is set to one if a single module is
	   being configured. */
	if   ((!loop) && ((modIn[1] == NO_MODULE) || (modIn[1] == modIn[0])) ) {jMax= 1; }
	else                                                                   {jMax= 2; }

	/* determine which chip configuration function is being used. */
	if      (dataType == CONFIG_MODULE_BASIC) {chipFxn= addConfigChipBasic; }
	else if (dataType == CONFIG_MODULE_TRIM)  {chipFxn= addConfigChipTrim;	}
	else                                      {chipFxn= addConfigChip;      }

	waitingMacro_cfg0();

	/* The loop limits are only relevant if more than two modules have been
	   specified (using ALL_MODULES). If loop is false, the loop will break
	   off after the 1st iteration. The limits determine which modules get
	   configured: ones using the primary command link, or the redundant
	   command link (with the data lines off the ROD). */
	if      (loop == NORMAL_CONFIG_LOOP)  {
		loopLimit[0]= 0;             loopLimit[1]= N_MODULES-1;
	}
	else if (loop == OFF_ROD_CONFIG_LOOP) {
		loopLimit[0]= N_MODULES;     loopLimit[1]= N_TOTMODULES -1;
	}

	for (i= loopLimit[0]; i <= loopLimit[1]; i+= 2) {

		if (loop) { mod[0]= i;        mod[1]= i +1; }
		else      { mod[0]= modIn[0]; mod[1]= modIn[1]; }

		for (j= 0; j < 2; ++j) modCfg[j]= &moduleConfigSet[cfgSet][mod[j]];

		/* If both serial ports are being used (and both modules are present), stream
		   the modules' data out in parallel (two at a time) over the ports. If only
		   one serial port is being used, stream the modules' data out serially over
		   that port. Conflicts between command lines must be avoided (for modules
		   using the redundant command line). */
		if (   (sp == SP_BOTH) && (jMax == 2)
		    && (modCfg[0]->present) && (modCfg[1]->present) ) {
			sendMode= PARALLEL_SEND;
			if ((moduleMaskData[mod[0]].cmdLine) == (moduleMaskData[mod[1]].cmdLine)) 
				sendMode= SERIAL_SEND;  //TTC conflict.
		}
		else sendMode= SERIAL_SEND;

		/* count the # modules to be configured in this transmission (present, active
		   and (if configuring a particular group) in this group.  */

		for (j= nMod= 0; j < jMax; ++j) {
			if (  (!modCfg[j]->present)
			    ||((activeOnly) && (!modCfg[j]->active))
			    ||((groupId != MODULE_GROUP_ALL) && (modCfg[j]->groupId != groupId))
			   )  {
				transmit[j]= FALSE;
				continue;
			}

			transmit[j]= TRUE;
			++nMod;
		}
		if (nMod == 0) continue; //Leave if neither module of pair needs configuring.  

		for (j= nj= 0; j < jMax; ++j) {  //j ==> module(s)

			if (!transmit[j])  continue;
			else ++nj;  //increment the #modules processed this bit.

			if (sp == SP_BOTH) {port= j;}
			else               {port= sp;}

			/* Initialize the masks on the 1st serial port for this pair, or if
			   the data is being transmitted serially. */
			if ((sendMode == SERIAL_SEND) || (nj == 1)) {
				stream[port^1]= cmdBuffer[port^1].data;
				streamLen[port^1]= 0;

				if (setLinks) {
					setLinkMasks(0, SP_BOTH, COMMAND_LINK, INIT_MASK, MASK_SET_WORK1);
					setLinkMasks(0, SP_BOTH, COMMAND_LINK, SET_MASK, MASK_SET_WORK1);
				}
			}

			if (setLinks)
				setLinkMasks(mod[j], port, COMMAND_LINK_ON, UPDATE_MASK, MASK_SET_WORK1);

			initCmdBuffer(port);
			stream[port]= cmdBuffer[port].data;

			timingMacro_cfg1();
			for (cidx= fcidx; cidx < lcidx; ++cidx) {
				status= chipFxn(&cmdBuffer[port], &modCfg[j]->chip[cidx]);
			}
			timingMacro_cfg2();


			/* Enable data-taking if requested: loop over chips. */
			if (enableDataTaking) {
				timingMacro_cfg5();
				data= 0x80;
				status= addCommand(&cmdBuffer[port], DELAY, 0, &data, 0x20, 0);
				if (status != SUCCESS) {
					addError(&returnCode, status, "sendConfigSet", "addCommand",
			                 __FILE__, __LINE__);
					return returnCode;
				}

				/* An EDT command is sent to each chip */
				for (cidx= fcidx; cidx < lcidx; ++cidx) {
					data= 0;
					status= addCommand(&cmdBuffer[port], ENABLE_DATA_TAKING,
							           modCfg[j]->chip[cidx].address, &data, 0x00, 0);

					if (status != SUCCESS) {
						addError(&returnCode, status, 
						         "sendConfigSet", "addCommand", __FILE__, __LINE__);
						return returnCode;
					}

					data= 0x5;
					status= addCommand(&cmdBuffer[port], DELAY, 0, &data, 0x20, 0);
					if (status != SUCCESS) {
						addError(&returnCode, status,
						         "sendConfigSet", "addCommand", __FILE__, __LINE__);
						return returnCode;
					}
				}

				timingMacro_cfg6();
			} /* enableDataTaking? */

			streamLen[port]=   WORD_INDEX(cmdBuffer[port].bitIndex +31);
			waitingMacro_cfg1();


			if (sendMode == SERIAL_SEND) {
				t1= TIMER_getCount(timer1);
				interleave(streamLen, stream, &ptr, &nSets);
				cfgTiming.il+= delta_t(t1);

				t1= TIMER_getCount(timer1);
				serialStreamOut(FALSE, (TransData *) ptr, nSets);  
				cfgTiming.xmit+= delta_t(t1);
			}
		} /* loop over module(s) (jMax) */

		if (sendMode == PARALLEL_SEND) {
			t1= TIMER_getCount(timer1);
			interleave(streamLen, stream, &ptr, &nSets);
			cfgTiming.il+= delta_t(t1);

			t1= TIMER_getCount(timer1);
			serialStreamOut(FALSE, (TransData *) ptr, nSets);  
			cfgTiming.xmit+= delta_t(t1);
		}

		if (!loop) break;   /* break out of loop if only configuring 1 or 2 modules */
	} /* loop over modules (i) */

	if (setLinks) {
		setLinkMasks(0, SP_BOTH, ((COMMAND_LINK)+(LINK_CFG)), SET_MASK, MASK_SET_WORK0);
	}

	yellowLed_off;
	/* Restore the state of the main controller command register: */
	writeRegister(RRIF_CMND_1, 32, 0, cmd1);

	waitingMacro_cfg2();
			
	timingMacro_cfg7();
	timingMacro_cfg8();

	return returnCode;
}
#endif   //end of non-SIM sendConfig code


/************************************************************************************
 *  Functions below are command stream building functions adapted from the
 *  ABCDcommands.c library by Lukas Tomasek. The functions have been modified to
 *  standardize them with the rest of the DSP code. Also see the ABCD3T Chip
 *  Specification Version 1.2, which can be found at:
 *     http://scipp.ucsc.edu/groups/atlas/sct-docs.html, other documentation is at:
 *     http://s.home.cern.ch/s/sct/public/sctdaq/sctdaq.html
 ************************************************************************************/

/************************************************************************************
 *  addCommand
 *
 *   synopsis: Adds a command to the indicated command stream. For all commands but
 *             the mask command, the data is a single word.
 *
 *   author: Douglas Ferguson
 *
 *  modifications/bugs:    
 *   - Added setting of the SCT channel masks in the same function: the input data
 *     is now a pointer, with new parameter, input data length, added (which is
 *     only checked for the mask command.                              14.03.03 dpsf
 ************************************************************************************/
INT32 addCommand(CmdBuff *cb, UINT8 cmd, UINT8 chipAddress,
                 UINT32 *data, UINT32 dataLen /* in bits */, UINT8 fibre) {
	INT32  returnCode= SUCCESS, status; 
	UINT16 value;
	UINT8  channel;

	/* dpsf: fix chipAddress until Lukas has a chance to put into new TS */
	if (chipAddress == 0x2f) chipAddress= 0x3f;

	if (  (cmd == L1_TRIGGER)
	    ||(cmd == SOFT_RESET)
	    ||(cmd == BC_RESET)  ) {

		status= addFastCmd(cb, (FAST_CMD_ID) cmd);
	}
	else if (  (cmd == PULSE_INPUT_REG)
	         ||(cmd == ENABLE_DATA_TAKING)
	         ||(cmd == CALIBRATION_PULSE) ) {

		status= addSlowCmd(cb, (SLOW_CMD_ID) cmd, chipAddress);
	}
	else if (  (cmd == CONFIG_REG)
	         ||(cmd == STROBE_DELAY_REG)
	         ||(cmd == THRESHOLD_REG)
	         ||(cmd == BIAS_DAC_REG)  ) {

		status= addDataCmd(cb, (DATA_CMD_ID) cmd, chipAddress, (UINT16) *data);
	}
	else if (cmd == TRIM_DAC_REG) {
		/* load TrimDac Registers */
		for(channel=0; channel<N_SCT_CHANS; ++channel) {
			value= trimDACRegValue(channel, (UINT8) *data);
			status= addDataCmd(cb, TRIM_DAC_REG, chipAddress, value);
		}
	}
	else if (cmd == DELAY) {
		status= addCmdDelay(cb, *data);
	}
	else if (cmd == MASK) {
		if (dataLen != 128) { //bad dog. bad!
			sprintf(genStr,"%s%s%x%s", "A mask command was input, but ",
			        "the extra data field length was incorrect! (len= 0x",
			        dataLen, ").\n");
			newError(&returnCode, PARAM_ERROR, FATAL_ERR,
			         "addCommand", genStr, __FILE__, __LINE__);
			return returnCode;
		}
		status= addMaskCmd(cb, chipAddress, data);
	}

	if (status != SUCCESS) {
		addError(&returnCode, status, "addCommand", "command function",
		         __FILE__, __LINE__);
	}

	return returnCode;
}

/************************************************************************************
 *  addCmdDelay:  Add a delay (zero bits) to the command stream
 ************************************************************************************/
INT32 addCmdDelay(CmdBuff *cb, UINT32 numDelayBits) {
	INT32  returnCode= SUCCESS; 

	if (WORD_INDEX(cb->bitIndex +numDelayBits +31) > cb->bufferSizeWords) {
		newError(&returnCode, CMD_BUFFER_OVERFLOW, FATAL_ERR, "addCmdDelay",
		        "An overflow occurred in the command buffer.\n",
		         __FILE__, __LINE__);
		return returnCode;
	}

	cb->bitIndex+= numDelayBits;
	return returnCode;
}

/************************************************************************************
 *  addFastCmd:  Adds a fast command to the command stream. The 3 fast commands
 *               are (in bits):   L1 Trigger: 110,
 *                                Soft Reset: 101 0100, BC Reset: 101 0010
 *
 *    Fast commands can come at any point in a command stream; they do not have to
 *    be aligned on a word boundary.
 ************************************************************************************/
INT32 addFastCmd(CmdBuff *cb, FAST_CMD_ID fastCmd) {
	INT32  returnCode= SUCCESS; 
	UINT32 command[3]= {0xC0000000, 0xA8000000, 0xA4000000};
	UINT32 cmdIndex= fastCmd -L1_TRIGGER, length, i;
	

	if (fastCmd == L1_TRIGGER) {length= L1_TRIGGER_LENGTH; }
	else                       {length= FAST_CMD_LENGTH;   }

	if (  (fastCmd != L1_TRIGGER)
	    &&(fastCmd != SOFT_RESET)
	    &&(fastCmd != BC_RESET)  ) {
		sprintf(genStr,"%s%d%s", "Unknown fast command ID! (", fastCmd, ")\n");
		newError(&returnCode, WRONG_COMMAND_ID, FATAL_ERR, "addFastCmd",
		         genStr, __FILE__, __LINE__);
		return returnCode;
	}

	if (WORD_INDEX(cb->bitIndex +length +31) > cb->bufferSizeWords) {
		newError(&returnCode, CMD_BUFFER_OVERFLOW, FATAL_ERR, "addFastCmd",
		         "An overflow occurred in the command buffer.\n", __FILE__, __LINE__);
		return returnCode;
	}

	for (i=0; i<length; ++i) {
		if (command[cmdIndex]&(0x80000000>>i)) {
			cb->data[WORD_INDEX(cb->bitIndex)]|= 0x80000000>>(cb->bitIndex%32);
		}
		++cb->bitIndex;
	}

	return returnCode;
}

/************************************************************************************
 *  addSlowCmd:  Adds a slow command with no register data to the command stream;
 *               these can either pulse an input register, enable data taking, or
 *    issue a calibration pulse. Slow commands take the form:
 *
 *       101 0111 00001100 aaaaaa xxx000   (27 bits)
 *     
 *    where xxx= 100 for pulsing an input register, xxx= 101 for enabling data taking,
 *    and   xxx= 110 for issuing a calibration pulse.
 * 
 *    ==> Do slow commands need to be on a word boundary?
 *        Bug: Calibration pulse was 0xAE180500
 ************************************************************************************/
INT32 addSlowCmd(CmdBuff *cb, SLOW_CMD_ID slowCmd, UINT8 chipAddress) {
	INT32  returnCode= SUCCESS; 
	UINT32 command[3]= {0xAE180400, 0xAE180500, 0xAE180600};
	UINT32 cmdIndex= slowCmd -PULSE_INPUT_REG, length= SLOW_CMD_LENGTH,
	       chAddress= (UINT32) chipAddress, fullCmd, i;

	if (  (slowCmd != PULSE_INPUT_REG)
	    &&(slowCmd != ENABLE_DATA_TAKING)
	    &&(slowCmd != CALIBRATION_PULSE) ) {
		sprintf(genStr,"%s%d%s", "Unknown slow command ID! (", slowCmd, ")\n");
		newError(&returnCode, WRONG_COMMAND_ID, FATAL_ERR, "addSlowCmd",
		         genStr, __FILE__, __LINE__);
		return returnCode;
	}

	if (WORD_INDEX(cb->bitIndex +length +31) > cb->bufferSizeWords) {
		newError(&returnCode, CMD_BUFFER_OVERFLOW, FATAL_ERR, "addSlowCmd",
		         "An overflow occurred in the command buffer.\n", __FILE__, __LINE__);
		return returnCode;
	}

	fullCmd= command[cmdIndex] | (chAddress<<11); /* add chip address */

	for(i=0; i<length; ++i) {
		if (fullCmd&(0x80000000>>i)) {
			cb->data[WORD_INDEX(cb->bitIndex)]|= 0x80000000>>(cb->bitIndex%32);
		}
		++cb->bitIndex;
	}

	return returnCode;
}

/************************************************************************************
 *  addDataCmd:  Adds a slow command with 16 bits of register data to the command
 *               stream. Data commands must be aligned to start on a word boundary,
 *    and must have a minimum of 5 delay counts (zeros) beforehand and exactly 16
 *    delay counts afterward (64 bits); they take the form:
 *
 *   00000 101 0111 00011100 aaaaaa xxxx00 + vvvvvvvv vvvvvvvv 00000000 00000000
 *     
 *    where xxxx= 0000 for the configuration register, 0100 for the strobe delay,
 *                0110 for the threshold registers,    1110 for the bias DAC,
 *           and  0001 to load a trim-DAC register for a channel (the channel number
 *           is included in the register value). 
 *
 *   bug: data cmd length= 64 not 43!
 ************************************************************************************/
INT32 addDataCmd(CmdBuff *cb, DATA_CMD_ID dataCmd, UINT8 chipAddress,
                 UINT16 registerValue) {
	INT32  returnCode= SUCCESS; 
	UINT32 command[5]= {0x0571C000, 0x0571C010, 0x0571C018, 0x0571C038, 0x0571C004};
	UINT32 cmdIndex= dataCmd -CONFIG_REG, length= DATA_CMD_LENGTH;
	UINT32 chAddress= (UINT32) chipAddress, rValue= (UINT32) registerValue;;
	
	/* align first command bit on 32bit word boundary, ie. add zero "padding" bits */
	cb->bitIndex= WORD_INDEX(cb->bitIndex+31)*32; 
	
	if (  (dataCmd != CONFIG_REG)
	    &&(dataCmd != STROBE_DELAY_REG)
	    &&(dataCmd != THRESHOLD_REG)
	    &&(dataCmd != BIAS_DAC_REG)
	    &&(dataCmd != TRIM_DAC_REG) ) {
		sprintf(genStr,"%s%d%s", "Unknown data command ID! (", dataCmd, ")\n");
		newError(&returnCode, WRONG_COMMAND_ID, FATAL_ERR, "addDataCmd",
		         genStr, __FILE__, __LINE__);
		return returnCode;
	}

	if (WORD_INDEX(cb->bitIndex +length +31) > cb->bufferSizeWords) {
		newError(&returnCode, CMD_BUFFER_OVERFLOW, FATAL_ERR, "addDataCmd",
		         "An overflow occurred in the command buffer.\n", __FILE__, __LINE__);
		return returnCode;
	}

	/* add chip address */  
	cb->data[WORD_INDEX(cb->bitIndex)]= command[cmdIndex] | (chAddress<<6); 
	cb->bitIndex+= 32;

	/* add reg. value */
	cb->data[WORD_INDEX(cb->bitIndex)]= (rValue<<16);
	cb->bitIndex+= 32;
	
	return returnCode;
}

/************************************************************************************
 *  addMaskCmd:  Adds a slow command with mask register data to the command stream.
 *               Data commands must be aligned to start on a word boundary, and must
 *    have a minimum of 5 delay counts (zeros) beforehand. While the other data
 *    commands have 16 delay counts afterward, mask commands require 32. They take
 *    the form:
 *
 *   00000 101 0111 10001100 aaaaaa 001000 + mask data (4 words) + empty word.
 *
 *   bug: mask cmd length= 192 not 155!
 ************************************************************************************/
INT32 addMaskCmd(CmdBuff *cb, UINT8 chipAddress, UINT32 mask[4]) {
	INT32  returnCode= SUCCESS; 
	UINT32 command= 0x0578C008, length= MASK_CMD_LENGTH;
	UINT32 chAddress= (UINT32) chipAddress, i;
	
	/* dpsf: fix chipAddress until Lukas has a chance to put into new TS */
	if (chipAddress == 0x2f) chipAddress= 0x3f;

	/* align first command bit on 32bit word boundary, ie. add zero "padding" bits */
	cb->bitIndex= WORD_INDEX(cb->bitIndex+31)*32; 
	
	if (WORD_INDEX(cb->bitIndex +length +31) > cb->bufferSizeWords) {
		newError(&returnCode, CMD_BUFFER_OVERFLOW, FATAL_ERR, "addMaskCmd",
		         "An overflow occurred in the command buffer.\n", __FILE__, __LINE__);
		return returnCode;
	}
	
	/* add chip address */  
	cb->data[WORD_INDEX(cb->bitIndex)]= command | (chAddress<<6); 
	cb->bitIndex+= 32;

	/* add mask */
	for(i=0; i<4; ++i) {
		cb->data[WORD_INDEX(cb->bitIndex)]= mask[3-i]; 
		cb->bitIndex+= 32;
	}	

	/* add padding */  
	cb->data[WORD_INDEX(cb->bitIndex)]= 0x0;
	cb->bitIndex+= 32;
	
	return returnCode;
}

/************************************************************************************
 *      Utility routines to mask and unmask channels, and pack register values.     *
 ************************************************************************************/
void maskAllChannels(UINT32 mask[4]) {
	int i;
	for (i=0; i<4; ++i) { mask[i]= 0x00000000; }	
}

void unmaskAllChannels(UINT32 mask[4]) {
	int i;  
	for (i=0; i<4; ++i) { mask[i]= 0xFFFFFFFF; }	
}

void maskChannel(UINT32 mask[4], UINT32 channel) {
	mask[channel/32]&= ~(1<<(channel%32));
}

void unmaskChannel(UINT32 mask[4], UINT32 channel) {
	mask[channel/32]|= 1<<(channel%32);
}

UINT32 getChannelMask(UINT32 mask[4], UINT32 channel) {
	return (mask[channel/32]>>(channel%32)) & 1;  /* returns channel's mask */
}

/* Bias DAC Reg. value: preamp bias current(bits 12:8), shaper current (bits 4:0) */
UINT16 biasDACRegValue(UINT8 preamp, UINT8 shaper) {
	UINT16 value= preamp;
	value<<= 8;    value|= shaper;
	return value;
}	

/* TrimDAC Reg. value: channel (bits 10:4), trim (bits 3:0) */
UINT16 trimDACRegValue(UINT8 channel, UINT8 trim) {
	UINT16 value= channel;
	value<<= 4;    value|= trim;
	return value;
}	

/* Threshold Reg. value: vthr(bits 15:8), vcal(bits 7:0) */
UINT16 thresholdRegValue(UINT8 vthr, UINT8 vcal) {
	UINT16 value= vthr;
	value<<= 8;    value|= vcal;
	return value;
}	

/************************************************************************************
 *                         Chip & Module configuration routines.                    *
 ************************************************************************************/

/************************************************************************************
 *  addConfigChipMask:  Build cb to load just the mask register of the specified
 *                      chip. Note that the registers of an inactive chip should
 *                      still be programmed.
 ************************************************************************************/
INT32 addConfigChipMask(CmdBuff *cb, ABCDChip *chip) {  
	INT32  returnCode= SUCCESS, status; 
	
	/* load mask */
	status= addMaskCmd(cb, chip->address, chip->basic.mask);
	if (status != SUCCESS) return status;
	
	return returnCode;
}	

/************************************************************************************
 *  addConfigChipBasic:  Build cb to load all basic registers of the specified chip 
 *                       (except TrimDACs). cb size - 14 words. Note that the
 *                       registers of an inactive chip should still be programmed.
 ************************************************************************************/
INT32 addConfigChipBasic(CmdBuff *cb, ABCDChip *chip) {  
	INT32  returnCode= SUCCESS, status; 
 	UINT16 value;
	
	/* load Config Reg. */
	value= *((UINT16 *) &chip->basic.config);
	status= addDataCmd(cb, CONFIG_REG, chip->address, value);
	if (status != SUCCESS) return status;
	
	/* load mask */
	status= addMaskCmd(cb, chip->address, chip->basic.mask);
	if (status != SUCCESS) return status;
	
	/* load StrobeDelay Reg. */ 
	value= (UINT16) chip->basic.delay;
	status= addDataCmd(cb, STROBE_DELAY_REG, chip->address, value);
	if (status != SUCCESS) return status;
	
	/* load Threshold Reg. */ 
	value= thresholdRegValue(chip->basic.vthr, chip->basic.vcal);
	status= addDataCmd(cb, THRESHOLD_REG, chip->address, value);
	if (status != SUCCESS) return status;
	
	/* load BiasDac Reg. */
	value= biasDACRegValue(chip->basic.preamp, chip->basic.shaper);
	status= addDataCmd(cb, BIAS_DAC_REG, chip->address, value);
	if (status != SUCCESS) return status;
	
	return returnCode;
}	

/************************************************************************************
 *  addConfigChipTrim:  Build cb to load all TrimDAC registers of the specified chip.
 *                      cb size - 256 words. Note that the registers of an inactive
 *                      chip should still be programmed.
 ************************************************************************************/
INT32 addConfigChipTrim(CmdBuff *cb, ABCDChip *chip) {  
	INT32  returnCode= SUCCESS, status; 
 	UINT16 value;
	UINT8 channel;
	
	/* load TrimDac Registers */
	for(channel=0; channel<N_SCT_CHANS; ++channel) {
		value= trimDACRegValue(channel, chip->trim[channel]);
		status= addDataCmd(cb, TRIM_DAC_REG, chip->address, value);
		if (status != SUCCESS) return status;
	}
	return returnCode;
}	

/************************************************************************************
 *  addConfigChip:      Build cb to load all registers of the specified chip.
 *                      cb size - 270 words. Note that the registers of an inactive
 *                      chip should still be programmed.
 ************************************************************************************/
INT32 addConfigChip(CmdBuff *cb, ABCDChip *chip) {  
	INT32  returnCode= SUCCESS, status; 

	status= addConfigChipBasic(cb, chip);
	if (status != SUCCESS) return status;

	status= addConfigChipTrim(cb, chip);
	if (status != SUCCESS) return status;

	return returnCode;
}


/************************************************************************************
 *  addConfigModuleMask:    Build cb to load the masks of the specified module.
 ************************************************************************************/
INT32 addConfigModuleMask(CmdBuff *cb, ABCDModule *module) {
	INT32  returnCode= SUCCESS, status; 
	int chipIndex;
	ABCDChip *chip;
	
	for(chipIndex=0; chipIndex<N_SCT_CHIPS; ++chipIndex) {
		chip= &module->chip[chipIndex];
		
		status= addConfigChipMask(cb, chip);
		if (status != SUCCESS) return status;
	}	

	return returnCode;
}

/************************************************************************************
 *  addConfigModuleBasic:   Build cb to load all registers of the specified module.
 *                          cb size - 168 words. Note that the registers of an
 *                          inactive module should still be programmed.
 ************************************************************************************/
INT32 addConfigModuleBasic(CmdBuff *cb, ABCDModule *module) {
	INT32  returnCode= SUCCESS, status; 
	int chipIndex;
	ABCDChip *chip;
	
	for(chipIndex=0; chipIndex<N_SCT_CHIPS; ++chipIndex) {
		chip= &module->chip[chipIndex];
		
		status= addConfigChipBasic(cb, chip);
		if (status != SUCCESS) return status;
	}	

	return returnCode;
}


/************************************************************************************
 *  addConfigModuleTrim:    Build cb to load all TrimDAC registers of the specified
 *                          module. cb size - 3072 words. Note that the registers of
 *                          an inactive module should still be programmed.
 *   bug: was calling addConfigChip instead of ChipTrim
 ************************************************************************************/
INT32 addConfigModuleTrim(CmdBuff *cb, ABCDModule *module) {
	INT32  returnCode= SUCCESS, status; 
	int chipIndex;
	ABCDChip *chip;
	
	for(chipIndex=0; chipIndex<N_SCT_CHIPS; ++chipIndex) {
		chip= &module->chip[chipIndex];
		
		status= addConfigChipTrim(cb, chip);
		if (status != SUCCESS) return status;
	}	

	return returnCode;
}

/************************************************************************************
 *  addConfigModule:    Build cb to load all TrimDAC registers of the specified
 *                      module. cb size - 3240 words. Note that the registers of
 *                      an inactive module should still be programmed.
 ************************************************************************************/
INT32 addConfigModule(CmdBuff *cb, ABCDModule *module) {
	INT32  returnCode= SUCCESS, status; 
	int chipIndex;
	ABCDChip *chip;
	
	for(chipIndex=0; chipIndex<N_SCT_CHIPS; ++chipIndex) {
		chip= &module->chip[chipIndex];
		
		status= addConfigChip(cb, chip);
		if (status != SUCCESS) return status;
	}	

	return returnCode;
}