simulator/Simulator.cxx (r4864/r4190)

Go to the documentation of this file.

#include "Simulator.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <string.h>


#include <time.h>
#include <sys/time.h>
#include <sys/timex.h>

#include "roc/Message.h"

struct sc_config gC;

struct sc_data gD;

double get_clock()
{
   static double firsttm = 0.;

   //   if (errno!=0) printf("Before gettimeofday errno!=%d\n", errno);
   
   struct timeval tv;
   struct timezone tz;
   if (gettimeofday(&tv, &tz)<0) {
      int k = errno;
      printf("problem to get time err = %d %d %d\n", k, EFAULT, EINVAL);
   }
   double tttt = 1.*tv.tv_sec + tv.tv_usec * 1e-6;

   if (firsttm == 0.) firsttm = tttt;

   //   if (errno!=0) printf("After gettimeofday errno!=0 tm = %5.3f\n", tttt-firsttm);

   /*   ntptimeval val;
   ntp_gettime(&val);
   double tttt = val.time.tv_sec + val.time.tv_usec * 1e-6;
   if (firsttm == 0.) firsttm = tttt;
   */
   return tttt - firsttm;
}

bool SetNonBlockSocket(int fd)
{
   int opts = fcntl(fd, F_GETFL);
   if (opts < 0) {
      printf(("fcntl(F_GETFL) failed\n"));
      return false;
   }
   opts = (opts | O_NONBLOCK);
   if (fcntl(fd, F_SETFL,opts) < 0) {
      printf(("fcntl(F_SETFL) failed\n"));
      return false;
   }
   return true;
}

int StartUdp(int& portnum, int portmin = 0, int portmax = 0)
{

   int fd = socket(PF_INET, SOCK_DGRAM, 0);
   if (fd<0) return -1;

   struct sockaddr_in m_addr;
   int numtests = 1; // at least test value of portnum
   if ((portmin>0) && (portmax>0) && (portmin<=portmax)) numtests+=(portmax-portmin+1);

   if (SetNonBlockSocket(fd))
      for(int ntest=0;ntest<numtests;ntest++) {
         if ((ntest==0) && (portnum<0)) continue;
         if (ntest>0) portnum = portmin - 1 + ntest;

         memset(&m_addr, 0, sizeof(m_addr));
         m_addr.sin_family = AF_INET;
         m_addr.sin_port = htons(portnum);

         if (!bind(fd, (struct sockaddr*)&m_addr, sizeof(m_addr))) return fd;
      }

   close(fd);
   return -1;
}

const char* IpToStr(in_addr_t src_ip)
{
   static char buf[200];
   snprintf(buf, sizeof(buf), "%u.%u.%u.%u",
            src_ip & 0xff, (src_ip >> 8) & 0xff,
            (src_ip >> 16) & 0xff, (src_ip >> 24) & 0xff);
   return buf;
}


UdpSimulator::UdpSimulator(int rocid, int syncscale, int period0, int period1, int ctrlport)
{
   printf("Starting ROC Simulator...\n");
   memset(nxRegister, 0, sizeof(nxRegister));
   nxRegister[0] = ROC_SIMULATOR_HW_VERSION;

   memset(gpioRegs, 0, sizeof(gpioRegs));

   syncMDelay() = syncscale;
   gpioConfig() = (3 << (base::SYNC_S0*4));

   periodAUX0 = 2000000;
   if (period0>=0) gpioConfig() |= (3 << (base::AUX0*4));
   if (period0>0) periodAUX0 = (unsigned) period0;

   periodAUX1 = 20000;
   if (period1>=0) gpioConfig() |= (3 << (base::AUX1*4));
   if (period1>0) periodAUX1 = (unsigned) period1;

   resetSimulator();

   recvDataSize = 0;
   sendDataSize = 0;
   takeDataSize = 0;
   memset(&fStat, 0, sizeof(fStat));

   setLostRates(0., 0.);

   srand(3442143);

   gC.ctrlPort = ROC_DEFAULT_CTRLPORT;
   gC.dataPort = ROC_DEFAULT_DATAPORT;

   if (ctrlport>1000) {
      gC.ctrlPort = ctrlport;
      gC.dataPort = ctrlport-1;
   }

   gC.rocNumber = rocid;
   gC.flushTimeout = 1000;

   gD.high_water = 2000;   /* Maximum number of buffers to be used in normal situation */
   gD.low_water = 50;      /* Number of buffers to enable data taking after is was stopped */

   gD.headid = 0;         // id of buffer which is now filled
   gD.send_limit = 0;    /* limit for sending - can be in front of existing header */
   gD.resend_size = 0;   /* number of indexes to be resended */
   gD.last_req_pkt_id = 0; /* id of last request packet, should be always send back */

   gD.masterConnected = false; // 1 if master controller is connected
   gD.masterDataPort = 0;
   gD.masterPort = 0;
   gD.masterIp = 0;
   gD.lastMasterPacketId = 0;
   gD.master_retry_buf_rawsize = 0;

   gD.controlPort = 0;
   gD.controlIp = 0;
   gD.lastControlPacketId = 0;
   gD.control_retry_buf_rawsize = 0;

   gD.controlLocked = 0;
   gD.lockedTime = get_clock();

   gD.daqState = 0;
   gD.data_taking_on = false;
   gD.lastMasterTime = get_clock();

   for (int n=0;n<ROC_CMD_LST_SIZE/4;n++)
      gD.cmdsList[n] = 0;

   for (int n=0;n<ROC_CMD_LST_NUMBER;n++) {
      int aa = n * ROC_CMD_LST_SIZE / 4 / ROC_CMD_LST_NUMBER;

      if (n==0) {
         gD.cmdsList[aa++] = ROC_CMD_LST_PUT | ROC_NX_FIFO_RESET;
         gD.cmdsList[aa++] = 1;
         gD.cmdsList[aa++] = ROC_CMD_LST_PUT | ROC_NX_FIFO_RESET;
         gD.cmdsList[aa++] = 0;
         gD.cmdsList[aa++] = ROC_CMD_LST_PUT | ROC_ETH_START_DAQ;
         gD.cmdsList[aa++] = 1;
      } else
      if (n==1) {
         gD.cmdsList[aa++] = ROC_CMD_LST_PUT | ROC_ETH_STOP_DAQ;
         gD.cmdsList[aa++] = 1;
      }

      gD.cmdsList[aa++] = ROC_CMD_LST_PUT | ROC_CMD_LST_ACTIVE;
      gD.cmdsList[aa++] = 0;
   }

   gD.cmdListPtr = ROC_CMD_LST_SIZE / 4;


   s_data = 0;
   s_ctrl = 0;
}

