/*
 * Copyright (C) 2018-2023 CERN
 * Author: Maciej Suminski <maciej.suminski@cern.ch>
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * FMC-nanoFIP test program, based on fiptestbed
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <sys/time.h>
#include <unistd.h>
#include <time.h>
#include <math.h>
#include <sys/socket.h>
#include <netinet/ip.h>

#include <elf.h>

#include <masterfip/libmasterfip.h>

#include "mfip_urv.h"
#include "fip_urv_cmd.h"
#include "fip_urv_regs.h"
#include "hydra_supervisor_regs.h"

#ifndef C_PEC_EN
#define C_PEC_EN          0x0
#endif

#define printf @

#define SUPERVISOR_ADDR 0x80000
#define IO_ADDR 0x400000

#define VAR3_ID 0x06  /* produced by agent */
#define VAR1_ID 0x05  /* produced by master */

#define VAR_PRESENCE_ID 0x14 /* len=8, produced by agent */
#define VAR_IDENT_ID 0x10    /* len=11, produced by agent */
#define VAR2_ID 0x91         /* produced by master */
#define VAR_RST_ID 0xe0      /* produced by master */
#define VAR4_ID 0xaa         /* jtag, produced by master */
#define VAR5_ID 0xab         /* len=6, produced by agent */

#define MAX_CMDS 4

#define C_ADC7291_FRACTIONAL_BITS   2
#define C_ADC7291_SIGNED_BIT        11
#define C_ADC7291_SIGNED_VAL        (1 << (C_ADC7291_SIGNED_BIT - C_ADC7291_FRACTIONAL_BITS))

/* ENVM control registers.  Cf UG0448 V8 table 18 */
#define ENVM_WDBUFF    0x80080
#define ENVM_STATUS    0x80120
#define ENVM_CMD       0x80148
#define ENVM_REQACCESS 0x801fc

static unsigned agent_idx = 1;

#define PROD_VAR_LENGTH	 16

/* var3 length (produced by the agent) */
static unsigned cons_var_length = 16;

#if (PROD_VAR_LENGTH < 16) || (PROD_VAR_LENGTH > 128)
#error "bad value for PROD_VAR_LENGTH"
#endif

struct urv_cmd {
  unsigned char cmd;
  unsigned char id;  /* Set when command is sent.  */
  unsigned char len;  /* Length of args (in bytes).  */
  unsigned char args[128];

  int (*cb)(struct urv_cmd *, unsigned char *rep);
  void *cb_data;
};

struct ftb_ba {
    struct mstrfip_macrocycle *mc;
    struct mstrfip_data *cons_varlist[1];/* consumed var */
    struct mstrfip_data *prod_varlist[2];/* produced var */
};

struct ftb_dev {
    struct mstrfip_dev *dev;
    struct ftb_ba ba;

    /* Number of consecutive errors (before the reset).  */
    unsigned nbr_errors;

    unsigned reset;

    enum mstrfip_bitrate fbus_speed; /* FIP bus speed get from the HW */
    int cycle_uslength;
    uint32_t tr_nstime;

  /* Commands to be sent.  */
  struct urv_cmd cmds[MAX_CMDS];
  unsigned nbr_cmds;
  unsigned idx_cmd;
  unsigned waitrep;
  unsigned char idx;
} *ftb;


#if C_PEC_EN == 0x1
static uint8_t crc8_byte(uint8_t data)
{
    uint8_t poly = 0x07; /* x^8 + x^2 + x + 1 */
    uint8_t crc = data;
    for (uint8_t i = 0; i < 8; ++i)
            if (crc & 0x80)
                    crc = ((crc << 1) ^ poly);
            else
                    crc = (crc << 1);
    return crc;
}
#endif

/* Trace packets.  */
static int flag_trace;

/* Get commands from stdin */
static int flag_pipe;

static FILE *out;

static const char *now(void)
{
  time_t t;
  struct tm tm;
  static char buf[80];

  time(&t);
  localtime_r(&t, &tm);

  strftime(buf, sizeof buf, "%Y-%m-%d %H:%M:%S", &tm);
  return buf;
}

/* Finish the current command (discard it).  */
static void next_cmd(void)
{
  ftb->idx_cmd++;

  if (flag_pipe && ftb->idx_cmd == ftb->nbr_cmds) {
    /* Display status.  */
    fprintf (out, "# OK\n");
    fflush(out);
    ftb->idx_cmd = MAX_CMDS + 1;
  }
}

/* Call back.  */
static int read_cons_vars(void)
{
    struct mstrfip_data *pvar = ftb->ba.cons_varlist[0];

    /* Get consumed variable.  */
    memset(pvar->buffer, 0x00, cons_var_length);
    mstrfip_var_update(ftb->dev, pvar);

    switch (pvar->status) {
    case MSTRFIP_DATA_OK:
      if (flag_trace) {
	fprintf (stderr, "Recv:");
	for (unsigned j = 0; j < pvar->bsz; j++)
	  fprintf(stderr, " %02x", pvar->buffer[j]);
	fprintf(stderr, "\n");
      }
      if (pvar->bsz < cons_var_length) {
        fprintf(stderr, "%s cons var too short (got %u bytes, expect %u)\n",
                now(), pvar->bsz, cons_var_length);
        return 1;
      }

      if (pvar->buffer[0] & CMD_APP_FLAG) {
        return application_cb(pvar->buffer, pvar->bsz);
      }

      if (ftb->idx_cmd < ftb->nbr_cmds) {
	struct urv_cmd *cmd = &ftb->cmds[ftb->idx_cmd];

	if (pvar->buffer[0] == cmd->cmd
	    && pvar->buffer[1] == cmd->id) {
	  int res;

	  /* Command has been ack-ed, next one.  */
	  ftb->waitrep = 0;

	  if (cmd->cb != NULL) {
	    res = cmd->cb (cmd, pvar->buffer);
	    if (res < 0) {
              if (flag_pipe) {
                /* Display status.  */
                fprintf (out, "# ERR\n");
                ftb->idx_cmd = MAX_CMDS + 1;
              }
              else {
                /* Will stop.  */
                ftb->idx_cmd = ftb->nbr_cmds;
              }
            }
	  }
	  else
	    res = 0;

	  if (res == 0) {
	    /* By default: next command.  */
            next_cmd();
	  }
	}
        else if (pvar->buffer[0] == CMD_ERROR && pvar->buffer[1] == 0xff) {
          /* Padding */
          if (ftb->waitrep == 0)
            ;
          else if (ftb->waitrep == 5) {
            if (flag_trace)
              fprintf (stderr, "Timeout\n");
            fprintf (out, "# Timeout\n");
            fflush(stdout);
            ftb->waitrep = 0;
            ftb->idx_cmd = MAX_CMDS + 1;
          }
          else
            ftb->waitrep++;
        }
        else { /* if (pvar->buffer[1] != 0) */
          if (flag_trace)
            fprintf (stderr,
                     "Unexpected sequence cmd/id recv:%u/%u, expect:%u/%u\n",
                     pvar->buffer[0], pvar->buffer[1],
                     cmd->cmd, cmd->id);
          fprintf (out, "Unexpected packet\n");
        }
      }
      return 0;

    case MSTRFIP_DATA_PAYLOAD_ERROR:
      if (0) {
        ftb->nbr_errors++;
        fprintf (stderr, "%s var cons: payload err (%u):",
                 now(), ftb->nbr_errors);
        if (pvar->payload_error & MSTRFIP_FRAME_PAYLOAD_NOT_REFRESH)
          fprintf (stderr, " not-refreshed");
        if (pvar->payload_error & MSTRFIP_FRAME_PAYLOAD_NOT_SIGNIFICANT)
          fprintf (stderr, " not-significant");
        fprintf (stderr, "\n");
      }
      return 1;

    case MSTRFIP_DATA_NOT_RECEIVED:
      ftb->nbr_errors++;
      fprintf (stderr, "%s var cons: not received\n", now());
      return 1;

    case MSTRFIP_DATA_FRAME_ERROR:
      ftb->nbr_errors++;
      fprintf (stderr, "%s var cons: frame error (%u):",
               now(), ftb->nbr_errors);
      if (pvar->frame_error & MSTRFIP_FRAME_TMO)
        fprintf (stderr, " timeout");
      if (pvar->frame_error & MSTRFIP_FRAME_ERR)
        fprintf (stderr, " hw-err");
      if (pvar->frame_error & MSTRFIP_FRAME_BAD_CTRL)
        fprintf (stderr, " bad-ctrl");
      if (pvar->frame_error & MSTRFIP_FRAME_BAD_PDU)
        fprintf (stderr, " bad-pdu");
      if (pvar->frame_error & MSTRFIP_FRAME_BAD_BSZ)
        fprintf (stderr, " bad-bsize");
      fprintf (stderr, "\n");
      return 1;

    default:
      return 1;
    }
 }

static int update_prod_vars(void)
{
    int res;
    struct mstrfip_data *pvar;

    /* var3: to nanofip  */
    pvar = ftb->ba.prod_varlist[0];

    if (ftb->waitrep
        || (flag_pipe && ftb->idx_cmd >= ftb->nbr_cmds)) {
      /* Waiting for a reply.  Do not transmit any new command.  */
      pvar->bsz = PROD_VAR_LENGTH;
      pvar->buffer[0] = CMD_ERROR;
      pvar->buffer[1] = 0xff;
      pvar->buffer[2] = 0;
      pvar->buffer[3] = 0;
      if (flag_trace)
        fprintf (stderr, "Send padding (%s)\n",
                 ftb->waitrep ? "waiting for reply" : "idle");
    }
    else if (ftb->idx_cmd >= ftb->nbr_cmds) {
        /* End of command list.  */
        kill(getpid(), SIGINT);
        return 0;
    }
    else {
      /* Send next command.  */
      struct urv_cmd *cmd = &ftb->cmds[ftb->idx_cmd];

      pvar->bsz = PROD_VAR_LENGTH;
      if (cmd->cmd & CMD_APP_FLAG) {
        /* User data.  */
        memcpy (pvar->buffer, cmd->args, cmd->len);

        /* Do not expect answer, next command.  */
        next_cmd();
      }
      else {
        /* A command. */
        cmd->id = ftb->idx++;

        pvar->buffer[0] = cmd->cmd;
        pvar->buffer[1] = cmd->id;
        pvar->buffer[2] = cmd->len;
        memcpy (pvar->buffer + 4, cmd->args, cmd->len);

        /* Command sent, wait for a reply.  */
        ftb->waitrep = 1;
      }

      if (flag_trace) {
	fprintf (stderr, "Send 0x%02x, len=%d:", cmd->cmd, pvar->bsz);
	for (unsigned j = 0; j < 4 + cmd->len; j++)
	  fprintf (stderr, " %02x", pvar->buffer[j]);
	fprintf (stderr, "\n");
      }
    }

    /* Send new ctrl value.  */
    res = mstrfip_var_write(ftb->dev, pvar);
    if (res != 0) {
      fprintf(stderr, "%s mstrfip_var_write error:%d(%s)\n",
              now(), res, strerror(res));
      return 1;
    }

    if (ftb->nbr_errors > 1000 || ftb->reset) {
      pvar = ftb->ba.prod_varlist[1];

      pvar->bsz = 4;
      pvar->buffer[0] = 0;
      if (ftb->nbr_errors > 1000 || ftb->reset == 2) {
        /* Do reset */
        if (flag_trace)
          fprintf(stderr, "Send reset\n");
        pvar->buffer[1] = agent_idx;
        ftb->nbr_errors = 0;
        ftb->reset = 1;
      }
      else {
        /* Clear reset */
        if (flag_trace)
          fprintf(stderr, "Send no-reset\n");
        pvar->buffer[1] = 0;
        ftb->reset = 0;
      }

      res = mstrfip_var_write(ftb->dev, pvar);
      if (res != 0) {
        fprintf(stderr, "%s mstrfip_var_write error:%d(%s)\n",
                now(), res, strerror(res));
        return 1;
      }
    }
    return 0;
}

static void mstrfip_cons_var_handler(struct mstrfip_dev *dev,
                                     struct mstrfip_data *pvar,
                                     struct mstrfip_irq *irq)
{
    if (flag_trace > 1) {
      fprintf(stderr, "cons handler: type: %u, id: %04x, bsz: %u, status: %u, ferr: %u, perr: %u\n",
	     pvar->type, pvar->id, pvar->bsz, pvar->status,
	     pvar->frame_error, pvar->payload_error);
    }

    read_cons_vars();
    update_prod_vars();
}

static void mstrfip_prod_var_handler(struct mstrfip_dev *dev, struct mstrfip_data *pvar,
                                     struct mstrfip_irq *irq)
{
    if (flag_trace > 1) {
      fprintf(stderr, "prod handler: type: %u, id: %04x, bsz: %u, status: %u, ferr: %u, perr: %u\n",
	     pvar->type, pvar->id, pvar->bsz, pvar->status,
	     pvar->frame_error, pvar->payload_error);
    }
}

static void mstrfip_error_handler(struct mstrfip_dev *dev, enum mstrfip_error_list error)
{
  fprintf(stderr, "error_handler\n");
}

static int ftb_create_per_var(struct ftb_ba *ba)
{
    struct mstrfip_data_cfg var_cfg;
    struct mstrfip_data *pvar;

    /* Build produced var1 (sent by masterfip).  */
    var_cfg.max_bsz = PROD_VAR_LENGTH;
    var_cfg.id = (VAR1_ID << 8) | agent_idx;
    var_cfg.flags = MSTRFIP_DATA_FLAGS_PROD;
    if (flag_trace > 1)
      var_cfg.mstrfip_data_handler = mstrfip_prod_var_handler;
    else
      var_cfg.mstrfip_data_handler = NULL;
    pvar = mstrfip_var_create(ba->mc, &var_cfg);
    if (pvar == NULL)
      return -1;
    ba->prod_varlist[0] = pvar;

#if 0
    pvar->bsz = PROD_VAR_LENGTH;
    pvar->buffer[0] = CMD_ERROR;
    pvar->buffer[1] = 0xff;
    res = mstrfip_var_write(ftb->dev, pvar);
    if (res != 0) {
      fprintf(stderr, "%s mstrfip_var_write error:%d(%s)\n",
              now(), res, strerror(res));
      return -1;
    }
#endif

    /* Build produced var for reset (sent by masterfip).  */
    var_cfg.max_bsz = 4;
    var_cfg.id = (VAR_RST_ID << 8) | agent_idx;
    var_cfg.flags = MSTRFIP_DATA_FLAGS_PROD;
    var_cfg.mstrfip_data_handler = NULL; // mstrfip_prod_var_handler;
    pvar = mstrfip_var_create(ba->mc, &var_cfg);
    if (pvar == NULL)
      return -1;
    ba->prod_varlist[1] = pvar;

    /* build consumed var3 (received by masterfip).  */
    var_cfg.max_bsz = cons_var_length;
    var_cfg.id = (VAR3_ID << 8) | agent_idx;
    var_cfg.flags = MSTRFIP_DATA_FLAGS_CONS;
    var_cfg.mstrfip_data_handler = mstrfip_cons_var_handler;
    ba->cons_varlist[0] = mstrfip_var_create(ba->mc, &var_cfg);
    if (ba->cons_varlist[0] == NULL)
      return -1;

    return 0;
}

static int ftb_define_macrocycle(struct ftb_ba *ba)
{
    struct mstrfip_per_var_wind_cfg pwind_cfg = {0};

    /* append a periodic window for produced variables */
    pwind_cfg.varlist = ba->prod_varlist;
    pwind_cfg.var_count = 2;
    if (mstrfip_per_var_wind_append(ba->mc, &pwind_cfg) < 0)
        return -1;

    /* append a wait window without padding (silent) (absolute time). */
    if (mstrfip_wait_wind_append(ba->mc, 1, ftb->cycle_uslength / 2) < 0)
      return -1;

    /* append a periodic window for consumed variables */
    pwind_cfg.varlist = ba->cons_varlist;
    pwind_cfg.var_count = 1;
    if (mstrfip_per_var_wind_append(ba->mc, &pwind_cfg) < 0)
        return -1;

    /* append a wait window without padding (silent).
       As the time is absolute, this defines the cycle length.  */
    if (mstrfip_wait_wind_append(ba->mc, 1, ftb->cycle_uslength) < 0)
        return -1;

    return 0;
}

static int cycle_config(struct mstrfip_hw_cfg *hw_cfg, struct mstrfip_sw_cfg *sw_cfg)
{
    if (mstrfip_hw_cfg_set(ftb->dev, hw_cfg) < 0) {
        fprintf(stderr, "Cannot configure HW: %s\n", mstrfip_strerror(errno));
        return -1;
    }

    /* set SW config */
    if (mstrfip_sw_cfg_set(ftb->dev, sw_cfg) < 0) {
        fprintf(stderr, "Cannot configure SW: %s\n", mstrfip_strerror(errno));
        return -1;
    }

    /* create macro cycle */
    ftb->ba.mc = mstrfip_macrocycle_create(ftb->dev);
    if (ftb->ba.mc == NULL) {
        fprintf(stderr, "Create macrocyle failed: %s\n", mstrfip_strerror(errno));
        return -1;
    }

    /* create periodic variables */
    if (ftb_create_per_var(&ftb->ba) < 0 ) {
        fprintf(stderr, "Create periodic variable failed: %s\n", mstrfip_strerror(errno));
        return -1;
    }

    if (ftb_define_macrocycle(&ftb->ba) < 0) {
        fprintf(stderr, "Programming BA failed: %s\n", mstrfip_strerror(errno));
        return -1;
    }

    if (mstrfip_macrocycle_isvalid(ftb->dev, ftb->ba.mc) == 0) {
        fprintf(stderr, "Macrocycle invalid: %s\n", mstrfip_strerror(errno));
        return -1;
    }

    return 0;
}

static int ftb_ba_start(struct ftb_dev *ftb)
{
    int res;
    int agt_addr;

    if (mstrfip_ba_reset(ftb->dev) < 0) {
        fprintf(stderr, "Reset BA failed: %s\n",
            mstrfip_strerror(errno));
        return -1;
    }

    if (mstrfip_ba_load(ftb->dev, ftb->ba.mc) < 0) {
        fprintf(stderr, "Loading macrocycle failed: %s\n",
            mstrfip_strerror(errno));
        return -1;
    }

    /* get turn around time of each agents */
    agt_addr = agent_idx;

    res = mstrfip_hw_response_time_get(ftb->dev, 1, &agt_addr, &ftb->tr_nstime);

    if (res) {
        fprintf(stderr, "Read turn around time failed: %s\n",
            mstrfip_strerror(errno));
    }

    if (mstrfip_ba_start(ftb->dev) < 0) {
        fprintf(stderr, "Start BA failed: %s\n",
            mstrfip_strerror(errno));
        return -1;
    }
    return 0;
}

static int append_command(struct ftb_dev *dev, const struct urv_cmd *cmd)
{
    if (dev->nbr_cmds >= MAX_CMDS)
        return -1;
    memcpy (&dev->cmds[dev->nbr_cmds], cmd, sizeof(struct urv_cmd));
    dev->nbr_cmds++;
    return 0;
}

