/*******************************************************************************
 *	Pupe.cc	Pupe process
 *			T.Barnaby,	BEAM Ltd,	2007-02-13
 *******************************************************************************
 */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <syslog.h>
#include <Pupe.h>
#include <SigGen.h>
#include <Control.h>
#include <TmsD.h>
#include <Debug.h>
#include <signal.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/fcntl.h>
#include <errno.h>
#include <main.h>
#include <TmsLib.h>
#include <BFile.h>

/* Debug options */
#define	USE_DMA		1
#define ALL_CHANNELS	1
#define TEST_TESTDATA	1
#define DEBUG_CTRL	1
#define	DEBUG_DATA	1

// FPGA Control Registers
const int	IMEM_REG	= 0x0000 >> 2;	// Block RAM Index
const int	LOCKED		= 0x0008 >> 2;	// DCM Locked Status
const int	ADDR_REG	= 0x0010 >> 2;	// PAGE Register for SDRAM access
const int	MEM_REG		= 0x0018 >> 2;	// SDRAM Bank Select Register and BlockRam enable
const int	IER		= 0x0020 >> 2;	// Interrupt Enable Register
const int	ISR		= 0x0028 >> 2;	// Interrupt Status Register
const int	MEM_RAS		= 0x0030 >> 2;	// SDRAM Read Address Scaling
const int	MEM_RAO		= 0x0038 >> 2;	// SDRAM Read Address Offset
const int	MEM_GNT0	= 0x0400 >> 2;	// Memory Grant Registers
const int	MEM_GNT1	= 0x0408 >> 2;	// Memory Grant Registers
const int	MEM_GNT2	= 0x0410 >> 2;	// Memory Grant Registers
const int	MEM_GNT3	= 0x0408 >> 2;	// Memory Grant Registers

const int	FIRMWARE	= 0x0800 >> 2;	// Firmware ID code "CN" + version numbers
const int	ADC		= 0x0808 >> 2;	// ADC Control Register
const int	TIMING_IO	= 0x0810 >> 2;	// Timing I/O Register
const int	TESTCTRL	= 0x0818 >> 2;	// Test Data Control Register
const int	TESTLEN		= 0x0820 >> 2;	// Test Data Pattern Length

// Per Pick-Up registers
const int	PU0_REG_BASE	= 0x0880 >> 2;	// PickUp 0 Register Base
const int	PU1_REG_BASE	= 0x0900 >> 2;	// PickUp 1 Register Base
const int	PU2_REG_BASE	= 0x0980 >> 2;	// PickUp 2 Register Base

const int	CONTROL		= 0x0000 >> 2;	// PU General Control and Status register
const int	CYCLE		= 0x0008 >> 2;	// Cycle number
const int	TIME		= 0x0010 >> 2;	// Time in ms from start of cycle
const int	TIME_TBLADDR	= 0x0018 >> 2;	// Last write address in timing table
const int	PLL_FREQUENCY	= 0x0020 >> 2;	// PLL Reference orbit frequency
const int	PLL_FREQDELAY	= 0x0028 >> 2;	// PLL frequency load delay
const int	PLL_PHASEDELAY	= 0x0030 >> 2;	// PLL phase delay
const int	PLL_GAIN	= 0x0038 >> 2;	// PLL gain
const int	DDS_FREQ_MIN	= 0x0040 >> 2;	// PLL DDS minimum frequency
const int	DDS_FREQ_MAX	= 0x0048 >> 2;	// PLL DDS maximum frequency
const int	DIAG_CTRL	= 0x0050 >> 2;	// Diagnostics Control/Status
const int	DIAG_TRIGGER	= 0x0058 >> 2;	// Diagnostics Trigger
const int	TEST		= 0x0060 >> 2;	// Timing Test

// Memory access window
const int	FPGA_DATA_ADDRESS	= 0x200000;	// The FPGA memory
const int	FPGA_MEM	= 0x200000 >> 2;	// The FPGA memory

// SDRAM Data Banks
const int	SDRAM_DATA_PU0		= 0x08;	// PickUp 0 data
const int	SDRAM_DATA_PU1		= 0x09;	// PickUp 1 data
const int	SDRAM_DATA_PU2		= 0x0A;	// PickUp 2 data
const int	SDRAM_DATA_TEST		= 0x0B;	// Test input data
const int	SDRAM_BLOCKRAM		= 0x00;	// FPGA Memory

const int	PU0_BRAM_BASE		= 0;	// PickUp 0 Block Ram ID base
const int	PU1_BRAM_BASE		= 8;	// PickUp 0 Block Ram ID base
const int	PU2_BRAM_BASE		= 16;	// PickUp 0 Block Ram ID base

const int	BRAM_CYCLE_TIMING	= 0;	// Cycle Timing Table
const int	BRAM_CYCLE_INFO		= 1;	// Cycle Information Table
const int	BRAM_PHASE_TABLE	= 2;	// Timing Phase Table
const int	BRAM_SWITCH_TABLE	= 3;	// Timing Switch Table
const int	BRAM_DIAG_TABLE		= 4;	// Diagnostics table
const int	BRAM_BUNCH_MEAN0	= 5;	// Bunch Mean Table #0
const int	BRAM_BUNCH_MEAN1	= 6;	// Bunch Mean Table #1

// Control register bits
const int	CONTROL_INIT				= 0x01;	// Reset
const int	CONTROL_LOOP_CONTROL			= 0x02;	// 
const int	CONTROL_DDS_FREQUENCY_LIMIT_ENABLE	= 0x04;	// 

// Interrupt bits
const int	INT_PU0_CYCLE_START	= (1<<0);	// 0 PU #0 CYCLE_START
const int	INT_PU0_CYCLE_STOP	= (1<<1);	// 1 PU #0 CYCLE_STOP
const int	INT_PU0_ERROR		= (1<<2);	// 2 PU #0 ERROR
const int	INT_PU0_DIAG		= (1<<3);	// 3 PU #0 DIAGNOSTIC INFO CAPTURED
const int	INT_PU0_WRITE_FIFO	= (1<<4);	// 4 PU #0 SDRAM Write FIFO Half Full
const int	INT_PU0_USER0		= (1<<5);	// 5 PU #0 User Interrupts (Switch Table bits 5)
const int	INT_PU0_USER1		= (1<<6);	// 6 PU #0 User Interrupts (Switch Table bits 6)
const int	INT_PU0_USER2		= (1<<7);	// 7 PU #0 User Interrupts (Switch Table bits 7)

const int	INT_PU1_CYCLE_START	= (1<<8);	// 8 PU #1 CYCLE_START
const int	INT_PU1_CYCLE_STOP	= (1<<9);	// 9 PU #1 CYCLE_STOP
const int	INT_PU1_ERROR		= (1<<10);	// 10 PU #1 ERROR
const int	INT_PU1_DIAG		= (1<<11);	// 11 PU #1 DIAGNOSTIC INFO CAPTURED
const int	INT_PU1_WRITE_FIFO	= (1<<12);	// 12 PU #1 SDRAM Write FIFO Half Full
const int	INT_PU1_USER0		= (1<<13);	// 13 PU #1 User Interrupts (Switch Table bits 5)
const int	INT_PU1_USER1		= (1<<14);	// 14 PU #1 User Interrupts (Switch Table bits 6)
const int	INT_PU1_USER2		= (1<<15);	// 15 PU #1 User Interrupts (Switch Table bits 7)

const int	INT_PU2_CYCLE_START	= (1<<16);	// 16 PU #2 CYCLE_START
const int	INT_PU2_CYCLE_STOP	= (1<<17);	// 17 PU #2 CYCLE_STOP
const int	INT_PU2_ERROR		= (1<<18);	// 18 PU #2 ERROR
const int	INT_PU2_DIAG		= (1<<19);	// 19 PU #2 DIAGNOSTIC INFO CAPTURED
const int	INT_PU2_WRITE_FIFO	= (1<<20);	// 20 PU #2 SDRAM Write FIFO Half Full
const int	INT_PU2_USER0		= (1<<21);	// 21 PU #2 User Interrupts (Switch Table bits 5)
const int	INT_PU2_USER1		= (1<<22);	// 22 PU #2 User Interrupts (Switch Table bits 6)
const int	INT_PU2_USER2		= (1<<23);	// 23 PU #2 User Interrupts (Switch Table bits 7)

