/*******************************************************************************
 *	Pupe.cc	Pupe process
 *			T.Barnaby,	BEAM Ltd,	2007-02-13
 *
 * Removed 128 offset and changed sign of frefPhaseDelay. JMB -	20110608
 * Increased max value of orbitNumber to 1M5.		  JMB -	20110713
 * Added state-qualified trigger bit to	DIAG_CTRL	  JMB -	20130111
 * Fooled around in eventsToPeriods() to fix 2nd INJ	  JMB -	20141205 (ZAP_CERN_1)
 *******************************************************************************
 */
#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>

/* Build Options */
#define	USE_DMA			1
#define	LOAD_USE_DMA		1
#define	NEW_BUNCH_MEAN		1
#define NEW_CYCLE_INFO		1

/* Debug options */
#define	SYSCLK_INTERNAL		0
#define DEBUG_CTRL		0
#define	DEBUG_INFO_TABLE	0
#define	DEBUG_DATA		0
#define	DEBUG_DATA_ERROR	0
#define	DEBUG_DATA_TIME		0
#define DEBUG_CONTROL_TIME	0
#define DEBUG_DISABLE_ERROR_IRQ	1
#define DEBUG_VALIDATE_CYCLE_INFO	0

struct MeanValue {
	int32_t		sigma;
	int32_t		deltaX;
	int32_t		deltaY;
	int32_t		numSamples;
};

struct MeanValueSmall {
	int16_t		sigma;			///< The Sigma value
	int16_t		deltaX;			///< The DeltaX value
	int16_t		deltaY;			///< The DeltaY value
	int16_t		numSamples;		///< The number of samples averaged
};


// 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	SYNTH_CNTRL_REG	= 0x0080 >> 2;	// ADC PLL Registers
const int	SYNTH_STRB_REG	= 0x0088 >> 2;	// ADC PLL Registers
const int	SYNTH_RESET	= 0x0090 >> 2;	// ADC PLL Registers

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	DIAG_DELAY	= 0x0060 >> 2;	// Diagnostics Capture Delay
const int	TEST		= 0x0068 >> 2;	// Timing Test
const int	EVENT_DELAY	= 0x0070 >> 2;	// Programable event delay

// 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

#if NEW_BUNCH_MEAN
const uint32_t	SDRAM_DATA_TOP		= (254*1024*1024);	// The SDRAM bank size minus the area for bunch mean tables
#else
const uint32_t	SDRAM_DATA_TOP		= (256*1024*1024);		// The SDRAM bank size in words
#endif

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's PLL and State/Phase tables
const int	CONTROL_LOOP_CONTROL			= 0x02;	// Disables PLL feedback
const int	CONTROL_DDS_FREQUENCY_LIMIT_ENABLE	= 0x04;	// Enables PLL frequency limits
const int	CONTROL_BLR_DISABLE			= 0x08;	// Disables the BLR algorithm

// 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)

// ADC register bits
const int	ADC_LED_YELLOW		= (1 << 10);	///< The Yellow LED
const int	ADC_LED_RED		= (1 << 11);	///< The Red LED

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

const int	TIMING_FREF		= (143 << 16);	// The FREF frequency for timing simulation

const int	minTimingDiff		= 500;		// The minimum time difference in the timing table
const int	diagnosticsNumSamples	= 4096;		// The number of diagnostics samples
const int	diagnosticsTimeout	= 800;		// The diagnostics timeout in 10ms steps
const uint32_t	maxNumValues		= (SDRAM_DATA_TOP / 2 / 8);	// Maximum number of samples to return

// System information factors
const float	sysFactors[10] = { 0.0130, 0.0141, 0.0172, 0.0260, 0.0625, 0.0141, 1.0000, 1.0000, 0.0098, 0.0098 };

int compare(double v1, double v2, double diff){
	if(v1 >= 0){
		if(v1 > (v2 + (v2 * diff)))	return 1;
		if(v1 < (v2 - (v2 * diff)))	return 1;
	}
	else {
		if(v1 < (v2 + (v2 * diff)))	return 1;
		if(v1 > (v2 - (v2 * diff)))	return 1;
	}
	return 0;
}

/// 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 long int)to & 0x07) || ((unsigned long 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;
}

uint32_t sdramAddressRange(uint32_t start, uint32_t end){
	if(start == end)
		return 0;
	else if(end > start)
		return end - start;
	else
		return (SDRAM_DATA_TOP / 8) - start + end;
}

uint32_t sdramAddressAdd(uint32_t address, uint32_t offset){
	address += offset;
	
	if(address > (SDRAM_DATA_TOP / 8))
		address -= (SDRAM_DATA_TOP / 8);

	return address;
}

uint32_t sdramAddressByteAdd(uint32_t address, uint32_t offset){
	address += offset;
	
	if(address >= SDRAM_DATA_TOP)
		address -= SDRAM_DATA_TOP;

	return address;
}

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 slot, int board) : ocontrol(control), ointerruptThread(*this){
	osimulate = 0;
	oinitialised = 0;
	omaster = 0;
	oboard = board;
	oslot = slot;
	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 BUInt64 [odmaBufferSize/sizeof(BUInt64)];
	odmabuf = 0;
	opllRefErrors = 0;
	opllLocked = 0;
	oprocessingCycle = 0;
	
	memset(&opupeConfig, 0, sizeof(opupeConfig));
	memset(ocycleParams, 0, sizeof(ocycleParams));

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

	ocycleCount = 0;
}

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

BString Pupe::getName(){
	return BString("Pupe: ") + (oslot + 1) + ": ";
}

BError Pupe::init(){
	BError	err;
	BUInt32	simulateTiming;
	BString	s;

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

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

	olock.lock();

	memset(&opupeConfig, 0, sizeof(opupeConfig));

	s = config.findValue("SimulateTiming:");
	if(s != "")
		simulateTiming = s.retInt();
	else
		simulateTiming = 0;

	opupeConfig.internalTimingMask = simulateTiming;

	if(!osimulate){
		if(oinitialised){
			fpgaClose();
			oinitialised = 0;
		}

		if(oboard >= 0){	
			if(!(err = fpgaInit())){
				// Set up Pll Synthesisor
				opllSynth.init(ofpga);
//				opllSynth.displayRegisters();

				// Clear CycleInfo table
				ofpga[MEM_REG] = SDRAM_BLOCKRAM;
				memset(ocycleInfoTable, 0, sizeof(ocycleInfoTable));
				ofpga[IMEM_REG] = PU0_BRAM_BASE + BRAM_CYCLE_INFO;
				fpgaWriteData64(ocycleInfoTable, 0, sizeof(ocycleInfoTable));
				ofpga[IMEM_REG] = PU1_BRAM_BASE + BRAM_CYCLE_INFO;
				fpgaWriteData64(ocycleInfoTable, 0, sizeof(ocycleInfoTable));
				ofpga[IMEM_REG] = PU2_BRAM_BASE + BRAM_CYCLE_INFO;
				fpgaWriteData64(ocycleInfoTable, 0, sizeof(ocycleInfoTable));

				oinitialised = 1;
			}
		}
		else {
			err.set(ErrorFpga, getName() + "Not present");
		}

	}
	else {
		oinitialised = 1;
	}
	ostatus = err;


	olock.unlock();

	return err;
}

int Pupe::getSlot(){
	return oslot;
}

BError Pupe::status(){
	BError	err;

	olock.lock();
	err = ostatus.copy();
	olock.unlock();
	return err;
}

void Pupe::setStatus(BError err, int powerOff){
	olock.lock();
	ostatus = err.copy();
	fpgaShutdown();
	olock.unlock();
}

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

	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}

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

	omaster = on;
	
	return err;
}

BError Pupe::setNextCycle(PuChannel puChannel, BUInt32 cycleNumber, BString cycleType, CycleParam& params){
	BError	err;

	dprintf(DBG_SETNEXTCYCLE, "Pupe::setNextCycle: %u\n", cycleNumber);

	olock.lock();
	ocycleNumberNext = cycleNumber;
	ocycleTypeNext = cycleType;
	
	if(puChannel.pupeChan == 0){
		ocycleParams[0][cycleNumber % 8] = &params;
		ocycleParams[1][cycleNumber % 8] = &params;
		ocycleParams[2][cycleNumber % 8] = &params;
	}
	else {
		ocycleParams[puChannel.pupeChan - 1][cycleNumber % 8] = &params;
	}
	
	if(!oprocessingCycle){
		err = setControlInfo(puChannel, cycleNumber, params);
		if(!err && oprocessingCycle){
			err.set(ErrorCycleNumber, getName() + "The next cycle has already started");
		}
	}
	olock.unlock();
	dprintf(DBG_SETNEXTCYCLE, "Pupe::setNextCycle: %u: End\n", cycleNumber);

	return err;
}

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

	dprintf(DBG_PUPE, "Pupe::setControlInfo: %u.%u.%u %u %s %s\n", puChannel.moduleNum, puChannel.pupeNum, puChannel.pupeChan, cycleNumber, params.cycleType.retStr(), params.name.retStr());

	if(ofpgaCard){
		if(puChannel.pupeChan == 0){
			for(n = 1; (n < numChan + 1) && !err; n++){
				puChannel.pupeChan = n;
				err = setControlInfoPickUp(puChannel, cycleNumber, params);
			}
		}
		else if((puChannel.pupeChan > 0) && (puChannel.pupeChan < numChan + 1)){
			err = setControlInfoPickUp(puChannel, cycleNumber, params);
		}
		else {
			return err.set(ErrorParam, getName() + "Incorrect puChannel");
		}
	}

	return err;
}