UdpSimulator::~UdpSimulator()
{
   //readout network fifo
   close(s_data);
   close(s_ctrl);
}

void UdpSimulator::resetSimulator()
{

   // start with epoch wich has 24 bit already filled
//   uint64_t first_stamp = (0x1000000ULL << 14);
   uint64_t first_stamp = 0;

   nextEventNumber = 0;
   clockScrewUp = 1. + gC.rocNumber / 1000.;
   nextTimeHit = first_stamp + 5674;
   nextHitCounter = 1;                 // generate only one hit for first event
   nextTimeNoise = first_stamp + 3456; // indicate when time stamp message should be
   nextTimeSync = first_stamp + 0x345; // indicate when next time sync message should appear
   nextTimeAUX0 = first_stamp + 0x500 + periodAUX0;  // first AUX0 signal will be after first sync
   nextTimeAUX1 = first_stamp + 0x764 + periodAUX1;  // first AUX1 signal will be after first sync
   nextAUX0Falling = true;
   nextAUX1Falling = true;
   lastEpoch = (first_stamp >> 14) - 1;    // last epoch set in data stream
   nextSyncEvent = 0;
}


bool UdpSimulator::doRecvLost()
{
   totalRecvCounter++;

   if ((recvLostRate <=0.) || (recvLostRate >=1)) return false;

   if (recvLostCounter>0) {
      --recvLostCounter;
      if (recvLostCounter==0) totalRecvLosts++;
      return (recvLostCounter==0);
   }

   recvLostCounter = 1 + int (2. * rand() / RAND_MAX / recvLostRate);

   return false;
}


bool UdpSimulator::doSendLost()
{
   totalSendCounter++;

   if ((sendLostRate <=0.) || (sendLostRate >=1)) return false;

   if (sendLostCounter>0) {
      sendLostCounter--;
      if (sendLostCounter==0) totalSendLosts++;
      return sendLostCounter==0;
   }

   sendLostCounter = 1 + int (2. * rand() / RAND_MAX / sendLostRate);

   return false;
}

void UdpSimulator::setLostRates(double recv, double send)
{
   recvLostRate = recv; // relative loss rate of packets recieve
   sendLostRate = send; // relative loss rate of packets send

   recvLostCounter = 0; // backward counter to next recv lost
   sendLostCounter = 0; // backward counter to next send lost

   totalSendCounter = 0; totalSendLosts = 0;
   totalRecvCounter = 0; totalRecvLosts = 0;
}

bool UdpSimulator::initNewSockets()
{
   int portnum = gC.ctrlPort;

   s_ctrl = StartUdp(portnum);
   if (s_ctrl<=0) {
      printf("Cannot start control port %d\n", portnum);
      return false;
   }

   portnum = gC.dataPort;
   s_data = StartUdp(portnum, portnum, portnum+100);
   if (s_data<=0) {
      printf("Cannot start data port %d\n", portnum);
      return false;
   }
   gC.dataPort = portnum;

   printf("New ctrl port %d data port %d\n", gC.ctrlPort, gC.dataPort);

   return true;
}

