#include <stddef.h>
#include <assert.h>
#include <stdio.h>
#include <string.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include "libwr2rf/board.h"
#include "libwr2rf/api.h"
#include "regs.h"
#include "wr2rf_rfnco_regs.h"
#include "ipInfo.h"
#include "oc_spi16.h"

uint32_t
libwr2rf_16x32_read32(struct libwr2rf_dev *dev, unsigned off)
{
  uint32_t l, h;

  /* Note: the order is important.  */
  l = libwr2rf_read16 (dev, off);
  h = libwr2rf_read16 (dev, off + 2);
  return (h << 16) | l;
}

void
libwr2rf_16x32_write32(struct libwr2rf_dev *dev, unsigned off, uint32_t val)
{
  /* Note: the order is important.  */
  libwr2rf_write16 (dev, off + 2, val >> 16);
  libwr2rf_write16 (dev, off, val & 0xffff);
}

void
libwr2rf_16x32_write64(struct libwr2rf_dev *dev, unsigned off, uint64_t val)
{
  libwr2rf_16x32_write32(dev, off, val);
  libwr2rf_16x32_write32(dev, off + 4, val >> 32);
}

uint64_t
libwr2rf_be_read64(struct libwr2rf_dev *dev, unsigned off)
{
  uint64_t l, h;

  h = libwr2rf_be_read32 (dev, off);
  l = libwr2rf_be_read32 (dev, off + 4);
  return (h << 32) | l;
}

void
libwr2rf_be_write64(struct libwr2rf_dev *dev, unsigned off, uint64_t val)
{
  libwr2rf_be_write32 (dev, off, val >> 32);
  libwr2rf_be_write32 (dev, off + 4, val & 0xffffffff);
}

uint32_t
libwr2rf_be_read32(struct libwr2rf_dev *dev, unsigned off)
{
  uint32_t l, h;

  h = libwr2rf_read16 (dev, off);
  l = libwr2rf_read16 (dev, off + 2);
  return (h << 16) | l;
}

void
libwr2rf_be_write32(struct libwr2rf_dev *dev, unsigned off, uint32_t val)
{
  libwr2rf_write16 (dev, off, val >> 16);
  libwr2rf_write16 (dev, off + 2, val & 0xffff);
}

uint32_t
libwr2rf_wrc_read32(struct libwr2rf_dev *dev, unsigned off)
{
        return libwr2rf_16x32_read32(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRPC + off);
}

void
libwr2rf_wrc_write32(struct libwr2rf_dev *dev, unsigned off, uint32_t val)
{
        libwr2rf_16x32_write32(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRPC + off, val);
}


void libwr2rf_pll_spi_init(struct libwr2rf_dev *dev)
{
  /* Min period is 25+25ns, so max freq is 20Mhz.  */
  libwr2rf_oc_spi_init(dev, PLL_BASE, 2);
}

uint8_t libwr2rf_pll_read(struct libwr2rf_dev *dev, unsigned reg)
{
  uint32_t rval;
  libwr2rf_oc_spi_txrx16(dev, PLL_BASE, PLL_CS, 16, (reg << 9) | (1 << 8), &rval);
  return rval & 0xff;
}

void libwr2rf_pll_write(struct libwr2rf_dev *dev, unsigned reg, uint8_t value)
{
  libwr2rf_oc_spi_txrx16(dev, PLL_BASE, PLL_CS, 16, (reg << 9) | (value & 0xff),
                         NULL);
}

void libwr2rf_dds_spi_init(struct libwr2rf_dev *dev)
{
  /* Min period is 25+25ns, so max freq is 20Mhz.  */
  libwr2rf_oc_spi_init(dev, DDS_BASE, 2);
}

void libwr2rf_dds_write(struct libwr2rf_dev *dev, unsigned reg, uint64_t value, unsigned nbits)
{
  uint16_t vals[8];

  assert (reg < 32);

  /* Note: valid nbits are: 64, 32, 16.   */
  assert (nbits == 16 || nbits == 32 || nbits == 64);

  vals[0] = value & 0xffff;
  vals[1] = (value >> 16) & 0xffff;
  vals[2] = (value >> 32) & 0xffff;
  vals[3] = (value >> 48) & 0xffff;
  vals[nbits / 16] = reg;

  libwr2rf_oc_spi_txrx128(dev, DDS_BASE, DDS_CS, nbits + 8, vals);
}

uint64_t libwr2rf_dds_read(struct libwr2rf_dev *dev, unsigned reg, unsigned nbits)
{
  uint16_t vals[8];
  uint64_t res;

  assert (reg < 32);

  /* Note: valid nbits are: 64, 32, 16.   */
  assert (nbits == 16 || nbits == 32 || nbits == 64);

  vals[0] = 0;
  vals[1] = 0;
  vals[2] = 0;
  vals[3] = 0;
  vals[nbits / 16] = 0x80 | reg;

  libwr2rf_oc_spi_txrx128(dev, DDS_BASE, DDS_CS, nbits + 8, vals);

  res = (uint64_t)vals[0];
  if (nbits > 16) {
      res |= ((uint64_t)vals[1]) << 16;
  }
  if (nbits > 32) {
    res |= ((uint64_t)vals[2]) << 32;
    res |= ((uint64_t)vals[3]) << 48;
  }

  return res;
}

void libwr2rf_dds_reset(struct libwr2rf_dev *dev, unsigned reset)
{
  unsigned dds_reset_addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_DDS_CTRL;

  if (reset)
    libwr2rf_write16(dev, dds_reset_addr, WR2RF_INIT_REGS_DDS_CTRL_RESET);
  else
    libwr2rf_write16(dev, dds_reset_addr, 0);
}

void libwr2rf_dds_reset_pulse(struct libwr2rf_dev *dev)
{
  libwr2rf_dds_reset(dev, 1);
  libwr2rf_dds_reset(dev, 0);
}

void libwr2rf_dds_ioupdate(struct libwr2rf_dev *dev)
{
  libwr2rf_write16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_DDS_CTRL,
                   WR2RF_INIT_REGS_DDS_CTRL_IOUPDATE);
}

unsigned libwr2rf_dds_sync_error(struct libwr2rf_dev *dev)
{
  return libwr2rf_read16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_DDS_STATUS)
    & WR2RF_INIT_REGS_DDS_STATUS_SYNC_ERROR;
}

uint8_t libwr2rf_dac_read(struct libwr2rf_dev *dev, unsigned ch, unsigned reg)
{
  uint32_t rval;
  libwr2rf_oc_spi_txrx16(dev, DAC_BASE, ch, 16, 0x8000 | (reg << 8), &rval);
  return rval & 0xff;
}

void libwr2rf_dac_write(struct libwr2rf_dev *dev, unsigned ch, unsigned reg, uint8_t value)
{
  libwr2rf_oc_spi_txrx16(dev, DAC_BASE, ch, 16, (reg << 8) | (value & 0xff),
    NULL);
}

#define DAC_IQGAIN_MAX 0x7fff
#define DAC_IQGAIN_MIN 0x8000

/* Convert to short.
   The dynamic is asymetric.  */
static int dac_convert(float gain)
{
  if (gain >= 0.0)
    return gain * DAC_IQGAIN_MAX;
  else
    return gain * DAC_IQGAIN_MIN;
}

static int dac_check_range(int gain)
{
  if (gain > DAC_IQGAIN_MAX
      || gain < -DAC_IQGAIN_MIN)
    return 0;
  return 1;
}

