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