e1608: link and device layers (discovery, handshake, framed commands, calibration, decode)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Martino FerrariandClaude Sonnet 4.6 committed
1 parent
558d13d521
commit
5ac1b36a1f
5 files changed
+766
No files matched your search
@@ -0,0 +1,360 @@
|
||||
// SPDX-License-Identifier: MIT
|
||||
// SPDX-FileCopyrightText: 2026 Martino Ferrari
|
||||
|
||||
#include "source/e1608/e1608_link.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <arpa/inet.h>
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <poll.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace udpscope::e1608 {
|
||||
|
||||
void Fd::reset(int fd) noexcept {
|
||||
if (fd_ >= 0) ::close(fd_);
|
||||
fd_ = fd;
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
using Clock = std::chrono::steady_clock;
|
||||
using Kind = LinkError::Kind;
|
||||
|
||||
constexpr std::size_t kMaxFrame = 1024;
|
||||
|
||||
[[noreturn]] void sys_fail(const std::string& what) {
|
||||
throw LinkError(Kind::Io, what + ": " + std::strerror(errno));
|
||||
}
|
||||
|
||||
int ms_left(Clock::time_point deadline) {
|
||||
const auto d = std::chrono::duration_cast<std::chrono::milliseconds>(deadline - Clock::now());
|
||||
return d.count() < 0 ? 0 : static_cast<int>(d.count());
|
||||
}
|
||||
|
||||
/// True when `fd` is ready for `events` (or has an error/hang-up to report); false on timeout.
|
||||
bool wait_fd(int fd, short events, int timeout_ms) {
|
||||
pollfd p{fd, events, 0};
|
||||
for (;;) {
|
||||
const int r = ::poll(&p, 1, timeout_ms);
|
||||
if (r > 0) return true;
|
||||
if (r == 0) return false;
|
||||
if (errno == EINTR) continue;
|
||||
sys_fail("poll");
|
||||
}
|
||||
}
|
||||
|
||||
sockaddr_in make_addr(const std::string& host, int port) {
|
||||
sockaddr_in a{};
|
||||
a.sin_family = AF_INET;
|
||||
a.sin_port = htons(static_cast<std::uint16_t>(port));
|
||||
if (::inet_pton(AF_INET, host.c_str(), &a.sin_addr) == 1) return a;
|
||||
addrinfo hints{};
|
||||
hints.ai_family = AF_INET;
|
||||
hints.ai_socktype = SOCK_STREAM;
|
||||
addrinfo* res = nullptr;
|
||||
if (::getaddrinfo(host.c_str(), nullptr, &hints, &res) != 0 || res == nullptr)
|
||||
throw LinkError(Kind::Io, "cannot resolve '" + host + "'");
|
||||
a.sin_addr = reinterpret_cast<const sockaddr_in*>(res->ai_addr)->sin_addr;
|
||||
::freeaddrinfo(res);
|
||||
return a;
|
||||
}
|
||||
|
||||
void send_all(int fd, const std::uint8_t* p, std::size_t n, Clock::time_point deadline) {
|
||||
while (n > 0) {
|
||||
if (!wait_fd(fd, POLLOUT, ms_left(deadline)))
|
||||
throw LinkError(Kind::Timeout, "timed out sending to the board");
|
||||
const ssize_t w = ::send(fd, p, n, MSG_NOSIGNAL | MSG_DONTWAIT);
|
||||
if (w < 0) {
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) continue;
|
||||
sys_fail("send");
|
||||
}
|
||||
p += w;
|
||||
n -= static_cast<std::size_t>(w);
|
||||
}
|
||||
}
|
||||
|
||||
void read_exact(int fd, std::uint8_t* p, std::size_t n, Clock::time_point deadline) {
|
||||
while (n > 0) {
|
||||
if (!wait_fd(fd, POLLIN, ms_left(deadline)))
|
||||
throw LinkError(Kind::Timeout, "timed out waiting for the board");
|
||||
const ssize_t r = ::recv(fd, p, n, MSG_DONTWAIT);
|
||||
if (r == 0) throw LinkError(Kind::Io, "the board closed the connection");
|
||||
if (r < 0) {
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) continue;
|
||||
sys_fail("recv");
|
||||
}
|
||||
p += r;
|
||||
n -= static_cast<std::size_t>(r);
|
||||
}
|
||||
}
|
||||
|
||||
/// A connected, non-blocking TCP socket with TCP_NODELAY. `rcvbuf > 0` sets SO_RCVBUF first.
|
||||
Fd tcp_connect(const std::string& host, int port, int timeout_ms, int rcvbuf) {
|
||||
const sockaddr_in a = make_addr(host, port);
|
||||
Fd fd(::socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0));
|
||||
if (!fd.valid()) sys_fail("socket");
|
||||
const int one = 1;
|
||||
::setsockopt(fd.get(), IPPROTO_TCP, TCP_NODELAY, &one, sizeof one);
|
||||
if (rcvbuf > 0) ::setsockopt(fd.get(), SOL_SOCKET, SO_RCVBUF, &rcvbuf, sizeof rcvbuf);
|
||||
const std::string where = host + ":" + std::to_string(port);
|
||||
if (::connect(fd.get(), reinterpret_cast<const sockaddr*>(&a), sizeof a) < 0 &&
|
||||
errno != EINPROGRESS)
|
||||
sys_fail("cannot connect to " + where);
|
||||
if (!wait_fd(fd.get(), POLLOUT, timeout_ms))
|
||||
throw LinkError(Kind::Timeout, "timed out connecting to " + where);
|
||||
int err = 0;
|
||||
socklen_t len = sizeof err;
|
||||
::getsockopt(fd.get(), SOL_SOCKET, SO_ERROR, &err, &len);
|
||||
if (err != 0) {
|
||||
errno = err;
|
||||
sys_fail("cannot connect to " + where);
|
||||
}
|
||||
return fd;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// --- addresses ------------------------------------------------------------------------------
|
||||
|
||||
bool parse_mac(const std::string& text, Mac& out) {
|
||||
Mac m{};
|
||||
std::size_t pos = 0;
|
||||
for (std::size_t i = 0; i < 6; ++i) {
|
||||
if (pos + 2 > text.size()) return false;
|
||||
unsigned v = 0;
|
||||
for (std::size_t k = 0; k < 2; ++k) {
|
||||
const char c = text[pos + k];
|
||||
v <<= 4;
|
||||
if (c >= '0' && c <= '9') v |= static_cast<unsigned>(c - '0');
|
||||
else if (c >= 'a' && c <= 'f') v |= static_cast<unsigned>(c - 'a' + 10);
|
||||
else if (c >= 'A' && c <= 'F') v |= static_cast<unsigned>(c - 'A' + 10);
|
||||
else return false;
|
||||
}
|
||||
m[i] = static_cast<std::uint8_t>(v);
|
||||
pos += 2;
|
||||
if (i < 5) {
|
||||
if (pos >= text.size() || (text[pos] != ':' && text[pos] != '-')) return false;
|
||||
++pos;
|
||||
}
|
||||
}
|
||||
if (pos != text.size()) return false;
|
||||
out = m;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string format_mac(const Mac& mac) {
|
||||
char buf[24];
|
||||
std::snprintf(buf, sizeof buf, "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1], mac[2], mac[3],
|
||||
mac[4], mac[5]);
|
||||
return buf;
|
||||
}
|
||||
|
||||
std::vector<std::string> sweep_targets(const std::string& subnet) {
|
||||
std::string s = subnet;
|
||||
while (!s.empty() && s.front() == ' ') s.erase(s.begin());
|
||||
while (!s.empty() && (s.back() == ' ' || s.back() == '.')) s.pop_back();
|
||||
if (s.empty()) return {"255.255.255.255"};
|
||||
int parts = 0;
|
||||
std::size_t i = 0;
|
||||
while (i <= s.size()) {
|
||||
const std::size_t dot = s.find('.', i);
|
||||
const std::string part = s.substr(i, dot == std::string::npos ? std::string::npos : dot - i);
|
||||
if (part.empty() || part.size() > 3) return {};
|
||||
int v = 0;
|
||||
for (char c : part) {
|
||||
if (c < '0' || c > '9') return {};
|
||||
v = v * 10 + (c - '0');
|
||||
}
|
||||
if (v > 255) return {};
|
||||
++parts;
|
||||
if (dot == std::string::npos) break;
|
||||
i = dot + 1;
|
||||
}
|
||||
if (parts != 3) return {};
|
||||
std::vector<std::string> out;
|
||||
out.reserve(254);
|
||||
for (int h = 1; h <= 254; ++h) out.push_back(s + "." + std::to_string(h));
|
||||
return out;
|
||||
}
|
||||
|
||||
std::vector<DeviceInfo> discover(const std::vector<std::string>& targets, int port, int wait_ms) {
|
||||
std::vector<DeviceInfo> found;
|
||||
Fd u(::socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, 0));
|
||||
if (!u.valid()) sys_fail("socket");
|
||||
const int one = 1;
|
||||
::setsockopt(u.get(), SOL_SOCKET, SO_BROADCAST, &one, sizeof one);
|
||||
const std::uint8_t probe = 'D';
|
||||
for (const std::string& t : targets) {
|
||||
sockaddr_in a{};
|
||||
try {
|
||||
a = make_addr(t, port);
|
||||
} catch (const LinkError&) {
|
||||
continue;
|
||||
}
|
||||
// A sweep may hit unreachable hosts; those sends fail and are simply skipped.
|
||||
(void)::sendto(u.get(), &probe, 1, MSG_NOSIGNAL, reinterpret_cast<const sockaddr*>(&a),
|
||||
sizeof a);
|
||||
}
|
||||
const auto deadline = Clock::now() + std::chrono::milliseconds(wait_ms);
|
||||
for (;;) {
|
||||
if (!wait_fd(u.get(), POLLIN, ms_left(deadline))) break;
|
||||
std::uint8_t buf[128];
|
||||
sockaddr_in from{};
|
||||
socklen_t flen = sizeof from;
|
||||
const ssize_t n = ::recvfrom(u.get(), buf, sizeof buf, MSG_DONTWAIT,
|
||||
reinterpret_cast<sockaddr*>(&from), &flen);
|
||||
if (n < 0) {
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) continue;
|
||||
break;
|
||||
}
|
||||
if (n < 29 || buf[0] != 'D') continue;
|
||||
DeviceInfo info;
|
||||
info.product_id = static_cast<std::uint16_t>(buf[7] | (buf[8] << 8));
|
||||
if (info.product_id != kProductId) continue;
|
||||
std::copy(buf + 1, buf + 7, info.mac.begin());
|
||||
bool dup = false;
|
||||
for (const DeviceInfo& d : found) dup = dup || d.mac == info.mac;
|
||||
if (dup) continue;
|
||||
char ip[INET_ADDRSTRLEN] = {};
|
||||
::inet_ntop(AF_INET, &from.sin_addr, ip, sizeof ip);
|
||||
info.ip = ip;
|
||||
found.push_back(info);
|
||||
}
|
||||
return found;
|
||||
}
|
||||
|
||||
// --- Link -----------------------------------------------------------------------------------
|
||||
|
||||
void Link::connect(const std::string& host, int port, std::uint32_t code, int timeout_ms) {
|
||||
close();
|
||||
host_ = host;
|
||||
port_ = port;
|
||||
timeout_ms_ = timeout_ms;
|
||||
next_id_ = 0;
|
||||
|
||||
// The connection-code handshake: a connected UDP socket, so an ICMP refusal surfaces.
|
||||
{
|
||||
const sockaddr_in a = make_addr(host, port);
|
||||
Fd u(::socket(AF_INET, SOCK_DGRAM, 0));
|
||||
if (!u.valid()) sys_fail("socket");
|
||||
if (::connect(u.get(), reinterpret_cast<const sockaddr*>(&a), sizeof a) < 0)
|
||||
sys_fail("handshake with " + host);
|
||||
const std::uint8_t msg[5] = {'C', static_cast<std::uint8_t>(code & 0xFF),
|
||||
static_cast<std::uint8_t>((code >> 8) & 0xFF),
|
||||
static_cast<std::uint8_t>((code >> 16) & 0xFF),
|
||||
static_cast<std::uint8_t>((code >> 24) & 0xFF)};
|
||||
if (::send(u.get(), msg, sizeof msg, MSG_NOSIGNAL) < 0) sys_fail("handshake with " + host);
|
||||
if (!wait_fd(u.get(), POLLIN, timeout_ms))
|
||||
throw LinkError(Kind::Timeout, "no answer from the board at " + host);
|
||||
std::uint8_t rep[16];
|
||||
const ssize_t n = ::recv(u.get(), rep, sizeof rep, 0);
|
||||
if (n < 0) sys_fail("handshake with " + host);
|
||||
if (n < 2 || rep[0] != 'C') throw LinkError(Kind::Protocol, "unexpected handshake reply");
|
||||
switch (rep[1]) {
|
||||
case 0:
|
||||
case 2:
|
||||
break;
|
||||
case 1:
|
||||
throw LinkError(Kind::Rejected, "the board rejected the connection code");
|
||||
case 3:
|
||||
throw LinkError(Kind::Busy, "the board is busy (another client holds it)");
|
||||
default:
|
||||
throw LinkError(Kind::Protocol, "unexpected handshake status " + std::to_string(rep[1]));
|
||||
}
|
||||
}
|
||||
cmd_ = tcp_connect(host, port, timeout_ms, 0);
|
||||
}
|
||||
|
||||
void Link::close() {
|
||||
scan_.reset();
|
||||
cmd_.reset();
|
||||
}
|
||||
|
||||
std::vector<std::uint8_t> Link::command(std::uint8_t cmd, const std::vector<std::uint8_t>& payload,
|
||||
int expect) {
|
||||
if (!cmd_.valid()) throw LinkError(Kind::Io, "not connected");
|
||||
if (payload.size() + 7 > kMaxFrame) throw LinkError(Kind::Protocol, "command too long");
|
||||
// Bytes left by an earlier exchange that timed out must not be read as this reply.
|
||||
{
|
||||
std::uint8_t junk[256];
|
||||
while (::recv(cmd_.get(), junk, sizeof junk, MSG_DONTWAIT) > 0) {
|
||||
}
|
||||
}
|
||||
const std::uint8_t id = ++next_id_;
|
||||
std::vector<std::uint8_t> f;
|
||||
f.reserve(7 + payload.size());
|
||||
f.push_back(0xDB);
|
||||
f.push_back(cmd);
|
||||
f.push_back(id);
|
||||
f.push_back(0);
|
||||
f.push_back(static_cast<std::uint8_t>(payload.size() & 0xFF));
|
||||
f.push_back(static_cast<std::uint8_t>((payload.size() >> 8) & 0xFF));
|
||||
f.insert(f.end(), payload.begin(), payload.end());
|
||||
unsigned sum = 0;
|
||||
for (std::uint8_t b : f) sum += b;
|
||||
f.push_back(static_cast<std::uint8_t>(0xFF - (sum & 0xFF)));
|
||||
|
||||
const auto deadline = Clock::now() + std::chrono::milliseconds(timeout_ms_);
|
||||
send_all(cmd_.get(), f.data(), f.size(), deadline);
|
||||
|
||||
std::uint8_t h[6];
|
||||
read_exact(cmd_.get(), h, sizeof h, deadline);
|
||||
if (h[0] != 0xDB) throw LinkError(Kind::Protocol, "bad sync byte in a reply");
|
||||
const std::size_t n = static_cast<std::size_t>(h[4] | (h[5] << 8));
|
||||
if (n + 7 > kMaxFrame) throw LinkError(Kind::Protocol, "oversized reply");
|
||||
std::vector<std::uint8_t> body(n + 1);
|
||||
read_exact(cmd_.get(), body.data(), body.size(), deadline);
|
||||
unsigned rsum = 0;
|
||||
for (std::uint8_t b : h) rsum += b;
|
||||
for (std::uint8_t b : body) rsum += b;
|
||||
if ((rsum & 0xFF) != 0xFF) throw LinkError(Kind::Protocol, "bad checksum in a reply");
|
||||
if (h[1] != (cmd | 0x80) || h[2] != id)
|
||||
throw LinkError(Kind::Protocol, "reply does not match the command");
|
||||
if (h[3] != 0) {
|
||||
static const char* const kText[] = {"ok", "protocol error", "parameter error",
|
||||
"busy", "not ready", "timeout",
|
||||
"other error"};
|
||||
const char* t = h[3] < 7 ? kText[h[3]] : "unknown error";
|
||||
char hex[8];
|
||||
std::snprintf(hex, sizeof hex, "%02x", cmd);
|
||||
throw LinkError(Kind::Command, std::string("the board refused command 0x") + hex + ": " + t);
|
||||
}
|
||||
body.pop_back();
|
||||
if (expect >= 0 && body.size() != static_cast<std::size_t>(expect))
|
||||
throw LinkError(Kind::Command, "reply of " + std::to_string(body.size()) +
|
||||
" bytes, expected " + std::to_string(expect));
|
||||
return body;
|
||||
}
|
||||
|
||||
void Link::open_scan() {
|
||||
close_scan();
|
||||
scan_ = tcp_connect(host_, port_ + 1, timeout_ms_, 1 << 20);
|
||||
}
|
||||
|
||||
void Link::close_scan() { scan_.reset(); }
|
||||
|
||||
void Link::send_filler() {
|
||||
if (!cmd_.valid()) throw LinkError(Kind::Io, "not connected");
|
||||
const std::uint8_t z = 0;
|
||||
send_all(cmd_.get(), &z, 1, Clock::now() + std::chrono::milliseconds(timeout_ms_));
|
||||
}
|
||||
|
||||
std::size_t Link::read_scan(std::uint8_t* buf, std::size_t cap, int timeout_ms) {
|
||||
if (!scan_.valid()) throw LinkError(Kind::Io, "the scan socket is not open");
|
||||
if (!wait_fd(scan_.get(), POLLIN, timeout_ms)) return 0;
|
||||
const ssize_t n = ::recv(scan_.get(), buf, cap, MSG_DONTWAIT);
|
||||
if (n > 0) return static_cast<std::size_t>(n);
|
||||
if (n == 0) throw LinkError(Kind::Io, "scan socket closed by the board");
|
||||
if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) return 0;
|
||||
sys_fail("scan recv");
|
||||
}
|
||||
|
||||
} // namespace udpscope::e1608
|
||||
Reference in new issue
Block a user