// FPGA Control Space Registers
const int	SYSMON_CTL		= (0x00400 >> 2);
const int	SYSMON_BUF		= (0x00500 >> 2);
const int	SYSMON_SMB_ERROR	= (0x10000000);

/// 64bit memory copy function for PUPE board 64bit data access
static inline int memcpy_64(void* to, const void* from, size_t len){
	size_t		i;

	// Check alignment and size
	if(((unsigned int)to & 0x07) || ((unsigned int)from & 0x07) || (len & 0x07)){
		fprintf(stderr, "memcpy_mmx: Memory pointers and length need to be aligned to 64bit boundary\n");
		return 1;
	}
	
	i = len / 8;
	while(i--){
		__asm__ __volatile__ (
			"movq (%0), %%mm0\n"
			"movq %%mm0, (%1)\n"
			:: "r" (from), "r" (to) : "memory");
		from = ((const unsigned char *)from) + 8;
		to = ((unsigned char *)to) + 8;
	}
	__asm__ __volatile__ ("emms":::"memory");
	return 0;
}

PupeInterruptThread::PupeInterruptThread(Pupe& pupe) : opupe(pupe){
}

void* PupeInterruptThread::function(){
	dprintf(DBG_THREADS, "InterruptThread Thread: %d\n", gettid());
	
	opupe.fpgaInterruptLoop();

	return 0;
}


Pupe::Pupe(Control& control, int board) : ocontrol(control), ointerruptThread(*this){
	osimulate = 0;
	omaster = 0;
	oboard = board;
	ofpgaCard = 0;
	ofpga = 0;
	ofpgaControl = 0;
	ocycleNumberNext = 0;
	ocycleNumber = 0;
	ocycleCompletedNumber = 0;
	opuCycleNumbers[0] = 0;
	opuCycleNumbers[1] = 0;
	opuCycleNumbers[2] = 0;
	opageSize = 2*1024*1024;
	odmaBufferSize = opageSize;
	odmaBuffer = new UInt64 [odmaBufferSize/sizeof(UInt64)];
	odmabuf = 0;

	memset(&opupeConfig, 0, sizeof(opupeConfig));
	opupeConfig.internalTiming = ocontrol.osimulateTiming;
	opupeConfig.internalCycleStop = ocontrol.osimulateCycleStop;

	ostatus.set(ErrorFpga, "Not initialised");

	ocycleCount = 0;
}

Pupe::~Pupe(){
	if(!osimulate){
		fpgaClose();
	}
}
			
BError Pupe::init(){
	BError	err;

	if(config.findValue("SimulateFpga:").retInt() == 1)
		osimulate = 1;

	olock.lock();
	if(!osimulate){
		if(oinitialised){
			fpgaClose();
			oinitialised = 0;
		}
		
		if( ! (err = fpgaInit())){
			oinitialised = 1;
		}

	}
	ostatus = err;
	olock.unlock();

	return err;
}

BError Pupe::status(){
	BError	err;
	
	olock.lock();
	err = ostatus.copy();
	olock.unlock();
	return err;
}

BError Pupe::setMaster(int on){
	BError	err;

	if(!osimulate){
		if(on)
			ofpga[TIMING_IO] = ofpga[TIMING_IO] | (1 << 29);
		else
			ofpga[TIMING_IO] = ofpga[TIMING_IO] & ~0xFF000000;
	}

	omaster = on;
	
	return err;
}

BError Pupe::setNextCycle(UInt32 cycleNumber, BString cycleType){
	BError	err;

	olock.lock();
	ocycleNumberNext = cycleNumber;
	olock.unlock();

	return err;
}

BError Pupe::setControlInfo(PuChannel puChannel, CycleParam params){
	BError		err;
	int		n;

	dprintf(DBG_PUPE, "Pupe::setControlInfo: %u.%u.%u\n", puChannel.moduleNum, puChannel.pupeNum, puChannel.pupeChan);
	if(ofpgaCard){
		olock.lock();
		if(puChannel.pupeChan == 0){
			for(n = 1; (n < numChan + 1) && !err; n++){
				puChannel.pupeChan = n;
				err = setControlInfoPickUp(puChannel, params);
			}
		}
		else if((puChannel.pupeChan > 0) && (puChannel.pupeChan < numChan + 1)){
			err = setControlInfoPickUp(puChannel, params);
		}
		else {
			olock.unlock();
			return err.set(ErrorParam, "Incorrect puChannel");
		}
		olock.unlock();
	}

	return err;
}

void Pupe::setNumBunches(PuChannel puChannel, CycleParam params){
	UInt32		s;
	UInt32		h;
	UInt32		p = puChannel.pupeChan - 1;
	TmsState	state;
	
	onumBunches[p].assign(9, 0);
	
	for(s = 0, h = 0; s < params.stateTable.size(); s++){
		state.value = params.stateTable[s].state;
		if(state.calStart < 14){
			onumBunches[p][0] = params.stateTable[state.calStart].numBunches;
		}
		else if(state.injection < 14){
			onumBunches[p][1 + h] = params.stateTable[state.injection].numBunches;
			h++;
		}
		else if(state.hchange < 14){
			onumBunches[p][1 + h] = params.stateTable[state.hchange].numBunches;
			h++;
		}
	}
}

BError Pupe::setControlInfoPickUp(PuChannel puChannel, CycleParam params){
	BError	err;
	UInt32	regBase = 0;
	UInt32	brBase = 0;
	UInt32	n;
	UInt32	logicalPuChannel = 1;
	UInt32	stateTable[14];

#if !ALL_CHANNELS
	if(puChannel.pupeChan != 1)
		return err;
#endif

	dprintf(DBG_FPGA, "Pupe::setControlInfoPickUp: PupeChan: %d Type: %s\n", puChannel.pupeChan, params.cycleType.retStr());
	switch(puChannel.pupeChan){
	case 1:	regBase = PU0_REG_BASE;	brBase = PU0_BRAM_BASE; break;
	case 2:	regBase = PU1_REG_BASE;	brBase = PU1_BRAM_BASE; break;
	case 3:	regBase = PU2_REG_BASE;	brBase = PU2_BRAM_BASE; break;
	default:	return err.set(ErrorFpga, "Incorrect puChannel");
	}

#ifdef ZAP
	if(puChannel.pupeChan == 1)
		dumpState();
#endif

	puChannel.moduleNum = ocontrol.moduleNum();
	if(ocontrol.getPuChannel(puChannel, logicalPuChannel))
		logicalPuChannel = 1;

#if DEBUG_CTRL
//params.pllGain = 10;
//params.frefPhaseDelay[logicalPuChannel - 1] = 128;
printf("pllInitialFrequencyDelay: %d\n", params.pllInitialFrequencyDelay);
printf("pllFrefGain: %d\n", params.pllFrefGain);
printf("pllGain: %d\n", params.pllGain);
printf("frefPhaseDelay: %d\n", params.frefPhaseDelay[logicalPuChannel - 1]);
printf("FrefMsbSource: %d\n", params.stateTable[0].state & 0x10);
#endif

	if(!osimulate){
		ofpga[regBase + PLL_PHASEDELAY]	= 128 + params.frefPhaseDelay[logicalPuChannel - 1];
		ofpga[regBase + PLL_FREQUENCY]	= params.pllInitialFrequency;
		ofpga[regBase + PLL_FREQDELAY]	= params.pllInitialFrequencyDelay;
		ofpga[regBase + PLL_GAIN]	= (params.pllGain << 16) | (params.pllFrefGain & 0xFFFF);
		ofpga[regBase + DDS_FREQ_MIN]	= params.pllDdsMinimum;
		ofpga[regBase + DDS_FREQ_MAX]	= params.pllDdsMaximum;
		
		// Write State and Phase Tables
		ofpga[MEM_REG] = SDRAM_BLOCKRAM;
		ofpga[IMEM_REG] = brBase + BRAM_SWITCH_TABLE;
		for(n = 0; n < 14; n++){
			if(n < params.stateTable.size())
				stateTable[n] = params.stateTable[n].state;
			else
				stateTable[n] = 0x0eeeee00;
		}
		fpgaWriteData64(stateTable, 0, sizeof(stateTable));

		ofpga[IMEM_REG] = brBase + BRAM_PHASE_TABLE;
		for(n = 0; (n < 14) && (n < params.stateTable.size()); n++){
			fpgaWriteData64(params.stateTable[n].phaseTable.data(), n * 512, 512);
		}
		
		if(params.pllDdsMinimum ||  params.pllDdsMaximum){
			if(! (ofpga[regBase + CONTROL] & CONTROL_DDS_FREQUENCY_LIMIT_ENABLE))
				ofpga[regBase + CONTROL] |= CONTROL_DDS_FREQUENCY_LIMIT_ENABLE;
		}
		else {
			if(ofpga[regBase + CONTROL] & CONTROL_DDS_FREQUENCY_LIMIT_ENABLE)
				ofpga[regBase + CONTROL] &= ~CONTROL_DDS_FREQUENCY_LIMIT_ENABLE;
		}

	}

	ocycleParam[puChannel.pupeChan - 1] = params;
	setNumBunches(puChannel, params);
	
#ifndef ZAP
	if(puChannel.pupeChan == 1)
		dumpState();
#endif

	dprintf(DBG_FPGA, "Pupe::setControlInfoPickUp: End\n");

	return err;
}