int libwr2rf_dac_set_phase_amp(struct libwr2rf_dev *dev, unsigned ch,
                               float amplitude, float phase, unsigned verbose)
{
  if (!(amplitude >= 0.0 && amplitude <= 1.0))
    return LIBWR2RF_ERROR_BAD_PARAM;
  if (!(phase >= -360.0 && phase <= 360.0))
    return LIBWR2RF_ERROR_BAD_PARAM;

  float phase_rad = phase * M_PI / 180.0;
  float igainf = amplitude * cosf (phase_rad);
  float qgainf = amplitude * sinf (phase_rad);

  int igain = dac_convert(igainf);
  int qgain = dac_convert(qgainf);

  if (!dac_check_range(igain) || !dac_check_range(qgain))
    return LIBWR2RF_ERROR_BAD_PARAM;

  if (verbose)
    fprintf(stderr,
            "DAC IQ %u: ampl=%3.1f, phase=%3.1f, igain=%04x qgain=%04x\n",
            ch, amplitude, phase,
            (unsigned short)igain, (unsigned short)qgain);

  unsigned igain_addr;
  unsigned qgain_addr;
  unsigned phase_addr;
  unsigned updat_addr;

  switch (ch) {
    case 1:
        igain_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_IQDAC_IGAIN_ARM;
        qgain_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_IQDAC_QGAIN_ARM;
        phase_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_IQDAC_PHASE;
        updat_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_IQDAC_PHASE_UPDATE;
        break;
    case 2:
        igain_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_IQDAC_IGAIN_ARM;
        qgain_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_IQDAC_QGAIN_ARM;
        phase_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_IQDAC_PHASE;
        updat_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_IQDAC_PHASE_UPDATE;
        break;
    default:
      return LIBWR2RF_ERROR_BAD_ID;
    }

  libwr2rf_write16(dev, igain_addr, igain);
  libwr2rf_write16(dev, qgain_addr, qgain);
  libwr2rf_write16(dev, phase_addr, (short)phase);
  libwr2rf_write16(dev, updat_addr,
                   WR2RF_CTRL_REGS_RF1_IQDAC_PHASE_UPDATE_VALID);

  return 0;
}

int libwr2rf_dac_reset_phase_amp(struct libwr2rf_dev *dev, unsigned ch,
                                 float amplitude, float phase)
{
  return libwr2rf_dac_set_phase_amp(dev, ch, amplitude, phase, 0);
}

int libwr2rf_dac_phase_amp_ext(struct libwr2rf_dev *dev, unsigned ch,
                               float amplitude, float phase, unsigned verbose)
{
  const int phase_step = 15;
  int iphase = (int)phase;
  unsigned phase_addr;
  int last_phase;
  int res;

  if (!(phase >= -360.0 && phase <= 360.0))
    return LIBWR2RF_ERROR_BAD_PARAM;

  /* Current phase.  */
  switch (ch) {
    case 1:
        phase_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_IQDAC_PHASE;
        break;
    case 2:
        phase_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_IQDAC_PHASE;
        break;
    default:
      return LIBWR2RF_ERROR_BAD_ID;
    }

  last_phase = (short)libwr2rf_read16(dev, phase_addr);

  while (1) {
    int dph = iphase - last_phase;
    if (dph >= -phase_step && dph <= phase_step) {
      /* Smooth enough, apply directly.  */
      return libwr2rf_dac_set_phase_amp(dev, ch, amplitude, phase, verbose);
    }

    if (dph >= 0)
      last_phase += phase_step;
    else
      last_phase -= phase_step;

    res = libwr2rf_dac_set_phase_amp(dev, ch, amplitude, last_phase, verbose);
    if (res != 0)
      return res;

    /* A delay is implicit through the slow VME access (about 300ns per
       access).  */
  }
}

int libwr2rf_dac_phase_amp(struct libwr2rf_dev *dev, unsigned ch,
                           float amplitude, float phase)
{
  return libwr2rf_dac_phase_amp_ext(dev, ch, amplitude, phase, 0);
}

unsigned libwr2rf_read_fwversion(struct libwr2rf_dev *dev)
{
    unsigned addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_HWINFO
        + HWINFO_FIRMWAREVERSION;

    return libwr2rf_be_read32(dev, addr);
}

int libwr2rf_check_fwversion(struct libwr2rf_dev *dev)
{
  return !(libwr2rf_read_fwversion(dev) == 0x00001600);
}

void libwr2rf_read_nco_ipinfo(struct libwr2rf_dev *dev,
                              struct libwr2rf_nco_ipinfo_t* pinfo)
{
  unsigned addr = WR2RF_VME_REGS_CTRL
    + WR2RF_CTRL_REGS_RF1_RFNCO + RFNCO_IPINFO;

  pinfo->std_ver = libwr2rf_16x32_read32(dev, addr + IPINFO_STDVERSION);
  pinfo->ident = libwr2rf_16x32_read32(dev, addr + IPINFO_IDENT);
  pinfo->fw_ver = libwr2rf_16x32_read32(dev, addr + IPINFO_FIRMWAREVERSION);
  pinfo->mem_map_ver = libwr2rf_16x32_read32(dev, addr + IPINFO_MEMMAPVERSION);
}

int libwr2rf_enable_check_ocxo(struct libwr2rf_dev *dev)
{
  /* OCXO warmup time (in sec).  */
  const unsigned warmup_time = 5 * 60;
  unsigned v;

  /* First, enable the ocxo.
     It's a nop if the ocxo was already enabled.  */
  libwr2rf_write16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_OCXO_CTRL,
                   WR2RF_INIT_REGS_OCXO_CTRL_ENABLE);

  /* Read ocxo uptime.  */
  v = libwr2rf_read16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_OCXO_UPTIME);
  if (v >= warmup_time)
    return 0;
  else
    return warmup_time - v;
}

/* Read ocxo current sensor, return the current in mA. */
int libwr2rf_get_ocxo_current_ma(struct libwr2rf_dev *dev)
{
  unsigned addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_XADC;

  unsigned val = libwr2rf_read16(dev, addr + (0x12 << 1));

  /* See EDA-04387-V2 Sheet 24 (p43) Power for OCXO */
  /* V= 40 * 0.05 Ohm * ocxo_cur */
  double v = (val >> 4) / 4096.0;

  return (v / 2.0) * 1000;
}

int libwr2rf_check_link_time(struct libwr2rf_dev *dev)
{
  unsigned status;

  status = libwr2rf_read16
    (dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRC_STATUS);
  return (status & WR2RF_INIT_REGS_WRC_STATUS_LINKUP)
    && (status & WR2RF_INIT_REGS_WRC_STATUS_TIME_VALID);
}

int libwr2rf_get_mac(struct libwr2rf_dev *dev, unsigned char *mac)
{
  unsigned h, l;
  libwr2rf_be_write32(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRC_PAGE1,
                      0x40000);
  h = libwr2rf_wrc_read32(dev, 0x1000 + 0x100 + 0x24);
  mac[0] = (h >> 8) & 0xff;
  mac[1] = (h >> 0) & 0xff;
  l = libwr2rf_wrc_read32(dev, 0x1000 + 0x100 + 0x28);
  mac[2] = (l >> 24) & 0xff;
  mac[3] = (l >> 16) & 0xff;
  mac[4] = (l >> 8) & 0xff;
  mac[5] = (l >> 0) & 0xff;

  return 0;
}

int libwr2rf_wrc_time_valid(struct libwr2rf_dev *dev)
{
    unsigned status;

    status = libwr2rf_read16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRC_STATUS);
    return !(status & WR2RF_INIT_REGS_WRC_STATUS_TIME_VALID);
}

