initial import from picomap firmware
This commit is contained in:
Ian Gulliver
@@ -0,0 +1,24 @@
|
||||
#pragma once
|
||||
#include <span>
|
||||
#include "eth.h"
|
||||
#include "ipv4.h"
|
||||
#include "span_writer.h"
|
||||
|
||||
namespace arp {
|
||||
|
||||
struct __attribute__((packed)) header {
|
||||
uint16_t htype;
|
||||
uint16_t ptype;
|
||||
uint8_t hlen;
|
||||
uint8_t plen;
|
||||
uint16_t oper;
|
||||
eth::mac_addr sha;
|
||||
ipv4::ip4_addr spa;
|
||||
eth::mac_addr tha;
|
||||
ipv4::ip4_addr tpa;
|
||||
};
|
||||
static_assert(sizeof(header) == 28);
|
||||
|
||||
void handle(std::span<const uint8_t> frame, span_writer& tx);
|
||||
|
||||
} // namespace arp
|
||||
@@ -0,0 +1,80 @@
|
||||
#pragma once
|
||||
#include <utility>
|
||||
|
||||
template <typename T, int N>
|
||||
struct callback_list {
|
||||
struct node {
|
||||
T value;
|
||||
node* prev = nullptr;
|
||||
node* next = nullptr;
|
||||
};
|
||||
|
||||
node nodes[N];
|
||||
node* free_head = &nodes[0];
|
||||
node* head = nullptr;
|
||||
|
||||
callback_list() {
|
||||
for (int i = 0; i < N - 1; i++) nodes[i].next = &nodes[i + 1];
|
||||
nodes[N - 1].next = nullptr;
|
||||
}
|
||||
|
||||
bool empty() const { return head == nullptr; }
|
||||
|
||||
node* insert(T value) {
|
||||
if (!free_head) return nullptr;
|
||||
node* n = free_head;
|
||||
free_head = n->next;
|
||||
n->value = std::move(value);
|
||||
n->prev = nullptr;
|
||||
n->next = head;
|
||||
if (head) head->prev = n;
|
||||
head = n;
|
||||
return n;
|
||||
}
|
||||
|
||||
template <typename Less>
|
||||
node* insert_sorted(T value, Less&& less) {
|
||||
if (!free_head) return nullptr;
|
||||
node* n = free_head;
|
||||
free_head = n->next;
|
||||
n->value = std::move(value);
|
||||
if (!head || less(n->value, head->value)) {
|
||||
n->prev = nullptr;
|
||||
n->next = head;
|
||||
if (head) head->prev = n;
|
||||
head = n;
|
||||
return n;
|
||||
}
|
||||
node* cur = head;
|
||||
while (cur->next && !less(n->value, cur->next->value))
|
||||
cur = cur->next;
|
||||
n->prev = cur;
|
||||
n->next = cur->next;
|
||||
if (cur->next) cur->next->prev = n;
|
||||
cur->next = n;
|
||||
return n;
|
||||
}
|
||||
|
||||
void remove(node* n) {
|
||||
if (!n) return;
|
||||
if (n->prev) n->prev->next = n->next;
|
||||
else head = n->next;
|
||||
if (n->next) n->next->prev = n->prev;
|
||||
n->value = T{};
|
||||
n->next = free_head;
|
||||
n->prev = nullptr;
|
||||
free_head = n;
|
||||
}
|
||||
|
||||
node* front() { return head; }
|
||||
|
||||
template <typename Fn>
|
||||
void for_each(Fn&& fn) {
|
||||
node* cur = head;
|
||||
while (cur) {
|
||||
node* next = cur->next;
|
||||
fn(cur);
|
||||
cur = next;
|
||||
}
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,39 @@
|
||||
#pragma once
|
||||
#include <cstdarg>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <vector>
|
||||
#include "pico/time.h"
|
||||
#include "ring_buffer.h"
|
||||
|
||||
struct log_entry {
|
||||
uint32_t timestamp_us;
|
||||
std::string message;
|
||||
};
|
||||
|
||||
inline ring_buffer<log_entry, 32> g_debug_log;
|
||||
|
||||
inline void dlog(std::string_view msg) {
|
||||
g_debug_log.push_overwrite(log_entry{static_cast<uint32_t>(time_us_32()), std::string(msg)});
|
||||
}
|
||||
|
||||
__attribute__((format(printf, 1, 2)))
|
||||
inline void dlogf(const char* fmt, ...) {
|
||||
char buf[128];
|
||||
va_list args;
|
||||
va_start(args, fmt);
|
||||
vsnprintf(buf, sizeof(buf), fmt, args);
|
||||
va_end(args);
|
||||
dlog(buf);
|
||||
}
|
||||
|
||||
template <typename F>
|
||||
inline void dlog_if_slow(std::string_view label, uint32_t threshold_us, F&& fn) {
|
||||
uint32_t t0 = time_us_32();
|
||||
fn();
|
||||
uint32_t elapsed = time_us_32() - t0;
|
||||
if (elapsed > threshold_us)
|
||||
dlogf("%.*s %luus", static_cast<int>(label.size()), label.data(), static_cast<unsigned long>(elapsed));
|
||||
}
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <optional>
|
||||
#include <span>
|
||||
#include "wire.h"
|
||||
#include "timer_queue.h"
|
||||
#include "net.h"
|
||||
#include "prepend_buffer.h"
|
||||
#include "udp.h"
|
||||
|
||||
uint16_t dispatch_listen_port_be();
|
||||
|
||||
struct responder {
|
||||
uint32_t message_id;
|
||||
udp::address reply_to;
|
||||
|
||||
template <typename T>
|
||||
void respond(const T& msg) const {
|
||||
const auto& ns = net_get_state();
|
||||
prepend_buffer<4096> buf;
|
||||
span_writer out(buf.payload_ptr(), 2048);
|
||||
auto r = encode_response_into(out, message_id, msg);
|
||||
if (!r) return;
|
||||
buf.append(*r);
|
||||
udp::prepend(buf, reply_to.mac, ns.mac, ns.ip, reply_to.ip,
|
||||
dispatch_listen_port_be(), reply_to.port, *r);
|
||||
net_send_raw(buf.span());
|
||||
}
|
||||
};
|
||||
|
||||
using handler_fn = void (*)(const responder& resp, std::span<const uint8_t> payload);
|
||||
|
||||
struct handler_entry {
|
||||
int8_t type_id;
|
||||
handler_fn handle;
|
||||
};
|
||||
|
||||
template <typename Req, auto Fn>
|
||||
void typed_handler(const responder& resp, std::span<const uint8_t> payload) {
|
||||
msgpack::parser p(payload.data(), static_cast<int>(payload.size()));
|
||||
Req req;
|
||||
auto tup = req.as_tuple();
|
||||
auto r = msgpack::unpack(p, tup);
|
||||
if (!r) {
|
||||
char err[64];
|
||||
snprintf(err, sizeof(err), "decode request ext_id=%d: msgpack error %d",
|
||||
Req::ext_id, static_cast<int>(r.error()));
|
||||
resp.respond(DeviceError{1, err});
|
||||
return;
|
||||
}
|
||||
auto result = Fn(resp, req);
|
||||
if (result)
|
||||
resp.respond(*result);
|
||||
}
|
||||
|
||||
void dispatch_init(uint16_t listen_port_be);
|
||||
timer_handle dispatch_schedule_ms(uint32_t ms, void (*fn)());
|
||||
bool dispatch_cancel_timer(timer_handle h);
|
||||
[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers);
|
||||
@@ -0,0 +1,28 @@
|
||||
#pragma once
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
|
||||
namespace eth {
|
||||
|
||||
using mac_addr = std::array<uint8_t, 6>;
|
||||
|
||||
static constexpr mac_addr MAC_BROADCAST = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||||
static constexpr uint16_t ETH_ARP = __builtin_bswap16(0x0806);
|
||||
static constexpr uint16_t ETH_IPV4 = __builtin_bswap16(0x0800);
|
||||
|
||||
struct __attribute__((packed)) header {
|
||||
mac_addr dst;
|
||||
mac_addr src;
|
||||
uint16_t ethertype;
|
||||
};
|
||||
static_assert(sizeof(header) == 14);
|
||||
|
||||
template <typename Buf>
|
||||
void prepend(Buf& buf, const mac_addr& dst, const mac_addr& src, uint16_t ethertype) {
|
||||
auto* h = buf.template prepend<header>();
|
||||
h->dst = dst;
|
||||
h->src = src;
|
||||
h->ethertype = ethertype;
|
||||
}
|
||||
|
||||
} // namespace eth
|
||||
@@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <tuple>
|
||||
|
||||
namespace flash {
|
||||
|
||||
constexpr uint32_t FLASH_BASE = 0x10000000;
|
||||
constexpr uint32_t FLASH_SIZE = 2 * 1024 * 1024;
|
||||
|
||||
struct slot {
|
||||
bool valid;
|
||||
uint32_t version;
|
||||
bool hash_ok;
|
||||
auto as_tuple() const { return std::tie(valid, version, hash_ok); }
|
||||
auto as_tuple() { return std::tie(valid, version, hash_ok); }
|
||||
};
|
||||
|
||||
slot scan(uint32_t flash_offset);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
#include <span>
|
||||
|
||||
namespace halfsiphash {
|
||||
|
||||
namespace detail {
|
||||
|
||||
constexpr uint32_t rotl(uint32_t x, int b) {
|
||||
return (x << b) | (x >> (32 - b));
|
||||
}
|
||||
|
||||
constexpr uint32_t load_le32(const uint8_t *p) {
|
||||
return static_cast<uint32_t>(p[0])
|
||||
| (static_cast<uint32_t>(p[1]) << 8)
|
||||
| (static_cast<uint32_t>(p[2]) << 16)
|
||||
| (static_cast<uint32_t>(p[3]) << 24);
|
||||
}
|
||||
|
||||
inline void store_le32(uint8_t *p, uint32_t v) {
|
||||
p[0] = static_cast<uint8_t>(v);
|
||||
p[1] = static_cast<uint8_t>(v >> 8);
|
||||
p[2] = static_cast<uint8_t>(v >> 16);
|
||||
p[3] = static_cast<uint8_t>(v >> 24);
|
||||
}
|
||||
|
||||
inline void sipround(uint32_t &v0, uint32_t &v1, uint32_t &v2, uint32_t &v3) {
|
||||
v0 += v1; v1 = rotl(v1, 5); v1 ^= v0; v0 = rotl(v0, 16);
|
||||
v2 += v3; v3 = rotl(v3, 8); v3 ^= v2;
|
||||
v0 += v3; v3 = rotl(v3, 7); v3 ^= v0;
|
||||
v2 += v1; v1 = rotl(v1, 13); v1 ^= v2; v2 = rotl(v2, 16);
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
// Compute HalfSipHash-2-4 with an 8-byte key, returning a 32-bit hash.
|
||||
inline uint32_t hash32(std::span<const uint8_t> data, const uint8_t key[8]) {
|
||||
using namespace detail;
|
||||
|
||||
uint32_t k0 = load_le32(key);
|
||||
uint32_t k1 = load_le32(key + 4);
|
||||
|
||||
uint32_t v0 = 0 ^ k0;
|
||||
uint32_t v1 = 0 ^ k1;
|
||||
uint32_t v2 = UINT32_C(0x6c796765) ^ k0;
|
||||
uint32_t v3 = UINT32_C(0x74656462) ^ k1;
|
||||
|
||||
const uint8_t *end = data.data() + data.size() - (data.size() % 4);
|
||||
for (const uint8_t *p = data.data(); p != end; p += 4) {
|
||||
uint32_t m = load_le32(p);
|
||||
v3 ^= m;
|
||||
sipround(v0, v1, v2, v3);
|
||||
sipround(v0, v1, v2, v3);
|
||||
v0 ^= m;
|
||||
}
|
||||
|
||||
uint32_t b = static_cast<uint32_t>(data.size()) << 24;
|
||||
switch (data.size() & 3) {
|
||||
case 3: b |= static_cast<uint32_t>(end[2]) << 16; [[fallthrough]];
|
||||
case 2: b |= static_cast<uint32_t>(end[1]) << 8; [[fallthrough]];
|
||||
case 1: b |= static_cast<uint32_t>(end[0]); break;
|
||||
case 0: break;
|
||||
}
|
||||
|
||||
v3 ^= b;
|
||||
sipround(v0, v1, v2, v3);
|
||||
sipround(v0, v1, v2, v3);
|
||||
v0 ^= b;
|
||||
|
||||
v2 ^= 0xff;
|
||||
sipround(v0, v1, v2, v3);
|
||||
sipround(v0, v1, v2, v3);
|
||||
sipround(v0, v1, v2, v3);
|
||||
sipround(v0, v1, v2, v3);
|
||||
|
||||
return v1 ^ v3;
|
||||
}
|
||||
|
||||
} // namespace halfsiphash
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string_view>
|
||||
#include "dispatch.h"
|
||||
#include "ipv4.h"
|
||||
#include "wire.h"
|
||||
|
||||
inline constexpr uint16_t PICOMAP_PORT_BE = __builtin_bswap16(28781);
|
||||
inline constexpr ipv4::ip4_addr PICOMAP_DISCOVERY_GROUP = {239, 112, 77, 1};
|
||||
|
||||
extern std::string_view firmware_name;
|
||||
|
||||
void handlers_init();
|
||||
void handlers_start();
|
||||
std::optional<ResponseInfo> handle_info(const responder& resp, const RequestInfo&);
|
||||
std::optional<ResponseLog> handle_log(const responder& resp, const RequestLog&);
|
||||
std::optional<ResponseFlashErase> handle_flash_erase(const responder& resp, const RequestFlashErase&);
|
||||
std::optional<ResponseFlashWrite> handle_flash_write(const responder& resp, const RequestFlashWrite&);
|
||||
std::optional<ResponseReboot> handle_reboot(const responder& resp, const RequestReboot&);
|
||||
std::optional<ResponseFlashStatus> handle_flash_status(const responder& resp, const RequestFlashStatus&);
|
||||
@@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include "eth.h"
|
||||
#include "ipv4.h"
|
||||
#include "span_writer.h"
|
||||
|
||||
namespace icmp {
|
||||
|
||||
struct __attribute__((packed)) echo {
|
||||
uint8_t type;
|
||||
uint8_t code;
|
||||
uint16_t checksum;
|
||||
uint16_t id;
|
||||
uint16_t seq;
|
||||
};
|
||||
static_assert(sizeof(echo) == 8);
|
||||
|
||||
void handle(std::span<const uint8_t> frame, span_writer& tx);
|
||||
|
||||
template <typename Buf>
|
||||
void prepend_echo_request(Buf& buf,
|
||||
eth::mac_addr src_mac, ipv4::ip4_addr src_ip,
|
||||
eth::mac_addr dst_mac, ipv4::ip4_addr dst_ip,
|
||||
uint16_t id, uint16_t seq,
|
||||
size_t payload_len = 0) {
|
||||
auto* e = buf.template prepend<echo>();
|
||||
e->type = 8;
|
||||
e->code = 0;
|
||||
e->checksum = 0;
|
||||
e->id = id;
|
||||
e->seq = seq;
|
||||
size_t icmp_len = sizeof(echo) + payload_len;
|
||||
e->checksum = ipv4::checksum(e, icmp_len);
|
||||
ipv4::prepend(buf, dst_mac, src_mac, src_ip, dst_ip, 1, icmp_len);
|
||||
}
|
||||
|
||||
bool parse_echo_reply(std::span<const uint8_t> frame, ipv4::ip4_addr& src_ip, uint16_t expected_id);
|
||||
|
||||
} // namespace icmp
|
||||
@@ -0,0 +1,57 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include "eth.h"
|
||||
#include "ipv4.h"
|
||||
#include "span_writer.h"
|
||||
|
||||
namespace igmp {
|
||||
|
||||
static constexpr ipv4::ip4_addr ALL_HOSTS = {224, 0, 0, 1};
|
||||
|
||||
struct __attribute__((packed)) message {
|
||||
uint8_t type;
|
||||
uint8_t max_resp_time;
|
||||
uint16_t checksum;
|
||||
ipv4::ip4_addr group;
|
||||
};
|
||||
static_assert(sizeof(message) == 8);
|
||||
|
||||
eth::mac_addr mac_for_ip(const ipv4::ip4_addr& group);
|
||||
bool is_member(const ipv4::ip4_addr& ip);
|
||||
bool is_member_mac(const eth::mac_addr& mac);
|
||||
|
||||
void join(const ipv4::ip4_addr& group);
|
||||
|
||||
void send_all_reports();
|
||||
|
||||
void handle(std::span<const uint8_t> frame, span_writer& tx);
|
||||
|
||||
template <typename Buf>
|
||||
void prepend_report(Buf& buf, const eth::mac_addr& src_mac, ipv4::ip4_addr src_ip,
|
||||
const ipv4::ip4_addr& group) {
|
||||
auto* m = buf.template prepend<message>();
|
||||
m->type = 0x16;
|
||||
m->max_resp_time = 0;
|
||||
m->checksum = 0;
|
||||
m->group = group;
|
||||
m->checksum = ipv4::checksum(m, sizeof(message));
|
||||
ipv4::prepend(buf, mac_for_ip(group), src_mac, src_ip, group, 2, sizeof(message), 1);
|
||||
}
|
||||
|
||||
template <typename Buf>
|
||||
void prepend_query(Buf& buf, const eth::mac_addr& src_mac, ipv4::ip4_addr src_ip,
|
||||
const ipv4::ip4_addr& group) {
|
||||
ipv4::ip4_addr dst_ip = (group == ipv4::ip4_addr{0, 0, 0, 0}) ? ALL_HOSTS : group;
|
||||
auto* m = buf.template prepend<message>();
|
||||
m->type = 0x11;
|
||||
m->max_resp_time = 100;
|
||||
m->checksum = 0;
|
||||
m->group = group;
|
||||
m->checksum = ipv4::checksum(m, sizeof(message));
|
||||
ipv4::prepend(buf, mac_for_ip(dst_ip), src_mac, src_ip, dst_ip, 2, sizeof(message), 1);
|
||||
}
|
||||
|
||||
bool parse_report(std::span<const uint8_t> frame, ipv4::ip4_addr& group);
|
||||
|
||||
} // namespace igmp
|
||||
@@ -0,0 +1,67 @@
|
||||
#pragma once
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include "eth.h"
|
||||
#include "span_writer.h"
|
||||
|
||||
namespace ipv4 {
|
||||
|
||||
using ip4_addr = std::array<uint8_t, 4>;
|
||||
|
||||
inline std::string to_string(const ip4_addr& ip) {
|
||||
char buf[16];
|
||||
snprintf(buf, sizeof(buf), "%u.%u.%u.%u", ip[0], ip[1], ip[2], ip[3]);
|
||||
return buf;
|
||||
}
|
||||
|
||||
struct __attribute__((packed)) header {
|
||||
uint8_t ver_ihl;
|
||||
uint8_t dscp_ecn;
|
||||
uint16_t total_len;
|
||||
uint16_t identification;
|
||||
uint16_t flags_frag;
|
||||
uint8_t ttl;
|
||||
uint8_t protocol;
|
||||
uint16_t checksum;
|
||||
ip4_addr src;
|
||||
ip4_addr dst;
|
||||
|
||||
size_t header_len() const { return (ver_ihl & 0x0F) * 4; }
|
||||
size_t total() const { return __builtin_bswap16(total_len); }
|
||||
};
|
||||
static_assert(sizeof(header) == 20);
|
||||
|
||||
uint16_t checksum(const void* data, size_t len);
|
||||
|
||||
static constexpr ip4_addr SUBNET_BROADCAST = {169, 254, 255, 255};
|
||||
|
||||
template <typename Buf>
|
||||
void prepend(Buf& buf, const eth::mac_addr& dst_mac, const eth::mac_addr& src_mac,
|
||||
ip4_addr src_ip, ip4_addr dst_ip, uint8_t protocol,
|
||||
size_t payload_len, uint8_t ttl = 64) {
|
||||
auto* h = buf.template prepend<header>();
|
||||
h->ver_ihl = 0x45;
|
||||
h->dscp_ecn = 0;
|
||||
h->total_len = __builtin_bswap16(sizeof(header) + payload_len);
|
||||
h->identification = 0;
|
||||
h->flags_frag = 0;
|
||||
h->ttl = ttl;
|
||||
h->protocol = protocol;
|
||||
h->checksum = 0;
|
||||
h->src = src_ip;
|
||||
h->dst = dst_ip;
|
||||
h->checksum = checksum(h, sizeof(header));
|
||||
eth::prepend(buf, dst_mac, src_mac, eth::ETH_IPV4);
|
||||
}
|
||||
|
||||
void handle(std::span<const uint8_t> frame, span_writer& tx);
|
||||
|
||||
bool addressed_to_us(ip4_addr dst);
|
||||
|
||||
using protocol_handler = void (*)(std::span<const uint8_t> frame, span_writer& tx);
|
||||
void register_protocol(uint8_t protocol, protocol_handler fn);
|
||||
|
||||
} // namespace ipv4
|
||||
@@ -0,0 +1,857 @@
|
||||
#pragma once
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <expected>
|
||||
#include <iterator>
|
||||
#include <limits>
|
||||
#include <string_view>
|
||||
#include <tuple>
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
#include "span_writer.h"
|
||||
|
||||
namespace msgpack {
|
||||
|
||||
enum class error_code {
|
||||
overflow,
|
||||
empty,
|
||||
lack,
|
||||
invalid,
|
||||
type_error,
|
||||
};
|
||||
|
||||
namespace format {
|
||||
constexpr uint8_t POSITIVE_FIXINT_MIN = 0x00;
|
||||
constexpr uint8_t POSITIVE_FIXINT_MAX = 0x7F;
|
||||
constexpr uint8_t FIXMAP_MIN = 0x80;
|
||||
constexpr uint8_t FIXMAP_MAX = 0x8F;
|
||||
constexpr uint8_t FIXARRAY_MIN = 0x90;
|
||||
constexpr uint8_t FIXARRAY_MAX = 0x9F;
|
||||
constexpr uint8_t FIXSTR_MIN = 0xA0;
|
||||
constexpr uint8_t FIXSTR_MAX = 0xBF;
|
||||
constexpr uint8_t NEGATIVE_FIXINT_MIN = 0xE0;
|
||||
constexpr uint8_t NEGATIVE_FIXINT_MAX = 0xFF;
|
||||
|
||||
constexpr uint8_t NIL = 0xC0;
|
||||
constexpr uint8_t NEVER_USED = 0xC1;
|
||||
constexpr uint8_t FALSE = 0xC2;
|
||||
constexpr uint8_t TRUE = 0xC3;
|
||||
constexpr uint8_t BIN8 = 0xC4;
|
||||
constexpr uint8_t BIN16 = 0xC5;
|
||||
constexpr uint8_t BIN32 = 0xC6;
|
||||
constexpr uint8_t EXT8 = 0xC7;
|
||||
constexpr uint8_t EXT16 = 0xC8;
|
||||
constexpr uint8_t EXT32 = 0xC9;
|
||||
constexpr uint8_t FLOAT32 = 0xCA;
|
||||
constexpr uint8_t FLOAT64 = 0xCB;
|
||||
constexpr uint8_t UINT8 = 0xCC;
|
||||
constexpr uint8_t UINT16 = 0xCD;
|
||||
constexpr uint8_t UINT32 = 0xCE;
|
||||
constexpr uint8_t UINT64 = 0xCF;
|
||||
constexpr uint8_t INT8 = 0xD0;
|
||||
constexpr uint8_t INT16 = 0xD1;
|
||||
constexpr uint8_t INT32 = 0xD2;
|
||||
constexpr uint8_t INT64 = 0xD3;
|
||||
constexpr uint8_t FIXEXT1 = 0xD4;
|
||||
constexpr uint8_t FIXEXT2 = 0xD5;
|
||||
constexpr uint8_t FIXEXT4 = 0xD6;
|
||||
constexpr uint8_t FIXEXT8 = 0xD7;
|
||||
constexpr uint8_t FIXEXT16 = 0xD8;
|
||||
constexpr uint8_t STR8 = 0xD9;
|
||||
constexpr uint8_t STR16 = 0xDA;
|
||||
constexpr uint8_t STR32 = 0xDB;
|
||||
constexpr uint8_t ARRAY16 = 0xDC;
|
||||
constexpr uint8_t ARRAY32 = 0xDD;
|
||||
constexpr uint8_t MAP16 = 0xDE;
|
||||
constexpr uint8_t MAP32 = 0xDF;
|
||||
|
||||
constexpr bool is_positive_fixint(uint8_t b) { return b <= POSITIVE_FIXINT_MAX; }
|
||||
constexpr bool is_fixmap(uint8_t b) { return b >= FIXMAP_MIN && b <= FIXMAP_MAX; }
|
||||
constexpr bool is_fixarray(uint8_t b) { return b >= FIXARRAY_MIN && b <= FIXARRAY_MAX; }
|
||||
constexpr bool is_fixstr(uint8_t b) { return b >= FIXSTR_MIN && b <= FIXSTR_MAX; }
|
||||
constexpr bool is_negative_fixint(uint8_t b) { return b >= NEGATIVE_FIXINT_MIN; }
|
||||
} // namespace format
|
||||
|
||||
template <typename T>
|
||||
using result = std::expected<T, error_code>;
|
||||
|
||||
template <typename T>
|
||||
result<T> body_number(const uint8_t *p, int size) {
|
||||
if (size < 1 + static_cast<int>(sizeof(T))) {
|
||||
return std::unexpected(error_code::lack);
|
||||
}
|
||||
if constexpr (sizeof(T) == 1) {
|
||||
return static_cast<T>(p[1]);
|
||||
} else if constexpr (sizeof(T) == 2) {
|
||||
return static_cast<T>((p[1] << 8) | p[2]);
|
||||
} else if constexpr (sizeof(T) == 4) {
|
||||
uint8_t buf[] = {p[4], p[3], p[2], p[1]};
|
||||
T val;
|
||||
__builtin_memcpy(&val, buf, sizeof(T));
|
||||
return val;
|
||||
} else if constexpr (sizeof(T) == 8) {
|
||||
uint8_t buf[] = {p[8], p[7], p[6], p[5], p[4], p[3], p[2], p[1]};
|
||||
T val;
|
||||
__builtin_memcpy(&val, buf, sizeof(T));
|
||||
return val;
|
||||
} else {
|
||||
return std::unexpected(error_code::invalid);
|
||||
}
|
||||
}
|
||||
|
||||
struct body_info {
|
||||
int header; // bytes before the body (includes format byte + length fields + ext type byte)
|
||||
uint32_t body; // body size in bytes (0 for containers, computed for variable-length)
|
||||
};
|
||||
|
||||
inline result<body_info> get_body_info(const uint8_t *p, int size) {
|
||||
if (size < 1) return std::unexpected(error_code::empty);
|
||||
uint8_t b = p[0];
|
||||
|
||||
using namespace format;
|
||||
|
||||
if (is_positive_fixint(b)) return body_info{1, 0};
|
||||
if (is_negative_fixint(b)) return body_info{1, 0};
|
||||
if (is_fixmap(b)) return body_info{1, 0}; // container
|
||||
if (is_fixarray(b)) return body_info{1, 0}; // container
|
||||
if (is_fixstr(b)) return body_info{1, static_cast<uint32_t>(b & 0x1F)};
|
||||
|
||||
switch (b) {
|
||||
case NIL: case FALSE: case TRUE:
|
||||
return body_info{1, 0};
|
||||
case NEVER_USED:
|
||||
return std::unexpected(error_code::invalid);
|
||||
|
||||
case BIN8: { auto n = body_number<uint8_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+1, *n}; }
|
||||
case BIN16: { auto n = body_number<uint16_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+2, *n}; }
|
||||
case BIN32: { auto n = body_number<uint32_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+4, *n}; }
|
||||
|
||||
case EXT8: { auto n = body_number<uint8_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+1+1, *n}; }
|
||||
case EXT16: { auto n = body_number<uint16_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+2+1, *n}; }
|
||||
case EXT32: { auto n = body_number<uint32_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+4+1, *n}; }
|
||||
|
||||
case FLOAT32: return body_info{1, 4};
|
||||
case FLOAT64: return body_info{1, 8};
|
||||
case UINT8: return body_info{1, 1};
|
||||
case UINT16: return body_info{1, 2};
|
||||
case UINT32: return body_info{1, 4};
|
||||
case UINT64: return body_info{1, 8};
|
||||
case INT8: return body_info{1, 1};
|
||||
case INT16: return body_info{1, 2};
|
||||
case INT32: return body_info{1, 4};
|
||||
case INT64: return body_info{1, 8};
|
||||
|
||||
case FIXEXT1: return body_info{1+1, 1};
|
||||
case FIXEXT2: return body_info{1+1, 2};
|
||||
case FIXEXT4: return body_info{1+1, 4};
|
||||
case FIXEXT8: return body_info{1+1, 8};
|
||||
case FIXEXT16: return body_info{1+1, 16};
|
||||
|
||||
case STR8: { auto n = body_number<uint8_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+1, *n}; }
|
||||
case STR16: { auto n = body_number<uint16_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+2, *n}; }
|
||||
case STR32: { auto n = body_number<uint32_t>(p, size); if (!n) return std::unexpected(n.error()); return body_info{1+4, *n}; }
|
||||
|
||||
case ARRAY16: case ARRAY32:
|
||||
case MAP16: case MAP32:
|
||||
return body_info{1 + (b == ARRAY16 || b == MAP16 ? 2 : 4), 0}; // container
|
||||
|
||||
default:
|
||||
return std::unexpected(error_code::invalid);
|
||||
}
|
||||
}
|
||||
|
||||
class packer {
|
||||
private:
|
||||
span_writer &m_buf;
|
||||
|
||||
template <typename T> void push_big_endian(T n) {
|
||||
auto p = reinterpret_cast<std::uint8_t *>(&n) + (sizeof(T) - 1);
|
||||
for (size_t i = 0; i < sizeof(T); ++i, --p) {
|
||||
m_buf.push_back(*p);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Range> void push(const Range &r) {
|
||||
m_buf.insert(m_buf.end(), std::begin(r), std::end(r));
|
||||
}
|
||||
|
||||
public:
|
||||
packer(span_writer &buf) : m_buf(buf) {}
|
||||
|
||||
packer(const packer &) = delete;
|
||||
packer &operator=(const packer &) = delete;
|
||||
|
||||
using pack_result = result<std::reference_wrapper<packer>>;
|
||||
|
||||
pack_result pack_nil() {
|
||||
m_buf.push_back(format::NIL);
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_bool(bool v) {
|
||||
m_buf.push_back(v ? format::TRUE : format::FALSE);
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
pack_result pack_integer(T n) {
|
||||
if constexpr (std::is_signed_v<T>) {
|
||||
if (n >= 0 && n <= 0x7F) {
|
||||
m_buf.push_back(static_cast<uint8_t>(n));
|
||||
} else if (n >= -32 && n < 0) {
|
||||
m_buf.push_back(static_cast<uint8_t>(n)); // negative fixint
|
||||
} else if (n >= std::numeric_limits<int8_t>::min() && n <= std::numeric_limits<int8_t>::max()) {
|
||||
m_buf.push_back(format::INT8);
|
||||
m_buf.push_back(static_cast<uint8_t>(n));
|
||||
} else if (n >= std::numeric_limits<int16_t>::min() && n <= std::numeric_limits<int16_t>::max()) {
|
||||
m_buf.push_back(format::INT16);
|
||||
push_big_endian(static_cast<int16_t>(n));
|
||||
} else if (n >= std::numeric_limits<int32_t>::min() && n <= std::numeric_limits<int32_t>::max()) {
|
||||
m_buf.push_back(format::INT32);
|
||||
push_big_endian(static_cast<int32_t>(n));
|
||||
} else {
|
||||
m_buf.push_back(format::INT64);
|
||||
push_big_endian(static_cast<int64_t>(n));
|
||||
}
|
||||
} else {
|
||||
if (n <= 0x7F) {
|
||||
m_buf.push_back(static_cast<uint8_t>(n));
|
||||
} else if (n <= std::numeric_limits<uint8_t>::max()) {
|
||||
m_buf.push_back(format::UINT8);
|
||||
m_buf.push_back(static_cast<uint8_t>(n));
|
||||
} else if (n <= std::numeric_limits<uint16_t>::max()) {
|
||||
m_buf.push_back(format::UINT16);
|
||||
push_big_endian(static_cast<uint16_t>(n));
|
||||
} else if (n <= std::numeric_limits<uint32_t>::max()) {
|
||||
m_buf.push_back(format::UINT32);
|
||||
push_big_endian(static_cast<uint32_t>(n));
|
||||
} else {
|
||||
m_buf.push_back(format::UINT64);
|
||||
push_big_endian(static_cast<uint64_t>(n));
|
||||
}
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_uint32_fixed(uint32_t n) {
|
||||
m_buf.push_back(format::UINT32);
|
||||
push_big_endian(n);
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_float(float n) {
|
||||
m_buf.push_back(format::FLOAT32);
|
||||
push_big_endian(n);
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_double(double n) {
|
||||
m_buf.push_back(format::FLOAT64);
|
||||
push_big_endian(n);
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class Range>
|
||||
pack_result pack_str(const Range &r) {
|
||||
auto sz = static_cast<size_t>(std::distance(std::begin(r), std::end(r)));
|
||||
if (sz < 32) {
|
||||
m_buf.push_back(format::FIXSTR_MIN | static_cast<uint8_t>(sz));
|
||||
} else if (sz <= std::numeric_limits<uint8_t>::max()) {
|
||||
m_buf.push_back(format::STR8);
|
||||
m_buf.push_back(static_cast<uint8_t>(sz));
|
||||
} else if (sz <= std::numeric_limits<uint16_t>::max()) {
|
||||
m_buf.push_back(format::STR16);
|
||||
push_big_endian(static_cast<uint16_t>(sz));
|
||||
} else if (sz <= std::numeric_limits<uint32_t>::max()) {
|
||||
m_buf.push_back(format::STR32);
|
||||
push_big_endian(static_cast<uint32_t>(sz));
|
||||
} else {
|
||||
return std::unexpected(error_code::overflow);
|
||||
}
|
||||
push(r);
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_str(const char *s) {
|
||||
return pack_str(std::string_view(s));
|
||||
}
|
||||
|
||||
template <class Range>
|
||||
pack_result pack_bin(const Range &r) {
|
||||
auto sz = static_cast<size_t>(std::distance(std::begin(r), std::end(r)));
|
||||
if (sz <= std::numeric_limits<uint8_t>::max()) {
|
||||
m_buf.push_back(format::BIN8);
|
||||
m_buf.push_back(static_cast<uint8_t>(sz));
|
||||
} else if (sz <= std::numeric_limits<uint16_t>::max()) {
|
||||
m_buf.push_back(format::BIN16);
|
||||
push_big_endian(static_cast<uint16_t>(sz));
|
||||
} else if (sz <= std::numeric_limits<uint32_t>::max()) {
|
||||
m_buf.push_back(format::BIN32);
|
||||
push_big_endian(static_cast<uint32_t>(sz));
|
||||
} else {
|
||||
return std::unexpected(error_code::overflow);
|
||||
}
|
||||
push(r);
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_array(size_t n) {
|
||||
if (n <= 15) {
|
||||
m_buf.push_back(format::FIXARRAY_MIN | static_cast<uint8_t>(n));
|
||||
} else if (n <= std::numeric_limits<uint16_t>::max()) {
|
||||
m_buf.push_back(format::ARRAY16);
|
||||
push_big_endian(static_cast<uint16_t>(n));
|
||||
} else if (n <= std::numeric_limits<uint32_t>::max()) {
|
||||
m_buf.push_back(format::ARRAY32);
|
||||
push_big_endian(static_cast<uint32_t>(n));
|
||||
} else {
|
||||
return std::unexpected(error_code::overflow);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_map(size_t n) {
|
||||
if (n <= 15) {
|
||||
m_buf.push_back(format::FIXMAP_MIN | static_cast<uint8_t>(n));
|
||||
} else if (n <= std::numeric_limits<uint16_t>::max()) {
|
||||
m_buf.push_back(format::MAP16);
|
||||
push_big_endian(static_cast<uint16_t>(n));
|
||||
} else if (n <= std::numeric_limits<uint32_t>::max()) {
|
||||
m_buf.push_back(format::MAP32);
|
||||
push_big_endian(static_cast<uint32_t>(n));
|
||||
} else {
|
||||
return std::unexpected(error_code::overflow);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_ext16_header(char type, uint16_t len) {
|
||||
m_buf.push_back(format::EXT16);
|
||||
push_big_endian(len);
|
||||
m_buf.push_back(static_cast<uint8_t>(type));
|
||||
return *this;
|
||||
}
|
||||
|
||||
pack_result pack_bin16_header(uint16_t len) {
|
||||
m_buf.push_back(format::BIN16);
|
||||
push_big_endian(len);
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <class Range>
|
||||
pack_result pack_ext(char type, const Range &r) {
|
||||
auto sz = static_cast<size_t>(std::distance(std::begin(r), std::end(r)));
|
||||
|
||||
switch (sz) {
|
||||
case 1: m_buf.push_back(format::FIXEXT1); break;
|
||||
case 2: m_buf.push_back(format::FIXEXT2); break;
|
||||
case 4: m_buf.push_back(format::FIXEXT4); break;
|
||||
case 8: m_buf.push_back(format::FIXEXT8); break;
|
||||
case 16: m_buf.push_back(format::FIXEXT16); break;
|
||||
default:
|
||||
if (sz <= std::numeric_limits<uint8_t>::max()) {
|
||||
m_buf.push_back(format::EXT8);
|
||||
m_buf.push_back(static_cast<uint8_t>(sz));
|
||||
} else if (sz <= std::numeric_limits<uint16_t>::max()) {
|
||||
m_buf.push_back(format::EXT16);
|
||||
push_big_endian(static_cast<uint16_t>(sz));
|
||||
} else if (sz <= std::numeric_limits<uint32_t>::max()) {
|
||||
m_buf.push_back(format::EXT32);
|
||||
push_big_endian(static_cast<uint32_t>(sz));
|
||||
} else {
|
||||
return std::unexpected(error_code::overflow);
|
||||
}
|
||||
}
|
||||
m_buf.push_back(static_cast<uint8_t>(type));
|
||||
push(r);
|
||||
return *this;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
requires std::is_integral_v<T> && (!std::is_same_v<T, bool>)
|
||||
pack_result pack(T n) { return pack_integer(n); }
|
||||
|
||||
template <typename T>
|
||||
requires std::is_enum_v<T>
|
||||
pack_result pack(T v) { return pack_integer(static_cast<std::underlying_type_t<T>>(v)); }
|
||||
|
||||
pack_result pack(bool v) { return pack_bool(v); }
|
||||
pack_result pack(float v) { return pack_float(v); }
|
||||
pack_result pack(double v) { return pack_double(v); }
|
||||
pack_result pack(const char *v) { return pack_str(v); }
|
||||
pack_result pack(std::string_view v) { return pack_str(v); }
|
||||
pack_result pack(const std::string &v) { return pack_str(v); }
|
||||
|
||||
pack_result pack(const std::vector<uint8_t> &v) { return pack_bin(v); }
|
||||
|
||||
template <typename T>
|
||||
requires (!std::is_same_v<T, uint8_t>)
|
||||
pack_result pack(const std::vector<T> &v) {
|
||||
auto r = pack_array(v.size());
|
||||
if (!r) return r;
|
||||
for (auto& elem : v) {
|
||||
r = r->get().pack(elem);
|
||||
if (!r) return r;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
template <size_t N>
|
||||
pack_result pack(const std::array<uint8_t, N> &v) { return pack_bin(v); }
|
||||
|
||||
template <typename... Ts>
|
||||
pack_result pack(const std::tuple<Ts...> &t) {
|
||||
auto r = pack_array(sizeof...(Ts));
|
||||
if (!r) return r;
|
||||
return pack_tuple_elements(t, std::index_sequence_for<Ts...>{});
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
requires requires(const T &v) { { T::ext_id } -> std::convertible_to<int8_t>; v.as_tuple(); }
|
||||
pack_result pack(const T &v) {
|
||||
uint8_t ext_buf[256];
|
||||
span_writer ext_writer(ext_buf, sizeof(ext_buf));
|
||||
packer inner(ext_writer);
|
||||
auto r = inner.pack(v.as_tuple());
|
||||
if (!r) return r;
|
||||
return pack_ext(T::ext_id, inner.get_payload());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
requires (requires(const T &v) { v.as_tuple(); } && !requires { { T::ext_id } -> std::convertible_to<int8_t>; })
|
||||
pack_result pack(const T &v) {
|
||||
return pack(v.as_tuple());
|
||||
}
|
||||
|
||||
private:
|
||||
template <typename Tuple, size_t... Is>
|
||||
pack_result pack_tuple_elements(const Tuple &t, std::index_sequence<Is...>) {
|
||||
pack_result r = *this;
|
||||
((r = r ? r->get().pack(std::get<Is>(t)) : r), ...);
|
||||
return r;
|
||||
}
|
||||
|
||||
public:
|
||||
const span_writer &get_payload() const { return m_buf; }
|
||||
};
|
||||
|
||||
class parser {
|
||||
const uint8_t *m_p = nullptr;
|
||||
int m_size = 0;
|
||||
|
||||
result<uint8_t> header_byte() const {
|
||||
if (m_size < 1) return std::unexpected(error_code::empty);
|
||||
return m_p[0];
|
||||
}
|
||||
|
||||
public:
|
||||
parser() = default;
|
||||
|
||||
parser(const std::vector<uint8_t> &v)
|
||||
: m_p(v.data()), m_size(static_cast<int>(v.size())) {}
|
||||
|
||||
parser(const uint8_t *p, int size)
|
||||
: m_p(p), m_size(size < 0 ? 0 : size) {}
|
||||
|
||||
bool is_empty() const { return m_size == 0; }
|
||||
const uint8_t *data() const { return m_p; }
|
||||
int size() const { return m_size; }
|
||||
|
||||
result<parser> advance(int n) const {
|
||||
if (n > m_size) return std::unexpected(error_code::lack);
|
||||
return parser(m_p + n, m_size - n);
|
||||
}
|
||||
|
||||
result<parser> next() const {
|
||||
auto hdr = header_byte();
|
||||
if (!hdr) return std::unexpected(hdr.error());
|
||||
|
||||
if (is_array()) {
|
||||
auto info = get_body_info(m_p, m_size);
|
||||
if (!info) return std::unexpected(info.error());
|
||||
auto cnt = count();
|
||||
if (!cnt) return std::unexpected(cnt.error());
|
||||
auto cur = advance(info->header);
|
||||
if (!cur) return std::unexpected(cur.error());
|
||||
for (uint32_t i = 0; i < *cnt; ++i) {
|
||||
auto n = cur->next();
|
||||
if (!n) return std::unexpected(n.error());
|
||||
cur = *n;
|
||||
}
|
||||
return *cur;
|
||||
} else if (is_map()) {
|
||||
auto info = get_body_info(m_p, m_size);
|
||||
if (!info) return std::unexpected(info.error());
|
||||
auto cnt = count();
|
||||
if (!cnt) return std::unexpected(cnt.error());
|
||||
auto cur = advance(info->header);
|
||||
if (!cur) return std::unexpected(cur.error());
|
||||
for (uint32_t i = 0; i < *cnt; ++i) {
|
||||
auto k = cur->next();
|
||||
if (!k) return std::unexpected(k.error());
|
||||
cur = *k;
|
||||
auto v = cur->next();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
cur = *v;
|
||||
}
|
||||
return *cur;
|
||||
} else {
|
||||
auto info = get_body_info(m_p, m_size);
|
||||
if (!info) return std::unexpected(info.error());
|
||||
auto total = info->header + static_cast<int>(info->body);
|
||||
return advance(total);
|
||||
}
|
||||
}
|
||||
|
||||
bool is_nil() const {
|
||||
auto h = header_byte();
|
||||
return h && *h == format::NIL;
|
||||
}
|
||||
|
||||
bool is_bool() const {
|
||||
auto h = header_byte();
|
||||
return h && (*h == format::TRUE || *h == format::FALSE);
|
||||
}
|
||||
|
||||
bool is_number() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return false;
|
||||
uint8_t b = *h;
|
||||
if (format::is_positive_fixint(b)) return true;
|
||||
if (format::is_negative_fixint(b)) return true;
|
||||
return b >= format::FLOAT32 && b <= format::INT64;
|
||||
}
|
||||
|
||||
bool is_string() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return false;
|
||||
uint8_t b = *h;
|
||||
if (format::is_fixstr(b)) return true;
|
||||
return b == format::STR8 || b == format::STR16 || b == format::STR32;
|
||||
}
|
||||
|
||||
bool is_binary() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return false;
|
||||
uint8_t b = *h;
|
||||
return b == format::BIN8 || b == format::BIN16 || b == format::BIN32;
|
||||
}
|
||||
|
||||
bool is_ext() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return false;
|
||||
uint8_t b = *h;
|
||||
return (b >= format::FIXEXT1 && b <= format::FIXEXT16) ||
|
||||
b == format::EXT8 || b == format::EXT16 || b == format::EXT32;
|
||||
}
|
||||
|
||||
bool is_array() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return false;
|
||||
uint8_t b = *h;
|
||||
if (format::is_fixarray(b)) return true;
|
||||
return b == format::ARRAY16 || b == format::ARRAY32;
|
||||
}
|
||||
|
||||
bool is_map() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return false;
|
||||
uint8_t b = *h;
|
||||
if (format::is_fixmap(b)) return true;
|
||||
return b == format::MAP16 || b == format::MAP32;
|
||||
}
|
||||
|
||||
|
||||
result<bool> get_bool() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return std::unexpected(h.error());
|
||||
if (*h == format::TRUE) return true;
|
||||
if (*h == format::FALSE) return false;
|
||||
return std::unexpected(error_code::type_error);
|
||||
}
|
||||
|
||||
result<std::string_view> get_string() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return std::unexpected(h.error());
|
||||
uint8_t b = *h;
|
||||
size_t offset, len;
|
||||
if (format::is_fixstr(b)) {
|
||||
len = b & 0x1F;
|
||||
offset = 1;
|
||||
} else if (b == format::STR8) {
|
||||
auto n = body_number<uint8_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
len = *n; offset = 1 + 1;
|
||||
} else if (b == format::STR16) {
|
||||
auto n = body_number<uint16_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
len = *n; offset = 1 + 2;
|
||||
} else if (b == format::STR32) {
|
||||
auto n = body_number<uint32_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
len = *n; offset = 1 + 4;
|
||||
} else {
|
||||
return std::unexpected(error_code::type_error);
|
||||
}
|
||||
if (static_cast<int>(offset + len) > m_size) {
|
||||
return std::unexpected(error_code::lack);
|
||||
}
|
||||
return std::string_view(reinterpret_cast<const char *>(m_p + offset), len);
|
||||
}
|
||||
|
||||
result<std::string_view> get_binary_view() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return std::unexpected(h.error());
|
||||
uint8_t b = *h;
|
||||
size_t offset, len;
|
||||
|
||||
if (b == format::BIN8) {
|
||||
auto n = body_number<uint8_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
len = *n; offset = 1 + 1;
|
||||
} else if (b == format::BIN16) {
|
||||
auto n = body_number<uint16_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
len = *n; offset = 1 + 2;
|
||||
} else if (b == format::BIN32) {
|
||||
auto n = body_number<uint32_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
len = *n; offset = 1 + 4;
|
||||
} else {
|
||||
return std::unexpected(error_code::type_error);
|
||||
}
|
||||
if (static_cast<int>(offset + len) > m_size) {
|
||||
return std::unexpected(error_code::lack);
|
||||
}
|
||||
return std::string_view(reinterpret_cast<const char *>(m_p + offset), len);
|
||||
}
|
||||
|
||||
result<std::tuple<int8_t, std::string_view>> get_ext() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return std::unexpected(h.error());
|
||||
uint8_t b = *h;
|
||||
int8_t ext_type;
|
||||
size_t data_offset, data_len;
|
||||
|
||||
switch (b) {
|
||||
case format::FIXEXT1: ext_type = m_p[1]; data_offset = 2; data_len = 1; break;
|
||||
case format::FIXEXT2: ext_type = m_p[1]; data_offset = 2; data_len = 2; break;
|
||||
case format::FIXEXT4: ext_type = m_p[1]; data_offset = 2; data_len = 4; break;
|
||||
case format::FIXEXT8: ext_type = m_p[1]; data_offset = 2; data_len = 8; break;
|
||||
case format::FIXEXT16: ext_type = m_p[1]; data_offset = 2; data_len = 16; break;
|
||||
case format::EXT8: {
|
||||
auto n = body_number<uint8_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
ext_type = m_p[2]; data_offset = 3; data_len = *n;
|
||||
break;
|
||||
}
|
||||
case format::EXT16: {
|
||||
auto n = body_number<uint16_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
ext_type = m_p[3]; data_offset = 4; data_len = *n;
|
||||
break;
|
||||
}
|
||||
case format::EXT32: {
|
||||
auto n = body_number<uint32_t>(m_p, m_size);
|
||||
if (!n) return std::unexpected(n.error());
|
||||
ext_type = m_p[5]; data_offset = 6; data_len = *n;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return std::unexpected(error_code::type_error);
|
||||
}
|
||||
if (static_cast<int>(data_offset + data_len) > m_size) {
|
||||
return std::unexpected(error_code::lack);
|
||||
}
|
||||
return std::tuple{ext_type,
|
||||
std::string_view(reinterpret_cast<const char *>(m_p + data_offset), data_len)};
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
result<T> get_number() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return std::unexpected(h.error());
|
||||
uint8_t b = *h;
|
||||
|
||||
if (format::is_positive_fixint(b)) return static_cast<T>(b);
|
||||
if (format::is_negative_fixint(b)) return static_cast<T>(static_cast<int8_t>(b));
|
||||
|
||||
switch (b) {
|
||||
case format::UINT8: { auto n = body_number<uint8_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::UINT16: { auto n = body_number<uint16_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::UINT32: { auto n = body_number<uint32_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::UINT64: { auto n = body_number<uint64_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::INT8: { auto n = body_number<int8_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::INT16: { auto n = body_number<int16_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::INT32: { auto n = body_number<int32_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::INT64: { auto n = body_number<int64_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::FLOAT32: { auto n = body_number<float>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
case format::FLOAT64: { auto n = body_number<double>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<T>(*n); }
|
||||
default:
|
||||
return std::unexpected(error_code::type_error);
|
||||
}
|
||||
}
|
||||
|
||||
result<uint32_t> count() const {
|
||||
auto h = header_byte();
|
||||
if (!h) return std::unexpected(h.error());
|
||||
uint8_t b = *h;
|
||||
|
||||
if (format::is_fixarray(b)) return static_cast<uint32_t>(b & 0x0F);
|
||||
if (format::is_fixmap(b)) return static_cast<uint32_t>(b & 0x0F);
|
||||
|
||||
switch (b) {
|
||||
case format::ARRAY16: { auto n = body_number<uint16_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<uint32_t>(*n); }
|
||||
case format::ARRAY32: { auto n = body_number<uint32_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return *n; }
|
||||
case format::MAP16: { auto n = body_number<uint16_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return static_cast<uint32_t>(*n); }
|
||||
case format::MAP32: { auto n = body_number<uint32_t>(m_p, m_size); if (!n) return std::unexpected(n.error()); return *n; }
|
||||
default:
|
||||
return std::unexpected(error_code::type_error);
|
||||
}
|
||||
}
|
||||
|
||||
result<parser> first_item() const {
|
||||
if (!is_array() && !is_map()) return std::unexpected(error_code::type_error);
|
||||
auto info = get_body_info(m_p, m_size);
|
||||
if (!info) return std::unexpected(info.error());
|
||||
return advance(info->header);
|
||||
}
|
||||
|
||||
parser operator[](int index) const {
|
||||
auto cur = first_item();
|
||||
if (!cur) return {};
|
||||
for (int i = 0; i < index; ++i) {
|
||||
auto n = cur->next();
|
||||
if (!n) return {};
|
||||
cur = *n;
|
||||
}
|
||||
return *cur;
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
requires std::is_enum_v<T>
|
||||
result<parser> unpack(const parser &p, T &out) {
|
||||
std::underlying_type_t<T> v;
|
||||
auto r = unpack(p, v);
|
||||
if (!r) return r;
|
||||
out = static_cast<T>(v);
|
||||
return r;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
requires std::is_integral_v<T> && (!std::is_same_v<T, bool>)
|
||||
result<parser> unpack(const parser &p, T &out) {
|
||||
auto v = p.get_number<T>();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
out = *v;
|
||||
return p.next();
|
||||
}
|
||||
|
||||
inline result<parser> unpack(const parser &p, bool &out) {
|
||||
auto v = p.get_bool();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
out = *v;
|
||||
return p.next();
|
||||
}
|
||||
|
||||
inline result<parser> unpack(const parser &p, std::string_view &out) {
|
||||
auto v = p.get_string();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
out = *v;
|
||||
return p.next();
|
||||
}
|
||||
|
||||
inline result<parser> unpack(const parser &p, std::string &out) {
|
||||
auto v = p.get_string();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
out = std::string(v->data(), v->size());
|
||||
return p.next();
|
||||
}
|
||||
|
||||
template <size_t N>
|
||||
result<parser> unpack(const parser &p, std::array<uint8_t, N> &out) {
|
||||
auto v = p.get_binary_view();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
if (v->size() != N) return std::unexpected(error_code::type_error);
|
||||
std::copy(v->begin(), v->end(), out.begin());
|
||||
return p.next();
|
||||
}
|
||||
|
||||
inline result<parser> unpack(const parser &p, std::vector<uint8_t> &out) {
|
||||
auto v = p.get_binary_view();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
out.assign(v->begin(), v->end());
|
||||
return p.next();
|
||||
}
|
||||
|
||||
inline result<parser> unpack(const parser &p, std::span<const uint8_t> &out) {
|
||||
auto v = p.get_binary_view();
|
||||
if (!v) return std::unexpected(v.error());
|
||||
out = std::span<const uint8_t>(reinterpret_cast<const uint8_t*>(v->data()), v->size());
|
||||
return p.next();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
requires (!std::is_same_v<T, uint8_t>)
|
||||
result<parser> unpack(const parser &p, std::vector<T> &out) {
|
||||
auto cnt = p.count();
|
||||
if (!cnt) return std::unexpected(cnt.error());
|
||||
out.resize(*cnt);
|
||||
result<parser> cur = p.first_item();
|
||||
for (size_t i = 0; i < *cnt; i++) {
|
||||
if (!cur) return cur;
|
||||
cur = unpack(*cur, out[i]);
|
||||
}
|
||||
if (!cur) return cur;
|
||||
return p.next();
|
||||
}
|
||||
|
||||
template <typename... Ts, size_t... Is>
|
||||
result<parser> unpack_tuple_elements(const parser &p, std::tuple<Ts...> &t, std::index_sequence<Is...>) {
|
||||
result<parser> cur = p.first_item();
|
||||
if (!cur) return cur;
|
||||
((cur = cur ? unpack(*cur, std::get<Is>(t)) : cur), ...);
|
||||
return cur;
|
||||
}
|
||||
|
||||
template <typename... Ts>
|
||||
result<parser> unpack(const parser &p, std::tuple<Ts...> &t) {
|
||||
auto cnt = p.count();
|
||||
if (!cnt) return std::unexpected(cnt.error());
|
||||
if (*cnt != sizeof...(Ts)) return std::unexpected(error_code::type_error);
|
||||
auto r = unpack_tuple_elements(p, t, std::index_sequence_for<Ts...>{});
|
||||
if (!r) return r;
|
||||
return p.next();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
requires (requires(T &v) { v.as_tuple(); } && !requires { { T::ext_id } -> std::convertible_to<int8_t>; })
|
||||
result<parser> unpack(const parser &p, T &out) {
|
||||
auto tup = out.as_tuple();
|
||||
auto cnt = p.count();
|
||||
if (!cnt) return std::unexpected(cnt.error());
|
||||
if (*cnt != std::tuple_size_v<decltype(tup)>) return std::unexpected(error_code::type_error);
|
||||
auto r = unpack_tuple_elements(p, tup, std::make_index_sequence<std::tuple_size_v<decltype(tup)>>{});
|
||||
if (!r) return r;
|
||||
return p.next();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
requires requires(T &v) { { T::ext_id } -> std::convertible_to<int8_t>; v.as_tuple(); }
|
||||
result<parser> unpack(const parser &p, T &out) {
|
||||
auto ext = p.get_ext();
|
||||
if (!ext) return std::unexpected(ext.error());
|
||||
auto [ext_type, ext_data] = *ext;
|
||||
if (ext_type != T::ext_id) return std::unexpected(error_code::type_error);
|
||||
parser inner(reinterpret_cast<const uint8_t *>(ext_data.data()),
|
||||
static_cast<int>(ext_data.size()));
|
||||
auto tup = out.as_tuple();
|
||||
auto r = unpack_tuple_elements(inner, tup, std::make_index_sequence<std::tuple_size_v<decltype(tup)>>{});
|
||||
if (!r) return r;
|
||||
return p.next();
|
||||
}
|
||||
|
||||
} // namespace msgpack
|
||||
@@ -0,0 +1,27 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include "eth.h"
|
||||
#include "ipv4.h"
|
||||
#include "span_writer.h"
|
||||
#include "callback_list.h"
|
||||
|
||||
struct net_state {
|
||||
eth::mac_addr mac;
|
||||
ipv4::ip4_addr ip;
|
||||
};
|
||||
|
||||
using net_frame_callback = bool (*)(std::span<const uint8_t> frame);
|
||||
|
||||
using frame_cb_list = callback_list<net_frame_callback, 16>;
|
||||
using frame_cb_handle = frame_cb_list::node*;
|
||||
|
||||
using ethertype_handler = void (*)(std::span<const uint8_t> frame, span_writer& tx);
|
||||
|
||||
bool net_init();
|
||||
const net_state& net_get_state();
|
||||
frame_cb_handle net_add_frame_callback(net_frame_callback cb);
|
||||
void net_remove_frame_callback(frame_cb_handle h);
|
||||
void net_poll(std::span<uint8_t> tx);
|
||||
void net_send_raw(std::span<const uint8_t> data);
|
||||
void net_register_ethertype(uint16_t ethertype_be, ethertype_handler fn);
|
||||
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
|
||||
class parse_buffer {
|
||||
const uint8_t* m_data;
|
||||
size_t m_remaining;
|
||||
|
||||
public:
|
||||
parse_buffer(std::span<const uint8_t> data)
|
||||
: m_data(data.data()), m_remaining(data.size()) {}
|
||||
|
||||
template <typename T>
|
||||
const T* consume() {
|
||||
if (m_remaining < sizeof(T)) return nullptr;
|
||||
auto* p = reinterpret_cast<const T*>(m_data);
|
||||
m_data += sizeof(T);
|
||||
m_remaining -= sizeof(T);
|
||||
return p;
|
||||
}
|
||||
|
||||
bool skip(size_t len) {
|
||||
if (m_remaining < len) return false;
|
||||
m_data += len;
|
||||
m_remaining -= len;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::span<const uint8_t> remaining() const { return {m_data, m_remaining}; }
|
||||
size_t remaining_size() const { return m_remaining; }
|
||||
};
|
||||
@@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <span>
|
||||
|
||||
template <size_t N>
|
||||
class prepend_buffer {
|
||||
uint8_t m_buf[N];
|
||||
size_t m_start = N / 2;
|
||||
size_t m_end = N / 2;
|
||||
|
||||
public:
|
||||
template <typename T>
|
||||
T* prepend() {
|
||||
m_start -= sizeof(T);
|
||||
return reinterpret_cast<T*>(m_buf + m_start);
|
||||
}
|
||||
|
||||
uint8_t* append(size_t len) {
|
||||
uint8_t* p = m_buf + m_end;
|
||||
m_end += len;
|
||||
return p;
|
||||
}
|
||||
|
||||
void append_copy(std::span<const uint8_t> data) {
|
||||
memcpy(append(data.size()), data.data(), data.size());
|
||||
}
|
||||
|
||||
uint8_t* payload_ptr() { return m_buf + m_end; }
|
||||
|
||||
uint8_t* data() { return m_buf + m_start; }
|
||||
const uint8_t* data() const { return m_buf + m_start; }
|
||||
size_t size() const { return m_end - m_start; }
|
||||
|
||||
std::span<const uint8_t> span() const { return {data(), size()}; }
|
||||
};
|
||||
@@ -0,0 +1,67 @@
|
||||
#pragma once
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
|
||||
template <typename T, uint16_t N>
|
||||
struct ring_buffer {
|
||||
std::array<T, N> data = {};
|
||||
uint16_t head = 0;
|
||||
uint16_t tail = 0;
|
||||
|
||||
uint16_t used() const { return tail - head; }
|
||||
uint16_t free() const { return N - used(); }
|
||||
bool empty() const { return head == tail; }
|
||||
|
||||
bool push(std::span<const T> src) {
|
||||
if (src.size() > free()) return false;
|
||||
for (auto& v : src)
|
||||
data[(tail++) % N] = v;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool push(const T& v) {
|
||||
if (free() == 0) return false;
|
||||
data[(tail++) % N] = v;
|
||||
return true;
|
||||
}
|
||||
|
||||
void push_overwrite(const T& v) {
|
||||
if (free() == 0) head++;
|
||||
data[(tail++) % N] = v;
|
||||
}
|
||||
|
||||
uint16_t peek(std::span<T> dst) const {
|
||||
uint16_t len = dst.size() < used() ? dst.size() : used();
|
||||
for (uint16_t i = 0; i < len; i++)
|
||||
dst[i] = data[(head + i) % N];
|
||||
return len;
|
||||
}
|
||||
|
||||
void consume(uint16_t len) {
|
||||
head += len;
|
||||
if (head >= N) {
|
||||
head -= N;
|
||||
tail -= N;
|
||||
}
|
||||
}
|
||||
|
||||
std::span<const T> read_contiguous() const {
|
||||
uint16_t offset = head % N;
|
||||
uint16_t contig = N - offset;
|
||||
uint16_t pending = used();
|
||||
uint16_t len = pending < contig ? pending : contig;
|
||||
return {data.data() + offset, len};
|
||||
}
|
||||
|
||||
struct iterator {
|
||||
const ring_buffer* rb;
|
||||
uint16_t index;
|
||||
const T& operator*() const { return rb->data[(rb->head + index) % N]; }
|
||||
iterator& operator++() { index++; return *this; }
|
||||
bool operator!=(const iterator& o) const { return index != o.index; }
|
||||
};
|
||||
|
||||
iterator begin() const { return {this, 0}; }
|
||||
iterator end() const { return {this, used()}; }
|
||||
};
|
||||
@@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <span>
|
||||
|
||||
class span_writer {
|
||||
uint8_t *m_data;
|
||||
size_t m_capacity;
|
||||
size_t m_size = 0;
|
||||
bool m_overflow = false;
|
||||
|
||||
public:
|
||||
span_writer(uint8_t *data, size_t capacity) : m_data(data), m_capacity(capacity) {}
|
||||
span_writer(std::span<uint8_t> buf) : m_data(buf.data()), m_capacity(buf.size()) {}
|
||||
|
||||
void push_back(uint8_t v) {
|
||||
if (m_size < m_capacity) m_data[m_size++] = v;
|
||||
else m_overflow = true;
|
||||
}
|
||||
|
||||
template <class It>
|
||||
void insert(uint8_t *, It first, It last) {
|
||||
while (first != last) {
|
||||
if (m_size < m_capacity) m_data[m_size++] = *first++;
|
||||
else { m_overflow = true; return; }
|
||||
}
|
||||
}
|
||||
|
||||
size_t size() const { return m_size; }
|
||||
size_t capacity() const { return m_capacity; }
|
||||
bool full() const { return m_size >= m_capacity; }
|
||||
bool overflow() const { return m_overflow; }
|
||||
|
||||
uint8_t *data() { return m_data; }
|
||||
const uint8_t *data() const { return m_data; }
|
||||
|
||||
uint8_t *begin() { return m_data; }
|
||||
uint8_t *end() { return m_data + m_size; }
|
||||
const uint8_t *begin() const { return m_data; }
|
||||
const uint8_t *end() const { return m_data + m_size; }
|
||||
|
||||
span_writer subspan(size_t offset) {
|
||||
return span_writer(m_data + offset, m_capacity - offset);
|
||||
}
|
||||
|
||||
span_writer subspan(size_t offset, size_t len) {
|
||||
return span_writer(m_data + offset, len);
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,33 @@
|
||||
#pragma once
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
|
||||
template <typename T, size_t Capacity>
|
||||
class static_vector {
|
||||
T m_data[Capacity];
|
||||
size_t m_size = 0;
|
||||
|
||||
public:
|
||||
void push_back(const T &v) {
|
||||
if (m_size < Capacity) m_data[m_size++] = v;
|
||||
}
|
||||
|
||||
void clear() { m_size = 0; }
|
||||
|
||||
size_t size() const { return m_size; }
|
||||
size_t capacity() const { return Capacity; }
|
||||
bool full() const { return m_size >= Capacity; }
|
||||
bool empty() const { return m_size == 0; }
|
||||
|
||||
T *data() { return m_data; }
|
||||
const T *data() const { return m_data; }
|
||||
|
||||
T &operator[](size_t i) { return m_data[i]; }
|
||||
const T &operator[](size_t i) const { return m_data[i]; }
|
||||
|
||||
T *begin() { return m_data; }
|
||||
T *end() { return m_data + m_size; }
|
||||
const T *begin() const { return m_data; }
|
||||
const T *end() const { return m_data + m_size; }
|
||||
};
|
||||
@@ -0,0 +1,7 @@
|
||||
#pragma once
|
||||
#include <optional>
|
||||
#include "dispatch.h"
|
||||
#include "wire.h"
|
||||
|
||||
std::optional<ResponseListTests> handle_list_tests(const responder& resp, const RequestListTests&);
|
||||
std::optional<ResponseTest> handle_test(const responder& resp, const RequestTest&);
|
||||
@@ -0,0 +1,63 @@
|
||||
#pragma once
|
||||
#include "pico/time.h"
|
||||
#include "callback_list.h"
|
||||
|
||||
struct timer_entry {
|
||||
absolute_time_t when;
|
||||
void (*fn)() = nullptr;
|
||||
};
|
||||
|
||||
using timer_handle = callback_list<timer_entry, 16>::node*;
|
||||
|
||||
struct timer_queue {
|
||||
callback_list<timer_entry, 16> list;
|
||||
alarm_id_t alarm = -1;
|
||||
volatile bool irq_pending = false;
|
||||
|
||||
timer_handle schedule(absolute_time_t when, void (*fn)()) {
|
||||
auto* n = list.insert_sorted({when, fn},
|
||||
[](const timer_entry& a, const timer_entry& b) {
|
||||
return absolute_time_diff_us(b.when, a.when) < 0;
|
||||
});
|
||||
arm();
|
||||
return n;
|
||||
}
|
||||
|
||||
timer_handle schedule_ms(uint32_t ms, void (*fn)()) {
|
||||
return schedule(make_timeout_time_ms(ms), fn);
|
||||
}
|
||||
|
||||
bool cancel(timer_handle h) {
|
||||
if (!h) return false;
|
||||
list.remove(h);
|
||||
arm();
|
||||
return true;
|
||||
}
|
||||
|
||||
void run() {
|
||||
if (!irq_pending) return;
|
||||
irq_pending = false;
|
||||
while (auto* n = list.front()) {
|
||||
if (absolute_time_diff_us(get_absolute_time(), n->value.when) > 0) break;
|
||||
auto fn = n->value.fn;
|
||||
list.remove(n);
|
||||
fn();
|
||||
}
|
||||
arm();
|
||||
}
|
||||
|
||||
bool empty() const { return list.empty(); }
|
||||
|
||||
private:
|
||||
static int64_t alarm_cb(alarm_id_t, void* user_data) {
|
||||
static_cast<timer_queue*>(user_data)->irq_pending = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void arm() {
|
||||
if (alarm >= 0) cancel_alarm(alarm);
|
||||
alarm = -1;
|
||||
if (auto* n = list.front())
|
||||
alarm = add_alarm_at(n->value.when, alarm_cb, this, false);
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,45 @@
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include "eth.h"
|
||||
#include "ipv4.h"
|
||||
#include "net.h"
|
||||
#include "span_writer.h"
|
||||
|
||||
namespace udp {
|
||||
|
||||
struct __attribute__((packed)) header {
|
||||
uint16_t src_port;
|
||||
uint16_t dst_port;
|
||||
uint16_t length;
|
||||
uint16_t checksum;
|
||||
};
|
||||
static_assert(sizeof(header) == 8);
|
||||
|
||||
struct address {
|
||||
// mac is carried here until we grow an ARP cache; once we do, a
|
||||
// destination mac comes from resolving ip and this field goes away.
|
||||
eth::mac_addr mac;
|
||||
ipv4::ip4_addr ip;
|
||||
uint16_t port;
|
||||
};
|
||||
|
||||
template <typename Buf>
|
||||
void prepend(Buf& buf, const eth::mac_addr& dst_mac, const eth::mac_addr& src_mac,
|
||||
ipv4::ip4_addr src_ip, ipv4::ip4_addr dst_ip,
|
||||
uint16_t src_port, uint16_t dst_port,
|
||||
size_t payload_len, uint8_t ttl = 64) {
|
||||
auto* u = buf.template prepend<header>();
|
||||
u->src_port = src_port;
|
||||
u->dst_port = dst_port;
|
||||
u->length = __builtin_bswap16(sizeof(header) + payload_len);
|
||||
u->checksum = 0;
|
||||
ipv4::prepend(buf, dst_mac, src_mac, src_ip, dst_ip, 17, sizeof(header) + payload_len, ttl);
|
||||
}
|
||||
|
||||
void handle(std::span<const uint8_t> frame, span_writer& tx);
|
||||
|
||||
using port_handler = void (*)(std::span<const uint8_t> payload, const address& from);
|
||||
void register_port(uint16_t port_be, port_handler fn);
|
||||
|
||||
} // namespace udp
|
||||
+245
@@ -0,0 +1,245 @@
|
||||
#pragma once
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
#include <vector>
|
||||
#include "msgpack.h"
|
||||
#include "halfsiphash.h"
|
||||
#include "static_vector.h"
|
||||
#include "flash.h"
|
||||
|
||||
struct Envelope {
|
||||
static constexpr int8_t ext_id = 0;
|
||||
uint32_t message_id;
|
||||
uint32_t checksum;
|
||||
std::span<const uint8_t> payload;
|
||||
auto as_tuple() const { return std::tie(message_id, checksum, payload); }
|
||||
auto as_tuple() { return std::tie(message_id, checksum, payload); }
|
||||
};
|
||||
|
||||
struct DeviceError {
|
||||
static constexpr int8_t ext_id = 1;
|
||||
uint32_t code;
|
||||
std::string message;
|
||||
auto as_tuple() const { return std::tie(code, message); }
|
||||
auto as_tuple() { return std::tie(code, message); }
|
||||
};
|
||||
|
||||
struct RequestInfo {
|
||||
static constexpr int8_t ext_id = 4;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
enum class boot_reason : uint8_t {
|
||||
cold_boot = 0,
|
||||
request_reboot = 1,
|
||||
watchdog = 2,
|
||||
};
|
||||
|
||||
struct ResponseInfo {
|
||||
static constexpr int8_t ext_id = 5;
|
||||
std::array<uint8_t, 8> board_id;
|
||||
std::array<uint8_t, 6> mac;
|
||||
std::array<uint8_t, 4> ip;
|
||||
std::string firmware_name;
|
||||
boot_reason boot;
|
||||
uint32_t build_epoch;
|
||||
auto as_tuple() const { return std::tie(board_id, mac, ip, firmware_name, boot, build_epoch); }
|
||||
auto as_tuple() { return std::tie(board_id, mac, ip, firmware_name, boot, build_epoch); }
|
||||
};
|
||||
|
||||
struct RequestLog {
|
||||
static constexpr int8_t ext_id = 6;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
struct LogEntry {
|
||||
uint32_t timestamp_us;
|
||||
std::string message;
|
||||
auto as_tuple() const { return std::tie(timestamp_us, message); }
|
||||
auto as_tuple() { return std::tie(timestamp_us, message); }
|
||||
};
|
||||
|
||||
struct ResponseLog {
|
||||
static constexpr int8_t ext_id = 7;
|
||||
std::vector<LogEntry> entries;
|
||||
auto as_tuple() const { return std::tie(entries); }
|
||||
auto as_tuple() { return std::tie(entries); }
|
||||
};
|
||||
|
||||
struct RequestFlashErase {
|
||||
static constexpr int8_t ext_id = 8;
|
||||
uint32_t addr;
|
||||
uint32_t len;
|
||||
auto as_tuple() const { return std::tie(addr, len); }
|
||||
auto as_tuple() { return std::tie(addr, len); }
|
||||
};
|
||||
|
||||
struct ResponseFlashErase {
|
||||
static constexpr int8_t ext_id = 9;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
struct RequestFlashWrite {
|
||||
static constexpr int8_t ext_id = 10;
|
||||
uint32_t addr;
|
||||
std::span<const uint8_t> data;
|
||||
auto as_tuple() const { return std::tie(addr, data); }
|
||||
auto as_tuple() { return std::tie(addr, data); }
|
||||
};
|
||||
|
||||
struct ResponseFlashWrite {
|
||||
static constexpr int8_t ext_id = 11;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
struct RequestReboot {
|
||||
static constexpr int8_t ext_id = 12;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
struct ResponseReboot {
|
||||
static constexpr int8_t ext_id = 13;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
struct RequestFlashStatus {
|
||||
static constexpr int8_t ext_id = 14;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
struct ResponseFlashStatus {
|
||||
static constexpr int8_t ext_id = 15;
|
||||
int8_t boot_partition;
|
||||
flash::slot slot_a;
|
||||
flash::slot slot_b;
|
||||
auto as_tuple() const { return std::tie(boot_partition, slot_a, slot_b); }
|
||||
auto as_tuple() { return std::tie(boot_partition, slot_a, slot_b); }
|
||||
};
|
||||
|
||||
struct RequestListTests {
|
||||
static constexpr int8_t ext_id = 125;
|
||||
auto as_tuple() const { return std::tie(); }
|
||||
auto as_tuple() { return std::tie(); }
|
||||
};
|
||||
|
||||
struct ResponseListTests {
|
||||
static constexpr int8_t ext_id = 124;
|
||||
std::vector<std::string> names;
|
||||
auto as_tuple() const { return std::tie(names); }
|
||||
auto as_tuple() { return std::tie(names); }
|
||||
};
|
||||
|
||||
struct RequestTest {
|
||||
static constexpr int8_t ext_id = 127;
|
||||
std::string name;
|
||||
auto as_tuple() const { return std::tie(name); }
|
||||
auto as_tuple() { return std::tie(name); }
|
||||
};
|
||||
|
||||
struct ResponseTest {
|
||||
static constexpr int8_t ext_id = 126;
|
||||
bool pass;
|
||||
std::vector<std::string> messages;
|
||||
auto as_tuple() const { return std::tie(pass, messages); }
|
||||
auto as_tuple() { return std::tie(pass, messages); }
|
||||
};
|
||||
|
||||
static constexpr uint8_t hash_key[8] = {};
|
||||
|
||||
struct DecodedMessage {
|
||||
uint32_t message_id;
|
||||
int8_t type_id;
|
||||
std::span<const uint8_t> payload;
|
||||
};
|
||||
|
||||
static constexpr size_t ext16_header_len = 4;
|
||||
static constexpr size_t array3_header_len = 1;
|
||||
static constexpr size_t uint32_fixed_len = 5;
|
||||
static constexpr size_t bin16_header_len = 3;
|
||||
|
||||
static constexpr size_t envelope_hdr_len =
|
||||
ext16_header_len + array3_header_len +
|
||||
uint32_fixed_len + uint32_fixed_len + bin16_header_len;
|
||||
|
||||
static constexpr size_t response_prefix_len = envelope_hdr_len + ext16_header_len;
|
||||
|
||||
template <typename T>
|
||||
inline msgpack::result<size_t> encode_response_into(span_writer &out, uint32_t message_id, const T &msg) {
|
||||
auto body = out.subspan(response_prefix_len);
|
||||
msgpack::packer body_p(body);
|
||||
body_p.pack(msg.as_tuple());
|
||||
|
||||
auto inner_ext = out.subspan(envelope_hdr_len, ext16_header_len);
|
||||
msgpack::packer inner_ext_p(inner_ext);
|
||||
inner_ext_p.pack_ext16_header(T::ext_id, static_cast<uint16_t>(body.size()));
|
||||
|
||||
size_t bin_len = inner_ext.size() + body.size();
|
||||
uint32_t checksum = halfsiphash::hash32({inner_ext.data(), bin_len}, hash_key);
|
||||
|
||||
auto env_hdr = out.subspan(0, envelope_hdr_len);
|
||||
size_t env_body_len = array3_header_len + uint32_fixed_len + uint32_fixed_len + bin16_header_len + bin_len;
|
||||
msgpack::packer hdr(env_hdr);
|
||||
hdr.pack_ext16_header(Envelope::ext_id, static_cast<uint16_t>(env_body_len));
|
||||
hdr.pack_array(3);
|
||||
hdr.pack_uint32_fixed(message_id);
|
||||
hdr.pack_uint32_fixed(checksum);
|
||||
hdr.pack_bin16_header(static_cast<uint16_t>(bin_len));
|
||||
|
||||
if (body.overflow() || inner_ext.overflow() || env_hdr.overflow())
|
||||
return std::unexpected(msgpack::error_code::overflow);
|
||||
return response_prefix_len + body.size();
|
||||
}
|
||||
|
||||
inline msgpack::result<DecodedMessage> try_decode(const uint8_t *data, size_t len) {
|
||||
msgpack::parser p(data, static_cast<int>(len));
|
||||
|
||||
Envelope env;
|
||||
auto r = msgpack::unpack(p, env);
|
||||
if (!r) return std::unexpected(r.error());
|
||||
|
||||
uint32_t expected = halfsiphash::hash32(env.payload, hash_key);
|
||||
if (env.checksum != expected) return std::unexpected(msgpack::error_code::invalid);
|
||||
|
||||
msgpack::parser inner(env.payload.data(), static_cast<int>(env.payload.size()));
|
||||
if (!inner.is_ext()) return std::unexpected(msgpack::error_code::type_error);
|
||||
auto ext = inner.get_ext();
|
||||
if (!ext) return std::unexpected(ext.error());
|
||||
|
||||
auto& [type_id, ext_data] = *ext;
|
||||
return DecodedMessage{env.message_id, type_id,
|
||||
std::span<const uint8_t>(reinterpret_cast<const uint8_t*>(ext_data.data()), ext_data.size())};
|
||||
}
|
||||
|
||||
template <size_t N>
|
||||
inline msgpack::result<DecodedMessage> try_decode(const static_vector<uint8_t, N> &buf) {
|
||||
return try_decode(buf.data(), buf.size());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline msgpack::result<T> decode_response(const uint8_t *data, size_t len) {
|
||||
msgpack::parser p(data, static_cast<int>(len));
|
||||
|
||||
Envelope env;
|
||||
auto r = msgpack::unpack(p, env);
|
||||
if (!r) return std::unexpected(r.error());
|
||||
|
||||
uint32_t expected = halfsiphash::hash32(env.payload, hash_key);
|
||||
if (env.checksum != expected) return std::unexpected(msgpack::error_code::invalid);
|
||||
|
||||
msgpack::parser inner(env.payload.data(), static_cast<int>(env.payload.size()));
|
||||
T out;
|
||||
auto r2 = msgpack::unpack(inner, out);
|
||||
if (!r2) return std::unexpected(r2.error());
|
||||
return out;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user