/*

program to read/write SRU registers

Modify 
LOCAL_IP
SRU_IP
to proper addresses

modify
SRU_WAIT_WRITE to test the needed time between writes.

compile:
 
gcc -Wall -o sruwrrdreg sruwrrdreg.c
./sruwrrdreg --reg <register> --write <hexvalue> --verify --loop <loops> --expected


*/
#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 FEC 1
#define CHIP 0
#define CHANNEL 0
#define SRU_WAIT_WRITE 100

//#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 NODESEL_SRU  0x00100000


#define SRU_REG_SN             0x01 // serial numner
#define SRU_REG_IP             0x02 // IP address
#define SRU_REG_FMVER          0x03 // Firmware version
#define SRU_REG_SRUST          0x04 // Status
#define SRU_REG_DTCERRFLAG1    0x05 // Error flag received from DTC0-19
#define SRU_REG_DTCERRFLAG2    0x06 // Error flag received from DTC0-19

// R/W
#define SRU_REG_FPGA_MON       0x07 // FPGA Monitor flag, Addr and Temp
#define SRU_REG_READOUT_IP     0x08 // FPGA Monitor flag, Addr and Temp

#define SRU_REG_CFG            0x20 // Configuration register
#define SRU_REG_DTC_CLKEN1     0x21 // Clock enable for DTC  0-19 links
#define SRU_REG_DTC_CLKEN2     0x22 // Clock enable for DTC 20-39 links
#define SRU_REG_DTCRDOMASK1    0x23 // Readout mask of DTC   0-19 links
#define SRU_REG_DTCRDOMASK2    0x24 // Readout mask of DTC  20-39 links

#define SRU_REG_SALTRO_CFG1    0x27 // Zero suppresion stuff
#define SRU_REG_SALTRO_CFG2    0x28 // Other SALTRO config stuff.
#define SRU_REG_TRIG_L1_TW     0x29 // Timing window for L1 trigger
#define SRU_REG_TRIG_L2_TW     0x2A // Timing window for L2 trigger


#define SRU_REG_PTRIG_CFG      0x50 // Periodical trigger configuration
#define SRU_REG_TEST_TRIG_CFG0 0x51 // Test trigger configuration 0
#define SRU_REG_TEST_TRIG_CFG1 0x52 // Test trigger configuration 1
#define SRU_REG_TEST_PULSE_CFG 0x53 // Test pulse configuration

#define SRU_REG_RX_L0_CNT      0x60 //
#define SRU_REG_RX_L1_CNT      0x61 //
#define SRU_REG_RX_L2A_CNT     0x62 //
#define SRU_REG_RX_L2R_CNT     0x62 //


unsigned int  valuerd,valuewr;
int fec;
int loop;
int chip;
int channel;
int reg;
int verify;
int pattern;
int signal_stop = 0;
int dowrite;
int expected;



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;

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




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 sruReadRegister(int handle,SRUSERVER *srv,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);
  int mynode,myreg;
  buf[0] = htonl(NODESEL_SRU);
  buf[1] = 0;
  buf[2] = htonl(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 != 41) {
    printf("%s: Received not expected node %08X expected %08X\n",
	   __FUNCTION__,mynode,41);
    return -6;
  }

  myreg = ntohl(buf[1]);

  if (myreg != (reg | 0x80000000)) {
    printf("%s: Received not expected register %08X expected %08X\n",
	   __FUNCTION__,myreg,reg | 0x80000000);
    return -7;
  }

  *value = ntohl(buf[2]);
  printf("%s: %08X %08X %08X\n",__FUNCTION__,mynode,myreg,*value);
  return 0;
}

int sruWriteRegister(int handle,SRUSERVER *srv,int reg, unsigned int value)
{
  unsigned int buf[4];
  int s_sent,s_bytes;
  socklen_t s_length=sizeof(srv->si_sru);
  buf[0] = htonl(NODESEL_SRU);
  buf[1] = 0;
  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 decodeParameters(int argc, char **argv)
{
  int c;
  int option_index = 0;
  char *short_options = "e:hl:p:r:vw:";
  const struct option long_options[]=
  {
    {"expected",1,0,'e'},
    {"help",0,0,'h'},
    {"loop",1,0,'l'},
    {"pattern",1,0,'p'},
    {"register",1,0,'r'},
    {"write",1,0,'w'},
    {"verify",0,0,'v'},
    {0, 0, 0, 0}
  };
  channel = CHANNEL;
  chip = CHIP;
  fec = FEC;
  loop = 1;
  verify = 0;
  valuewr = 0;
  expected = 0;
  dowrite = 0;
  pattern = 0;
  if (argc > 1)
  {
    while ((c = getopt_long(argc, argv, short_options,
                                        long_options, &option_index)) != EOF)
    {
      switch (c)
      {
      case 'e': sscanf(optarg,"%d",&expected); break;
      case 'h':
	puts("Options");
	puts("--fec <fec>");
	puts("--chip <chip>");
	puts("--channel <channel>");
	puts("--expected <value to expect> (only if verify read)");
	puts("--register <register in hex>");
	puts("--write <value in hex>");
	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",&pattern); break;
      case 'r': sscanf(optarg,"%X",&reg); break;
      case 'v': verify = 1; break;
      case 'w': sscanf(optarg,"%X",&valuewr); dowrite = 1; break;
      default: printf("Unkown paramater\n"); return 1;
      }
    }
  }
  return 0;
}


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

  SRU srus;
  int cont;
  int ret1,ret2;
  int errst;

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

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

    if (decodeParameters(argc,argv)) return 0;
    printf("Parameters: REG %04X LOOP %d VER %d WRITE %X\n",
	   reg,loop,verify,valuewr);

    errst = scSruConnect(&srus);
    if (errst != 0) return(0);
    
    cont = 1;
    while(cont) {
      if (dowrite != 0) {
	ret1 = sruWriteRegister(srus.controlsocket,&srus.sruserver,reg,valuewr);
	ret2 = sruReadRegister(srus.controlsocket,&srus.sruserver,reg,&valuerd);
	if (verify) {
	  if ((valuewr != valuerd) || (ret1 != 0) || (ret2 != 0)) {
	    printf("ERROR %d %d RD %08X != WR %08X\n",ret1,ret2,valuerd,valuewr);
	  }
	}
      }
      else {
	ret2 = sruReadRegister(srus.controlsocket,&srus.sruserver,reg,&valuerd);
	if (verify && ((ret2 != 0) || (expected != valuerd))) {
	  printf("RD %08X EXPECTED %08X err %d\n",valuerd,expected,ret2);
	}
      }
      if ((++cont % 1000) == 0) printf("LOOP %8d\r",cont);
      if (--loop == 0) cont = 0;
      if (loop < 0) loop = 0;
      if (signal_stop) cont = 0;
      //      if (cont > 0)  usleep(SRU_WAIT_WRITE);
    } 
    
    close(srus.controlsocket);
    
    puts("\nEXIT");
    return 0;
}