int libwr2rf_set_rf_out(struct libwr2rf_dev *dev, unsigned ch, unsigned cfg)
{
  unsigned rf;
  unsigned addr;
  unsigned val;

  switch (ch) {
  case 1:
    rf = WR2RF_INIT_RF_REGS_RF1;
    break;
  case 2:
    rf = WR2RF_INIT_RF_REGS_RF2;
    break;
  default:
    return LIBWR2RF_ERROR_BAD_ID;
  }

  switch (cfg) {
  case LIBWR2RF_RF_OUT_GND:
    val = 0x2;
    break;
  case LIBWR2RF_RF_OUT_MIXER:
    val = WR2RF_INIT_RF_CH_REGS_COMMON_MIXER_EN | 0x6;
    break;
  case LIBWR2RF_RF_OUT_DDS:
    val = 0x4;
    break;
  default:
    return LIBWR2RF_ERROR_BAD_PARAM;
  }

  addr = WR2RF_VME_REGS_INIT
    + WR2RF_INIT_REGS_RF + rf + WR2RF_INIT_RF_CH_REGS_COMMON;
  libwr2rf_write16(dev, addr, val);

  return 0;
}

static unsigned diag_id_to_addr(unsigned id)
{
  switch (id) {
  case LIBWR2RF_DIAG_ID_RF_1:
    return WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_VTUS
      + WR2RF_RFTRIGGER_REGS_RF_DIAG;
  case LIBWR2RF_DIAG_ID_RF_2:
    return WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_VTUS
      + WR2RF_RFTRIGGER_REGS_RF_DIAG;
  case LIBWR2RF_DIAG_ID_RF_1_TRIG_1:
    return WR2RF_VME_REGS_CTRL +  WR2RF_CTRL_REGS_RF1_VTUS
      + WR2RF_RFTRIGGER_REGS_T1 + TRIGUNIT_REGS_TRIGDIAG;
  case LIBWR2RF_DIAG_ID_RF_1_TRIG_2:
    return WR2RF_VME_REGS_CTRL +  WR2RF_CTRL_REGS_RF1_VTUS
      + WR2RF_RFTRIGGER_REGS_T2 + TRIGUNIT_REGS_TRIGDIAG;
  case LIBWR2RF_DIAG_ID_RF_2_TRIG_1:
    return WR2RF_VME_REGS_CTRL +  WR2RF_CTRL_REGS_RF2_VTUS
      + WR2RF_RFTRIGGER_REGS_T1 + TRIGUNIT_REGS_TRIGDIAG;
  case LIBWR2RF_DIAG_ID_RF_2_TRIG_2:
    return WR2RF_VME_REGS_CTRL +  WR2RF_CTRL_REGS_RF2_VTUS
      + WR2RF_RFTRIGGER_REGS_T2 + TRIGUNIT_REGS_TRIGDIAG;
  default:
    return 0;
  }
}

int libwr2rf_enable_diag(struct libwr2rf_dev *dev, unsigned id,
                         unsigned window_us, unsigned *generation)
{
  unsigned addr = diag_id_to_addr(id);
  unsigned window;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  switch (window_us) {
  case 5:
    window = 0;
    break;
  case 500:
    window = 2;
    break;
  case 50000: /* 50ms */
    window = 4;
    break;
  case 5000000: /* 5sec */
    window = 5;
    break;
  case 20000000: /* 20sec */
    window = 6;
    break;
  default:
    return LIBWR2RF_ERROR_BAD_PARAM;
  }
  libwr2rf_write16(dev, addr + VTUDIAG_REGS_CONTROL,
                   window | VTUDIAG_REGS_CONTROL_ENABLE);

  *generation = libwr2rf_read16(dev, addr + VTUDIAG_REGS_GENERATION);
  return 0;
}

int libwr2rf_read_diag(struct libwr2rf_dev *dev, unsigned id,
                       unsigned *freq, unsigned *count, unsigned *generation)
{
  unsigned addr = diag_id_to_addr(id);
  unsigned gen, gen1;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  /* Be sure FREQ and GENERATION match.  */
  gen = libwr2rf_read16(dev, addr + VTUDIAG_REGS_GENERATION);
  while (1) {
    *freq = libwr2rf_be_read32(dev, addr + VTUDIAG_REGS_FREQ);
    gen1 = libwr2rf_read16(dev, addr + VTUDIAG_REGS_GENERATION);
    if (gen == gen1)
      break;
    gen = gen1;
  }

  *count = libwr2rf_be_read32(dev, addr + VTUDIAG_REGS_COUNTER);
  *generation = gen;
  return 0;
}

static unsigned libwr2rf_lemo_to_bit(unsigned lemo)
{
  switch (lemo)
    {
    case LIBWR2RF_LEMO_NONE:
      return 0;
    case LIBWR2RF_LEMO_TMGIO1:
      return 1 << 1;
    case LIBWR2RF_LEMO_TMGIO2:
      return 1 << 2;
    case LIBWR2RF_LEMO_TMGIO3:
      return 1 << 3;
    case LIBWR2RF_LEMO_TMGIO4:
      return 1 << 4;
    case LIBWR2RF_LEMO_TMGCLK1:
      return 1 << 5;
    case LIBWR2RF_LEMO_TMGCLK2:
      return 1 << 6;
    default:
      return ~0U;
    }
}

int libwr2rf_vtu_start_lemo(struct libwr2rf_dev *dev, unsigned id,
                            unsigned lemo)
{
  unsigned addr = WR2RF_VME_REGS_INIT;
  unsigned val;

  switch (id)
    {
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_1:
      addr += WR2RF_INIT_REGS_LEMO_RF1_T1_START;
      break;
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_2:
      addr += WR2RF_INIT_REGS_LEMO_RF1_T2_START;
      break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_1:
      addr += WR2RF_INIT_REGS_LEMO_RF2_T1_START;
      break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_2:
      addr += WR2RF_INIT_REGS_LEMO_RF2_T2_START;
      break;
    default:
      return LIBWR2RF_ERROR_BAD_ID;
    }

  val = libwr2rf_lemo_to_bit(lemo);
  if (val == ~0U)
    return LIBWR2RF_ERROR_BAD_PARAM;

  libwr2rf_write16(dev, addr, val);

  return 0;
}

int libwr2rf_vtu_stop_lemo(struct libwr2rf_dev *dev, unsigned id,
                            unsigned lemo)
{
  unsigned addr = WR2RF_VME_REGS_INIT;
  unsigned val;

  switch (id)
    {
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_1:
      addr += WR2RF_INIT_REGS_LEMO_RF1_T1_STOP;
      break;
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_2:
      addr += WR2RF_INIT_REGS_LEMO_RF1_T2_STOP;
      break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_1:
      addr += WR2RF_INIT_REGS_LEMO_RF2_T1_STOP;
      break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_2:
      addr += WR2RF_INIT_REGS_LEMO_RF2_T2_STOP;
      break;
    default:
      return LIBWR2RF_ERROR_BAD_ID;
    }

  val = libwr2rf_lemo_to_bit(lemo);
  if (val == ~0U)
    return LIBWR2RF_ERROR_BAD_PARAM;

  libwr2rf_write16(dev, addr, val);

  return 0;
}