void UdpSimulator::dispatchNewSockets(int tmout)
{
   struct pollfd fds[2];
   fds[0].fd = s_ctrl;
   fds[0].events = POLLIN;
   fds[0].revents = 0;
   fds[1].fd = s_data;
   fds[1].events = POLLIN;
   fds[1].revents = 0;

   int res = poll(fds, 2, tmout);

   if (res==0) return;

   struct sockaddr from;
   socklen_t fromlen;

   char recvbuf[2000];

   for (int n=0;n<2;n++) {
      if ((fds[n].revents & POLLIN) == 0) continue;

      res = recvfrom(n==0 ? s_ctrl : s_data, recvbuf, sizeof(recvbuf), MSG_DONTWAIT, &from, &fromlen);

      if (res<=0)
         printf("fail by receiving from %s socket res = %d err = %d socket = %d\n", (n==0 ? "ctrl" : "data"), res, errno, (n==0 ? s_ctrl : s_data));
      else
         OnRawData(n, recvbuf, res, &from, fromlen);
   }
}

#define NODE_FORBIDDEN 0
#define NODE_MASTER    1
#define NODE_CONTROL   2
#define NODE_OBSERVER  3
#define NODE_SKIP      4


int UdpSimulator::checkCtrlOperationAllowed(roc::UdpMessageFull* pkt, in_addr_t src_ip, int src_port)
{
   bool ismaster = (gD.masterIp == src_ip) && (gD.masterPort == src_port);
   bool iscontrol = (gD.controlIp == src_ip) && (gD.controlPort == src_port);

   if ((pkt->tag == ROC_POKE) && (pkt->address == ROC_ETH_MASTER_LOGIN)) {

      if (pkt->value==0) {
         if (gD.masterConnected && !ismaster) {
            printf("Master already connected\n");
            return NODE_FORBIDDEN;
         }

         if (iscontrol) {
            gD.controlIp = 0;
            gD.controlPort = 0;
            iscontrol = false;
         }

         if (!ismaster) {
            gD.masterIp = src_ip;
            gD.masterPort = src_port;

            gD.masterConnected = true;
            ismaster = true;
            gD.lastMasterPacketId = 0;

            printf("Master is %s : %d\n", IpToStr(src_ip), src_port);
         }
      } else
      if (pkt->value==1) {
         if (gD.controlConnected && !iscontrol) {
            printf("Control already connected\n");
            return NODE_FORBIDDEN;
         }

         if (ismaster) {
            gD.masterIp = 0;
            gD.masterPort = 0;
            ismaster = false;
         }

         if (!iscontrol) {
            gD.controlIp = src_ip;
            gD.controlPort = src_port;

            gD.controlConnected = true;
            iscontrol = true;
            gD.lastControlPacketId = 0;

            printf("Control is %s : %d\n", IpToStr(src_ip), src_port);
         }
      } else
         return NODE_OBSERVER;
   }

   if (gD.controlLocked) {
      if (((gD.controlLocked==1) && ismaster) || ((gD.controlLocked==2) && iscontrol)) {
         // if locked, operation from the same node are allowed
      } else
         return NODE_SKIP;
   }

   if (ismaster) {
      gD.masterConnected = true;
      gD.lastMasterTime = get_clock();
      return NODE_MASTER;
   }

   if (iscontrol) {
      gD.controlConnected = true;
      gD.lastControlTime = get_clock();
      return NODE_CONTROL;
   }

   // for moment allow all PEEK operations for slave
   if (pkt->tag == ROC_PEEK) return NODE_OBSERVER;


   return NODE_FORBIDDEN; // reject operation
}

