software: C library/test program for V3

f82c33d7 · Tomasz Wlostowski · ef09fc06 · f82c33d7 · f82c33d7 · f82c33d7
Commit f82c33d7 authored Feb 27, 2012 by Tomasz Wlostowski
9 changed files
--- a/software/lib/Makefile
+++ b/software/lib/Makefile
-CFLAGS = -I../include -g -Imini_bone
+CFLAGS = -I../include -g -Imini_bone -Ispll
-OBJS_LIB = fdelay_lib.o rr_io.o  i2c_master.o mini_bone/minibone_lib.o mini_bone/ptpd_netif.o  fdelay_bus.o
+OBJS_LIB = fdelay_lib.o rr_io.o  i2c_master.o onewire.o mini_bone/minibone_lib.o mini_bone/ptpd_netif.o  fdelay_bus.o 
-all:	$(OBJS_LIB)
+all:    testprog lib testprog2
+lib:	$(OBJS_LIB)
 		gcc -shared -o libfinedelay.so $(OBJS_LIB)
+testprog:	lib fdelay_test.o
+		gcc -o fdelay_test $(OBJS_LIB) fdelay_test.o -lm
+testprog2:	lib fdelay_cal.o
+		gcc -o fdelay_cal $(OBJS_LIB) fdelay_cal.o -lm
 clean:	
 		rm -f libfinedelay.so $(OBJS_LIB)
\ No newline at end of file
--- a/software/lib/fdelay_cal.c
+++ b/software/lib/fdelay_cal.c
+#include <stdio.h>
+#include <stdint.h>
+#include "fdelay_lib.h"
+#include "fdelay_private.h"
+#include "fd_main_regs.h"
+#include "onewire.h"
+#include "rr_io.h"
+typedef struct {
+   float kp, ki, err, pwm, setpoint, i, bias;
+} pi_t;
+pi_t pi_state = {15.0, 5.0, 0, 0, 20, 0, 2048};
+void pi_update(fdelay_device_t *dev, float temp)
+{
+    fd_decl_private(dev);
+    pi_state.err = temp - pi_state.setpoint;
+    pi_state.i += pi_state.err;
+    pi_state.pwm = pi_state.bias + pi_state.kp * pi_state.err + pi_state.ki * pi_state.i;
+    dbg("t %.1f err:%.1f DRIVE: %d\n", temp, pi_state.err, (int)pi_state.pwm);
+    fd_writel(FD_I2CR_DBGOUT_W((int)pi_state.pwm), FD_REG_I2CR);
+}
+extern int64_t get_tics();
+static int64_t last_tics = 0;
+#define TEMP_REG_PERIOD 1000000LL
+int pi_set_temp(fdelay_device_t *dev, float new_temp)
+{
+    int temp;
+    float temp_f;
+    if(get_tics() - last_tics < TEMP_REG_PERIOD)
+        return 0;
+    last_tics = get_tics();
+    if(ds18x_read_temp(dev, &temp) < 0)
+        return 0;
+    temp_f = (float)temp / 16.0;
+    pi_state.setpoint = new_temp;
+    pi_update(dev, temp_f);
+    dbg("Temperature: %.1f degC err %.1f\n", temp_f, pi_state.err);
+    return fabs(pi_state.err) < 0.1 ? 1: 0;
+}
+void my_writel(void *priv, uint32_t data, uint32_t addr)
+{
+ 	rr_writel(data, addr);
+}
+uint32_t my_readl(void *priv, uint32_t addr)
+{
+	uint32_t d = rr_readl(addr);
+	return d;
+}
+main()
+{
+	fdelay_device_t *dev = malloc(sizeof(fdelay_device_t));
+	rr_init();
+	dev->writel = my_writel;
+	dev->readl = my_readl;
+	dev->base_addr = 0x80000;
+	if(fdelay_init(dev) < 0)
+		return -1;
+    float t_min = 40.0, t_max = 80.0, t_cur;
+    t_cur = t_min;
+    for(;;)
+    {
+        if(pi_set_temp(dev, t_cur))
+        {
+            fd_decl_private(dev);
+            calibrate_outputs(dev);
+            fprintf(stderr, "> %.1f %d %d %d %d\n", t_cur, hw->frr_cur[0],
+             hw->frr_cur[1], hw->frr_cur[2], hw->frr_cur[3]);
+             t_cur += 1.0;
+             if(t_cur > t_max)
+             break;
+        }
+        usleep(10000);
+    }
+}
--- a/software/lib/fdelay_lib.c
+++ b/software/lib/fdelay_lib.c
-/* 
+/*
 	FmcDelay1ns4Cha (a.k.a. The Fine Delay Card)
 	User-space driver/library
 	Tomasz Włostowski/BE-CO-HT, 2011
 	(c) Copyright CERN 2011
 	Licensed under LGPL 2.1
 */
@@ -12,27 +12,38 @@
 #include <stdlib.h>
 #include <stdint.h>
 #include <stdarg.h>
+#include <string.h>
 #include <unistd.h>
 #include <sys/time.h>
 #include <math.h>
-#include "fdelay_regs.h"
+#include "fd_channel_regs.h"
+#include "fd_main_regs.h"
 #include "pll_config.h"
 #include "acam_gpx.h"
 #include "fdelay_lib.h"
 #include "fdelay_private.h"
+#include "spll_defs.h"
+#include "spll_common.h"
+#include "spll_helper.h"
+#include "onewire.h"
+static int acam_test_bus(fdelay_device_t *dev);
-/* 
+/*
 ----------------------
 Some utility functions
---------------------- 
+----------------------
 */
 static int extra_debug = 1;
-static void dbg(const char *fmt, ...)
+void dbg(const char *fmt, ...)
 {
 	va_list ap;
 	va_start(ap, fmt);
@@ -42,7 +53,7 @@ static void dbg(const char *fmt, ...)
 }
 /* Returns the numer of microsecond timer ticks */
-static inline int64_t get_tics()
+int64_t get_tics()
 {
    struct timezone tz= {0,0};
    struct timeval tv;
@@ -58,7 +69,34 @@ void udelay(uint32_t usecs)
  while(get_tics() - ts < (int64_t)usecs);
 }
-/* 
+/* Card reset. When mode == RESET_HW, resets the FMC hardware by asserting the reset line in the FMC
+   connector, if mode == RESET_CORE, the FPGA Fine Delay core is reset. Since HW reset operation also
+   reinitializes the PLL, the HW reset must be followed by a reinitialization of the FD Core. */
+#define FD_RESET_HW 1
+#define FD_RESET_CORE 0
+static void fd_do_reset(fdelay_device_t *dev, int mode)
+{
+  fd_decl_private(dev) ;
+  if(mode == FD_RESET_HW) {
+    fd_writel(FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_CORE_MASK, FD_REG_RSTR);
+    udelay(10000);
+    fd_writel(FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_CORE_MASK | FD_RSTR_RST_FMC_MASK, FD_REG_RSTR);
+    udelay(600000); /* Leave the TPS3307 supervisor some time to de-assert the master reset line */
+  } else if (mode == FD_RESET_CORE)
+    {
+    fd_writel(FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_FMC_MASK, FD_REG_RSTR);
+    udelay(1000);
+    fd_writel(FD_RSTR_LOCK_W(0xdead) | FD_RSTR_RST_FMC_MASK | FD_RSTR_RST_CORE_MASK, FD_REG_RSTR);
+    udelay(1000);
+    }
+}
+/*
 ----------------------------------
 Simple SPI Master driver
 ----------------------------------
@@ -67,7 +105,7 @@ Simple SPI Master driver
 /* Initializes the SPI Controller */
 static void oc_spi_init(fdelay_device_t *dev)
 {
-	fd_decl_private(dev) 
+	fd_decl_private(dev)
 }
@@ -75,31 +113,29 @@ static void oc_spi_init(fdelay_device_t *dev)
 static void oc_spi_txrx(fdelay_device_t *dev, int ss, int num_bits, uint32_t in, uint32_t *out)
 {
 	fd_decl_private(dev);
-    uint32_t scr = 0, r;
+	uint32_t scr = 0, r;
-    scr = FD_SCR_DATA_W(in) | FD_SCR_CPOL;
+	scr = FD_SCR_DATA_W(in)| FD_SCR_CPOL;
 	if(ss == CS_PLL)
 		scr |= FD_SCR_SEL_PLL;
 	else if(ss == CS_GPIO)
 		scr |= FD_SCR_SEL_GPIO;
 	fd_writel(scr, FD_REG_SCR);
 	fd_writel(scr | FD_SCR_START, FD_REG_SCR);
 	while(! (fd_readl(FD_REG_SCR) & FD_SCR_READY));
 	scr = fd_readl(FD_REG_SCR);
 	r = FD_SCR_DATA_R(scr);
-//	if(ss==CS_PLL)
-//		printf("IN %x OUT %x\n", in, scr);
 	if(out) *out=r;
 	udelay(100);
 }
-/* 
+/*
 -----------------
 AD9516 PLL Driver
 -----------------
-*/ 
+*/
 /* Writes an AD9516 register */
 static inline void ad9516_write_reg(fdelay_device_t *dev, uint16_t reg, uint8_t val)