BError Pupe::setControlInfoPickUp(PuChannel puChannel,  BUInt32 cycleNumber, CycleParam& params){
	BError	err;
	BUInt32	regBase = 0;
	BUInt32	brBase = 0;
	BUInt32	n;
	BUInt32	ring = 0;
	BUInt32	logicalPuChannel = 1;
	BUInt32	stateTable[16];
	BUInt8	phaseTables[14][512];

#if DEBUG_CONTROL_TIME
	double	t1 = 0, t2 = 0, t3 = 0, t4 = 0, t5 = 0;
	
	t1 = getTime();
#endif

	dprintf(DBG_CYCLEPARAM, "Pupe::setControlInfoPickUp: PupeNum: %d PupeChan: %d Type: %s\n", puChannel.pupeNum, puChannel.pupeChan, params.cycleType.retStr());
	if(ostatus)
		return ostatus;

	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, getName() + "Incorrect puChannel");
	}

	dprintf(DBG_CYCLEPARAM, "Load Cycle Information: %d %s\n", puChannel.pupeChan, ocycleTypeNext.retStr());

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

	puChannel.moduleNum = ocontrol.moduleNum();
	puChannel.pupeNum = (oslot + 1);
	if(ocontrol.getPuChannel(puChannel, ring, logicalPuChannel)){
		ring = 1;
		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_PHASEDELAY]	= params.frefPhaseDelay[logicalPuChannel - 1];
		ofpga[regBase + PLL_FREQUENCY]	= params.pllCycleStartFrequency;
		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;
		ofpga[regBase + EVENT_DELAY]	= params.stateDelay;

#if DEBUG_CONTROL_TIME
		t2 = getTime();
#endif
	
		// Write State and Phase Tables
		ofpga[MEM_REG] = SDRAM_BLOCKRAM;
		ofpga[IMEM_REG] = brBase + BRAM_SWITCH_TABLE;

		for(n = 0; n < 16; n++){
			if(n < params.stateTable.size())
				stateTable[n] = params.stateTable[n].state;
			else if(n == 15)
				stateTable[n] = 0xffffff00;
			else
				stateTable[n] = (n << 28) | 0x0eeeee00;
		}
		fpgaWriteData64(stateTable, 0, sizeof(stateTable));

#if DEBUG_CONTROL_TIME
		t3 = getTime();
#endif
		ofpga[IMEM_REG] = brBase + BRAM_PHASE_TABLE;
		for(n = 0; (n < 14) && (n < params.stateTable.size()); n++){
			memcpy(phaseTables[n], params.stateTable[n].phaseTable.data(), 512);
		}
		fpgaWriteData64(phaseTables, 0, 512 * params.stateTable.size());
		
#if DEBUG_CONTROL_TIME
		t4 = getTime();
#endif
		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;
		}
	}

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

	opuCycleTypes[puChannel.pupeChan - 1] = ocycleTypeNext;

#if DEBUG_CONTROL_TIME
	t5 = getTime();
	printf("SetControlInfoPickUp: Times: %f, %f, %f, %f, %f Total: %f\n", t2 - t1, t3 - t2, t4 - t3, t5 - t4, t5 - t4, t5 - t1);
#endif
	dprintf(DBG_FPGA, "Pupe::setControlInfoPickUp: End\n");

	return err;
}

PuStateTable* Pupe::getPuStateTable(CycleParam* params, BUInt32 period){
	BUInt32	s;

	for(s = 0; s < params->stateTable.size();  s++){
		if(params->stateTable[s].period == period){
			return &params->stateTable[s];
		}
	}
	return 0;
}


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

	err = status();

	olock.lock();


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

	statusList.append(NameValue(baseName + "_Error", err.getString()));
	statusList.append(NameValue(baseName + "_AdcPllLock", opllLocked));
	statusList.append(NameValue(baseName + "_AdcPllLockErrors", opllRefErrors));

	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(BUInt32 cycleNumber){
	BError	err;
	BInt32	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 < -3)
		return err.set(ErrorDataGone, getName() + "Cycle's data has already been overwritten: Current: " + ocycleCompletedNumber + " Requested: " + cycleNumber);

	if(diff > 1)
		return err.set(ErrorDataFuture, getName() + "Cycle data required is to far in the future: Current: " + ocycleCompletedNumber + " Requested: " + cycleNumber);

	while(diff == 1){
		olock.unlock();
		usleep(1000);
		olock.lock();
		diff = cycleNumber - ocycleCompletedNumber;
	}
		
	return err;
}

BError Pupe::getEventList(BUInt32 pupeCycleNumber, BList<PupeEvent>& events){
	BError		err;
	int		i;
	PupeEvent	event;
	PupeEvent	eventPrev = { 0, 0, 0, 0 };
	BUInt		a;
	BUInt		eventNext;
	
	events.clear();
	pupeCycleNumber &= 0x03;
	
#if DEBUG_INFO_TABLE
	printf("Pupe::getEventList\n");
	printf("CycleInfoTable Entry: %d\n", pupeCycleNumber);
	hd64(ocycleInfoTable, 4 * 16);
#endif

	// First look for CYCLE_START event in first entry
	i = 0;
	event.event = (ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 24) & 0xFF;
	event.statePre = (ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 20) & 0x0F;
	event.statePost = (ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 16) & 0x0F;
	event.timingAddress = ocycleInfoTable[(pupeCycleNumber << 4) + i] & FpgaCycleTimingTableMask;
	event.time = ocycleTimingTable[event.timingAddress] & 0xFFFFFFFF;
	event.dataAddress = ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 32;

	if(event.event != EventCycleStart){
		return err.set(ErrorFpga, getName() + "No CYCLE_START event at start of cycle");
	}
	
	// Now look for first valid timing table entry, this is where the time is 1ms
	for(a = 0; a < 4; a++){
		if((ocycleTimingTable[(event.timingAddress + a) & FpgaCycleTimingTableMask] & 0xFFFFFFFF) == 1)
			break;
	}
	if(a >= 4)
		return err.set(ErrorFpga, getName() + "No 1ms timing table entry for CYCLE_START event");

	// Set the Timing Address to be one before the 1ms timing table entry. This is not actually a valid
	// entry for the CYCLE, but it allows time offsets in later code to work correctly.
	event.timingAddress = (event.timingAddress + a - 1) & FpgaCycleTimingTableMask;
	event.time = 0;
	
#if DEBUG_INFO_TABLE
	printf("%d: Event: %x States(%d,%d) TimingAddress: %x Time: %d (NextTime: %d) DataAddress: %x\n", i, event.event, event.statePre, event.statePost, event.timingAddress, event.time, int(ocycleTimingTable[event.timingAddress + 1] & 0xFFFFFFFF), event.dataAddress);
#endif

	// Add the event to the events list
	events.append(event);
	eventPrev = event;

	// Gather the rest of the events from Cycle Information table
	for(i = 1; i < 16; i++){
		event.event = (ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 24) & 0xFF;
		event.statePre = (ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 20) & 0x0F;
		event.statePost = (ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 16) & 0x0F;

#ifdef ZAP
		event.timingAddress = ocycleInfoTable[(pupeCycleNumber << 4) + i] & FpgaCycleTimingTableMask;
		event.time = ocycleTimingTable[event.timingAddress] & 0xFFFFFFFF;
#else
		event.timingAddress = (ocycleInfoTable[(pupeCycleNumber << 4) + i] - 1) & FpgaCycleTimingTableMask;
		event.time = (ocycleTimingTable[event.timingAddress] & 0xFFFFFFFF) + 1;
#endif
		event.dataAddress = ocycleInfoTable[(pupeCycleNumber << 4) + i] >> 32;
		eventNext = (ocycleInfoTable[(pupeCycleNumber << 4) + i + 1] >> 24) & 0xFF;
		
		if(event.event == 0){
			return err.set(ErrorFpga, "No CYCLE_STOP event during cycle");
		}

#if DEBUG_INFO_TABLE
		printf("%d: Event: %x States(%d,%d) TimingAddress: %x Time: %d (NextTime: %d) DataAddress: %x\n", i, event.event, event.statePre, event.statePost, event.timingAddress, event.time, int(ocycleTimingTable[event.timingAddress + 1] & 0xFFFFFFFF), event.dataAddress);
#endif

		// Check validity of events
		if(eventPrev.event && (event.time < eventPrev.time)){
#if DEBUG_DATA_ERROR
			printf("Event timing backwards in CycleInfoTable Entry: %d Event: %x Times: %d <= %d\n", i, event.event, event.time, eventPrev.time);
			hd64(ocycleInfoTable, 4 * 16);
#endif
			return err.set(ErrorFpga, getName() + "Multiple events at the same time or later events have time before previous events");
		}

		events.append(event);
		eventPrev = event;

		if(event.event & EventCycleStop)
			break;
	}
	return err;
}