static void word_pack(unsigned char *buf, unsigned val)
{
  buf[0] = val;
  buf[1] = val >> 8;
  buf[2] = val >> 16;
  buf[3] = val >> 24;
}

static unsigned word_unpack(unsigned char *buf)
{
  unsigned val;

  val = buf[0];
  val |= (buf[1] << 8);
  val |= (buf[2] << 16);
  val |= (buf[3] << 24);
  return val;
}

static int cb_get_leds (struct urv_cmd *cmd, unsigned char *rep)
{
  fprintf (out, "Led status: 0x%02x\n", rep[4]);
  return 0;
}

static int cb_read_word (struct urv_cmd *cmd, unsigned char *rep)
{
  fprintf (out, "0x%08x\n", word_unpack(&rep[4]));
  return 0;
}

struct dump_cmd {
  unsigned int addr;
  unsigned int len;
};

static int cb_dump(struct urv_cmd *cmd, unsigned char *rep)
{
  struct dump_cmd *dmp = cmd->cb_data;

  if ((dmp->addr & 0x0f) == 0)
    fprintf (out, "%08x: ", dmp->addr);
  fprintf (out, " 0x%08x", word_unpack(&rep[4]));

  dmp->len -= 4;
  dmp->addr += 4;

  if ((dmp->addr & 0x0f) == 0 || dmp->len == 0)
    fprintf (out, "\n");

  if (dmp->len == 0) {
    free (dmp);
    return 0;
  }
  word_pack(&cmd->args[0], dmp->addr);
  return 1;
}

static int cb_ping (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned i;
  unsigned l = cons_var_length < PROD_VAR_LENGTH ?
    cons_var_length : PROD_VAR_LENGTH;

  fprintf (out, "ping:");
  for (i = 0; i < l - 4; i++) {
    fprintf (out, " %02x", rep[4 + i]);
    if (rep[4 + i] != (~cmd->args[i] & 0xff))
      fprintf(out, "!");
  }
  fputc ('\n', out);
  return 0;
}

static int cb_read_psu (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned val = word_unpack(&rep[4]);
  unsigned i;

  for (i = 0; i < 2; i++)
    fprintf (out, "PSU %u: presence: %u\n", i, (val >> i) & 1);
  return 0;
}

static int cb_read_i2c (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned val = word_unpack(&rep[4]);

  fprintf (out, "I2C controller: %u\n", val);
  fprintf (out, "0: monimod, 1: backplane, 2: psu, 3: pcbver\n");
  return 0;
}

static int cb_i2c_stat (struct urv_cmd *cmd, unsigned char *rep)
{
  fprintf (out, "state: %u\n", rep[4 + 0]);
  fprintf (out, "len:   %u\n", rep[4 + 1]);
  fprintf (out, "coff:  %u\n", rep[4 + 2]);
  fprintf (out, "boff:  %u\n", rep[4 + 3]);
  return 0;
}

static int cb_i2c_off (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned val = word_unpack(&rep[4]);
  fprintf (out, "i2c-off: 0x%x\n", val);
  return 0;
}