@@ -112,141 +148,153 @@ static inline uint8_t ad9516_read_reg(fdelay_device_t *dev, uint16_t reg)
 {
 	uint32_t rval;
 	oc_spi_txrx(dev, CS_PLL, 24, ((uint32_t)(reg & 0xfff) << 8) | (1<<23), &rval);
-	printf("ReadReg: %x %x\n", reg, rval);
 	return rval & 0xff;
 }
 /* Initializes the AD9516 PLL by loading a pre-defined register set and waiting until the PLL has locked */
 static int ad9516_init(fdelay_device_t *dev)
 {
-	fd_decl_private(dev)
+  fd_decl_private(dev)
-	int i;
+    int i;
-	const int64_t lock_timeout = 10000000LL;
+  const int64_t lock_timeout = 10000000LL;
-	int64_t start_tics;
+  int64_t start_tics;
-    dbg("%s: Initializing AD9516 PLL...\n", __FUNCTION__);
-	ad9516_write_reg(dev, 0, 0x99);
-//xit(-1);
-	ad9516_write_reg(dev, 0x232, 1);
-	/* Check if the chip is present by reading its ID register */
+  dbg("%s: Initializing AD9516 PLL...\n", __FUNCTION__);
-	if(ad9516_read_reg(dev, 0x3) != 0xc3)
+  ad9516_write_reg(dev, 0, 0x99);
-	{
+  ad9516_write_reg(dev, 0x232, 1);
-	  dbg("%s: AD9516 PLL not responding.\n", __FUNCTION__);
-	  return -1;
-	}
-	/* Load the regs */
-	for(i=0;ad9516_regs[i].reg >=0 ;i++)
-		ad9516_write_reg (dev, ad9516_regs[i].reg, ad9516_regs[i].val);
-	ad9516_write_reg(dev, 0x232, 1);
+  /* Check if the chip is present by reading its ID register */
+  if(ad9516_read_reg(dev, 0x3) != 0xc3)
+    {
+      dbg("%s: AD9516 PLL not responding.\n", __FUNCTION__);
+      return -1;
+    }
-	/* Wait until the PLL has locked */
+  /* Load the regs */
+  for(i=0;ad9516_regs[i].reg >=0 ;i++)
+    ad9516_write_reg (dev, ad9516_regs[i].reg, ad9516_regs[i].val);
-	start_tics = get_tics();
+  ad9516_write_reg(dev, 0x232, 1);
-	for(;;)
+  /* Wait until the PLL has locked */
+  start_tics = get_tics();
+  for(;;)
+    {
+      if(ad9516_read_reg(dev, 0x1f) & 1)
+	break;
+      if(get_tics() - start_tics > lock_timeout)
 	{
-		if(ad9516_read_reg(dev, 0x1f) & 1)
+	  dbg("%s: AD9516 PLL does not lock.\n", __FUNCTION__);
-			break;
+	  return -1;
+	}
-		if(get_tics() - start_tics > lock_timeout)
+      udelay(100);
-		{
-			 dbg("%s: AD9516 PLL does not lock.\n", __FUNCTION__);
-			 return -1;
-		}
-		udelay(100);
    }
-	/* Synchronize the phase of all clock outputs (this is critical for the accuracy!) */
-	ad9516_write_reg(dev, 0x230, 1);
-	ad9516_write_reg(dev, 0x232, 1);
-	ad9516_write_reg(dev, 0x230, 0);
-	ad9516_write_reg(dev, 0x232, 1);
-    dbg("%s: AD9516 locked.\n", __FUNCTION__);
+  /* Synchronize the phase of all clock outputs (this is critical for the accuracy!) */
+  ad9516_write_reg(dev, 0x230, 1);
+  ad9516_write_reg(dev, 0x232, 1);
+  ad9516_write_reg(dev, 0x230, 0);
+  ad9516_write_reg(dev, 0x232, 1);
-	return 0;
+  dbg("%s: AD9516 locked.\n", __FUNCTION__);
+  return 0;
 }
-/* 
+/*
 ----------------------------
 MCP23S17 SPI I/O Port Driver
 ----------------------------
-*/ 
+*/
 /* Writes MCP23S17 register */
 static inline void mcp_write(fdelay_device_t *dev, uint8_t reg, uint8_t val)
 {
-	oc_spi_txrx(dev, CS_GPIO, 24, 0x4e0000 | ((uint32_t)reg<<8) | val, NULL);
+  oc_spi_txrx(dev, CS_GPIO, 24, 0x4e0000 | (((uint32_t)reg)<<8) | (uint32_t)val, NULL);
 }
 /* Reads MCP23S17 register */
 static uint8_t mcp_read(fdelay_device_t *dev, uint8_t reg)
 {
-	uint32_t rval;
+  uint32_t rval;
-	oc_spi_txrx(dev, CS_GPIO, 24, 0x4f0000 | ((uint32_t)reg<<8), &rval);
+  oc_spi_txrx(dev, CS_GPIO, 24, 0x4f0000 | (((uint32_t)reg)<<8), &rval);
-	return rval & 0xff;
+  return rval & 0xff;
+}
+static void sgpio_init(fdelay_device_t *dev)
+{
+  mcp_write(dev, MCP_IOCON, 0);
 }
 /* Sets the direction (0 = input, non-zero = output) of a particular MCP23S17 GPIO pin */
 static void sgpio_set_dir(fdelay_device_t *dev, int pin, int dir)
 {
-	uint8_t iodir = (MCP_IODIR) + (pin & 0x100 ? 1 : 0);
+  uint8_t iodir = (MCP_IODIR) + (pin & 0x100 ? 1 : 0);
-    uint8_t x;
+  uint8_t x;
-    x = mcp_read(dev, iodir);
+  x = mcp_read(dev, iodir);
-	if(dir) x &= ~(pin); else x |= (pin);
+  if(dir) x &= ~(pin); else x |= (pin);
-	mcp_write(dev, iodir, x);
+  mcp_write(dev, iodir, x);
 }
 /* Sets the value on a given MCP23S17 GPIO pin */
 static void sgpio_set_pin(fdelay_device_t *dev, int pin, int val)
 {
-	uint8_t gpio = (MCP_GPIO) + (pin & 0x100 ? 1 : 0);
+  uint8_t gpio = (MCP_OLAT) + (pin & 0x100 ? 1 : 0);
-    uint8_t x;
+  uint8_t x;
-    x = mcp_read(dev, gpio);
+  x = mcp_read(dev, gpio);
+  if(!val) x &= ~(pin); else x |= (pin);
-	if(!val) x &= ~(pin); else x |= (pin);
+  mcp_write(dev, gpio, x);
-	mcp_write(dev, gpio, x);
 }
-/* 
+/*
 ----------------------------------------
 ACAM Time To Digital Converter functions
 ----------------------------------------
 */