static unsigned libwr2rf_vtu_csr(unsigned id)
{
  unsigned csr_addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_RF;

  switch (id) {
  case LIBWR2RF_TRIG_ID_RF_1_TRIG_1:
    return csr_addr + WR2RF_INIT_RF_REGS_RF1 + WR2RF_INIT_RF_CH_REGS_CH;
  case LIBWR2RF_TRIG_ID_RF_1_TRIG_2:
    return csr_addr + WR2RF_INIT_RF_REGS_RF1 + WR2RF_INIT_RF_CH_REGS_CH
      + WR2RF_INIT_RF_CH_REGS_CH_SIZE;
    break;
  case LIBWR2RF_TRIG_ID_RF_2_TRIG_1:
    return csr_addr + WR2RF_INIT_RF_REGS_RF2 + WR2RF_INIT_RF_CH_REGS_CH;
    break;
  case LIBWR2RF_TRIG_ID_RF_2_TRIG_2:
    return csr_addr + WR2RF_INIT_RF_REGS_RF2 + WR2RF_INIT_RF_CH_REGS_CH
      + WR2RF_INIT_RF_CH_REGS_CH_SIZE;
    break;
  default:
    return 0;
  }
}

int libwr2rf_vtu_output_ff_delay(struct libwr2rf_dev *dev, unsigned id,
                                 unsigned val)
{
  unsigned csr_addr;
  unsigned del_addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_RF +
    WR2RF_INIT_RF_REGS_RF_TRIGGER_FF_DELAY;
  unsigned v;

  csr_addr = libwr2rf_vtu_csr(id);
  if (csr_addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, csr_addr);
  libwr2rf_write16(dev, csr_addr,
                   v | WR2RF_INIT_RF_CH_REGS_CH_CSR_DELAY_LATCH);
  /* pulse.  */
  libwr2rf_write16(dev, del_addr, val);
  libwr2rf_write16(dev, csr_addr,
                   v & ~WR2RF_INIT_RF_CH_REGS_CH_CSR_DELAY_LATCH);
  libwr2rf_write16(dev, csr_addr,
                   v | WR2RF_INIT_RF_CH_REGS_CH_CSR_DELAY_LATCH);

  return 0;
}

int libwr2rf_vtu_output_pad_delay(struct libwr2rf_dev *dev, unsigned id,
                                  unsigned hc, unsigned odelay)
{
    unsigned baddr;

    switch (id) {
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_1:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_VTUS + WR2RF_RFTRIGGER_REGS_T1;
        break;
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_2:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_VTUS + WR2RF_RFTRIGGER_REGS_T2;
        break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_1:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_VTUS + WR2RF_RFTRIGGER_REGS_T1;
        break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_2:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_VTUS + WR2RF_RFTRIGGER_REGS_T2;
        break;
    default:
        return LIBWR2RF_ERROR_BAD_ID;
    }

    libwr2rf_write16(dev, baddr + TRIGUNIT_REGS_TRIGOHCDELAY, hc);
    libwr2rf_write16(dev, baddr + TRIGUNIT_REGS_TRIGODELAY, odelay);

    return 0;
}

int libwr2rf_vtu_output_enable(struct libwr2rf_dev *dev, unsigned id,
                               unsigned en)
{
  unsigned csr_addr;
  unsigned v;

  csr_addr = libwr2rf_vtu_csr(id);
  if (csr_addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, csr_addr);

  if (en)
    v &= ~WR2RF_INIT_RF_CH_REGS_CH_CSR_DELAY_OEN;
  else
    v |= WR2RF_INIT_RF_CH_REGS_CH_CSR_DELAY_OEN;
  libwr2rf_write16(dev, csr_addr, v);

  return 0;
}

int libwr2rf_vtu_output_shaper(struct libwr2rf_dev *dev, unsigned id,
                               unsigned en)
{
  unsigned csr_addr;
  unsigned v;

  csr_addr = libwr2rf_vtu_csr(id);
  if (csr_addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, csr_addr);

  if (en)
    v &= ~WR2RF_INIT_RF_CH_REGS_CH_CSR_MUX_SEL;
  else
    v |= WR2RF_INIT_RF_CH_REGS_CH_CSR_MUX_SEL;
  libwr2rf_write16(dev, csr_addr, v);

  return 0;
}

int libwr2rf_vtu_output_reset(struct libwr2rf_dev *dev, unsigned id,
                              unsigned rst)
{
  unsigned csr_addr;
  unsigned v;

  csr_addr = libwr2rf_vtu_csr(id);
  if (csr_addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, csr_addr);

  if (rst)
    v |= WR2RF_INIT_RF_CH_REGS_CH_CSR_TRIG_RST;
  else
    v &= ~WR2RF_INIT_RF_CH_REGS_CH_CSR_TRIG_RST;
  libwr2rf_write16(dev, csr_addr, v);

  return 0;
}

static unsigned trig_id_to_addr(unsigned id)
{
  switch (id) {
  case LIBWR2RF_TRIG_ID_RF_1_TRIG_1:
    return WR2RF_VME_REGS_CTRL
      + WR2RF_CTRL_REGS_RF1_VTUS + WR2RF_RFTRIGGER_REGS_T1;
  case LIBWR2RF_TRIG_ID_RF_1_TRIG_2:
    return WR2RF_VME_REGS_CTRL
      + WR2RF_CTRL_REGS_RF1_VTUS + WR2RF_RFTRIGGER_REGS_T2;
  case LIBWR2RF_TRIG_ID_RF_2_TRIG_1:
    return WR2RF_VME_REGS_CTRL
      + WR2RF_CTRL_REGS_RF2_VTUS + WR2RF_RFTRIGGER_REGS_T1;
  case LIBWR2RF_TRIG_ID_RF_2_TRIG_2:
    return WR2RF_VME_REGS_CTRL
      + WR2RF_CTRL_REGS_RF2_VTUS + WR2RF_RFTRIGGER_REGS_T2;
  default:
    return 0;
  }
}

int libwr2rf_vtu_reset (struct libwr2rf_dev *dev, unsigned id, unsigned rst)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONTROL);
  if (rst)
    v |= TRIGUNIT_REGS_CONTROL_VTURESET;
  else
    v &= ~TRIGUNIT_REGS_CONTROL_VTURESET;
  libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONTROL, v);

  if (rst) {
      /* Also clear offline configuration in case of reset.  */
      v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE);
      if (v & TRIGUNIT_REGS_CONFIGOFFLINE_VALID)
        {
          v &= ~TRIGUNIT_REGS_CONFIGOFFLINE_VALID;
          libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE, v);
        }
  }

  return 0;
}

int libwr2rf_vtu_softstop (struct libwr2rf_dev *dev, unsigned id)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONTROL);
  v |= TRIGUNIT_REGS_CONTROL_STOPSOFTTRIGGER;
  libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONTROL, v);

  return 0;
}

int libwr2rf_vtu_softstart (struct libwr2rf_dev *dev, unsigned id)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONTROL);
  v |= TRIGUNIT_REGS_CONTROL_STARTSOFTTRIGGER;
  libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONTROL, v);

  return 0;
}

int libwr2rf_vtu_program_pulse (struct libwr2rf_dev *dev, unsigned id,
                                uint64_t b)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  /* Check the vtu can be programmed, ie the offline configuration is
     not valid.  */
  if (libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE)
      & TRIGUNIT_REGS_CONFIGOFFLINE_VALID)
    return LIBWR2RF_ERROR_VTU_SET;

  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_BVALUEOFFLINE, b);

  v = 0x00 << TRIGUNIT_REGS_CONFIGOFFLINE_MODE_SHIFT;
  v |= TRIGUNIT_REGS_CONFIGOFFLINE_VALID;
  libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE, v);

  return 0;
}