BError Pupe::getStatusList(PuChannel puChannel, BList<NameValue>& statusList){
	BError			err;
	BString			baseName = BString("TmsPuServer") + ocontrol.moduleNum() + "_Pupe" + oboard;
	BList<NameValue>	il;
	BIter			i;

	err = status();

	olock.lock();

	if(!err)
		statusList.append(NameValue(baseName + "_Running", "1"));
	else
		statusList.append(NameValue(baseName + "_Running", "0"));

	statusList.append(NameValue(baseName + "_Error", err.getString()));

	fpgaGetInfo(il);
	for(il.start(i); !il.isEnd(i); il.next(i)){
		statusList.append(NameValue(baseName + "_" + il[i].name, il[i].value));
	}
	olock.unlock();

	return err;
}

BError Pupe::getStatus(PuChannel puChannel, PuStatus& puStatus){
	BError	err;
	
	err = status();

	olock.lock();
	if(!err)
		puStatus.running = 1;
	else
		puStatus.running = 0;
	puStatus.error = err;
	olock.unlock();

	return err;
}

BError Pupe::checkData(UInt32 cycleNumber){
	BError	err;
	Int32	diff;
	
	// In the initial state we need to check if we have actually captured the cycle
	// We also need to check here when the cycle number wraps.

	// First check if we are within the possible cycle range
	diff = cycleNumber - ocycleCompletedNumber;
	if(diff < -4)
		return err.set(ErrorDataGone, "Cycle's data has already been overwritten");
	if(diff > 0)
		return err.set(ErrorDataFuture, "Cycle data required is to far in the future");
		
	return err;
}


BError Pupe::getData(PuChannel puChannel, DataInfo dataInfo, Data& data){
	BError			err;
	int			regBase;
	int			brBase;
	uint32_t		cycleTimingAddress = 0;
	int32_t			diff;
	uint64_t		cycleTimingEntry;
	uint32_t		cycleDataAddress;
	int			i;
	int			e;
	int			ph = 0;
	int			h;
	int			found = 0;
	int			nbunchesPerOrbit = 0;
	uint32_t		pupeCycleNumber;
	int			timeOut;
	
	dprintf(DBG_FPGA, "Pupe::getData: RequiredCycleNumber: %u CurrentCycleNumber: %u LastCompletedCycle: %u\n", dataInfo.cycleNumber, ocycleNumber, ocycleCompletedNumber);

	switch(puChannel.pupeChan){
	case 1:	regBase = PU0_REG_BASE;	brBase = PU0_BRAM_BASE; break;
	case 2:	regBase = PU1_REG_BASE;	brBase = PU1_BRAM_BASE; break;
	case 3:	regBase = PU2_REG_BASE;	brBase = PU2_BRAM_BASE; break;
	default:	return err.set(ErrorParam, "Incorrect puChan");
	}

	olock.lock();

	// Return an error if already gone or to far in the future
	if(err = checkData(dataInfo.cycleNumber)){
		olock.unlock();
		return err;
	}

	if(osimulate){
		err = getSimData(dataInfo, data);
	}
	else {
		data.dataValues.resize(dataInfo.numValues);
		data.numValues = dataInfo.numValues;

		// Get Cycle information
		ofpga[MEM_REG] = SDRAM_BLOCKRAM;

		// Need to test if the cycle number has changed while we read the following two data tables
		timeOut = 1;
		do {
			pupeCycleNumber = ofpga[PU0_REG_BASE + CYCLE];
			ofpga[IMEM_REG] = brBase + BRAM_CYCLE_INFO;
			fpgaReadData64(0, ocycleInfoTable, sizeof(ocycleInfoTable));

			ofpga[IMEM_REG] = brBase + BRAM_CYCLE_TIMING;
			fpgaReadData64(0, ocycleTimingTable, sizeof(ocycleTimingTable));
		} while(timeOut-- && (pupeCycleNumber != ofpga[PU0_REG_BASE + CYCLE]));
		
		if(timeOut < 0){
			olock.unlock();
			return err.set(ErrorFpga, "Timed out while getting Cycle Information tables");
		}
		
		// Return an error if already gone or to far in the future
		if(err = checkData(dataInfo.cycleNumber)){
			olock.unlock();
			return err;
		}
		
		// Calculate actual Pupe Cycle number required
		diff = ocycleNumber - dataInfo.cycleNumber;
		pupeCycleNumber = opuCycleNumbers[puChannel.pupeChan - 1] - diff;

#if DEBUG_DATA		
		printf("CycleWanted: %u CurrentCycle: %u Diff: %d PupeCycle: %u CurrentPupeCycle: %u Period: %d\n", dataInfo.cycleNumber, ocycleNumber, diff,pupeCycleNumber,  ofpga[PU0_REG_BASE + CYCLE], dataInfo.cyclePeriod); 
#endif

		if(dataInfo.cyclePeriod == CyclePeriodStart){
		}
		else if(dataInfo.cyclePeriod == CyclePeriodCalibration){
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic0){
			ph = 0;
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic1){
			ph = 1;
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic2){
			ph = 2;
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic3){
			ph = 3;
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic4){
			ph = 4;
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic5){
			ph = 5;
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic6){
			ph = 6;
		}
		else if(dataInfo.cyclePeriod == CyclePeriodHarmonic7){
			ph = 7;
		}
		else {
			err.set(ErrorParam, "Incorrect cycle period");
			olock.unlock();
			return err;
		}

		nbunchesPerOrbit = onumBunches[puChannel.pupeChan-1][dataInfo.cyclePeriod];

#if DEBUG_DATA
		printf("Number of bunches: %d\n", nbunchesPerOrbit);
		printf("CycleInfoTable:\n");
		hd32(ocycleInfoTable, 4 * 16);
#endif
		
		if(dataInfo.cyclePeriod == CyclePeriodStart){
			// Get address of Cycle Start
			cycleTimingAddress = ocycleInfoTable[((pupeCycleNumber & 0x03) << 4) + 0] & 0x00FFFFFF;
			
			if((ocycleInfoTable[((pupeCycleNumber & 0x03) << 4) + 0] >> 24) != 0x01){
				eprintf("CycleInformationTable Error\n");
				hd32(ocycleInfoTable, 4 * 16);
			}

			// Offset by start time and orbit number
			cycleTimingAddress += dataInfo.startTime;
			cycleTimingAddress += (nbunchesPerOrbit * dataInfo.orbitNumber);
		}
		else {		
			// Find CyclePeriod Entry in Cycle Information table
			for(i = 0, h = 0, found = 0; i < 16; i++){
				e = ocycleInfoTable[((pupeCycleNumber & 0x03) << 4) + i] >> 24;
				if((dataInfo.cyclePeriod == CyclePeriodCalibration) && (e & 0x04)){
					cycleTimingAddress = ocycleInfoTable[((pupeCycleNumber & 0x03) << 4) + i] & 0x00FFFFFF;
					found = 1;
					break;
				}
				else if((dataInfo.cyclePeriod == CyclePeriodHarmonic0) && (e & 0x10) && (h == ph)){
					cycleTimingAddress = ocycleInfoTable[((pupeCycleNumber & 0x03) << 4) + i] & 0x00FFFFFF;
					found = 1;
					break;
				}
				else if((dataInfo.cyclePeriod > CyclePeriodHarmonic0) && (e & 0x20) && (h == ph)){
					cycleTimingAddress = ocycleInfoTable[((pupeCycleNumber & 0x03) << 4) + i] & 0x00FFFFFF;
					found = 1;
					break;
				}

				if((e & 0x10) || (e & 0x20))
					h++;
			}
			if(!found){
				err.set(ErrorDataNotAvailable, "The data required could not be found");
#if DEBUG_DATA
				printf("CycleInfoTable: Period: %d\n", dataInfo.cyclePeriod);
				hd32(ocycleInfoTable, 4 * 16);
#endif
				olock.unlock();
				return err;
			}
			
			// Offset by start time and orbit number
			cycleTimingAddress += dataInfo.startTime;
			cycleTimingAddress += (nbunchesPerOrbit * dataInfo.orbitNumber);
		}

		// Get Cycle Data Table Address
		cycleTimingAddress &= FpgaCycleTimingTableMask;
		cycleTimingEntry = ocycleTimingTable[cycleTimingAddress];
		cycleDataAddress = cycleTimingEntry >> 32;

#if DEBUG_DATA
		printf("CycleTimingAddress: %u(%x) CycleTime: %u CycleDataAddress: %u(%x)\n", cycleTimingAddress, cycleTimingAddress, uint32_t(cycleTimingEntry & 0xFFFFFFFF), cycleDataAddress, cycleDataAddress);
		printf("CycleTimingTable:\n");
		hd64(&ocycleTimingTable[cycleTimingAddress], 16);
#endif
		
		if(dataInfo.function == DataFunctionRaw){		
			if(dataInfo.bunchNumber){
				ofpga[MEM_RAO] = dataInfo.bunchNumber - 1;
				ofpga[MEM_RAS] = nbunchesPerOrbit;
			}
			else {
				ofpga[MEM_RAO] = 0;
				ofpga[MEM_RAS] = 1;
			}

			// Read the Data
			err = fpgaCopyData(puChannel.pupeChan, cycleDataAddress * 8, dataInfo.numValues, data);
		}
		else if(dataInfo.function == DataFunctionMean0){
			ofpga[IMEM_REG] = brBase + BRAM_BUNCH_MEAN0;
			fpgaReadData64(cycleTimingAddress * sizeof(uint64_t), data.dataValues.data(), dataInfo.numValues * sizeof(uint64_t));
		}
		else if(dataInfo.function == DataFunctionMean1){
			ofpga[IMEM_REG] = brBase + BRAM_BUNCH_MEAN1;
			fpgaReadData64(cycleTimingAddress * sizeof(uint64_t), data.dataValues.data(), dataInfo.numValues * sizeof(uint64_t));
		}
		else {
			err.set(ErrorNotImplemented, BString("Data function: ") + dataInfo.function + " has not been implemented");
		}

#if DEBUG_DATA
		printf("CycleDataTable: %d\n", data.dataValues.size());
		hd64(data.dataValues.data(), 16);
#endif

		if(!err){
			// Need to check the FPGA has not overwritten the data while we were reading it.
#ifdef ZAP
			printf("CheckTime: cycleTimingEntry: %u TIME_TBLADDR: %u\n", (UInt32)(cycleTimingEntry >> 32), ofpga[regBase + TIME_TBLADDR]);
#endif
			diff = cycleDataAddress - ofpga[regBase + TIME_TBLADDR];
			if(diff <= 0)
				err.set(ErrorDataGone, "Data has already gone");
		}
	}
	olock.unlock();
	
	return err;
}