-/* Writes a particular ACAM register. Works only if (GCR.BYPASS == 1) - i.e. when
+/* Sets the address on ACAM's address bus to addr using the SPI GPIO expander */
-   the ACAM is controlled from the host instead of the delay core. */
-static void acam_write_reg(fdelay_device_t *dev, uint8_t reg, uint32_t data)
-{
-	fd_decl_private(dev)
-	fd_writel((((uint32_t) (reg)) << 28) | (data & 0xfffffff), FD_REG_TAR);
+static inline void acam_set_address(fdelay_device_t *dev, uint8_t addr)
-	udelay(1);
+{
-	fd_writel(FD_TDCSR_WRITE, FD_REG_TDCSR);
+  fd_decl_private(dev);
-	udelay(1);
+  /* A hack to speed up calibration - avoid setting the same address several times */
+  if(addr != hw->acam_addr)
+  {
+      mcp_write(dev, MCP_IODIR + 1, 0);
+      mcp_write(dev, MCP_OLAT + 1,  addr & 0xf);
+      hw->acam_addr = addr;
+  }
 }
 /* Reads a register from the ACAM TDC. As for the function above, GCR.BYPASS must be enabled */
 static uint32_t acam_read_reg(fdelay_device_t *dev, uint8_t reg)
 {
-	fd_decl_private(dev)
+  fd_decl_private(dev)
+  acam_set_address(dev, reg);
+  fd_writel(FD_TDCSR_READ, FD_REG_TDCSR);
+  return fd_readl(FD_REG_TDR) & 0xfffffff;
+}
-	fd_writel((((uint32_t) (reg)) << 28), FD_REG_TAR);
-	udelay(1);
+/* Writes a particular ACAM register. Works only if (GCR.BYPASS == 1) - i.e. when
-	fd_writel(FD_TDCSR_READ, FD_REG_TDCSR);
+   the ACAM is controlled from the host instead of the delay core. */
-	udelay(1);
+static void acam_write_reg(fdelay_device_t *dev, uint8_t reg, uint32_t data)
-	return fd_readl(FD_REG_TAR) & 0xfffffff;
+{
+	fd_decl_private(dev)
+	acam_set_address(dev, reg);
+  fd_writel(data & 0xfffffff, FD_REG_TDR);
+  fd_writel(FD_TDCSR_WRITE, FD_REG_TDCSR);
 }
-/* Calculates the parameters of the ACAM PLL (hsdiv and refdiv) 
+/* Calculates the parameters of the ACAM PLL (hsdiv and refdiv)
   for a given bin size and reference clock frequency. Returns the closest
   achievable bin size. */
 static double acam_calc_pll(int *hsdiv, int *refdiv, double bin, double clock_freq)
@@ -277,6 +325,7 @@ static double acam_calc_pll(int *hsdiv, int *refdiv, double bin, double clock_fr
 	return best_bin;
 }
 /* Returns non-zero if the ACAM's internal PLL is locked */
 static inline int acam_pll_locked(fdelay_device_t *dev)
 {
@@ -287,11 +336,32 @@ static inline int acam_pll_locked(fdelay_device_t *dev)
 /* Configures the ACAM TDC to work in a particular mode. Currently there are two modes
   supported: R-Mode for the normal operation (delay/timestamper) and I-Mode for the purpose
   of calibrating the fine delay lines. */
+static int acam_test_bus(fdelay_device_t *dev)
+{
+    int i;
+    dbg("Testing ACAM Bus...\n");
+    for(i=0;i<28;i++)
+    {
+        acam_write_reg(dev, 5, (1<<i));
+        acam_read_reg(dev, 0);
+        uint32_t rb = acam_read_reg(dev, 5);
+        if(rb != (1<<i))
+        {
+            dbg("Bit failure on ACAM_D[%d]: %x shouldbe %x \n", i, rb, (1<<i));
+            return -1;
+        }
+    }
+}
 static int acam_configure(fdelay_device_t *dev, int mode)
 {
 	fd_decl_private(dev)
 	int hsdiv, refdiv;
 	int64_t start_tics;
 	const int64_t lock_timeout = 2000000LL;
@@ -304,12 +374,12 @@ static int acam_configure(fdelay_device_t *dev, int mode)
 	if(mode == ACAM_RMODE)
 	{
 	 	acam_write_reg(dev, 0, AR0_ROsc | AR0_RiseEn0 | AR0_RiseEn1 | AR0_HQSel );
-	 	acam_write_reg(dev, 1, AR1_Adj(0, 0) | 
+	 	acam_write_reg(dev, 1, AR1_Adj(0, 0) |
-	 					  AR1_Adj(1, 2) | 
+	 					  AR1_Adj(1, 2) |
-	 					  AR1_Adj(2, 6) | 
+	 					  AR1_Adj(2, 6) |
-	 					  AR1_Adj(3, 0) | 
+	 					  AR1_Adj(3, 0) |
-	 					  AR1_Adj(4, 2) | 
+	 					  AR1_Adj(4, 2) |
-	 					  AR1_Adj(5, 6) | 
+	 					  AR1_Adj(5, 6) |
 	 					  AR1_Adj(6, 0));
 	   	acam_write_reg(dev, 2, AR2_RMode | AR2_Adj(7, 2) | AR2_Adj(8, 6));
 	   	acam_write_reg(dev, 3, 0);
@@ -320,9 +390,10 @@ static int acam_configure(fdelay_device_t *dev, int mode)
 	   	acam_write_reg(dev, 11, 0x7ff0000);
 	   	acam_write_reg(dev, 12, 0x0000000);
 	   	acam_write_reg(dev, 14, 0);
 		/* Reset the ACAM after the configuration */
 	   	acam_write_reg(dev, 4, AR4_EFlagHiZN | AR4_MasterReset | AR4_StartTimer(0));
 	} else if (mode == ACAM_IMODE)
 	{
 		acam_write_reg(dev, 0, AR0_TRiseEn(0) | AR0_HQSel | AR0_ROsc);
@@ -336,9 +407,11 @@ static int acam_configure(fdelay_device_t *dev, int mode)
 		/* Reset the ACAM after the configuration */
 		acam_write_reg(dev, 4, AR4_EFlagHiZN | AR4_MasterReset | AR4_StartTimer(0));
-	} else         
+	} else
 		return -1;  /* Unsupported mode? */
+	int i;
 	dbg("%s: Waiting for ACAM ring oscillator lock...\n", __FUNCTION__);
 	start_tics = get_tics();
@@ -346,7 +419,7 @@ static int acam_configure(fdelay_device_t *dev, int mode)
 	{
 		if(acam_pll_locked(dev))
 			break;
 		if(get_tics() - start_tics > lock_timeout)
 		{
 			 dbg("%s: ACAM PLL does not lock.\n", __FUNCTION__);
@@ -355,20 +428,27 @@ static int acam_configure(fdelay_device_t *dev, int mode)
 		usleep(10000);
    }
+	acam_set_address(dev, 8); /* Permamently select FIFO1 register for readout */
    return 0;
 }
-/* 
+/*
 ---------------------
 Calibration functions
 ---------------------
 */
+#define chan_writel(data, addr) fd_writel((data),  channel * 0x100 + (addr))
+#define chan_readl(addr) fd_readl(channel * 0x100 + (addr))
 /* Measures the the FPGA-generated TDC start and the output of one of the fine delay chips (channel)
   at a pre-defined number of taps (fine). Retuns the delay in picoseconds. The measurement is repeated
-   and averaged (n_avgs) times. Also, the standard deviation of the result can be written to (sdev) 
+   and averaged (n_avgs) times. Also, the standard deviation of the result can be written to (sdev)
   if it's not NULL. */
 static double measure_output_delay(fdelay_device_t *dev, int channel, int fine, int n_avgs, double *sdev)
 {
 	fd_decl_private(dev)
@@ -377,51 +457,61 @@ static double measure_output_delay(fdelay_device_t *dev, int channel, int fine,
 	int i;
 /* Mapping between the channel of the delay card and the stop inputs of the ACAM */
-	int chan_to_acam[5] = {0, 5, 6, 3, 4}; 
+	int chan_to_acam[5] = {0, 4, 3, 2, 1};
 /* Mapping between the channel number and the time tag FIFOs of the ACAM */
-	int chan_to_fifo[5] = {0, 9, 9, 8, 8};
+	int chan_to_fifo[5] = {0, 8, 8, 8, 8};
 	double rec[1024];
+/* Disable the output for the channel being calibrated */
+    sgpio_set_pin(dev, SGPIO_OUTPUT_EN(channel), 0);
 	/* Enable the stop input in the ACAM corresponding to the channel being calibrated */
 	acam_write_reg(dev, 0, AR0_TRiseEn(0) | AR0_TRiseEn(chan_to_acam[channel]) | AR0_HQSel | AR0_ROsc);
    /* Program the output delay line setpoint */
-	fd_writel( fine, FD_REG_FRR1  + 0x20 * (channel - 1));
+	chan_writel( fine, FD_REG_FRR);
-   	fd_writel( FD_DCR1_FORCE_DLY | FD_DCR1_POL, FD_REG_DCR1 + 0x20 * (channel - 1));
+   	chan_writel( FD_DCR_ENABLE | FD_DCR_MODE | FD_DCR_UPDATE, FD_REG_DCR);
+   	chan_writel( FD_DCR_FORCE_DLY | FD_DCR_ENABLE, FD_REG_DCR);
   	/* Set the calibration pulse mask to genrate calibration pulses only on one channel at a time.
   	   This minimizes the crosstalk in the output buffer which can severely decrease the accuracy
   	   of calibration measurements */
    fd_writel( FD_CALR_PSEL_W(1<<(channel-1)), FD_REG_CALR);
 	udelay(1);
 	/* Do n_avgs single measurements and average */
-	for(i=0;i<n_avgs;i++)
+    for(i=0;i<n_avgs;i++)
 	{
 		uint32_t fr;
 		/* Re-arm the ACAM (it's working in a single-shot mode) */
 		fd_writel( FD_TDCSR_ALUTRIG, FD_REG_TDCSR);
-	udelay(1);
+    	udelay(1);
 		/* Produce a calibration pulse on the TDC start and the appropriate output channel */
        fd_writel( FD_CALR_CAL_PULSE | FD_CALR_PSEL_W((1<<(channel-1))), FD_REG_CALR);
-           	udelay(1);
+      	udelay(1);
 		/* read the tag, convert to picoseconds and average */
 		fr = acam_read_reg(dev, chan_to_fifo[channel]);
 	 	double tag = (double)((fr >> 0) & 0x1ffff) * hw->acam_bin * 3.0;
+//	 	dbg("Tag %.1f\n", tag);
 	 	acc += tag;
 	 	rec[i] = tag;
 	}
-	/* Calculate standard dev and average value */	
+	/* Calculate standard dev and average value */
 	acc /= (double) n_avgs;
 	for(i=0;i<n_avgs;i++)
 		std += (rec[i] - acc) * (rec[i] - acc);
 	if(sdev) *sdev = sqrt(std /(double) n_avgs);
+   	chan_writel( 0, FD_REG_DCR);
 	return acc;
 }
@@ -432,11 +522,11 @@ static void dbg_transfer_function(fdelay_device_t *dev)
 	fd_decl_private(dev)
 	int channel, i;
-	double bias, x, meas[FDELAY_NUM_TAPS][4], sdev[FDELAY_NUM_TAPS][4];	
+	double bias, x, meas[FDELAY_NUM_TAPS][4], sdev[FDELAY_NUM_TAPS][4];
 	fd_writel( FD_GCR_BYPASS, FD_REG_GCR);
 	acam_configure(dev, ACAM_IMODE);
 	fd_writel( FD_TDCSR_START_EN | FD_TDCSR_STOP_EN, FD_REG_TDCSR);
 	for(channel = 1; channel <= 4; channel++)
@@ -445,35 +535,37 @@ static void dbg_transfer_function(fdelay_device_t *dev)
 		bias = measure_output_delay(dev, channel, 0, FDELAY_CAL_AVG_STEPS, &sdev[0][channel-1]);
 		meas[0][channel-1] = 0.0;
 		for(i=FDELAY_NUM_TAPS-1;i>=0;i--)
-		{	
+		{
-			x = measure_output_delay(dev, channel, i, 
+			x = measure_output_delay(dev, channel, i,
 				FDELAY_CAL_AVG_STEPS, &sdev[i][channel-1]);
 			meas[i][channel-1] = x - bias;
 		}
 	}
 	FILE *f=fopen("t_func.dat","w");
 	for(i=0;i<FDELAY_NUM_TAPS;i++)
 	{
-	 	fprintf(f, "%d %.0f %.0f %.0f %.0f %.0f %.0f %.0f %.0f\n", i, 
+	 	fprintf(f, "%d %.0f %.0f %.0f %.0f %.0f %.0f %.0f %.0f\n", i,
 	 	meas[i][0], meas[i][1], meas[i][2], meas[i][3],
 	 	sdev[i][0], sdev[i][1], sdev[i][2], sdev[i][3]);
 	}
 	fclose(f);
 }
-/* Finds the preset (i.e. the numer of taps) of the output delay line in (channel) 
+/* Finds the preset (i.e. the numer of taps) of the output delay line in (channel)
   at which it introduces exactly 8 ns more than when it's programmed to 0 taps.
-   Uses a binary search algorithm to speed up the calibration (assuming that the 
+   Uses a binary search algorithm to speed up the calibration (assuming that the
   line is monotonous). */
 static int find_8ns_tap(fdelay_device_t *dev, int channel)
 {
 	int l = 0, r=FDELAY_NUM_TAPS-1;
-/* Measure the delay at zero setting, so it can be further subtracted to get only the 
+    dbg("Calibrating: %d\n", channel);
+/* Measure the delay at zero setting, so it can be further subtracted to get only the
   delay part introduced by the delay line (ingoring the TDC, FPGA and routing delays). */
 	double bias = measure_output_delay(dev, channel, 0, FDELAY_CAL_AVG_STEPS, NULL);
@@ -481,33 +573,70 @@ static int find_8ns_tap(fdelay_device_t *dev, int channel)
 	{
 		int mid = (l+r) / 2;
 	 	double dly = measure_output_delay(dev, channel, mid, FDELAY_CAL_AVG_STEPS, NULL) - bias;
 	 	if(dly < 8000.0) l = mid; else r = mid;
 	}
 	return l;
+}
+/* Evaluates 2nd order polynomial. Coefs have 32 fractional bits. */
+static int32_t eval_poly(int64_t *coef, int32_t x)
+{
+    int32_t y;
+    y= (coef[0] * (int64_t)x * (int64_t)x + coef[1] * (int64_t) x + coef[2]) >> 32;
+    return (int32_t) y;
 }
 /* Performs the startup calibration of the output delay lines. */
 void calibrate_outputs(fdelay_device_t *dev)
 {
 	fd_decl_private(dev)
-	int i, channel;
+	int i, channel, temp;
 //	dbg_transfer_function(dev);
-	fd_writel( FD_GCR_BYPASS, FD_REG_GCR);
+    fd_writel( FD_GCR_BYPASS, FD_REG_GCR);
 	acam_configure(dev, ACAM_IMODE);
 	fd_writel( FD_TDCSR_START_EN | FD_TDCSR_STOP_EN, FD_REG_TDCSR);
 	for(channel = 1; channel <= 4; channel++)
-	{
+	{   
-		int cal_val = find_8ns_tap(dev, channel);
+        while(ds18x_read_temp(dev, &temp) < 0)
-     	dbg("%s: CH%d: 8ns @ %d\n", __FUNCTION__, channel, cal_val);
+            usleep(100000);
-     	hw->frr[channel-1] = cal_val;
+    	int cal_measd = find_8ns_tap(dev, channel);
+    	int cal_fitted = eval_poly(hw->calib.frr_poly, temp);
+     	dbg("%s: CH%d: 8ns @ %d (fitted %d, offset %d, temperature %d.%1d)\n", __FUNCTION__, channel, cal_measd, cal_fitted, cal_measd-cal_fitted, temp);
+     	hw->frr_cur[channel-1] = cal_measd;
+     	hw->frr_offset[channel-1] = cal_measd - cal_fitted;
 	}
 }
+/* TODO: run in a timer context every few seconds instead of the main program loop */
+void fdelay_update_calibration(fdelay_device_t *dev)
+{
+	fd_decl_private(dev);
+	int channel, temp;
+    ds18x_read_temp(dev, &temp);
+	for(channel = 1; channel <= 4; channel++)
+	{   
+    	int cal_fitted = eval_poly(hw->calib.frr_poly, temp) + hw->frr_offset[channel-1];
+     	dbg("%s: CH%d: FRR = %d\n", __FUNCTION__, channel,  cal_fitted);
+     	hw->frr_cur[channel-1] = cal_fitted;
+     	chan_writel(hw->frr_cur[channel-1],  FD_REG_FRR);
+	}
+}
 #if 0
 void poll_stats()
 {
@@ -530,15 +659,20 @@ void poll_stats()
 static int read_calibration_eeprom(fdelay_device_t *dev, struct fine_delay_calibration *d_cal)
 {
 	struct fine_delay_calibration cal;
 	mi2c_init(dev);
 	if(eeprom_read(dev, EEPROM_ADDR, 0, (uint8_t *) &cal, sizeof(struct fine_delay_calibration)) != sizeof(struct fine_delay_calibration))
+ 	{
+ 	    dbg("Can't read calibration EEPROM.\n");
 		return -1;
+    }
 	if(cal.magic != FDELAY_MAGIC_ID)
-		return -1;
+	{
+	    dbg("EEPROM doesn't contain valid calibration block.\n");
+ 	    return -1;
+	}
 	memcpy(d_cal, &cal, sizeof(cal));
 	return 0;
 }
@@ -552,170 +686,172 @@ static int read_calibration_eeprom(fdelay_device_t *dev, struct fine_delay_calib
 /* Initialize & self-calibrate the Fine Delay card */
 int fdelay_init(fdelay_device_t *dev)
 {
-	struct fine_delay_hw *hw;
+  int i;
+  struct fine_delay_hw *hw;
-	dbg("Init: dev %x\n", dev);
+  fdelay_time_t t_zero;
-	hw = (struct fine_delay_hw *) malloc(sizeof(struct fine_delay_hw));
-	if(! hw)
-		return -1;
-	dev->priv_fd = (void *) hw;
+  dbg("Init: dev %x\n", dev);
+  hw = (struct fine_delay_hw *) malloc(sizeof(struct fine_delay_hw));
-	hw->base_addr = dev->base_addr;	
+  if(! hw)
- 	hw->base_i2c = 0x100;
+    return -1;
- 	hw->base_onewire = 0x200;
- 	hw->wr_enabled = 0;
- 	hw->wr_state = FDELAY_FREE_RUNNING;
+  dev->priv_fd = (void *) hw;
-	dbg("%s: Initializing the Fine Delay Card\n", __FUNCTION__);
+  hw->base_addr = dev->base_addr;
+  hw->base_i2c = 0x100;
-	/* Read the Identification register and check if we are talking to a proper Fine Delay HDL Core */
+  hw->base_onewire = dev->base_addr + 0x500;
-	if(fd_readl(FD_REG_IDR) != FDELAY_MAGIC_ID)
+  hw->wr_enabled = 0;
-	{
+  hw->wr_state = FDELAY_FREE_RUNNING;
-	 	dbg("%s: invalid core signature. Are you sure you have loaded the FPGA with the Fine Delay firmware?\n", __FUNCTION__);
+  hw->acam_addr = 0xff;
-	 	return -1;
-	}
+  dbg("%s: Initializing the Fine Delay Card\n", __FUNCTION__);
-	//if(read_calibration_eeprom(dev, &hw->calib) < 0)
+  /* Read the Identification register and check if we are talking to a proper Fine Delay HDL Core */
+  if(fd_readl(FD_REG_IDR) != FDELAY_MAGIC_ID)
+    {
+      dbg("%s: invalid core signature. Are you sure you have loaded the FPGA with the Fine Delay firmware?\n", __FUNCTION__);
+      return -1;
+    }
+  if(read_calibration_eeprom(dev, &hw->calib) < 0)
+  {
+    int i;
+    dbg("%s: Calibration EEPROM not found or unreadable. Using default calibration values\n", __FUNCTION__);
+    hw->calib.frr_poly[0] = -165202LL;
+    hw->calib.frr_poly[1] = -29825595LL;
+    hw->calib.frr_poly[2] = 3801939743082LL;
+    hw->calib.tdc_zero_offset = 35600;
+    hw->calib.atmcr_val =  2 | (1000 << 4);
+    hw->calib.adsfr_val = 56648;
+    hw->calib.acam_start_offset = 10000;
+    for(i=0;i<4;i++)
+      hw->calib.zero_offset[i] = 50000;
+  }
+  /* Reset the FMC hardware. */
+  fd_do_reset(dev, FD_RESET_HW);
+  /* Initialize the clock system - AD9516 PLL */
+  oc_spi_init(dev);
+  sgpio_init(dev);
+  if(ad9516_init(dev) < 0)
+    return -1;
+	if(ds18x_init(dev) < 0)
 	{
-		int i;
+		dbg("DS18x sensor not detected. Bah!\n");
-		dbg("%s: Calibration EEPROM not found or unreadable. Using default calibration values\n", __FUNCTION__);
+	return -1;
-		hw->calib.tdc_zero_offset = 35600;
-		hw->calib.atmcr_val =  1 | (2000 << 4);
-		hw->calib.adsfr_val = 56648;
-		hw->calib.acam_start_offset = 10000;
-     	for(i=0;i<4;i++)
-     		hw->calib.zero_offset[i] = 50000;
 	}
-//	exit(-1);
+	int temp;
+	ds18x_read_temp(dev, &temp);
-	/* Initialize the clock system - AD9516 PLL */	
+	dbg("Device temperature: %d\n", temp);
-	oc_spi_init(dev);
+  /* Configure default states of the SPI GPIO pins */
-	if(ad9516_init(dev) < 0)
+  sgpio_set_dir(dev, SGPIO_TRIG_SEL, 1);
-		return -1;
+  sgpio_set_pin(dev, SGPIO_TRIG_SEL, 1);
-	/* Configure default states of the SPI GPIO pins */
+  for(i=1;i<=4;i++)
-	sgpio_set_dir(dev, SGPIO_LED_TERM, 1);
+    {
-	sgpio_set_pin(dev, SGPIO_LED_TERM, 0);
+      sgpio_set_pin(dev, SGPIO_OUTPUT_EN(i), 0);
-	sgpio_set_dir(dev, SGPIO_TRIG_SEL, 1);
+      sgpio_set_dir(dev, SGPIO_OUTPUT_EN(i), 1);
-	sgpio_set_pin(dev, SGPIO_TRIG_SEL, 1);
+    }
-	sgpio_set_dir(dev, SGPIO_DRV_OEN, 1);
-	sgpio_set_pin(dev, SGPIO_DRV_OEN, 1);
-	sgpio_set_dir(dev, SGPIO_TERM_EN, 1);
-	sgpio_set_pin(dev, SGPIO_TERM_EN, 0);
-	/* Reset the FD core once we have proper reference/TDC clocks */
+  sgpio_set_dir(dev, SGPIO_TERM_EN, 1);
-	fd_writel( 0xdeadbeef, FD_REG_RSTR);
+  sgpio_set_pin(dev, SGPIO_TERM_EN, 0);
+  /* Reset the FD core once we have proper reference/TDC clocks */
+  fd_do_reset(dev, FD_RESET_CORE);
+  while(! (fd_readl(FD_REG_GCR) & FD_GCR_DDR_LOCKED))
+    udelay(1);
+  fd_do_reset(dev, FD_RESET_CORE);
+  /* Disable the delay generator core, so we can access the ACAM from the host, both for
+     initialization and calibration */
+  fd_writel( FD_GCR_BYPASS, FD_REG_GCR);
+  acam_test_bus(dev);
-	/* Disable the delay generator core, so we can access the ACAM from the host, both for 
-	   initialization and calibration */
-	fd_writel( FD_GCR_BYPASS, FD_REG_GCR);
 	/* Calibrate the output delay lines */
-//	calibrate_outputs(dev);
+  calibrate_outputs(dev);
-	/* Switch to the R-MODE (more precise) */
+  /* Switch to the R-MODE (more precise) */
-	acam_configure(dev, ACAM_RMODE);
+  acam_configure(dev, ACAM_RMODE);
-	/* Switch the ACAM to be driven by the delay core instead of the host */
+  /* Switch the ACAM to be driven by the delay core instead of the host */
-	fd_writel( 0, FD_REG_GCR);
+  fd_writel( 0, FD_REG_GCR);
-	/* Clear and disable the timestamp readout buffer */
+  /* Clear and disable the timestamp readout buffer */
-	fd_writel( FD_TSBCR_PURGE | FD_TSBCR_RST_SEQ, FD_REG_TSBCR);
+  fd_writel( FD_TSBCR_PURGE | FD_TSBCR_RST_SEQ, FD_REG_TSBCR);
-	/* Program the ACAM-specific timestamper registers using pre-defined calibration values:
-	   - bin -> internal timebase scalefactor (ADSFR),
-	   - Start offset (must be consistent with the value written to the ACAM reg 4)
-	   - timestamp merging control register (ATMCR) */
-	fd_writel( hw->calib.adsfr_val, FD_REG_ADSFR);
-	fd_writel( 3 * hw->calib.acam_start_offset, FD_REG_ASOR);
-	fd_writel( hw->calib.atmcr_val, FD_REG_ATMCR);
-	/* Synchronize the internal time base - for the time being to itself (i.e. ensure that
-	   all the time counters inside the FD Core are in sync */
-	fd_writel(FD_GCR_CSYNC_INT, FD_REG_GCR);
-	/* Enable input */	
-	udelay(1);
-	fd_writel(FD_GCR_INPUT_EN, FD_REG_GCR);
-	/* Enable output driver */
+  /* Program the ACAM-specific timestamper registers using pre-defined calibration values:
-	sgpio_set_pin(dev, SGPIO_DRV_OEN, 1);
+     - bin -> internal timebase scalefactor (ADSFR),
+     - Start offset (must be consistent with the value written to the ACAM reg 4)
+     - timestamp merging control register (ATMCR) */
+  fd_writel( hw->calib.adsfr_val, FD_REG_ADSFR);
+  fd_writel( 3 * hw->calib.acam_start_offset, FD_REG_ASOR);
+  fd_writel( hw->calib.atmcr_val, FD_REG_ATMCR);
-	dbg("FD initialized\n");
+  t_zero.utc = 0;
-	return 0;
+  t_zero.coarse = 0;
+  fdelay_set_time(dev, t_zero);
+  /* Enable input */
+  udelay(1);
+  fd_writel(FD_GCR_INPUT_EN, FD_REG_GCR);
+  /* Enable output driver */
+  //	sgpio_set_pin(dev, SGPIO_DRV_OEN, 1);
+  dbg("FD initialized\n");
+  return 0;
 }
 /* Configures the trigger input. Enable enables the input, termination selects the impedance
   of the trigger input (0 == 2kohm, 1 = 50 ohm) */
 int fdelay_configure_trigger(fdelay_device_t *dev, int enable, int termination)
 {
+	fd_decl_private(dev)
 	if(termination)
 	{
 		dbg("%s: 50-ohm terminated mode\n", __FUNCTION__);
-		  sgpio_set_pin(dev,SGPIO_LED_TERM,1);
 		  sgpio_set_pin(dev,SGPIO_TERM_EN,1);
 	} else {
 			dbg("%s: high impedance mode\n", __FUNCTION__);
-		  sgpio_set_pin(dev,SGPIO_LED_TERM,0);
 		  sgpio_set_pin(dev,SGPIO_TERM_EN,0);
 	};
 	if(enable)
 	{
-//		fd_writel(FD_GCR_BYPASS, FD_REG_GCR);
-//		dbg("EF %d\n", fd_readl(FD_REG_TDCSR) & FD_TDCSR_EMPTY ?  1: 0);
-//		fd_writel(0, FD_REG_GCR);
 		fd_writel(fd_readl(FD_REG_GCR) | FD_GCR_INPUT_EN, FD_REG_GCR);
-	} else	
+	} else
 		fd_writel(fd_readl(FD_REG_GCR) & (~FD_GCR_INPUT_EN) , FD_REG_GCR);
 	return 0;
 }
 /* Converts a positive time interval expressed in picoseconds to the timestamp format used in the Fine Delay core */
-/*fdelay_time_t fdelay_from_picos(const uint64_t ps)
-{
-	fdelay_time_t t;
-	int64_t rescaled;
-	rescaled = (int64_t) ((long double) ps * (long double)4096 / (long double)8000);
-	t.frac = rescaled % 4096;
-	rescaled -= t.frac;
-	rescaled /= 4096;
-	t.coarse = rescaled % 125000000ULL;
-	rescaled -= t.coarse;
-	rescaled /= 125000000ULL;
-	t.utc = rescaled;
-	dbg("fdelay_from_picos: %d:%d:%d\n", t.utc, t.coarse, t.frac);
-	return t;
-}*/
 fdelay_time_t fdelay_from_picos(const uint64_t ps)
 {
 	fdelay_time_t t;
 	uint64_t tmp = ps;
-//	int64_t rescaled;
+	t.frac = (tmp % 8000ULL) * (uint64_t)(1<<FDELAY_FRAC_BITS) / 8000ULL;
-//	rescaled = (int64_t) ((long double) ps * (long double)4096 / (long double)8000);
-	t.frac = (tmp % 8000ULL) * 4096ULL / 8000ULL;
 	tmp -= (tmp % 8000ULL);
 	tmp /= 8000ULL;
 	t.coarse = tmp % 125000000ULL;
 	tmp -= (tmp % 125000000ULL);
 	tmp /= 125000000ULL;
 	t.utc = tmp;
-	dbg("fdelay_from_picos: %d:%d:%d\n", t.utc, t.coarse, t.frac);
 	return t;
 }
@@ -740,50 +876,59 @@ static fdelay_time_t ts_sub(fdelay_time_t a, fdelay_time_t b)
 /* Converts a Fine Delay time stamp to plain picoseconds */
 int64_t fdelay_to_picos(const fdelay_time_t t)
 {
-	int64_t tp = (((int64_t)t.frac * 8000LL) >> 12) + ((int64_t) t.coarse * 8000LL) + ((int64_t)t.utc * 1000000000000LL);
+	int64_t tp = (((int64_t)t.frac * 8000LL) >> FDELAY_FRAC_BITS) + ((int64_t) t.coarse * 8000LL) + ((int64_t)t.utc * 1000000000000LL);
 	return tp;
 }
 static int poll_rbuf(fdelay_device_t *dev)
 {
+ 	fd_decl_private(dev)
 	if((fd_readl(FD_REG_TSBCR) & FD_TSBCR_EMPTY) == 0)
 		return 1;
 	return 0;
 }
+/* TODO: chan_mask */
 int fdelay_configure_readout(fdelay_device_t *dev, int enable)
 {
+	fd_decl_private(dev)
 	if(enable)
 	{
 		fd_writel( FD_TSBCR_PURGE | FD_TSBCR_RST_SEQ, FD_REG_TSBCR);
-		fd_writel( FD_TSBCR_ENABLE, FD_REG_TSBCR);
+		fd_writel( FD_TSBCR_CHAN_MASK_W(1) | FD_TSBCR_ENABLE, FD_REG_TSBCR);
    } else
 		fd_writel( FD_TSBCR_PURGE | FD_TSBCR_RST_SEQ, FD_REG_TSBCR);
 	return 0;
 }
-/* Reads up to (how_many) timestamps from the FD ring buffer and stores them in (timestamps). 
+/* Reads up to (how_many) timestamps from the FD ring buffer and stores them in (timestamps).
   Returns the number of read timestamps. */
 int fdelay_read(fdelay_device_t *dev, fdelay_time_t *timestamps, int how_many)
 {
 	fd_decl_private(dev)
 	int n_read = 0;
+//	dbg("tsbcr %x\n", fd_readl(FD_REG_TSBCR));
 	while(poll_rbuf(dev))
 	{
 		fdelay_time_t ts;
 		uint32_t seq_frac;
 		if(!how_many) break;
-		ts.utc = fd_readl(FD_REG_TSBR_U);
+		ts.utc = ((int64_t) (fd_readl(FD_REG_TSBR_SECH) & 0xff) << 32) | fd_readl(FD_REG_TSBR_SECL);
-		ts.coarse = fd_readl(FD_REG_TSBR_C) & 0xfffffff;
+		ts.coarse = fd_readl(FD_REG_TSBR_CYCLES) & 0xfffffff;
+//		dbg("Coarse %d\n", ts.coarse);
 		seq_frac =  fd_readl(FD_REG_TSBR_FID);
-		ts.frac = seq_frac & 0xfff;
+		ts.frac = FD_TSBR_FID_FINE_R(seq_frac);
-		ts.seq_id = seq_frac >> 16;
+		ts.seq_id = FD_TSBR_FID_SEQID_R(seq_frac);
+		ts.channel = FD_TSBR_FID_CHANNEL_R(seq_frac);
 		*timestamps++ = ts_sub(ts, fdelay_from_picos(hw->calib.tdc_zero_offset));
 		how_many--;
 		n_read++;
 	}
@@ -794,37 +939,80 @@ int fdelay_read(fdelay_device_t *dev, fdelay_time_t *timestamps, int how_many)
 /* Configures the output channel (channel) to produce pulses delayed from the trigger by (delay_ps).
   The output pulse width is proviced in (width_ps) parameter. */
-int fdelay_configure_output(fdelay_device_t *dev, int channel, int enable, int64_t delay_ps, int64_t width_ps)
+int fdelay_configure_output(fdelay_device_t *dev, int channel, int enable, int64_t delay_ps, int64_t width_ps, int64_t delta_ps, int rep_count)
 {
 	fd_decl_private(dev)
 	uint32_t base = (channel-1) * 0x20;
 	uint32_t dcr;
- 	fdelay_time_t start, end;
+ 	fdelay_time_t start, end, delta;
 	if(channel < 1 || channel > 4)
 		return -1;
 	delay_ps -= hw->calib.zero_offset[channel-1];
 	start = fdelay_from_picos(delay_ps);
 	end = fdelay_from_picos(delay_ps + width_ps);
+    delta = fdelay_from_picos(delta_ps);
- 	fd_writel(hw->frr[channel-1], base + FD_REG_FRR1);
+// 	printf("Start: %lld: %d:%d\n", start.utc, start.coarse, start.frac);
- 	fd_writel(start.utc, base + FD_REG_U_START1);
- 	fd_writel(start.coarse, base + FD_REG_C_START1);
- 	fd_writel(start.frac, base + FD_REG_F_START1);
+ 	chan_writel(hw->frr_cur[channel-1],  FD_REG_FRR);
- 	fd_writel(end.utc, base + FD_REG_U_END1);
+ 	chan_writel(start.utc >> 32, FD_REG_U_STARTH);
- 	fd_writel(end.coarse, base + FD_REG_C_END1);
+ 	chan_writel(start.utc & 0xffffffff, FD_REG_U_STARTL);
- 	fd_writel(end.frac, base + FD_REG_F_END1);
+ 	chan_writel(start.coarse, FD_REG_C_START);
+ 	chan_writel(start.frac, FD_REG_F_START);
- 	dcr = (enable ? FD_DCR1_ENABLE : 0)
+ 	chan_writel(end.utc >> 32,  FD_REG_U_ENDH);
- 		| FD_DCR1_POL
+ 	chan_writel(end.utc & 0xffffffff,  FD_REG_U_ENDL);
- 		| FD_DCR1_UPDATE;
+ 	chan_writel(end.coarse, FD_REG_C_END);
+ 	chan_writel(end.frac, FD_REG_F_END);
- 	fd_writel(dcr, base + FD_REG_DCR1);
+ 	chan_writel(delta.utc & 0xf,  FD_REG_U_DELTA);
+ 	chan_writel(delta.coarse, FD_REG_C_DELTA);
+ 	chan_writel(delta.frac, FD_REG_F_DELTA);
+// 	chan_writel(0, FD_REG_RCR);
+ 	chan_writel(FD_RCR_REP_CNT_W(rep_count) | (rep_count < 0 ? FD_RCR_CONT : 0), FD_REG_RCR);
+        dcr = 0;
+    /* For narrowly spaced pulses, we don't have enough time to reload the tap number into the corresponding
+        SY89295 - therefore, the width/spacing resolution is limited to 4 ns. */
+    if((delta_ps - width_ps) < 200000 || (width_ps < 200000))
+        dcr = FD_DCR_NO_FINE;
+ 	chan_writel(dcr | FD_DCR_UPDATE, FD_REG_DCR);
+ 	chan_writel(dcr | FD_DCR_ENABLE, FD_REG_DCR);
+ 	sgpio_set_pin(dev, SGPIO_OUTPUT_EN(channel), enable ? 1 : 0);
 	return 0;
 }
+/* Todo: write get_time() */
+int fdelay_set_time(fdelay_device_t *dev, const fdelay_time_t t)
+{
+	fd_decl_private(dev)
+	uint32_t tcr;
+	uint32_t gcr;
+	fd_writel(0, FD_REG_GCR);
+    fd_writel(t.utc >> 32, FD_REG_TM_SECH);
+    fd_writel(t.utc & 0xffffffff, FD_REG_TM_SECL);
+    fd_writel(t.coarse, FD_REG_TM_CYCLES);
+    tcr = fd_readl(FD_REG_TCR);
+    fd_writel(tcr | FD_TCR_SET_TIME, FD_REG_TCR);
+    return 0;
+}
+#if 0
+/* To be rewritten to use interrupts and new WR FSM (see TCR register description).
+   Use the API provided in fdelay_lib.h */
 int fdelay_configure_sync(fdelay_device_t *dev, int mode)
 {
 	fd_decl_private(dev)
@@ -832,7 +1020,7 @@ int fdelay_configure_sync(fdelay_device_t *dev, int mode)
 	if(mode == FDELAY_SYNC_LOCAL)
 	{
 	 	fd_writel(0, FD_REG_GCR);
-	 	fd_writel(FD_GCR_CSYNC_INT, FD_REG_GCR);
+//	 	fd_writel(FD_GCR_CSYNC_INT, FD_REG_GCR);
 	 	hw->wr_enabled = 0;
 	} else {
 	 	fd_writel(0, FD_REG_GCR);
@@ -846,17 +1034,17 @@ int fdelay_get_sync_status(fdelay_device_t *dev)
 	fd_decl_private(dev)
 	if(!hw->wr_enabled) return FDELAY_FREE_RUNNING;
 	switch(hw->wr_state)
 	{
-	 	case FDELAY_WR_OFFLINE: 
+	 	case FDELAY_WR_OFFLINE:
 	 		if(fd_readl(FD_REG_GCR) & FD_GCR_WR_READY)
 	 			{
 	 			 	dbg("-> WR Core synced\n");
 	 			 	hw->wr_state = FDELAY_WR_READY;
 	 			}
 			break;
 		case FDELAY_WR_READY:
 		 	fd_writel(FD_GCR_WR_LOCK_EN, FD_REG_GCR);
 		 	hw->wr_state = FDELAY_WR_SYNCING;
@@ -868,11 +1056,11 @@ int fdelay_get_sync_status(fdelay_device_t *dev)
 	 			fd_writel(FD_GCR_WR_LOCK_EN | FD_GCR_CSYNC_WR, FD_REG_GCR);
 	 			fd_writel(FD_GCR_WR_LOCK_EN , FD_REG_GCR);
 	 			fd_writel(FD_GCR_WR_LOCK_EN | FD_GCR_INPUT_EN, FD_REG_GCR);
 				hw->wr_state = FDELAY_WR_SYNCED;
 			}
 			break;
 		case FDELAY_WR_SYNCED:
 			if((fd_readl(FD_REG_GCR) & FD_GCR_WR_LOCKED) == 0)
 				hw->wr_state = FDELAY_WR_OFFLINE;
@@ -882,3 +1070,31 @@ int fdelay_get_sync_status(fdelay_device_t *dev)
 	return hw->wr_state;
 }
+#endif
+# if 0
+/* We might implement SPLL-based DMTD calibration, but not now  - don't include in the driver */
+int fd_update_spll(fdelay_device_t *dev)
+{
+	struct spll_helper_state pll;
+	fd_decl_private(dev)
+    int i =0;
+    helper_start(dev, &pll);
+	fd_writel(FD_CALR_CAL_DMTD, FD_REG_CALR);
+	sgpio_set_pin(dev, SGPIO_TRIG_SEL, 0);
+    for(;;)
+    {
+		helper_update(&pll);
+		//if(pll.prelock.ld.locked)
+		//	dbg("LOCK!");
+    }
+}
+#endif
\ No newline at end of file
--- a/software/lib/fdelay_test.c
+++ b/software/lib/fdelay_test.c
@@ -17,21 +17,49 @@ uint32_t my_readl(void *priv, uint32_t addr)
 main()
 {
 	fdelay_device_t dev;
 	rr_init();
 	dev.writel = my_writel;
 	dev.readl = my_readl;
-	dev.base_addr = 0x80400;
+	dev.base_addr = 0x84000;
 	if(fdelay_init(&dev) < 0)
 		return -1;
 	fdelay_configure_trigger(&dev, 1,1);
- 	fdelay_configure_output(&dev,1,1,500000, 200000);
- 	fdelay_configure_output(&dev,2,1,504000, 200000);
- 	fdelay_configure_output(&dev,3,1,500000, 200000);
- 	fdelay_configure_output(&dev,4,1,500000, 200000);
+    fdelay_configure_output(&dev,1,1,500000, 100000, 100000, 0);
+    fdelay_configure_output(&dev,2,1,500000, 100000, 100000, 0);
+    fdelay_configure_output(&dev,3,1,500000, 100000, 100000, 0);
+    fdelay_configure_output(&dev,4,1,500000, 100000, 100000, 0);
+	fdelay_configure_readout(&dev, 1);
+//	fd_update_spll(&dev);
+	int64_t prev = 0, dp, pmin=10000000000LL,pmax=0;
-}
+	#if 0
\ No newline at end of file
+	for(;;)
+	  {
+	    fdelay_time_t ts;
+	    if(fdelay_read(&dev, &ts, 1) == 1)
+	      {
+    		int64_t ts_p = fdelay_to_picos(ts), d;
+    		d=ts_p - prev;
+    		if(prev > 0)
+    		{
+            if(d<pmin) pmin=d;
+            if(d>pmax) pmax=d;
+    		fprintf(stderr,"Got it %lld:%d:%d delta %lld span %lld\n", ts.utc, ts.coarse, ts.frac, d, pmax-pmin);
+    		}
+    		prev = ts_p;
+	      }
+	  }
+	  #endif
+    for(;;)
+    {
+     fdelay_update_calibration(&dev);
+     sleep(1);
+    }
+}
--- a/software/lib/i2c_master.c
+++ b/software/lib/i2c_master.c
@@ -2,7 +2,7 @@
 #include "fdelay_lib.h"
 #include "fdelay_private.h"
-#include "fdelay_regs.h"
+#include "fd_main_regs.h"
 #define M_SDA_OUT(x) {							\
@@ -73,6 +73,7 @@ static void mi2c_stop(fdelay_device_t *dev)
 void mi2c_get_byte(fdelay_device_t *dev, unsigned char *data, int ack)
 {
+	fd_decl_private(dev)
  int i;
  unsigned char indata = 0;
@@ -99,7 +100,7 @@ void mi2c_get_byte(fdelay_device_t *dev, unsigned char *data, int ack)
 void mi2c_init(fdelay_device_t *dev)
 {
 	fd_decl_private(dev);
 	M_SCL_OUT(1);
 	M_SDA_OUT(1);
 }
@@ -113,7 +114,7 @@ void mi2c_scan(fdelay_device_t *dev)
 		if(!mi2c_put_byte(dev,i))
 			printf("Found device at 0x%x\n", i>>1);
 		mi2c_stop(dev);
 	}
 }
@@ -137,7 +138,7 @@ int eeprom_read(fdelay_device_t *dev, uint8_t i2c_addr, uint32_t offset, uint8_t
 	mi2c_start(dev);
 	mi2c_put_byte(dev, (i2c_addr << 1) | 1);
 	mi2c_get_byte(dev, &c, 0);
-	printf("readback: %x\n", c);
+//	printf("readback: %x\n", c);
 	*buf++ = c;
 	mi2c_stop(dev);
 	}
@@ -148,9 +149,9 @@ int eeprom_write(fdelay_device_t *dev, uint8_t i2c_addr, uint32_t offset, uint8_
 {
 	int i, busy;
 	for(i=0;i<size;i++)
-	{	
+	{
 	 	mi2c_start(dev);
 	 	if(mi2c_put_byte(dev, i2c_addr << 1) < 0)
 	 	{
 		 	mi2c_stop(dev);
@@ -163,14 +164,14 @@ int eeprom_write(fdelay_device_t *dev, uint8_t i2c_addr, uint32_t offset, uint8_
 		offset++;
 		mi2c_stop(dev);
 		do /* wait until the chip becomes ready */
 		{
            mi2c_start(dev);
-			busy = mi2c_put_byte(dev, i2c_addr << 1); 
+			busy = mi2c_put_byte(dev, i2c_addr << 1);
 			mi2c_stop(dev);
 		} while(busy);
 	}
 	return size;
 }
\ No newline at end of file
--- a/software/lib/spll/softpll_ng.c
+++ b/software/lib/spll/softpll_ng.c
+#include <stdio.h>
+#include "board.h"
+#include "hw/softpll_regs.h"
+#include "irq.h"
+static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
+/* The includes below contain code (not only declarations) to enable the compiler
+   to inline functions where necessary and save some CPU cycles */
+#include "spll_defs.h"
+#include "spll_common.h"
+#include "spll_helper.h"
+volatile int irq_count = 0,eee,yyy;
+struct spll_helper_state helper;
+void _irq_entry()
+{
+	volatile uint32_t trr;
+	int src = -1, tag;
+	if(! (SPLL->CSR & SPLL_TRR_CSR_EMPTY))
+	{
+		trr = SPLL->TRR_R0;
+		src = SPLL_TRR_R0_CHAN_ID_R(trr);
+		tag = SPLL_TRR_R0_VALUE_R(trr);
+		eee = tag;
+	}
+		helper_update(&helper, tag, src);
+		yyy=helper.phase.pi.y;
+		irq_count++;
+		clear_irq();
+}
+void spll_init()
+{
+	volatile int dummy;
+	disable_irq();
+	SPLL->CSR= 0 ;
+	SPLL->OCER = 0;
+	SPLL->RCER = 0;
+	SPLL->DEGLITCH_THR = 2000;
+	while(! (SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY)) dummy = SPLL->TRR_R0;
+	dummy = SPLL->PER_HPLL;
+	SPLL->EIC_IER = 1;
+}
+void spll_test()
+{
+	int i = 0;
+	volatile	int dummy;
+	spll_init();
+	helper_start(&helper, 6);
+	enable_irq();
+	for(;;)
+	{
+		mprintf("cnt %d serr %d src %d y %d d %d\n", irq_count, eee, serr, yyy, delta);
+	}
+}
\ No newline at end of file
--- a/software/lib/spll/spll_common.h
+++ b/software/lib/spll/spll_common.h
+/*
+White Rabbit Softcore PLL (SoftPLL) - common definitions
+*/
+/* PI regulator state */
+typedef struct {
+  	int ki, kp; 		/* integral and proportional gains (1<<PI_FRACBITS == 1.0f) */
+  	int integrator;		/* current integrator value */
+  	int bias;			/* DC offset always added to the output */
+		int anti_windup;	/* when non-zero, anti-windup is enabled */
+		int y_min;			/* min/max output range, used by claming and antiwindup algorithms */
+		int y_max;			
+		int x,y;            /* Current input and output value */
+} spll_pi_t;
+/* Processes a single sample (x) using PI controller (pi). Returns the value (y) which should
+   be used to drive the actuator. */
+static inline int pi_update(spll_pi_t *pi, int x)
+{
+	int i_new, y;
+	pi->x = x;
+	i_new = pi->integrator + x;
+	y = ((i_new * pi->ki + x * pi->kp) >> PI_FRACBITS) + pi->bias;
+	/* clamping (output has to be in <y_min, y_max>) and anti-windup:
+	   stop the integretor if the output is already out of range and the output
+	   is going further away from y_min/y_max. */
+    if(y < pi->y_min)
+    {
+       	y = pi->y_min;
+       	if((pi->anti_windup && (i_new > pi->integrator)) || !pi->anti_windup)
+       		pi->integrator = i_new;
+	} else if (y > pi->y_max) {
+       	y = pi->y_max;
+      	if((pi->anti_windup && (i_new < pi->integrator)) || !pi->anti_windup)
+       		pi->integrator = i_new;
+	} else
+		pi->integrator = i_new;
+    pi->y = y;
+    return y;
+}
+/* initializes the PI controller state. Currently almost a stub. */
+static inline void pi_init(spll_pi_t *pi)
+{
+ 	pi->integrator = 0;
+}
+/* lock detector state */
+typedef struct {
+  	int lock_cnt;
+  	int lock_samples;
+  	int delock_samples;
+  	int threshold;
+  	int locked;
+} spll_lock_det_t;
+/* Lock detector state machine. Takes an error sample (y) and checks if it's withing an acceptable range
+   (i.e. <-ld.threshold, ld.threshold>. If it has been inside the range for (ld.lock_samples) cyckes, the 
+   FSM assumes the PLL is locked. */
+static inline int ld_update(spll_lock_det_t *ld, int y)
+{
+	if (abs(y) <= ld->threshold)
+	{
+		if(ld->lock_cnt < ld->lock_samples)
+			ld->lock_cnt++;
+		if(ld->lock_cnt == ld->lock_samples)
+			ld->locked = 1;
+	} else {
+	 	if(ld->lock_cnt > ld->delock_samples)
+	 		ld->lock_cnt--;
+		if(ld->lock_cnt == ld->delock_samples)
+		{
+		 	ld->lock_cnt=  0;
+		 	ld->locked = 0;
+		}
+	}
+	return ld->locked;
+}
+static void ld_init(spll_lock_det_t *ld)
+{
+ 	ld->locked = 0;
+ 	ld->lock_cnt = 0;
+}
--- a/software/lib/spll/spll_defs.h
+++ b/software/lib/spll/spll_defs.h
+/*
+White Rabbit Softcore PLL (SoftPLL) - common definitions
+*/
+#include <stdio.h>
+/* Reference clock frequency */
+#define CLOCK_FREQ 125000000
+/* Bit size of phase tags generated by the DMTDs. Used to sign-extend the tags. */
+#define TAG_BITS 20
+/* Helper PLL N divider (1/2**N is the frequency offset) */
+#define HPLL_N 14
+/* Fractional bits in PI controller coefficients */
+#define PI_FRACBITS 12
--- a/software/lib/spll/spll_helper.h
+++ b/software/lib/spll/spll_helper.h
+/* State of the Helper PLL producing a clock (clk_dmtd_i) which is
+   slightly offset in frequency from the recovered/reference clock (clk_rx_i or clk_ref_i), so the
+   Main PLL can use it to perform linear phase measurements. This structure keeps the state of the pre-locking
+   stage */
+struct spll_helper_prelock_state {
+ 	spll_pi_t pi;
+ 	spll_lock_det_t ld;
+ 	int f_setpoint;
+	int ref_select;
+	fdelay_device_t *dev;
+};
+volatile int serr;
+void helper_prelock_init(struct spll_helper_prelock_state *s)
+{
+	/* Frequency branch PI controller */
+	s->pi.y_min = 5;
+	s->pi.y_max = 65530;
+	s->pi.anti_windup = 0;
+	s->pi.kp = 28*32*16;
+	s->pi.ki = 50*32*16;
+	s->pi.bias = 32000;
+	/* Freqency branch lock detection */
+	s->ld.threshold = 2;
+	s->ld.lock_samples = 1000;
+	s->ld.delock_samples = 990;
+	s->f_setpoint = 131072 / (1<<HPLL_N);
+	pi_init(&s->pi);
+	ld_init(&s->ld);
+}
+void helper_prelock_enable(struct spll_helper_prelock_state *state, int ref_channel, int enable)
+{
+	fdelay_device_t *dev = state->dev;
+	fd_decl_private(dev);
+	fd_writel(0, FD_REG_SPLLR);
+}
+#define SPLL_LOCKED 1
+#define SPLL_LOCKING 0
+int helper_prelock_update(struct spll_helper_prelock_state *s, int tag)
+{
+	fdelay_device_t *dev = s->dev;
+	fd_decl_private(dev);
+	int y;
+	volatile uint32_t per = fd_readl(FD_REG_SPLLR);
+	short err = (short) (tag & 0xffff);
+	serr = (int)err;
+	err -= s->f_setpoint;
+	y = pi_update(&s->pi, err);
+	fd_writel(y, FD_REG_SDACR);
+	if(ld_update(&s->ld, err))
+		return SPLL_LOCKED;
+	return SPLL_LOCKING;
+}
+struct spll_helper_phase_state {
+ 	spll_pi_t pi;
+ 	spll_lock_det_t ld;
+ 	int p_setpoint, tag_d0;
+ 	int ref_src;
+ 	fdelay_device_t *dev;
+};
+void helper_phase_init(struct spll_helper_phase_state *s)
+{
+	/* Phase branch PI controller */
+	s->pi.y_min = 5;
+	s->pi.y_max = 65530;
+ 	s->pi.kp = (int)(2.0 * 32.0 * 16.0);
+	s->pi.ki = (int)(0.05 * 32.0 * 3.0);
+	s->pi.anti_windup = 0;
+	s->pi.bias = 32000;
+	/* Phase branch lock detection */
+	s->ld.threshold = 500;
+	s->ld.lock_samples = 10000;
+	s->ld.delock_samples = 9900;
+	s->ref_src = 6;
+	s->p_setpoint = -1;
+	pi_init(&s->pi);
+	ld_init(&s->ld);
+}
+void helper_phase_enable(struct spll_helper_phase_state *state, int ref_channel, int enable)
+{
+	fdelay_device_t *dev = state->dev;
+	fd_decl_private(dev);
+	fd_writel(FD_SPLLR_MODE, FD_REG_SPLLR);
+}
+volatile int delta;
+int helper_phase_update(struct spll_helper_phase_state *s, int tag, int source)
+{
+	fdelay_device_t *dev = s->dev;
+	fd_decl_private(dev);
+	int err, y;
+	serr = source;
+//	if(source == s->ref_src)
+	{
+		if(s->p_setpoint < 0)
+		{
+		 	s->p_setpoint = tag;
+		 	return;
+		}
+		err = tag - s->p_setpoint;
+		delta = tag - s->tag_d0;
+		s->tag_d0 = tag;
+		s->p_setpoint += (1<<HPLL_N);
+		if(s->p_setpoint > (1<<TAG_BITS))
+			s->p_setpoint -= (1<<TAG_BITS);
+		y = pi_update(&s->pi, err);
+		//printf("t %d sp %d\n", tag, s->p_setpoint);
+		fd_writel(y, FD_REG_SDACR);
+		if(ld_update(&s->ld, err))
+		{
+			return SPLL_LOCKED;
+		};
+	}
+	return SPLL_LOCKING;
+}
+#define HELPER_PRELOCKING 1
+#define HELPER_PHASE 2
+#define HELPER_LOCKED 3
+struct spll_helper_state {
+	struct spll_helper_prelock_state prelock;
+	struct 	spll_helper_phase_state phase;
+	int state;
+	int ref_channel;
+};
+void helper_start(fdelay_device_t *dev, struct spll_helper_state *s)
+{
+	s->state = HELPER_PRELOCKING;
+	s->ref_channel = 0;
+	s->prelock.dev = dev;
+	s->phase.dev = dev;
+	helper_prelock_init(&s->prelock);
+	helper_phase_init(&s->phase);
+	helper_prelock_enable(&s->prelock, 0, 1);
+}
+void helper_update(struct spll_helper_state *s)
+{
+	fdelay_device_t *dev = s->prelock.dev;
+	fd_decl_private(dev);
+	uint32_t spllr = fd_readl(FD_REG_SPLLR);
+	if(! (spllr & FD_SPLLR_TAG_RDY))
+		return;
+	int tag = FD_SPLLR_TAG_R(spllr);
+	switch(s->state)
+	{
+		case HELPER_PRELOCKING:
+			if(helper_prelock_update(&s->prelock, tag) == SPLL_LOCKED)
+			{
+				s->state = HELPER_PHASE;
+				helper_prelock_enable(&s->prelock, 0, 0);
+				s->phase.pi.bias = s->prelock.pi.y;
+				helper_phase_enable(&s->phase, 0, 1);
+			}
+		break;
+		case HELPER_PHASE:
+			helper_phase_update(&s->phase, tag, 0);
+		break;
+	}
+}