void cycleParamPrint(CycleParam* p){
	printf("CycleParam: Name: %s Type: %s Ring: %d Channel: %d\n", p->name.retStr(), p->cycleType.retStr(), p->ring, p->channel);
	printf("StateTables: Num: %lu\n", p->stateTable.size());
	for(BUInt i = 0; i < p->stateTable.size(); i++){
		printf("%2d: %2d %8.8x %8.8x %8.8x %8.8x\n", i, p->stateTable[i].period, p->stateTable[i].state, p->stateTable[i].harmonic, p->stateTable[i].numBunches, p->stateTable[i].bunchMask);
	}
}

BError Pupe::eventsToPeriods(BUInt32 cycleNumber, BList<PupeEvent>& events, BList<CycleInformationPeriodInternal>& periods){
	BError				err;
	BIter				i;
	BIter				i1;
	int				h;
	CycleParam*			param;
	CycleInformationPeriodInternal	period;
	PupeEvent			eventNull = { 0, 0, 0, 0, 0, 0 };

	// Find CyclePeriods in Cycle Information table
	param = ocycleParams[0][cycleNumber % 8];
	if(param == 0){
		return err.set(ErrorParam, BString("Cycle parameters not set for Cycle number: ") + cycleNumber);
	}

	period.cyclePeriod = 0;
	period.startTime = 0;
	period.endTime = 0;
	period.harmonic = 0;
	period.bunchMask = 0;
	period.numBunches = 0;
	period.numValues = 0;
	period.dataAddress = 0;
	period.eventStart = eventNull;
	period.eventEnd = eventNull;
	period.eventStop = eventNull;

	// Get Cycle parameters for the given cycle
	for(events.start(i), h = 0; !events.isEnd(i); events.next(i)){
		if(events[i].event == 0){
			return err.set(ErrorFpga, "No CYCLE_STOP event during cycle");
		}
		else if(events[i].event & EventCycleStart){
			period.cyclePeriod = CyclePeriodAll;
			period.startTime = events[i].time;
			period.dataAddress = events[i].dataAddress;
			period.eventStart  = events[i];
			periods.append(period);
			period.cyclePeriod = 0;
		}
		else if(events[i].event & EventCycleStop){
			if(period.cyclePeriod){
				// Save period
				period.endTime = events[i].time;
				period.numValues = sdramAddressRange(period.dataAddress, events[i].dataAddress);
				period.eventEnd  = events[i];
				period.eventStop  = events[i];
				periods.append(period);
				period.cyclePeriod = 0;
			}

			periods[0].endTime = events[i].time;
			periods[0].numValues = sdramAddressRange(periods[0].dataAddress, events[i].dataAddress);
			periods[0].eventEnd  = events[i];
			periods[0].eventStop  = events[i];
			for(periods.start(i1); !periods.isEnd(i1); periods.next(i1)){
				periods[i1].eventStop  = events[i];
			}
			break;
		}
		else if(events[i].event & EventDelay){
			// Ignore these
		}
#ifdef ZAP_CERN_1	// JMB
		else if((events[i].event & EventInjection) && (period.cyclePeriod == CyclePeriodEvent0)){
			// Ignore these multiple injections
		}
#endif
		else {
			// Save period if needed
			if(period.cyclePeriod){
				period.endTime = events[i].time;
				period.numValues = sdramAddressRange(period.dataAddress, events[i].dataAddress);
				period.eventEnd  = events[i];
				periods.append(period);
				period.cyclePeriod = 0;
			}
			
			if(events[i].event & EventCalibrationStart){
				period.cyclePeriod = CyclePeriodCalibration;
			}
			else if(events[i].event & EventInjection){
#ifdef ZAP_CERN_1	// JMB
				period.cyclePeriod = CyclePeriodEvent0;
#else
				period.cyclePeriod = CyclePeriodEvent0 + h++;
#endif
			}
			else if(events[i].event & EventHChange){
				period.cyclePeriod = CyclePeriodEvent1 + h++;
			}
			else {
				period.cyclePeriod = 0;
			}

			period.eventStart  = events[i];
			period.startTime = events[i].time;
			if(events[i].statePost < 14){
				if(events[i].statePost >= param->stateTable.size())
					return err.set(ErrorParam, BString("Cycle parameter stateTable too short in: ") + param->cycleType);

				period.harmonic = param->stateTable[events[i].statePost].harmonic;
				period.numBunches = param->stateTable[events[i].statePost].numBunches;
				period.bunchMask = param->stateTable[events[i].statePost].bunchMask;
			}
			else {
				period.harmonic = 0;
				period.numBunches = 0;
				period.bunchMask = 0x00;
			}
			period.dataAddress = events[i].dataAddress;
		}
	}

#if DEBUG_INFO_TABLE
	for(periods.start(i); !periods.isEnd(i); periods.next(i)){
		CycleInformationPeriodInternal&	p = periods[i];
		
		printf("Period: %2d: StartTime: %4d EndTime: %4d Harm: %2d NumBunch: %d BunchMask: %4x NumValues: %8d Address: 0x%08x\n",
			p.cyclePeriod, p.startTime, p.endTime, p.harmonic, p.numBunches, p.bunchMask, p.numValues, p.dataAddress); 
	}
#endif

	return err;
}

void Pupe::printEventList(BList<PupeEvent>& events){
	BIter	i;
	int	n;
	
	for(events.start(i), n = 0; !events.isEnd(i); events.next(i), n++){
		printf("Event: %d Type: %x TimingAddress: %x Time: %d DataAddress: %x\n", n, events[i].event, events[i].timingAddress, events[i].time, events[i].dataAddress);
	}
}

BError Pupe::getCycleInformation(BUInt32 cycleNumber, CycleInformation& cycleInformation){
	BError					err;
	int					timeOut;
	uint32_t				pupeCycleNumber;
	int32_t					diff;
	BList<PupeEvent>			events;
	BIter					i;
	BIter					n;
	BList<CycleInformationPeriodInternal>	periods;
	CycleInformationPeriodInternal		period;
	CycleParam*				param;
#if !NEW_CYCLE_INFO
	uint32_t				s;
	int					h;
#endif

	dprintf(DBG_FPGA, "Pupe::getCycleInformation: Cycle: %u\n", cycleNumber);
	if(ostatus)
		return ostatus;

	olock.lock();

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

	// Get Cycle parameters for the given cycle
	param = ocycleParams[0][cycleNumber % 8];
	if(param == 0){
		olock.unlock();
		return err.set(ErrorParam, BString("Cycle parameters not set for Cycle number: ") + cycleNumber);
	}
		
	
	cycleInformation.cycleNumber = cycleNumber;
	cycleInformation.cycleType = param->cycleType;

	ofpga[MEM_REG] = SDRAM_BLOCKRAM;

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

		ofpga[IMEM_REG] = PU0_BRAM_BASE + 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, getName() + "Timed out while getting Cycle Information tables");
	}

	// Return an error if already gone or to far in the future
	if(err = checkData(cycleNumber)){
		olock.unlock();
		return err;
	}
		
	// Calculate actual Pupe Cycle number required
	diff = ocycleNumber - cycleNumber;
	pupeCycleNumber = (opuCycleNumbers[0] - diff) & 0x03;

#if DEBUG_DATA
	printf("CycleInfoTable Entry: %d\n", pupeCycleNumber);
	hd64(ocycleInfoTable, 4 * 16);
#endif

	if(err = getEventList(pupeCycleNumber, events)){
		olock.unlock();
		return err;
	}

#if NEW_CYCLE_INFO
	if(err = eventsToPeriods(cycleNumber, events, periods)){
		olock.unlock();
		return err;
	}

	// Find required CyclePeriod in Events list
	for(periods.start(i); !periods.isEnd(i); periods.next(i)){
		cycleInformation.periods.append(periods[i]);
	}
#else
	// Find CyclePeriods in Cycle Information table
	for(events.start(i), h = 0; !events.isEnd(i); events.next(i)){
		n = i;
		events.next(n);

		period.cyclePeriod = 0;
		period.startTime = events[i].time;
		period.endTime = 0;
		period.harmonic = 0;
		period.bunchMask = 0;
		period.numBunches = 0;
		period.numValues = 0;

		if(events[i].event & 0x01){
			period.cyclePeriod = CyclePeriodAll;
			cycleInformation.periods.append(period);
		}
		else if(events[i].event & 0x04){
			period.cyclePeriod = CyclePeriodCalibration;
			if(!events.isEnd(n)){
				period.endTime = events[n].time;
				period.numValues = sdramAddressRange(events[i].dataAddress, events[n].dataAddress);
			}
			for(s = 0; s < param->stateTable.size();  s++){
				if(param->stateTable[s].period == period.cyclePeriod){
					period.harmonic = param->stateTable[s].harmonic;
					period.numBunches = param->stateTable[s].numBunches;
					period.bunchMask = param->stateTable[s].bunchMask;
					break;
				}
			}
			cycleInformation.periods.append(period);
		}
		else if((events[i].event & 0x10) || (events[i].event & 0x20)){
			period.cyclePeriod = CyclePeriodEvent0 + h;
			if(!events.isEnd(n)){
				period.endTime = events[n].time;
				period.numValues = sdramAddressRange(events[i].dataAddress, events[n].dataAddress);
			}
			for(s = 0; s < param->stateTable.size();  s++){
				if(param->stateTable[s].period == period.cyclePeriod){
					period.harmonic = param->stateTable[s].harmonic;
					period.numBunches = param->stateTable[s].numBunches;
					period.bunchMask = param->stateTable[s].bunchMask;
					break;
				}
			}
			cycleInformation.periods.append(period);
			h++;
		}
		else if(events[i].event & 0x02){
			events.start(n);
			cycleInformation.periods[0].endTime = events[i].time;
			cycleInformation.periods[0].numValues = sdramAddressRange(events[n].dataAddress, events[i].dataAddress);
			break;
		}
	}