BError Pupe::getSimData(DataInfo dataInfo, Data& data){
	BError		err;
	SigGenBeam	sigBeam;
	UInt32		i;
	Sample*		samples;
	static Data	dataCache;
	
	if(dataCache.dataValues.size() != dataInfo.numValues){
		samples = new Sample[dataInfo.numValues];
		sigBeam.config(125000000, 437000, 8, 0x3C, 0.0, 0, 1.0);
		sigBeam.generateIntegrated(samples, dataInfo.numValues);

		dataCache.numValues = dataInfo.numValues;
		dataCache.dataType = 0;
		dataCache.dataValues.resize(dataInfo.numValues);

		for(i = 0; i < dataInfo.numValues; i++){
			dataCache.dataValues[i].sigma = Int16(32767.0 * samples[i]);
			dataCache.dataValues[i].deltaX = Int16(0.2 * 32767.0 * samples[i]);
			dataCache.dataValues[i].deltaY = Int16(-0.1 * 32767.0 * samples[i]);
		}

		delete [] samples;
	}
	
	data = dataCache;

	return err;
}

BError Pupe::setTestMode(PuChannel puChannel, UInt32 testOutput, UInt32 timingDisableMask){
	BError	err;
	int	regBase = 0;
	int	brBase = 0;
	
	if(osimulate){
		err.set(ErrorNotImplemented, "Not Implemented");
	}
	else {
		switch(puChannel.pupeChan){
		case 1:	regBase = PU0_REG_BASE;	brBase = PU0_BRAM_BASE; break;
		case 2:	regBase = PU1_REG_BASE;	brBase = PU1_BRAM_BASE; break;
		case 3:	regBase = PU2_REG_BASE;	brBase = PU2_BRAM_BASE; break;
		default:	return err.set(ErrorFpga, "Incorrect puChannel");
		}

		olock.lock();
		ofpga[TIMING_IO] = (ofpga[TIMING_IO] & ~0xFF00) | ((timingDisableMask & 0xFF) << 8);
		olock.unlock();
	}
	
	return err;
}

BError Pupe::setTimingSignals(PuChannel puChannel, UInt32 timingSignals){
	BError	err;
	
	if(osimulate){
		err.set(ErrorNotImplemented, "Not Implemented");
	}
	else {
		olock.lock();
		ofpga[TIMING_IO] = (ofpga[TIMING_IO] & ~0xFF) | (timingSignals & 0xFF);
		olock.unlock();
	}
	
	return err;
}

BError Pupe::captureTestData(PuChannel puChannel, TestCaptureInfo captureInfo, BArray<UInt64>& data){
	BError		err;
	int		regBase = 0;
	int		brBase = 0;
	UInt32		v = 0;
	int		num = 1024;
	int		timeOut;
	
	dprintf(DBG_FPGA, "Pupe::captureTestData\n");

#ifdef ZAP
//ofpga[TIMING_IO] = (1 << 29) | 0x8100;
//ofpga[TIMING_IO] = (1 << 29) | 0x0100;
ofpga[TIMING_IO] = (1 << 29) | 0x0100;
//ofpga[TIMING_IO] = (1 << 29) | 0x8100;
#endif

	// Check Parameters
	if(captureInfo.startTime && captureInfo.postTriggerDelay)
		return err.set(ErrorParam, "Parameter error: Can only have startTime or postTriggerDelay not both");
		
	if(osimulate){
		err.set(ErrorNotImplemented, "Not Implemented");
	}
	else {
		switch(puChannel.pupeChan){
		case 1:	regBase = PU0_REG_BASE;	brBase = PU0_BRAM_BASE; break;
		case 2:	regBase = PU1_REG_BASE;	brBase = PU1_BRAM_BASE; break;
		case 3:	regBase = PU2_REG_BASE;	brBase = PU2_BRAM_BASE; break;
		default:	return err.set(ErrorFpga, "Incorrect puChannel");
		}

printf("TIMING_IO: %x\n", ofpga[TIMING_IO]);
printf("TEST: %x\n", ofpga[regBase + TEST]);
printf("ADC: %x\n", ofpga[ADC]);

		odiagLock.lock();

		v |= ((0x1) << 0);
		v |= ((captureInfo.source & 0x03) << 2);
		v |= ((captureInfo.triggerStore & 0x01) << 4);
		v |= ((captureInfo.triggerAnd & 0x1) << 5);
		v |= ((captureInfo.triggerSourceData & 0x1) << 6);
		v |= ((captureInfo.clock & 0x1F) << 8);
		
		if(captureInfo.startTime){
			v |= ((captureInfo.startTime & 0xFFFF) << 16);
			v |= ((0x1) << 7);
		}
		else {
			v |= ((captureInfo.postTriggerDelay & 0xFFFF) << 16);
		}

		ofpga[regBase + DIAG_TRIGGER]	= captureInfo.triggerMask;
		ofpga[regBase + DIAG_CTRL]	= v;

		timeOut = 2000;
		while((ofpga[regBase + DIAG_CTRL] & 0x01) && timeOut--){
			usleep(1000);
		}
		if(timeOut <= 0){
			odiagLock.unlock();
			return err.set(ErrorTimeout, "Diagnostics capture timed out");
		}

		olock.lock();
		ofpga[MEM_REG] = SDRAM_BLOCKRAM;
		ofpga[IMEM_REG] = brBase + BRAM_DIAG_TABLE;
		
		data.resize(num);

		// Fetch data
		fpgaReadData64(0, data.data(), data.size() * sizeof(UInt64));

		olock.unlock();
		odiagLock.unlock();
	}
	
	return err;
}