void UdpSimulator::OnRawData(int number, const char* buf, int datalen, struct sockaddr* sa_from, socklen_t sa_len)
{
   if (sa_len != sizeof(struct sockaddr_in)) return;// IPv4

   if (doRecvLost()) return;

   recvDataSize += datalen;

   //   printf("OnRawData number %d  size=%d addrlen=%d errno=%d   from:", number, datalen, sa_len, (int) errno);
   //   uint8_t* p1 = (uint8_t*) sa_from;
   //   for (int n=0;n<sa_len;n++) printf(" %3u", (unsigned) p1[n]);
   //   printf("\n");

   struct sockaddr_in sa;
   memcpy(&sa, sa_from, sa_len);
   in_addr_t src_ip;
   memcpy(&src_ip, &sa.sin_addr, 4);

   if (src_ip == 0) {
     printf("Dummy source IP address - ignore\n");
     return; 
   }

   int src_port = ntohs(sa.sin_port);

   if (number==0) {

      roc::UdpMessageFull* pkt = (roc::UdpMessageFull*) buf;

      if (ntohl(pkt->password) != ROC_PASSWORD) return;

      pkt->address = ntohl(pkt->address);
      pkt->value = ntohl(pkt->value);

      int nodekind = checkCtrlOperationAllowed(pkt, src_ip, src_port);

      // ignore packet if control channel locked
      if (nodekind==NODE_SKIP) return;

      int rawdatasize = 0;

      if (nodekind == NODE_FORBIDDEN) {
         // if operation not allowed, just reply with same packet, but
         // set error return code 8
         pkt->value = 8; // forbidden
      } else
      if ((nodekind==NODE_MASTER) && (pkt->id == gD.lastMasterPacketId)) {
          // just send again previous reply
         rawdatasize = gD.master_retry_buf_rawsize;
         memcpy(pkt->rawdata, gD.master_retry_buf, rawdatasize);
         printf("!!! resend master packet %u!!!\n", (unsigned) ntohl(pkt->id));
      } else
      if ((nodekind==NODE_CONTROL) && (pkt->id == gD.lastControlPacketId)) {
         // just send again previous reply
         rawdatasize = gD.control_retry_buf_rawsize;
         memcpy(pkt->rawdata, gD.control_retry_buf, rawdatasize);
         printf("!!! resend control packet %u!!!\n", (unsigned) ntohl(pkt->id));
      } else
      switch(pkt->tag) {
         case ROC_PEEK:
            pkt->address = GetReg(nodekind, pkt->address, &pkt->value, pkt->rawdata, &rawdatasize);
            break;

         case ROC_POKE:
            pkt->value = PutReg(nodekind, pkt->address, pkt->value, pkt->rawdata);
            break;

         case ROC_NOPER:
            // pkt->address = pkt->address;

            // if we get request to lock our control
            if (pkt->value == 1) {
               printf("LOCK control\n");
               if (nodekind==NODE_MASTER) gD.controlLocked = 1; else
               if (nodekind==NODE_CONTROL) gD.controlLocked = 2;
               gD.lockedTime = get_clock();
            } else
            if (gD.controlLocked) {
               printf("UNLOCK control\n");
               gD.controlLocked = 0;
               gD.lockedTime = 0;
            }

            pkt->value = NOper(nodekind, pkt->address, (uint32_t*) pkt->rawdata);
            rawdatasize = pkt->address * 8;
            break;
      }

      pkt->address = htonl(pkt->address);
      pkt->value = htonl(pkt->value);

      if (!doSendLost()) {
         sendto(s_ctrl, (char*)pkt, sizeof(roc::UdpMessage) + rawdatasize, MSG_DONTWAIT, sa_from, sa_len);
         sendDataSize += sizeof(roc::UdpMessage) + rawdatasize;
      }

      if ((nodekind==NODE_MASTER) && (pkt->id != gD.lastMasterPacketId)) {
         gD.lastMasterPacketId = pkt->id;
         memcpy(gD.master_retry_buf, pkt->rawdata, rawdatasize);
         gD.master_retry_buf_rawsize = rawdatasize;
      } else
      if ((nodekind==NODE_CONTROL) && (pkt->id != gD.lastControlPacketId)) {
         gD.lastControlPacketId = pkt->id;
         memcpy(gD.control_retry_buf, pkt->rawdata, rawdatasize);
         gD.control_retry_buf_rawsize = rawdatasize;
      }

   } else {
      roc::UdpDataRequestFull* pkt = (roc::UdpDataRequestFull*) buf;

      if (ntohl(pkt->password) != ROC_PASSWORD) return;

      gD.lastMasterTime = get_clock();

      pkt->reqpktid = ntohl(pkt->reqpktid);
      pkt->frontpktid = ntohl(pkt->frontpktid);
      pkt->tailpktid = ntohl(pkt->tailpktid);
      pkt->numresend = ntohl(pkt->numresend);

      for (unsigned n=0; n<pkt->numresend; n++)
         pkt->resend[n] = ntohl(pkt->resend[n]);

      memcpy(&gD.masterDataAddr, sa_from, sa_len);

      processDataRequest(pkt);
   }
}

uint32_t UdpSimulator::GetReg(int nodekind, uint32_t addr, uint32_t *retVal, void* rawdata, int* rawdatasize)
{
   switch (addr) {
      case ROC_ETH_SWV:
         *retVal = ROC_SIMULATOR_SW_VERSION;
         return 0;
      case ROC_HWV:
         *retVal = ROC_SIMULATOR_HW_VERSION;
         return 0;
      case ROC_ETH_HWV:
         *retVal = ROC_SIMULATOR_HW_VERSION;
         return 0;
      case ROC_FPGA_TYPE:
         *retVal = ROC_SIMULATOR_FPGA_TYPE;
         return 0;
      case ROC_SVN_REVISION:
         *retVal = 1;
         return 0;
      case ROC_BUILD_TIME:
         *retVal = 41*3600*24*365;
         return 0;
      case ROC_ROCID:
         *retVal = gC.rocNumber;
         return 0;
      case ROC_TYPE:
         *retVal = 0x10002; // NXYTER FX20
         return 0;
      case ROC_ETH_LOWWATER:
         *retVal = gD.low_water;
         return 0;
      case ROC_ETH_HIGHWATER:
         *retVal = gD.high_water;
         return 0;
      case ROC_ETH_DATAPORT:
         *retVal = gC.dataPort;
         return 0;
      case ROC_ETH_STATBLOCK: {
         if ((rawdata==0) || (rawdatasize==0)) return 2;
         roc::BoardStatistic* stat = (roc::BoardStatistic*) rawdata;
         stat->daqState = htonl(gD.daqState);
         stat->dataRate = htonl(fStat.dataRate);
         stat->sendRate = htonl(fStat.sendRate);
         stat->recvRate = htonl(fStat.recvRate);
         stat->nopRate = htonl(0);
         stat->frameRate = htonl(0);
         stat->takePerf = htonl(0);
         stat->dispPerf = htonl(0);
         stat->sendPerf = htonl(0);

         *rawdatasize = sizeof(roc::BoardStatistic);
         return 0;
      }
      case ROC_NX_I2C_DATA:
      case ROC_NX_I2C_SLAVEADDR:
      case ROC_NX_I2C_ERROR:
         return 0;

      default:
         if ((addr>=ROC_CMD_LST_MEM) && (addr<ROC_CMD_LST_MEM + ROC_CMD_LST_SIZE)) {
            if(addr % 4) return 2;
            addr = (addr - ROC_CMD_LST_MEM) / 4;
            *retVal = gD.cmdsList[addr];
            return 0;
         } else
         if ((addr>=ROC_NX_HWV) && (addr<ROC_NX_HWV + NX_REG_SIZE*4)) {
            *retVal = nxRegister[(addr-ROC_NX_HWV)/4];
            return 0;
         } else
         if ((addr>=base::GPIO_CONFIG) && (addr < base::GPIO_CONFIG + GPIO_REG_SIZE*4)) {
            *retVal = gpioRegs[(addr - base::GPIO_CONFIG)/4];
            return 0;
         } else {
            printf("Unknown register 0x%x\n", addr);
         }
         break;
   }

   return 2;
}

