/*
 * Copyright (C) 2018 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 <math.h>

#include <masterfip/libmasterfip.h>

#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

static unsigned agent_idx = 1;

#define VAR_LENGTH      8

/* counter for various errors */
struct ftb_var_error {
    int write_failed; /* writing new value failed */
    int missed; /* missed data (not updated) */
    int not_freshed; /* other than 0x5 promptness&significance */
    int not_significant; /* other than 0x5 promptness&significance */
    int tmo; /* time out no reply from agent */
    int hw_err; /* hw bit error in the frame */
    int hw_bsz; /* hw wrong nb of bytes in the frame */
    int hw_ctrl; /* hw bad control byte in the frame */
    int hw_pdu; /* hw bad pdu byte in the frame */
};

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

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

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

    unsigned done;
} *ftb;

/* Trace packets.  */
static int flag_trace;

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

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

    switch (pvar->status) {
    case MSTRFIP_DATA_OK:
      if (flag_trace) {
	printf ("Recv:");
	for (unsigned j = 0; j < VAR_LENGTH; j++)
	  printf(" %02x", pvar->buffer[j]);
	printf("\n");
      }
      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.  */
                    printf ("# 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.  */
	    ftb->idx_cmd++;
            if (flag_pipe && ftb->idx_cmd == ftb->nbr_cmds) {
                    /* Display status.  */
                    printf ("# OK\n");
                    fflush(stdout);
                    ftb->idx_cmd = MAX_CMDS + 1;
            }
	  }
	}
      }
      return 0;

    case MSTRFIP_DATA_PAYLOAD_ERROR:
      if (pvar->payload_error & MSTRFIP_FRAME_PAYLOAD_NOT_REFRESH)
        ++(ftb->var_err.not_freshed);
      if (pvar->payload_error & MSTRFIP_FRAME_PAYLOAD_NOT_SIGNIFICANT)
        ++(ftb->var_err.not_significant);
      return 1;

    case MSTRFIP_DATA_NOT_RECEIVED:
      ++(ftb->var_err.missed);
      return 1;

    case MSTRFIP_DATA_FRAME_ERROR:
      if (pvar->frame_error & MSTRFIP_FRAME_TMO)
        ++(ftb->var_err.tmo);
      if (pvar->frame_error & MSTRFIP_FRAME_ERR)
        ++(ftb->var_err.hw_err);
      if (pvar->frame_error & MSTRFIP_FRAME_BAD_CTRL)
        ++(ftb->var_err.hw_ctrl);
      if (pvar->frame_error & MSTRFIP_FRAME_BAD_PDU)
        ++(ftb->var_err.hw_pdu);
      if (pvar->frame_error & MSTRFIP_FRAME_BAD_BSZ)
        ++(ftb->var_err.hw_bsz);
      return 1;

    default:
      return 1;
    }
 }
#endif

static int update_vars(void)
{
    int res;
    struct mstrfip_data *pvar = ftb->ba.prod_varlist[0];

    if (!ftb->done) {
        pvar->bsz = 4;
        pvar->buffer[0] = agent_idx;
        pvar->buffer[1] = agent_idx;
        printf ("Send packet\n");
        ftb->done = 1;
    }
    else {
        /* End of command list.  */
        kill(getpid(), SIGINT);
        return 0;
    }

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

#if 0
static void mstrfip_var_handler(struct mstrfip_dev *dev, struct mstrfip_data *pvar,
                                struct mstrfip_irq *irq)
{
    int err1 = read_vars();
    int err2 = 0;

    if (err1 || err2) {
        printf ("var_handler: error\n");
        if (ftb->var_err.not_freshed)
          printf(" agent not_freshed: %d\n", ftb->var_err.not_freshed);
        if (ftb->var_err.not_significant)
          printf(" agent not_significant: %d\n", ftb->var_err.not_significant);
        if (ftb->var_err.missed)
          printf(" agent missed: %d\n", ftb->var_err.missed);
        if (ftb->var_err.tmo)
          printf(" agent tmo: %d\n", ftb->var_err.tmo);
        if (ftb->var_err.hw_err)
          printf(" agent hw_err: %d\n", ftb->var_err.hw_err);
        if (ftb->var_err.hw_ctrl)
          printf(" agent hw_ctrl: %d\n", ftb->var_err.hw_ctrl);
        if (ftb->var_err.hw_pdu)
          printf(" agent hw_pdu: %d\n", ftb->var_err.hw_pdu);
        if (ftb->var_err.hw_bsz)
          printf(" agent hw_bsz: %d\n", ftb->var_err.hw_bsz);
    }
}
#endif

static void mstrfip_prod_var_handler(struct mstrfip_dev *dev, struct mstrfip_data *pvar,
                                     struct mstrfip_irq *irq)
{
    if (1) {
      printf("prod_var_handler\n");
      printf(" 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);
    }
    update_vars ();
}

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

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

    /* build prod varlist (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 = mstrfip_prod_var_handler;
    ba->prod_varlist[0] = mstrfip_var_create(ba->mc, &var_cfg);
    if (ba->prod_varlist[0] == NULL)
      return -1;

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

    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 ftb_set_ba_instr(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; /* periodic var list */
    pwind_cfg.var_count = 1; /* periodic var count */
    if (mstrfip_per_var_wind_append(ba->mc, &pwind_cfg) < 0)
        return -1;


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

    /* append a wait window without padding (silent) */
    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_set_ba_instr(&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;
}

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;

    /* 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->cycle_uslength = 5000;
    ftb->done = 0;

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

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

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

    {
        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;
}
