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