/*

program to write to CPLD registers of which channels to read.

Modify 
LOCAL_IP
SRU_IP
to proper addresses

modify
SRU_WAIT_WRITE to test the needed time between writes.

compile:
gcc -Wall -o mcmpca16 mcmpca16.c

write and read and verify
./mcmpca16 --fec [0-31] --preamp [0-1] --gain [0-3] --shaper [0-7] --shutdown [0-1] --polarity [0-1] --biasctrl [0:1] --grst [0:1] --write --verify --alternate

read and verify previuosly written value
./mcmpca16 --fec [0-31] --preamp [0-1] --gain [0-3] --shaper [0-7] --shutdown [0-1] --polarity  [0-1] --verify

argument --loop <nb> to do many times, 0 = inifinite
the fec argument can be repeated, so several mcm can be loaded.

PasaCtrl bits:
0 - preampenable
1 - Gain2
2 - Gain1
3 - Shaper3 0= 120ns,1 = 90 , 3= 60, 7 = 30 ns
4 - Shaper2
5 - Shaper1
6 - Shutdown
7 - Polarity
8 - global reset (always 1)
9 - biascontrol (always 1)

Register to use:
#define MCM_REG_SALTROCTRL  0x06

NOTE: DecayBias is set externally


*/

#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>

#define __DEBUG 1

#define FEC 1
#define CHIP 0
#define CHANNEL 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 BC_IP "192.168.4.255"


// microseconds delay between SRUSC writes
// no need for this when reading, there is a timeout of 100 milliseconds
// set in the socket.
//#define SRU_WAIT_WRITE 100
#define SRU_WAIT_WRITE 200

#define MCM_REG_SALTROCTRL  0x06

unsigned int readout;
unsigned int pasactrl;

unsigned int  valuerd,valuewr;
int dovalue = 0;
int setvalue;
int fec;
int loop;
int verify;
int signal_stop = 0;
int dowrite;
int sruwr;
int doreset = 0;
int alternate = 0;
int grst = 1;
int biasctrl = 1;

#define PASACTRL_PREAMP 0
#define PASACTRL_GAIN 1
#define PASACTRL_GAIN2 1
#define PASACTRL_GAIN1 2
#define PASACTRL_SHAPER 3
#define PASACTRL_SHAPER3 3
#define PASACTRL_SHAPER2 4
#define PASACTRL_SHAPER1 5
#define PASACTRL_SHUTDOWN 6
#define PASACTRL_POLARITY 7
/*
#define PASACTRL_GRST 8
#define PASACTRL_BIASCTRL 9
*/
#define PASACTRL_BIASCTRL 8
#define PASACTRL_GRST 9

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;

unsigned int aclwr[8];
unsigned int aclrd[8];


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

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

