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