BError Pupe::setTestData(PuChannel puChannel, Int32 on, BArray<UInt32> data){
	BError		err;
	int		regBase = 0;
	int		brBase = 0;
	BArray<UInt32>	rdata;
	uint32_t	i;
	
	dprintf(DBG_FPGA, "Pupe::setTestData: Len: %d\n", data.size());

	switch(puChannel.pupeChan){
	case 1:	regBase = PU0_REG_BASE;	brBase = PU0_BRAM_BASE; break;
	case 2:	regBase = PU1_REG_BASE;	brBase = PU1_BRAM_BASE; break;
	case 3:	regBase = PU2_REG_BASE;	brBase = PU2_BRAM_BASE; break;
	default:	return err.set(ErrorFpga, "Incorrect puChannel");
	}

	if(data.size() > (opageSize / sizeof(UInt32))){
		return err.set(ErrorParam, "Data length is to long");
	}
	if(data.size() % 1){
		// Round down to a multiple of 64bits
		data.resize(data.size() - 1);
	}

	if(osimulate){
		err.set(ErrorNotImplemented, "Not Implemented");
	}
	else {
		olock.lock();

		if(on){
			ofpga[TESTCTRL] = 0;
			ofpga[MEM_RAO] = 0;
			ofpga[MEM_RAS] = 1;
			ofpga[MEM_REG] = SDRAM_DATA_TEST;
			ofpga[ADDR_REG] = 0;
			

			// Delay needed so that TEST DATA engine has released access to SDRAM
			usleep(1000);

			// Write to test data SDRAM
			fpgaWriteData64(data.data(), 0x0, data.size() * sizeof(UInt32));

			ofpga[MEM_REG] = 0;

#if TEST_TESTDATA
			// Check data read
			ofpga[MEM_REG] = SDRAM_DATA_TEST;
			ofpga[ADDR_REG] = 0;
	
			rdata.resize(data.size());
			fpgaReadData64(0x0, rdata.data(), rdata.size() * sizeof(UInt32));
	
			printf("\n\n####### TEST DATA TEST ############\n");
			for(i = 0; i < data.size(); i++){
				if(data[i] != rdata[i]){
					printf("ERROR TestData ERROR: %d %x %x\n", i, data[i], rdata[i]);
					olock.unlock();
					return err.set(ErrorFpga, "Error setting test data");
				}
			}
#endif
			ofpga[TESTLEN] = data.size() / 2;
			ofpga[TESTCTRL] = (1 << 25);
			
			switch(puChannel.pupeChan){
			case 1:		ofpga[ADC] &= ~0x07;	break;
			case 2:		ofpga[ADC] &= ~0x38;	break;
			case 3:		ofpga[ADC] &= ~0x1C;	break;
			}

			ofpga[regBase + TEST] = ofpga[regBase + TEST] | 0x8000;
		}
		else {
			ofpga[regBase + TEST] = ofpga[regBase + TEST] & ~0x8000;

			switch(puChannel.pupeChan){
			case 1:		ofpga[ADC] |= 0x07;	break;
			case 2:		ofpga[ADC] |= 0x38;	break;
			case 3:		ofpga[ADC] |= 0x1C;	break;
			}

			ofpga[TESTCTRL] = 0;
		}

		olock.unlock();
	}
	
	return err;
}

BError Pupe::setPupeConfig(PuChannel puChannel, PupeConfig pupeConfig){
	BError		err;
	int		regBase = 0;
	int		brBase = 0;

	dprintf(DBG_FPGA, "Pupe::setPupeConfig: %d,%d\n", pupeConfig.internalAdcClock, pupeConfig.internalTiming);

	switch(puChannel.pupeChan){
	case 1:	regBase = PU0_REG_BASE;	brBase = PU0_BRAM_BASE; break;
	case 2:	regBase = PU1_REG_BASE;	brBase = PU1_BRAM_BASE; break;
	case 3:	regBase = PU2_REG_BASE;	brBase = PU2_BRAM_BASE; break;
	default:	return err.set(ErrorFpga, "Incorrect puChannel");
	}
	
	opupeConfig = pupeConfig;

	// Enable timing bus simulation
	if(opupeConfig.internalTiming){
		ofpga[TIMING_IO] = ofpga[TIMING_IO] | 0xFF00;
	}
	else if(opupeConfig.internalCycleStop){
		ofpga[TIMING_IO] = ofpga[TIMING_IO] | (TimingSigCycleStop << 8);
	}
	else {
		ofpga[TIMING_IO] = ofpga[TIMING_IO] & ~0xFF000;
	}
	
	// Should setup ADC Clock	

	return err;
}


void Pupe::cycleStart(){
	if(osimulate){
		dprintf(DBG_FPGA, "Pupe::cycleStart\n");
	}
	else {
		dprintf(DBG_FPGA, "Pupe::cycleStart: PupeCycle: %u Ctime: %u\n", ofpga[PU0_REG_BASE + CYCLE], ofpga[PU0_REG_BASE + TIME]);
	}

	olock.lock();
	ocycleNumber = ocycleNumberNext;
	if(osimulate){
		opuCycleNumbers[0]++;
		opuCycleNumbers[1]++;
		opuCycleNumbers[2]++;
	}
	else {
		opuCycleNumbers[0] = ofpga[PU0_REG_BASE + CYCLE];
		opuCycleNumbers[1] = ofpga[PU1_REG_BASE + CYCLE];
		opuCycleNumbers[2] = ofpga[PU2_REG_BASE + CYCLE];
	}
	olock.unlock();

	// Only master should send this
	if(omaster)
		ocontrol.cycleStart(ocycleNumber);
}

void Pupe::cycleStop(){
	UInt32	currentCycle;

	olock.lock();
	currentCycle = ocycleNumber;
	ocycleCompletedNumber = ocycleNumber;
	olock.unlock();

	if(osimulate){
		dprintf(DBG_FPGA, "Pupe::cycleStop\n");
	}
	else {
		dprintf(DBG_FPGA, "Pupe::cycleStop: PupeCycle: %u Ctime: %u\n", ofpga[PU0_REG_BASE + CYCLE], ofpga[PU0_REG_BASE + TIME]);
	}

	// Only master should send this
	if(omaster)
		ocontrol.cycleStop(currentCycle);

	dprintf(DBG_FPGA, "Pupe::cycleStop: %d %d %d %d\n", ocycleNumber, opuCycleNumbers[0], opuCycleNumbers[1], opuCycleNumbers[2]);
}

void Pupe::cycleError(int puChan){
	dprintf(DBG_FPGA, "Pupe::cycleError: %d\n", puChan);

	olock.lock();
	
	// Clear the error
	ocontrol.cycleError(ocycleNumber, BError(ErrorStateTable, "FPGA State table transition error"));

	eprintf("Error: FPGA State table transition error\n");
#ifdef ZAP
	exit(1);
#endif

	olock.unlock();
}

/*******************************************************************************
 *	FPGA access functions
 *******************************************************************************
 */