#endif

#if DEBUG_VALIDATE_CYCLE_INFO
	{
		BUInt			c;
		CycleInformation&	ci = cycleInformation;
		bool			e = 0;
		
		for(c = 0; c < ci.periods.number(); c++){
			printf("  %2d  %4d  %4d %2d  %2d    0x%05x %8d\n", ci.periods[c].cyclePeriod, ci.periods[c].startTime,
				ci.periods[c].endTime, ci.periods[c].harmonic, ci.periods[c].numBunches, ci.periods[c].bunchMask, ci.periods[c].numValues);
			if(ci.periods[c].harmonic > 128)
				e = 1;
			if(ci.periods[c].numBunches > 128)
				e = 1;
		}
		
		if(e){
			printf("CycleInformation table error\n");
			dumpState();
		}
	}
#endif

	if(!err){
		// Need to check the FPGA has not overwritten the data while we were reading it.
#ifdef ZAP
		printf("CheckTime: cycleTimingAddressStart: %u TIME_TBLADDR: %u\n", events.front().timingAddress, ofpga[PU0_REG_BASE + TIME_TBLADDR]);
#endif
		diff = (events.front().timingAddress - ofpga[PU0_REG_BASE + TIME_TBLADDR]) & FpgaCycleTimingTableMask;
		if(diff < minTimingDiff)
			err.set(ErrorDataGone, getName() + "Data has already gone");
	}

	olock.unlock();

	return err;
}

void Pupe::dataAverage(uint32_t timeMs, Data& data){
	BUInt32		n;
	MeanValueSmall*	v;
	int		s;
	
	for(n = 0; n < data.dataValues.size(); n++){
		v = (MeanValueSmall*)&data.dataValues[n];

		if(v->numSamples != 0){
			if(v->numSamples < 256)
				s = 256;
			else if(v->numSamples < 512)
				s = 512;
			else if(v->numSamples < 1024)
				s = 1024;
			else if(v->numSamples < 2048)
				s = 2048;
			else 
				s = 4096;

			data.dataValues[n].sigma = (int(v->sigma) * s) / v->numSamples;
			data.dataValues[n].deltaX = (int(v->deltaX) * s) / v->numSamples;
			data.dataValues[n].deltaY = (int(v->deltaY) * s) / v->numSamples;
			data.dataValues[n].time = timeMs + n;
		}
		else {
			data.dataValues[n].sigma = 0;
			data.dataValues[n].deltaX = 0;
			data.dataValues[n].deltaY = 0;
			data.dataValues[n].time = 0;
		}
	}
}

BError Pupe::getData(PuChannel puChannel, DataInfo dataInfo, Data& data, BUInt32& orbitNumber){
	BError			err;
	int			regBase;
	int			brBase;
	BList<PupeEvent>	events;
	BList<CycleInformationPeriodInternal>	periods;
	CycleInformationPeriodInternal	period;
	BIter			i;
	BIter			n;
	PupeEvent		eventStart = { 0, 0, 0, 0 };
	PupeEvent		eventEnd = { 0, 0, 0, 0 };
	PupeEvent		eventStop = { 0, 0, 0, 0 };
	int32_t			diff;
	uint32_t		timingAddress = 0;
	uint32_t		dataAddress = 0;
	int			found = 0;
	uint32_t		numBunches = 0;
	uint32_t		pupeCycleNumber;
	int			timeOut;
	uint32_t		numValues;
	CycleParam*		cycleParam = 0;
#if !NEW_CYCLE_INFO
	uint32_t		p;
	int			h;
	PuStateTable*		stateTable = 0;
#endif
#if DEBUG_DATA_TIME
	double			t1 = 0, t2 = 0, t3 = 0, t4 = 0, t5 = 0;
	
	t1 = getTime();
#endif

	dprintf(DBG_FPGA, "Pupe::getData: PuChannel: %d.%d RequiredCycleNumber: %u CurrentCycleNumber: %u LastCompletedCycle: %u\n", puChannel.pupeNum, puChannel.pupeChan, dataInfo.cycleNumber, ocycleNumber, ocycleCompletedNumber);
//	printf("Pupe::getData: PuChannel: %d.%d RequiredCycleNumber: %u CurrentCycleNumber: %u LastCompletedCycle: %u\n", puChannel.pupeNum, puChannel.pupeChan, dataInfo.cycleNumber, ocycleNumber, ocycleCompletedNumber);
//	printf("Pupe::getData: PuChannel: %d.%d	startTime: %d orbitNumber: %d\n", puChannel.pupeNum, puChannel.pupeChan, dataInfo.startTime, dataInfo.orbitNumber);
	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}
	if(ostatus)
		return ostatus;
	
	// Basic parameter validation
	if(dataInfo.startTime > 4000){
		return err.set(ErrorParam, getName() + "Start time beyond 4000ms");
	}
	if((dataInfo.cyclePeriod == CyclePeriodAll) && (dataInfo.function != DataFunctionRaw) && (dataInfo.startTime == 0)){
		return err.set(ErrorParam, getName() + "Cannot return data with time offset 0 from CYCLE_START when using averaged data");
	}
	if(dataInfo.orbitNumber > 1500000){
		return err.set(ErrorParam, getName() + "Orbit number beyond 1500000");
	}
	if(dataInfo.numValues > (40 * 8 * 500000)){
		return err.set(ErrorParam, getName() + "NumValues number beyond (40 * 8 * 500000)");
	}

	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, getName() + "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;
	}
	
	cycleParam = ocycleParams[puChannel.pupeChan - 1][dataInfo.cycleNumber % 8];
	if(cycleParam == 0){
		olock.unlock();
		return err.set(ErrorParam, "Cycle parameters have not been set for this cycle");
	}
	
	if(osimulate){
		err = getSimData(dataInfo, data);
		if(dataInfo.startTime)
			orbitNumber = dataInfo.startTime * 2000;
		else
			orbitNumber = dataInfo.orbitNumber;
	}
	else {
#if DEBUG_DATA_TIME
		t2 = getTime();
#endif
		// 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 = 2;
		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, getName() + "Timed out while getting Cycle Information tables");
		}
		
#if DEBUG_DATA_TIME
		t3 = getTime();
#endif
		// 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("GetData: 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(err = getEventList(pupeCycleNumber, events)){
			olock.unlock();
			return err;
		}

#if NEW_CYCLE_INFO		
		if(err = eventsToPeriods(dataInfo.cycleNumber, events, periods)){
			olock.unlock();
			return err;
		}

		// Find required CyclePeriod in Events list
		for(periods.start(i), found = 0; !periods.isEnd(i); periods.next(i)){
			if(periods[i].cyclePeriod == dataInfo.cyclePeriod){
				period = periods[i];
				found = 1;
				break;
			}
		}

		eventStart = period.eventStart;
		eventEnd = period.eventEnd;
		eventStop = period.eventStop;
		numBunches = period.numBunches;
#else
		// Find required CyclePeriod in Events list
		for(events.start(i), h = 0, p = CyclePeriodAll, found = 0; !events.isEnd(i); events.next(i)){
			n = i;
			events.next(n);
			
			p = 0xFFFF;
			if(events[i].event == 0x01){
				p = CyclePeriodAll;
			}
			else if(events[i].event == 0x04){
				p = CyclePeriodCalibration;
			}
			else if((events[i].event == 0x10) || (events[i].event == 0x20)){
				p = CyclePeriodEvent0 + h;
				h++;
			}
			else if(events[i].event == 0x02){
				eventStop = events[i];
				break;
			}
		
			if(events.isEnd(n))
				break;

			if(p == dataInfo.cyclePeriod){
				eventStart = events[i];
				eventEnd = events[n];
				found = 1;
			}
		}
		if(!found){
			err.set(ErrorDataNotAvailable, getName() + "The CyclePeriod required could not be found");
			olock.unlock();
			return err;
		}

		if(stateTable = getPuStateTable(cycleParam, dataInfo.cyclePeriod))
			numBunches = stateTable->numBunches;
		else
			numBunches = 0;

#endif

		// Default is to limit data to this Cycle
		if(dataInfo.beyondPeriod || (dataInfo.cyclePeriod == CyclePeriodAll)){
			eventEnd = eventStop;
		}

#if DEBUG_DATA
		printf("\n");
		printf("CycleInfoTable Entry: %d\n", pupeCycleNumber & 0x03);
		hd64(ocycleInfoTable, 4 * 16);
		printf("\n");
#if NEW_CYCLE_INFO
		printf("CyclePeriod: %d\n", dataInfo.cyclePeriod);
#else
		printf("CyclePeriod: %d Found: %d\n", dataInfo.cyclePeriod, p);
