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