flatudp: the source, kind, mock sender and stream probes
Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
Martino FerrariandClaude Opus 4.6 committed
1 parent
6e8e71b32f
commit
78a29fbc39
13 files changed
+962
-7
No files matched your search
@@ -0,0 +1,286 @@
|
||||
// SPDX-License-Identifier: MIT
|
||||
// SPDX-FileCopyrightText: 2026 Martino Ferrari
|
||||
|
||||
#include "source/flatudp/flatudp_source.hpp"
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <poll.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cctype>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
namespace udpscope::flatudp {
|
||||
namespace {
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
std::string lower(std::string s) {
|
||||
for (char& c : s) c = static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
|
||||
return s;
|
||||
}
|
||||
|
||||
std::string trim(const std::string& s) {
|
||||
std::size_t a = 0, b = s.size();
|
||||
while (a < b && std::isspace(static_cast<unsigned char>(s[a]))) ++a;
|
||||
while (b > a && std::isspace(static_cast<unsigned char>(s[b - 1]))) --b;
|
||||
return s.substr(a, b - a);
|
||||
}
|
||||
|
||||
double epoch_seconds() {
|
||||
return std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
}
|
||||
|
||||
constexpr std::size_t kMaxReportedSizes = 8; ///< a sender of random sizes must not flood stderr
|
||||
|
||||
} // namespace
|
||||
|
||||
bool valid_ipv4(const std::string& s) {
|
||||
in_addr a{};
|
||||
return inet_pton(AF_INET, s.c_str(), &a) == 1;
|
||||
}
|
||||
|
||||
bool valid_multicast(const std::string& s) {
|
||||
in_addr a{};
|
||||
if (inet_pton(AF_INET, s.c_str(), &a) != 1) return false;
|
||||
return (ntohl(a.s_addr) >> 28) == 0xE;
|
||||
}
|
||||
|
||||
// --- FlatUdpSettings ------------------------------------------------------------------------
|
||||
|
||||
FlatUdpSettings FlatUdpSettings::from(const SourceConfig& c) {
|
||||
FlatUdpSettings s;
|
||||
s.port = static_cast<int>(c.get_int("port", kDefaultPort));
|
||||
s.bind = trim(c.get("bind"));
|
||||
s.multicast = trim(c.get("multicast"));
|
||||
s.iface = trim(c.get("iface"));
|
||||
s.rate = c.get_double("rate", 0.0);
|
||||
const std::string e = lower(trim(c.get("endian", "little")));
|
||||
s.endian_ok = e == "little" || e == "big";
|
||||
s.big_endian = e == "big";
|
||||
s.signals_error = parse(c.get("signals"), s.rows);
|
||||
return s;
|
||||
}
|
||||
|
||||
std::string FlatUdpSettings::problem() const {
|
||||
if (port < 1 || port > 65535) return "the port must be 1 to 65535";
|
||||
if (!signals_error.empty()) return signals_error;
|
||||
const std::string p = flatudp::problem(rows);
|
||||
if (!p.empty()) return p;
|
||||
if (!(rate > 0.0) || !std::isfinite(rate)) return "give the sender's cycle rate in Hz (rate)";
|
||||
if (!endian_ok) return "endian must be little or big";
|
||||
if (!bind.empty() && !valid_ipv4(bind)) return "the bind address '" + bind + "' is not an IPv4 address";
|
||||
if (!multicast.empty() && !valid_multicast(multicast))
|
||||
return "'" + multicast + "' is not a multicast group (224.0.0.0 to 239.255.255.255)";
|
||||
if (!iface.empty() && !valid_ipv4(iface))
|
||||
return "the interface address '" + iface + "' is not an IPv4 address";
|
||||
return {};
|
||||
}
|
||||
|
||||
std::string FlatUdpSettings::identity() const {
|
||||
return (multicast.empty() ? std::string() : multicast + ":") + std::to_string(port);
|
||||
}
|
||||
|
||||
std::size_t FlatUdpSettings::block() const {
|
||||
if (!all_scalar(rows) || !(rate > 0.0) || !std::isfinite(rate)) return 1;
|
||||
const long b = std::lround(rate / 100.0);
|
||||
return static_cast<std::size_t>(std::min(1024L, std::max(1L, b)));
|
||||
}
|
||||
|
||||
// --- FlatUdpSource --------------------------------------------------------------------------
|
||||
|
||||
FlatUdpSource::FlatUdpSource(const SourceConfig& cfg) : cfg_(FlatUdpSettings::from(cfg)) {}
|
||||
|
||||
FlatUdpSource::~FlatUdpSource() { close_fd(); }
|
||||
|
||||
std::string FlatUdpSource::last_message() const {
|
||||
std::lock_guard<std::mutex> lk(msg_mu_);
|
||||
return last_message_;
|
||||
}
|
||||
|
||||
void FlatUdpSource::note(const std::string& msg) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(msg_mu_);
|
||||
last_message_ = msg;
|
||||
}
|
||||
std::fprintf(stderr, "[flatudp] %s\n", msg.c_str());
|
||||
std::fflush(stderr);
|
||||
}
|
||||
|
||||
void FlatUdpSource::close_fd() {
|
||||
if (fd_ >= 0) {
|
||||
::close(fd_);
|
||||
fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
void FlatUdpSource::open(SourceSink& sink) {
|
||||
const std::string p = cfg_.problem();
|
||||
if (!p.empty()) throw std::runtime_error(p);
|
||||
sink_ = &sink;
|
||||
|
||||
signals_.clear();
|
||||
for (const Row& r : cfg_.rows) {
|
||||
SignalInfo s;
|
||||
s.name = r.name;
|
||||
s.type_code = static_cast<std::uint8_t>(r.type);
|
||||
s.rows = r.count;
|
||||
s.cols = 1;
|
||||
s.time_mode = kTimePacket;
|
||||
s.sampling_rate = cfg_.rate;
|
||||
signals_.push_back(s);
|
||||
}
|
||||
expected_ = datagram_size(cfg_.rows);
|
||||
block_ = cfg_.block();
|
||||
pending_.assign(cfg_.rows.size(), {});
|
||||
pending_n_ = 0;
|
||||
rx_.assign(65536, 0);
|
||||
|
||||
const auto fail = [this](const std::string& what) {
|
||||
const std::string msg = what + ": " + std::strerror(errno);
|
||||
close_fd();
|
||||
throw std::runtime_error(msg);
|
||||
};
|
||||
|
||||
fd_ = ::socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK | SOCK_CLOEXEC, 0);
|
||||
if (fd_ < 0) fail("cannot create a UDP socket");
|
||||
// Receivers of one multicast group share its port; a unicast port is the one app's.
|
||||
if (!cfg_.multicast.empty()) {
|
||||
const int one = 1;
|
||||
::setsockopt(fd_, SOL_SOCKET, SO_REUSEADDR, &one, sizeof one);
|
||||
}
|
||||
const int rcvbuf = 4 << 20; // best effort: a 1 kHz sender at 64 KiB would not fit a default one
|
||||
::setsockopt(fd_, SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof rcvbuf);
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(static_cast<std::uint16_t>(cfg_.port));
|
||||
addr.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
if (!cfg_.bind.empty()) inet_pton(AF_INET, cfg_.bind.c_str(), &addr.sin_addr);
|
||||
if (::bind(fd_, reinterpret_cast<const sockaddr*>(&addr), sizeof addr) < 0)
|
||||
fail("cannot bind UDP port " + std::to_string(cfg_.port));
|
||||
|
||||
if (!cfg_.multicast.empty()) {
|
||||
ip_mreq mreq{};
|
||||
inet_pton(AF_INET, cfg_.multicast.c_str(), &mreq.imr_multiaddr);
|
||||
if (cfg_.iface.empty()) {
|
||||
mreq.imr_interface.s_addr = htonl(INADDR_ANY);
|
||||
} else {
|
||||
inet_pton(AF_INET, cfg_.iface.c_str(), &mreq.imr_interface);
|
||||
}
|
||||
if (::setsockopt(fd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof mreq) < 0)
|
||||
fail("cannot join multicast group " + cfg_.multicast);
|
||||
}
|
||||
}
|
||||
|
||||
void FlatUdpSource::close() {
|
||||
close_fd();
|
||||
connected_.store(false, std::memory_order_relaxed);
|
||||
}
|
||||
|
||||
SourceStats FlatUdpSource::stats() const {
|
||||
SourceStats s;
|
||||
s.packets = valid_.load(std::memory_order_relaxed) + wrong_.load(std::memory_order_relaxed);
|
||||
s.frames = frames_;
|
||||
s.stale = wrong_.load(std::memory_order_relaxed);
|
||||
s.reconnects = reconnects_;
|
||||
s.connected = connected_.load(std::memory_order_relaxed);
|
||||
return s;
|
||||
}
|
||||
|
||||
void FlatUdpSource::poll(int timeout_ms) {
|
||||
drain_commands();
|
||||
// Never wait long: the partial-block flush and the silence timeout are checked on this cadence.
|
||||
const int wait = std::clamp(timeout_ms, 0, 20);
|
||||
if (fd_ < 0 || sink_ == nullptr) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(wait));
|
||||
return;
|
||||
}
|
||||
if (!configured_) {
|
||||
configured_ = true;
|
||||
sink_->on_config(signals_, 1);
|
||||
}
|
||||
pollfd p{};
|
||||
p.fd = fd_;
|
||||
p.events = POLLIN;
|
||||
if (::poll(&p, 1, wait) > 0 && (p.revents & POLLIN)) drain_socket();
|
||||
|
||||
const auto now = Clock::now();
|
||||
if (pending_n_ > 0 && now - batch_start_ >= 100ms) flush(); // a stopped sender holds nothing back
|
||||
if (connected_.load(std::memory_order_relaxed) && now - last_valid_ >= 2s) {
|
||||
connected_.store(false, std::memory_order_relaxed);
|
||||
sink_->on_event(SourceEvent::Disconnected, "no datagram for 2 s");
|
||||
}
|
||||
}
|
||||
|
||||
void FlatUdpSource::drain_socket() {
|
||||
for (int i = 0; i < 8192; ++i) { // bounded, so a flood cannot starve the timers
|
||||
const ssize_t n = ::recv(fd_, rx_.data(), rx_.size(), 0);
|
||||
if (n < 0) {
|
||||
if (errno == EINTR) continue;
|
||||
break; // EAGAIN: drained
|
||||
}
|
||||
const double t = epoch_seconds();
|
||||
const std::size_t len = static_cast<std::size_t>(n);
|
||||
if (tap_) tap_(rx_.data(), len, t);
|
||||
if (len == expected_) {
|
||||
accept(rx_.data());
|
||||
} else {
|
||||
reject(len);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FlatUdpSource::accept(const std::uint8_t* data) {
|
||||
valid_.fetch_add(1, std::memory_order_relaxed);
|
||||
last_valid_ = Clock::now();
|
||||
if (!connected_.load(std::memory_order_relaxed)) {
|
||||
if (ever_connected_) ++reconnects_;
|
||||
ever_connected_ = true;
|
||||
connected_.store(true, std::memory_order_relaxed);
|
||||
sink_->on_event(SourceEvent::Connected, "");
|
||||
}
|
||||
decode_datagram(cfg_.rows, data, cfg_.big_endian, pending_);
|
||||
if (pending_n_ == 0) batch_start_ = last_valid_;
|
||||
if (++pending_n_ >= block_) flush();
|
||||
}
|
||||
|
||||
void FlatUdpSource::reject(std::size_t len) {
|
||||
wrong_.fetch_add(1, std::memory_order_relaxed);
|
||||
if (reported_sizes_.size() >= kMaxReportedSizes || !reported_sizes_.insert(len).second) return;
|
||||
const std::string msg = "datagram of " + std::to_string(len) + " bytes, expected " +
|
||||
std::to_string(expected_) + ", check the signal list";
|
||||
note(msg);
|
||||
sink_->on_event(SourceEvent::Error, msg.c_str());
|
||||
}
|
||||
|
||||
void FlatUdpSource::flush() {
|
||||
const std::size_t n = signals_.size();
|
||||
views_.resize(n);
|
||||
for (std::size_t i = 0; i < n; ++i)
|
||||
views_[i] = SignalValues{pending_[i].data(), static_cast<std::uint32_t>(pending_[i].size())};
|
||||
Frame f;
|
||||
f.counter = frame_counter_++;
|
||||
f.lost = 0;
|
||||
f.late = false;
|
||||
f.recv_time = epoch_seconds();
|
||||
f.publish_mode = 1;
|
||||
f.num_samples = static_cast<std::uint32_t>(pending_n_);
|
||||
f.num_signals = static_cast<std::uint32_t>(n);
|
||||
f.signals = signals_.data();
|
||||
f.values = views_.data();
|
||||
++frames_;
|
||||
sink_->on_data(f);
|
||||
for (auto& c : pending_) c.clear();
|
||||
pending_n_ = 0;
|
||||
}
|
||||
|
||||
} // namespace udpscope::flatudp
|
||||
Reference in new issue
Block a user