#endif
		printf("EventStart: %x Time: %d TimingAddress: %x DataAddress: %x\n", eventStart.event, eventStart.time, eventStart.timingAddress, eventStart.dataAddress);
		printf("EventEnd: %x Time: %d TimingAddress: %x DataAddress: %x\n", eventEnd.event, eventEnd.time, eventEnd.timingAddress, eventEnd.dataAddress);
		printf("EventStop: %x Time: %d TimingAddress: %x DataAddress: %x\n", eventStop.event, eventStop.time, eventStop.timingAddress, eventStop.dataAddress);
#endif

		// Offset by Start time
		timingAddress = (eventStart.timingAddress + dataInfo.startTime) & FpgaCycleTimingTableMask;
#ifdef ZAP_TIME
		if(dataInfo.startTime)
			dataAddress = ocycleTimingTable[timingAddress] >> 32;
		else
			dataAddress = eventStart.dataAddress;
#else
		if(dataInfo.startTime){
			uint32_t	dataAddressOffset;

			// Validate startTime
			if(dataInfo.startTime >	(eventEnd.time - eventStart.time)){
				err.set(ErrorParam, getName() +	"Start time beyond cycle period's end time: " +	(eventEnd.time - eventStart.time) + "ms");
				olock.unlock();
				return err;
			}

			// Get appoximate data position	within an orbits data
			if((ocycleTimingTable[timingAddress] >>	32) > eventStart.dataAddress){
				dataAddressOffset = (ocycleTimingTable[timingAddress] >> 32) - eventStart.dataAddress;
			}
			else {
				dataAddressOffset = (ocycleTimingTable[timingAddress] >> 32) + (SDRAM_DATA_TOP / 8) - eventStart.dataAddress;
			}

			// Round down to the nearest orbit start
			if(numBunches)
				dataAddress = eventStart.dataAddress + (numBunches * (dataAddressOffset	/ numBunches));
			else
				dataAddress = eventStart.dataAddress + dataAddressOffset;
		}
		else {
			dataAddress = eventStart.dataAddress;
		}
#endif
		// Offset by orbit number
		if(numBunches){
			dataAddress = sdramAddressAdd(dataAddress, numBunches *	dataInfo.orbitNumber);
			if(dataAddress > eventStart.dataAddress){
				orbitNumber = (dataAddress - eventStart.dataAddress) / numBunches;
			}
			else {
				orbitNumber = (dataAddress + (SDRAM_DATA_TOP / 8) - eventStart.dataAddress) / numBunches;
			}
		}
		else {
			dataAddress = sdramAddressAdd(dataAddress, dataInfo.orbitNumber);
			if(dataAddress > eventStart.dataAddress){
				orbitNumber = (dataAddress - eventStart.dataAddress);
			}
			else {
				orbitNumber = (dataAddress + (SDRAM_DATA_TOP / 8) - eventStart.dataAddress);
			}
		}


		// Make sure data is within cycle period
		numValues = sdramAddressRange(dataAddress, eventEnd.dataAddress);

		if(numValues > maxNumValues){
			err.set(ErrorDataNotAvailable, getName() + "CycleTable Error: Data beyond cycle period or the amount of data for a period exceeds a maximum cycle period");
#if DEBUG_INFO_TABLE
			printf("CycleInfoTable Entry: %d\n", pupeCycleNumber & 0x03);
			hd64(ocycleInfoTable, 4 * 16);
			printf("\n");
			printf("EventStart: %x Time: %d TimingAddress: %x DataAddress: %x\n", eventStart.event, eventStart.time, eventStart.timingAddress, eventStart.dataAddress);
			printf("EventEnd: %x Time: %d TimingAddress: %x DataAddress: %x\n", eventEnd.event, eventEnd.time, eventEnd.timingAddress, eventEnd.dataAddress);
			printf("EventStop: %x Time: %d TimingAddress: %x DataAddress: %x\n", eventStop.event, eventStop.time, eventStop.timingAddress, eventStop.dataAddress);

			printf("NumValues: %d %d:%x %d:%x\n", numValues, dataAddress, dataAddress, eventEnd.dataAddress, eventEnd.dataAddress);
			printf("Range: %d %d\n", SDRAM_DATA_TOP, SDRAM_DATA_TOP - dataAddress + eventEnd.dataAddress);

#endif
			olock.unlock();
			return err;

		}

		// Perform data access functions depending on type
		if(dataInfo.function == DataFunctionRaw){		
			if((dataInfo.bunchNumber == 0) && (dataInfo.numValues < numBunches)){
				err.set(ErrorParam, getName() + "NumValues to small for request");
				olock.unlock();
				return err;
			}
			if(dataInfo.bunchNumber){
				if(numBunches)
					numValues /= numBunches;
			}

			// Set up return data size		
			if(numValues < dataInfo.numValues){
				dataInfo.numValues = numValues;
			}

			data.dataValues.resize(dataInfo.numValues);
			data.numValues = dataInfo.numValues;
			if(dataInfo.bunchNumber)
				data.numBunches = 1;
			else
				data.numBunches = numBunches;
			data.numChannels = 1;

#if DEBUG_DATA
			printf("Data: TimingAddress: %d Orbits: %d DataStart: %d DataEnd: %d NumValues: %d\n", timingAddress, dataInfo.orbitNumber, dataAddress, eventEnd.dataAddress, numValues);
			printf("BunchNumber: %d NumBunches: %d\n", dataInfo.bunchNumber, numBunches);
			printf("CycleTimingAddress: %u(%x) CycleTime: %u CycleDataAddress: %u(%x)\n", timingAddress, timingAddress, uint32_t(ocycleTimingTable[timingAddress] & 0xFFFFFFFF), dataAddress, dataAddress);
			printf("CycleTimingTable:\n");
			hd64(&ocycleTimingTable[timingAddress], 16);
#endif
#if DEBUG_DATA_TIME
			t4 = getTime();
#endif
			if(dataInfo.bunchNumber && numBunches){
				ofpga[MEM_RAO] = (dataAddress % numBunches) + dataInfo.bunchNumber - 1;
				ofpga[MEM_RAS] = numBunches;
				dataAddress = dataAddress / numBunches;
			}
			else {
				ofpga[MEM_RAO] = 0;
				ofpga[MEM_RAS] = 1;
			}

			// Read the Data
			err = fpgaCopyData(puChannel.pupeChan, dataAddress * 8, dataInfo.numValues, data);
		}
		else if(dataInfo.function == DataFunctionMean){
			if(dataInfo.bunchNumber > 24){
				err.set(ErrorParam, getName() + "Can only return average for up to bunch 24");
				olock.unlock();
				return err;
			}
			if(dataInfo.orbitNumber != 0){
				err.set(ErrorNotImplemented, getName() + "Cannot offset by orbit");
				olock.unlock();
				return err;
			}
			if((dataInfo.bunchNumber == 0) && (dataInfo.numValues < numBunches)){
				err.set(ErrorParam, getName() + "NumValues to small for request");
				olock.unlock();
				return err;
			}
			
			numValues = (eventEnd.timingAddress - timingAddress) & FpgaCycleTimingTableMask;
				
			if(dataInfo.bunchNumber == 0)
				numValues *= numBunches;

			// Set up return data size		
			if(numValues < dataInfo.numValues){
				dataInfo.numValues = numValues;
			}

			data.dataValues.resize(dataInfo.numValues);
			data.numValues = dataInfo.numValues;
			data.numChannels = 1;

			if(dataInfo.bunchNumber == 0){
				data.numBunches = numBunches;
				err = fpgaGetAverageDataAll(puChannel.pupeChan, eventStart.time, timingAddress, numBunches, data.numValues, data);
			}
			else {
				data.numBunches = 1;
				err = fpgaGetAverageData(puChannel.pupeChan, eventStart.time, timingAddress, dataInfo.bunchNumber, data.numValues, data);
			}
		}
		else if(dataInfo.function == DataFunctionMeanAll){
			if(dataInfo.bunchNumber != 0){
				err.set(ErrorParam, getName() + "Can only return average for all bunches");
				olock.unlock();
				return err;
			}
			if(dataInfo.orbitNumber != 0){
				err.set(ErrorNotImplemented, getName() + "Cannot offset by orbit");
				olock.unlock();
				return err;
			}
			
			numValues = (eventEnd.timingAddress - timingAddress) & FpgaCycleTimingTableMask;

			// Set up return data size		
			if(numValues < dataInfo.numValues){
				dataInfo.numValues = numValues;
			}

			data.dataValues.resize(dataInfo.numValues);
			data.numValues = dataInfo.numValues;
			data.numBunches = 0;
			data.numChannels = 1;

			err = fpgaGetAverageData(puChannel.pupeChan, eventStart.time, timingAddress, 0, data.numValues, data);
		}
		else if(dataInfo.function == DataFunctionMean0){
			if(dataInfo.bunchNumber != 0){
				err.set(ErrorNotImplemented, getName() + "Can only return average for all bunches");
				olock.unlock();
				return err;
			}
			if(dataInfo.orbitNumber != 0){
				err.set(ErrorNotImplemented, getName() + "Cannot offset by orbit");
				olock.unlock();
				return err;
			}
			
			numValues = (eventEnd.timingAddress - timingAddress) & FpgaCycleTimingTableMask;

			// Set up return data size		
			if(numValues < dataInfo.numValues){
				dataInfo.numValues = numValues;
			}

			data.dataValues.resize(dataInfo.numValues);
			data.numValues = dataInfo.numValues;
			data.numBunches = 1;
			data.numChannels = 1;

			ofpga[IMEM_REG] = brBase + BRAM_BUNCH_MEAN0;
			fpgaReadData64(timingAddress * sizeof(uint64_t), data.dataValues.data(), dataInfo.numValues * sizeof(uint64_t));
			dataAverage(eventStart.time, data);
		}
		else if(dataInfo.function == DataFunctionMean1){
			if(dataInfo.orbitNumber != 0){
				err.set(ErrorNotImplemented, getName() + "Cannot offset by orbit");
				olock.unlock();
				return err;
			}
			
			numValues = (eventEnd.timingAddress - timingAddress) & FpgaCycleTimingTableMask;

			// Set up return data size		
			if(numValues < dataInfo.numValues){
				dataInfo.numValues = numValues;
			}

			data.dataValues.resize(dataInfo.numValues);
			data.numValues = dataInfo.numValues;
			data.numBunches = 1;
			data.numChannels = 1;

			ofpga[IMEM_REG] = brBase + BRAM_BUNCH_MEAN1;
			fpgaReadData64(timingAddress * sizeof(uint64_t), data.dataValues.data(), dataInfo.numValues * sizeof(uint64_t));
			dataAverage(eventStart.time, data);
		}
		else {
			err.set(ErrorNotImplemented, getName() + "Data function: " + dataInfo.function + " has not been implemented");
		}
