00001 #include "artdaq-demo/Generators/UDPReceiver.hh"
00002
00003 #include "canvas/Utilities/Exception.h"
00004
00005 #include "artdaq/Application/GeneratorMacros.hh"
00006 #include "cetlib_except/exception.h"
00007 #include "fhiclcpp/ParameterSet.h"
00008 #include "artdaq-core/Utilities/SimpleLookupPolicy.hh"
00009 #include "messagefacility/MessageLogger/MessageLogger.h"
00010 #include "artdaq-core-demo/Overlays/UDPFragmentWriter.hh"
00011
00012 #include <fstream>
00013 #include <iomanip>
00014 #include <iterator>
00015 #include <iostream>
00016 #include <sys/poll.h>
00017
00018 demo::UDPReceiver::UDPReceiver(fhicl::ParameterSet const& ps)
00019 : CommandableFragmentGenerator(ps)
00020 , dataport_(ps.get<int>("port", 6343))
00021 , ip_(ps.get<std::string>("ip", "127.0.0.1"))
00022 , expectedPacketNumber_(0)
00023 , sendCommands_(ps.get<bool>("send_CAPTAN_commands", false))
00024 , rawOutput_(ps.get<bool>("raw_output_enabled", false))
00025 , rawPath_(ps.get<std::string>("raw_output_path", "/tmp"))
00026 {
00027 datasocket_ = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
00028 if (datasocket_ < 0)
00029 {
00030 throw art::Exception(art::errors::Configuration) << "UDPReceiver: Error creating socket!" << std::endl;
00031 exit(1);
00032 }
00033
00034 struct sockaddr_in si_me_data;
00035 si_me_data.sin_family = AF_INET;
00036 si_me_data.sin_port = htons(dataport_);
00037 si_me_data.sin_addr.s_addr = htonl(INADDR_ANY);
00038 if (bind(datasocket_, (struct sockaddr *)&si_me_data, sizeof(si_me_data)) == -1)
00039 {
00040 throw art::Exception(art::errors::Configuration) <<
00041 "UDPReceiver: Cannot bind data socket to port " << dataport_ << std::endl;
00042 exit(1);
00043 }
00044
00045 si_data_.sin_family = AF_INET;
00046 si_data_.sin_port = htons(dataport_);
00047 if (inet_aton(ip_.c_str(), &si_data_.sin_addr) == 0)
00048 {
00049 throw art::Exception(art::errors::Configuration) <<
00050 "UDPReceiver: Could not translate provided IP Address: " << ip_ << "\n";
00051 exit(1);
00052 }
00053 }
00054
00055
00056 bool demo::UDPReceiver::getNext_(artdaq::FragmentPtrs& frags)
00057 {
00058 if (should_stop())
00059 {
00060 return false;
00061 }
00062
00063 demo::UDPFragment::Metadata metadata;
00064 metadata.port = dataport_;
00065 metadata.address = si_data_.sin_addr.s_addr;
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085 std::size_t initial_payload_size = 0;
00086
00087 frags.emplace_back(artdaq::Fragment::FragmentBytes(initial_payload_size,
00088 ev_counter(), fragment_id(),
00089 artdaq::Fragment::FirstUserFragmentType, metadata));
00090
00091 demo::UDPFragmentWriter thisFrag(*frags.back());
00092
00093 bool haveData = false;
00094 int16_t burst_end = -1;
00095 uint8_t droppedPackets = 0;
00096 while (!haveData)
00097 {
00098 if (should_stop())
00099 {
00100 return false;
00101 }
00102 struct pollfd ufds[1];
00103 ufds[0].fd = datasocket_;
00104 ufds[0].events = POLLIN | POLLPRI;
00105
00106 int rv = poll(ufds, 1, 1000);
00107 if (rv > 0)
00108 {
00109
00110 if (ufds[0].revents == POLLIN || ufds[0].revents == POLLPRI)
00111 {
00112 uint8_t peekBuffer[2];
00113 recvfrom(datasocket_, peekBuffer, sizeof(peekBuffer), MSG_PEEK,
00114 (struct sockaddr *) &si_data_, (socklen_t*)sizeof(si_data_));
00115
00116 mf::LogInfo("UDPReceiver") << "Recieved UDP Packet with sequence number " << std::hex << (int)peekBuffer[1] << "!";
00117
00118
00119 uint8_t seqNum = peekBuffer[1];
00120 ReturnCode dataCode = getReturnCode(peekBuffer[0]);
00121 if (seqNum >= expectedPacketNumber_ || (seqNum < 10 && expectedPacketNumber_ > 200) || droppedPackets > 0 || expectedPacketNumber_ - seqNum > 20)
00122 {
00123 if (seqNum != expectedPacketNumber_ && (seqNum >= expectedPacketNumber_ || (seqNum < 10 && expectedPacketNumber_ > 200)))
00124 {
00125 int deltaHi = seqNum - expectedPacketNumber_;
00126 int deltaLo = 255 + seqNum - expectedPacketNumber_;
00127 droppedPackets += deltaLo < 255 ? deltaLo : deltaHi;
00128 mf::LogWarning("UDPReceiver") << "Dropped/Delayed packets detected: " << std::to_string(droppedPackets) << std::endl;
00129 expectedPacketNumber_ = seqNum;
00130 }
00131 else if (seqNum != expectedPacketNumber_)
00132 {
00133 int delta = expectedPacketNumber_ - seqNum;
00134 mf::LogWarning("UDPReceiver") << "Sequence Number significantly different than expected! (delta: " << delta << ")";
00135 }
00136
00137 if (dataCode == ReturnCode::Read || dataCode == ReturnCode::First)
00138 {
00139 packetBuffers_.clear();
00140 packetBuffer_t buffer;
00141 memset(&buffer[0], 0, sizeof(packetBuffer_t));
00142 recvfrom(datasocket_, &buffer[0], sizeof(packetBuffer_t), 0, (struct sockaddr *) &si_data_, (socklen_t*)sizeof(si_data_));
00143 packetBuffers_.push_back(buffer);
00144 mf::LogDebug("UDPReceiver") << "Now placing UDP packet with sequence number " << std::hex << (int)seqNum << " into buffer.";
00145 if (dataCode == ReturnCode::Read) { haveData = true; }
00146 else
00147 {
00148 droppedPackets = 0;
00149 burst_end = -1;
00150 }
00151 }
00152 else if ((dataCode == ReturnCode::Middle || dataCode == ReturnCode::Last) && packetBuffers_.size() > 0)
00153 {
00154 packetBuffer_t buffer;
00155 memset(&buffer[0], 0, sizeof(packetBuffer_t));
00156 recvfrom(datasocket_, &buffer[0], sizeof(packetBuffer_t), 0, (struct sockaddr *) &si_data_, (socklen_t*)sizeof(si_data_));
00157 if (droppedPackets == 0)
00158 {
00159 packetBuffers_.push_back(buffer);
00160 }
00161 else if (burst_end == -1 || seqNum < burst_end)
00162 {
00163 bool found = false;
00164 for (packetBuffer_list_t::iterator it = packetBuffers_.begin(); it != packetBuffers_.end(); ++it)
00165 {
00166 if (seqNum < (*it)[1])
00167 {
00168 packetBuffers_.insert(it, buffer);
00169 droppedPackets--;
00170 expectedPacketNumber_--;
00171 }
00172 }
00173 if (!found)
00174 {
00175 packetBuffers_.push_back(buffer);
00176 }
00177 }
00178 mf::LogDebug("UDPReceiver") << "Now placing UDP packet with sequence number " << std::hex << (int)seqNum << " into buffer.";
00179 if (dataCode == ReturnCode::Last && droppedPackets == 0)
00180 {
00181 while (getReturnCode(packetBuffers_.back()[0]) != ReturnCode::Last) { packetBuffers_.pop_back(); }
00182 haveData = true;
00183 }
00184 else if (dataCode == ReturnCode::Last) { burst_end = seqNum; }
00185 else if (burst_end >= 0 && droppedPackets == 0)
00186 {
00187 while (getReturnCode(packetBuffers_.back()[0]) != ReturnCode::Last) { packetBuffers_.pop_back(); }
00188 haveData = true;
00189 }
00190 }
00191
00192 ++expectedPacketNumber_;
00193 }
00194 else
00195 {
00196 packetBuffer_t discardBuffer;
00197 recvfrom(datasocket_, &discardBuffer[0], sizeof(discardBuffer), 0, (struct sockaddr *) &si_data_, (socklen_t*)sizeof(si_data_));
00198 mf::LogWarning("UDPReceiver") << "Out-of-sequence packet detected and discarded!";
00199 }
00200 }
00201 }
00202 }
00203
00204 packetBuffer_t& firstPacket = packetBuffers_.front();
00205 mf::LogDebug("UDPReceiver") << "Recieved data, now placing data with UDP sequence number " << (int)firstPacket[1] << " into UDPFragment";
00206 thisFrag.resize(1500 * packetBuffers_.size() + 1);
00207 std::ofstream output;
00208 if (rawOutput_)
00209 {
00210 std::string outputPath = rawPath_ + "/UDPReceiver-" + ip_ + ":" + std::to_string(dataport_) + ".bin";
00211 output.open(outputPath, std::ios::out | std::ios::app | std::ios::binary);
00212 }
00213
00214 DataType dataType = getDataType(firstPacket[0]);
00215 thisFrag.set_hdr_type((int)dataType);
00216 int pos = 0;
00217 for (auto jj : packetBuffers_)
00218 {
00219 for (int ii = 2; ii < 1500; ++ii)
00220 {
00221
00222 if (jj[ii] == 0 && (dataType == DataType::JSON || dataType == DataType::String)) { break; }
00223
00224 if (rawOutput_) output.write((char*)&(jj[ii]), sizeof(uint8_t));
00225 *(thisFrag.dataBegin() + pos) = jj[ii];
00226 ++pos;
00227 }
00228 }
00229 if (dataType == DataType::JSON || dataType == DataType::String)
00230 {
00231 *(thisFrag.dataBegin() + pos) = 0;
00232 char zero = 0;
00233 if (rawOutput_) output.write(&zero, sizeof(char));
00234 }
00235 if (rawOutput_) output.close();
00236
00237 return true;
00238 }
00239
00240 void demo::UDPReceiver::start()
00241 {
00242 send(CommandType::Start_Burst);
00243 }
00244
00245 void demo::UDPReceiver::stop()
00246 {
00247 send(CommandType::Stop_Burst);
00248 }
00249
00250 void demo::UDPReceiver::pause()
00251 {
00252 send(CommandType::Stop_Burst);
00253 }
00254
00255 void demo::UDPReceiver::resume()
00256 {
00257 send(CommandType::Start_Burst);
00258 }
00259
00260 void demo::UDPReceiver::send(CommandType command)
00261 {
00262 if (sendCommands_)
00263 {
00264 CommandPacket packet;
00265 packet.type = command;
00266 packet.dataSize = 0;
00267 sendto(datasocket_, &packet, sizeof(packet), 0, (struct sockaddr *) &si_data_, sizeof(si_data_));
00268 }
00269 }
00270
00271
00272 DEFINE_ARTDAQ_COMMANDABLE_GENERATOR(demo::UDPReceiver)