void Pupe::dumpState(){
	int	regBase = 0;
	int	brBase = 0;
	UInt32	switchTable[14];
	UInt8	phaseTables[16][512];
	int	s;
	int	n;
	int	pupeChan = 1;
	
	switch(pupeChan){
	case 1:	regBase = PU0_REG_BASE;	brBase = PU0_BRAM_BASE; break;
	case 2:	regBase = PU1_REG_BASE;	brBase = PU1_BRAM_BASE; break;
	case 3:	regBase = PU2_REG_BASE;	brBase = PU2_BRAM_BASE; break;
	}

	printf("CycleNumber:            %u\n", ocycleNumber);
	printf("CycleCompletedNumber:   %u\n", ocycleCompletedNumber);
	printf("CycleType:              %u\n", ocycleNumber);

	if(!osimulate){
		ofpga[MEM_REG] = SDRAM_BLOCKRAM;
		ofpga[IMEM_REG] = brBase + BRAM_SWITCH_TABLE;
		fpgaReadData64(0, switchTable, sizeof(switchTable));

		ofpga[IMEM_REG] = brBase + BRAM_PHASE_TABLE;
		fpgaReadData64(0, phaseTables, sizeof(phaseTables));

		printf("CONTROL:	%x\n", ofpga[regBase + CONTROL]);
		printf("PLL_PHASEDELAY:	%d\n", ofpga[regBase + PLL_PHASEDELAY]);
		printf("PLL_FREQUENCY:	%d\n", ofpga[regBase + PLL_FREQUENCY]);
		printf("PLL_FREQDELAY:	%d\n", ofpga[regBase + PLL_FREQDELAY]);
		printf("PLL_GAIN:	%d\n", ofpga[regBase + PLL_GAIN]);
		printf("DDS_FREQ_MIN:	%d\n", ofpga[regBase + DDS_FREQ_MIN]);
		printf("DDS_FREQ_MAX:	%d\n", ofpga[regBase + DDS_FREQ_MAX]);

		printf("SwitchTable:\n");
		for(n = 0; n < 14; n++){
			printf("%4.4x, ", switchTable[n]);
		}
		printf("\n");

		printf("PhaseTables:\n");
		for(s = 0; s < 14; s++){
			for(n = 0; n < 16; n++){
				printf("%2.2x,", phaseTables[s][n]);
			}
			printf("\n");
		}
	}
}

BError Pupe::fpgaInit(){
	BError			err;
	ADMXRC2_STATUS		status;
	BString			s;
	double			lclk = 50.0e6;
	double			mclk = 125.0e6;

	dprintf(DBG_FPGA, "Pupe::fpgaInit\n");
	s = config.findValue("FpgaLclk:");
	if(s != "")
		lclk = atof(s) * 1e6;

	s = config.findValue("FpgaMclk:");
	if(s != "")
		mclk = atof(s) * 1e6;

	nprintf("Pupe: Setting LClk: %f Mhz, MClk: %f MHz\n", lclk/1e6, mclk/1e6);

	// Open FPGA Card	
	if((status = ADMXRC2_OpenCard(oboard, &ofpgaCard)) != ADMXRC2_SUCCESS){
		err.set(Tms::ErrorFpga, BString("FpgaError: ") + ADMXRC2_GetStatusString(status));
		return err;
	}
	
	if((status = ADMXRC2_GetCardInfo(ofpgaCard, &ocardInfo)) != ADMXRC2_SUCCESS){
		err.set(Tms::ErrorFpga, BString("FpgaError: ") + ADMXRC2_GetStatusString(status));
		return err;
	}

	if((status = ADMXRC2_SetClockRate(ofpgaCard, 0, lclk, NULL)) != ADMXRC2_SUCCESS){
		err.set(Tms::ErrorFpga, BString("FpgaError: ") + ADMXRC2_GetStatusString(status));
		return err;
	}
	if((status = ADMXRC2_SetClockRate(ofpgaCard, 1, mclk, NULL)) != ADMXRC2_SUCCESS){
		err.set(Tms::ErrorFpga, BString("FpgaError: ") + ADMXRC2_GetStatusString(status));
		return err;
	}
	
	if(err = fpgaLoad())
		return err;

	return fpgaConfig();	
}

BError Pupe::fpgaLoad(){
	BError			err;
	ADMXRC2_STATUS		status;
	ADMXRC2_SPACE_INFO	spInfo;
	BString			fileName = "/usr/tms/fpga/tms-fpga.bit";
	BString			s;
	uint32_t		v;

	dprintf(DBG_FPGA, "Pupe::fpgaLoad\n");
	s = config.findValue("FpgaFirmwareFile:");
	if(s != "")
		fileName = s;

	// Load FPGA
	if(!access(fileName, R_OK)){
		// Try to load file.
		if((status = ADMXRC2_ConfigureFromFile(ofpgaCard, fileName)) != ADMXRC2_SUCCESS){
			return err.set(Tms::ErrorFpga, BString("FpgaError: ") + ADMXRC2_GetStatusString(status));
		}
		else {
			usleep(500000);
		}
	}
	else {
		return err.set(Tms::ErrorFpga, BString("FpgaError: File: ") + fileName + " not found");
	}

	if((status = ADMXRC2_GetSpaceInfo(ofpgaCard, 0, &spInfo)) != ADMXRC2_SUCCESS){
		err.set(Tms::ErrorFpga, BString("FpgaError: ") + ADMXRC2_GetStatusString(status));
		return err;
	}
	ofpga = (volatile uint32_t*)spInfo.VirtualBase;

	if((status = ADMXRC2_GetSpaceInfo(ofpgaCard, 1, &spInfo)) != ADMXRC2_SUCCESS){
		err.set(Tms::ErrorFpga, BString("FpgaError: ") + ADMXRC2_GetStatusString(status));
		return err;
	}
	ofpgaControl = (volatile uint32_t*)spInfo.VirtualBase;

	v = ofpga[FIRMWARE];
	if((((v >> 24) & 0xFF) != 'C') || (((v >> 16) & 0xFF) != 'N')){
		err.set(ErrorFpga, "FPGA Firmware version incorrect");
	}
	else {
		nprintf("Pupe: FpgaVersion: %d.%d\n", ((v >> 8) & 0xFF), (v & 0xFF));
	}

	return err;
}

BError Pupe::fpgaInterruptLoop(){
	BError	err;
	uint	i;
	
	while(1){
		dprintf(DBG_INTERRUPT, "Pupe::fpgaInterruptLoop\n");
		ADMXRC2_WaitForInterrupt(ofpgaCard, 0, 0, 0);
		dprintf(DBG_INTERRUPT, "Interrupt: %x !!\n", ofpga[ISR]);
		
		// Check for interrupts
		i = ofpga[ISR];

		if(i & INT_PU0_ERROR)
			cycleError(0);
		if(i & INT_PU1_ERROR)
			cycleError(1);
		if(i & INT_PU2_ERROR)
			cycleError(2);
		if(i & INT_PU0_CYCLE_STOP)
			cycleStop();
		if(i & INT_PU0_CYCLE_START)
			cycleStart();

		if(i & ~(INT_PU0_CYCLE_START|INT_PU0_CYCLE_STOP|INT_PU0_ERROR|INT_PU1_ERROR|INT_PU2_ERROR)){
			nprintf("Unknown interrupt: %x\n", i);
		}
				
		// Clear all interrupts
		ofpga[ISR] = 0xFFFFFFFF;
		ofpga[ISR] = 0x0;
	}
	
	return err;
}

