tdaq-develop-2025-02-12
ots_udp_hw_emulator.cpp
1 // ots_udp_hw_emulator.cpp
2 // by rrivera at fnal dot gov
3 // created Feb 2016
4 //
5 // This is a simple emulator of a "data gen" front-end (hardware) interface
6 // using the otsdaq UDP protocol.
7 //
8 // compile with:
9 // g++ ots_udp_hw_emulator.cpp -o hw.o
10 //
11 // if developing, consider appending -D_GLIBCXX_DEBUG to get more
12 // descriptive error messages
13 //
14 // run with:
15 //./hw.o
16 //
17 
18 #include <arpa/inet.h>
19 #include <errno.h>
20 #include <netdb.h>
21 #include <netinet/in.h>
22 #include <stdio.h>
23 #include <stdlib.h>
24 #include <string.h>
25 #include <sys/socket.h>
26 #include <sys/types.h>
27 #include <unistd.h>
28 #include <iomanip>
29 #include <iostream>
30 
31 // take only file name
32 #define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)
33 
34 // use this for normal printouts
35 #define __PRINTF__ printf
36 #define __COUT__ cout << __FILENAME__ << std::dec << " [" << __LINE__ << "]\t"
37 
38 // and use this to suppress
39 //#define __PRINTF__ if(0) printf
40 //#define __COUT__ if(0) cout
41 
42 using namespace std;
43 
44 #define MAXBUFLEN 1492
45 #define EMULATOR_PORT "4950" // Can be also passed as first argument
46 
47 // get sockaddr, IPv4 or IPv6:
48 void* get_in_addr(struct sockaddr* sa)
49 {
50  if(sa->sa_family == AF_INET)
51  {
52  return &(((struct sockaddr_in*)sa)->sin_addr);
53  }
54 
55  return &(((struct sockaddr_in6*)sa)->sin6_addr);
56 }
57 
58 //==============================================================================
59 int makeSocket(const char* ip, int port, struct sockaddr_in &ai_addr)
60 {
61  int sockfd;
62  struct addrinfo hints, *servinfo, *p;
63  int rv;
64  // int numbytes;
65  // struct sockaddr_storage their_addr;
66  // socklen_t addr_len;
67  // char s[INET6_ADDRSTRLEN];
68 
69  memset(&hints, 0, sizeof hints);
70  hints.ai_family = AF_UNSPEC;
71  hints.ai_socktype = SOCK_DGRAM;
72 
73  if((rv = getaddrinfo(ip, std::to_string(port).c_str(), &hints, &servinfo)) != 0)
74  {
75  __SS__ << "getaddrinfo: " << gai_strerror(rv) << __E__;
76  __SS_THROW__;
77  }
78 
79  // loop through all the results and make a socket
80  for(p = servinfo; p != NULL; p = p->ai_next)
81  {
82  if((sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1)
83  {
84  __COUT_WARN__ << "sw: socket failed, trying other address..." << __E__;
85  continue;
86  }
87 
88  break;
89  }
90 
91  if(p == NULL)
92  {
93  __SS__ << "sw: failed to create socket" << __E__;
94  __SS_THROW__;
95  }
96 
97  //copy ai_addr, which is needed for sendto
98  memcpy(&ai_addr, p->ai_addr, sizeof(ai_addr));
99 
100  freeaddrinfo(servinfo);
101  return sockfd;
102 } //end makeSocket()
103 
104 int main(int argc, char** argv)
105 {
106  std::string emulatorPort(EMULATOR_PORT);
107  if(argc == 2)
108  emulatorPort = argv[1];
109 
110  __COUT__ << std::hex << ":::"
111  << "\n\nUsing emulatorPort=" << emulatorPort << "\n"
112  << std::endl;
113 
114  std::string streamToIP;
115  unsigned short streamToPort;
116 
117  int sockfd;
118  int sendSockfd = -1;
119  struct addrinfo hints, *servinfo, *p;
120  int rv;
121  int numberOfBytes;
122  struct sockaddr_storage their_addr;
123  char buff[MAXBUFLEN];
124  socklen_t addr_len;
125  char s[INET6_ADDRSTRLEN];
126 
127  memset(&hints, 0, sizeof hints);
128  hints.ai_family = AF_UNSPEC; // set to AF_INET to force IPv4
129  hints.ai_socktype = SOCK_DGRAM;
130  hints.ai_flags = AI_PASSIVE; // use my IP
131 
132  if((rv = getaddrinfo(NULL, emulatorPort.c_str(), &hints, &servinfo)) != 0)
133  {
134  fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(rv));
135  return 1;
136  }
137 
138  // loop through all the results and bind to the first we can
139  for(p = servinfo; p != NULL; p = p->ai_next)
140  {
141  if((sockfd = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1)
142  {
143  perror("listener: socket");
144  continue;
145  }
146 
147  if(bind(sockfd, p->ai_addr, p->ai_addrlen) == -1)
148  {
149  close(sockfd);
150  perror("listener: bind");
151  continue;
152  }
153 
154  break;
155  }
156 
157  if(p == NULL)
158  {
159  fprintf(stderr, "listener: failed to bind socket\n");
160 
161  __COUT__ << "\n\nYou can try a different port by passing an argument.\n\n";
162 
163  return 2;
164  }
165 
166  freeaddrinfo(servinfo);
167 
168 
169  struct sockaddr_in ai_addr;
170 
174 
175  // print address space
176  std::stringstream addressSpaceSS;
177  addressSpaceSS << "Address space:\n";
178  addressSpaceSS << "\t 0x0000000000001001 \t W/R \t 'Data count'\n";
179  addressSpaceSS << "\t 0x0000000000001002 \t W/R \t 'Data rate'\n";
180  addressSpaceSS << "\t 0x0000000000001003 \t W/R \t 'LEDs'\n";
181  addressSpaceSS << "\t 0x0000000100000006 \t W \t 'Stream destination IP'\n";
182  // addressSpaceSS << "\t 0x0000000100000007 \t W \t 'Destination MAC address
183  // ignored'\n";
184  addressSpaceSS << "\t 0x0000000100000008 \t W \t 'Stream destination port'\n";
185  addressSpaceSS << "\t 0x0000000100000009 \t W/R \t 'Burst data enable'\n";
186 
187  __COUT__ << addressSpaceSS.str() << "\n\n";
188 
189  // hardware "registers"
190  uint64_t data_gen_cnt = 0;
191  uint64_t data_gen_rate = 100; // number of loops to wait
192  uint8_t led_register = 0;
193  uint8_t dataEnabled = 0;
194 
195  const unsigned int RX_ADDR_OFFSET = 2;
196  const unsigned int RX_DATA_OFFSET = 10;
197  const unsigned int TX_DATA_OFFSET = 2;
198 
199  bool wasDataEnable = false;
200  unsigned char sequence = 0;
201  unsigned int packetSz;
202 
203  // for timeout/select
204  struct timeval tv;
205  fd_set readfds, masterfds;
206  tv.tv_sec = 0;
207  tv.tv_usec = 0; // 500000; RAR moved timeout to sleep to free up processor
208  FD_ZERO(&masterfds);
209  FD_SET(sockfd, &masterfds);
210 
211  time_t count = 0;
212 
213  int handlerIndex;
214  int totalNumberOfBytes;
215 
216  while(1)
217  {
218  usleep(3000); // sleep 3ms to free up processor (downfall is less
219  // responsive, but likely not noticeable)
220 
221  readfds = masterfds; // copy to reset timeout select
222  select(sockfd + 1, &readfds, NULL, NULL, &tv);
223 
224  if(FD_ISSET(sockfd, &readfds))
225  {
226  // packet received
227  __COUT__ << std::hex << ":::"
228  << "Packet Received!" << std::endl;
229 
230  addr_len = sizeof their_addr;
231  if((totalNumberOfBytes = recvfrom(sockfd,
232  buff,
233  MAXBUFLEN - 1,
234  0,
235  (struct sockaddr*)&their_addr,
236  &addr_len)) == -1)
237  {
238  perror("recvfrom");
239  exit(1);
240  }
241 
242  __COUT__ << ":::"
243  << "hw: got packet from "
244  << inet_ntop(their_addr.ss_family,
245  get_in_addr((struct sockaddr*)&their_addr),
246  s,
247  sizeof s)
248  << std::endl;
249  __COUT__ << ":::"
250  << "hw: packet total is " << totalNumberOfBytes << " bytes long"
251  << std::endl;
252  // __COUT__ << ":::" << "hw: packet contents \n";
253  // for(int i=0; i<totalNumberOfBytes; i++)
254  // {
255  // if(i%8==0) __COUT__ << "\t" << i << " "
256  //<< std::endl;
257  // __PRINTF__("%2.2X", (unsigned
258  // char)buff[handlerIndex + i]);
259  // //__COUT__ << std::hex << std::setw(2)
260  //<< (int)(unsigned char)buff[i] << std::dec;
261  // }
262 
263  // treat as stacked packets
264  handlerIndex = 0;
265 
266  while(handlerIndex < totalNumberOfBytes &&
267  (numberOfBytes = (buff[handlerIndex + 0]
268  ? 18
269  : 10)) && // assume write is 18 and read is 10
270  handlerIndex + numberOfBytes <= totalNumberOfBytes)
271  {
272  __COUT__ << ":::"
273  << "hw: packet byte index " << handlerIndex << std::endl;
274  __COUT__ << ":::"
275  << "hw: packet is " << numberOfBytes << " bytes long"
276  << std::endl;
277 
278  for(int i = 0; i < numberOfBytes; i++)
279  {
280  if((i - RX_ADDR_OFFSET) % 8 == 0)
281  __PRINTF__("\n");
282  __PRINTF__("%2.2X", (unsigned char)buff[handlerIndex + i]);
283  //__COUT__ << std::hex << std::setw(2) << (int)(unsigned char)buff[i]
284  //<< std::dec;
285  }
286  __PRINTF__("\n");
287 
288  // handle packet
289  if(numberOfBytes == 10 && // size is valid (type, size, 8-byte address)
290  buff[handlerIndex + 0] == 0) // read
291  {
292  uint64_t addr;
293  memcpy((void*)&addr, (void*)&buff[handlerIndex + RX_ADDR_OFFSET], 8);
294 
295  __COUT__ << std::hex << ":::"
296  << "Read address: 0x" << hex << addr;
297  __PRINTF__(" 0x%16.16lX \n", addr);
298 
299  // setup response packet based on address
300  buff[handlerIndex + 0] = 0; // read type
301  buff[handlerIndex + 1] =
302  sequence++; // 1-byte sequence id increments and wraps
303 
304  switch(addr) // define address space
305  {
306  case 0x1001:
307  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
308  (void*)&data_gen_cnt,
309  8);
310  __COUT__ << std::hex << ":::"
311  << "Read data count: 0x" << data_gen_cnt << endl;
312  break;
313  case 0x1002:
314  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
315  (void*)&data_gen_rate,
316  8);
317  __COUT__ << std::hex << ":::"
318  << "Read data rate: 0x" << data_gen_rate << endl;
319  break;
320  case 0x1003:
321  memset((void*)&buff[handlerIndex + TX_DATA_OFFSET + 1], 0, 7);
322  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
323  (void*)&led_register,
324  1);
325  __COUT__ << std::hex << ":::"
326  << "Read LED register: 0x" << (unsigned int)led_register
327  << endl;
328  break;
329  case 0x0000000100000009:
330  memset((void*)&buff[handlerIndex + TX_DATA_OFFSET + 1], 0, 7);
331  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
332  (void*)&dataEnabled,
333  1);
334  __COUT__ << std::hex << ":::"
335  << "Read data enable: 0x" << dataEnabled << endl;
336  break;
337  default:
338  __COUT__ << std::hex << ":::"
339  << "Unknown read address received." << endl;
340  }
341 
342  packetSz = TX_DATA_OFFSET + 8; // only response with 1 QW
343  if((numberOfBytes = sendto(sockfd,
344  buff,
345  packetSz,
346  0,
347  (struct sockaddr*)&their_addr,
348  sizeof(struct sockaddr_storage))) == -1)
349  {
350  perror("hw: sendto");
351  exit(1);
352  }
353  __PRINTF__("hw: sent %d bytes back. sequence=%d\n",
354  numberOfBytes,
355  (unsigned char)buff[handlerIndex + 1]);
356  }
357  else if(numberOfBytes >= 18 &&
358  (numberOfBytes - 2) % 8 ==
359  0 && // size is valid (multiple of 8 data)
360  buff[handlerIndex + 0] == 1) // write
361  {
362  uint64_t addr;
363  memcpy((void*)&addr, (void*)&buff[handlerIndex + RX_ADDR_OFFSET], 8);
364  __COUT__ << std::hex << ":::"
365  << "hw: Line " << std::dec << __LINE__ << ":::"
366  << "Write address: 0x" << std::hex << addr;
367  __PRINTF__(" 0x%16.16lX \n", addr);
368 
369  switch(addr) // define address space
370  {
371  case 0x1001:
372  memcpy((void*)&data_gen_cnt,
373  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
374  8);
375  __COUT__ << std::hex << ":::"
376  << "Write data count: 0x" << data_gen_cnt << endl;
377  count = 0; // reset count
378  break;
379  case 0x1002:
380  memcpy((void*)&data_gen_rate,
381  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
382  8);
383  __COUT__ << std::hex << ":::"
384  << "Write data rate: 0x" << data_gen_rate << endl;
385  break;
386  case 0x1003:
387  memcpy((void*)&led_register,
388  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
389  1);
390  __COUT__ << std::hex << ":::"
391  << "Write LED register: 0x" << (unsigned int)led_register
392  << endl;
393  // show "LEDs"
394  __COUT__ << "\n\n";
395  for(int l = 0; l < 8; ++l)
396  __COUT__ << ((led_register & (1 << (7 - l))) ? '*' : '-');
397  __COUT__ << "\n\n";
398  break;
399  case 0x0000000100000006:
400  {
401  uint32_t ip;
402  struct sockaddr_in socketAddress;
403  memcpy(
404  (void*)&ip, (void*)&buff[handlerIndex + RX_DATA_OFFSET], 4);
405  ip = htonl(ip);
406  memcpy((void*)&socketAddress.sin_addr, (void*)&ip, 4);
407  streamToIP = inet_ntoa(socketAddress.sin_addr);
408  __COUT__ << std::hex << ":::"
409  << "Stream destination IP: " << streamToIP << std::endl;
410  __COUT__ << streamToIP << std::endl;
411  }
412  break;
413  case 0x0000000100000007:
414  __COUT__ << std::hex << ":::"
415  << "Destination MAC address ignored!" << std::endl;
416  break;
417  case 0x0000000100000008:
418  {
419  // unsigned int myport;
420  memcpy((void*)&streamToPort,
421  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
422  4);
423  __COUT__ << std::hex << ":::"
424  << "Stream destination port: 0x" << streamToPort << dec
425  << " " << streamToPort << endl;
426 
427  if(sendSockfd != -1)
428  close(sendSockfd);
429  sendSockfd = -1;
430  sendSockfd = makeSocket(streamToIP.c_str(), streamToPort, ai_addr);//p);
431  if(sendSockfd != -1)
432  {
433  __COUT__ << "************************************************"
434  "********"
435  << endl;
436  __COUT__ << "************************************************"
437  "********"
438  << endl;
439  __COUT__ << std::hex << ":::"
440  << "Streaming to ip: " << streamToIP << " port: 0x"
441  << streamToPort << endl;
442  __COUT__ << "************************************************"
443  "********"
444  << endl;
445  __COUT__ << "************************************************"
446  "********"
447  << endl;
448  }
449  else
450  __COUT__ << std::hex << ":::"
451  << "Failed to create streaming socket to ip: "
452  << streamToIP << " port: 0x" << streamToPort << endl;
453  }
454 
455  break;
456  case 0x0000000100000009:
457  memcpy((void*)&dataEnabled,
458  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
459  1);
460  __COUT__ << std::hex << ":::"
461  << "Write data enable: 0x" << (int)dataEnabled << endl;
462  count = 0; // reset count
463  break;
464  default:
465  __COUT__ << std::hex << ":::"
466  << "Unknown write address received." << endl;
467  }
468  }
469  else
470  __COUT__ << std::hex << ":::"
471  << "ERROR: The formatting of the string received is wrong! "
472  "Number of bytes: "
473  << numberOfBytes << " Read/Write " << buff[handlerIndex + 0]
474  << std::endl;
475 
476  handlerIndex += numberOfBytes;
477  }
478 
479  __COUT__ << std::hex << ":::"
480  << "\n\n"
481  << addressSpaceSS.str() << "\n\n";
482 
483  } // end packet received if
484  else
485  {
486  // no packet received (timeout)
487 
488  //__COUT__ << "[" << __LINE__ << "]Is burst enabled? " << (int)dataEnabled
489  //<< endl;
490  if((int)dataEnabled)
491  {
492  // generate data
493  //__COUT__ << "[" << __LINE__ << "]Count? " << count << " rate: " <<
494  // data_gen_rate << " counter: " << data_gen_cnt << endl;
495  if(count % data_gen_rate == 0 && // if delayed enough for proper rate
496  data_gen_cnt != 0) // still packets to send
497  {
498  // if(count%0x100000 == 0)
499  __COUT__ << std::hex << ":::"
500  << "Count: " << count << " rate: " << data_gen_rate
501  << " packet-counter: " << data_gen_cnt << std::endl;
502  __COUT__ << std::hex << ":::"
503  << "Send Burst at count:" << count << std::endl;
504  // send a packet
505  buff[0] =
506  wasDataEnable ? 2 : 1; // type := burst middle (2) or first (1)
507  buff[1] = sequence++; // 1-byte sequence id increments and wraps
508  memcpy((void*)&buff[TX_DATA_OFFSET],
509  (void*)&count,
510  8); // make data counter
511  // memcpy((void *)&buff[TX_DATA_OFFSET],(void *)&data_gen_cnt,8);
512  // //make data counter
513 
514  packetSz = TX_DATA_OFFSET + 8; // only response with 1 QW
515  // packetSz = TX_DATA_OFFSET + 180; //only response with 1 QW
516  // __COUT__ << packetSz << std::endl;
517  // std::string data(buff,packetSz);
518  // unsigned long long value;
519  // memcpy((void *)&value, (void *)
520  // data.substr(2).data(),8); //make data counter
521  // __COUT__ << value << std::endl;
522 
523  if((numberOfBytes = sendto(
524  sendSockfd, buff, packetSz, 0, (struct sockaddr*)&ai_addr, sizeof(ai_addr)) == //p->ai_addr, p->ai_addrlen)) ==
525  -1)
526  {
527  perror("Hw: sendto");
528  exit(1);
529  }
530  __PRINTF__("hw: sent %d bytes back. sequence=%d\n",
531  numberOfBytes,
532  (unsigned char)buff[1]);
533 
534  if(data_gen_cnt != (uint64_t)-1)
535  --data_gen_cnt;
536  }
537 
538  wasDataEnable = true;
539  }
540  else if(wasDataEnable) // send last in burst packet
541  {
542  wasDataEnable = false;
543  __COUT__ << std::hex << ":::"
544  << "Send Last in Burst at count:" << count << endl;
545  // send a packet
546  buff[0] = 3; // type := burst last (3)
547  buff[1] = sequence++; // 1-byte sequence id increments and wraps
548  memcpy((void*)&buff[TX_DATA_OFFSET],
549  (void*)&count,
550  8); // make data counter
551 
552  packetSz = TX_DATA_OFFSET + 8; // only response with 1 QW
553 
554  if(sendSockfd != -1)
555  {
556  if((numberOfBytes = sendto(
557  sendSockfd, buff, packetSz, 0, (struct sockaddr*)&ai_addr, sizeof(ai_addr)) == //p->ai_addr, p->ai_addrlen)) ==
558  -1)
559  {
560  perror("hw: sendto");
561  exit(1);
562  }
563  __PRINTF__("hw: sent %d bytes back. sequence=%d\n",
564  numberOfBytes,
565  (unsigned char)buff[1]);
566  }
567  else
568  __COUT__ << std::hex << ":::"
569  << "Send socket not defined." << endl;
570  }
571 
572  ++count;
573  }
574  } // end main loop
575 
576  close(sockfd);
577  if(sendSockfd != -1)
578  close(sendSockfd);
579 
580  return 0;
581 }