Switch to IPv4 zeroconf, add test framework with discovery test, fix serial enumeration

firmware/firmware.cpp

@@ -6,13 +6,10 @@ std::string_view firmware_name = "picomap";
 
 static constexpr uint16_t PICOMAP_DISCOVERY_PORT = 28777;
 
-static constexpr std::array<uint8_t, 16> picomap_discovery_ip = {
-    0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x70, 0x69, 0x63, 0x6f, 0x6d, 0x61, 0x70, 0x00,
-};
+static constexpr std::array<uint8_t, 4> picomap_discovery_ip = {239, 0, 112, 109};
 
 static constexpr std::array<uint8_t, 6> picomap_discovery_mac = {
-    0x33, 0x33, 0x6d, 0x61, 0x70, 0x00,
+    0x01, 0x00, 0x5e, 0x00, 0x70, 0x6d,
 };
 
 static constexpr handler_entry handlers[] = {
@@ -27,10 +24,10 @@ int main() {
     w6300::set_socket_dest_mac(sn, picomap_discovery_mac);
     w6300::ip_address addr = {};
     std::copy(picomap_discovery_ip.begin(), picomap_discovery_ip.end(), addr.ip.begin());
-    addr.len = 16;
+    addr.len = 4;
     w6300::set_socket_dest_ip(sn, addr);
     w6300::set_socket_dest_port(sn, w6300::port_num{PICOMAP_DISCOVERY_PORT});
-    w6300::open_socket(sn, w6300::protocol::udp6, w6300::port_num{PICOMAP_DISCOVERY_PORT},
+    w6300::open_socket(sn, w6300::protocol::udp, w6300::port_num{PICOMAP_DISCOVERY_PORT},
                        w6300::sock_flag::multi_enable);
 
     w6300::socket_id sockets[] = {sn};

firmware/include/dispatch.h

@@ -2,13 +2,29 @@
 #include <cstdint>
 #include <span>
 #include <vector>
+#include "wire.h"
 #include "w6300.h"
 
+using handler_fn = std::vector<std::vector<uint8_t>> (*)(uint32_t message_id, std::span<const uint8_t> payload);
+
 struct handler_entry {
     int8_t type_id;
-    std::vector<std::vector<uint8_t>> (*handle)(uint32_t message_id);
+    handler_fn handle;
 };
 
+template <typename Req, auto Fn>
+std::vector<std::vector<uint8_t>> typed_handler(uint32_t message_id, 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) {
+        return {encode_response(message_id, DeviceError{1, "decode request ext_id=" +
+            std::to_string(Req::ext_id) + ": msgpack error " + std::to_string(static_cast<int>(r.error()))})};
+    }
+    return Fn(message_id, req);
+}
+
 void dispatch_init();
 [[noreturn]] void dispatch_run(std::span<const handler_entry> handlers,
                                std::span<const w6300::socket_id> sockets = {});

firmware/include/handlers.h

@@ -1,10 +1,11 @@
 #pragma once
 #include <cstdint>
+#include <span>
 #include <string_view>
 #include <vector>
 #include "wire.h"
 
 extern std::string_view firmware_name;
 
-std::vector<std::vector<uint8_t>> handle_picoboot(uint32_t message_id);
-std::vector<std::vector<uint8_t>> handle_info(uint32_t message_id);
+std::vector<std::vector<uint8_t>> handle_picoboot(uint32_t message_id, std::span<const uint8_t> payload);
+std::vector<std::vector<uint8_t>> handle_info(uint32_t message_id, std::span<const uint8_t> payload);

firmware/include/msgpack.h

@@ -367,6 +367,18 @@ public:
 
   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); }
 
