/*
  gcc -Wall -o saltrobcmem saltrobcmem.c
*/
#include <getopt.h>
#include <signal.h>
#include <stdio.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>

#define ALTRO_ZSTHR     0x08
#define ALTRO_BCTHR     0x09
#define ALTRO_TRCFG     0x0A
#define ALTRO_DPCFG     0x0B
#define ALTRO_BFNPT     0x0C
#define ALTRO_PMADD     0x0D
#define ALTRO_ERSTR     0x10
#define ALTRO_TRCNT     0x12


#define SALTRO_PMADD     0x0D
#define SALTRO_PMDTA    0x14

/* ALTRO lookup table */
#define ALTRO_LOOKUP_NONE           0
#define ALTRO_LOOKUP_RAMPUP         1
#define ALTRO_LOOKUP_RAMPDOWN       2
#define ALTRO_LOOKUP_2AA            4
#define ALTRO_LOOKUP_155            5
#define ALTRO_LOOKUP_PEAKS          6
#define ALTRO_LOOKUP_FILE           7
#define ALTRO_LOOKUP_ZERO           8
#define ALTRO_LOOKUP_3FF            9
#define ALTRO_LOOKUP_WALK1         10
#define ALTRO_LOOKUP_RANDOM        11
#define ALTRO_LOOKUP_CH            12
#define ALTRO_LOOKUP_155_2AA       13
#define ALTRO_LOOKUP_VALUE         14
#define ALTRO_LOOKUP_3FF_000       15


#define FEC 1
#define CHIP 0
//#define LOCAL_IP "130.235.185.193"
//#define SRU_IP "130.235.185.193"
#define LOCAL_IP "192.168.4.11"
#define SRU_IP "192.168.4.4"

#define SRU_WAIT_WRITE 500


unsigned int  valuerd,valuewr;
int broadcast;
int fec;
int chip;
int pattern;
int signal_stop = 0;
char inputfile[256];

typedef struct {
  char controlip[64];
  int controlport;
  struct sockaddr_in si_sru;
  int scsocket; // socket for slow control
  int listenport;
  char readoutip[64];
  unsigned int readoutipnb;
  int status; // status from configfile
  int linkup; // it it answered ping
  int active; // status & linkup is set and FMVER could be read
} SRUSERVER;


typedef struct {
  struct sockaddr_in si_controlme;
  int controlsocket; // socket for control
  int listenport; // port for listen on control
  int nbrdpackets; // Number of packets expected back
  SRUSERVER sruserver;
} SRU;

int scSruConnect(SRU *srus);
int saltroWriteRegister(int handle,SRUSERVER *srv,int broadcast, int node, int chip, int channel, int reg, unsigned int value);
int saltroReadRegister(int handle, SRUSERVER *srv, int node, int chip, int channel, int reg, unsigned int *value);
void signal_handler(int signum);
int decodeParameters(int argc, char **argv);
int dumpLookupTable(char *filename,unsigned int *buffer);
int saltroWriteLookupTable(int dev,SRUSERVER *srv,int type,int broadcast,int mcm, int chip, int channel);
int saltroPedmemBlock(int handle,SRUSERVER *srv,int broadcast, int node, int chip, unsigned int address, unsigned int value);
int saltroPedMulitiBlock(int handle,SRUSERVER *srv,int broadcast, int node, int chip, unsigned int address, int nbval, unsigned int *value);


/* SRU part */
int scSruConnect(SRU *srus)
{
  int s;
  struct timeval timeout;
  timeout.tv_sec = 0;
  timeout.tv_usec = 500000;

  if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1) {
    printf("%s: socket error %s\n",__FUNCTION__,strerror(errno));
    return -1;
  }
  setsockopt(s, SOL_SOCKET, SO_RCVTIMEO,&timeout,sizeof(timeout));
  memset((char *) &srus->si_controlme, 0, sizeof(struct sockaddr));
  srus->si_controlme.sin_family = AF_INET;
  srus->si_controlme.sin_port = htons(srus->listenport);
  if (inet_aton(LOCAL_IP, &srus->si_controlme.sin_addr)==0) {
    printf("%s: inet_aton error %s\n",__FUNCTION__,strerror(errno));	
    close(s);
    return -2;
  }
  if (bind(s, (struct sockaddr *)&srus->si_controlme, sizeof(struct sockaddr)) != 0) {
    printf("%s: bind error %s\n",__FUNCTION__,strerror(errno));	
    close(s);
    return -3;
  }
  srus->controlsocket = s;

  memset((char *) &srus->sruserver.si_sru, 0, sizeof(struct sockaddr_in));
  srus->sruserver.si_sru.sin_family = AF_INET;
  srus->sruserver.si_sru.sin_port = htons(srus->sruserver.controlport);
  if (inet_aton(srus->sruserver.controlip, &srus->sruserver.si_sru.sin_addr)==0) {
    printf("%s: sru inet_aton error %s\n",__FUNCTION__,strerror(errno));	
    close(s);
    srus->controlsocket = 0;
    return -4;
  }
  return 0;
}