static int cb_supervisor (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned addr = word_unpack(&cmd->args[0]);
  unsigned reg = (addr - SUPERVISOR_ADDR);
  unsigned val;

  val = word_unpack(&rep[4]);

  switch (reg) {
  case HYDRA_SUPERVISOR_REGS_RESET_CAUSE:
      fprintf (out, "reset cause: 0x%02x\n", val);
      reg = HYDRA_SUPERVISOR_REGS_CPU;
      break;
  case HYDRA_SUPERVISOR_REGS_CPU:
      fprintf (out, "cpus on reset: 0x%1x\n", val);
      reg = HYDRA_SUPERVISOR_REGS_WD_PERIOD;
      break;
  case HYDRA_SUPERVISOR_REGS_WD_PERIOD:
      fprintf (out, "watchdog period: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_WD_COUNT;
      break;
  case HYDRA_SUPERVISOR_REGS_WD_COUNT:
      fprintf (out, "watchdog count: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_ECC_SE;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_ECC_SE:
      fprintf (out, "IRAM corrected errors: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_CYCLE;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_CYCLE:
      fprintf (out, "IRAM scrub cycle number: 0x%04x\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_SE;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_SE:
      fprintf (out, "IRAM corrected errors by scrubber: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_DE_CURR;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_DE_CURR:
      fprintf (out, "IRAM double errors detected in current cycle: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_DE_LAST;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_DE_LAST:
      fprintf (out, "IRAM double errors detected in last cycle: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_DRAM_ECC_SE;
      break;
  case HYDRA_SUPERVISOR_REGS_DRAM_ECC_SE:
      fprintf (out, "DRAM corrected errors: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_CYCLE;
      break;
  case HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_CYCLE:
      fprintf (out, "DRAM scrub cycle number: 0x%04x\n", val);
      reg = HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_SE;
      break;
  case HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_SE:
      fprintf (out, "DRAM corrected errors by scrubber: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_DE_CURR;
      break;
  case HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_DE_CURR:
      fprintf (out, "DRAM double errors detected in current cycle: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_DE_LAST;
      break;
  case HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_DE_LAST:
      fprintf (out, "DRAM double errors detected in last cycle: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_CPU_DATA_ERR;
      break;
  case HYDRA_SUPERVISOR_REGS_CPU_DATA_ERR:
      fprintf (out, "Errors on cpu data bus: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_CPU_IADDR_ERR;
      break;
  case HYDRA_SUPERVISOR_REGS_CPU_IADDR_ERR:
      fprintf (out, "Errors on cpu insn bus: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_SCRUB_CFG;
      break;
  case HYDRA_SUPERVISOR_REGS_SCRUB_CFG:
      fprintf (out, "Scrubber config: %x\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_PERIOD;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_SCRUB_PERIOD:
      fprintf (out, "IRAM scrubber period: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_PERIOD;
      break;
  case HYDRA_SUPERVISOR_REGS_DRAM_SCRUB_PERIOD:
      fprintf (out, "DRAM scrubber period: %u\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_ECC_ADDR;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_ECC_ADDR:
      fprintf (out, "IRAM ecc addr: %08x\n", val);
      reg = HYDRA_SUPERVISOR_REGS_IRAM_ERR_ADDR;
      break;
  case HYDRA_SUPERVISOR_REGS_IRAM_ERR_ADDR:
      fprintf (out, "IRAM err addr: %08x\n", val);
      return 0;
  default:
      abort ();
  }
  /* Read next word */
  word_pack(&cmd->args[0], SUPERVISOR_ADDR + reg);
  return 1;
}

static int cb_stat_regs (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned reg = cmd->args[0];
  unsigned val = word_unpack(&rep[4]);

  /* Build Time Variables */
  time_t date;
  struct tm *build_date_tm;
  char buffer[80];

  switch (reg) {
    case STAT_MONIMOD_RST:
      fprintf (out, "nbr monimod reset: %u\n", val);
      break;
    case STAT_FANTRAY_RST:
      fprintf (out, "nbr fantray reset: %u\n", val);
      break;
    case STAT_RATOPUS_ERR:
      fprintf (out, "nbr ratopus errors: %u\n", val);
      break;
    case STAT_MIN_WD_CNT:
      fprintf (out, "minimum watchdog count: %u\n", val);
      break;
    case STAT_RESET:
      fprintf (out, "nbr resets: %u\n", val);
      break;
    case STAT_RECOVERY:
      fprintf (out, "nbr recovery: %u\n", val);
      break;
    case STAT_LAST_RST:
      fprintf (out, "last reset: %u\n", val);
      break;
    case STAT_CYCLE_CNT:
      fprintf (out, "cycle count: %u\n", val);
      break;
    case STAT_FIP_CMD:
      fprintf (out, "fip commands: %u\n", val);
      break;
    case STAT_FIP_PAD:
      fprintf (out, "fip pad: %u\n", val);
      break;
    case STAT_BUILD_DATE:
      date = (time_t) val;
      build_date_tm = localtime(&date);
      strftime(buffer,80,"%d-%m-%Y %H:%M:%S", build_date_tm);
      fprintf (out, "build date: %s\n", buffer);
      break;
    case STAT_GIT_HASH:
      fprintf (out, "git hash commit: %08x\n", val);
      /* The last one.  */
      //      return 0;
      break;
    case STAT_APP0_START:
      fprintf (out, "app0 eNVM address: %08x\n", val);
      break;
    case STAT_FIP_WRITES:
      fprintf (out, "fip writes: %u\n", val);
      break;
    case STAT_WRITE_ADDR:
      fprintf (out, "write addr: 0x%08x\n", val);
      break;
    case STAT_DATA_WR:
      fprintf (out, "app pkt written: %u\n", val);
      break;
    case STAT_DATA_WDIS:
      fprintf (out, "app pkt discarded: %u\n", val);
      break;
    default:
      fprintf (out, "stat_regs[%u]: %u\n", reg, val);
      break;
  }
  if (++cmd->args[0] < STAT_NBR)
    return 1;
  return 0;
}

static int cb_fip_err (struct urv_cmd *cmd, unsigned char *rep)
{
  fprintf (out, "fip errors: %u\n", word_unpack(&rep[4]));
  return 0;
}

static int cb_slots (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned addr = word_unpack(&cmd->args[0]);
  unsigned slot = (addr - IO_ADDR - FIP_URV_REGS_BOARDS) >> 2;
  int i;
  unsigned val;

  val = word_unpack(&rep[4]);

  fprintf (out, "Slot %d: ", slot);
  for (i = 31; i >= 0; i--) {
    fputc ((val & (1 << i)) ? '*' : '.', out);
    if (i == 16)
      fputc (' ', out);
  }
  putc ('\n', out);
  if (slot == 7)
    return 0;
  addr += 4;
  word_pack(&cmd->args[0], addr);
  return 1;
}

static int cb_check_off (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned val = word_unpack(&rep[4]);
  if (val != 2) {
    fprintf (out, "PLC cpu is not on halt (do 'plc halt')\n");
    return -1;
  }
  return 0;
}

static int cb_plc_status (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned addr = word_unpack(&cmd->args[0]);
  unsigned base = IO_ADDR + FIP_URV_REGS_APP;
  unsigned val = word_unpack(&rep[4]);
  unsigned reg;

  if (addr >= base + FIP_URV_REGS_APP_SIZE)
    base += FIP_URV_REGS_APP_SIZE;
  reg = addr - base;

  if (reg == FIP_URV_REGS_APP_CR) {
    fprintf (out, "plc reset:  %u  # 1: sys-rst, 2: cpu-rst\n", val);
    word_pack(&cmd->args[0], base + FIP_URV_REGS_APP_SR);
    return 1;
  }
  else if (reg == FIP_URV_REGS_APP_SR) {
    fprintf (out, "plc status: %u  # 1: fault\n", val);
  }
  return 0;
}

static int cb_plc_supervisor (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned addr = word_unpack(&cmd->args[0]);
  unsigned base = IO_ADDR + FIP_URV_REGS_APP;
  unsigned reg;
  unsigned val;

  if (addr >= base + FIP_URV_REGS_APP_SIZE)
    base += FIP_URV_REGS_APP_SIZE;
  reg = addr - base;

  val = word_unpack(&rep[4]);

  switch (reg) {
  case FIP_URV_REGS_APP_RESET_CAUSE:
      fprintf (out, "PLC reset cause: %1x", val);
      fprintf (out, "  # 1: cpu, 2: iram, 4: dram, 8: wd, 16: recov\n");
      reg = FIP_URV_REGS_APP_IRAM_ECC_SE;
      break;
  case FIP_URV_REGS_APP_IRAM_ECC_SE:
      fprintf (out, "PLC IRAM corrected errors: %u\n", val);
      reg = FIP_URV_REGS_APP_IRAM_SCRUB_CYCLE;
      break;
  case FIP_URV_REGS_APP_IRAM_SCRUB_CYCLE:
      fprintf (out, "PLC IRAM scrub cycle number: 0x%04x\n", val);
      reg = FIP_URV_REGS_APP_IRAM_SCRUB_SE;
      break;
  case FIP_URV_REGS_APP_IRAM_SCRUB_SE:
      fprintf (out, "PLC IRAM corrected errors by scrubber: %u\n", val);
      reg = FIP_URV_REGS_APP_IRAM_SCRUB_DE_LAST;
      break;
  case FIP_URV_REGS_APP_IRAM_SCRUB_DE_LAST:
      fprintf (out, "PLC IRAM double errors detected in last cycle: %u\n", val);
      reg = FIP_URV_REGS_APP_DRAM_ECC_SE;
      break;
  case FIP_URV_REGS_APP_DRAM_ECC_SE:
      fprintf (out, "PLC DRAM corrected errors: %u\n", val);
      reg = FIP_URV_REGS_APP_DRAM_SCRUB_CYCLE;
      break;
  case FIP_URV_REGS_APP_DRAM_SCRUB_CYCLE:
      fprintf (out, "PLC DRAM scrub cycle number: 0x%04x\n", val);
      reg = FIP_URV_REGS_APP_DRAM_SCRUB_SE;
      break;
  case FIP_URV_REGS_APP_DRAM_SCRUB_SE:
      fprintf (out, "PLC DRAM corrected errors by scrubber: %u\n", val);
      reg = FIP_URV_REGS_APP_DRAM_SCRUB_DE_LAST;
      break;
  case FIP_URV_REGS_APP_DRAM_SCRUB_DE_LAST:
      fprintf (out, "PLC DRAM double errors detected in last cycle: %u\n", val);
      reg = FIP_URV_REGS_APP_SCRUB_CFG;
      break;
  case FIP_URV_REGS_APP_SCRUB_CFG:
      fprintf (out, "PLC scrub config: %u\n", val);
      reg = FIP_URV_REGS_APP_NBR_CPU_FAULT;
      break;
  case FIP_URV_REGS_APP_NBR_CPU_FAULT:
      fprintf (out, "PLC nbr cpu fault: %u\n", val);
      return 0;
  default:
      abort ();
  }
  /* Read next word */
  word_pack(&cmd->args[0], base + reg);
  return 1;
}

static int cb_mbox (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned addr = word_unpack(&cmd->args[0]);
  unsigned val = word_unpack(&rep[4]);
  unsigned base = IO_ADDR + FIP_URV_REGS_APP;
  unsigned reg;

  if (addr >= base + FIP_URV_REGS_APP_SIZE)
    base += FIP_URV_REGS_APP_SIZE;
  reg = addr - base;

  if (reg == FIP_URV_REGS_APP_MAILBOXES + MBOX_REGS_STATUS) {
    fprintf (out, "mailbox status    (@0x%08x): %u\n", addr, val);
    reg += (unsigned)(MBOX_REGS_MBOXIN - MBOX_REGS_STATUS);
    word_pack(&cmd->args[0], base + reg);
    return 1;
  }
  if (reg  == FIP_URV_REGS_APP_MAILBOXES + MBOX_REGS_MBOXIN) {
    fprintf (out, "mailbox in value  (@0x%08x): 0x%08x\n", addr, val);
    reg += (unsigned)(MBOX_REGS_MBOXOUT - MBOX_REGS_MBOXIN);
    word_pack(&cmd->args[0], base + reg);
    return 1;
  }
  if (reg  == FIP_URV_REGS_APP_MAILBOXES + MBOX_REGS_MBOXOUT) {
    fprintf (out, "mailbox out value (@0x%08x): 0x%08x\n", addr, val);
  }
  return 0;
}

static int cb_restart_cfg (struct urv_cmd *cmd, unsigned char *rep)
{
  unsigned val = word_unpack(&rep[4]);
  fprintf (out, "restart config: 0x%08x\n", val);
  fprintf (out, "  iram copy:  0x%08lx (/ 0x%08lx)\n",
          val & FIP_URV_REGS_RESTART_CFG_IRAM_COPY,
          FIP_URV_REGS_RESTART_CFG_IRAM_COPY);
  fprintf (out, "  dram clear: 0x%08lx (/ 0x%08lx)\n",
          val & FIP_URV_REGS_RESTART_CFG_DRAM_CLEAR,
          FIP_URV_REGS_RESTART_CFG_DRAM_CLEAR);
  fprintf (out, "  iram err:   0x%08lx (/ 0x%08lx)\n",
          val & FIP_URV_REGS_RESTART_CFG_IRAM_ERR,
          FIP_URV_REGS_RESTART_CFG_IRAM_ERR);
  fprintf (out, "  iram src:   0x%08lx (/ 0x%08lx)\n",
          val & FIP_URV_REGS_RESTART_CFG_IRAM_SRC,
          FIP_URV_REGS_RESTART_CFG_IRAM_SRC);
  fprintf (out, "  src page:   0x%08lx (/ 0x%08lx) (addr=page*256)\n",
          ((val & FIP_URV_REGS_RESTART_CFG_SRC_PAGE_MASK)
           >> FIP_URV_REGS_RESTART_CFG_SRC_PAGE_SHIFT),
          FIP_URV_REGS_RESTART_CFG_SRC_PAGE_MASK);
  return 0;
}

static int cb_wait (struct urv_cmd *cmd, unsigned char *rep)
{
  fprintf (out, "wait done\n");
  return 0;
}

static int cb_monimod_power (struct urv_cmd *cmd, unsigned char *rep)
{
  fprintf (out, "monimod shutdown: %x\n", word_unpack(&rep[4]));
  return 0;
}

static int cb_monimod_scan (struct urv_cmd *cmd, unsigned char *rep)
{
  fprintf (out, "monimod-scan state: %u\n", rep[4]);
  return 0;
}

static int float_to_linear (float val)
{
    int16_t exp = -16;

    if (val)
        exp = ceilf(log2f(fabsf(val)) - log2f(1023));
    else
        return 0;

    if (exp < -16)
        exp = -16;
    if (exp > 15)
        exp = 15;

    int16_t man = floorf(val / powf(2, exp));
    if (man < -1024)
        man = -1024;
    if (man > 1023)
        man = 1023;

    return ((exp & 0x1f) << 11) | (man & 0x7ff);
}

static float linear_to_float(unsigned char *rep)
{
  unsigned val = (rep[1] << 8) | rep[0];

  int16_t exp = ((val & 0x8000)?0xffe0:0) | ((val >> 11) & 0x1f);
  int16_t man = ((val & 0x0400)?0xf800:0) | (val & 0x7ff);

  return ldexpf(man, exp);
}

static float linear16_to_float(unsigned char *rep)
{
  unsigned val = (rep[1] << 8) | rep[0];
  return ldexpf(val, -10);
}

static Elf32_Half read_elf_half (const Elf32_Half *v)
{
  const unsigned char *p = (const unsigned char *)v;
  return p[0] | (p[1] << 8);   /* LE  */
}

static Elf32_Word read_elf_word (const Elf32_Word *v)
{
  const unsigned char *p = (const unsigned char *)v;
  return p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);   /* LE  */
}

static void cmd_write_word(struct urv_cmd *cmd, unsigned addr, unsigned val)
{
  cmd->cmd = CMD_WRITE_WORD;
  cmd->len = 8;
  word_pack(&cmd->args[0], addr);
  word_pack(&cmd->args[4], val);
}

static void cmd_read_word(struct urv_cmd *cmd, unsigned addr)
{
  cmd->cmd = CMD_READ_WORD;
  cmd->len = 4;
  word_pack(&cmd->args[0], addr);
}

static void cmd_write_bytes(struct urv_cmd *cmd, unsigned addr,
                            unsigned char *buf, unsigned len)
{
  if (len > PROD_VAR_LENGTH - 8 || (len & 3))
    abort();
  cmd->cmd = CMD_WRITE_WORD;
  cmd->len = 4 + len;
  word_pack(&cmd->args[0], addr);
  memcpy (&cmd->args[4], buf, len);
}

struct load_cb_data {
  unsigned mem_addr;
  unsigned char *buf;
  size_t len;
  size_t off;
};

static int load_cb_prepare(struct urv_cmd *cmd);

static int cb_load (struct urv_cmd *cmd, unsigned char *rep)
{
  if (load_cb_prepare (cmd) == 0)
    return 0;
  return 1;
}

static int load_cb_prepare(struct urv_cmd *cmd)
{
  struct load_cb_data *dat = (struct load_cb_data *)cmd->cb_data;
  unsigned l;

  if (dat->off >= dat->len) {
    free (dat->buf);
    free (dat);
    return 0;
  }
  l = dat->len - dat->off;
  if (l > PROD_VAR_LENGTH - 8)
    l = PROD_VAR_LENGTH - 8;
  l &= ~3;

  cmd->cmd = CMD_WRITE_WORD;
  cmd->len = 4 + l;
  word_pack(&cmd->args[0], dat->mem_addr + dat->off);
  memcpy (&cmd->args[4], dat->buf + dat->off, l);
  cmd->cb = cb_load;
  dat->off += l;
  return 1;
}

static void *xmalloc (size_t sz)
{
  void *res = malloc (sz);
  if (res == NULL) {
    fprintf (stderr, "virtual memory exhausted\n");
    exit (2);
  }
  return res;
}

static int load_elf (const char *filename, unsigned char **buf, unsigned *sz)
{
  FILE *f;
  Elf32_Ehdr ehdr;
  unsigned poff;
  unsigned pnum;
  unsigned filesz;
  unsigned memsz;
  unsigned loff;
  unsigned i;

  *sz = 0;
  *buf = NULL;

  f = fopen (filename, "r");
  if (f == NULL) {
    fprintf (stderr, "cannot open %s\n", filename);
    goto err;
  }

  if (fread (&ehdr, sizeof (ehdr), 1, f) != 1) {
    fprintf (stderr, "cannot read ELF header of %s\n", filename);
    goto err;
  }
  if (ehdr.e_ident[EI_MAG0] != ELFMAG0
      || ehdr.e_ident[EI_MAG1] != ELFMAG1
      || ehdr.e_ident[EI_MAG2] != ELFMAG2
      || ehdr.e_ident[EI_MAG3] != ELFMAG3) {
    fprintf (stderr, "file %s is not an ELF file\n", filename);
    goto err;
  }

  if (ehdr.e_ident[EI_CLASS] != ELFCLASS32
      || ehdr.e_ident[EI_DATA] != ELFDATA2LSB
      || ehdr.e_ident[EI_VERSION] != EV_CURRENT) {
    fprintf (stderr, "file %s is not expect ELF class\n", filename);
    goto err;
  }

  if (read_elf_half (&ehdr.e_type) != ET_EXEC
      || read_elf_half (&ehdr.e_machine) != 0xf3
      || read_elf_word (&ehdr.e_version) != EV_CURRENT) {
    fprintf (stderr, "file %s is not a risc-v executable\n", filename);
    goto err;
  }

  if (read_elf_half (&ehdr.e_phentsize) != sizeof (Elf32_Phdr)) {
    fprintf (stderr, "file %s has bad phdr size\n", filename);
    goto err;
  }

  pnum = read_elf_half (&ehdr.e_phnum);

  if (read_elf_word (&ehdr.e_entry) != 0) {
    fprintf (stderr, "file %s entry point is not 0\n", filename);
    goto err;
  }

  poff = read_elf_word (&ehdr.e_phoff);

  if (fseek (f, poff, SEEK_SET) != 0) {
    fprintf (stderr, "%s: cannot seek to first program header\n", filename);
    goto err;
  }

  loff = 0;
  for (i = 0; i < pnum; i++) {
    Elf32_Phdr phdr;
    unsigned sz;

    if (fread (&phdr, sizeof (phdr), 1, f) != 1) {
      fprintf (stderr, "%s: cannot read program header\n", filename);
      goto err;
    }
    if (read_elf_word (&phdr.p_type) != PT_LOAD)
      continue;
    /* TODO: review that change slowly */
    //sz = read_elf_word (&phdr.p_memsz);
    sz = read_elf_word (&phdr.p_filesz);
    if (sz == 0)
      continue;
    if (loff != 0) {
      fprintf (stderr, "%s: multiple load headers\n", filename);
      goto err;
    }
    //memsz = sz;
    //filesz = read_elf_word (&phdr.p_filesz);
    filesz = sz;
    memsz = read_elf_word (&phdr.p_memsz);
    loff = read_elf_word (&phdr.p_offset);
  }

  if (loff == 0) {
    fprintf (stderr, "%s: missing load program header\n", filename);
    goto err;
  }

  memsz = (memsz + 3) & ~3;
  *buf = xmalloc (memsz);
  if (fseek (f, loff, SEEK_SET) != 0
      || fread (*buf, filesz, 1, f) != 1) {
    fprintf (stderr, "%s: cannot read program\n", filename);
    goto err;
  }
  memset (*buf + filesz, 0, memsz - filesz);
  *sz = memsz;

  fclose (f);

  return 0;

 err:
  fclose (f);
  return -1;
}

static int cmd_load (unsigned mem_addr, const char *filename)
{
  struct load_cb_data *data;
  unsigned char *buf;
  unsigned memsz;
  struct urv_cmd cmd;

  if (load_elf(filename, &buf, &memsz) < 0)
    return -1;

  data = xmalloc (sizeof (struct load_cb_data));
  data->mem_addr = mem_addr;
  data->buf = buf;
  data->off = 0;
  data->len = memsz;

  fprintf (out, "Load elf file %s at 0x%08x for 0x%08x bytes\n",
           filename, mem_addr, memsz);
  cmd.cb_data = data;
  if (load_cb_prepare(&cmd) == 0)
    return 0;
  if (append_command(ftb, &cmd) != 0)
    return -1;

  return 0;
}

struct i2c_cb_tfr {
  unsigned cmd;
  unsigned flags;
  unsigned err;
  unsigned len;
  unsigned off;
  unsigned char buf[32];
  int (*cb)(struct urv_cmd *, unsigned char *rep);
  void *cb_data;
};


static int cb_i2c_data(struct urv_cmd *cmd, unsigned char *rep)
{
  struct i2c_cb_tfr *dat = (struct i2c_cb_tfr *)cmd->cb_data;
  unsigned l, i;

  l = dat->len - dat->off;
  if (l > 4)
    l = 4;

  if (0)
    for (i = 0; i < l; i++)
      fprintf (out, " %02x", rep[4 + i]);

  for (i = 0; i < l; i++)
    dat->buf[dat->off + i] = rep[4 + i];

  dat->off += l;
  if (dat->off >= dat->len) {
    if (0)
      fprintf (out, "\n");
    if (dat->cb)
      return (*dat->cb)(cmd, dat->cb_data);
    else {
      free (dat);
      return 0;
    }
  }

  cmd->cmd = CMD_I2C_BUF;
  cmd->len = 4;
  cmd->args[0] = dat->off;
  cmd->cb = cb_i2c_data;
  return 1;
}

static int cb_i2c_status(struct urv_cmd *cmd, unsigned char *rep)
{
  struct i2c_cb_tfr *tfr = cmd->cb_data;

  if (0)
    fprintf (out, "stat: %02x, len: %02x coff: %02x boff: %02x\n",
            rep[4], rep[5], rep[6], rep[7]);

  if ((rep[4] & 0x0f) == 0) {
    tfr->len = rep[7];
    tfr->off = 0;
    tfr->err = rep[4];

    if (rep[4] == 0) {
      if (0)
        fprintf (out, "Done. len=%u\n", rep[7]);
    }
    else {
      /* Error.  Call the callback.  */
      if (tfr->cb)
        return (*tfr->cb)(cmd, tfr->cb_data);
      else {
        free (tfr);
        return 0;
      }
#if 0
      fprintf (out, "Error\n");
      tfr->len = 32;
#endif
    }

    /* Read data */
    cmd->cmd = CMD_I2C_BUF;
    cmd->len = 4;
    cmd->args[0] = 0;
    cmd->cb = cb_i2c_data;
    cmd->cb_data = tfr;
    return 1;
  }

  /* Still busy, ask again.  */
  cmd->cmd = CMD_I2C_STAT;
  cmd->len = 0;
  cmd->cb = cb_i2c_status;
  return 1;
}

static int cb_i2c_tfr(struct urv_cmd *cmd, unsigned char *rep)
{
  struct i2c_cb_tfr *tfr = (struct i2c_cb_tfr *)cmd->cb_data;

  if (rep[4] != 0) {
    fprintf (out, "i2c is busy???\n");
    free (tfr);
    return -1;
  }

  cmd->cmd = CMD_I2C_STAT;
  cmd->len = 0;
  cmd->cb = cb_i2c_status;
  return 1;
}

static void i2c_write_reg(struct urv_cmd *cmd, unsigned reg, unsigned val)
{
  cmd->cmd = CMD_WRITE_WORD;
  cmd->len = 8;
  word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_I2C + reg);
  word_pack(&cmd->args[4], val);
}

static void i2c_read_reg(struct urv_cmd *cmd, unsigned reg)
{
  cmd->cmd = CMD_READ_WORD;
  cmd->len = 4;
  word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_I2C + reg);
}

static void i2c_write_tx(struct urv_cmd *cmd, unsigned val)
{
  i2c_write_reg(cmd, 0x0c, val);
}

static void i2c_write_cr(struct urv_cmd *cmd, unsigned val)
{
  i2c_write_reg(cmd, 0x10, val);
}

static void i2c_read_sr(struct urv_cmd *cmd)
{
  i2c_read_reg(cmd, 0x10);
}

static void i2c_read_rx(struct urv_cmd *cmd)
{
  i2c_read_reg(cmd, 0x0c);
}

enum i2c_byte_state
  {
   I2C_B_INIT,
   I2C_B_BYTE,
   I2C_B_SEND,
   I2C_B_POLL
  };

struct i2c_cb_byte {
  enum i2c_byte_state state;
  unsigned char val;
  unsigned char cr;
  int (*cb)(struct urv_cmd *);
  void *cb_data;
};

static int cb_i2c_tx(struct urv_cmd *cmd, unsigned char *rep)
{
  struct i2c_cb_byte *s = (struct i2c_cb_byte *)cmd->cb_data;

  switch (s->state)
    {
    case I2C_B_INIT:
      cmd->cb = cb_i2c_tx;
      i2c_write_tx(cmd, s->val);
      s->state++;
      return 1;
    case I2C_B_BYTE:
      i2c_write_cr(cmd, s->cr);
      s->state++;
      return 1;
    case I2C_B_SEND:
      i2c_read_sr(cmd);
      s->state++;
      return 1;
    case I2C_B_POLL:
      if (rep[4] & 2) {
        /* Busy, try again  */
        i2c_read_sr(cmd);
        return 1;
      }
      if (s->cr == 0x90 && (rep[4] & 0x80) != 0) {
        fprintf (out, "i2c tx: no slave\n");
        return -1;
      }
      if ((s->cr == 0x50 && rep[4] != 0x01)
          || (s->cr != 0x50 && rep[4] != 0x41))
        fprintf (out, "i2c tx stat: %02x, cr=%02x\n", rep[4], s->cr);

      /* Done */
      cmd->cb_data = s->cb_data;
      return (s->cb)(cmd);
    default:
      abort();
    }
}

static int i2c_tx(struct urv_cmd *cmd, unsigned int val, unsigned char flags)
{
  struct i2c_cb_byte *s = (struct i2c_cb_byte *)cmd->cb_data;

  s->state = I2C_B_INIT;
  s->val = val;
  s->cr = 0x10 | flags;  // WRITE + flags
  cmd->cb_data = s;
  return cb_i2c_tx(cmd, NULL);
}

static int cb_i2c_rx(struct urv_cmd *cmd, unsigned char *rep)
{
  struct i2c_cb_byte *s = (struct i2c_cb_byte *)cmd->cb_data;

  switch (s->state)
    {
    case I2C_B_INIT:
      cmd->cb = cb_i2c_rx;
      i2c_write_cr(cmd, s->cr);
      s->state = I2C_B_SEND;
      return 1;
    case I2C_B_SEND:
      i2c_read_sr(cmd);
      s->state = I2C_B_POLL;
      return 1;
    case I2C_B_POLL:
      if (rep[4] & 2) {
        /* Busy, try again  */
        i2c_read_sr(cmd);
        return 1;
      }
      if ((s->cr == 0x28 && rep[4] != 0xc1)
          || (s->cr == 0x68 && rep[4] != 0x81)
          || (s->cr == 0x20 && rep[4] != 0x41))
        fprintf (out, "i2c rx stat: %02x, cr=%02x\n", rep[4], s->cr);
      /* Done */
      i2c_read_rx(cmd);
      s->state = I2C_B_BYTE;
      return 1;
    case I2C_B_BYTE:
      {
        unsigned val = word_unpack(&rep[4]);
        s->val = val;
        cmd->cb_data = s->cb_data;
        return (s->cb)(cmd);
      }
    default:
      abort();
    }
}

static int i2c_rx(struct urv_cmd *cmd, unsigned char flags)
{
  struct i2c_cb_byte *s = (struct i2c_cb_byte *)cmd->cb_data;

  s->state = I2C_B_INIT;
  s->cr = 0x20 | flags;  // READ + flags
  cmd->cb_data = s;
  return cb_i2c_rx(cmd, NULL);
}

enum i2c_msg_state
  {
   I2C_M_INIT,
   I2C_M_TARG,
   I2C_M_CMD,
   I2C_M_CMD2,
   I2C_M_TARG2,
   I2C_M_LEN,
   I2C_M_DATA,
   I2C_M_DONE
  };

struct i2c_cb_msg {
  enum i2c_msg_state state;
  struct i2c_cb_byte b;
  unsigned char targ;
  unsigned cmd;
  unsigned len;
  unsigned off;
  unsigned char buf[256];

  int (*cb)(void *data);
  void *cb_data;
};

static int cb_cmd_i2c_read(struct urv_cmd *cmd)
{
  struct i2c_cb_msg *rd = (struct i2c_cb_msg *)cmd->cb_data;

  cmd->cb_data = &rd->b;

  switch (rd->state)
    {
    case I2C_M_INIT:
      rd->b.cb = cb_cmd_i2c_read;
      rd->b.cb_data = rd;

      rd->state = I2C_M_TARG;
      return i2c_tx(cmd, rd->targ << 1, 0x80);  // START
    case I2C_M_TARG:
      rd->state = I2C_M_CMD;
      return i2c_tx(cmd, rd->cmd, 0);
    case I2C_M_CMD:
      rd->state = I2C_M_TARG2;
      return i2c_tx(cmd, (rd->targ << 1) | 1, 0x80);  // START
    case I2C_M_TARG2:
      rd->off = 0;
      if (rd->len == 0) {
        rd->state = I2C_M_LEN;
        return i2c_rx(cmd, 0);
      }
      else {
        rd->state = I2C_M_DATA;
        return i2c_rx(cmd, rd->len == 1 ? 0x08 : 0);
      }
    case I2C_M_LEN:
      rd->len = rd->b.val;
      if (0)
        fprintf (out, "i2c rx (len): %02x\n", rd->len);
      rd->state = I2C_M_DATA;
      return i2c_rx(cmd, rd->len == 1 ? 0x48 : 0);
    case I2C_M_DATA:
      {
        unsigned val = rd->b.val;
        if (0)
          fprintf (out, "i2c rx (dat): %02x %c [len=%u]\n",
                  val, val >= ' ' && val < 127 ? val : '.', rd->len);
        rd->buf[rd->off++] = val;
        if (rd->off == rd->len) {
          return (rd->cb)(rd->cb_data);
        }
        return i2c_rx(cmd, rd->off == rd->len - 1 ? 0x48 : 0);
      }
    default:
      fprintf(out, "i2c_read: unknown state %u\n", rd->state);
      return 0;
    }
}

static void
disp_mon_reply_value (unsigned cmd, unsigned len, unsigned char *buf)
{
  unsigned char rx_byte = 0x0;
  switch (cmd)
    {
    case 0x00:
      fprintf (out, "%02x\n", buf[0]);
      break;
    case 0x19:
      rx_byte = buf[0] & 0x80;
      fprintf (out, "%02x\n", rx_byte);
      if (rx_byte == 0x80) {
        fprintf (out, "Packet Error Checking is supported");
      } else {
        fprintf (out, "Packet Error Checking is not supported");
      }
      break;
    case 0x78:
      rx_byte = buf[0] & 0x02;
      fprintf (out, "%02x\n", rx_byte);
      if (rx_byte == 0x02) {
        fprintf (out, "Status Byte reports a communication, memory or logic fault has occurred");
      }
      break;
    case 0x7e:
      rx_byte = buf[0] & 0x32;
      fprintf (out, "%02x\n", rx_byte);
      if (rx_byte == 0x20) {
        fprintf (out, "Status CML --- Packet Error Check Failed");
      }
      if (rx_byte == 0x10) {
        fprintf (out, "Status CML --- TMR error");
      }
      if (rx_byte == 0x02) {
        fprintf (out, "Status CML --- Communication fault");
      }
      break;
    case 0x3a:
    case 0x3d:
      fprintf (out, "%02x", buf[0]);
      break;
    case 0x3b:
    case 0x3c:
    case 0x3e:
      fprintf (out, "Written Speed %f\n", linear_to_float(buf));
      for (unsigned i = 0; i < len; i++) {
        fprintf (out, " %02x", buf[i]);
      }
      break;
    case 0x8b:
      fprintf (out, "%2.3f V", linear16_to_float(buf));
      break;
    case 0x8c:
      fprintf (out, "%4.3f A", linear_to_float(buf));
      break;
    case 0x8d:
    case 0x8e:
    case 0x8f:
      fprintf (out, "%2.3f C",  linear_to_float(buf));
      break;
    case 0x90:
    case 0x91:
    case 0x92:
      for (unsigned i = 0; i < len; i++) {
        fprintf (out, " %02x", buf[i]);
      }
      fprintf (out, "\n");
      if (len == 2) {
        fprintf (out, "Fan %u: %f\n", cmd-0x8F, linear_to_float(buf));
      };
      break;
    case 0x99:
    case 0x9a:
    case 0x9b:
    case 0x9c:
    case 0x9d:
    case 0x9e:
    case 0xae:
      buf[len + 1] = 0;
      fprintf (out, "%s", buf + 1);
      break;
    case 0xff10:
      {
        unsigned v = buf[0] | (buf[1] << 8) | (buf[2] << 16) | (buf[3] << 24);
        unsigned hr, min, sec, day;
        min = v / 60;
        sec = v - min * 60;
        hr = min / 60;
        min = min - hr * 60;
        day = hr / 24;
        hr = hr - day * 24;
        fprintf (out, "%ud %u:%02u:%02u", day, hr, min, sec);
      }
      break;
    default:
      for (unsigned i = 0; i < len; i++)
        fprintf (out, " %02x", buf[i]);
    }
  if (flag_pipe)
    fprintf(out, "\n");
}

static void disp_mon_reply (unsigned cmd, unsigned len, unsigned char *buf)
{
  switch (cmd)
    {
    case 0x00:
      fprintf (out, "PAGE");
      break;
    case 0x19:
      fprintf (out, "CAPABILITY");
      break;
    case 0x78:
      fprintf (out, "STATUS_BYTE");
      break;
    case 0x7e:
      fprintf (out, "STATUS_CML");
      break;
    case 0x3a:
      fprintf (out, "FAN_CONFIG 1-2");
      break;
    case 0x3b:
      fprintf (out, "FAN_COMMAND 1");
      break;
    case 0x3c:
      fprintf (out, "FAN_COMMAND 2");
      break;
    case 0x3d:
      fprintf (out, "FAN_CONFIG 3-4");
      break;
    case 0x3e:
      fprintf (out, "FAN_COMMAND 3");
      break;
    case 0x8b:
      fprintf (out, "VOUT");
      break;
    case 0x8c:
      fprintf (out, "IOUT");
      break;
    case 0x8d:
    case 0x8e:
    case 0x8f:
      fprintf (out, "TEMP-%u", cmd - 0x8c);
      break;
    case 0x90:
    case 0x91:
    case 0x92:
      fprintf (out, "FAN %u SPEED is ", (cmd - 0x90)+1);
      break;
    case 0x99:
      fprintf (out, "MFR-ID");
      break;
    case 0x9a:
      fprintf (out, "MFR-MODEL");
      break;
    case 0x9b:
      fprintf (out, "MFR-REVISION");
      break;
    case 0x9c:
      fprintf (out, "MFR-LOCATION");
      break;
    case 0x9d:
      fprintf (out, "MFR-DATE");
      break;
    case 0x9e:
      fprintf (out, "MFR-SERIAL");
      break;
    case 0xae:
      fprintf (out, "IC-DEVICE-REV");
      break;
    case 0xff10:
      fprintf (out, "UPTIME");
      break;
    default:
      fprintf (out, "??? cmd %02x", cmd);
    }
  fprintf (out, " [l=%u", len);

  for (unsigned i = 0; i < len; i++)
    fprintf (out, " %02x", buf[i]);
  fprintf (out, "]: ");

  disp_mon_reply_value(cmd, len, buf);
  fputc('\n', out);
}

static void disp_ratopus_reply (unsigned cmd, unsigned len, unsigned char *buf) {

    unsigned data = 0;

    switch (cmd) {
        case 0x00:
            if (len == 2) {
                fprintf (out, "command register value: 0x%04x\n", ((buf[0] << 8) | buf[1]) & 0xFFFF);
            } else {
                fprintf (out, "Error reading command register");
            }
            break;
        case 0x01:
            if (len == 8) {
                fprintf (out, " [L=%u", len);
                for (unsigned i = 0; i < len; i++)
                    fprintf (out, " %02x", buf[i]);
                fprintf (out, "]\n");
                for (unsigned i = 0; i < (8/2); i ++) {
                    data = ( ((((unsigned) (buf[2*i])) << 8) & 0x0F00) | ((unsigned) (buf[(2*i)+1])) );
                    fprintf (out, "Channel %u -- Value: %u\n", (buf[2*i] >> 4) & 0x0F, data );
                }
            } else {
                fprintf (out, "Error reading voltage conversion values");
            }
            break;
        case 0x02:
            if (len == 2) {
               fprintf (out, " [L=%u", len);
                for (unsigned i = 0; i < len; i++)
                    fprintf (out, " %02x", buf[i]);
                fprintf (out, "]\n");
                data = ( ((((unsigned) (buf[0])) << 8) & 0x0700) | ((unsigned) (buf[1])) ) - (C_ADC7291_SIGNED_VAL * ((buf[0] >> 3) & 0x01));
                fprintf (out, "Channel %u -- Temperature: %.2f\n", (buf[0] >> 4) & 0x0F, ((float) data) / (float) (1 << C_ADC7291_FRACTIONAL_BITS) );
            } else {
                fprintf (out, "Error reading temperature register");
            }
            break;
        default:
            fprintf (out, "Unknown CMD %02x", cmd);
            fprintf (out, " [L=%u", len);
            for (unsigned i = 0; i < len; i++)
                fprintf (out, " %02x", buf[i]);
            fprintf (out, "]\n");
    }
}

static int cb_mon_read (struct urv_cmd *cmd, unsigned char *rep)
{
  struct i2c_cb_tfr *tfr = cmd->cb_data;

  unsigned icmd = tfr->cmd & 0xffff;

  if (tfr->flags & 1)
    disp_mon_reply_value(icmd, tfr->len, tfr->buf);
  else
    disp_mon_reply(icmd, tfr->len, tfr->buf);

  free(tfr);
  return 0;
}

static int cb_ratopus_read (struct urv_cmd *cmd, unsigned char *rep) {
    struct i2c_cb_tfr *tfr = cmd->cb_data;

    unsigned icmd = tfr->cmd & 0xffff;
    disp_ratopus_reply(icmd, tfr->len, tfr->buf);

    free(tfr);
    return 0;
}

static int cb_i2c_id (void *data)
{
  struct i2c_cb_msg *s = (struct i2c_cb_msg *)data;

  fprintf (out, "Id: %02x\n", s->buf[0]);
  return 0;
}

static int cb_i2c_id2 (struct urv_cmd *cmd, unsigned char *rep)
{
  struct i2c_cb_tfr *tfr = cmd->cb_data;

  if (tfr->flags) {
    if (tfr->err)
      fprintf (out, "NA");
    else
      fprintf (out, "%02x", tfr->buf[0]);
    if (flag_pipe)
      fprintf (out, "\n");
  }
  else {
    if (tfr->err)
      fprintf (out, "Error (%02x)\n", tfr->err);
    else
      fprintf (out, "Id: %02x\n", tfr->buf[0]);
  }
  return 0;
}

static int cmd_i2c_read (struct i2c_cb_msg *rd)
{
  struct urv_cmd cmd;

  rd->state = I2C_M_INIT;
  cmd.cb_data = rd;
  cb_cmd_i2c_read (&cmd);
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

/*
static int cb_cmd_i2c_write(struct urv_cmd *cmd)
{
  struct i2c_cb_msg *m = (struct i2c_cb_msg *)cmd->cb_data;

  cmd->cb_data = &m->b;

  switch (m->state)
    {
    case I2C_M_INIT:
      m->b.cb = cb_cmd_i2c_write;
      m->b.cb_data = m;

      if (m->cmd & 0xff00)
        m->state = I2C_M_CMD2;
      else
        m->state = I2C_M_CMD;
      return i2c_tx(cmd, m->targ << 1, 0x80);  // START
    case I2C_M_CMD2:
      m->state = I2C_M_CMD;
      return i2c_tx(cmd, m->cmd >> 8, 0);
    case I2C_M_CMD:
      m->state = I2C_M_DATA;
      m->off = 0;
      return i2c_tx(cmd, m->cmd & 0xff, 0);
    case I2C_M_DATA:
      {
        unsigned b = m->buf[m->off++];
        if (m->off >= m->len)
          m->state = I2C_M_DONE;
        return i2c_tx(cmd, b, m->off >= m->len ? 0x40 : 0); // STOP
      }
    case I2C_M_DONE:
      return 0;
    default:
      fprintf (out, "i2c_write: unknown state %u\n", m->state);
      return 0;
    }
}

static int cmd_i2c_write (struct i2c_cb_msg *m)
{
  struct urv_cmd cmd;

  m->state = I2C_M_INIT;
  cmd.cb_data = m;
  cb_cmd_i2c_write (&cmd);
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}*/

static int cmd_reset (unsigned plc_addr, unsigned val)
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_WRITE_WORD;
  cmd.len = 8;
  word_pack(&cmd.args[0], plc_addr + FIP_URV_REGS_APP_CR);
  word_pack(&cmd.args[4], val);
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

static int cmd_check_off (unsigned addr)
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], addr + FIP_URV_REGS_APP_CR);
  cmd.cb = cb_check_off;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

static int cmd_spi_init (void)
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_WRITE_WORD;
  cmd.len = 8;
  word_pack(&cmd.args[0], IO_ADDR + FIP_URV_REGS_SPI + 0x14);
  word_pack(&cmd.args[4], 1);
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

struct spi_cb_tfr {
  enum { SPI_CTRL, SPI_WRITE, SPI_SS1, SPI_GO, SPI_READ, SPI_DONE} state;
  unsigned l;
  unsigned len;
  unsigned char tx[32];
  unsigned char rx[32];
  /* User data */
  unsigned addr;
  unsigned arg;
  unsigned alen;
  unsigned aoff;
  unsigned char *abuf;

  unsigned release_ss;  /* Release SS at the end of the transfer.  */
  int (*cb)(struct urv_cmd *, struct spi_cb_tfr *tfr);
  void *cb_data;
};

static int cb_spi_cmd (struct urv_cmd *cmd, unsigned char *rep)
{
  struct spi_cb_tfr *tfr = cmd->cb_data;

  switch (tfr->state) {
  case SPI_CTRL:
    cmd->cmd = CMD_WRITE_WORD;
    cmd->len = 8;
    word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_SPI + 0x10);
    word_pack(&cmd->args[4], 0x400 + tfr->len*8);
    tfr->state = SPI_WRITE;
    tfr->l = 0;
    return 1;
  case SPI_WRITE:
    /* Write data */
    {
      unsigned ra = (tfr->len - 1 - tfr->l) & ~3;
      unsigned w = 0;

      cmd->cmd = CMD_WRITE_WORD;
      cmd->len = 8;
      word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_SPI + ra);
      for (unsigned i = 0; i < 4 && tfr->l < tfr->len; i++) {
        unsigned off = (tfr->len - 1 - i) & 3;
        w |= tfr->tx[tfr->l++] << (off * 8);
      }
      word_pack(&cmd->args[4], w);
      if (tfr->l == tfr->len)
        tfr->state = SPI_SS1;
      return 1;
    }
  case SPI_SS1:
    word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_SPI + 0x18);
    word_pack(&cmd->args[4], 1);
    tfr->state = SPI_GO;
    return 1;
  case SPI_GO:
    cmd->cmd = CMD_WRITE_WORD;
    cmd->len = 8;
    word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_SPI + 0x10);
    word_pack(&cmd->args[4], 0x500 + tfr->len*8);
    tfr->state = SPI_READ;
    tfr->l = 0;
    return 1;
  case SPI_READ:
    if (cmd->cmd == CMD_READ_WORD) {
      unsigned full_l = tfr->len - tfr->l;
      unsigned l = full_l > 4 ? 4 : full_l;
      unsigned w = word_unpack(&rep[4]);
      for (unsigned i = 0; i < l; i++) {
        tfr->rx[tfr->len - 1 - tfr->l++] = w & 0xff;
        w >>= 8;
      }
    }
    else {
      cmd->cmd = CMD_READ_WORD;
      cmd->len = 4;
    }
    if (tfr->l < tfr->len)
      word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_SPI + tfr->l);
    else {
      if (tfr->release_ss) {
        //  SS=0
        cmd->cmd = CMD_WRITE_WORD;
        cmd->len = 8;
        word_pack(&cmd->args[0], IO_ADDR + FIP_URV_REGS_SPI + 0x18);
        word_pack(&cmd->args[4], 0);
        tfr->state = SPI_DONE;
      }
      else
        return tfr->cb(cmd, tfr);
    }
    return 1;
  case SPI_DONE:
    return tfr->cb(cmd, tfr);
  default:
    abort();
  }
}

static int cb_spi_read(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  unsigned i;
  fprintf (out, "%02x%02x%02x: ", tfr->tx[1], tfr->tx[2], tfr->tx[3]);
  for (i = 4; i < tfr->len; i++)
    fprintf (out, " %02x", tfr->rx[i]);
  fprintf (out, "\n");

  free (tfr);
  return 0;
}

static int start_spi_read(struct urv_cmd *cmd, struct spi_cb_tfr *tfr,
                          unsigned addr, unsigned len)
{
  unsigned i;

  cmd->cb = cb_spi_cmd;
  cmd->cb_data = tfr;

  tfr->tx[0] = 0x03;
  tfr->tx[1] = (addr >> 16) & 0xff;
  tfr->tx[2] = (addr >> 8) & 0xff;
  tfr->tx[3] = (addr >> 0) & 0xff;
  for (i = 0; i < len; i++)
    tfr->tx[4 + i] = 0;
  tfr->len = len + 4;
  tfr->state = SPI_CTRL;

  return cb_spi_cmd(cmd, NULL);
}

static int cmd_spi_read(unsigned addr, unsigned len)
{
  struct urv_cmd cmd;
  struct spi_cb_tfr *tfr;

  tfr = xmalloc(sizeof(struct spi_cb_tfr));
  tfr->cb = cb_spi_read;
  tfr->release_ss = 1;

  start_spi_read(&cmd, tfr, addr, len);

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

static int cb_spi_dump(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  unsigned i;

  if ((tfr->addr & 0x0f) == 0)
    fprintf (out, "%06x: ", tfr->addr);
  for (i = tfr->len - 8; i < tfr->len; i++)
    fprintf (out, " %02x", tfr->rx[i]);
  if ((tfr->addr & 0x0f) != 0)
    fprintf (out, "\n");

  if (tfr->release_ss) {
    if ((tfr->addr & 0x0f) == 0)
      fprintf (out, "\n");
    free (tfr);
    return 0;
  }
  tfr->arg -= 8;
  tfr->addr += 8;

  tfr->state = SPI_GO;
  tfr->len = 8;
  tfr->l = 0;
  tfr->release_ss = tfr->arg <= 8;

  return cb_spi_cmd (cmd, NULL);
}

static int cmd_spi_dump(unsigned addr, unsigned len)
{
  struct urv_cmd cmd;
  struct spi_cb_tfr *tfr;

  tfr = xmalloc(sizeof(struct spi_cb_tfr));
  tfr->cb = cb_spi_dump;
  tfr->addr = addr;
  tfr->arg = len;
  tfr->release_ss = len <= 8;

  start_spi_read(&cmd, tfr, addr, 8);

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

static int cb_spi_prog_page_erase(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  /* Write a block: start to send the command  */
  fprintf (out, "spi: erase page at %06x\n", tfr->addr);

  tfr->tx[0] = 0x81;
  tfr->tx[1] = tfr->addr >> 16;
  tfr->tx[2] = tfr->addr >> 8;
  tfr->tx[3] = tfr->addr >> 0;
  tfr->len = 4;
  tfr->state = SPI_CTRL;
  tfr->l = 0;
  tfr->release_ss = 1;
  return cb_spi_cmd (cmd, NULL);
}

static int cb_spi_prog_start(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  /* Write a block: start to send the command  */
  fprintf (out, "spi: write at %06x\n", tfr->addr);

  tfr->tx[0] = 0x02;
  tfr->tx[1] = tfr->addr >> 16;
  tfr->tx[2] = tfr->addr >> 8;
  tfr->tx[3] = tfr->addr >> 0;
  tfr->len = 4;
  tfr->state = SPI_CTRL;
  tfr->release_ss = 0;
  return cb_spi_cmd (cmd, NULL);
}

static int cb_spi_prog_data(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  /* Send data.  */
  unsigned l = tfr->alen;
  unsigned i;
  /* Complete the page, but no wrapping */
  unsigned plen = 0x100 - (tfr->addr & 0xff);
  if (l > plen)
    l = plen;
  if (l > 8)
    l = 8;
  for (i = 0; i < l; i++)
    tfr->tx[i] = tfr->abuf[tfr->aoff + i];
  tfr->len = l;
  tfr->state = SPI_CTRL;

  fprintf (out, "spi: program %u bytes (at %06x, alen=%u)\n",
           l, tfr->addr, tfr->alen);

  tfr->addr += l;
  tfr->alen -= l;
  tfr->aoff += l;
  if ((tfr->addr & 0xff) == 0 || tfr->alen == 0) {
    tfr->release_ss = 1;
    tfr->arg++;
  }
  return cb_spi_cmd (cmd, NULL);
}

static int cb_spi_prog_enable(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  fprintf (out, "spi: enable write\n");

  /* Write enable.  */
  tfr->tx[0] = 0x06;
  tfr->len = 1;
  tfr->state = SPI_CTRL;
  tfr->release_ss = 1;

  return cb_spi_cmd(cmd, NULL);
}

static int cb_spi_prog_disable(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  fprintf (out, "spi: disable write\n");

  /* Write disable.  */
  tfr->tx[0] = 0x04;
  tfr->len = 1;
  tfr->release_ss = 1;
  tfr->state = SPI_CTRL;

  return cb_spi_cmd(cmd, NULL);
}

static int cb_spi_prog_wait(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  fprintf (out, "spi: read status\n");

  /* Send read status.  */
  tfr->tx[0] = 0x05;
  tfr->tx[1] = 0;
  tfr->len = 2;
  tfr->release_ss = 1;
  tfr->state = SPI_CTRL;

  return cb_spi_cmd(cmd, NULL);
}

static int cb_spi_prog(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  if (tfr->arg == 0) {
    tfr->arg = 1;
    return cb_spi_prog_page_erase(cmd, tfr);
  }
  else if (tfr->arg == 1) {
    tfr->arg = 2;
    return cb_spi_prog_wait(cmd, tfr);
  }
  else if (tfr->arg == 2) {
    fprintf (out, "spi status: %02x\n", tfr->rx[1]);
    if (tfr->rx[1] & 1) {
      /* Still busy, try again */
      return cb_spi_prog_wait(cmd, tfr);
    }
    else {
      tfr->arg = 3;
      return cb_spi_prog_enable(cmd, tfr);
    }
  }
  else if (tfr->arg == 3) {
    tfr->arg = 4;
    return cb_spi_prog_start(cmd, tfr);
  }
  else if (tfr->arg == 4)
    return cb_spi_prog_data(cmd, tfr);
  else if (tfr->arg == 5) {
    tfr->arg = 6;
    return cb_spi_prog_wait(cmd, tfr);
  }
  else if (tfr->arg == 6) {
    fprintf (out, "spi status: %02x\n", tfr->rx[1]);
    if (tfr->rx[1] & 1) {
      /* Still busy, try again */
      return cb_spi_prog_wait(cmd, tfr);
    }
    else
      if (tfr->alen == 0) {
        /* Done */
        free (tfr->abuf);
        return 0;
      }
      else {
        tfr->arg = 0;
        return cb_spi_prog_enable(cmd, tfr);
      }
  }
  else
    abort ();
}

static int cmd_spi_prog(unsigned addr, unsigned char *buf, unsigned len)
{
  struct urv_cmd cmd;
  struct spi_cb_tfr *tfr;

  tfr = xmalloc(sizeof(struct spi_cb_tfr));
  tfr->cb = cb_spi_prog;
  tfr->addr = addr;
  tfr->alen = len;
  tfr->aoff = 0;
  tfr->abuf = buf;
  tfr->arg = 0;
  tfr->cb_data = NULL;
  tfr->release_ss = 1;

  cmd.cb = cb_spi_cmd;
  cmd.cb_data = tfr;

  cb_spi_prog_enable(&cmd, tfr);

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

static int cb_spi_page_erase(struct urv_cmd *cmd, struct spi_cb_tfr *tfr)
{
  if (tfr->arg == 0) {
    tfr->arg = 1;
    return cb_spi_prog_page_erase(cmd, tfr);
  }
  else if (tfr->arg == 1) {
    tfr->arg = 2;
    return cb_spi_prog_wait(cmd, tfr);
  }
  else if (tfr->arg == 2) {
    fprintf (out, "spi status: %02x\n", tfr->rx[1]);
    if (tfr->rx[1] & 1) {
      /* Still busy, try again */
      return cb_spi_prog_wait(cmd, tfr);
    }
    else {
      /* Done */
      tfr->arg = 3;
      return cb_spi_prog_disable(cmd, tfr);
    }
  }
  else if (tfr->arg == 3) {
    /* Done */
    return 0;
  }
  else
    abort ();
}

static int cmd_spi_page_erase(unsigned pg)
{
  struct urv_cmd cmd;
  struct spi_cb_tfr *tfr;

  tfr = xmalloc(sizeof(struct spi_cb_tfr));
  tfr->cb = cb_spi_page_erase;
  tfr->addr = pg << 8;
  tfr->arg = 0;
  tfr->cb_data = NULL;
  tfr->release_ss = 1;

  cmd.cb = cb_spi_cmd;
  cmd.cb_data = tfr;

  cb_spi_prog_enable(&cmd, tfr);

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

struct envm_cb_tfr {
  enum {
        /* Request exclusive access. */
        ENVM_EXCL,
        /* Fill the WDbuff. */
        ENVM_FILL,
        /* Program the page. */
        ENVM_PROG,
        /* Wait until eNVM is ready. */
        ENVM_PWAIT,
        ENVM_PWAIT2,
        /* Wait until verification. */
        ENVM_VWAIT,
        ENVM_VWAIT2,
        /* Release exclusive access. */
        ENVM_RELEASE
  } state;
  /* User data */
  unsigned char *abuf;	/* Data to be written */
  unsigned alen;	/* Data length (in words).  */
  unsigned addr;	/* eNVM address */
  unsigned aoff;	/* Data and address offset. */

  unsigned verbose;

  int (*cb)(struct urv_cmd *, struct envm_cb_tfr *tfr);
  void *cb_data;
};

static int cb_envm_cmd (struct urv_cmd *cmd, unsigned char *rep)
{
  struct envm_cb_tfr *tfr = cmd->cb_data;

 again:
  switch (tfr->state) {
  case ENVM_EXCL:
    if (tfr->verbose > 0)
      fprintf (out, "envm: request access\n");
    cmd_write_word(cmd, IO_ADDR + FIP_URV_REGS_ENVM + ENVM_REQACCESS, 1);
    tfr->state = ENVM_FILL;
    return 1;
  case ENVM_FILL:
    /* Fill (32 words).  */
    {
      unsigned off = tfr->aoff & 0x7f;
      unsigned len = 128 - off;
      if (len > PROD_VAR_LENGTH - 8)
        len = PROD_VAR_LENGTH - 8;
      if (tfr->verbose > 1)
        fprintf (out, "envm: fill %u bytes at off %u\n", len, off);
      if (tfr->verbose > 2) {
        unsigned j;
        for (j = 0; j < len; j++)
          fprintf (out, " %02x", tfr->abuf[tfr->aoff + j]);
        fprintf(out,"\n");
      }
      cmd_write_bytes(cmd,
                      IO_ADDR + FIP_URV_REGS_ENVM + ENVM_WDBUFF + off,
                      tfr->abuf + tfr->aoff, len);
      tfr->aoff += len;
      if (off + len == 128)
        tfr->state = ENVM_PROG;
    }
    return 1;
  case ENVM_PROG:
    {
      unsigned val = 0x08000000 | tfr->addr | (tfr->aoff - 0x80);
      if (tfr->verbose > 0)
        fprintf (out, "envm: issue ProgramADS 0x%08x\n", val);
      cmd_write_word(cmd,
                     IO_ADDR + FIP_URV_REGS_ENVM + ENVM_CMD, val);
      tfr->state = ENVM_PWAIT;
      return 1;
    }
  case ENVM_PWAIT2:
    {
      unsigned val = word_unpack(&rep[4]);
      if (tfr->verbose > 2)
        fprintf (out, "envm: status=%08x\n", val);
      if (val & 1) {
        /* Done, send verify. */
        unsigned val = 0x10000000 | tfr->addr | (tfr->aoff - 0x80);
        if (tfr->verbose > 0)
          fprintf (out, "envm: issue VerifyADS 0x%08x\n", val);
        cmd_write_word(cmd,
                       IO_ADDR + FIP_URV_REGS_ENVM + ENVM_CMD, val);
        tfr->state = ENVM_VWAIT;
        return 1;
      }
      /* Fallthrough */
    }
  case ENVM_PWAIT:
    if (tfr->verbose > 1)
      fprintf (out, "envm: wait ready\n");
    cmd_read_word(cmd, IO_ADDR + FIP_URV_REGS_ENVM + ENVM_STATUS);
    tfr->state = ENVM_PWAIT2;
    return 1;
  case ENVM_VWAIT2:
    {
      unsigned val = word_unpack(&rep[4]);
      if (tfr->verbose > 2)
        fprintf (out, "envm: status=%08x\n", val);
      if (val & 1) {
        /* Done.  */
        if (tfr->aoff < tfr->alen) {
          tfr->state = ENVM_FILL;
          goto again;
        }
        else {
          if (tfr->verbose > 0)
            fprintf (out, "envm: release access\n");
          cmd_write_word(cmd, IO_ADDR + FIP_URV_REGS_ENVM + ENVM_REQACCESS, 0);
          tfr->state = ENVM_RELEASE;
        }
        return 1;
      }
      else {
        tfr->state = ENVM_VWAIT;
        goto again;
      }
    }
  case ENVM_VWAIT:
    if (tfr->verbose > 1)
      fprintf (out, "envm: wait ready\n");
    cmd_read_word(cmd, IO_ADDR + FIP_URV_REGS_ENVM + ENVM_STATUS);
    tfr->state = ENVM_VWAIT2;
    return 1;
  case ENVM_RELEASE:
    if (tfr->cb)
      return tfr->cb(cmd, tfr);
    else {
      free(tfr);
      return 0;
    }
  default:
    abort();
  }
}

static int cmd_envm_prog(unsigned addr, unsigned char *buf, unsigned len,
                         unsigned verbose)
{
  struct urv_cmd cmd;
  struct envm_cb_tfr *tfr;

  tfr = xmalloc(sizeof(struct envm_cb_tfr));
  tfr->state = ENVM_EXCL;
  tfr->addr = addr;
  tfr->alen = len;
  tfr->aoff = 0;
  tfr->abuf = buf;
  tfr->verbose = verbose;
  tfr->cb = NULL;
  tfr->cb_data = NULL;

  cmd.cb = cb_envm_cmd;
  cmd.cb_data = tfr;

  cb_envm_cmd(&cmd, NULL);

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 0;
}

static int do_set_leds (int argc, char *argv[])
{
  unsigned long val;
  char *e;
  struct urv_cmd cmd;

  if (argc < 2) {
    fprintf (out, "set-leds needs a value\n");
    return -1;
  }
  val = strtoul (argv[1], &e, 0);
  if (*e != 0 || val > 255) {
    fprintf (out, "incorrect value for set-leds\n");
    return -1;
  }

  cmd.cmd = CMD_SET_LEDS;
  cmd.len = 1;
  cmd.args[0] = val;
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 2;
}

static int do_get_leds (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_GET_LEDS;
  cmd.len = 0;
  cmd.cb = cb_get_leds;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_read (int argc, char *argv[])
{
  unsigned long val;
  char *e;
  struct urv_cmd cmd;

  if (argc < 2) {
    fprintf (out, "read needs an address\n");
    return -1;
  }
  val = strtoul (argv[1], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect address for read\n");
    return -1;
  }

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], val);
  cmd.cb = cb_read_word;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 2;
}

static int do_dump (int argc, char *argv[])
{
  unsigned orig_argc = argc;
  unsigned addr;
  unsigned len;
  char *e;
  struct urv_cmd cmd;
  struct dump_cmd *dmp;

  if (argc < 2 || argc > 3) {
    fprintf (out, "usage: dump ADDR [LEN]\n");
    fprintf (out, " ADDR can be an hex value, or 'shmem'\n");
    return -1;
  }
  argv++;
  argc--;

  if (!strcmp(argv[0], "shmem"))
    addr = IO_ADDR + FIP_URV_REGS_SH_MEM;
  else {
    addr = strtoul (argv[0], &e, 0);
    if (*e != 0) {
      fprintf (out, "incorrect address for dump\n");
      return -1;
    }
  }
  argv++;
  argc--;

  if (argc > 0) {
    len = strtoul (argv[0], &e, 0);
    if (*e != 0) {
      fprintf (out, "incorrect length for dump\n");
      return -1;
    }
    argv++;
    argc--;
  }
  else
    len = 16;

  dmp = xmalloc(sizeof (struct dump_cmd));
  dmp->addr = addr & ~0x0f;
  dmp->len = (len + 3) & ~3;

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], addr);
  cmd.cb = cb_dump;
  cmd.cb_data = dmp;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return orig_argc - argc;
}

static int do_write (int argc, char *argv[])
{
  unsigned long val;
  unsigned long addr;
  char *e;
  struct urv_cmd cmd;

  if (argc < 3) {
    fprintf (out, "write needs an address and a value\n");
    return -1;
  }
  addr = strtoul (argv[1], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect address for write\n");
    return -1;
  }
  val = strtoul (argv[2], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect value for write\n");
    return -1;
  }

  cmd.cmd = CMD_WRITE_WORD;
  cmd.len = 8;
  word_pack(&cmd.args[0], addr);
  word_pack(&cmd.args[4], val);
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 3;
}

static int do_write_ecc (int argc, char *argv[])
{
  unsigned long val;
  unsigned long addr;
  unsigned long ecc;
  char *e;
  struct urv_cmd cmd;

  if (argc < 4) {
    fprintf (out, "usage: write-ecc ADDR VAL ECC-MASK\n");
    return -1;
  }
  addr = strtoul (argv[1], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect address for write\n");
    return -1;
  }
  val = strtoul (argv[2], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect value for write\n");
    return -1;
  }
  ecc = strtoul (argv[3], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect ecc for write\n");
    return -1;
  }

  cmd.cmd = CMD_WRITE_ECC;
  cmd.len = 9;
  word_pack(&cmd.args[0], addr);
  word_pack(&cmd.args[4], val);
  cmd.args[8] = ecc;
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 4;
}

static int do_set_wd (int argc, char *argv[])
{
  unsigned long val;
  char *e;
  struct urv_cmd cmd;

  if (argc < 2) {
    fprintf (out, "usage: set-wd PERIOD\n");
    return -1;
  }
  val = strtoul (argv[1], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect period\n");
    return -1;
  }

  cmd.cmd = CMD_WRITE_WORD;
  cmd.len = 8;
  word_pack(&cmd.args[0],
            SUPERVISOR_ADDR + HYDRA_SUPERVISOR_REGS_WD_PERIOD);
  word_pack(&cmd.args[4], val);
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 2;
}

static int do_ping (int argc, char *argv[])
{
  unsigned i;
  struct urv_cmd cmd;

  cmd.cmd = CMD_FIP_PING;
  cmd.len = PROD_VAR_LENGTH - 4;
  for (i = 0; i < cmd.len; i++)
    cmd.args[i] = rand();
  cmd.cb = cb_ping;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_data (int argc, char *argv[])
{
  unsigned i;
  struct urv_cmd cmd;

  cmd.cmd = CMD_APP_FLAG;
  cmd.len = PROD_VAR_LENGTH;

  cmd.args[0] = CMD_APP_FLAG;
  for (i = 1; i < cmd.len; i++)
    cmd.args[i] = rand();
  cmd.cb = NULL;

  fprintf (out, "Data to app:");
  for (i = 0; i < cmd.len; i++)
    fprintf(out, " %02x", cmd.args[i]);
  fprintf(out, "\n");

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static unsigned plc_addr(const char *arg)
{
  unsigned addr = IO_ADDR + FIP_URV_REGS_APP;

  if (strcmp(arg, "0") == 0)
    return addr;
  else if (strcmp(arg, "1") == 0)
    return addr + FIP_URV_REGS_APP_SIZE;
  else {
    fprintf (out, "bad plc id\n");
    return 0;
  }
}

static int do_plc (int argc, char *argv[])
{
  struct urv_cmd cmd;
  unsigned addr;

  if (argc > 1) {
    addr = plc_addr(argv[1]);
    if (addr == 0)
      return -1;
  }
  else
    addr = 0;

  if (addr != 0) {
    if (argc == 2) {
      cmd.cmd = CMD_READ_WORD;
      cmd.len = 4;
      word_pack(&cmd.args[0], addr + FIP_URV_REGS_APP_CR);
      cmd.cb = cb_plc_status;

      if (append_command(ftb, &cmd) != 0)
        return -1;
      return 2;
    }
    if (argc == 3) {
      if (strcmp (argv[2], "reset") == 0)
        return cmd_reset (addr, 3) < 0 ? -1 : 3;
      if (strcmp (argv[2], "halt") == 0)
        return cmd_reset (addr, 2) < 0 ? -1 : 3;
      if (strcmp (argv[2], "run") == 0)
        return cmd_reset (addr, 0) < 0 ? -1 : 3;
    }
  }
  fprintf (out, "usage: plc 0|1 [reset|halt|run]\n");
  return -1;
}

static int do_plc_supervisor (int argc, char *argv[])
{
  struct urv_cmd cmd;
  unsigned addr;

  if (argc != 2) {
    fprintf (out, "usage: %s 0|1\n", argv[0]);
    return -1;
  }

  addr = plc_addr(argv[1]);
  if (addr == 0)
    return -1;

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], addr + FIP_URV_REGS_APP_RESET_CAUSE);
  cmd.cb = cb_plc_supervisor;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 2;
}

static int do_load (int argc, char *argv[])
{
  unsigned addr;
  unsigned mem_addr;

  if (argc < 3) {
    fprintf (out, "usage: %s 0|1 ELF-FILE\n", argv[0]);
    return -1;
  }

  addr = plc_addr(argv[1]);
  if (addr == 0)
    return -1;

  if (addr == IO_ADDR + FIP_URV_REGS_APP)
    mem_addr = IO_ADDR + FIP_URV_REGS_APP0_MEM;
  else if (addr == IO_ADDR + FIP_URV_REGS_APP + FIP_URV_REGS_APP_SIZE)
    mem_addr = IO_ADDR + FIP_URV_REGS_APP1_MEM;
  else
    abort();

  if (cmd_check_off(addr) < 0)
    return -1;
  if (cmd_load(mem_addr, argv[2]) < 0)
    return -1;
  if (cmd_reset(addr, 0) != 0)
    return -1;

  return 3;
}

static int do_mbox (int argc, char *argv[])
{
  unsigned addr;
  struct urv_cmd cmd;

  if (argc < 2 || argc > 3) {
    fprintf (out, "usage: %s 0|1 [VALUE]\n", argv[0]);
    return -1;
  }

  addr = plc_addr(argv[1]);
  if (addr == 0)
    return -1;

  if (argc == 2) {
      cmd.cmd = CMD_READ_WORD;
      cmd.len = 4;
      word_pack(&cmd.args[0],
                addr + FIP_URV_REGS_APP_MAILBOXES + MBOX_REGS_STATUS);
      cmd.cb = cb_mbox;
  }
  else {
    unsigned long val;
    char *e;
    val = strtoul (argv[2], &e, 0);
    if (*e != 0) {
      fprintf (out, "incorrect value\n");
      return -1;
    }

    cmd.cmd = CMD_WRITE_WORD;
    cmd.len = 8;
    word_pack(&cmd.args[0],
              addr + FIP_URV_REGS_APP_MAILBOXES + MBOX_REGS_MBOXOUT);
    word_pack(&cmd.args[4], val);
    cmd.cb = NULL;
  }
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return argc;
}

static int do_slots (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], IO_ADDR + FIP_URV_REGS_BOARDS);
  cmd.cb = cb_slots;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  fprintf (out, "        1111111   relays 1111111    loops\n");
  fprintf (out, "slot    6543210987654321 6543210987654321\n");
  return 1;
}

static int do_supervisor (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], SUPERVISOR_ADDR);
  cmd.cb = cb_supervisor;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_stat_regs (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_STAT_BUF;
  cmd.len = 4;
  cmd.args[0] = 0;
  cmd.cb = cb_stat_regs;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_cpu_fault (int argc, char *argv[])
{
  struct urv_cmd cmd;
  unsigned val;

  if (argc != 2) {
    fprintf (out, "usage: cpu-fault CPU\n");
    return -1;
  }

  val = strtoul (argv[1], NULL, 0);

  cmd.cmd = CMD_CPU_FLT;
  cmd.len = 4;
  cmd.args[0] = 1 << val;
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 2;
}

static int do_wait (int argc, char *argv[])
{
  unsigned long val;
  unsigned long param;
  struct urv_cmd cmd;

  if (argc == 2)
    param = 0;
  else if (argc == 3)
    param = strtoul (argv[2], NULL, 0);
  else {
    fprintf (out, "usage: wait VAL [PARAM]\n");
    return -1;
  }
  val = strtoul (argv[1], NULL, 0);

  cmd.cmd = CMD_WAIT;
  cmd.len = 8;
  word_pack(&cmd.args[0], val);
  word_pack(&cmd.args[4], param);
  cmd.cb = cb_wait;
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return argc;
}

static int do_restart_cfg (int argc, char *argv[])
{
  unsigned long val;
  struct urv_cmd cmd;

  if (argc == 1) {
    cmd.cmd = CMD_READ_WORD;
    cmd.len = 4;
    word_pack(&cmd.args[0], IO_ADDR | FIP_URV_REGS_RESTART_CFG);
    cmd.cb = cb_restart_cfg;
    if (append_command(ftb, &cmd) != 0)
      return -1;
    return 1;
  } else if (argc == 2) {
    val = strtoul (argv[1], NULL, 0);

    cmd.cmd = CMD_WRITE_WORD;
    cmd.len = 8;
    word_pack(&cmd.args[0], IO_ADDR | FIP_URV_REGS_RESTART_CFG);
    word_pack(&cmd.args[4], val);
    cmd.cb = NULL;
  }
  else {
    fprintf (out, "usage: restart-cfg [VAL]\n");
    return -1;
  }
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return argc;
}

static int do_restart (int argc, char *argv[])
{
  struct urv_cmd cmd;

  if (argc == 2 && strcmp(argv[1], "now") == 0 ) {
    cmd.cmd = CMD_WRITE_WORD;
    cmd.len = 8;
    word_pack(&cmd.args[0], IO_ADDR | FIP_URV_REGS_RESTART);
    word_pack(&cmd.args[4], 0x8e51a610);
    cmd.cb = NULL;
  }
  else {
    fprintf (out, "usage: restart now\n");
    return -1;
  }
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return argc;
}

static int do_monimod_power (int argc, char *argv[])
{
  unsigned long val;
  struct urv_cmd cmd;

  if (argc == 1) {
    cmd.cmd = CMD_READ_WORD;
    cmd.len = 4;
    word_pack(&cmd.args[0], IO_ADDR | FIP_URV_REGS_MONIMOD_SHDN);
    cmd.cb = cb_monimod_power;
    if (append_command(ftb, &cmd) != 0)
      return -1;
    return 1;
  }
  else if (argc == 2) {
    if (strcmp (argv[1], "on") == 0)
      val = 0;
    else if (strcmp (argv[1], "off") == 0)
      val = 1;
    else {
      fprintf (out, "monimod-power [on|off]\n");
      return -1;
    }
    cmd.cmd = CMD_WRITE_WORD;
    cmd.len = 8;
    word_pack(&cmd.args[0], IO_ADDR | FIP_URV_REGS_MONIMOD_SHDN);
    word_pack(&cmd.args[4], val);
    cmd.cb = NULL;

    if (append_command(ftb, &cmd) != 0)
      return -1;
    return 2;
  }
  else {
    fprintf (out, "monimod-power [on|off]\n");
    return -1;
  }
}

static int do_fip_err (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], IO_ADDR | FIP_URV_REGS_FIP_ERR);
  cmd.cb = cb_fip_err;
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_fip_reset (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_ERROR;
  cmd.len = 0;
  cmd.cb = NULL;
  /* Twice so the reset is sent.  */
  ftb->reset = 2;
  if (append_command(ftb, &cmd) != 0)
    return -1;
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_scrubber (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_WRITE_WORD;
  cmd.len = 8;
  word_pack(&cmd.args[0],
            SUPERVISOR_ADDR + HYDRA_SUPERVISOR_REGS_SCRUB_CFG);
  word_pack(&cmd.args[4],
            HYDRA_SUPERVISOR_REGS_SCRUB_CFG_IRAM_EN
            | HYDRA_SUPERVISOR_REGS_SCRUB_CFG_DRAM_EN);
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_mon_vout (int argc, char *argv[])
{
  struct urv_cmd cmd;
  unsigned len = 1;

  cmd.cmd = CMD_I2C_TFR;
  cmd.args[len++] = I2C_CMD_START;
  cmd.args[len++] = 0x19 << 1;
  cmd.args[len++] = I2C_CMD_WRITE | (1 << 4);
  cmd.args[len++] = 0x8b;
  cmd.args[len++] = I2C_CMD_START;
  cmd.args[len++] = (0x19 << 1) | 1;
  cmd.args[len++] = I2C_CMD_READ_STOP | (2 << 4);
  cmd.args[0] = len - 1;
  cmd.len = len;
  cmd.cb = cb_i2c_tfr;
  cmd.cb_data = xmalloc(sizeof(struct i2c_cb_tfr));

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_mon (int argc, char *argv[])
{
  unsigned oargc = argc;
  unsigned targ = 0x19;
  unsigned ilen = 0;
  unsigned icmd;
  unsigned flags;
  unsigned alen;
  unsigned arg[4];

  flags = 0;
  alen = 0;
  argc--;
  argv++;

  while (argc > 0 && argv[0][0] == '-') {
    if (strcmp(argv[0], "-t") == 0) {
      if (argc < 2) {
        fprintf (out, "option -t needs the target\n");
        return -1;
      }
      targ = strtoul(argv[1], NULL, 0);
      argv += 2;
      argc -= 2;
    }
    else if (strcmp(argv[0], "-s") == 0) {
      flags |= 1;
      argv++;
      argc--;
    }
  }

  if (argc > 0) {
    if (strcmp (argv[0], "page") == 0) {
      icmd = 0x00;
      ilen = 1;
    }
    else if (strcmp (argv[0], "capability") == 0) {
      icmd = 0x19;
      ilen = 1;
    }
    else if (strcmp (argv[0], "status-byte") == 0) {
      icmd = 0x78;
      ilen = 1;
    }
    else if (strcmp (argv[0], "status-cml") == 0) {
      icmd = 0x7e;
      ilen = 1;
    }
    else if (strcmp (argv[0], "fan-speed-1") == 0) {
      icmd = 0x90;
      ilen = 2;
    }
    else if (strcmp (argv[0], "fan-speed-2") == 0) {
      icmd = 0x91;
      ilen = 2;
    }
    else if (strcmp (argv[0], "fan-speed-3") == 0) {
      icmd = 0x92;
      ilen = 2;
    }
    else if (strcmp (argv[0], "id") == 0)
      icmd = 0x99;
    else if (strcmp (argv[0], "model") == 0)
      icmd = 0x9a;
    else if (strcmp (argv[0], "revision") == 0)
      icmd = 0x9b;
    else if (strcmp (argv[0], "location") == 0)
      icmd = 0x9c;
    else if (strcmp (argv[0], "date") == 0)
      icmd = 0x9d;
    else if (strcmp (argv[0], "serial") == 0)
      icmd = 0x9e;
    else if (strcmp (argv[0], "ic-device-rev") == 0)
      icmd = 0xae;
    else if (strcmp (argv[0], "vout") == 0) {
      ilen = 2;
      icmd = 0x8b;
    }
    else if (strcmp (argv[0], "iout") == 0) {
      ilen = 2;
      icmd = 0x8c;
    }
    else if (strcmp (argv[0], "0x66") == 0) {
      ilen = 1;
      icmd = 0x66;
    }
    else if (strcmp (argv[0], "fan-config-1-2") == 0) {
      ilen = 1;
      icmd = 0x3a;
    }
    else if (strcmp (argv[0], "fan-config-3-4") == 0) {
      ilen = 1;
      icmd = 0x3d;
    }
    else if (strcmp (argv[0], "fan-command-1") == 0) {
      ilen = 2;
      icmd = 0x3b;
    }
    else if (strcmp (argv[0], "fan-command-2") == 0) {
      ilen = 2;
      icmd = 0x3c;
    }
    else if (strcmp (argv[0], "fan-command-3") == 0) {
      ilen = 2;
      icmd = 0x3e;
    }
    else if (strcmp (argv[0], "temp-1") == 0) {
      ilen = 2;
      icmd = 0x8d;
    }
    else if (strcmp (argv[0], "temp-2") == 0) {
      ilen = 2;
      icmd = 0x8e;
    }
    else if (strcmp (argv[0], "temp-3") == 0) {
      ilen = 2;
      icmd = 0x8f;
    }
    else if (strcmp (argv[0], "uptime") == 0) {
      ilen = 4;
      icmd = 0xff10;
    }
    else if (strcmp (argv[0], "query") == 0) {
      if (argc < 2) {
        fprintf (out, "query command needs an argument\n");
        return -1;
      }
      arg[0] = 1;
      arg[1] = strtoul(argv[1], NULL, 0);
      alen = 2;
      ilen = 0;
      icmd = 0x1a;
      argc--;
      argv++;
    }
    else {
      fprintf (out, "unknown pmbus command, expect model, revision\n");
      return -1;
    }

    argc--;
    argv++;
  }
  else {
    fprintf (out, "need a pmbus command\n");
    return -1;
  }

  struct i2c_cb_tfr *tfr;
  struct urv_cmd cmd;
  unsigned clen;

  tfr = xmalloc(sizeof(struct i2c_cb_tfr));
  tfr->cb = cb_mon_read;
  tfr->cb_data = tfr;
  tfr->cmd = icmd | (ilen << 16);
  tfr->flags = flags;

  cmd.cmd = CMD_I2C_TFR;
  clen = 1;
  cmd.args[clen++] = I2C_CMD_START;
  cmd.args[clen++] = targ << 1;
  if (icmd & 0xff00) {
    cmd.args[clen++] = I2C_CMD_WRITE | ((alen + 2) << 4);
    cmd.args[clen++] = icmd >> 8;
  }
  else
    cmd.args[clen++] = I2C_CMD_WRITE | ((alen + 1) << 4);
  cmd.args[clen++] = icmd & 0xff;
  for (unsigned i = 0; i < alen; i++)
    cmd.args[clen++] = arg[i];
  cmd.args[clen++] = I2C_CMD_START;
  cmd.args[clen++] = (targ << 1) | 1;
  if (ilen == 0)
    cmd.args[clen++] = I2C_CMD_READLEN;
  cmd.args[clen++] = I2C_CMD_READ_STOP | (ilen << 4);
  cmd.args[0] = clen - 1;
  cmd.len = 4 + clen;
  cmd.cb = cb_i2c_tfr;
  cmd.cb_data = tfr;
  if (append_command(ftb, &cmd) != 0)
    return -1;

  return oargc - argc;
}

static int do_wmon (int argc, char *argv[])
{
  unsigned oargc = argc;
  unsigned targ = 0x19;
  unsigned icmd;
  unsigned ilen = 0;
  unsigned iparam;

  argv++;
  argc--;

  if (argc > 0 && strcmp(argv[0], "-t") == 0) {
    if (argc < 2) {
      fprintf (out, "option -t needs the target\n");
      return -1;
    }
    targ = strtoul(argv[1], NULL, 0);
    argv += 2;
    argc -= 2;
  }

  if (argc > 0) {
    if (strcmp (argv[0], "reset") == 0) {
      icmd = 0xff06;
      ilen = 0;
      argc--;
    }
    else if (strcmp (argv[0], "page") == 0) {
      if (argc <= 1) {
        fprintf (out, "page command need an argument\n");
        return -1;
      }
      icmd = 0x00;
      iparam = atoi(argv[1]);
      ilen = 1;
      argc -= 2;
    }
    else if (strcmp (argv[0], "status-cml") == 0) {
      icmd = 0x7e;
      iparam = (atoi(argv[1])) & 0x32;
      ilen = 1;
      argc -= 2;
    }
    else if (strcmp (argv[0], "fan-config-1-2") == 0) {
      fprintf (out, "Write fan config 1-2\n");
      icmd = 0x3a;
      iparam = atoi(argv[1]);
      ilen = 1;
      argc -= 2;
    }
    else if (strcmp (argv[0], "fan-config-3-4") == 0) {
      fprintf (out, "Write fan config 3-4\n");
      icmd = 0x3d;
      iparam = atoi(argv[1]);
      ilen = 1;
      argc -= 2;
    }
    else if (strcmp (argv[0], "fan-command-1") == 0) {
      fprintf (out, "Write fan command 1\n");
      icmd = 0x3b;
      iparam = atoi(argv[1]);
      iparam = float_to_linear(iparam);
      ilen = 2;
      argc -= 2;
    }
    else if (strcmp (argv[0], "fan-command-2") == 0) {
      fprintf (out, "Write fan command 2\n");
      icmd = 0x3c;
      iparam = atoi(argv[1]);
      iparam = float_to_linear(iparam);
      ilen = 2;
      argc -= 2;
    }
    else if (strcmp (argv[0], "fan-command-3") == 0) {
      fprintf (out, "Write fan command 3\n");
      icmd = 0x3e;
      iparam = atoi(argv[1]);
      iparam = float_to_linear(iparam);
      ilen = 2;
      argc -= 2;
    }
    else {
      fprintf (out, "unknown pmbus command, expect reset\n");
      return -1;
    }
  }
  else {
    fprintf (out, "need a pmbus command\n");
    return -1;
  }
/*
  if (0) {
    struct i2c_cb_msg *m = xmalloc(sizeof(struct i2c_cb_msg));
    m->cmd = icmd;
    m->state = I2C_M_INIT;
    m->len = ilen;
    m->buf[0] = iparam;
    m->targ = targ;
    if (cmd_i2c_write(m) != 1)
      return -1;
  }
  else {*/

    struct i2c_cb_tfr *tfr;
    struct urv_cmd cmd;
    unsigned clen;
#if C_PEC_EN == 0x1
    uint8_t pec_checksum = 0;
#endif

    tfr = xmalloc(sizeof(struct i2c_cb_tfr));
    tfr->cb = NULL;
    tfr->cb_data = tfr;
    tfr->cmd = icmd | (ilen << 16);

    cmd.cmd = CMD_I2C_TFR;
    clen = 1;
    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = (targ << 1);
#if C_PEC_EN == 0x1
    pec_checksum = crc8_byte((uint8_t)targ << 1);
#endif

    if (icmd & 0xff00) {
        cmd.args[clen++] = I2C_CMD_WRITE_STOP | ((ilen + 2) << 4);
        cmd.args[clen++] = icmd >> 8;
#if C_PEC_EN == 0x1
        pec_checksum = crc8_byte(pec_checksum ^ ((((uint8_t)icmd) >> 8) & 0xff));
#endif
    } else {
        /* 0x03 | 0x30 */
        cmd.args[clen++] = I2C_CMD_WRITE_STOP | ((ilen + 1) << 4);
    }

    cmd.args[clen++] = icmd & 0xff;
#if C_PEC_EN == 0x1
    pec_checksum = crc8_byte(pec_checksum ^ ((uint8_t)icmd & 0xff));
#endif
    for (unsigned i = 0; i < ilen; i++) {
        cmd.args[clen++] = (unsigned char) (iparam & 0x000000ff);
#if C_PEC_EN == 0x1
        pec_checksum = crc8_byte(pec_checksum ^ ((uint8_t)(iparam & 0x000000ff)));
#endif
        iparam = iparam >> 8;
    }
#if C_PEC_EN == 0x1
    cmd.args[clen++] = (unsigned char) pec_checksum;
    fprintf (out, "PEC chksum --- %02x\n", pec_checksum);
#endif

    cmd.args[0] = clen - 1;
    //for (unsigned i = 0; i < clen; i++) {
    //  fprintf (out, "cmd.args[%u] --- %02x\n", i,cmd.args[i]);
    //}
    cmd.len = 4 + clen;
    cmd.cb = cb_i2c_tfr;
    cmd.cb_data = tfr;

    if (append_command(ftb, &cmd) != 0) {
        return -1;
    }
  //}

  //fprintf (out, "Return OARGC: %u --- ARGC: %u --- DIFF: %u\n",oargc, argc, oargc - argc);
  return oargc - argc;
}

static int do_i2c (int argc, char *argv[])
{
  struct urv_cmd cmd;
  unsigned oargc = argc;

  argc--;
  argv++;

  word_pack(&cmd.args[0], IO_ADDR + FIP_URV_REGS_I2C + 0x14);

  if (argc == 0) {
    cmd.cmd = CMD_READ_WORD;
    cmd.len = 4;
    cmd.cb = cb_read_i2c;
  }
  else {
    unsigned ch = strtoul (argv[0], NULL, 0);
    cmd.cmd = CMD_WRITE_WORD;
    cmd.len = 8;
    cmd.cb = NULL;
    word_pack(&cmd.args[4], ch);
    argc--;
  }

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return oargc - argc;
}

static int do_i2c_id (int argc, char *argv[])
{
  unsigned oargc = argc;
  unsigned targ;
  unsigned flags = 0;

  argc--;
  argv++;

  if (strcmp(argv[0], "-s") == 0) {
    flags |= 1;
    argv++;
    argc--;
  }

  if (argc != 1) {
    fprintf (out, "command needs i2c address (0-7)\n");
    return -1;
  }
  targ = strtoul(argv[0], NULL, 0);
  argc--;

  targ |= 0x20;

  if (0) {
    struct i2c_cb_msg *rd = xmalloc(sizeof(struct i2c_cb_msg));
    rd->state = I2C_M_INIT;
    rd->targ = targ;
    rd->cmd = 0x12;
    rd->len = 1;
    rd->cb = cb_i2c_id;
    rd->cb_data = rd;
    if (cmd_i2c_read(rd) != 1)
      return -1;
  }
  else {
    struct i2c_cb_tfr *tfr;
    struct urv_cmd cmd;
    unsigned clen;

    tfr = xmalloc(sizeof(struct i2c_cb_tfr));
    tfr->cb = cb_i2c_id2;
    tfr->cb_data = tfr;
    tfr->flags = flags;

    cmd.cmd = CMD_I2C_TFR;
    clen = 1;
    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = targ << 1;
    cmd.args[clen++] = I2C_CMD_WRITE | (1 << 4);
    cmd.args[clen++] = 0x12;
    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = (targ << 1) | 1;
    cmd.args[clen++] = I2C_CMD_READ_STOP | (1 << 4);
    cmd.args[0] = clen - 1;
    cmd.len = 4 + clen;
    cmd.cb = cb_i2c_tfr;
    cmd.cb_data = tfr;
    if (append_command(ftb, &cmd) != 0)
      return -1;
  }

  return oargc - argc;
}

static int do_i2c_eeprom (int argc, char *argv[])
{
  unsigned oargc = argc;
  unsigned targ;
  unsigned flags = 0;

  argc--;
  argv++;

  if (strcmp(argv[0], "-s") == 0) {
    flags |= 1;
    argv++;
    argc--;
  }

  if (argc == 1) {
    targ = strtoul(argv[0], NULL, 0);
    argc--;
  }
  else
    targ = 0;

  if (argc != 0) {
    fprintf (out, "too many arguments\n");
    return -1;
  }

  targ |= 0x50;

  {
    struct i2c_cb_tfr *tfr;
    struct urv_cmd cmd;
    unsigned clen;

    tfr = xmalloc(sizeof(struct i2c_cb_tfr));
    tfr->cb = cb_i2c_id2;
    tfr->cb_data = tfr;
    tfr->flags = flags;

    cmd.cmd = CMD_I2C_TFR;
    clen = 1;
    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = (targ << 1) | 0;
    cmd.args[clen++] = I2C_CMD_WRITE | (1 << 4);
    cmd.args[clen++] = 0x00;
    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = (targ << 1) | 1;
    cmd.args[clen++] = I2C_CMD_READ_STOP | (1 << 4);
    cmd.args[0] = clen - 1;
    cmd.len = 4 + clen;
    cmd.cb = cb_i2c_tfr;
    cmd.cb_data = tfr;
    if (append_command(ftb, &cmd) != 0)
      return -1;
  }

  return oargc - argc;
}

static int do_psu (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_READ_WORD;
  cmd.len = 4;
  word_pack(&cmd.args[0], IO_ADDR + FIP_URV_REGS_PSU_STAT);
  cmd.cb = cb_read_psu;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_pwr_cyc (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_WRITE_WORD;
  cmd.len = 8;
  word_pack(&cmd.args[0], IO_ADDR + FIP_URV_REGS_PSU_CTRL);
  word_pack(&cmd.args[4], 3);
  cmd.cb = NULL;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}

static int do_monimod_scan (int argc, char *argv[])
{
  struct urv_cmd cmd;
  unsigned val;

  if (argc == 1)
    val = 2;
  else if (argc == 2 && strcmp (argv[1], "on") == 0)
    val = 1;
  else if (argc == 2 && strcmp (argv[1], "off") == 0)
    val = 0;
  else {
    fprintf (out, "usage: monimod-scan [on|off]\n");
    return -1;
  }

  cmd.cmd = CMD_I2C_MON;
  cmd.len = 4;
  cmd.args[0] = val;
  cmd.cb = cb_monimod_scan;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1 + (val != 2);
}

static int do_i2c_stat (int argc, char *argv[])
{
  struct urv_cmd cmd;

  cmd.cmd = CMD_I2C_STAT;
  cmd.len = 0;
  cmd.cb = cb_i2c_stat;

  if (append_command(ftb, &cmd) != 0)
    return -1;
  return 1;
}


static int do_i2c_off (int argc, char *argv[])
{
  struct urv_cmd cmd;

  if (argc < 1 || argc > 2) {
    fprintf (out, "usage: %s [VALUE]\n", argv[0]);
    return -1;
  }

  if (argc == 1) {
    cmd.cmd = CMD_READ_WORD;
    cmd.len = 4;
    word_pack(&cmd.args[0], IO_ADDR + FIP_URV_REGS_I2C_OFF);
    cmd.cb = cb_i2c_off;
  }
  else {
    unsigned long val;
    char *e;
    val = strtoul (argv[1], &e, 0);
    if (*e != 0) {
      fprintf (out, "incorrect value\n");
      return -1;
    }

    cmd.cmd = CMD_WRITE_WORD;
    cmd.len = 8;
    word_pack(&cmd.args[0], IO_ADDR + FIP_URV_REGS_I2C_OFF);
    word_pack(&cmd.args[4], val);
    cmd.cb = NULL;
  }
  if (append_command(ftb, &cmd) != 0)
    return -1;
  return argc;
}

static int do_spi_init (int argc, char *argv[])
{
  if (cmd_spi_init() != 0)
    return -1;
  return 1;
}

static int do_spi_read (int argc, char *argv[])
{
  unsigned long val;
  char *e;

  if (argc < 2) {
    fprintf (out, "usage: spi-read ADDR\n");
    return -1;
  }
  val = strtoul (argv[1], &e, 0);
  if (*e != 0) {
    fprintf (out, "incorrect address for read\n");
    return -1;
  }

  if (cmd_spi_read(val, 4) != 0)
    return -1;

  return 2;
}

static int do_spi_dump (int argc, char *argv[])
{
  unsigned long addr;
  unsigned long len;
  char *e;

  if (argc < 3) {
    fprintf (out, "usage: spi-dump ADDR LEN\n");
    return -1;
  }
  addr = strtoul (argv[1], &e, 0);
  if (*e != 0) {
    fprintf (out, "bad value for address\n");
    return -1;
  }
  len = strtoul (argv[2], &e, 0);
  if (*e != 0) {
    fprintf (out, "bad value for length\n");
    return -1;
  }

  if (cmd_spi_dump(addr, len) != 0)
    return -1;

  return 3;
}

/* ECC:
  0 => "11000001010010000100000011111111",
    1 => "00100001001001001111111110010000",
    2 => "01101100111111110000100000001000",
    3 => "11111111000000011010010001000100",
    4 => "00010110111100001001001010100110",
    5 => "00010000000111110111000101100001",
    6 => "10001010100000100000111100011011"
*/
static unsigned syndroms[7] =
  {
   0xc14840ff,
   0x2124ff90,
   0x6cff0808,
   0xff01a444,
   0x16f092a6,
   0x101f7161,
   0x8a820f1b
  };

static unsigned parity(unsigned v)
{
  unsigned t;
  t = v;
  t = (t >> 16) ^ t;
  t = (t >> 8) ^ t;
  t = (t >> 4) ^ t;
  t = (t >> 2) ^ t;
  t = (t >> 1) ^ t;
  return t & 1;
}

static void compute_ecc (unsigned char **buf, unsigned *sz)
{
  unsigned char *b = *buf;
  unsigned char *nbuf;
  unsigned nsz;
  unsigned i, ni, j;
  unsigned w;
  unsigned char e;

  nsz = (*sz / 4) * 5;
  nbuf = xmalloc(nsz);
  ni = 0;
  for (i = 0; i < *sz; i += 4) {
    w = b[i + 0] | (b[i + 1] << 8) | (b[i + 2] << 16) | (b[i + 3] << 24);
    e = 0;
    for (j = 0; j < 7; j++)
      e |= parity(w & syndroms[j]) << j;
    nbuf[ni++] = b[i + 0];
    nbuf[ni++] = b[i + 1];
    nbuf[ni++] = b[i + 2];
    nbuf[ni++] = b[i + 3];
    nbuf[ni++] = e;
  }

  free(b);
  *buf = nbuf;
  *sz = nsz;
}

static int do_spi_flash_elf (int argc, char *argv[])
{
  unsigned orig_argc = argc;
  unsigned char *buf;
  unsigned memsz;
  unsigned padsz = 4096;
  unsigned page = 0;
  unsigned flag_disp;
  unsigned version;
  char *e;

  flag_disp = 0;
  argc--;
  argv++;

  if (argc > 0 && strcmp(argv[0], "-d") == 0) {
    flag_disp = 1;
    argc--;
    argv++;
  }
  if (argc > 0 && strcmp(argv[0], "-D") == 0) {
    flag_disp = 2;
    argc--;
    argv++;
  }

  if (argc < 2 || argc > 3) {
    fprintf (out, "usage: spi-flash-elf [-d] FILENAME VERSION [PAGE]\n");
    fprintf (out, " load address is PAGE * 256\n");
    return -1;
  }
  version = strtoul (argv[1], &e, 0);
  if (*e != 0 || version == 0) {
    fprintf (out, "bad value for version (cannot be 0)\n");
    return -1;
  }
  if (argc >= 3) {
    page = strtoul (argv[2], &e, 0);
    if (*e != 0 || (page & 31) != 0) {
      fprintf (out, "bad value for page (must be a multiple of 32)\n");
      return -1;
    }
  }
  version |= page << 16;

  if (load_elf(argv[0], &buf, &memsz) < 0)
    return -1;
  fprintf (out, "Size: %uB (%ukB)\n", memsz, memsz / 1024);
  if (memsz & 3) {
    fprintf (out, "bad size (not a multiple of 4)\n");
    return -1;
  }
  if (padsz & 3) {
    fprintf (out, "bad pad size (not a multiple of 4)\n");
    return -1;
  }
  if (memsz > padsz) {
    fprintf (out, "file is too big (%u > %u)\n", memsz, padsz);
    return -1;
  }
  /* Pad.  */
  buf = realloc(buf, padsz);
  for (unsigned i = memsz; i < padsz; i++)
    buf[i] = 0;
  memsz = padsz;

  /* Add version.  */
  buf[padsz - 16] = version >> 0;
  buf[padsz - 15] = version >> 8;
  buf[padsz - 14] = version >> 16;

  /* Compute ecc. */
  compute_ecc(&buf, &memsz);
  fprintf (out, "Size (with ECC+padding): %uB (%ukB)\n", memsz, memsz / 1024);

  if (flag_disp == 1) {
    unsigned i, j;
    unsigned addr = 0;
    for (i = 0; i < memsz; i += 5) {
      fprintf (out, "%04x (%04x): ", addr, i);
      for (j = 0; j < 5; j++)
        fprintf (out, " %02x", buf[i + j]);
      addr += 4;
      fprintf (out, "\n");
    }
    return 0;
  }
  if (flag_disp == 2) {
    unsigned i, j;
    for (i = 0; i < memsz; i += 16) {
      fprintf (out, "%06x: ", page * 0x100 + i);
      for (j = 0; j < 16; j++)
        fprintf (out, " %02x", buf[i + j]);
      fprintf (out, "\n");
    }
    return 0;
  }

  if (cmd_spi_prog (page * 0x100, buf, memsz) != 0)
    return -1;
  return orig_argc;
}

static int do_spi_page_erase (int argc, char *argv[])
{
  unsigned long pg;
  char *e;

  if (argc < 2) {
    fprintf (out, "usage: spi-page-erase PAGE\n");
    return -1;
  }
  pg = strtoul (argv[1], &e, 0);
  if (*e != 0) {
    fprintf (out, "bad value for page\n");
    return -1;
  }

  if (cmd_spi_page_erase(pg) != 0)
    return -1;
  return 2;
}

static int do_envm_flash (const char *filename, unsigned addr, unsigned verbose)
{
  unsigned char *buf;
  unsigned memsz;
  unsigned maxsz = 16 << 10;
  unsigned padmask = 0x100 - 1;
  unsigned padsz;

  if (addr & padmask) {
      fprintf (out, "bad value for address (must be a multiple of %u)\n",
               padmask + 1);
      return -1;
  }

  if (load_elf(filename, &buf, &memsz) < 0)
    return -1;
  fprintf (out, "Size: %uB (%ukB)\n", memsz, memsz / 1024);
  if (memsz > maxsz) {
    fprintf (out, "file is too big (%u > %u)\n", memsz, maxsz);
    return -1;
  }
  /* Pad.  */
  padsz = (memsz + padmask) & ~padmask;
  buf = realloc(buf, padsz);
  for (unsigned i = memsz; i < padsz; i++)
    buf[i] = 0;
  memsz = padsz;

  if (cmd_envm_prog (addr, buf, memsz, verbose) != 0)
    return -1;
  return 0;
}

static int do_envm_flash_app (int argc, char *argv[])
{
  unsigned orig_argc = argc;
  unsigned addr = 0x4000;
  unsigned verbose = 0;
  char *e;
  int res;

  argc--;
  argv++;

  while (argc > 0 && argv[0][0] == '-') {
    if (strcmp(argv[0], "-v") == 0)
      verbose++;
    else {
      fprintf (out, "unknown option %s\n", argv[0]);
      return -1;
    }
    argc--;
    argv++;
  }

  if (argc < 1 || argc > 2) {
    fprintf (out, "usage: envm-flash-elf [-v] FILENAME [ADDR]\n");
    fprintf (out, " Default address is 0x%x\n", addr);
    return -1;
  }
  if (argc > 1) {
    addr = strtoul (argv[1], &e, 0);
    if (*e != 0) {
      fprintf (out, "bad addres number\n");
      return -1;
    }
  }

  if (addr < 0x2000) {
    fprintf(out, "bad address (0x%x) for application\n", addr);
    return -1;
  }
  res = do_envm_flash(argv[0], addr, verbose);
  if (res < 0)
    return -1;
  return orig_argc;
}

static int do_envm_flash_mgmt (int argc, char *argv[])
{
  unsigned orig_argc = argc;
  unsigned addr = 0x0000;
  unsigned verbose = 0;
  char *e;
  int res;

  argc--;
  argv++;

  while (argc > 0 && argv[0][0] == '-') {
    if (strcmp(argv[0], "-v") == 0)
      verbose++;
    else {
      fprintf (out, "unknown option %s\n", argv[0]);
      return -1;
    }
    argc--;
    argv++;
  }

  if (argc < 1 || argc > 2) {
    fprintf (out, "usage: envm-flash-mgmt [-v] FILENAME [ADDR]\n");
    fprintf (out, " Default address is 0x%x\n", addr);
    return -1;
  }
  if (argc > 1) {
    addr = strtoul (argv[1], &e, 0);
    if (*e != 0) {
      fprintf (out, "bad address number\n");
      return -1;
    }
  }

  res = do_envm_flash(argv[0], addr, verbose);
  if (res < 0)
    return -1;
  return orig_argc;
}

static int do_envm_flash_erase (int argc, char *argv[])
{
  unsigned orig_argc = argc;
  unsigned addr = 0x4000;
  unsigned char buf[128];
  unsigned verbose = 0;
  char *e;

  argc--;
  argv++;

  while (argc > 0 && argv[0][0] == '-') {
    if (strcmp(argv[0], "-v") == 0)
      verbose++;
    else {
      fprintf (out, "unknown option %s\n", argv[0]);
      return -1;
    }
    argc--;
    argv++;
  }

  if (argc > 1) {
    fprintf (out, "usage: envm-flash-erase [-v] [ADDR]\n");
    fprintf (out, " Default address is 0x%x (application)\n", addr);
    return -1;
  }
  if (argc > 1) {
    addr = strtoul (argv[1], &e, 0);
    if (*e != 0) {
      fprintf (out, "bad address number\n");
      return -1;
    }
  }

  memset(buf, 0xff, sizeof (buf));
  if (cmd_envm_prog (addr, buf, sizeof(buf), verbose) != 0)
    return -1;
  return orig_argc;
}

static int do_ratopus_read (int argc, char *argv[])
{
    unsigned oargc = argc;
    unsigned targ = 0x2B;
    unsigned ilen = 0;
    unsigned addr;
    unsigned flags;
    unsigned alen;
    unsigned arg[4];

    flags = 0;
    alen = 0;
    argc--;
    argv++;

    while (argc > 0 && argv[0][0] == '-') {
        if (strcmp(argv[0], "-t") == 0) {
            if (argc < 2) {
                fprintf (out, "option -t needs the target\n");
                return -1;
            }
            targ = strtoul(argv[1], NULL, 0);
            argv += 2;
            argc -= 2;
        } else if (strcmp(argv[0], "-s") == 0) {
            flags |= 1;
            argv++;
            argc--;
        }
    }

    if (argc > 0) {
        if (strcmp (argv[0], "command-reg") == 0) {
            addr = 0x00;
            ilen = 2;
        } else if (strcmp (argv[0], "voltage-conv") == 0) {
            addr = 0x01;
            ilen = 8;
        } else if (strcmp (argv[0], "tsense") == 0) {
            addr = 0x02;
            ilen = 2;
        } else {
            fprintf (out, "unknown ratopus command\n");
            return -1;
        }
        argc--;
        argv++;
    } else {
        fprintf (out, "ratopus command is required\n");
        return -1;
    }

    struct i2c_cb_tfr *tfr;
    struct urv_cmd cmd;
    unsigned clen;

    tfr = xmalloc(sizeof(struct i2c_cb_tfr));
    tfr->cb = cb_ratopus_read;
    tfr->cb_data = tfr;
    tfr->cmd = addr | (ilen << 16);
    tfr->flags = flags;

    cmd.cmd = CMD_I2C_TFR;
    clen = 1;

    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = targ << 1;
    cmd.args[clen++] = I2C_CMD_WRITE | ((alen + 1) << 4);
    cmd.args[clen++] = addr & 0xff;

    for (unsigned i = 0; i < alen; i++) {
        cmd.args[clen++] = arg[i];
    }

    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = (targ << 1) | 1;

    if (ilen == 0) {
        cmd.args[clen++] = I2C_CMD_READLEN;
    }

    cmd.args[clen++] = I2C_CMD_READ_STOP | (ilen << 4);
    cmd.args[0] = clen - 1;

    //for (unsigned i = 0; i < clen; i++) {
    //    fprintf(out, "IT: %i -- VAL: %x\n", i, cmd.args[i]);
    //}

    cmd.len = 4 + clen;
    cmd.cb = cb_i2c_tfr;
    cmd.cb_data = tfr;

    if (append_command(ftb, &cmd) != 0) {
        return -1;
    }

    return oargc - argc;
}

static int do_ratopus_write (int argc, char *argv[])
{
  unsigned oargc = argc;
  unsigned targ = 0x19;
  unsigned addr;
  unsigned ilen = 0;
  unsigned iparam;

  argv++;
  argc--;

    if (argc > 0 && strcmp(argv[0], "-t") == 0) {
        if (argc < 2) {
            fprintf (out, "option -t needs the target\n");
            return -1;
        }
        targ = strtoul(argv[1], NULL, 0);
        argv += 2;
        argc -= 2;
    }

    if (argc > 0) {
        if (strcmp (argv[0], "command-reg") == 0) {
            addr = 0x00;
            iparam = (atoi(argv[1])) & 0x0000FFFF;
            ilen = 2;
            argc -= 2;
        } else {
            fprintf (out, "unknown ratopus command\n");
            return -1;
        }
    } else {
        fprintf (out, "need a ratopus command\n");
        return -1;
    }

    struct i2c_cb_tfr *tfr;
    struct urv_cmd cmd;
    unsigned clen;

    tfr = xmalloc(sizeof(struct i2c_cb_tfr));
    tfr->cb = NULL;
    tfr->cb_data = tfr;
    tfr->cmd = addr | (ilen << 16);

    cmd.cmd = CMD_I2C_TFR;
    clen = 1;
    cmd.args[clen++] = I2C_CMD_START;
    cmd.args[clen++] = (targ << 1);
    cmd.args[clen++] = I2C_CMD_WRITE_STOP | ((ilen + 1) << 4);
    cmd.args[clen++] = addr & 0xff;

    for (int i = ilen-1; i >= 0; i--) {
        cmd.args[clen++] = (unsigned char) ((iparam >> (8*i)) & 0x000000ff);
    }

    cmd.args[0] = clen - 1;
    //for (unsigned i = 0; i < clen; i++) {
    //  fprintf (out, "cmd.args[%u] --- %02x\n", i,cmd.args[i]);
    //}

    cmd.len = 4 + clen;
    cmd.cb = cb_i2c_tfr;
    cmd.cb_data = tfr;

    if (append_command(ftb, &cmd) != 0) {
        return -1;
    }

    return oargc - argc;
}

static int cb_power_fan (struct urv_cmd *cmd, unsigned char *rep)
{
    if (cmd->args[1] == 0) {
        fprintf (out, "off %u: %02x %02x %02x %02x\n",
                          cmd->args[0], rep[4], rep[5], rep[6], rep[7]);
    }

    fprintf (out, "Response: [ %02x %02x %02x %02x ]\n", rep[4], rep[5], rep[6], rep[7]);

    return 0;

}

static int do_poweroff_fan (int argc, char *argv[])
{
    struct urv_cmd cmd;

    if (argc == 1) {
        cmd.args[0] = 0;
        cmd.args[1] = 1;
    } else {
        fprintf (out, "usage: monimod-read [off]\n");
        return -1;
    }

    cmd.cmd = CMD_FAN_OFF;
    cmd.len = 4;
    cmd.cb = cb_power_fan;

    if (append_command(ftb, &cmd) != 0)
        return -1;

    return argc;
}

static int do_poweron_fan (int argc, char *argv[])
{
    struct urv_cmd cmd;

    if (argc == 1) {
        cmd.args[0] = 0;
        cmd.args[1] = 1;
    } else {
        fprintf (out, "usage: monimod-read [off]\n");
        return -1;
    }

    cmd.cmd = CMD_FAN_ON;
    cmd.len = 4;
    cmd.cb = cb_power_fan;

    if (append_command(ftb, &cmd) != 0)
        return -1;

    return argc;
}

struct cb_system_data {
  /* Length of the buffer.  */
  unsigned char len;
  /* i2c bus scanned.  */
  unsigned char bus;
  /* Start address of each slave (0 when not present).  */
  unsigned mon_off;
  unsigned ft_off;
  unsigned rtp_off;
  unsigned max_off;
};

static int cb_system_read (struct urv_cmd *cmd, unsigned char *rep)
{
    struct cb_system_data *data = cmd->cb_data;

    if (cmd->args[1] == 0) {
        fprintf (out, "off %u: %02x %02x %02x %02x\n",
                          cmd->args[0], rep[4], rep[5], rep[6], rep[7]);
    }

    if (cmd->args[0] == 0) {
      unsigned off;

      fprintf (out, "Cycle: 0x%02x\n", rep[7]);
      fprintf (out, "Buffer-Length: 0x%02x\n", rep[4]);
      fprintf (out, "Buses: %02x (mon:%x, ft:%x, rtp:%x)\n",
               rep[5],
               (rep[5] >> I2C_SLV_MON) & 1,
               (rep[5] >> I2C_SLV_FT) & 1,
               (rep[5] >> I2C_SLV_RTP) & 1);
      data->len = rep[4];
      data->bus = rep[5];
      off = 1;
      data->mon_off = off;
      if ((data->bus >> I2C_SLV_MON) & 1)
        off += 8;
      data->ft_off = off;
      if ((data->bus >> I2C_SLV_FT) & 1)
        off += 7;
      data->rtp_off = off;
      if ((data->bus >> I2C_SLV_RTP) & 1)
        off += 5;
      data->max_off = off;
      if (data->max_off * 2 != data->len)
        fprintf (out, "Bad length: %u\n", data->max_off);
    }
    else if (cmd->args[0] >= data->rtp_off) {
      unsigned off = cmd->args[0] - data->rtp_off;
      unsigned data;
      switch (off) {
      case 0:
      case 1:
      case 2:
      case 3:
        data = (((unsigned)rep[4] << 8) & 0x0F00) | ((unsigned)rep[5]);
        fprintf (out, "ADC %u: %u\n", off + 4, data);
        break;
      case 4:
        data = ((((unsigned)rep[4] << 8) & 0x0700) | ((unsigned)rep[5]))
          - (C_ADC7291_SIGNED_VAL * ((rep[4] >> 3) & 1));
        fprintf (out, "adcT: %.2f C\n",
                 ((float) data) / (float) (1 << C_ADC7291_FRACTIONAL_BITS));
        break;
      default:
        fprintf (out, "Bad offset for rtp (%u)\n", off);
        break;
      }
    }
    else if (cmd->args[0] >= data->ft_off) {
      unsigned off = cmd->args[0] - data->ft_off;
      switch (off) {
      case 0:
        fprintf (out, "FTP12V: %2.3f V\n", linear16_to_float(rep + 4));
        break;
      case 1:
        fprintf (out, "FTP12V: %4.3f A\n", linear_to_float(rep + 4));
        break;
      case 2:
        fprintf (out, "FTP5V: %2.3f V\n", linear16_to_float(rep + 4));
        break;
      case 3:
        fprintf (out, "FTtemp: %2.3f C\n",  linear_to_float(rep + 4));
        break;
      case 4:
      case 5:
      case 6:
        fprintf (out, "fan %u: %f\n", off - 4 + 1, linear_to_float(rep + 4));
        break;
      default:
        fprintf (out, "Bad offset for ft (%u)\n", off);
        break;
      }
    }
    else if (cmd->args[0] >= data->mon_off) {
      unsigned off = cmd->args[0] - data->mon_off;
      switch (off) {
      case 0:
        fprintf (out, "P12V: %2.3f V\n", linear16_to_float(rep + 4));
        break;
      case 1:
        fprintf (out, "P12V: %4.3f A\n", linear_to_float(rep + 4));
        break;
      case 2:
        fprintf (out, "P1.2V: %2.3f V\n", linear16_to_float(rep + 4));
        break;
      case 3:
        fprintf (out, "P3.3V: %2.3f V\n", linear16_to_float(rep + 4));
        break;
      case 4:
        fprintf (out, "Vperi: %2.3f V\n", linear_to_float(rep + 4));
        break;
      case 5:
        fprintf (out, "Tamb: %2.3f C\n",  linear_to_float(rep + 4));
        break;
      case 6:
        fprintf (out, "Tldo: %2.3f C\n",  linear_to_float(rep + 4));
        break;
      case 7:
        fprintf (out, "Tfpga: %2.3f C\n",  linear_to_float(rep + 4));
        break;
      default:
        fprintf (out, "Bad offset for mon (%u)\n", off);
        break;
      }
    }

    if (cmd->args[1] && ++cmd->args[0] < data->max_off) {
        return 1;
    }

    free(data);
    return 0;
}

static int do_system_read (int argc, char *argv[])
{
    struct urv_cmd cmd;

    if (argc == 1) {
        cmd.args[0] = 0;
        cmd.args[1] = 1;
    } else {
        fprintf (out, "usage: system-read\n");
        return -1;
    }

    cmd.cmd = CMD_MON_BUF;
    cmd.len = 4;
    cmd.cb = cb_system_read;
    cmd.cb_data = xmalloc (sizeof (struct cb_system_data));

    if (append_command(ftb, &cmd) != 0)
        return -1;

    return argc;
}

static int do_close (int argc, char *argv[])
{
  if (!flag_pipe || out == stdout) {
    fprintf (out, "close allowed only with --port\n");
    return -1;
  }
  fclose(out);
  out = stdout;
  return 1;
}

static int do_date (int argc, char *argv[])
{
  fprintf (out, "%s\n", now());
  return 1;
}

struct menu_t {
  const char *name;
  int (*handler)(int, char **);
  const char *help;
};

static int do_help (int argc, char *argv[]);

static const struct menu_t menu [] =
  {
   { "help", do_help, "print this help" },
   { "close", do_close, "close the connection" },
   { "date", do_date, "print current date" },
   { "set-leds", do_set_leds, "set leds" },
   { "get-leds", do_get_leds, "read leds value" },
   { "read", do_read, "read a word" },
   { "dump", do_dump, "dump memory" },
   { "write", do_write, "write a word" },
   { "write-ecc", do_write_ecc, "write a word with ecc (to force error)" },
   { "set-wd", do_set_wd, "set watchdog period" },
   { "ping", do_ping, "ping (send/recv data)" },
   { "data", do_data, "send data to app cpu" },
   { "plc", do_plc, "start/stop plc cpu" },
   { "load", do_load, "load image for plc cpu" },
   { "mbox", do_mbox, "read/write plc mailbox" },
   { "slots", do_slots, "disp slots state" },
   { "supervisor", do_supervisor, "disp supervisor registers" },
   { "plc-supervisor", do_plc_supervisor, "disp PLC supervisor registers" },
   { "stat-regs", do_stat_regs, "disp stat registers" },
   { "fip-err", do_fip_err, "display number of FIP errors" },
   { "fip-reset", do_fip_reset, "do a board reset through fip" },
   { "scrubber", do_scrubber, "enable scrubber" },
   { "mon-vout", do_mon_vout, "display voltage" },
   { "mon", do_mon, "monimod read command" },
   { "wmon", do_wmon, "monimod write command" },
   { "i2c", do_i2c, "select i2c channel" },
   { "i2c-id", do_i2c_id, "read board id using i2c gpio" },
   { "i2c-eeprom", do_i2c_eeprom, "read board id using i2c eeprom" },
   { "i2c-stat", do_i2c_stat, "display i2c fsm status" },
   { "i2c-off", do_i2c_off, "tie i2c lines to 0 register" },
   { "psu", do_psu, "PSU state" },
   { "pwr-cyc", do_pwr_cyc, "power cycle the crate" },
   { "cpu-fault", do_cpu_fault, "force a cpu divergence" },
   { "restart-cfg", do_restart_cfg, "get/set restart config" },
   { "restart", do_restart, "restart the cpus (reload ram)" },
   { "wait", do_wait, "busy waiting loop" },
   { "monimod-power", do_monimod_power, "power on/off monimod" },
   { "monimod-scan", do_monimod_scan, "monimod scan on/off" },
   { "spi-init", do_spi_init, "init SPI periph" },
   { "spi-read", do_spi_read, "read a word from SPI flash" },
   { "spi-dump", do_spi_dump, "dump SPI flash memory" },
   { "spi-flash-elf", do_spi_flash_elf, "flash ELF program to SPI flash" },
   { "spi-page-erase", do_spi_page_erase, "erase SPI page" },
   { "envm-flash-app", do_envm_flash_app, "flash ELF application to eNVM" },
   { "envm-flash-mgmt", do_envm_flash_mgmt, "flash ELF management to eNVM" },
   { "envm-erase-page", do_envm_flash_erase, "erase a page in eNVM" },
   { "read-ratopus", do_ratopus_read, "Read Ratopus Registers"},
   { "write-ratopus", do_ratopus_write, "Write Ratopus Registers"},
   { "poweroff-fantray", do_poweroff_fan, "switch off fantray"},
   { "poweron-fantray", do_poweron_fan, "switch on fantray"},
   { "system-read", do_system_read, "read all scan value" },
   { NULL, NULL, NULL }
  };


static int do_help (int argc, char *argv[])
{
  unsigned i;
  for (i = 0; menu[i].name; i++)
    fprintf (out, "%s - %s\n", menu[i].name, menu[i].help);
  return 1;
}

/* Return the number of arguments consumed.
   0: argument unknown,
   < 0: bad argument (error).  */
static int decode_command (int argc, char *argv[])
{
  unsigned i;
  if (argc == 0)
    return 0;

  for (i = 0; menu[i].name; i++) {
    if (strcmp (argv[0], menu[i].name) == 0)
      return menu[i].handler (argc, argv);
  }
  return 0;
}

static void usage(void)
{
  fprintf (stderr, "usage: fip_urv [OPTIONS] commands\n");
  fprintf (stderr, "options are:\n");
  fprintf (stderr, " -t                Trace\n");
  fprintf (stderr, " -h                Display this help\n");
  fprintf (stderr, " -p                Pipe commands from stdin\n");
  fprintf (stderr, " --port PORT       Get commands from TCP socket\n");
  fprintf (stderr, " --cons-vl LENGTH  Set variable length (default: %u)\n",
           cons_var_length);
  fprintf (stderr, "commands are:\n");
  out = stdout;
  do_help(0, NULL);
}

int main(int argc, char *argv[])
{
    int res;
    struct mstrfip_hw_cfg hw_cfg = {0};
    struct mstrfip_sw_cfg sw_cfg = {0};
    int dev_lun = 0;
    unsigned port = 0;
    int sock = -1;
    int fdin = -1;

    /* Handler  */
    ftb = malloc(sizeof(struct ftb_dev));
    if (ftb == NULL) {
        perror("Can't instantiate FTB device\n");
        exit(1);
    }

    memset(ftb, 0, sizeof(struct ftb_dev));

    ftb->reset = 0;
    ftb->nbr_errors = 0;
    ftb->cycle_uslength = 2000; /* For 124B: 5000, for 16B: 1000 */
    ftb->nbr_cmds = 0;
    ftb->idx_cmd = 0;
    ftb->waitrep = 0;
    ftb->idx = 0x22;

    /* Skip progname.  */
    argc--;
    argv++;

    while (argc > 0 && argv[0][0] == '-') {
      if (strcmp (argv[0], "-t") == 0)
	flag_trace++;
      else if (strcmp (argv[0], "-p") == 0) {
	flag_pipe++;
        fdin = 0;
      }
      else if (strcmp (argv[0], "--cons-vl") == 0) {
        if (argc == 1) {
          fprintf (stderr, "missing argument to %s\n", argv[0]);
          return 1;
        }
        cons_var_length = atoi (argv[1]);
        argc--;
        argv++;
      }
      else if (strcmp (argv[0], "--port") == 0) {
        if (argc == 1) {
          fprintf (stderr, "missing argument to %s\n", argv[0]);
          return 1;
        }
        port = atoi (argv[1]);
        if (port == 0) {
          fprintf (stderr, "bad value for %s\n", argv[0]);
          return 1;
        }
        flag_pipe++;
        argc--;
        argv++;
      }
      else if (strcmp (argv[0], "-h") == 0) {
        usage();
        return 0;
      }
      else {
	fprintf (stderr, "unknown option %s\n", argv[0]);
	return 1;
      }
      argc--;
      argv++;
    }

    if (cons_var_length < 8) {
      fprintf (stderr, "var3 length too short (%u < 8)\n", cons_var_length);
      return 1;
    }

    out = stdout;

    if (flag_pipe) {
      if (argc != 0) {
        fprintf (stderr, "extra commands not allowed with -p or --port\n");
        return 1;
      }
      ftb->idx_cmd = MAX_CMDS + 1;
    }
    else {
      if (argc == 0) {
        fprintf (stderr, "no commands\n");
        return 1;
      }

      while (argc > 0) {
        unsigned clen;
        for (clen = 0; argv[clen] && strcmp(argv[clen], ";"); clen++)
          ;

        if (0) {
          unsigned k;
          fprintf (stderr, "execute:");
          for (k = 0; k < clen; k++)
            fprintf (stderr, " %s", argv[k]);
          fprintf (stderr, "\n");
        }

        res = decode_command (clen, argv);
        if (res == 0) {
          fprintf (out, "%s invalid command '%s'\n", now(), argv[0]);
          return 1;
        }
        else if (res < 0)
          return 1;
        else if (res != clen) {
          fprintf (stderr, "ignored arguments (got %u args, used %u)\n",
                   clen, res);
          return 1;
        }
        argc -= clen;
        argv += clen;
        if (argc == 0)
          break;
        /* Skip ';' */
        argc --;
        argv++;
      }
    }

    if (port != 0) {
      struct sockaddr_in name;

      /* Create the socket. */
      sock = socket (PF_INET, SOCK_STREAM, 0);
      if (sock < 0) {
        perror ("socket");
        exit (1);
      }

      /* Give the socket a name. */
      name.sin_family = AF_INET;
      name.sin_port = htons (port);
      name.sin_addr.s_addr = htonl (INADDR_LOOPBACK);
      if (bind (sock, (struct sockaddr *) &name, sizeof (name)) < 0) {
        perror ("bind");
        exit (1);
      }

      if (listen (sock, 0) < 0) {
        perror ("listen");
        exit (1);
      }

      fdin = -1;
    }

    res = mstrfip_init();
    if (res) {
        fprintf (stderr, "Cannot init fip library: %s\n", mstrfip_strerror(errno));
        exit(1);
    }

    ftb->dev = mstrfip_open_by_lun(dev_lun);
    if (ftb->dev == NULL) {
        fprintf (stderr, "Cannot open mstrfip dev: %s\n", mstrfip_strerror(errno));
        exit(1);
    }

    /* reset RT app */
    if (mstrfip_rtapp_reset(ftb->dev) < 0) {
        fprintf (stderr, "Cannot reset rt app: %s\n", mstrfip_strerror(errno));
        exit(1);
    }

    /* Get FIP bus speed */
    if (mstrfip_hw_speed_get(ftb->dev, &ftb->fbus_speed)) {
        fprintf (stderr, "Can't get FIP speed: %s. Exit\n", mstrfip_strerror(errno));
        exit(1);
    }

    /* Cycle configuration */
    hw_cfg.enable_ext_trig = 0;
    hw_cfg.enable_int_trig = 0;
    hw_cfg.enable_ext_trig_term = 0;
    hw_cfg.turn_around_ustime = 14;         // 14 us corresponds to 1 Mbit/s
    sw_cfg.irq_thread_prio = 50;
    sw_cfg.diag_thread_prio = 20;
    sw_cfg.mstrfip_error_handler = mstrfip_error_handler;

    if (cycle_config(&hw_cfg, &sw_cfg) < 0) {
        fprintf (stderr, "Cannot init ba: %s\n", mstrfip_strerror(errno));
        exit(1);
    }

    /* time to start ba */
    res = ftb_ba_start(ftb);
    if (res) {
        fprintf (stderr, "ba_start: %d %s\n", res, mstrfip_strerror(res));
        exit(1);
    }

    if (flag_pipe) {
        while(1) {
            char buf[128];
            char *bargv[8];
            int i;
            int bargc;
            int len;

            if (ftb->idx_cmd != MAX_CMDS + 1) {
                /* Still executing commands.  */
                usleep (10000);
                continue;
            }

            fflush(out);

            if (fdin < 0) {
              struct sockaddr addr;
              socklen_t alen;

              alen = sizeof(addr);
              fdin = accept (sock, &addr, &alen);
              if (fdin < 0) {
                perror ("accept");
                break;
              }
              out = fdopen(fdin, "w");
            }

            len = read(fdin, buf, sizeof(buf) - 1);
            if (len <= 0) {
                if (port != 0) {
                    fclose(out);
                    out = stdout;
                    fdin = -1;
                    continue;
                }
                else
                    break;
            }
            while (len > 0 && (buf[len - 1] == '\n' || buf[len - 1] == '\r'))
                len--;
            buf[len] = 0;
            bargc = 0;
            i = 0;
            while (1) {
                while (buf[i] == ' ')
                    i++;
                if (buf[i] == 0)
                    break;
                bargv[bargc] = buf + i;
                bargc++;
                while (buf[i] != ' ' && buf[i] != 0)
                    i++;
                if (buf[i] == 0)
                    break;
                buf[i++] = 0;
            }

            ftb->nbr_cmds = 0;

            if (0) {
                fprintf (stderr, "bargc=%d", bargc);
                for (i = 0; i < bargc; i++)
                        fprintf (stderr, " %d:%s", i, bargv[i]);
                fprintf (stderr, "|\n");
            }

            res = decode_command (bargc, bargv);
            if (res == 0) {
                fprintf (out, "# invalid command\n");
                fflush(out);
                continue;
            }
            if (res != bargc) {
                fprintf (out, "# too many arguments\n");
                fflush(out);
                continue;
            }

            /* Will start.  */
            if (ftb->nbr_cmds != 0)
              ftb->idx_cmd = 0;
        }
    }
    else {
        sigset_t newmask;

        /* run for ever till a signal is raised */
        sigemptyset(&newmask); /* unblock any signal */
        sigaddset(&newmask, SIGINT);
        sigaddset(&newmask, SIGTERM);
        sigaddset(&newmask, SIGABRT);
        sigaddset(&newmask, SIGFPE);
        sigaddset(&newmask, SIGILL);
        sigaddset(&newmask, SIGUSR1);
        pthread_sigmask(SIG_BLOCK, &newmask, NULL);

        while(1) {
            siginfo_t siginfo;
            int signo;

            signo = sigwaitinfo(&newmask, &siginfo);
            if(signo > 0)
                break;
        }
    }

    mstrfip_exit();

    /* TO be completed if necessary */
    if (ftb->dev != NULL)
        mstrfip_close(ftb->dev);

    return 0;
}