@@ -744,6 +756,21 @@ inline result<parser> unpack(const parser &p, std::vector<uint8_t> &out) {
   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();

firmware/include/wire.h

@@ -47,10 +47,25 @@ 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, 16> link_local;
+    std::array<uint8_t, 4> ip;
     std::string firmware_name;
-    auto as_tuple() const { return std::tie(board_id, mac, link_local, firmware_name); }
-    auto as_tuple() { return std::tie(board_id, mac, link_local, firmware_name); }
+    auto as_tuple() const { return std::tie(board_id, mac, ip, firmware_name); }
+    auto as_tuple() { return std::tie(board_id, mac, ip, firmware_name); }
+};
+
+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] = {};
@@ -58,6 +73,7 @@ static constexpr uint8_t hash_key[8] = {};
 struct DecodedMessage {
     uint32_t message_id;
     int8_t type_id;
+    std::vector<uint8_t> payload;
 };
 
 inline std::vector<uint8_t> pack_envelope(uint32_t message_id, const std::vector<uint8_t> &payload) {
@@ -89,10 +105,37 @@ inline msgpack::result<DecodedMessage> try_decode(const uint8_t *data, size_t le
     auto ext = inner.get_ext();
     if (!ext) return std::unexpected(ext.error());
 
-    return DecodedMessage{env.message_id, std::get<0>(*ext)};
+    auto& [type_id, ext_data] = *ext;
+    return DecodedMessage{env.message_id, type_id,
+        std::vector<uint8_t>(reinterpret_cast<const uint8_t*>(ext_data.data()),
+                             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.data(), env.payload.size(), 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;
+}
+
+inline std::vector<uint8_t> encode_request(uint32_t message_id, const auto &msg) {
+    msgpack::packer inner;
+    inner.pack(msg);
+    return pack_envelope(message_id, inner.get_payload());
+}

firmware/lib/dispatch.cpp

@@ -15,7 +15,7 @@ void dispatch_init() {
 
 [[noreturn]] void dispatch_run(std::span<const handler_entry> handlers,
                                std::span<const w6300::socket_id> sockets) {
-    std::unordered_map<int8_t, std::vector<std::vector<uint8_t>> (*)(uint32_t)> handler_map;
+    std::unordered_map<int8_t, std::vector<std::vector<uint8_t>> (*)(uint32_t, std::span<const uint8_t>)> handler_map;
     for (auto& entry : handlers) {
         handler_map[entry.type_id] = entry.handle;
     }
@@ -50,7 +50,7 @@ void dispatch_init() {
 
             auto it = handler_map.find(msg->type_id);
             if (it != handler_map.end()) {
-                for (auto& response : it->second(msg->message_id)) {
+                for (auto& response : it->second(msg->message_id, msg->payload)) {
                     usb.send(response);
                 }
                 if (msg->type_id == RequestPICOBOOT::ext_id) {
@@ -73,7 +73,7 @@ void dispatch_init() {
 
             auto it = handler_map.find(msg->type_id);
             if (it != handler_map.end()) {
-                for (auto& response : it->second(msg->message_id)) {
+                for (auto& response : it->second(msg->message_id, msg->payload)) {
                     w6300::sendto(sn, std::span<const uint8_t>{response}, src_addr, src_port);
                 }
             }

firmware/lib/handlers.cpp

@@ -2,18 +2,18 @@
 #include "pico/unique_id.h"
 #include "w6300.h"
 
-std::vector<std::vector<uint8_t>> handle_picoboot(uint32_t message_id) {
+std::vector<std::vector<uint8_t>> handle_picoboot(uint32_t message_id, std::span<const uint8_t>) {
     return {encode_response(message_id, ResponsePICOBOOT{})};
 }
 
-std::vector<std::vector<uint8_t>> handle_info(uint32_t message_id) {
+std::vector<std::vector<uint8_t>> handle_info(uint32_t message_id, std::span<const uint8_t>) {
     ResponseInfo resp;
     pico_unique_board_id_t uid;
     pico_get_unique_board_id(&uid);
     std::copy(uid.id, uid.id + 8, resp.board_id.begin());
     auto ninfo = w6300::get_net_info();
     resp.mac = ninfo.mac;
-    resp.link_local = ninfo.lla;
+    resp.ip = ninfo.ip;
     resp.firmware_name = firmware_name;
     return {encode_response(message_id, resp)};
 }

firmware/lib/net.cpp

@@ -19,16 +19,11 @@ bool net_init() {
     info.mac[4] = uid.id[4];
     info.mac[5] = uid.id[5];
 
-    info.lla[0] = 0xfe;
-    info.lla[1] = 0x80;
-    info.lla[8] = info.mac[0] ^ 0x02;
-    info.lla[9] = info.mac[1];
-    info.lla[10] = info.mac[2];
-    info.lla[11] = 0xff;
-    info.lla[12] = 0xfe;
-    info.lla[13] = info.mac[3];
-    info.lla[14] = info.mac[4];
-    info.lla[15] = info.mac[5];
+    info.ip[0] = 169;
+    info.ip[1] = 254;
+    info.ip[2] = info.mac[4];
+    info.ip[3] = info.mac[5];
+    info.sn = {255, 255, 0, 0};
 
     w6300::init_net(info);
 

firmware/test.cpp

@@ -1,14 +1,121 @@
+#include <unordered_map>
+#include "pico/time.h"
 #include "dispatch.h"
 #include "handlers.h"
+#include "w6300.h"
 
 std::string_view firmware_name = "picomap_test";
 
+static constexpr uint16_t PICOMAP_DISCOVERY_PORT = 28777;
+
+static constexpr std::array<uint8_t, 4> picomap_discovery_ip = {239, 0, 112, 109};
+
+static constexpr std::array<uint8_t, 6> picomap_discovery_mac = {
+    0x01, 0x00, 0x5e, 0x00, 0x70, 0x6d,
+};
+
+static w6300::socket_id test_socket{1};
+
+static ResponseTest test_discovery() {
+    ResponseTest resp;
+    resp.pass = true;
+
+    w6300::ip_address dest = {};
+    std::copy(picomap_discovery_ip.begin(), picomap_discovery_ip.end(), dest.ip.begin());
+    dest.len = 4;
+
+    w6300::set_socket_dest_mac(test_socket, picomap_discovery_mac);
+
+    auto req = encode_request(0, RequestInfo{});
+    auto send_result = w6300::sendto(test_socket, std::span<const uint8_t>{req}, dest,
+                                     w6300::port_num{PICOMAP_DISCOVERY_PORT});
+    if (!send_result) {
+        resp.pass = false;
+        resp.messages.push_back("sendto: error " + std::to_string(static_cast<int>(send_result.error())));
+        return resp;
+    }
+
+    uint8_t rx_buf[512];
+    w6300::ip_address src_addr = {};
+    w6300::port_num src_port{0};
+
+    auto deadline = make_timeout_time_ms(5000);
+    std::expected<uint16_t, w6300::sock_error> recv_result = std::unexpected(w6300::sock_error::busy);
+    while (get_absolute_time() < deadline) {
+        recv_result = w6300::recvfrom(test_socket, std::span{rx_buf}, src_addr, src_port);
+        if (recv_result || recv_result.error() != w6300::sock_error::busy) break;
+    }
+
+    if (!recv_result) {
+        resp.pass = false;
+        if (recv_result.error() == w6300::sock_error::busy) {
+            resp.messages.push_back("recvfrom: timed out after 5s");
+        } else {
+            resp.messages.push_back("recvfrom: error " + std::to_string(static_cast<int>(recv_result.error())));
+        }
+        return resp;
+    }
+
+    resp.messages.push_back("received " + std::to_string(*recv_result) + " bytes from port " +
+                            std::to_string(static_cast<uint16_t>(src_port)));
+
+    auto info = decode_response<ResponseInfo>(rx_buf, *recv_result);
+    if (!info) {
+        resp.pass = false;
+        resp.messages.push_back("decode: msgpack error " + std::to_string(static_cast<int>(info.error())));
+        return resp;
+    }
+
+    if (info->firmware_name.empty()) {
+        resp.pass = false;
+        resp.messages.push_back("firmware_name is empty");
+    } else {
+        resp.messages.push_back("firmware_name: " + info->firmware_name);
+    }
+
+    bool mac_zero = true;
+    for (auto b : info->mac) { if (b != 0) { mac_zero = false; break; } }
+    if (mac_zero) {
+        resp.pass = false;
+        resp.messages.push_back("mac is all zeros");
+    }
+
+    bool ip_zero = true;
+    for (auto b : info->ip) { if (b != 0) { ip_zero = false; break; } }
+    if (ip_zero) {
+        resp.pass = false;
+        resp.messages.push_back("ip is all zeros");
+    }
+
+    return resp;
+}
+
+using test_fn = ResponseTest (*)();
+
+static const std::unordered_map<std::string_view, test_fn> tests = {
+    {"discovery", test_discovery},
+};
+
+static std::vector<std::vector<uint8_t>> handle_test(uint32_t message_id, const RequestTest& req) {
+    auto it = tests.find(req.name);
+    if (it == tests.end()) {
+        return {encode_response(message_id, ResponseTest{false, {"unknown test: " + req.name}})};
+    }
+
+    return {encode_response(message_id, it->second())};
+}
+
 static constexpr handler_entry handlers[] = {
     {RequestPICOBOOT::ext_id, handle_picoboot},
     {RequestInfo::ext_id, handle_info},
+    {RequestTest::ext_id, typed_handler<RequestTest, handle_test>},
 };
 
 int main() {
     dispatch_init();
+
+    w6300::open_socket(test_socket, w6300::protocol::udp, w6300::port_num{0}, w6300::sock_flag::dha_manual);
+    w6300::set_socket_io_mode(test_socket, w6300::sock_io_mode::nonblock);
+
     dispatch_run(handlers);
 }