/* MCM part */
int mcmWriteRegister(int handle,SRUSERVER *srv,int node, int reg, unsigned int value)
{
  unsigned int buf[4];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);

  if (node >= 20) {
    buf[0] = htonl(1 << (node - 20));
    buf[1] = 0;
  }
  else {
    buf[0] = 0;
    buf[1] = htonl(1 << node);
  }
  buf[2] = htonl(reg);
  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 mcmReadRegister(int handle,SRUSERVER *srv,int node, 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;
  if (node >= 20) {
    buf[0] = htonl(1 << (node - 20));
    buf[1] = 0;
  }
  else {
    buf[0] = 0;
    buf[1] = htonl(1 << node);
 }
  buf[2] = htonl((unsigned int) reg | 0x80000000);
  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) {
    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 packet length %d from %s: %d\n",__FUNCTION__,s_bytes, 
             inet_ntoa(si_from.sin_addr), ntohs(si_from.sin_port));
  }
  else {
    printf("%s: Received illegal packet length %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 != ((unsigned int) reg | 0x80000000)) {
    printf("%s: Received not expected register %08X expected %08X\n",
	   __FUNCTION__,myreg,(unsigned int) reg | 0x80000000);
    return -7;
  }
  *value = ntohl(buf[2]);
  printf("%s: %08X %08X %08X\n",__FUNCTION__,mynode,myreg,*value);
  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 iarg;
  int option_index = 0;
  char *short_options = "ab:f:G:g:hl:P:p:rS:s:vV:w";
  const struct option long_options[]=
  {
    {"alternate",0,0,'a'},
    {"biasctrl",1,0,'b'},
    {"fec",1,0,'f'},
    {"gain",1,0,'g'},
    {"grst",1,0,'G'},
    {"help",0,0,'h'},
    {"loop",1,0,'l'},
    {"polarity",1,0,'p'},
    {"preamp",1,0,'P'},
    {"reset",0,0,'r'},
    {"shaper",1,0,'S'},
    {"shutdown",1,0,'s'},
    {"write",0,0,'w'},
    {"verify",0,0,'v'},
    {"value",1,0,'V'},
    {0, 0, 0, 0}
  };
  readout = 0;
  pasactrl = (1 << PASACTRL_GRST) | (1 << PASACTRL_BIASCTRL);

  fec = FEC;
  loop = 1;
  verify = 0;
  dowrite = 0;
  if (argc > 1)
  {
    while ((c = getopt_long(argc, argv, short_options,
                                        long_options, &option_index)) != EOF)
    {
      switch (c)
      {
      case 'a': alternate = 1; break;
      case 'f':
	sscanf(optarg,"%d",&fec); 
	readout |= (1 << fec);
	break;
      case 'b':
	sscanf(optarg,"%d",&iarg);
	if (iarg != 0) pasactrl |= (1 <<  PASACTRL_BIASCTRL);
	else pasactrl &= ~(1 <<  PASACTRL_BIASCTRL);
	break;
      case 'G':
	sscanf(optarg,"%d",&iarg);
	if (iarg != 0) pasactrl |= (1 <<  PASACTRL_GRST);
	else pasactrl &= ~(1 <<  PASACTRL_GRST);
	break;
      case 'g':
	sscanf(optarg,"%d",&iarg);
	iarg &= 3;
	pasactrl &= ~(3 <<  PASACTRL_GAIN);
	pasactrl |= (iarg <<  PASACTRL_GAIN);
	break;
      case 'h':
	puts("Options");
	puts("--fec <fec>");
	puts("--write");
	puts("--loop <number of loops, 0 = infinity>");
	puts("--verify <verify written value>");
	return 1;
	break;
      case 'l': sscanf(optarg,"%d",&loop); break;
      case 'P':
	sscanf(optarg,"%d",&iarg);
	if (iarg != 0) pasactrl |= (1 <<  PASACTRL_PREAMP);
	else pasactrl &= ~(1 <<  PASACTRL_PREAMP);
	break;
      case 'p':
	sscanf(optarg,"%d",&iarg);
	if (iarg != 0) pasactrl |= (1 <<  PASACTRL_POLARITY);
	else pasactrl &= ~(1 <<  PASACTRL_POLARITY);
	break;
      case 'r':
	doreset = 1;
	break;
      case 'S':
	sscanf(optarg,"%d",&iarg);
	iarg &= 7;
	pasactrl &= ~(7 <<  PASACTRL_SHAPER);
	pasactrl |= (iarg <<  PASACTRL_SHAPER);
	break;
      case 's':
	sscanf(optarg,"%d",&iarg);
	if (iarg != 0) pasactrl |= (1 <<  PASACTRL_SHUTDOWN);
	else pasactrl &= ~(1 <<  PASACTRL_SHUTDOWN);
	break;
      case 'v': verify = 1; break;
      case 'V': dovalue = 1; sscanf(optarg,"%X",&setvalue); break;
      case 'w': dowrite = 1; break;
      default: printf("Unkown paramater\n"); return 1;
      }
    }
  }
  return 0;
}

int mcmLoadPca16(int handle,SRUSERVER *srv,unsigned int nodes, unsigned int pasactrl, unsigned int *error)
{
  int n;
  int nberr = 0;
  int ret1;
  int iarg;
  *error = 0;

  iarg = (pasactrl >>  PASACTRL_GAIN) & 3;
  if ((iarg & 1) != 0)  pasactrl |= (1 <<  PASACTRL_GAIN1);
  else   pasactrl &= ~(1 <<  PASACTRL_GAIN1);
  if ((iarg & 2) != 0) pasactrl |= (1 <<  PASACTRL_GAIN2);
  else   pasactrl &= ~(1 <<  PASACTRL_GAIN2);
  /*
  iarg = (pasactrl >>  PASACTRL_SHAPER) & 7;
  if ((iarg & 1) != 0)  pasactrl |= (1 <<  PASACTRL_SHAPER1);
  else   pasactrl &= ~(1 <<  PASACTRL_SHAPER1);
  if ((iarg & 2) != 0) pasactrl |= (1 <<  PASACTRL_SHAPER2);
  else   pasactrl &= ~(1 <<  PASACTRL_SHAPER2);
  if ((iarg & 4) != 0) pasactrl |= (1 <<  PASACTRL_SHAPER3);
  else   pasactrl &= ~(1 <<  PASACTRL_SHAPER3);
  */
  for (n = 0; n <= 31; n++) {
    if ((nodes & (1 << n)) != 0) {
      ret1 = mcmWriteRegister(handle,srv,n,MCM_REG_SALTROCTRL,pasactrl);
      if (ret1 != 0) {
	nberr++;
	*error |= (1 << n);
#ifdef __DEBUG
	printf("%s:  mcmWriteRegister node %d error %d\n",__FUNCTION__,n,ret1);
#endif
      }
    }
  }
  return nberr;
}
 