void Pupe::fpgaTriggerTimingInputs(TimingSig input){
	dprintf(DBG_FPGA, "Pupe::fpgaTriggerTimingInput: %d\n", input);

	if(ofpga){
		dprintf(DBG_FPGA_TIMING, "Pupe::Trigger: %x PupeCycle: %u Ctime: %u\n", input, ofpga[PU0_REG_BASE + CYCLE], ofpga[PU0_REG_BASE + TIME]);
		ofpga[TIMING_IO] = ofpga[TIMING_IO] | (input & 0xFF);
		usleep(100);
		ofpga[TIMING_IO] = (ofpga[TIMING_IO] & ~0xFF);
	}

#ifdef ZAP
	// Warning this code is not thread safe
	if(input & 0x04){
		uint32_t	add;
		uint32_t	cycle;
		uint32_t	dataAdd;
		uint64_t	data[64];
		
		ofpga[MEM_REG] = SDRAM_BLOCKRAM;
		ofpga[IMEM_REG] = PU0_BRAM_BASE + BRAM_CYCLE_INFO;
		fpgaReadData64(0, ocycleInfoTable, sizeof(ocycleInfoTable));

		printf("TriggerCycleStop: CycleInfoTable:\n");
		hd32(ocycleInfoTable, 4 * 16);

		ofpga[MEM_REG] = SDRAM_BLOCKRAM;
		ofpga[IMEM_REG] = PU0_BRAM_BASE + BRAM_CYCLE_TIMING;
		fpgaReadData64(0, ocycleTimingTable, sizeof(ocycleTimingTable));

		cycle = ofpga[PU0_REG_BASE + CYCLE] - 1;
		add = ocycleInfoTable[(cycle << 4) + 1] & 0x00FFFFFF;
		
		printf("CycleTimingTable Cycle: %u Starting at: %x:\n", cycle, add);
		hd32(&ocycleTimingTable[add], 8 * 16);

		dataAdd = ocycleTimingTable[add] >> 32;
		printf("CycleDataTable Starting at: %x (%x Bytes)\n", dataAdd, dataAdd * 8);
		ofpga[MEM_RAS] = 1;
		ofpga[MEM_RAO] = 0;
		ofpga[MEM_REG] = SDRAM_DATA_PU0;
		ofpga[ADDR_REG] = 0;
		fpgaReadData64(dataAdd * 8, data, sizeof(data));
		hd32(data, 128);

		ofpga[MEM_REG] = SDRAM_DATA_PU1;
		ofpga[ADDR_REG] = 0;
		fpgaReadData64(dataAdd * 8, data, sizeof(data));
		hd32(data, 128);

		ofpga[MEM_REG] = SDRAM_DATA_PU2;
		ofpga[ADDR_REG] = 0;
		fpgaReadData64(dataAdd * 8, data, sizeof(data));
		hd32(data, 128);

	}
#endif
	
	if(input & 0x04)
		ocycleCount++;
	
}

BError Pupe::fpgaConfig(){
	BError			err;
	ADMXRC2_STATUS		status;
	UInt32			n;
	UInt32			stateTable[14];

	dprintf(DBG_FPGA, "Pupe::fpgaConfig:\n");

	status = ADMXRC2_SetupDMA(ofpgaCard, odmaBuffer, odmaBufferSize, 0, &odmabuf);
	if(status != ADMXRC2_SUCCESS){
		err.set(Tms::ErrorFpga, "Failed to set up DMA buffer");
		return err;
	}

#ifdef ZAP	
	// Reset ADC PLL clock source
	ofpga[ADC] = 0x200 | 0x1FF;
	usleep(1000);
	ofpga[ADC] = 0x000 | 0x1FF;
	usleep(10000);
#endif

#ifdef ZAP
	{
		int	data;
		
		printf("Resetting Card\n");
		data = 1;
		ADMXRC2_Write(ofpgaCard, ADMXRC2_IOWIDTH_32, ADMXRC2_IOADAPTER, 0x28, &data, 4);
		usleep(10000);
		data = 0;
		ADMXRC2_Write(ofpgaCard, ADMXRC2_IOWIDTH_32, ADMXRC2_IOADAPTER, 0x28, &data, 4);
		sleep(1);
	}
#endif
	ofpga[ADC] = 0x1FF;

	// Set State tables so that all paths lead to cycle stop
	for(n = 0; n < 14; n++){
		stateTable[n] = 0x0fffff00;
	}
	ofpga[MEM_REG] = SDRAM_BLOCKRAM;
	ofpga[IMEM_REG] = PU0_REG_BASE + BRAM_SWITCH_TABLE;
	fpgaWriteData64(stateTable, 0, sizeof(stateTable));
	ofpga[IMEM_REG] = PU1_REG_BASE + BRAM_SWITCH_TABLE;
	fpgaWriteData64(stateTable, 0, sizeof(stateTable));
	ofpga[IMEM_REG] = PU2_REG_BASE + BRAM_SWITCH_TABLE;
	fpgaWriteData64(stateTable, 0, sizeof(stateTable));

	// Reset PLL's
	ofpga[PU0_REG_BASE + CONTROL] = CONTROL_INIT;
	ofpga[PU1_REG_BASE + CONTROL] = CONTROL_INIT;
	ofpga[PU2_REG_BASE + CONTROL] = CONTROL_INIT;
	usleep(1000);
	ofpga[PU0_REG_BASE + CONTROL] = 0;
	ofpga[PU1_REG_BASE + CONTROL] = 0;
	ofpga[PU2_REG_BASE + CONTROL] = 0;

	// Setup Interrupt handler
	ointerruptThread.start();

#if ALL_CHANNELS	
	ofpga[IER] = INT_PU0_CYCLE_START | INT_PU0_CYCLE_STOP | INT_PU0_ERROR | INT_PU1_ERROR | INT_PU2_ERROR;
#else
	ofpga[IER] = INT_PU0_CYCLE_START | INT_PU0_CYCLE_STOP | INT_PU0_ERROR;
#endif

	// Enable timing bus simulation
	if(opupeConfig.internalTiming){
		ofpga[TIMING_IO] = 0xFF00;
	}
	else if(opupeConfig.internalCycleStop){
		ofpga[TIMING_IO] = (TimingSigCycleStop << 8);
	}
	else {
		ofpga[TIMING_IO] = 0x0000;
	}
#ifndef ZAP
	ofpga[TIMING_IO] |= 0x0100;			// Force internal SysClock
//	ofpga[TIMING_IO] |= 0x1900;
//	ofpga[TIMING_IO] = 0x7F00;
	printf("TIMING_IO: %x\n", ofpga[TIMING_IO]);
#endif
	dprintf(DBG_FPGA, "TIMING_IO: %x\n", ofpga[TIMING_IO]);
	
	ofpga[PU0_REG_BASE + CYCLE] = 0;
	ofpga[PU1_REG_BASE + CYCLE] = 0;
	ofpga[PU2_REG_BASE + CYCLE] = 0;

#ifdef ZAP
{
	uint8_t		data[2*1024*1024];
	uint32_t	p = 0;
	
	printf("Clear memory\n");
	memset(data, 0x55, sizeof(data));
	
	ofpga[MEM_REG] = SDRAM_DATA_PU0;
	for(p = 0; p < 1; p++){
		ofpga[ADDR_REG] = p << 19;
		fpgaWriteData64(data, 0, sizeof(data));
	}
	ofpga[MEM_REG] = SDRAM_DATA_PU1;
	for(p = 0; p < 1; p++){
		ofpga[ADDR_REG] = p << 19;
		fpgaWriteData64(data, 0, sizeof(data));
	}
	ofpga[MEM_REG] = SDRAM_DATA_PU2;
	for(p = 0; p < 1; p++){
		ofpga[ADDR_REG] = p << 19;
		fpgaWriteData64(data, 0, sizeof(data));
	}
	printf("Clear memory: Done\n");
}
#endif

#ifdef ZAP
{
	BArray<UInt32>	data;
	BFile		file;
	BString		fileName = "../datasrc/beam3-437000-8.psd";
	int		nb;
	
	printf("Load Test Data\n");
	
	if(err = file.open(fileName, "r")){
		printf("Error: Opening file:\n");
	}
	data.resize(1024);
		
	if((nb = file.read(&data[0], 1024 * sizeof(UInt32))) <= 0){
		printf("Error: Reading file:\n");
	}
	data.resize(nb / sizeof(UInt32));
	olock.unlock();
	setTestData(PuChannel(1,1,1), 1, data);
	olock.lock();
	printf("Load Test Data: Done\n");
}
#endif

#ifdef ZAP
	dprintf(DBG_FPGA, "Pupe::fpgaConfig: DumpState\n");
	dumpState();
#endif

#ifdef ZAP
	BIter			i;
	BList<NameValue>	infoList;

	fpgaGetInfo(infoList);
	for(infoList.start(i); !infoList.isEnd(i); infoList.next(i)){
		printf("%s: %s\n", infoList[i].name.retStr(), infoList[i].value.retStr());
	}
#endif

	dprintf(DBG_FPGA, "Pupe::fpgaConfig: End\n");
	
	return err;
}

