otsdaq  v2_01_00
UDPReceiver_generator.cc
1 #include "artdaq-ots/Generators/UDPReceiver.hh"
2 
3 #include "canvas/Utilities/Exception.h"
4 #include "artdaq/Application/GeneratorMacros.hh"
5 #include "cetlib/exception.h"
6 #include "fhiclcpp/ParameterSet.h"
7 #include "artdaq-core/Utilities/SimpleLookupPolicy.hh"
8 #include "artdaq-ots/Overlays/UDPFragmentWriter.hh"
9 #include "artdaq-ots/Overlays/FragmentType.hh"
10 #include "otsdaq-core/Macros/CoutMacros.h"
11 
12 
13 #include <fstream>
14 #include <iomanip>
15 #include <iterator>
16 #include <iostream>
17 #include <sys/poll.h>
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  datasocket_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
32  if (!datasocket_)
33  {
34  throw art::Exception(art::errors::Configuration) << "UDPReceiver: Error creating socket!";
35  exit(1);
36  }
37 
38  struct sockaddr_in si_me_data;
39  si_me_data.sin_family = AF_INET;
40  si_me_data.sin_port = htons(dataport_);
41  si_me_data.sin_addr.s_addr = htonl(INADDR_ANY);
42  if (bind(datasocket_, (struct sockaddr *)&si_me_data, sizeof(si_me_data)) == -1)
43  {
44  throw art::Exception(art::errors::Configuration) << "UDPReceiver: Cannot bind data socket to port " << dataport_;
45  exit(1);
46  }
47  /*if(fcntl(datasocket_, F_SETFL, O_NONBLOCK) == -1) {
48 
49  throw art::Exception(art::errors::Configuration) << "UDPReceiver: Cannot set socket to nonblocking!" << TLOG_ENDL;
50  }*/
51 
52  if (rcvbuf_ > 0 && setsockopt(datasocket_, SOL_SOCKET, SO_RCVBUF, &rcvbuf_, sizeof(rcvbuf_)))
53  {
54  throw art::Exception(art::errors::Configuration) << "UDPReceiver: Could not set receive buffer size: " << rcvbuf_;
55  exit(1);
56  }
57 
58  si_data_.sin_family = AF_INET;
59  si_data_.sin_port = htons(dataport_);
60  if (inet_aton(ip_.c_str(), &si_data_.sin_addr) == 0)
61  {
62  throw art::Exception(art::errors::Configuration) << "UDPReceiver: Could not translate provided IP Address: " << ip_;
63  exit(1);
64  }
65  TLOG_INFO("UDPReceiver") << "UDP Receiver Construction Complete!" << TLOG_ENDL;
66 }
67 
68 ots::UDPReceiver::~UDPReceiver()
69 {
70  receiverThread_.join();
71 }
72 
73 void ots::UDPReceiver::start() {
74  TLOG_INFO("UDPReceiver") << "Starting..." << TLOG_ENDL;
75 
76  receiverThread_ = std::thread(&UDPReceiver::receiveLoop_, this);
77  start_();
78 }
79 
80 void ots::UDPReceiver::receiveLoop_()
81 {
82  while (!should_stop())
83  {
84  struct pollfd ufds[1];
85  ufds[0].fd = datasocket_;
86  ufds[0].events = POLLIN | POLLPRI;
87 
88  int rv = poll(ufds, 1, 1000);
89  if (rv > 0)
90  {
91  TLOG_TRACE("UDPReceiver") << "revents: " << ufds[0].revents << ", " << TLOG_ENDL;// ufds[1].revents << TLOG_ENDL;
92  if (ufds[0].revents == POLLIN || ufds[0].revents == POLLPRI)
93  {
94 
95  //FIXME -> IN THE STIB GENERATOR WE DON'T HAVE A HEADER
96  //FIXME -> IN THE STIB GENERATOR WE DON'T HAVE A HEADER
97  //FIXME -> IN THE STIB GENERATOR WE DON'T HAVE A HEADER
98  uint8_t peekBuffer[4];
99  socklen_t dataSz = sizeof(si_data_);
100  recvfrom(datasocket_, peekBuffer, sizeof(peekBuffer), MSG_PEEK,
101  (struct sockaddr *) &si_data_, &dataSz);
102 
103  TLOG_TRACE("UDPReceiver") << "Received UDP Datagram with sequence number " << std::hex << "0x" << static_cast<int>(peekBuffer[1]) << "!" << std::dec << TLOG_ENDL;
104  TLOG_TRACE("UDPReceiver") << "peekBuffer[1] == expectedPacketNumber_: " << std::hex << static_cast<int>(peekBuffer[1]) << " =?= " << (int)expectedPacketNumber_ << TLOG_ENDL;
105  TLOG_TRACE("UDPReceiver") << "peekBuffer: 0: " << std::hex << static_cast<int>(peekBuffer[0])
106  << ", 1: " << std::hex << static_cast<int>(peekBuffer[1])
107  << ", 2: " << std::hex << static_cast<int>(peekBuffer[2])
108  << ", 3: " << std::hex << static_cast<int>(peekBuffer[3]) << TLOG_ENDL;
109 
110  uint8_t seqNum = peekBuffer[1];
111  //ReturnCode dataCode = getReturnCode(peekBuffer[0]);
112  if (seqNum >= expectedPacketNumber_ || (seqNum < 64 && expectedPacketNumber_ > 192))
113  {
114  if (seqNum != expectedPacketNumber_) {
115  int delta = seqNum - expectedPacketNumber_;
116  TLOG_WARNING("UDPReceiver") << std::dec << "Sequence Number different than expected! (delta: " << delta << ")" << TLOG_ENDL;
117  expectedPacketNumber_ = seqNum;
118  }
119 
120  packetBuffer_t receiveBuffer;
121  receiveBuffer.resize(1500);
122  int sts = recvfrom(datasocket_, &receiveBuffer[0], receiveBuffer.size(), 0, (struct sockaddr *) &si_data_, &dataSz);
123  if (sts == -1)
124  {
125  TLOG_WARNING("UDPReceiver") << "Error on socket: " << strerror(errno) << TLOG_ENDL;
126  }
127  else
128  {
129  TLOG_TRACE("UDPReceiver") << "Received " << sts << " bytes." << TLOG_ENDL;
130  }
131 
132  std::unique_lock<std::mutex> lock(receiveBufferLock_);
133  TLOG_TRACE("UDPReceiver") << "Now placing UDP datagram with sequence number " << std::hex << (int)seqNum << " into buffer." << std::dec << TLOG_ENDL;
134  receiveBuffers_.push_back(receiveBuffer);
135 
136  ++expectedPacketNumber_;
137  }
138  else {
139  // Receiving out-of-order datagram, then moving on...
140  TLOG_WARNING("UDPReceiver") << "Received out-of-order datagram: " << seqNum << " != " << expectedPacketNumber_ << " (expected)" << TLOG_ENDL;
141  packetBuffer_t receiveBuffer;
142  receiveBuffer.resize(1500);
143  int sts = recvfrom(datasocket_, &receiveBuffer[0], receiveBuffer.size(), 0, (struct sockaddr *) &si_data_, &dataSz);
144  }
145 
146  }
147  }
148  }
149  TLOG_INFO("UDPReceiver") << "receive Loop exiting..." << TLOG_ENDL;
150 }
151 
152 bool ots::UDPReceiver::getNext_(artdaq::FragmentPtrs & output)
153 {
154  if (should_stop()) {
155  return false;
156  }
157 
158  {
159  std::unique_lock<std::mutex> lock(receiveBufferLock_);
160  std::move(receiveBuffers_.begin(), receiveBuffers_.end(), std::inserter(packetBuffers_, packetBuffers_.end()));
161  receiveBuffers_.clear();
162  }
163 
164  if (packetBuffers_.size() > 0) {
165  size_t packetBufferSize = 0;
166  for (auto& buf : packetBuffers_) {
167  packetBufferSize += buf.size();
168  }
169  TLOG_TRACE("UDPReceiver") << "Calling ProcessData, packetBuffers_.size() == " << std::to_string(packetBuffers_.size()) << ", sz = " << std::to_string(packetBufferSize) << TLOG_ENDL;
170  ProcessData_(output);
171 
172  packetBuffers_.clear();
173  TLOG_TRACE("UDPReceiver") << "Returning output of size " << output.size() << TLOG_ENDL;
174  }
175  else {
176  // Sleep 10 times per poll timeout
177  usleep(100000);
178  }
179  return true;
180 }
181 
182 void ots::UDPReceiver::ProcessData_(artdaq::FragmentPtrs & output) {
183 
185  metadata.port = dataport_;
186  metadata.address = si_data_.sin_addr.s_addr;
187 
188  std::size_t initial_payload_size = 0;
189 
190  output.emplace_back(artdaq::Fragment::FragmentBytes(initial_payload_size,
191  ev_counter(), fragment_id(),
192  ots::detail::FragmentType::UDP, metadata));
193  // We now have a fragment to contain this event:
194  ots::UDPFragmentWriter thisFrag(*output.back());
195 
196  TLOG_TRACE("UDPReceiver") << "Received data, now placing data with UDP sequence number "
197  << std::hex << static_cast<int>((packetBuffers_.front()).at(1))
198  << " into UDPFragment" << TLOG_ENDL;
199  thisFrag.resize(64050 * packetBuffers_.size() + 1);
200  std::ofstream rawOutput;
201  if (rawOutput_) {
202  std::string outputPath = rawPath_ + "/UDPReceiver-" + ip_ + ":" + std::to_string(dataport_) + ".bin";
203  rawOutput.open(outputPath, std::ios::out | std::ios::app | std::ios::binary);
204  }
205 
206  DataType dataType = getDataType((packetBuffers_.front()).at(0));
207  thisFrag.set_hdr_type((int)dataType);
208  int pos = 0;
209  for (auto jj = packetBuffers_.begin(); jj != packetBuffers_.end(); ++jj) {
210  for (int ii = 0; ii < 64050; ++ii) {
211  // Null-terminate string types
212  if ((jj)->at(ii) == 0 && (dataType == DataType::JSON || dataType == DataType::String)) { break; }
213 
214  if (rawOutput_) rawOutput.write((char*)&((jj)->at(ii)), sizeof(uint8_t));
215  *(thisFrag.dataBegin() + pos) = (jj)->at(ii);
216  ++pos;
217  }
218  }
219 
220  if (dataType == DataType::JSON || dataType == DataType::String) {
221  *(thisFrag.dataBegin() + pos) = 0;
222  char zero = 0;
223  if (rawOutput_) rawOutput.write(&zero, sizeof(char));
224  }
225  if (rawOutput_) rawOutput.close();
226 }
227 
228 void ots::UDPReceiver::send(CommandType command)
229 {
230  if (sendCommands_) {
231  CommandPacket packet;
232  packet.type = command;
233  packet.dataSize = 0;
234  sendto(datasocket_, &packet, sizeof(packet), 0, (struct sockaddr *) &si_data_, sizeof(si_data_));
235  }
236 }
237 
238 bool ots::UDPReceiver::isTimerExpired_()
239 {
240  auto now = std::chrono::high_resolution_clock::now();
241  auto diff = std::chrono::duration<double, std::milli>(now - lastFrag_).count();
242  return diff > fragmentWindow_;
243 }
244 
245 void ots::UDPReceiver::stop()
246 {
247 #pragma message "Using default implementation of UDPReceiver::stop()"
248 }
249 
250 void ots::UDPReceiver::stopNoMutex()
251 {
252 #pragma message "Using default implementation of UDPReceiver::stopNoMutex()"
253 }
254 
255 void ots::UDPReceiver::start_()
256 {
257 #pragma message "Using default implementation of UDPReceiver::start_()"
258 }
259 
260 // The following macro is defined in artdaq's GeneratorMacros.hh header
261 DEFINE_ARTDAQ_COMMANDABLE_GENERATOR(ots::UDPReceiver)