diff --git a/sf-stream/include/ZmqLiveReceiver.hpp b/sf-stream/include/ZmqLiveReceiver.hpp new file mode 100644 index 0000000..98c7cc1 --- /dev/null +++ b/sf-stream/include/ZmqLiveReceiver.hpp @@ -0,0 +1,33 @@ +#ifndef SF_DAQ_BUFFER_ZMQLIVERECEIVER_HPP +#define SF_DAQ_BUFFER_ZMQLIVERECEIVER_HPP + + +#include +#include +#include + +#include "formats.hpp" + +class ZmqLiveReceiver { + + const size_t n_modules_; + void* ctx_; + const std::string ipc_prefix_; + std::vector sockets_; + + void* connect_socket(size_t module_id); + void recv_single_module(void* socket, ModuleFrame* meta, char* data); + uint64_t align_modules(ModuleFrameBuffer *meta, char *data); + +public: + ZmqLiveReceiver(const size_t n_modules, + void* ctx, + const std::string& ipc_prefix); + + ~ZmqLiveReceiver(); + + void get_next_image(ModuleFrameBuffer* meta, char* data); +}; + + +#endif //SF_DAQ_BUFFER_ZMQLIVERECEIVER_HPP diff --git a/sf-stream/src/ZmqLiveReceiver.cpp b/sf-stream/src/ZmqLiveReceiver.cpp new file mode 100644 index 0000000..9833c40 --- /dev/null +++ b/sf-stream/src/ZmqLiveReceiver.cpp @@ -0,0 +1,175 @@ +#include "ZmqLiveReceiver.hpp" + +#include +#include +#include +#include + +#include "buffer_config.hpp" +#include "stream_config.hpp" + +using namespace std; +using namespace chrono; +using namespace buffer_config; +using namespace stream_config; + + +ZmqLiveReceiver::ZmqLiveReceiver( + const size_t n_modules, + void *ctx, + const std::string &ipc_prefix) : + n_modules_(n_modules), + ctx_(ctx_), + ipc_prefix_(ipc_prefix), + sockets_(n_modules) +{ + for (size_t i = 0; i < n_modules_; i++) { + sockets_[i] = connect_socket(i); + } +} + +ZmqLiveReceiver::~ZmqLiveReceiver() +{ + for (auto& socket:sockets_) { + zmq_close(socket); + } +} + +void* ZmqLiveReceiver::connect_socket(size_t module_id) +{ + void* socket = zmq_socket(ctx_, ZMQ_SUB); + if (socket == nullptr) { + throw runtime_error(zmq_strerror(errno)); + } + + int rcvhwm = STREAM_RCVHWM; + if (zmq_setsockopt(socket, ZMQ_RCVHWM, &rcvhwm, sizeof(rcvhwm)) != 0) { + throw runtime_error(zmq_strerror(errno)); + } + + int linger = 0; + if (zmq_setsockopt(socket, ZMQ_LINGER, &linger, sizeof(linger)) != 0) { + throw runtime_error(zmq_strerror(errno)); + } + + stringstream ipc_addr; + ipc_addr << ipc_prefix_ << module_id; + const auto ipc = ipc_addr.str(); + + if (zmq_connect(socket, ipc.c_str()) != 0) { + throw runtime_error(zmq_strerror(errno)); + } + + if (zmq_setsockopt(socket, ZMQ_SUBSCRIBE, "", 0) != 0) { + throw runtime_error(zmq_strerror(errno)); + } + + return socket; +} + +void ZmqLiveReceiver::recv_single_module( + void* socket, ModuleFrame* meta, char* data) +{ + auto n_bytes_meta = zmq_recv(socket, meta, sizeof(ModuleFrame), 0); + + if (n_bytes_meta == -1) { + throw runtime_error(zmq_strerror(errno)); + } + if (n_bytes_meta != sizeof(ModuleFrame)) { + throw runtime_error("Stream header of wrong size."); + } + if (meta->pulse_id == 0) { + throw runtime_error("Received invalid pulse_id=0."); + } + + auto n_bytes_frame = zmq_recv(socket, data, MODULE_N_BYTES, 0); + + if (n_bytes_frame == -1) { + throw runtime_error(zmq_strerror(errno)); + } + if (n_bytes_frame != MODULE_N_BYTES) { + throw runtime_error("Stream data of wrong size."); + } +} + +uint64_t ZmqLiveReceiver::align_modules(ModuleFrameBuffer *meta, char *data) +{ + uint64_t max_pulse_id = 0; + uint64_t min_pulse_id = numeric_limits::max(); + + // First pass - determine current min and max pulse_id. + for (auto& module_meta : meta->module) { + min_pulse_id = min(min_pulse_id, module_meta.pulse_id); + max_pulse_id = max(max_pulse_id, module_meta.pulse_id); + } + + auto max_diff = max_pulse_id - min_pulse_id; + if (max_diff > PULSE_OFFSET_LIMIT) { + stringstream err_msg; + + err_msg << "[ZmqLiveReceiver::align_modules]"; + err_msg << " PULSE_OFFSET_LIMIT exceeded."; + err_msg << " Modules out of sync for " << max_diff << " pulses."; + + for (auto& module_meta : meta->module) { + err_msg << " (" << module_meta.module_id << ", "; + err_msg << module_meta.pulse_id << "),"; + } + + err_msg << endl; + + throw runtime_error(err_msg.str()); + } + + // Second pass - align all receivers to max_pulse_id. + for (size_t i_module = 0; i_module < n_modules_; i_module++) { + auto& module_meta = meta->module[i_module]; + + while (module_meta.pulse_id < max_pulse_id) { + recv_single_module( + sockets_[i_module], + &module_meta, + data + (MODULE_N_BYTES * i_module)); + } + + if (module_meta.pulse_id != max_pulse_id) { + throw runtime_error("Cannot align pulse_ids."); + } + } + + return max_pulse_id - min_pulse_id; +} + +void ZmqLiveReceiver::get_next_image(ModuleFrameBuffer* meta, char* data) +{ + uint64_t frame_pulse_id; + bool sync_needed = false; + for (size_t i_module = 0; i_module < n_modules_; i_module++) { + auto& module_metadata = meta->module[i_module]; + + recv_single_module( + sockets_[i_module], + &module_metadata, + data + (MODULE_N_BYTES * i_module)); + + if (i_module == 0) { + frame_pulse_id = module_metadata.pulse_id; + } else if (frame_pulse_id != module_metadata.pulse_id) { + sync_needed = true; + } + } + + if (sync_needed) { + auto start_time = steady_clock::now(); + + auto lost_pulses = align_modules(meta, data); + + auto end_time = steady_clock::now(); + auto us_duration = duration_cast( + end_time-start_time).count(); + + cout << "sf_stream:sync_lost_pulses " << lost_pulses; + cout << " sf_stream::sync_us " << us_duration; + cout << endl; + } +}