uint32_t UdpSimulator::PutReg(int nodekind, uint32_t addr, uint32_t val, uint8_t* rawdata)
{
   printf("Roc:%u Put 0x%04x = 0x%04x\n", gC.rocNumber, addr, val);

   switch (addr) {
      case ROC_ETH_START_DAQ:
         printf("Roc:%u Starting DAQ ...\n", gC.rocNumber);
         gD.buf.clear();
         gD.send_limit = 0;
         gD.headid = 0;
         gD.resend_size = 0;
         gD.last_req_pkt_id = 0;
         gD.next_send_id = 0;
         gD.data_taking_on = true;
         gD.lastFlushTime = get_clock();
         // if lost rate specified, drop start message
         gD.droppkt_id = (sendLostRate <=0.) ? 0 - 1 : gD.headid;
         makeSysMessage(roc::SYSMSG_DAQ_START);
         gD.daqState = 1;
         return 0;
      case ROC_ETH_SUSPEND_DAQ:
         printf("Roc:%u DAQ suspended.\n", gC.rocNumber);
         // if lost rate specified, drop stop message
         gD.droppkt_id = (sendLostRate <=0.) ? 0 - 1 : gD.headid;
         // gD.droppkt_id = 0 - 1;
         // printf("Try to drop packet 0x%x with stop message\n",gD.droppkt_id);
         makeSysMessage(roc::SYSMSG_DAQ_FINISH);
         gD.daqState = 2;
         gD.data_taking_on = false;
         return 0;
      case ROC_ETH_STOP_DAQ:
         printf("Roc:%u DAQ stopped.\n", gC.rocNumber);
         gD.daqState = 0;
         gD.buf.clear();
         gD.send_limit = 0;
         gD.headid = 0;
         gD.data_taking_on = false;
         return 0;
      case ROC_ROCID:
         printf("Set ROC id to %d.\n",val);
         gC.rocNumber = val;
         return 0;
      case ROC_ETH_MASTER_LOGIN:
         if (nodekind==NODE_MASTER) {
            printf("Master login & simulator reset.\n");
            resetSimulator();
         }
         return 0;

      case ROC_ETH_MASTER_LOGOUT:
         if (nodekind==NODE_MASTER) {
            printf("Master is logout.\n");
            gD.masterConnected = false;
            gD.daqState = 0;
            totalSendLosts = 0;
            totalSendCounter = 0;
            totalRecvLosts = 0;
            totalRecvCounter = 0;
            gD.masterIp = 0;
            gD.masterPort = 0;
            gD.lastMasterPacketId = 0;
            if (gD.controlLocked == 1) gD.controlLocked = 0;
         } else
         if (nodekind==NODE_CONTROL) {
            printf("Controller is logout.\n");
            gD.controlConnected = false;
            gD.controlPort = 0;
            gD.controlIp = 0;
            gD.lastControlPacketId = 0;
            if (gD.controlLocked == 2) gD.controlLocked = 0;
         }
         return 0;

      case ROC_ETH_LOWWATER:
         gD.low_water = val;
         return 0;

      case ROC_ETH_HIGHWATER:
         gD.high_water = val;
         return 0;

      case ROC_ETH_MASTER_DATAPORT:
         if (nodekind != NODE_MASTER) {
            printf("ERROR! Cannot change master data port from controller.\n");
            return 3;
         }
         gD.masterDataPort = val;
         printf("Set Target data port to (%d).\n",val);
         return 0;
      case ROC_NX_FIFO_RESET:
         return 0;
      case ROC_ETH_BUFFER_FLUSH_TIMER:
         gC.flushTimeout = val;
         return 0;
      case ROC_ETH_CONSOLE_OUTPUT:
         return 0;
      case ROC_ETH_CONSOLE_CMD:
         printf("Execute console command %s.\n", rawdata);
         return 0;
      case ROC_NX_NXACTIVE:
      case ROC_ETH_OCM:
         return 0;

      case ROC_NX_I2C_DATA:
      case ROC_NX_I2C_RESET:
      case ROC_NX_I2C_REGRESET:
      case ROC_NX_I2C_SLAVEADDR:
      case ROC_NX_I2C_REGISTER:
      case ROC_NX_I2C_SWITCH:
         return 0;

      case ROC_ADDSYSMSG:
         if (gD.data_taking_on)
            makeSysMessage(roc::SYSMSG_USER, val);
         return 0;

      case ROC_CMD_LST_ACTIVE:
         gD.cmdListPtr = ROC_CMD_LST_SIZE / 4;
         return 0;

      case ROC_CMD_LST_NR:
         // set pointer on the beginning of the list
         gD.cmdListPtr = (val % ROC_CMD_LST_NUMBER) * ROC_CMD_LST_SIZE / ROC_CMD_LST_NUMBER / 4;
         return 0;

      case ROC_ETH_DLM:
         // set pointer on the beginning of the correspondent list
         if (val>=8)
            gD.cmdListPtr = ((val-8) % ROC_CMD_LST_NUMBER) * ROC_CMD_LST_SIZE / ROC_CMD_LST_NUMBER / 4;
         return 0;

      default:
         if ((addr>=ROC_CMD_LST_MEM) && (addr<ROC_CMD_LST_MEM + ROC_CMD_LST_SIZE)) {
            if(addr % 4) return 2;
            addr = (addr - ROC_CMD_LST_MEM) / 4;
            gD.cmdsList[addr] = val;
            return 0;
         } else
         if ((addr>=ROC_NX_HWV) && (addr < ROC_NX_HWV + NX_REG_SIZE*4)) {
            nxRegister[(addr-ROC_NX_HWV)/4] = val;
            return 0;
         } else
         if ((addr>=base::GPIO_CONFIG) && (addr < base::GPIO_CONFIG + GPIO_REG_SIZE*4)) {
            gpioRegs[(addr - base::GPIO_CONFIG)/4] = val;
            return 0;
         }
   }

   printf("Roc:%u Put 0x%04x = 0x%04x Fails\n", gC.rocNumber, addr, val);

   return 2;
}

