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