int libwr2rf_vtu_program_infinite (struct libwr2rf_dev *dev, unsigned id,
                                   uint64_t b, unsigned ht)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  /* Check the vtu can be programmed, ie the offline configuration is
     not valid.  */
  if (libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE)
      & TRIGUNIT_REGS_CONFIGOFFLINE_VALID)
    return LIBWR2RF_ERROR_VTU_SET;

  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_BVALUEOFFLINE, b);
  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_HTVALUEOFFLINE, ht);

  v = 0x01 << TRIGUNIT_REGS_CONFIGOFFLINE_MODE_SHIFT;
  v |= TRIGUNIT_REGS_CONFIGOFFLINE_VALID;
  libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE, v);

  return 0;
}

int libwr2rf_vtu_program_window (struct libwr2rf_dev *dev, unsigned id,
                                 uint64_t b, unsigned ht, unsigned w)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  /* Check the vtu can be programmed, ie the offline configuration is
     not valid.  */
  if (libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE)
      & TRIGUNIT_REGS_CONFIGOFFLINE_VALID)
    return LIBWR2RF_ERROR_VTU_SET;

  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_BVALUEOFFLINE, b);
  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_HTVALUEOFFLINE, ht);
  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_WVALUEOFFLINE, w);

  v = 0x02 << TRIGUNIT_REGS_CONFIGOFFLINE_MODE_SHIFT;
  v |= TRIGUNIT_REGS_CONFIGOFFLINE_VALID;
  libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE, v);

  return 0;
}


int libwr2rf_vtu_program_highfreq (struct libwr2rf_dev *dev, unsigned id,
                                   uint64_t b, unsigned div)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  /* Check the vtu can be programmed, ie the offline configuration is
     not valid.  */
  if (libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE)
      & TRIGUNIT_REGS_CONFIGOFFLINE_VALID)
    return LIBWR2RF_ERROR_VTU_SET;

  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_BVALUEOFFLINE, b);
  libwr2rf_be_write64(dev, addr + TRIGUNIT_REGS_HTVALUEOFFLINE, div);

  v = 0x06 << TRIGUNIT_REGS_CONFIGOFFLINE_MODE_SHIFT;
  v |= TRIGUNIT_REGS_CONFIGOFFLINE_VALID;
  libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE, v);

  return 0;
}

int libwr2rf_vtu_program_invalidate (struct libwr2rf_dev *dev, unsigned id)
{
  unsigned addr = trig_id_to_addr (id);
  unsigned v;

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE);
  if (v & TRIGUNIT_REGS_CONFIGOFFLINE_VALID)
    {
      v &= ~TRIGUNIT_REGS_CONFIGOFFLINE_VALID;
      libwr2rf_write16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE, v);
    }

  return 0;
}

struct vtu_mode_xtab {
  unsigned val;
  const char *name;
};

static const struct vtu_mode_xtab vtu_modes[] =
  {
   { 0x00, "pulse"},
   { 0x01, "infinite"},
   { 0x02, "window"},
   { 0x03, "syncless"},
   { 0x04, "lowfreq"},
   { 0x05, "play"},
   { 0x06, "highfreq"},
   { 0, NULL}
  };

static const char *
libwr2rf_vtu_get_mode(unsigned val)
{
  const struct vtu_mode_xtab *e;

  for (e = vtu_modes; e->name; e++)
    if (e->val == val)
      return e->name;
  return "ERROR";
}

int
libwr2rf_vtu_get_mode_by_name(const char *name)
{
  const struct vtu_mode_xtab *e;

  for (e = vtu_modes; e->name; e++)
    if (strcmp (name, e->name) == 0) {
        return e->val;
    }

  return -1;
}

const char *
libwr2rf_vtu_get_mode_by_index(unsigned idx)
{
  if (idx >= sizeof(vtu_modes)/sizeof(vtu_modes[0]))
    return NULL;
  return vtu_modes[idx].name;
}