/* SALTRO part */
int saltroWriteRegister(int handle,SRUSERVER *srv,int broadcast, int node, int chip, int channel, int reg, unsigned int value)
{
  unsigned int buf[4];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  unsigned int myreg;


  if (node >= 20) {
    buf[0] = htonl(1 << (node - 20));
    buf[1] = 0;
  }
  else {
    buf[0] = 0;
    buf[1] = htonl(1 << node);
  }
  myreg = (broadcast << 18) | (node << 12) | (chip << 9) | (channel << 5) | reg;
  buf[2] = htonl(myreg | 0x40000000);
  buf[3] = htonl(value);
  s_bytes = 16;
  if ( (s_sent = sendto(handle, buf, 16, 0, (struct sockaddr *)&srv->si_sru, s_length)) ==-1) {
    return -1;
  }
  if (s_sent != s_bytes) {
    return -2;
  }
  usleep(SRU_WAIT_WRITE);
  return 0;
}


int saltroPedmemBlock(int handle,SRUSERVER *srv,int broadcast, int node, int chip, unsigned int address, unsigned int value)
{
  unsigned int buf[512];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  struct sockaddr_in si_from;
  socklen_t si_fromlen=sizeof(si_from);
  unsigned int myreg;
  int channel;
  int pos = 0;
  
  if (node >= 20) {
    buf[pos++] = htonl(1 << (node - 20));
    buf[pos++] = 0;
  }
  else {
    buf[pos++] = 0;
    buf[pos++] = htonl(1 << node);
  }

  myreg = (broadcast << 18) | (node << 12) | (chip << 9) | SALTRO_PMADD;
  buf[pos++] = htonl(myreg | 0x40000000);
  buf[pos++] = htonl(address);
  
  for (channel = 0; channel < 16; channel++) {
    myreg = (broadcast << 18) | (node << 12) | (chip << 9) | (channel << 5) | SALTRO_PMDTA;
    buf[pos++] = htonl(myreg | 0x40000000);
    buf[pos++] = htonl(value);
  }
  // read instrucions - to make sure all commands done
  myreg = (node << 12) | (chip << 9) | SALTRO_PMADD;
  buf[pos++] = htonl(myreg | 0xC0000000);
  buf[pos++] = 0;
 
  
  s_bytes = pos * 4;
  if ( (s_sent = sendto(handle, buf, s_bytes, 0, (struct sockaddr *)&srv->si_sru, s_length)) ==-1) {
    return -1;
  }
  if (s_sent != s_bytes) {
    return -2;
  }
  usleep(SRU_WAIT_WRITE);

  if ((s_bytes = recvfrom(handle, buf,sizeof(buf), 0, (struct sockaddr *) &si_from, &si_fromlen))==-1) {
    perror("-3");
    return -3;
  }


  return 0;
}