#if DEBUG_DATA_TIME
		t5 = getTime();
#endif


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

		if(!err){
			// Need to check the FPGA has not overwritten the data while we were reading it.
			diff = (timingAddress - ofpga[regBase + TIME_TBLADDR]) & FpgaCycleTimingTableMask;
			if(diff < minTimingDiff){
				err.set(ErrorDataGone, getName() + "Data has already gone: Difference: " + diff + " StartTimingAddress: " + timingAddress + " PupeWriteTimingAddress: " + ofpga[regBase + TIME_TBLADDR]);
#if DEBUG_DATA_ERROR
				printf("Data has already gone: Difference: %d StartTimingAddress: %x PupeWriteTimingAddress: %x\n", diff, timingAddress, ofpga[regBase + TIME_TBLADDR]);
				printf("PupeCycle: %d PupeTime: %d PupeTimingAddress: %x\n", ofpga[regBase + CYCLE], ofpga[regBase + TIME], ofpga[regBase + TIME_TBLADDR]);
				printf("GetData: CycleWanted: %u CurrentCycle: %u Diff: %d PupeCycle: %u CurrentPupeCycle: %u Period: %d\n", dataInfo.cycleNumber, ocycleNumber, diff,pupeCycleNumber,  ofpga[regBase + CYCLE], dataInfo.cyclePeriod); 
				printEventList(events);
				printf("CycleInfoTable Entry: %d\n", pupeCycleNumber & 0x03);
				hd64(ocycleInfoTable, 4 * 16);
#endif
			}
		}
	}
	olock.unlock();
	
#if DEBUG_DATA_TIME
	printf("GetData: Times: %f, %f, %f, %f, %f Total: %f\n", t2 - t1, t3 - t2, t4 - t3, t5 - t4, t5 - t4, t5 - t1);
#endif
	dprintf(DBG_FPGA, "Pupe::getData: End\n", puChannel.pupeNum, puChannel.pupeChan, dataInfo.cycleNumber, ocycleNumber, ocycleCompletedNumber);

	return err;
}

BError Pupe::fpgaGetAverageData(int pupeChan, uint32_t timeMs, uint32_t timingAddress, uint32_t bunch, uint32_t numValues, Data& data){
	BError			err;
	MeanValue*		v;
	uint32_t		nr;
	uint32_t		address;
	uint32_t		n;
	
	dprintf(DBG_FPGA, "Pupe::fpgaGetAverageData: Chan: %d StartTime: %d TimingAddress: %x\n", pupeChan, timeMs, timingAddress);
	if(ostatus)
		return ostatus;

	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");
	}
	
	v = new MeanValue[numValues];
	
	// Set up access to the top page of SDRAM
	ofpga[ADDR_REG] = 0x7F << 19;
	ofpga[MEM_RAO] = 0;
	ofpga[MEM_RAS] = 1;

	// Read the BunchMean Data
	nr = numValues;
	if((timingAddress + nr) > FpgaCycleTimingTableSize)
		nr = FpgaCycleTimingTableSize - timingAddress;

	// Set up base access address
	address = (bunch << 16) | (timingAddress << 4);

	// Fetch the data, possibly in two chunks to handle buffer wrap around
	if(err = fpgaReadData64(address, v, nr * sizeof(MeanValue))){
		ofpga[MEM_REG] = SDRAM_BLOCKRAM;
		return err;
	}

	if(nr != numValues){
		address = (bunch << 16) | (0 << 4);
		if(err = fpgaReadData64(address, &v[nr], (numValues - nr) * sizeof(MeanValue))){
			ofpga[MEM_REG] = SDRAM_BLOCKRAM;
			return err;
		}
	}
		
	ofpga[MEM_REG] = SDRAM_BLOCKRAM;

	// Copy and scale data
	for(n = 0; n < numValues; n++){
		if(v[n].numSamples == 0){
			data.dataValues[n].sigma = 0;
			data.dataValues[n].deltaX = 0;
			data.dataValues[n].deltaY = 0;
			data.dataValues[n].time = 0;
		}
		else {
			data.dataValues[n].sigma = v[n].sigma / v[n].numSamples;
			data.dataValues[n].deltaX = v[n].deltaX / v[n].numSamples;
			data.dataValues[n].deltaY = v[n].deltaY / v[n].numSamples;
			data.dataValues[n].time = timeMs + n;
		}
	}

	delete [] v;

	return err;
}

BError Pupe::fpgaGetAverageDataAll(int pupeChan, uint32_t timeMs, uint32_t timingAddress, uint32_t numBunches, uint32_t numValues, Data& data){
	BError			err;
	MeanValue*		v;
	uint32_t		nr;
	uint32_t		address;
	uint32_t		n;
	uint32_t		b;
	uint32_t		numPerBunch;
	
	dprintf(DBG_FPGA, "Pupe::fpgaGetAverageDataAll: Chan: %d StartTime: %d TimingAddress: %x\n", pupeChan, timeMs, timingAddress);
	if(ostatus)
		return ostatus;

	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(ErrorParam, "Incorrect puChannel");
	}
	
	if(numBunches == 0)
		return err.set(ErrorFpga, "Number of bunches is 0");

	numPerBunch = numValues / numBunches;
	data.dataValues.resize(numPerBunch * numBunches);
	data.numValues = numPerBunch * numBunches;
	
	v = new MeanValue[numPerBunch];
	
	// Set up access to the top page of SDRAM
	ofpga[MEM_RAO] = 0;
	ofpga[MEM_RAS] = 1;

	// Read the BunchMean Data
	for(b = 0; b < numBunches; b++){
		nr = numPerBunch;
		if((timingAddress + nr) > FpgaCycleTimingTableSize)
			nr = FpgaCycleTimingTableSize - timingAddress;

		// Set up base access address
		address = ((b + 1) << 16) | (timingAddress << 4);

		// Fetch the data, possibly in two chunks to handle buffer wrap around
		ofpga[ADDR_REG] = 0x7F << 19;
		
		if(err = fpgaReadData64(address, v, nr * sizeof(MeanValue))){
			ofpga[MEM_REG] = SDRAM_BLOCKRAM;
			return err;
		}

		if(nr != numPerBunch){
			address = ((b + 1) << 16) | (0 << 4);
			ofpga[ADDR_REG] = 0x7F << 19;
			if(err = fpgaReadData64(address, &v[nr], (numPerBunch - nr) * sizeof(MeanValue))){
				ofpga[MEM_REG] = SDRAM_BLOCKRAM;
				return err;
			}
		}

		// Copy and scale data
		for(n = 0; n < numPerBunch; n++){
			if(v[n].numSamples == 0){
				data.dataValues[n * numBunches + b].sigma = 0;
				data.dataValues[n * numBunches + b].deltaX = 0;
				data.dataValues[n * numBunches + b].deltaY = 0;
				data.dataValues[n * numBunches + b].time = 0;
			}
			else {
				data.dataValues[n * numBunches + b].sigma = v[n].sigma / v[n].numSamples;
				data.dataValues[n * numBunches + b].deltaX = v[n].deltaX / v[n].numSamples;
				data.dataValues[n * numBunches + b].deltaY = v[n].deltaY / v[n].numSamples;
				data.dataValues[n * numBunches + b].time = timeMs + n;
			}
		}
	}

	ofpga[MEM_REG] = SDRAM_BLOCKRAM;


	delete [] v;

	return err;
}