uint32_t UdpSimulator::NOper(int nodekind, uint32_t num, uint32_t* arr)
{
   for (unsigned n=0; n<num; n++) {
      uint32_t addr_oper = ntohl(*arr); arr++;
      uint32_t value = ntohl(*arr);

      uint32_t oper = addr_oper & NOPER_OPER_MASK;
      uint32_t addr = addr_oper & NOPER_ADDR_MASK;

      if (oper == NOPER_PUT) {
         uint32_t res = PutReg(nodekind, addr, value, 0);
         if (res!=0) return res | (n << 8);
      } else
      if (oper == NOPER_GET) {
         uint32_t res = GetReg(nodekind, addr, &value);
         if (res!=0) return res | (n << 8);
      } else
         return 2 | (n << 8);

      *arr = htonl(value); arr++;
   }

   return 0;
}


void UdpSimulator::fillRareSyncData(uint8_t* target, uint16_t numOfDatas)
{
   uint32_t nextepoch = 0;

   uint64_t min = 0;

   roc::Message msg;

   while (numOfDatas > 0) {

      min = nextTimeNoise;
      if (min > nextTimeSync) min = nextTimeSync;
      if (min > nextTimeAUX0) min = nextTimeAUX0;
      if (min > nextTimeAUX1) min = nextTimeAUX1;
      if (min > nextTimeHit) min = nextTimeHit;

      bool accept_data = true;

      nextepoch = min >> 14;
      msg.reset();

      if (nextepoch != lastEpoch) {
         msg.setMessageType(roc::MSG_EPOCH);
         msg.setEpochNumber(nextepoch);
         lastEpoch = nextepoch;
      } else

      if (nextTimeHit == min) {
        // random channel, random spectrum:

         //pseudo hit data
        msg.setMessageType(roc::MSG_HIT);
        msg.setNxNumber(rand() % 4);
        msg.setNxChNum(rand() % 128);
        msg.setNxTs(nextTimeHit & 0x3fff);
        msg.setNxAdcValue(rand() % 0x4000);
        msg.setNxLastEpoch(0);

        nextHitCounter--;
        if (nextHitCounter == 0) {
           nextEventNumber++;
           nextHitCounter = 3 + (rand() % 6); // produce between 3 and 8 hits per event
           nextTimeHit = uint64_t(nextEventNumber * 73747 * clockScrewUp); // produce event every 73.747 microsec or about 13.5 KHz
        }
        nextTimeHit += (1 + rand() % 7); // maximum event duration 8hits * 7 nanosec = 56 nanosec
      } else

      if (nextTimeNoise == min) {
        // random channel, random spectrum:

        msg.setMessageType(roc::MSG_HIT);
        msg.setNxNumber(rand() % 4);
        msg.setNxChNum(rand() % 128);
        msg.setNxTs(nextTimeNoise & 0x3fff);
        msg.setNxAdcValue(rand() % 0x4000);
        msg.setNxLastEpoch(0);

        nextTimeNoise += (rand() % 0x4000); // about 2 noise hits per epoch
      } else

      if (nextTimeSync == min) {

         accept_data = ((gpioConfig() >> (base::SYNC_S0*4)) & 3) != 0;

         if (accept_data) {
            msg.setMessageType(roc::MSG_SYNC);
            msg.setSyncChNum(0);
            msg.setSyncTs(nextTimeSync & 0x3fff);
            msg.setSyncEpochLSB(nextepoch & 0x1);
            msg.setSyncData(nextSyncEvent & 0xffffffL);
            msg.setSyncStFlag(0);
         }

         // every n epochs new time sync, period depends from id
         // as result, all other times must be suppressed
         nextSyncEvent += (1 << syncMDelay());
         nextTimeSync = uint64_t(0x345 + nextSyncEvent * 0x4000 * clockScrewUp);

      } else

      if (nextTimeAUX0 == min) {

         accept_data = (gpioConfig() & (1 << (base::AUX0*4 + (nextAUX0Falling ? 1 : 0)))) != 0;

         if (accept_data) {
            msg.setMessageType(roc::MSG_AUX);
            msg.setAuxChNum(0);
            msg.setAuxTs(nextTimeAUX0 & 0x3fff);
            msg.setAuxEpochLSB(nextepoch & 0x1);
            msg.setAuxFalling(nextAUX0Falling ? 1 : 0);
            msg.setAuxOverflow(0);
         }

         nextTimeAUX0 += periodAUX0; // without time correction
         nextAUX0Falling = !nextAUX0Falling;

      } else

      if (nextTimeAUX1 == min) {

         accept_data = (gpioConfig() & (1 << (base::AUX1*4 + (nextAUX1Falling ? 1 : 0)))) != 0;

         if (accept_data) {
            msg.setMessageType(roc::MSG_AUX);
            msg.setAuxChNum(1);
            msg.setAuxTs(nextTimeAUX1 & 0x3fff);
            msg.setAuxEpochLSB(nextepoch & 0x1);
            msg.setAuxFalling(nextAUX1Falling ? 1 : 0);
            msg.setAuxOverflow(0);
         }

         nextTimeAUX1 += periodAUX1; // without time correction
         nextAUX1Falling = !nextAUX1Falling;
      }

      if (accept_data) {
         msg.copyto(target, roc::formatEth2);
         numOfDatas--;
         target+=6;
      }
   }
}