int saltroPedMultiBlock(int handle,SRUSERVER *srv,int broadcast, int node, int chip, unsigned int address,  int nbval, unsigned int *value)
{
  unsigned int buf[512];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  struct sockaddr_in si_from;
  socklen_t si_fromlen=sizeof(si_from);
  unsigned int myreg;
  int channel;
  int pos = 0;
  unsigned int *ptr;
  ptr = value;
  if (node >= 20) {
    buf[pos++] = htonl(1 << (node - 20));
    buf[pos++] = 0;
  }
  else {
    buf[pos++] = 0;
    buf[pos++] = htonl(1 << node);
  }
  for (int j = 0; j < nbval; j++) {
    myreg = (broadcast << 18) | (node << 12) | (chip << 9) | SALTRO_PMADD;
    buf[pos++] = htonl(myreg | 0x40000000);
    buf[pos++] = htonl(address++);

    for (channel = 0; channel < 16; channel++) {
      myreg = (broadcast << 18) | (node << 12) | (chip << 9) | (channel << 5) | SALTRO_PMDTA;
      buf[pos++] = htonl(myreg | 0x40000000);
      buf[pos++] = htonl(ptr[j]);
    }
  }
  // read instrucions - to make sure all commands done
  myreg = (node << 12) | (chip << 9) | SALTRO_PMADD;
  buf[pos++] = htonl(myreg | 0xC0000000);
  buf[pos++] = 0;
 
  
  s_bytes = pos * 4;
  if ( (s_sent = sendto(handle, buf, s_bytes, 0, (struct sockaddr *)&srv->si_sru, s_length)) ==-1) {
    return -1;
  }
  if (s_sent != s_bytes) {
    return -2;
  }
  usleep(SRU_WAIT_WRITE);

  if ((s_bytes = recvfrom(handle, buf,sizeof(buf), 0, (struct sockaddr *) &si_from, &si_fromlen))==-1) {
    perror("-3");
    return -3;
  }

  return 0;
}



int saltroReadRegister(int handle, SRUSERVER *srv, int node, int chip, int channel, int reg, unsigned int *value)
{
  unsigned int buf[20];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  struct sockaddr_in si_from;
  socklen_t si_fromlen=sizeof(si_from);
  unsigned int myreg,mynode;
  unsigned int myregrd;
  if (node >= 20) {
    buf[0] = htonl(1 << (node - 20));
    buf[1] = 0;
  }
  else {
    buf[0] = 0;
    buf[1] = htonl(1 << node);
 }

  myregrd = (node << 12) | (chip << 9) | (channel << 5) | reg;
  buf[2] = htonl(myregrd | 0xC0000000);
  buf[3] = 0;
  s_bytes = 16;
  if ( (s_sent = sendto(handle, buf, 16, 0, (struct sockaddr *)&srv->si_sru, s_length)) ==-1) {
    return -1;
  }
  if (s_sent != s_bytes) {
    return -2;
  }
  if ((s_bytes = recvfrom(handle, buf,sizeof(buf), 0, (struct sockaddr *) &si_from, &si_fromlen))==-1) {
    perror("-3");
    return -3;
  }
  if (s_bytes != 12) {
    return -4;
  }

  if (si_from.sin_addr.s_addr != srv->si_sru.sin_addr.s_addr) {
    printf("%s: Received illegal packet (lengh = %d) from %s: %d\n",__FUNCTION__,s_bytes, 
	   inet_ntoa(si_from.sin_addr), ntohs(si_from.sin_port));
    return -5;
  }
 
  mynode = ntohl(buf[0]);
  if (mynode != node) {
    printf("%s: Received not expected node %08X expected %08X\n",
	   __FUNCTION__,mynode,node);
    return -6;
  }
  myreg = ntohl(buf[1]);
  if (myreg != (myregrd | 0xC0000000)) {
    printf("%s: Received not expected register %08X expected %08X\n",
	   __FUNCTION__,myreg,myregrd | 0xC0000000);
    return -7;
  }
  *value = ntohl(buf[2]);

  return 0;
}


void signal_handler(int signum)
{
  switch(signum) {
  case SIGQUIT:
  case SIGTERM:
  case SIGINT:
    puts("\nSIGTERM");
    signal_stop = 1; break;
    /*
  case SIGTERM:
  case SIGQUIT: break;
    */
  }
}

int decodeParameters(int argc, char **argv)
{
  int c;
  int option_index = 0;
  char *short_options = "bC:f:hi:l:p:r:Rvw:";
  const struct option long_options[]=
  {
    {"broadcast",0,0,'b'},
    {"chip",1,0,'C'},
    {"fec",1,0,'f'},
    {"help",0,0,'h'},
    {"infile",1,0,'i'},
    {"pattern",1,0,'p'},
    {0, 0, 0, 0}
  };
  broadcast = 0;
  chip = CHIP;
  fec = FEC;
  pattern = 0;
  inputfile[0] = '\0';
  if (argc > 1)
  {
    while ((c = getopt_long(argc, argv, short_options,
                                        long_options, &option_index)) != EOF)
    {
      switch (c)
      {
      case 'b': broadcast = 1; break;
      case 'C': sscanf(optarg,"%d",&chip); break;
      case 'f': sscanf(optarg,"%d",&fec); break;
      case 'h':
	puts("Options");
	puts("--broadcast");
	puts("--fec <fec>");
	puts("--chip <chip>");
	puts("--infile <file to read>");
	puts("--pattern <pattern to load>");
	return 1;
	break;
      case 'i': strncpy(inputfile,optarg,sizeof(inputfile)); break;
      case 'p': sscanf(optarg,"%d",&pattern); break;
      default: printf("Unkown paramater\n"); return 1;
      }
    }
  }
  if ((pattern == ALTRO_LOOKUP_FILE) && (inputfile[0] == '\0')) {
    printf("One must give inputfile (%s) when pattern = 7\n",inputfile); 
    return 1;
  }
  return 0;
}