BError Pupe::getSimData(DataInfo dataInfo, Data& data){
	BError			err;
	SigGenBeam		sigBeam;
	BUInt32			i;
	Sample*			samples;
	static Data		dataCache;
	double			t;
	double			tend;
	struct timespec		treq;
	
	t = getTime();
	tend = t + (dataInfo.numValues * sizeof(DataValue)) / 100e6;

	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.numBunches = 4;
		dataCache.numChannels = 1;
		dataCache.dataValues.resize(dataInfo.numValues);

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

		delete [] samples;
	}
	
	data = dataCache;

	// Delay to get data rate
	while((t = getTime()) < tend){
		if((tend - t) > 0.01){
			treq.tv_sec = 0;
			treq.tv_nsec = long((tend - t) * 1e9);
			nanosleep(&treq, 0);
		}
	}

	return err;
}

BError Pupe::setTestMode(PuChannel puChannel, BUInt32 testOutput, BUInt32 timingDisableMask){
	BError	err;
	int	regBase = 0;
	int	brBase = 0;
	
	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}
	if(ostatus)
		return ostatus;
	
	if(osimulate){
		err.set(ErrorNotImplemented, getName() + "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, getName() + "Incorrect puChannel");
		}

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

BError Pupe::setTimingSignals(PuChannel puChannel, BUInt32 timingSignals){
	BError	err;
	
	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}
	if(ostatus)
		return ostatus;
	
	if(osimulate){
		err.set(ErrorNotImplemented, getName() + "Not Implemented");
	}
	else {
		olock.lock();
		dprintf(DBG_FPGA, "Pupe::setTimingSignals: %x\n", timingSignals);
		ofpga[TIMING_IO] = (ofpga[TIMING_IO] & ~0xFF) | (timingSignals & 0xFF);
		olock.unlock();
	}
	
	return err;
}

BError Pupe::captureDiagnostics(PuChannel puChannel, TestCaptureInfo captureInfo, BArray<BUInt64>& data){
	BError		err;
	int		regBase = 0;
	int		brBase = 0;
	BUInt32		v = 0;
	int		num = diagnosticsNumSamples;
	int		timeOut;
	
	dprintf(DBG_FPGA, "Pupe::captureDiagnostics\n");

	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}
	if(ostatus)
		return ostatus;
	
	// Check Parameters
	if(captureInfo.startTime && captureInfo.postTriggerDelay)
		return err.set(ErrorParam, getName() + "Parameter error: Can only have startTime or postTriggerDelay not both");
		
	if(osimulate){
		data.resize(num);
	}
	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, getName() + "Incorrect puChannel");
		}

#if DEBUG_CTRL
printf("TIMING_IO: %x\n", ofpga[TIMING_IO]);
printf("TEST: %x\n", ofpga[regBase + TEST]);
printf("ADC: %x\n", ofpga[ADC]);
#endif
		odiagLock.lock();

		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);
		v |= ((captureInfo.triggerStateEnable) << 13); 	// State-qualified trigger enable - JMB

		if(captureInfo.startTime){
			v |= ((captureInfo.startTime & 0xFFFF) << 16);
			v |= ((0x1) << 7);
			ofpga[regBase + DIAG_DELAY] = 0;
		}
		else {
			ofpga[regBase + DIAG_DELAY] = captureInfo.postTriggerDelay;
		}

		// Trigger the data capture
		v |= ((0x1) << 0);

		ofpga[regBase + DIAG_TRIGGER]	= (captureInfo.triggerState << 8) | captureInfo.triggerMask;
		ofpga[regBase + DIAG_CTRL]	= v;

		timeOut = diagnosticsTimeout;
		while(((ofpga[regBase + DIAG_CTRL] & 0x01) || ((ofpga[regBase + DIAG_CTRL] & 0x02) & 0x02)) && timeOut--){
			usleep(10000);
		}
		if(timeOut <= 0){
			odiagLock.unlock();
			return err.set(ErrorTimeout, getName() + "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(BUInt64));

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

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

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

	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}
	if(ostatus)
		return ostatus;
	
	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, getName() + "Incorrect puChannel");
	}

	if(data.size() > (opageSize / sizeof(BUInt32))){
		return err.set(ErrorParam, getName() + "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, getName() + "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(20000);

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

			// Check data read
			ofpga[MEM_REG] = SDRAM_DATA_TEST;
			ofpga[ADDR_REG] = 0;
	
			rdata.resize(data.size());
			fpgaReadData64(0x0, rdata.data(), rdata.size() * sizeof(BUInt32));
	
			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]);
					ofpga[MEM_REG] = SDRAM_BLOCKRAM;
					olock.unlock();
					return err.set(ErrorFpga, getName() + "Error setting test data");
				}
			}

			ofpga[MEM_REG] = SDRAM_BLOCKRAM;
			ofpga[TESTLEN] = data.size() / 2;
			ofpga[TESTCTRL] = (1 << 25);
			
			switch(puChannel.pupeChan){
			case 1:		ofpga[ADC] &= ~0x007;	break;
			case 2:		ofpga[ADC] &= ~0x038;	break;
			case 3:		ofpga[ADC] &= ~0x1C0;	break;
			}

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

			switch(puChannel.pupeChan){
			case 1:		ofpga[ADC] |= 0x007;	break;
			case 2:		ofpga[ADC] |= 0x038;	break;
			case 3:		ofpga[ADC] |= 0x1C0;	break;
			}
		}

		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,%d\n", pupeConfig.adcSysclkSync, pupeConfig.internalTimingMask, pupeConfig.disableBlr);

	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}
	if(ostatus)
		return ostatus;
	
	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, getName() + "Incorrect puChannel");
	}
	
	if(!osimulate){
		// Enable timing bus simulation
		if(pupeConfig.internalTimingMask){
			ofpga[TIMING_IO] = (ofpga[TIMING_IO] & ~0x0FFFFF00) | TIMING_FREF | (pupeConfig.internalTimingMask << 8);
		}
		else {
			ofpga[TIMING_IO] = ofpga[TIMING_IO] & ~0x0FFFFF00;
		}
		if(pupeConfig.disableBlr){
			ofpga[regBase + CONTROL] |= CONTROL_BLR_DISABLE;
		}
		else {
			ofpga[regBase + CONTROL] &= ~CONTROL_BLR_DISABLE;
		}

		// This sets up the ADC Clock
		if(pupeConfig.adcSysclkSync != opupeConfig.adcSysclkSync){
			if(pupeConfig.adcSysclkSync){
				opllSynth.setMode(PllSynth::VCXO_REF);
			}
			else {
				opllSynth.setMode(PllSynth::VCXO);
			}
//			opllSynth.displayRegisters();

			// Reset memory DCM's
			ofpga[LOCKED] = 0x01;
		}
	}

	opupeConfig = pupeConfig;

	return err;
}

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

	dprintf(DBG_FPGA, "Pupe::getPupeConfig: %d.%d.%d\n", puChannel.moduleNum, puChannel.pupeNum, puChannel.pupeChan);

	if(!oinitialised){
		return err.set(ErrorFpga, getName() + "Not present");
	}
	if(ostatus)
		return ostatus;

	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, getName() + "Incorrect puChannel");
	}
	
	pupeConfig = opupeConfig;

	return err;
}

int Pupe::adcPllLockCheck(){
	int	v;

	if(ostatus)
		return ostatus;

	if(oinitialised	&& opupeConfig.adcSysclkSync){
		v = opllSynth.lockStatus();
		dprintf(DBG_FPGA, "LockStatus: %d Count: %d\n", v, opllRefErrors);
		if(!v){
			opllRefErrors++;
			opllSynth.lockReset();
		}
		opllLocked = opllSynth.lockStatus();
	}
	else {
		opllLocked = 0;
	}

	return opllLocked;
}


void Pupe::cycleStart(){
	
	if(ostatus)
		return;

	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();

	oprocessingCycle = 1;
	ocycleNumber = ocycleNumberNext;

	// Clear the Cycle Parameters for the next cycle
	ocycleParams[0][(ocycleNumber + 1) % 8] = 0;
	ocycleParams[1][(ocycleNumber + 1) % 8] = 0;
	ocycleParams[2][(ocycleNumber + 1) % 8] = 0;
	
	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];

		// Set up initial frequency for injection
		if(ocycleParams[0][ocycleNumber % 8]){
			ofpga[PU0_REG_BASE + PLL_FREQUENCY] = ocycleParams[0][ocycleNumber % 8]->pllInitialFrequency;
		}
		if(ocycleParams[1][ocycleNumber % 8]){
			ofpga[PU1_REG_BASE + PLL_FREQUENCY] = ocycleParams[1][ocycleNumber % 8]->pllInitialFrequency;
		}
		if(ocycleParams[2][ocycleNumber % 8]){
			ofpga[PU2_REG_BASE + PLL_FREQUENCY] = ocycleParams[2][ocycleNumber % 8]->pllInitialFrequency;
		}

		ofpga[ADC] |= ADC_LED_YELLOW;
	}

	olock.unlock();

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

void Pupe::cycleStop(){
	BUInt32	currentCycle;

	if(ostatus)
		return;

	olock.lock();
	oprocessingCycle = 0;

	currentCycle = ocycleNumber;
	ocycleCompletedNumber = ocycleNumber;


	// Load the Cycle Parameters if they are known
	if(ocycleParams[0][(currentCycle + 1) % 8]){
		setControlInfo(PuChannel(0, 0, 0), currentCycle + 1, *(ocycleParams[0][(currentCycle + 1) % 8]));
	}
	if(ocycleParams[1][(currentCycle + 1) % 8]){
		setControlInfo(PuChannel(0, 0, 1), currentCycle + 1, *(ocycleParams[1][(currentCycle + 1) % 8]));
	}
	if(ocycleParams[2][(currentCycle + 1) % 8]){
		setControlInfo(PuChannel(0, 0, 2), currentCycle + 1, *(ocycleParams[2][(currentCycle + 1) % 8]));
	}

	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]);
		ofpga[ADC] &= ~ADC_LED_YELLOW;
	}

	// 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);