int mcmVerifyPca16(int handle,SRUSERVER *srv,unsigned int nodes, unsigned int pasactrl, unsigned int *error)
{
  int n;
  int nberr = 0;
  int ret1;
  int iarg;
  unsigned int value;
  *error = 0;


  iarg = (pasactrl >>  PASACTRL_GAIN) & 3;
  if ((iarg & 1) != 0)  pasactrl |= (1 <<  PASACTRL_GAIN1);
  else   pasactrl &= ~(1 <<  PASACTRL_GAIN1);
  if ((iarg & 2) != 0) pasactrl |= (1 <<  PASACTRL_GAIN2);
  else   pasactrl &= ~(1 <<  PASACTRL_GAIN2);
  /*
  iarg = (pasactrl >>  PASACTRL_SHAPER) & 7;
  if ((iarg & 1) != 0)  pasactrl |= (1 <<  PASACTRL_SHAPER1);
  else   pasactrl &= ~(1 <<  PASACTRL_SHAPER1);
  if ((iarg & 2) != 0) pasactrl |= (1 <<  PASACTRL_SHAPER2);
  else   pasactrl &= ~(1 <<  PASACTRL_SHAPER2);
  if ((iarg & 4) != 0) pasactrl |= (1 <<  PASACTRL_SHAPER3);
  else   pasactrl &= ~(1 <<  PASACTRL_SHAPER3);
  */
  for (n = 0; n <= 31; n++) {
    if ((nodes & (1 << n)) != 0) {
      ret1 = mcmReadRegister(handle,srv,n,MCM_REG_SALTROCTRL,&value);
      if (ret1 != 0) {
	nberr++;
	*error |= (1 << n);
#ifdef __DEBUG
	printf("%s:  mcmReadRegister node %d register %d error %d (%s)\n",__FUNCTION__,n,MCM_REG_SALTROCTRL,ret1,strerror(errno));
#endif
      }
      else if (value != pasactrl) {
	nberr++;
	*error |= (1 << n);
#ifdef __DEBUG
	printf("%s:  mcmReadRegister node %d register %d read %X expected %X\n",__FUNCTION__,n,MCM_REG_SALTROCTRL,value,pasactrl);
#endif
      }
    }
  }
  return nberr;
}

int main(int argc, char **argv){

  SRU srus;
  int cont,ret1,ret2;
  unsigned int value;
  unsigned int errst1,errst2;

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

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

    memset(aclwr,0,sizeof(aclwr));
    memset(aclrd,0,sizeof(aclrd));
    if (decodeParameters(argc,argv)) return 0;
    if (dovalue != 0) pasactrl = 0x300 | setvalue;
    printf("Parameters: FEC= %X LOOP %d VER %d WRITE %d PASACTRL %X setvalue %X:\n",
	   readout,loop,verify,dowrite,pasactrl,setvalue);
    putchar('\n');

    errst1 = scSruConnect(&srus);
    if (errst1 != 0) {
      printf("exit after scSruConnect\n");
      return(0);
    }

    if (doreset != 0) {
      puts("RESET SALTRO");
      ret1 = mcmReadRegister(srus.controlsocket,&srus.sruserver,0,MCM_REG_SALTROCTRL,&value);
      value &= 0xFEFF;
      ret1 = mcmWriteRegister(srus.controlsocket,&srus.sruserver,0,MCM_REG_SALTROCTRL,value & 0xFEFF);
      ret1 = mcmWriteRegister(srus.controlsocket,&srus.sruserver,0,MCM_REG_SALTROCTRL,value);
    }
    
    cont = 1;
    while(cont) {
      if (dowrite != 0) {
	ret1 = mcmLoadPca16(srus.controlsocket,&srus.sruserver,readout,pasactrl,&errst1); 
	ret2 = mcmVerifyPca16(srus.controlsocket,&srus.sruserver,readout,pasactrl,&errst2);
	if (verify) {
	  if ((ret1 != 0) || (ret2 != 0)) {
	    printf("ERROR WR %d %X RD %d %X\n",ret1,ret2,errst1,errst2);
	  }
	}
	if (alternate != 0) pasactrl = (~pasactrl & 0x3FF);
	printf("PASA %X\n",pasactrl);
      }
      else {
	ret2 = mcmVerifyPca16(srus.controlsocket,&srus.sruserver,readout,pasactrl,&errst2);
	if (verify != 0) {
	  if (ret2 != 0) {
	    printf("RD ERROR %d %X\n",ret2,errst2);
	  }
	}
      }
      if ((++cont % 1000) == 0) printf("LOOP %8d\r",cont);
      if (--loop == 0) cont = 0;
      if (loop < 0) loop = 0;
      if (signal_stop) cont = 0;
    } 

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