int dumpLookupTable(char *filename,unsigned int *buffer)
{
  FILE *fout;
  int j;
  if ((fout = fopen(filename,"w")) == NULL) return -1;
  for (j = 0; j < 1024; j++) {
    fprintf(fout,"%4d %4d\n",j,buffer[j]);
  }
  fclose(fout);
  return 0;
}


int saltroWriteLookupTable(int dev,SRUSERVER *srv,int type,int broadcast,int mcm, int chip, int channel)
{
  int j;
  int status = 0;
  unsigned int buffer[1024];
  char name[128];

  /* Generate or read a lookup table, dump it in /tmp/lookuptable.dat and load it */

  printf("%s: for FEC %d CHIP %d CH %d TYPE = %d BCAST = %d \n",__FUNCTION__,mcm,chip,channel,type,broadcast);
  if (type == ALTRO_LOOKUP_NONE) return status;

  if (type == ALTRO_LOOKUP_RAMPUP) {
     for (j = 0; j < 1024; j++) buffer[j] = j;
  }
  else if (type == ALTRO_LOOKUP_RAMPDOWN) {
     for (j = 0; j < 1024; j++) buffer[1023-j] = j;
  }
  else if (type == ALTRO_LOOKUP_2AA) {
     for (j = 0; j < 1024; j++) buffer[j] = 0x2AA;
  }
  else if (type == ALTRO_LOOKUP_155) {
     for (j = 0; j < 1024; j++) buffer[j] = 0x155;
  }
  else if (type == ALTRO_LOOKUP_155_2AA) {
    for (j = 0; j < 1024; j++) {
      if ((j & 4) == 0) buffer[j] = 0x155;
      else  buffer[j] = 0x2AA;
    }
  }
  else if (type == ALTRO_LOOKUP_3FF_000) {
    for (j = 0; j < 1024; j++) {
      if ((j & 1) == 0) buffer[j] = 0x00;
      else  buffer[j] = 0x1FF;
    }
  }
  else if (type == ALTRO_LOOKUP_PEAKS) {
    for (j = 0; j < 64; j++) buffer[j] = j*2;
    for (j = 64; j < 128; j++) buffer[j] = 0;
    for (j = 128; j < 196; j++) buffer[j] = 0x2AA;
    for (j = 196; j < 256; j++) buffer[j] = 0;
    for (j = 256; j < 260; j++) buffer[j] = 0x155;
    for (j = 260; j < 512; j++) buffer[j] = 0;
    for (j = 512; j < 900; j++) buffer[j] = j;
    for (j = 900; j < 1024; j++) buffer[j] = 0;
  }
  else if (type == ALTRO_LOOKUP_FILE) {
    FILE *infile;
    int sample;
    infile = fopen(inputfile,"r");
    if (infile == NULL) {
      printf("Could not open input file %s\n",inputfile);
      return type;
    }
    for (j = 0; j < 1024; j++) {
      fscanf(infile,"%d %d",&sample,&buffer[j]);
    }
    fclose(infile);
  }
  else if (type == ALTRO_LOOKUP_ZERO) {
    for (j = 0; j < 1024; j++) buffer[j] = 0;
  }
  else if (type == ALTRO_LOOKUP_3FF) {
    for (j = 0; j < 1024; j++) buffer[j] = 0x3FF;
  }
  else if (type == ALTRO_LOOKUP_WALK1) {
    for (j = 0; j < 1024; j++) buffer[j] = (1 << (j % 10));
  }
  else if (type == ALTRO_LOOKUP_RANDOM) {
    for (j = 0; j < 1024; j++) buffer[j] = rand() & 0x3FF;
  }
  else if (type == ALTRO_LOOKUP_CH) {
    for (j = 0; j < 1024; j++) buffer[j] = 1 + channel;
  }
  else if (type == ALTRO_LOOKUP_VALUE) {
    for (j = 0; j < 1024; j++) buffer[j] = valuewr;
  }
  else { // Default is rampup
    for (j = 0; j < 1024; j++) buffer[j] = j;
  }
  sprintf(name,"/tmp/lookuptable-%d-%d-%d-%d.dat",mcm,chip,channel,type);
  dumpLookupTable(name,buffer);

  if (channel == -3) {
    for (j = 0; j < 1024; j+=8) {
      status = saltroPedMultiBlock(dev,srv,broadcast,fec,chip,j,8,&buffer[j]);
    }
    return status;
  }

  for (j = 0; j < 1024; j++) {
    // Write address in PMADD of ALTRO chip 
    status = saltroWriteRegister(dev,srv,broadcast,fec,chip,0,SALTRO_PMADD,j);
    if (status != 0) return (j + (chip + 1) * 10000);
    // write value into PMDTA
    if (channel > 0) {
      status = saltroWriteRegister(dev,srv,broadcast,fec,chip,channel,SALTRO_PMDTA,buffer[j]);
      if (status != 0) return (j + (chip + 1) * 10000);
    }
    else if (channel == -1) {
      for (int k = 0; k < 16; k++) {
	status = saltroWriteRegister(dev,srv,broadcast,fec,chip,k,SALTRO_PMDTA,buffer[j]);
	if (status != 0) return (j + (chip + 1) * 10000);
      }
    }
    else if (channel == -2) {
      status = saltroPedmemBlock(dev,srv,broadcast,fec,chip,j,buffer[j]);
    }
  }
  
  return status;
}