#if DEBUG_INFO_TABLE
	printf("Pupe::cycleError: %d\n", puChan);	
	dumpState();
#endif

	// Clear the error
	ocontrol.cycleError(ocycleNumber, BError(ErrorStateTable, "FPGA State table transition error"));
	
	dprintf(DBG_FPGA, "Pupe::cycleError: End\n");
}

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

void Pupe::dumpState(){
	int	regBase = 0;
	int	brBase = 0;
	BUInt32	switchTable[16];
	BUInt8	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_CYCLE_INFO;
		fpgaReadData64(0, ocycleInfoTable, sizeof(ocycleInfoTable));

		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("CYCLE:		0x%x\n", ofpga[regBase + CYCLE]);
		printf("CONTROL:	0x%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("CycleInformationTable:\n");
		hd64(ocycleInfoTable, 4 * 16);

		printf("SwitchTable:\n");
		for(n = 0; n < 16; 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_OpenCardByIndex(oboard, &ofpgaCard)) != ADMXRC2_SUCCESS){
		err.set(ErrorFpga, getName() + ADMXRC2_GetStatusString(status));
		return err;
	}

	if((status = ADMXRC2_GetCardInfo(ofpgaCard, &ocardInfo)) != ADMXRC2_SUCCESS){
		err.set(ErrorFpga, getName() + ADMXRC2_GetStatusString(status));
		return err;
	}

	if((status = ADMXRC2_SetClockRate(ofpgaCard, 0, lclk, NULL)) != ADMXRC2_SUCCESS){
		err.set(ErrorFpga, getName() + ADMXRC2_GetStatusString(status));
		return err;
	}
	if((status = ADMXRC2_SetClockRate(ofpgaCard, 1, mclk, NULL)) != ADMXRC2_SUCCESS){
		err.set(ErrorFpga, getName() + 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 LOAD_USE_DMA
		if((status = ADMXRC2_ConfigureFromFileDMA(ofpgaCard, fileName, ADMXRC2_DMACHAN_ANY, 0)) != ADMXRC2_SUCCESS){
#else
		if((status = ADMXRC2_ConfigureFromFile(ofpgaCard, fileName)) != ADMXRC2_SUCCESS){
#endif
			return err.set(ErrorFpga, getName() + "Fpga Error: " + "Configuring FPGA from File: " + ADMXRC2_GetStatusString(status));
		}
	}
	else {
		return err.set(ErrorFpga, getName() + "Fpga Error: " + "File: " + fileName + " not found");
	}

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

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

	v = ofpga[FIRMWARE];
	if(((v >> 24) & 0xFF) != 'C'){
		err.set(ErrorFpga, getName() + "FPGA Firmware type incorrect");
	}
	else {
		nprintf("Pupe: FpgaVersion: %d.%d.%d\n", ((v >> 16) & 0xFF), ((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);
		pthread_testcancel();
		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);
	}

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

BError Pupe::fpgaConfig(){
	BError			err;
	ADMXRC2_STATUS		status;
	BUInt32			n;
	BUInt32			stateTable[16];
	int			data;

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

	status = ADMXRC2_SetupDMA(ofpgaCard, odmaBuffer, odmaBufferSize, 0, &odmabuf);
	if(status != ADMXRC2_SUCCESS){
		err.set(ErrorFpga, getName() + "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

	// Reset FPGA
	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);
	usleep(100000);

	ofpga[ADC] = 0x1FF;

	// Set State tables so that all paths lead to cycle stop
	for(n = 0; n < 16; n++){
		stateTable[n] = 0xffffff00;
	}
	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;

	if(opupeConfig.disableBlr){
		wprintf("BLR is disabled\n");
		ofpga[PU0_REG_BASE + CONTROL] = CONTROL_BLR_DISABLE;
		ofpga[PU1_REG_BASE + CONTROL] = CONTROL_BLR_DISABLE;
		ofpga[PU2_REG_BASE + CONTROL] = CONTROL_BLR_DISABLE;
	}

	// Setup Interrupt handler
	ointerruptThread.start();

#if DEBUG_DISABLE_ERROR_IRQ
	ofpga[IER] = INT_PU0_CYCLE_START | INT_PU0_CYCLE_STOP;
#else
	ofpga[IER] = INT_PU0_CYCLE_START | INT_PU0_CYCLE_STOP | INT_PU0_ERROR | INT_PU1_ERROR | INT_PU2_ERROR;
#endif
	// Enable timing bus simulation
	if(opupeConfig.internalTimingMask){
		ofpga[TIMING_IO] = TIMING_FREF | (opupeConfig.internalTimingMask << 8);
	}
	else {
		ofpga[TIMING_IO] = 0x0000;
	}

#if SYSCLK_INTERNAL
	ofpga[TIMING_IO] |= 0x0100;			// Force internal SysClock

	printf("TIMING_IO: %x\n", ofpga[TIMING_IO]);
	printf("TEST: %x\n", ofpga[PU0_REG_BASE + TEST]);
#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<BUInt32>	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(BUInt32))) <= 0){
		printf("Error: Reading file:\n");
	}
	data.resize(nb / sizeof(BUInt32));
	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);

	dprintf(DBG_FPGA, "fpgaReadData64: Offset: 0x%x MEM_REG: 0x%x, ADDR_REG: 0x%x, MEM_RAO: 0x%x, MEM_RAS: 0x%x NBytes: %d\n", fpgaOffset, ofpga[MEM_REG], ofpga[ADDR_REG], ofpga[MEM_RAO], ofpga[MEM_RAS], nbytes);

	ocontrol.odmaLock.lock();
	status = ADMXRC2_DoDMA(ofpgaCard, odmabuf, 0, nbytes,
			FPGA_DATA_ADDRESS + fpgaOffset, ADMXRC2_LOCALTOPCI, 0, dmaMode, 0, 0, 0);
	ocontrol.odmaLock.unlock();

	memcpy(toAddress, odmaBuffer, nbytes);

	if(status){
		err.set(ErrorFpga, getName() + "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);
				  
	ocontrol.odmaLock.lock();
	status = ADMXRC2_DoDMA(ofpgaCard, odmabuf, 0, nbytes,
			FPGA_DATA_ADDRESS + fpgaOffset, ADMXRC2_LOCALTOPCI, 0, dmaMode, 0, 0, 0);
	ocontrol.odmaLock.unlock();

	memcpy(toAddress, odmaBuffer, nbytes);

	if(status){
		err.set(ErrorFpga, getName() + "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);

	ocontrol.odmaLock.lock();
	status = ADMXRC2_DoDMA(ofpgaCard, odmabuf, 0, nbytes,
			FPGA_DATA_ADDRESS + fpgaOffset, ADMXRC2_PCITOLOCAL, 0, dmaMode, 0, 0, 0);
	ocontrol.odmaLock.unlock();

	if(status){
		err.set(ErrorFpga, getName() + "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;
#if DEBUG_DATA
		printf("MEM_RAO: %d MEM_RAS: %d\n", ofpga[MEM_RAO], ofpga[MEM_RAS]);
		printf("ReadData: Chan: %d Address: %x Page: %x Offset: %x NBytes: %d\n", pupeChan, address, dataTablePage, dataTableOffset, ncopy);
#endif

#if USE_DMA
		if(err = fpgaReadData64(dataTableOffset, &data.dataValues[addressOut], ncopy)){
			ofpga[MEM_REG] = SDRAM_BLOCKRAM;
			return err;
		}
#else		
		uint64_t*	dataTable = (uint64_t*)&ofpga[FPGA_MEM];

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

		dataTableOffset = 0;
		address = sdramAddressByteAdd(address, ncopy);
		addressOut += (ncopy / 8);
		nsamples -= (ncopy / 8);
	}
	ofpga[MEM_REG] = SDRAM_BLOCKRAM;
	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();
		ADMXRC2_CancelWaitForInterrupt(ofpgaCard);
		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::fpgaShutdown(){
	BError	err;

	// Disable interrupts
	ofpga[IER] = 0;

	// Clear FPGA
	ADMXRC2_ConfigureFromBuffer(ofpgaCard, 0, 0);
	
	return err;
}

BError Pupe::fpgaGetInfo(BList<NameValue>& infoList){
	BError		err;
	uint8_t*	p;

	if(ofpgaControl){
		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, getName() + "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;
}

BError Pupe::getTemperatures(double& tempInternal, double& tempFpga){
	BError		err;
	uint8_t*	p;

	if(ofpgaControl){
		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, getName() + "SMB Serial Bus Error - Shadow Read Failed");
		}

		p = (uint8_t*)&ofpgaControl[SYSMON_BUF] + 0x20;
		tempInternal = p[7] * sysFactors[7];
		tempFpga = p[6] * sysFactors[6];
	}

	return err;
}
