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