void
libwr2rf_vtu_dump_by_addr(struct libwr2rf_dev *dev, unsigned addr)
{
  unsigned v;
  uint64_t v64;

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_STATUS);
  printf ("vtu status:   0x%04x [@ 0x%x]\n", v, addr);
  printf ("  started:    0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_STARTED));
  printf ("  idle:       0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_IDLE));
  printf ("  running:    0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_RUNNING));
  printf ("  missidle:   0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_MISSIDLE));
  printf ("  missvalid:  0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_MISSVALID));
  printf ("  missready:  0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_MISSREADY));
  printf ("  startready: 0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_STARTREADY));
  printf ("  wrongb:     0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_WRONGBVALUE));
  printf ("  wronght:    0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_WRONGHTVALUE));
  printf ("  wrongw:     0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_STATUS_WRONGWVALUE));

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONTROL);
  printf ("vtureset:     %u\n", !!(v & TRIGUNIT_REGS_CONTROL_VTURESET));

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGOFFLINE);
  printf ("configoffline:         0x%04x\n", v);
  printf ("   mode:                0x%04x (%s)\n",
          (unsigned)(v & TRIGUNIT_REGS_CONFIGOFFLINE_MODE_MASK),
          libwr2rf_vtu_get_mode((v & TRIGUNIT_REGS_CONFIGOFFLINE_MODE_MASK) >> TRIGUNIT_REGS_CONFIGOFFLINE_MODE_SHIFT));
  printf ("   htswitchingenable:   0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_CONFIGOFFLINE_HTSWITCHINGENABLE));
  printf ("   valid:               0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_CONFIGOFFLINE_VALID));

  v64 = libwr2rf_be_read64(dev, addr + TRIGUNIT_REGS_BVALUEOFFLINE);
  printf (" bvalueoffline:   %lu\n", v64);

  v64 = libwr2rf_be_read64(dev, addr + TRIGUNIT_REGS_HTVALUEOFFLINE);
  printf (" htvalueoffline:  %lu\n", v64);

  v64 = libwr2rf_be_read64(dev, addr + TRIGUNIT_REGS_WVALUEOFFLINE);
  printf (" wvalueoffline:   %lu\n", v64);

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_CONFIGONLINE);
  printf ("configonline:         0x%04x\n", v);
  printf ("   mode:                0x%04x (%s)\n",
          (unsigned)(v & TRIGUNIT_REGS_CONFIGONLINE_MODE_MASK),
          libwr2rf_vtu_get_mode((v & TRIGUNIT_REGS_CONFIGONLINE_MODE_MASK) >> TRIGUNIT_REGS_CONFIGONLINE_MODE_SHIFT));
  printf ("   htswitchingenable:   0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_CONFIGONLINE_HTSWITCHINGENABLE));

  v64 = libwr2rf_be_read64(dev, addr + TRIGUNIT_REGS_BVALUEONLINE);
  printf (" bvalueonline:   %lu\n", v64);

  v64 = libwr2rf_be_read64(dev, addr + TRIGUNIT_REGS_HTVALUEONLINE);
  printf (" htvalueonline:  %lu\n", v64);

  v64 = libwr2rf_be_read64(dev, addr + TRIGUNIT_REGS_WVALUEONLINE);
  printf (" wvalueonline:   %lu\n", v64);

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_TRIGODELAY);
  printf ("trigodelay:   0x%04x\n",
          (unsigned)(v & TRIGUNIT_REGS_TRIGODELAY_DELAY_MASK));

  v = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_FLAGS);
  printf ("flags:        0x%04x\n", v);
}


int
libwr2rf_vtu_dump (struct libwr2rf_dev *dev, unsigned id)
{
  unsigned addr = trig_id_to_addr (id);

  if (addr == 0)
    return LIBWR2RF_ERROR_BAD_ID;
  libwr2rf_vtu_dump_by_addr(dev, addr);
  return 0;
}

int libwr2rf_vtu_nco_reset_delay (struct libwr2rf_dev *dev, unsigned id,
                                  unsigned cdelay, unsigned fdelay, unsigned odelay)
{
  unsigned addr;

  addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_RF
    + WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY;

  switch (id)
    {
    case LIBWR2RF_RF1_CHANNEL_ID:
      addr = addr + WR2RF_INIT_RF_REGS_RF1;
      break;
    case LIBWR2RF_RF2_CHANNEL_ID:
      addr = addr + WR2RF_INIT_RF_REGS_RF2;
      break;
    default:
      return LIBWR2RF_ERROR_BAD_ID;
    }

  cdelay = (cdelay << WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_CDELAY_SHIFT) &
    WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_CDELAY_MASK;
  fdelay = (fdelay << WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_FDELAY_SHIFT) &
    WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_FDELAY_MASK;
  odelay = (odelay << WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_ODELAY_SHIFT) &
    WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_ODELAY_MASK;

  libwr2rf_write16(dev, addr, odelay | fdelay | cdelay);

  return 0;
}

uint32_t libwr2rf_rfnco_read32 (struct libwr2rf_dev *dev, unsigned id, unsigned off)
{
    unsigned addr = 0;

    if (id == LIBWR2RF_RF1_CHANNEL_ID)
        addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_RFNCO;
    else
        addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_RFNCO;

    addr = addr + off;
    return libwr2rf_16x32_read32(dev, addr);
}

void libwr2rf_rfnco_write32 (struct libwr2rf_dev *dev, unsigned id, unsigned off, uint32_t v)
{
    unsigned addr = 0;

    if (id == LIBWR2RF_RF1_CHANNEL_ID)
        addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_RFNCO;
    else
        addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_RFNCO;

    addr = addr + off;
    libwr2rf_16x32_write32(dev, addr, v);
}


int libwr2rf_nco_lcfg (struct libwr2rf_dev *dev, unsigned lcfg, unsigned long long ftw_rfnco1,
  unsigned long long ftw_rfnco2)
{
    unsigned control_lcfg        = RFNCO_CONTROL_SOFTLOAD;
    unsigned control_wrcfg       = RFNCO_CONTROL_SELFTWH1 | RFNCO_CONTROL_EXTRESETENABLE | RFNCO_CONTROL_SELLOAD | RFNCO_CONTROL_SELWRFTWH1;
    unsigned nco1_ftw_addr       = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_RFNCO + RFNCO_FTW_H1;
    unsigned nco2_ftw_addr       = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_RFNCO + RFNCO_FTW_H1;
    unsigned nco1_control_addr   = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_RFNCO + RFNCO_CONTROL;
    unsigned nco2_control_addr   = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_RFNCO + RFNCO_CONTROL;

    printf("nco_ftw_addr=%lx nco_ctrl_addr=%lx\n",  RFNCO_FTW_H1, RFNCO_CONTROL);

    if (lcfg == 0) {
        /* Use network as RFNCO source.  */
        libwr2rf_write16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_NCO_LOC_OR_WRS, 0);

        libwr2rf_16x32_write32(dev, nco1_control_addr, control_wrcfg);
        libwr2rf_16x32_write32(dev, nco2_control_addr, control_wrcfg);
        return 0;
    }

    libwr2rf_write16(dev, WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_NCO_LOC_OR_WRS, 1);

    libwr2rf_16x32_write64(dev, nco1_ftw_addr, ftw_rfnco1);
    libwr2rf_16x32_write64(dev, nco2_ftw_addr, ftw_rfnco2);

    libwr2rf_16x32_write32(dev, nco1_control_addr, control_lcfg);
    libwr2rf_16x32_write32(dev, nco2_control_addr, control_lcfg);

    return 0;
}

int libwr2rf_vtu_rf_reset_offset (struct libwr2rf_dev *dev, unsigned id, unsigned wr_cycles)
{
  unsigned addr = 0;

  addr += WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_RF;
  switch (id)
    {
    case LIBWR2RF_RF1_CHANNEL_ID:
      addr = addr + WR2RF_INIT_RF_REGS_RF1;
      break;
    case LIBWR2RF_RF2_CHANNEL_ID:
      addr = addr + WR2RF_INIT_RF_REGS_RF2;
      break;
    default:
      return LIBWR2RF_ERROR_BAD_ID;
    }
  addr += WR2RF_INIT_RF_CH_REGS_RST_CDELAY;

  libwr2rf_write16(dev, addr, wr_cycles);

  return 0;
}

int libwr2rf_vtu_sync_delays (struct libwr2rf_dev *dev, unsigned id,
                             unsigned *cdelay, unsigned *fdelay, unsigned *odelay, unsigned ref)
{
    unsigned addr;
    unsigned val;

    addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_RF + WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY;

    switch (id) {
    case LIBWR2RF_RF1_CHANNEL_ID:
        addr = addr + WR2RF_INIT_RF_REGS_RF1;
        break;
    case LIBWR2RF_RF2_CHANNEL_ID:
        addr = addr + WR2RF_INIT_RF_REGS_RF2;
        break;
    default:
        return LIBWR2RF_ERROR_BAD_ID;
    }

    if (ref == 1) {
        if (id == 1) {
            *cdelay = LIBWR2RF_VTU_CH1_CDELAY;
            *fdelay = LIBWR2RF_VTU_CH1_FDELAY;
            *odelay = LIBWR2RF_VTU_CH1_ODELAY;
        } else {
            *cdelay = LIBWR2RF_VTU_CH2_CDELAY;
            *fdelay = LIBWR2RF_VTU_CH2_FDELAY;
            *odelay = LIBWR2RF_VTU_CH2_ODELAY;
        }
    } else {
        val     = libwr2rf_read16(dev, addr);
        *cdelay = (val & WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_CDELAY_MASK) >>
                         WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_CDELAY_SHIFT;
        *fdelay = (val & WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_FDELAY_MASK) >>
                         WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_FDELAY_SHIFT;
        *odelay = (val & WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_ODELAY_MASK) >>
                         WR2RF_INIT_RF_CH_REGS_T1_SYNC_PROGDELAY_ODELAY_SHIFT;
    }
    return 0;
}

#if  (LIBWR2RF_GBL_RF1T1_START != WR2RF_CTRL_REGS_GBL_STATUS_RF1_T1_START) \
  || (LIBWR2RF_GBL_RF1T2_START != WR2RF_CTRL_REGS_GBL_STATUS_RF1_T2_START) \
  || (LIBWR2RF_GBL_RF2T1_START != WR2RF_CTRL_REGS_GBL_STATUS_RF2_T1_START) \
  || (LIBWR2RF_GBL_RF2T2_START != WR2RF_CTRL_REGS_GBL_STATUS_RF2_T2_START)
#error "incorrect constants"
#endif
int libwr2rf_vtu_global_status_read(struct libwr2rf_dev *dev, unsigned *res)
{
  unsigned addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_GBL_STATUS;
  unsigned val;

  val = libwr2rf_read16 (dev, addr);
  printf("raw status: %x\n", val);
  val &= (WR2RF_CTRL_REGS_GBL_STATUS_RF1_T1_START
          | WR2RF_CTRL_REGS_GBL_STATUS_RF1_T2_START
          | WR2RF_CTRL_REGS_GBL_STATUS_RF2_T1_START
          | WR2RF_CTRL_REGS_GBL_STATUS_RF2_T2_START);
  if (val) {
    libwr2rf_write16(dev, addr, val);
      printf("write, new status: %x\n", libwr2rf_read16 (dev, addr));
  }
  *res = val;
  return 0;
}

#if (LIBWR2RF_VTU_FAULT_MISSIDLE != TRIGUNIT_REGS_STATUS_MISSIDLE) \
  || (LIBWR2RF_VTU_FAULT_MISSVALID != TRIGUNIT_REGS_STATUS_MISSVALID) \
  || (LIBWR2RF_VTU_FAULT_MISSREADY != TRIGUNIT_REGS_STATUS_MISSREADY)
#error "incorrect constants"
#endif
int libwr2rf_vtu_fault_read(struct libwr2rf_dev *dev, unsigned id,
                            unsigned *fault)
{
    unsigned addr = trig_id_to_addr (id);
    unsigned val;

    if (addr == 0)
      return LIBWR2RF_ERROR_BAD_ID;

    val = libwr2rf_read16(dev, addr + TRIGUNIT_REGS_STATUS);
    val &= (TRIGUNIT_REGS_STATUS_MISSIDLE
            | TRIGUNIT_REGS_STATUS_MISSVALID
            | TRIGUNIT_REGS_STATUS_MISSREADY);
    *fault = val;
    return 0;
}

#if (LIBWR2RF_VTU_FLAGS_FORCE_VALID_OFFLINE != TRIGUNIT_REGS_FLAGS_FORCEVALIDOFFLINE) \
  || (LIBWR2RF_VTU_FLAGS_START_AS_STOP != TRIGUNIT_REGS_FLAGS_STARTASSTOP)
#error "incorrect constant"
#endif
int libwr2rf_vtu_set_flags(struct libwr2rf_dev *dev, unsigned id,
                           unsigned flags)
{
    unsigned addr = trig_id_to_addr (id);


    if (addr == 0)
      return LIBWR2RF_ERROR_BAD_ID;

    libwr2rf_write16(dev, addr + TRIGUNIT_REGS_FLAGS, flags);
    return 0;
}

int libwr2rf_ioupdate_delay (struct libwr2rf_dev *dev,
                                  unsigned fdelay, unsigned odelay)
{
  unsigned addr;

  addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_DDS_IOUPDATE;

  fdelay = (fdelay << WR2RF_INIT_REGS_DDS_IOUPDATE_FDELAY_SHIFT) &
    WR2RF_INIT_REGS_DDS_IOUPDATE_FDELAY_MASK;
  odelay = (odelay << WR2RF_INIT_REGS_DDS_IOUPDATE_ODELAY_SHIFT) &
    WR2RF_INIT_REGS_DDS_IOUPDATE_ODELAY_MASK;

  libwr2rf_write16(dev, addr, odelay | fdelay);

  return 0;
}


int
libwr2rf_configure_tmgio(struct libwr2rf_dev *dev, unsigned lemo, unsigned oe, unsigned term)
{
    unsigned addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_TMG;
    unsigned bval = libwr2rf_read16(dev, addr);
    unsigned mask;

    if (lemo < 1 || lemo > 4)
        return LIBWR2RF_ERROR_BAD_ID;
    // TERM bit
    mask = 1 << (lemo - 1 + WR2RF_INIT_REGS_TMG_IO_TERM_SHIFT);
    if (term)
        bval |= mask;
    else
        bval &= ~mask;

    // OE bit
    mask = 1 << (lemo - 1 + WR2RF_INIT_REGS_TMG_IO_OE_SHIFT);
    if (oe)
        bval |= mask;
    else
        bval &= ~mask;

    libwr2rf_write16(dev, addr, bval);
    return 0;
}

int
libwr2rf_configure_tmgclk(struct libwr2rf_dev *dev, unsigned lemo, unsigned oe, unsigned term)
{
    unsigned addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_TMG;
    unsigned bval = libwr2rf_read16(dev, addr);
    unsigned mask;

    if (lemo < 1 || lemo > 2)
        return LIBWR2RF_ERROR_BAD_ID;
    // TERM bit
    mask = 1 << (lemo - 1 + WR2RF_INIT_REGS_TMG_CLK_TERM_SHIFT);
    if (term)
        bval |= mask;
    else
        bval &= ~mask;

    // OE bit
    mask = 1 << (lemo - 1 + WR2RF_INIT_REGS_TMG_CLK_OE_SHIFT);
    if (oe)
        bval |= mask;
    else
        bval &= ~mask;

    libwr2rf_write16(dev, addr, bval);
    return 0;
}

int
libwr2rf_configure_lemo_debug(struct libwr2rf_dev *dev, unsigned lemo, unsigned dbg)
{
    unsigned baddr = WR2RF_VME_REGS_INIT;
    unsigned addr  = 0;

    if (dbg > LIBWR2RF_LEMO_DBG_NONE)
        return LIBWR2RF_ERROR_BAD_PARAM;

    switch (lemo) {
    case LIBWR2RF_LEMO_TMGIO1:
        addr = WR2RF_INIT_REGS_TMGIO1;
        break;
    case LIBWR2RF_LEMO_TMGIO2:
        addr = WR2RF_INIT_REGS_TMGIO2;
        break;
    case LIBWR2RF_LEMO_TMGIO3:
        addr = WR2RF_INIT_REGS_TMGIO3;
        break;
    case LIBWR2RF_LEMO_TMGIO4:
        addr = WR2RF_INIT_REGS_TMGIO4;
        break;
    case LIBWR2RF_LEMO_TMGCLK1:
        addr = WR2RF_INIT_REGS_TMGCLK1;
        break;
    case LIBWR2RF_LEMO_TMGCLK2:
        addr = WR2RF_INIT_REGS_TMGCLK2;
        break;
    default:
        return LIBWR2RF_ERROR_BAD_ID;
    }

	uint16_t val = (dbg == LIBWR2RF_LEMO_DBG_NONE) ? 0x0 : (1 << dbg);
    libwr2rf_write16(dev, baddr + addr, val);
    return 0;
}

int
libwr2rf_configure_softstart_sel(struct libwr2rf_dev *dev, unsigned sel)
{
    unsigned addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_SOFTSTART;

    switch (sel) {
    case LIBWR2RF_SOFTSTART_NONE_SEL:
        break;
    case LIBWR2RF_RF1T1_SOFTSTART_SEL:
        break;
    case LIBWR2RF_RF1T2_SOFTSTART_SEL:
        break;
    case LIBWR2RF_RF2T1_SOFTSTART_SEL:
        break;
    case LIBWR2RF_RF2T2_SOFTSTART_SEL:
        break;
    default:
        return LIBWR2RF_ERROR_BAD_PARAM;
    }

    libwr2rf_write16(dev, addr, sel);
    return 0;
}

int
libwr2rf_configure_softstop_sel(struct libwr2rf_dev *dev, unsigned sel)
{
    unsigned addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_SOFTSTOP;

    switch (sel) {
    case LIBWR2RF_SOFTSTOP_NONE_SEL:
        break;
    case LIBWR2RF_RF1T1_SOFTSTOP_SEL:
        break;
    case LIBWR2RF_RF1T2_SOFTSTOP_SEL:
        break;
    case LIBWR2RF_RF2T1_SOFTSTOP_SEL:
        break;
    case LIBWR2RF_RF2T2_SOFTSTOP_SEL:
        break;
    default:
        return LIBWR2RF_ERROR_BAD_PARAM;
    }

    libwr2rf_write16(dev, addr, sel);
    return 0;
}

void
libwr2rf_set_nco_reset_ignore (struct libwr2rf_dev *dev, unsigned ignore)
{
  unsigned base_rf1 = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_RFNCO;
  unsigned base_rf2 = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_RFNCO;
  unsigned control_rf1;
  unsigned control_rf2;

  unsigned ncoctrl_addr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_NCO_RESET_CTRL;
  unsigned ncoctrl;

  control_rf1 = libwr2rf_16x32_read32(dev, base_rf1 + RFNCO_CONTROL);
  control_rf2 = libwr2rf_16x32_read32(dev, base_rf2 + RFNCO_CONTROL);

  ncoctrl = libwr2rf_read16(dev, ncoctrl_addr);

  if (ignore) {
    control_rf1 &= ~RFNCO_CONTROL_EXTRESETENABLE;
    control_rf2 &= ~RFNCO_CONTROL_EXTRESETENABLE;
    ncoctrl |= WR2RF_CTRL_REGS_NCO_RESET_CTRL_MASK_DDS;
  }
  else {
    control_rf1 |= RFNCO_CONTROL_EXTRESETENABLE;
    control_rf2 |= RFNCO_CONTROL_EXTRESETENABLE;
    ncoctrl &= ~WR2RF_CTRL_REGS_NCO_RESET_CTRL_MASK_DDS;
  }

  libwr2rf_16x32_write32(dev, base_rf1 + RFNCO_CONTROL, control_rf1);
  libwr2rf_16x32_write32(dev, base_rf2 + RFNCO_CONTROL, control_rf2);

  libwr2rf_write16(dev, ncoctrl_addr, ncoctrl);
}

int
libwr2rf_rfframerx_ts(struct libwr2rf_dev *dev, unsigned ts_nco_reset, uint64_t *ts_tai, uint32_t *ts_cycles)
{
    uint64_t tai         = 0;
    uint32_t cyc         = 0;
    unsigned update_addr = WR2RF_VME_REGS_INIT + WR2RF_INIT_REGS_WRS_RXFRAME_RFFRAME_TS;
    unsigned tai_addr    = WR2RF_VME_REGS_INIT;
    unsigned cyc_addr    = WR2RF_VME_REGS_INIT;

    switch (ts_nco_reset) {
    case LIBWR2RF_RFFRAME_TS:
        tai_addr    += WR2RF_INIT_REGS_WRS_RXFRAME_TAI;
        cyc_addr += WR2RF_INIT_REGS_WRS_RXFRAME_CYCLES;
        break;
    case LIBWR2RF_RFFRAME_TS_NCO_RESET:
        tai_addr    += WR2RF_INIT_REGS_WRS_RXFRAME_NCO_RESET_TAI;
        cyc_addr += WR2RF_INIT_REGS_WRS_RXFRAME_NCO_RESET_CYCLES;
        break;
    default:
        return LIBWR2RF_ERROR_BAD_PARAM;
    }

    // Update the timestamps for reading
    libwr2rf_write16(dev, update_addr, WR2RF_INIT_REGS_WRS_RXFRAME_RFFRAME_TS_UPDATE);
    tai     = (((uint64_t)libwr2rf_read16(dev, tai_addr + 2)) << 32) |
              (((uint64_t)libwr2rf_read16(dev, tai_addr + 4)) << 16) |
              (((uint64_t)libwr2rf_read16(dev, tai_addr + 6)) <<  0);
    cyc     = (((uint64_t)libwr2rf_read16(dev, cyc_addr + 2)) << 32) |
              (((uint64_t)libwr2rf_read16(dev, cyc_addr + 4)) << 16) |
              (((uint64_t)libwr2rf_read16(dev, cyc_addr + 6)) <<  0);

    /* printf("tai %02x %02x %02x %02x %04x %04x\n", libwr2rf_read16(dev, tai_addr + 0), */
    /*        libwr2rf_read16(dev, tai_addr + 2), libwr2rf_read16(dev, tai_addr + 4), */
    /*        libwr2rf_read16(dev, tai_addr + 6), tai_addr, update_addr); */
    /* printf("cyc %02x %02x %02x %02x %04x %04x\n", libwr2rf_read16(dev, cyc_addr + 0), */
    /*        libwr2rf_read16(dev, tai_addr + 2), libwr2rf_read16(dev, tai_addr + 4), */
    /*        libwr2rf_read16(dev, tai_addr + 6), cyc_addr, update_addr); */

    *ts_tai    = tai;
    *ts_cycles = cyc;
    return 0;
}

int
libwr2rf_wr_leapsec(struct libwr2rf_dev *dev, uint32_t *ts_leapsec)
{
    *ts_leapsec = LIBWR2RF_LEAPSEC;

    return 0;
}

int libwr2rf_vtu_timestamps(struct libwr2rf_dev *dev,   unsigned id,
                            uint64_t *ts_vtu_tai_first, uint32_t *ts_vtu_cycles_first,
                            uint64_t *ts_vtu_tai_last,  uint32_t *ts_vtu_cycles_last)
{
    unsigned baddr;
    unsigned val;
    uint64_t secs   = 0;
    uint32_t cycles = 0;

    switch (id) {
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_1:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_VTUS + WR2RF_RFTRIGGER_REGS_T1;
        break;
    case LIBWR2RF_TRIG_ID_RF_1_TRIG_2:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF1_VTUS + WR2RF_RFTRIGGER_REGS_T2;
        break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_1:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_VTUS + WR2RF_RFTRIGGER_REGS_T1;
        break;
    case LIBWR2RF_TRIG_ID_RF_2_TRIG_2:
        baddr = WR2RF_VME_REGS_CTRL + WR2RF_CTRL_REGS_RF2_VTUS + WR2RF_RFTRIGGER_REGS_T2;
        break;
    default:
        return LIBWR2RF_ERROR_BAD_ID;
    }

    secs = 0;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_FIRSTTRIGTAISEC);
    secs = secs | ((uint64_t) val) << 48;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_FIRSTTRIGTAISEC+2);
    secs = secs | ((uint64_t) val) << 32;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_FIRSTTRIGTAISEC+4);
    secs = secs | ((uint64_t) val) << 16;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_FIRSTTRIGTAISEC+6);
    secs = secs | ((uint64_t) val) <<  0;
    *ts_vtu_tai_first = secs;

    cycles = 0;
    val    = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_FIRSTTRIGTAICYCLES+0);
    cycles = cycles | ((uint64_t) val) << 16;
    val    = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_FIRSTTRIGTAICYCLES+2);
    cycles = cycles | ((uint64_t) val) <<  0;
    *ts_vtu_cycles_first = cycles;

    secs = 0;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_LASTTRIGTAISEC);
    secs = secs | ((uint64_t) val) << 48;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_LASTTRIGTAISEC+2);
    secs = secs | ((uint64_t) val) << 32;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_LASTTRIGTAISEC+4);
    secs = secs | ((uint64_t) val) << 16;
    val  = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_LASTTRIGTAISEC+6);
    secs = secs | ((uint64_t) val) <<  0;
    *ts_vtu_tai_last = secs;

    cycles = 0;
    val    = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_LASTTRIGTAICYCLES+0);
    cycles = cycles | ((uint64_t) val) << 16;
    val    = libwr2rf_read16(dev, baddr + TRIGUNIT_REGS_TS_LASTTRIGTAICYCLES+2);
    cycles = cycles | ((uint64_t) val) <<  0;
    *ts_vtu_cycles_last = cycles;

    return 0;
}