BError Pupe::fpgaReadData64(uint32_t fpgaOffset, void* toAddress, int nbytes){
	BError		err;
	ADMXRC2_STATUS	status;
	uint32_t	dmaMode;
	
	dprintf(DBG_FPGA, "Pupe::fpgaReadData64\n");
	dmaMode = ADMXRC2_BuildDMAModeWord(ocardInfo.BoardType,
				  ADMXRC2_IOWIDTH_64, 0,
				  ADMXRC2_DMAMODE_USEREADY |
				  ADMXRC2_DMAMODE_USEBTERM |
				  ADMXRC2_DMAMODE_BURSTENABLE);
				  
	status = ADMXRC2_DoDMA(ofpgaCard, odmabuf, 0, nbytes,
			FPGA_DATA_ADDRESS + fpgaOffset, ADMXRC2_LOCALTOPCI, 0, dmaMode, 0, 0, 0);

	memcpy(toAddress, odmaBuffer, nbytes);

	if(status){
		err.set(ErrorFpga, "Fpga: DMA Error");
	}

	return err;
}

BError Pupe::fpgaReadData32(uint32_t fpgaOffset, void* toAddress, int nbytes){
	BError		err;
	ADMXRC2_STATUS	status;
	uint32_t	dmaMode;
	
	dprintf(DBG_FPGA, "Pupe::fpgaReadData32\n");
	dmaMode = ADMXRC2_BuildDMAModeWord(ocardInfo.BoardType,
				  ADMXRC2_IOWIDTH_32, 0,
				  ADMXRC2_DMAMODE_USEREADY |
				  ADMXRC2_DMAMODE_USEBTERM |
				  ADMXRC2_DMAMODE_BURSTENABLE);
				  
	status = ADMXRC2_DoDMA(ofpgaCard, odmabuf, 0, nbytes,
			FPGA_DATA_ADDRESS + fpgaOffset, ADMXRC2_LOCALTOPCI, 0, dmaMode, 0, 0, 0);

	memcpy(toAddress, odmaBuffer, nbytes);

	if(status){
		err.set(ErrorFpga, "Fpga: DMA Error");
	}

	return err;
}

BError Pupe::fpgaWriteData64(void* fromAddress, uint32_t fpgaOffset, int nbytes){
	BError		err;
	ADMXRC2_STATUS	status;
	uint32_t	dmaMode;
	
	dprintf(DBG_FPGA, "Pupe::fpgaWriteData64\n");
	dmaMode = ADMXRC2_BuildDMAModeWord(ocardInfo.BoardType,
				  ADMXRC2_IOWIDTH_64, 0,
				  ADMXRC2_DMAMODE_USEREADY |
				  ADMXRC2_DMAMODE_USEBTERM |
				  ADMXRC2_DMAMODE_BURSTENABLE);
				  
	memcpy(odmaBuffer, fromAddress, nbytes);

	status = ADMXRC2_DoDMA(ofpgaCard, odmabuf, 0, nbytes,
			FPGA_DATA_ADDRESS + fpgaOffset, ADMXRC2_PCITOLOCAL, 0, dmaMode, 0, 0, 0);

	if(status){
		err.set(ErrorFpga, "Fpga: DMA Error");
	}

	return err;
}

BError Pupe::fpgaCopyData(int pupeChan, uint32_t address, uint32_t nsamples, Data& data){
	BError			err;
	uint32_t		pageSize = 2*1024*1024;
	uint32_t		dataTablePage;
	uint32_t		dataTableOffset;
	uint32_t		ncopy;
	uint32_t		addressOut = 0;
	
	dprintf(DBG_FPGA, "Pupe::fpgaCopyData: %d\n", pupeChan);
	switch(pupeChan){
	case 1:	ofpga[MEM_REG] = SDRAM_DATA_PU0; break;
	case 2:	ofpga[MEM_REG] = SDRAM_DATA_PU1; break;
	case 3:	ofpga[MEM_REG] = SDRAM_DATA_PU2; break;
	default:	return err.set(ErrorFpga, "Incorrect puChannel");
	}
	
	while(nsamples){
		dataTablePage = address >> 21;		// 2MByte Region
		dataTableOffset = address & 0x001FFFFF;	// Offset
		
		ncopy = nsamples * 8;
		if(ncopy > (pageSize - dataTableOffset)){
			ncopy = pageSize - dataTableOffset;
		}
		
		ofpga[ADDR_REG] = dataTablePage << 19;
		printf("ReadData: Chan: %d Address: %x Page: %x Offset: %x NBytes: %d\n", pupeChan, address, dataTablePage, dataTableOffset, ncopy);
#if USE_DMA
		if(err = fpgaReadData64(dataTableOffset, &data.dataValues[addressOut], ncopy))
			return err;
#else		
		uint64_t*	dataTable = (uint64_t*)&ofpga[FPGA_MEM];

		memcpy_64(&data.dataValues[addressOut], &dataTable[dataTableOffset/8], ncopy);
#endif

		dataTableOffset = 0;
		address += ncopy;
		addressOut += (ncopy / 8);
		nsamples -= (ncopy / 8);
	}
	dprintf(DBG_FPGA, "Pupe::fpgaCopyData: End\n");

	return err;
}

void Pupe::fpgaClose(){

	dprintf(DBG_FPGA, "Pupe::fpgaClose\n");

	if(ofpgaCard){
		// Stop interrupt thread
		ointerruptThread.cancel();
		ointerruptThread.waitForCompletion();

		// Disable interrupts
		ofpga[IER] = 0;

		// Release DMA
		if(odmabuf){
			ADMXRC2_UnsetupDMA(ofpgaCard, odmabuf);
			odmabuf = 0;
		}
		
		ofpga = 0;

		// Close card
		ADMXRC2_CloseCard(ofpgaCard);
	}
}

BError Pupe::fpgaGetInfo(BList<NameValue>& infoList){
	BError		err;
	uint8_t*	p;
	float		sysFactors[10] = { 0.0130, 0.0141, 0.0172, 0.0260, 0.0625, 0.0141, 1.0000, 1.0000, 0.0098, 0.0098 };

	if(ofpgaControl){
#if 1
		/*
		* Removed.  Bridge now sets up the LM87 Automatically
		*/
		ofpgaControl[SYSMON_CTL] = 0x132D4080;
		usleep(1000);
		if (ofpgaControl[SYSMON_CTL] & SYSMON_SMB_ERROR) {
			printf("\nSMB Serial Bus Error - LM87 Reset Failed\n");
			printf("Ctl = %8.8x\n", ofpgaControl[SYSMON_CTL]);
			return err.set(ErrorFpga, "SMB Serial Bus Error - LM87 Reset Failed");
		}
		ofpgaControl[SYSMON_CTL] = 0x032D1603;
		usleep(1000);
		ofpgaControl[SYSMON_CTL] = 0x032D4001;
		usleep(1000);
#endif      
		ofpgaControl[SYSMON_CTL] = 0x042D0000;
		usleep(1000);
		if(ofpgaControl[SYSMON_CTL] & SYSMON_SMB_ERROR){
			printf("\nSMB Serial Bus Error - Shadow Read Failed\n");
			return err.set(ErrorFpga, "SMB Serial Bus Error - Shadow Read Failed");
		}

		p = (uint8_t*)&ofpgaControl[SYSMON_BUF] + 0x20;

		infoList.append(NameValue("+1V2", p[8] * sysFactors[8]));
		infoList.append(NameValue("+1V5", p[1] * sysFactors[1]));
		infoList.append(NameValue("+1V8", p[9] * sysFactors[9]));
		infoList.append(NameValue("+2V5", p[0] * sysFactors[0]));
		infoList.append(NameValue("+3V3", p[2] * sysFactors[2]));
		infoList.append(NameValue("+5V", p[3] * sysFactors[3]));
		infoList.append(NameValue("Vio", p[4] * sysFactors[4]));
		infoList.append(NameValue("FPIO", p[5] * sysFactors[5]));
		infoList.append(NameValue("TempInternal", p[7] * sysFactors[7]));
		infoList.append(NameValue("TempFpga", p[6] * sysFactors[6]));
	}

	return err;
}