otsdaq  v2_04_02
UDPReceiver_generator.cc
1 #include "artdaq/DAQdata/Globals.hh"
2 #define TRACE_NAME "UDPReceiver"
3 
4 #include "artdaq-ots/Generators/UDPReceiver.hh"
5 
6 #include "artdaq-core/Utilities/SimpleLookupPolicy.hh"
7 #include "artdaq-ots/Overlays/FragmentType.hh"
8 #include "artdaq-ots/Overlays/UDPFragmentWriter.hh"
9 #include "artdaq/Generators/GeneratorMacros.hh"
10 #include "canvas/Utilities/Exception.h"
11 #include "fhiclcpp/ParameterSet.h"
12 #include "otsdaq/Macros/CoutMacros.h"
13 
14 #include <sys/poll.h>
15 #include <fstream>
16 #include <iomanip>
17 #include <iostream>
18 #include <iterator>
19 
20 //========================================================================================================================
21 ots::UDPReceiver::UDPReceiver(fhicl::ParameterSet const& ps)
22  : CommandableFragmentGenerator(ps)
23  , rawOutput_(ps.get<bool>("raw_output_enabled", false))
24  , rawPath_(ps.get<std::string>("raw_output_path", "/tmp"))
25  , dataport_(ps.get<int>("port", 6343))
26  , ip_(ps.get<std::string>("ip", "127.0.0.1"))
27  , rcvbuf_(ps.get<int>("rcvbuf", 0x1000000))
28  , expectedPacketNumber_(0)
29  , sendCommands_(ps.get<bool>("send_OtsUDP_commands", false))
30  , fragmentWindow_(ps.get<double>("fragment_time_window_ms", 1000))
31  , lastFrag_(std::chrono::high_resolution_clock::now())
32 {
33  TLOG(TLVL_DEBUG) << "Constructor.";
34 
35  datasocket_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
36  if(!datasocket_)
37  {
38  throw art::Exception(art::errors::Configuration)
39  << "UDPReceiver: Error creating socket!";
40  exit(1);
41  }
42 
43  struct sockaddr_in si_me_data;
44  si_me_data.sin_family = AF_INET;
45  si_me_data.sin_port = htons(dataport_);
46  si_me_data.sin_addr.s_addr = htonl(INADDR_ANY);
47  if(bind(datasocket_, (struct sockaddr*)&si_me_data, sizeof(si_me_data)) == -1)
48  {
49  throw art::Exception(art::errors::Configuration)
50  << "UDPReceiver: Cannot bind data socket to port " << dataport_;
51  exit(1);
52  }
53  /*if(fcntl(datasocket_, F_SETFL, O_NONBLOCK) == -1) {
54 
55  throw art::Exception(art::errors::Configuration) << "UDPReceiver: Cannot set
56  socket to nonblocking!" ;
57  }*/
58 
59  if(rcvbuf_ > 0 &&
60  setsockopt(datasocket_, SOL_SOCKET, SO_RCVBUF, &rcvbuf_, sizeof(rcvbuf_)))
61  {
62  throw art::Exception(art::errors::Configuration)
63  << "UDPReceiver: Could not set receive buffer size: " << rcvbuf_;
64  exit(1);
65  }
66 
67  si_data_.sin_family = AF_INET;
68  si_data_.sin_port = htons(dataport_);
69  if(inet_aton(ip_.c_str(), &si_data_.sin_addr) == 0)
70  {
71  throw art::Exception(art::errors::Configuration)
72  << "UDPReceiver: Could not translate provided IP Address: " << ip_;
73  exit(1);
74  }
75  TLOG(TLVL_INFO) << "UDP Receiver Construction Complete!";
76 
77  TLOG(TLVL_DEBUG) << "Constructed.";
78 } // end constructor()
79 
80 //========================================================================================================================
81 ots::UDPReceiver::~UDPReceiver()
82 {
83  TLOG(TLVL_DEBUG) << "Destructor.";
84 
85  // join waits for thread to complete
86  if(receiverThread_.joinable()) // only join if thread has started
87  receiverThread_.join();
88 
89  if(datasocket_)
90  {
91  close(datasocket_);
92  datasocket_ = -1;
93  }
94 
95  TLOG(TLVL_DEBUG) << "Destructed.";
96 } // end destructor()
97 
98 //========================================================================================================================
99 void ots::UDPReceiver::start()
100 {
101  TLOG(TLVL_DEBUG) << "Starting...";
102 
103  TLOG(TLVL_INFO) << "Starting...";
104 
105  receiverThread_ = std::thread(&UDPReceiver::receiveLoop_, this);
106  start_();
107 
108  TLOG(TLVL_DEBUG) << "Started.";
109 } // end start()
110 
111 //========================================================================================================================
112 void ots::UDPReceiver::receiveLoop_()
113 {
114  while(!should_stop())
115  {
116  struct pollfd ufds[1];
117  ufds[0].fd = datasocket_;
118  ufds[0].events = POLLIN | POLLPRI;
119 
120  int rv = poll(ufds, 1, 1000);
121  if(rv > 0)
122  {
123  TLOG(TLVL_TRACE) << "revents: " << ufds[0].revents
124  << ", "; // ufds[1].revents ;
125  if(ufds[0].revents == POLLIN || ufds[0].revents == POLLPRI)
126  {
127  // FIXME -> IN THE STIB GENERATOR WE DON'T HAVE A HEADER
128  // FIXME -> IN THE STIB GENERATOR WE DON'T HAVE A HEADER
129  // FIXME -> IN THE STIB GENERATOR WE DON'T HAVE A HEADER
130  uint8_t peekBuffer[4];
131  socklen_t dataSz = sizeof(si_data_);
132  recvfrom(datasocket_,
133  peekBuffer,
134  sizeof(peekBuffer),
135  MSG_PEEK,
136  (struct sockaddr*)&si_data_,
137  &dataSz);
138 
139  TLOG(TLVL_TRACE)
140  << "Received UDP Datagram with sequence number " << std::hex << "0x"
141  << static_cast<int>(peekBuffer[1]) << "!" << std::dec;
142  TLOG(TLVL_TRACE) << "peekBuffer[1] == expectedPacketNumber_: " << std::hex
143  << static_cast<int>(peekBuffer[1])
144  << " =?= " << (int)expectedPacketNumber_;
145  TLOG(TLVL_TRACE)
146  << "peekBuffer: 0: " << std::hex << static_cast<int>(peekBuffer[0])
147  << ", 1: " << std::hex << static_cast<int>(peekBuffer[1])
148  << ", 2: " << std::hex << static_cast<int>(peekBuffer[2])
149  << ", 3: " << std::hex << static_cast<int>(peekBuffer[3]);
150 
151  uint8_t seqNum = peekBuffer[1];
152  // ReturnCode dataCode = getReturnCode(peekBuffer[0]);
153  if(seqNum >= expectedPacketNumber_ ||
154  (seqNum < 64 && expectedPacketNumber_ > 192))
155  {
156  if(seqNum != expectedPacketNumber_)
157  {
158  int delta = seqNum - expectedPacketNumber_;
159  TLOG(TLVL_WARNING)
160  << std::dec
161  << "Sequence Number different than expected! (delta: "
162  << delta << ")";
163  expectedPacketNumber_ = seqNum;
164  }
165 
166  packetBuffer_t receiveBuffer;
167  receiveBuffer.resize(1500);
168  int sts = recvfrom(datasocket_,
169  &receiveBuffer[0],
170  receiveBuffer.size(),
171  0,
172  (struct sockaddr*)&si_data_,
173  &dataSz);
174  receiveBuffer.resize(sts);
175 
176  if(sts == -1)
177  {
178  TLOG(TLVL_WARNING) << "Error on socket: " << strerror(errno);
179  }
180  else
181  {
182  TLOG(TLVL_TRACE) << "Received " << sts << " bytes.";
183  }
184 
185  std::unique_lock<std::mutex> lock(receiveBufferLock_);
186  TLOG(TLVL_TRACE)
187  << "Now placing UDP datagram with sequence number " << std::hex
188  << (int)seqNum << " into buffer." << std::dec;
189  receiveBuffers_.push_back(receiveBuffer);
190 
191  ++expectedPacketNumber_;
192  }
193  else
194  {
195  // Receiving out-of-order datagram, then moving on...
196  TLOG(TLVL_WARNING)
197  << "Received out-of-order datagram: " << seqNum
198  << " != " << expectedPacketNumber_ << " (expected)";
199  packetBuffer_t receiveBuffer;
200  receiveBuffer.resize(1500);
201  int sts = recvfrom(datasocket_,
202  &receiveBuffer[0],
203  receiveBuffer.size(),
204  0,
205  (struct sockaddr*)&si_data_,
206  &dataSz);
207  receiveBuffer.resize(sts);
208  }
209  }
210  }
211  }
212  TLOG(TLVL_INFO) << "receive Loop exiting...";
213 }
214 
215 //========================================================================================================================
216 bool ots::UDPReceiver::getNext_(artdaq::FragmentPtrs& output)
217 {
218  if(should_stop())
219  {
220  return false;
221  }
222 
223  {
224  std::unique_lock<std::mutex> lock(receiveBufferLock_);
225  std::move(receiveBuffers_.begin(),
226  receiveBuffers_.end(),
227  std::inserter(packetBuffers_, packetBuffers_.end()));
228  receiveBuffers_.clear();
229  }
230 
231  if(packetBuffers_.size() > 0)
232  {
233  size_t packetBufferSize = 0;
234  for(auto& buf : packetBuffers_)
235  {
236  packetBufferSize += buf.size();
237  }
238  TLOG(TLVL_TRACE) << "Calling ProcessData, packetBuffers_.size() == "
239  << std::to_string(packetBuffers_.size())
240  << ", sz = " << std::to_string(packetBufferSize);
241  ProcessData_(output, packetBufferSize);
242 
243  packetBuffers_.clear();
244  TLOG(TLVL_TRACE) << "Returning output of size " << output.size();
245  }
246  else
247  {
248  // Sleep 10 times per poll timeout
249  usleep(100000);
250  }
251  return true;
252 }
253 
254 //========================================================================================================================
255 void ots::UDPReceiver::ProcessData_(artdaq::FragmentPtrs& output, size_t totalSize)
256 {
257  TLOG(TLVL_TRACE) << "ProcessData_ start";
259  metadata.port = dataport_;
260  metadata.address = si_data_.sin_addr.s_addr;
261 
262  std::size_t initial_payload_size = 0;
263 
264  TLOG(TLVL_TRACE) << "Creating Fragment";
265 
266  output.emplace_back(artdaq::Fragment::FragmentBytes(initial_payload_size,
267  ev_counter(),
268  fragment_id(),
269  ots::detail::FragmentType::UDP,
270  metadata));
271 
272  ev_counter_inc();
273 
274  TLOG(TLVL_TRACE) << "Creating UDPFragmentWriter";
275  // We now have a fragment to contain this event:
276  ots::UDPFragmentWriter thisFrag(*output.back());
277 
278  TLOG(TLVL_TRACE) << "Received data, now placing data with UDP sequence number "
279  << std::hex << static_cast<int>((packetBuffers_.front()).at(1))
280  << " into UDPFragment";
281  thisFrag.resize(totalSize + 1);
282  std::ofstream rawOutput;
283  if(rawOutput_)
284  {
285  std::string outputPath =
286  rawPath_ + "/UDPReceiver-" + ip_ + ":" + std::to_string(dataport_) + ".bin";
287  rawOutput.open(outputPath, std::ios::out | std::ios::app | std::ios::binary);
288  }
289 
290  DataType dataType = getDataType((packetBuffers_.front()).at(0));
291  thisFrag.set_hdr_type((int)dataType);
292  int pos = 0;
293  for(auto jj = packetBuffers_.begin(); jj != packetBuffers_.end(); ++jj)
294  {
295  for(size_t ii = 0; ii < jj->size(); ++ii)
296  {
297  // Null-terminate string types
298  if((jj)->at(ii) == 0 &&
299  (dataType == DataType::JSON || dataType == DataType::String))
300  {
301  break;
302  }
303 
304  if(rawOutput_)
305  rawOutput.write((char*)&((jj)->at(ii)), sizeof(uint8_t));
306  *(thisFrag.dataBegin() + pos) = (jj)->at(ii);
307  ++pos;
308  }
309  }
310 
311  if(dataType == DataType::JSON || dataType == DataType::String)
312  {
313  *(thisFrag.dataBegin() + pos) = 0;
314  char zero = 0;
315  if(rawOutput_)
316  rawOutput.write(&zero, sizeof(char));
317  }
318  if(rawOutput_)
319  rawOutput.close();
320 }
321 
322 //========================================================================================================================
323 void ots::UDPReceiver::send(CommandType command)
324 {
325  if(sendCommands_)
326  {
327  CommandPacket packet;
328  packet.type = command;
329  packet.dataSize = 0;
330  sendto(datasocket_,
331  &packet,
332  sizeof(packet),
333  0,
334  (struct sockaddr*)&si_data_,
335  sizeof(si_data_));
336  }
337 }
338 
339 //========================================================================================================================
340 bool ots::UDPReceiver::isTimerExpired_()
341 {
342  auto now = std::chrono::high_resolution_clock::now();
343  auto diff = std::chrono::duration<double, std::milli>(now - lastFrag_).count();
344  return diff > fragmentWindow_;
345 }
346 
347 //========================================================================================================================
348 void ots::UDPReceiver::stop()
349 {
350  TLOG(TLVL_DEBUG) << "Stopping...";
351  //#pragma message "Using default implementation of UDPReceiver::stop()"
352 }
353 
354 //========================================================================================================================
355 void ots::UDPReceiver::stopNoMutex()
356 {
357  //#pragma message "Using default implementation of UDPReceiver::stopNoMutex()"
358 }
359 
360 //========================================================================================================================
361 void ots::UDPReceiver::start_()
362 {
363  //#pragma message "Using default implementation of UDPReceiver::start_()"
364 }
365 
366 // The following macro is defined in artdaq's GeneratorMacros.hh header
367 DEFINE_ARTDAQ_COMMANDABLE_GENERATOR(ots::UDPReceiver)