int main(int argc, char **argv){
  int j;
  SRU srus;
  int errst;
  unsigned int dpcfg;
  int status = 0;

    signal(SIGINT,signal_handler);
    signal(SIGQUIT,signal_handler);
    signal(SIGTERM,signal_handler);

    strcpy(srus.sruserver.controlip,SRU_IP);
    srus.sruserver.controlport = 4097;
    srus.listenport = 4100;

    if (decodeParameters(argc,argv)) return 0;
    printf("Parameters: FEC= %d CHIP %d PATTERN %X BCAST %d\n",
	   fec,chip,pattern,broadcast);

    errst = scSruConnect(&srus);
    if (errst != 0) return(0);
     puts("\nSTART");
     /* Remember current DPCFG and write mode fin - fpd */
     status = saltroReadRegister(srus.controlsocket,&srus.sruserver,fec,chip,0,ALTRO_DPCFG,&dpcfg);
     if (status != 0) {
       printf ("STATUS1 %d\n",status);
       close(srus.controlsocket);
       return(status);
     }
     printf("%s: DPCFG %X\n",__FUNCTION__,dpcfg);
     status = saltroWriteRegister(srus.controlsocket,&srus.sruserver,broadcast,fec,chip,0,ALTRO_DPCFG,dpcfg & 0xFFFFFFF0);
     if (status != 0) {
       printf ("STATUS2 %d\n",status);
       close(srus.controlsocket);
       return(status);
     }

     if (broadcast == 0) {
       for ( j= 0; j < 16; j++) {
	 saltroWriteLookupTable(srus.controlsocket,&srus.sruserver,pattern,0,fec,chip,j);
       }
     }
     else {
	 saltroWriteLookupTable(srus.controlsocket,&srus.sruserver,pattern,1,fec,chip,-3);
     }

    /* Write back DPCFG setting */
    status = saltroWriteRegister(srus.controlsocket,&srus.sruserver,broadcast,fec,chip,0,ALTRO_DPCFG,dpcfg);
    printf ("STATUS3 %d\n",status);
    if (status != 0) return (-3 - chip * 10);
    status = saltroReadRegister(srus.controlsocket,&srus.sruserver,fec,chip,0,ALTRO_DPCFG,&dpcfg);
    printf ("STATUS4 %d\n",status);
    printf("%s: DPCFG %X\n",__FUNCTION__,dpcfg);

    close(srus.controlsocket);
    
    puts("\nEXIT");
    return 0;
}