otsdaq_demo  v2_05_02_indev
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 <sstream>
30 #include <iostream>
31 
32 // take only file name
33 #define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)
34 
35 // use this for normal printouts
36 #define __PRINTF__ printf
37 #define __COUT__ std::cout << __FILENAME__ << std::dec << " [" << __LINE__ << "]\t"
38 
39 // and use this to suppress
40 //#define __PRINTF__ if(0) printf
41 //#define __COUT__ if(0) cout
42 
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  uint32_t 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 responsive, but
214  // 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 << " " << std::endl;
251  // __PRINTF__("%2.2X", (unsigned char)buff[handlerIndex + i]);
252  // //__COUT__ << std::hex << std::setw(2) << (int)(unsigned
253  // char)buff[i] << std::dec;
254  // }
255 
256  // treat as stacked packets
257  handlerIndex = 0;
258 
259  while(handlerIndex < totalNumberOfBytes &&
260  (numberOfBytes = (buff[handlerIndex + 0]
261  ? 18
262  : 10)) && // assume write is 18 and read is 10
263  handlerIndex + numberOfBytes <= totalNumberOfBytes)
264  {
265  __COUT__ << ":::"
266  << "hw: packet byte index " << handlerIndex << std::endl;
267  __COUT__ << ":::"
268  << "hw: packet is " << numberOfBytes << " bytes long"
269  << std::endl;
270 
271  for(int i = 0; i < numberOfBytes; i++)
272  {
273  if((i - RX_ADDR_OFFSET) % 8 == 0)
274  __PRINTF__("\n");
275  __PRINTF__("%2.2X", (unsigned char)buff[handlerIndex + i]);
276  //__COUT__ << std::hex << std::setw(2) << (int)(unsigned char)buff[i]
277  //<< std::dec;
278  }
279  __PRINTF__("\n");
280 
281  // handle packet
282  if(numberOfBytes == 10 && // size is valid (type, size, 8-byte address)
283  buff[handlerIndex + 0] == 0) // read
284  {
285  uint64_t addr;
286  memcpy((void*)&addr, (void*)&buff[handlerIndex + RX_ADDR_OFFSET], 8);
287 
288  __COUT__ << std::hex << ":::"
289  << "Read address: 0x" << std::hex << addr;
290  __PRINTF__(" 0x%16.16lX \n", addr);
291 
292  // setup response packet based on address
293  buff[handlerIndex + 0] = 0; // read type
294  buff[handlerIndex + 1] =
295  sequence++; // 1-byte sequence id increments and wraps
296 
297  switch(addr) // define address space
298  {
299  case 0x1001:
300  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
301  (void*)&data_gen_cnt,
302  8);
303  __COUT__ << std::hex << ":::"
304  << "Read data count: 0x" << data_gen_cnt << std::endl;
305  break;
306  case 0x1002:
307  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
308  (void*)&data_gen_rate,
309  8);
310  __COUT__ << std::hex << ":::"
311  << "Read data rate: 0x" << data_gen_rate << std::endl;
312  break;
313  case 0x1003:
314  memset((void*)&buff[handlerIndex + TX_DATA_OFFSET + 1], 0, 7);
315  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
316  (void*)&led_register,
317  1);
318  __COUT__ << std::hex << ":::"
319  << "Read LED register: 0x" << (unsigned int)led_register
320  << std::endl;
321  break;
322  case 0x0000000100000009:
323  memset((void*)&buff[handlerIndex + TX_DATA_OFFSET + 1], 0, 7);
324  memcpy((void*)&buff[handlerIndex + TX_DATA_OFFSET],
325  (void*)&dataEnabled,
326  1);
327  __COUT__ << std::hex << ":::"
328  << "Read data enable: 0x" << dataEnabled << std::endl;
329  break;
330  default:
331  __COUT__ << std::hex << ":::"
332  << "Unknown read address received." << std::endl;
333  }
334 
335  packetSz = TX_DATA_OFFSET + 8; // only response with 1 QW
336  if((numberOfBytes = sendto(sockfd,
337  buff,
338  packetSz,
339  0,
340  (struct sockaddr*)&their_addr,
341  sizeof(struct sockaddr_storage))) == -1)
342  {
343  perror("hw: sendto");
344  exit(1);
345  }
346  __PRINTF__("hw: sent %d bytes back. sequence=%d\n",
347  numberOfBytes,
348  (unsigned char)buff[handlerIndex + 1]);
349  }
350  else if(numberOfBytes >= 18 &&
351  (numberOfBytes - 2) % 8 ==
352  0 && // size is valid (multiple of 8 data)
353  buff[handlerIndex + 0] == 1) // write
354  {
355  uint64_t addr;
356  memcpy((void*)&addr, (void*)&buff[handlerIndex + RX_ADDR_OFFSET], 8);
357  __COUT__ << std::hex << ":::"
358  << "hw: Line " << std::dec << __LINE__ << ":::"
359  << "Write address: 0x" << std::hex << addr;
360  __PRINTF__(" 0x%16.16lX \n", addr);
361 
362  switch(addr) // define address space
363  {
364  case 0x1001:
365  memcpy((void*)&data_gen_cnt,
366  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
367  8);
368  __COUT__ << std::hex << ":::"
369  << "Write data count: 0x" << data_gen_cnt << std::endl;
370  count = 0; // reset count
371  break;
372  case 0x1002:
373  memcpy((void*)&data_gen_rate,
374  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
375  8);
376  __COUT__ << std::hex << ":::"
377  << "Write data rate: 0x" << data_gen_rate << std::endl;
378  break;
379  case 0x1003:
380  memcpy((void*)&led_register,
381  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
382  1);
383  __COUT__ << std::hex << ":::"
384  << "Write LED register: 0x" << (unsigned int)led_register
385  << std::endl;
386  // show "LEDs"
387  __COUT__ << "\n\n";
388  for(int l = 0; l < 8; ++l)
389  __COUT__ << ((led_register & (1 << (7 - l))) ? '*' : '-');
390  __COUT__ << "\n\n";
391  break;
392  case 0x0000000100000006:
393  {
394  uint32_t ip;
395  struct sockaddr_in socketAddress;
396  memcpy(
397  (void*)&ip, (void*)&buff[handlerIndex + RX_DATA_OFFSET], 4);
398  ip = htonl(ip);
399  memcpy((void*)&socketAddress.sin_addr, (void*)&ip, 4);
400  streamToIP = inet_ntoa(socketAddress.sin_addr);
401  __COUT__ << std::hex << ":::"
402  << "Stream destination IP: " << streamToIP << std::endl;
403  __COUT__ << streamToIP << std::endl;
404  }
405  break;
406  case 0x0000000100000007:
407  __COUT__ << std::hex << ":::"
408  << "Destination MAC address ignored!" << std::endl;
409  break;
410  case 0x0000000100000008:
411  {
412  // unsigned int myport;
413  memcpy((void*)&streamToPort,
414  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
415  4);
416  __COUT__ << std::hex << ":::"
417  << "Stream destination port: 0x" << streamToPort << std::dec
418  << " " << streamToPort << std::endl;
419 
420  close(sendSockfd);
421  sendSockfd = 0;
422  sendSockfd = makeSocket(streamToIP.c_str(), streamToPort, p);
423  if(sendSockfd != -1)
424  {
425  __COUT__ << "************************************************"
426  "********"
427  << std::endl;
428  __COUT__ << "************************************************"
429  "********"
430  << std::endl;
431  __COUT__ << std::hex << ":::"
432  << "Streaming to ip: " << streamToIP << " port: 0x"
433  << streamToPort << std::endl;
434  __COUT__ << "************************************************"
435  "********"
436  << std::endl;
437  __COUT__ << "************************************************"
438  "********"
439  << std::endl;
440  }
441  else
442  __COUT__ << std::hex << ":::"
443  << "Failed to create streaming socket to ip: "
444  << streamToIP << " port: 0x" << streamToPort << std::endl;
445  }
446 
447  break;
448  case 0x0000000100000009:
449  memcpy((void*)&dataEnabled,
450  (void*)&buff[handlerIndex + RX_DATA_OFFSET],
451  1);
452  __COUT__ << std::hex << ":::"
453  << "Write data enable: 0x" << (int)dataEnabled << std::endl;
454  count = 0; // reset count
455  break;
456  default:
457  __COUT__ << std::hex << ":::"
458  << "Unknown write address received." << std::endl;
459  }
460  }
461  else
462  __COUT__ << std::hex << ":::"
463  << "ERROR: The formatting of the string received is wrong! "
464  "Number of bytes: "
465  << numberOfBytes << " Read/Write " << buff[handlerIndex + 0]
466  << std::endl;
467 
468  handlerIndex += numberOfBytes;
469  }
470 
471  __COUT__ << std::hex << ":::"
472  << "\n\n"
473  << addressSpaceSS.str() << "\n\n";
474 
475  } // end packet received if
476  else
477  {
478  // no packet received (timeout)
479 
480  //__COUT__ << "[" << __LINE__ << "]Is burst enabled? " << (int)dataEnabled <<
481  // endl;
482  if((int)dataEnabled)
483  {
484  // generate data
485  //__COUT__ << "[" << __LINE__ << "]Count? " << count << " rate: " <<
486  // data_gen_rate << " counter: " << data_gen_cnt << endl;
487  if(count % data_gen_rate == 0 && // if delayed enough for proper rate
488  data_gen_cnt != 0) // still packets to send
489  {
490  // if(count%0x100000 == 0)
491  __COUT__ << std::hex << ":::"
492  << "Count: " << count << " rate: " << data_gen_rate
493  << " packet-counter: " << data_gen_cnt << std::endl;
494  __COUT__ << std::hex << ":::"
495  << "Send Burst at count:" << count << std::endl;
496  // send a packet
497  buff[0] =
498  wasDataEnable ? 2 : 1; // type := burst middle (2) or first (1)
499  buff[1] = sequence++; // 1-byte sequence id increments and wraps
500  memcpy((void*)&buff[TX_DATA_OFFSET],
501  (void*)&count,
502  8); // make data counter
503  // memcpy((void *)&buff[TX_DATA_OFFSET],(void *)&data_gen_cnt,8);
504  // //make data counter
505 
506  packetSz = TX_DATA_OFFSET + 8; // only response with 1 QW
507  // packetSz = TX_DATA_OFFSET + 180; //only response with 1 QW
508  // __COUT__ << packetSz << std::endl;
509  // std::string data(buff,packetSz);
510  // unsigned long long value;
511  // memcpy((void *)&value, (void *)
512  // data.substr(2).data(),8); //make data counter
513  // __COUT__ << value << std::endl;
514 
515  if((numberOfBytes = sendto(
516  sendSockfd, buff, packetSz, 0, p->ai_addr, p->ai_addrlen)) ==
517  -1)
518  {
519  perror("Hw: sendto");
520  exit(1);
521  }
522  __PRINTF__("hw: sent %d bytes back. sequence=%d\n",
523  numberOfBytes,
524  (unsigned char)buff[1]);
525 
526  if(data_gen_cnt != (uint64_t)-1)
527  --data_gen_cnt;
528  }
529 
530  wasDataEnable = true;
531  }
532  else if(wasDataEnable) // send last in burst packet
533  {
534  wasDataEnable = false;
535  __COUT__ << std::hex << ":::"
536  << "Send Last in Burst at count:" << count << std::endl;
537  // send a packet
538  buff[0] = 3; // type := burst last (3)
539  buff[1] = sequence++; // 1-byte sequence id increments and wraps
540  memcpy(
541  (void*)&buff[TX_DATA_OFFSET], (void*)&count, 8); // make data counter
542 
543  packetSz = TX_DATA_OFFSET + 8; // only response with 1 QW
544 
545  if(sendSockfd != -1)
546  {
547  if((numberOfBytes = sendto(
548  sendSockfd, buff, packetSz, 0, p->ai_addr, p->ai_addrlen)) ==
549  -1)
550  {
551  perror("hw: sendto");
552  exit(1);
553  }
554  __PRINTF__("hw: sent %d bytes back. sequence=%d\n",
555  numberOfBytes,
556  (unsigned char)buff[1]);
557  }
558  else
559  __COUT__ << std::hex << ":::"
560  << "Send socket not defined." << std::endl;
561  }
562 
563  ++count;
564  }
565  } // end main loop
566 
567  close(sockfd);
568 
569  return 0;
570 }