void UdpSimulator::makeSysMessage(uint8_t typ, uint32_t arg)
{
   roc::Message msg;

   msg.setMessageType(roc::MSG_SYS);
   msg.setSysMesType(typ);
   msg.setSysMesData(arg);

   roc::UdpDataPacketFull pkt;

//   pkt.msgs[0] = ((0x7 << 5) | (gC.rocNumber << 2) | 0)  & 0xFF;//msg type, roc#, unused
//   pkt.msgs[1] = typ;
//   pkt.msgs[2] = 0;
//   pkt.msgs[3] = 0;
//   pkt.msgs[4] = 0;
//   pkt.msgs[5] = 0;

   msg.copyto(pkt.msgs, roc::formatEth2);

   pkt.nummsg = htonl(1);
   pkt.pktid = htonl(gD.headid++);
   printf("Put sys msg %u arg %u in pkt with id %u\n", (unsigned) typ, (unsigned) arg, ntohl(pkt.pktid));
   gD.buf.push_back(pkt);
}

void UdpSimulator::processDataRequest(roc::UdpDataRequestFull* pkt)
{
   gD.last_req_pkt_id = pkt->reqpktid;
   gD.send_limit = pkt->frontpktid;

//   printf("Next send limit is %u size = %u\n",gD.send_limit, gD.buf.size());

   while (gD.buf.size() > 0) {
      uint32_t diff = pkt->tailpktid - ntohl(gD.buf.front().pktid);
      if ((diff !=0) && (diff < 0x1000000)) gD.buf.pop_front();
                                       else break;
      if ((gD.daqState==1) && !gD.data_taking_on)
         if (gD.buf.size() < gD.low_water)
            gD.data_taking_on = true;
   }

   for (unsigned n=0; n<pkt->numresend; n++)
      if (gD.resend_size<RESEND_MAX_SIZE) {
         gD.resend_indx[gD.resend_size] = pkt->resend[n];
         gD.resend_size++;
      }
}


bool UdpSimulator::sendDataPacket(uint32_t pktid)
{
   if (gD.droppkt_id == pktid) {
      printf("Explicit drop of packet 0x%x\n", gD.droppkt_id);
      gD.droppkt_id = rand();
      return true;
   }

   if (gD.buf.size()==0) return false;



//   printf("Send data packet %u size %u front %u\n",
//             pktid, gD.buf.size(), ntohl(gD.buf.front().pktid));

   std::list<roc::UdpDataPacketFull>::iterator iter = gD.buf.begin();

   while (iter != gD.buf.end()) {

      if (ntohl(iter->pktid) == pktid) {
          iter->lastreqid = htonl(gD.last_req_pkt_id);

//          if (!doSendLost())
//             dataSocket_.SendToBuf(gD.masterIp, gD.masterDataPort,
//                (char*) &(*iter), sizeof(roc::UdpDataPacket) + ntohl(iter->nummsg) * 6);

/*          struct sockaddr_in sa;
          memcpy(&sa.sin_addr, &gD.masterIp,  4);
          sa.sin_port = htons(gD.masterDataPort);

          printf("Sending addr: ");
          for (int n=0; n<sizeof(sa);n++) printf("%02x ", ((char*) &sa)[n]);
          printf("\n");
*/
          if (!doSendLost()) {
             sendto(s_data,
                   (char*) &(*iter),
                   sizeof(roc::UdpDataPacket) + ntohl(iter->nummsg) * 6,
                   MSG_DONTWAIT,
                   (const sockaddr*) & gD.masterDataAddr,
                   sizeof(gD.masterDataAddr));

             sendDataSize += sizeof(roc::UdpDataPacket) + ntohl(iter->nummsg) * 6;
          }

          return true;
      }

      iter++;
   }

   return false;
}



void UdpSimulator::mainLoop()
{
   roc::UdpDataPacketFull pkt;

   printf("UdpSimulator is running...\n");

   bool did_something = false;
   bool send_something = false;

   gD.lastMasterTime = get_clock();
   double last_tm = get_clock();
   double last_stat_time = last_tm;

   while (true) {

      if (!did_something)
         dispatchNewSockets(100);

      did_something = false;

      if (gD.data_taking_on) {
         fillRareSyncData(pkt.msgs, 243);
         pkt.nummsg = htonl(243);
         pkt.pktid = htonl(gD.headid++);
         gD.buf.push_back(pkt);
         takeDataSize += 243*6;

//         printf("Add packet %u  total size %u\n", ntohl(pkt.pktid), gD.buf.size());
         if (gD.buf.size() > gD.high_water) gD.data_taking_on = false;
      }


      // execute next command from the list
      if (gD.cmdListPtr < ROC_CMD_LST_SIZE / 4) {
         if (gD.cmdsList[gD.cmdListPtr] & ROC_CMD_LST_PUT)
            PutReg(NODE_MASTER, gD.cmdsList[gD.cmdListPtr] & ROC_CMD_LST_ADDRMASK, gD.cmdsList[gD.cmdListPtr+1], 0);
         gD.cmdListPtr += 2;
      }

      double curr_tm = get_clock();

      // process flush timeout
      if (gD.daqState && (fabs(curr_tm - gD.lastFlushTime) > gC.flushTimeout*0.001)) {
         // we have always full buffer, therefore just resend last buffer

         uint32_t prev_send_id = gD.next_send_id - 1;

         if (!send_something)
            if (gD.headid - prev_send_id <= gD.buf.size())
               if (sendDataPacket(prev_send_id))
                  printf("Flush buffer id 0x%x  datataking %d total %u limit 0x%x next_send 0x%x \n", prev_send_id, gD.data_taking_on, (unsigned) gD.buf.size(), (unsigned) gD.send_limit, gD.next_send_id);

         send_something = false;
         gD.lastFlushTime = get_clock();
      }

      if (gD.masterConnected && fabs(curr_tm - gD.lastMasterTime) > 10) {
         printf("Disconnect master after 10 sec!\n");
         gD.masterConnected = false;
         gD.data_taking_on = false;
         gD.daqState = 0;
      }

      if (gD.controlLocked && fabs(curr_tm - gD.lockedTime) > 3) {
         printf("Control unlocked after 3 sec!\n");
         gD.controlLocked = 0;
      }

      if (gD.controlConnected && fabs(gD.lastControlTime - curr_tm) > 10) {
         printf("Disconnect control after 10 sec!\n");
         gD.controlConnected = false;
      }

      if (fabs(gD.lastMasterTime - curr_tm) > 3000) {
         printf("No news from master last 3000 sec, stop main loop!!\n");
         break;
      }

      // retransmit when required
      if (gD.resend_size > 0) {
         sendDataPacket(gD.resend_indx[--gD.resend_size]);
         did_something = true;
         send_something = true;
      } else
      // if one allowed to send
      if (gD.send_limit != gD.next_send_id)
         if (gD.send_limit - gD.next_send_id < 0x1000000)
            if (sendDataPacket(gD.next_send_id)) {
               gD.next_send_id++;
               did_something = true;
               send_something = true;
            }

      if (fabs(curr_tm - last_tm) > 10.) {
         if (totalSendLosts>0)
            printf("Send:%8u Lost:%5.1f%s\n", (unsigned) totalSendCounter, 100. * totalSendLosts/totalSendCounter, "%");
         if (totalRecvLosts>0)
            printf("Recv:%8u Lost:%5.1f%s\n", (unsigned) totalRecvCounter, 100. * totalRecvLosts/totalRecvCounter, "%");
         last_tm = curr_tm;
      }

      if (curr_tm - last_stat_time >= 1.) {
         memset(&fStat, 0, sizeof(fStat));
         fStat.dataRate = takeDataSize;
         fStat.sendRate = sendDataSize;
         fStat.recvRate = recvDataSize;

         takeDataSize = 0;
         recvDataSize = 0;
         sendDataSize = 0;

         last_stat_time = curr_tm;
      }
   }
}