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

cmd/info/main.go

@@ -60,7 +60,7 @@ func run() error {
 			r.info.BoardID[0], r.info.BoardID[1], r.info.BoardID[2], r.info.BoardID[3],
 			r.info.BoardID[4], r.info.BoardID[5], r.info.BoardID[6], r.info.BoardID[7])
 		fmt.Printf("  MAC: %s\n", net.HardwareAddr(r.info.MAC[:]))
-		fmt.Printf("  Link-Local: %s\n", net.IP(r.info.LinkLocal[:]))
+		fmt.Printf("  IP: %s\n", net.IP(r.info.IP[:]))
 		fmt.Printf("  Firmware: %s\n", r.info.FirmwareName)
 	}
 

cmd/test/main.go

@@ -0,0 +1,73 @@
+package main
+
+import (
+	"fmt"
+	"os"
+	"time"
+
+	"github.com/theater/picomap/lib/client"
+)
+
+func main() {
+	if len(os.Args) < 2 {
+		fmt.Fprintf(os.Stderr, "usage: test <name>\n")
+		os.Exit(1)
+	}
+
+	if err := run(os.Args[1]); err != nil {
+		fmt.Fprintf(os.Stderr, "error: %v\n", err)
+		os.Exit(1)
+	}
+}
+
+func run(name string) error {
+	devs, err := client.ListSerial()
+	if err != nil {
+		return err
+	}
+
+	var testDev string
+	for _, dev := range devs {
+		c, err := client.NewSerial(dev, 2*time.Second)
+		if err != nil {
+			continue
+		}
+		info, err := c.Info()
+		c.Close()
+		if err != nil {
+			continue
+		}
+		if info.FirmwareName == "picomap_test" {
+			testDev = dev
+			break
+		}
+	}
+	if testDev == "" {
+		return fmt.Errorf("no picomap_test device found")
+	}
+
+	fmt.Printf("test %s on %s\n", name, testDev)
+
+	c, err := client.NewSerial(testDev, 10*time.Second)
+	if err != nil {
+		return err
+	}
+	defer c.Close()
+
+	result, err := c.Test(name)
+	if err != nil {
+		return fmt.Errorf("remote: %w", err)
+	}
+
+	for _, msg := range result.Messages {
+		fmt.Printf("  [remote] %s\n", msg)
+	}
+
+	if result.Pass {
+		fmt.Println("PASS")
+	} else {
+		fmt.Println("FAIL")
+		os.Exit(1)
+	}
+	return nil
+}

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 = {
+static constexpr std::array<uint8_t, 4> picomap_discovery_ip = {239, 0, 112, 109};
-    0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-    0x70, 0x69, 0x63, 0x6f, 0x6d, 0x61, 0x70, 0x00,
-};
 
 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_picoboot(uint32_t message_id, std::span<const uint8_t> payload);
-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> 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() const { return std::tie(board_id, mac, ip, firmware_name); }
-    auto as_tuple() { return std::tie(board_id, mac, link_local, 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.ip[0] = 169;
-    info.lla[1] = 0x80;
+    info.ip[1] = 254;
-    info.lla[8] = info.mac[0] ^ 0x02;
+    info.ip[2] = info.mac[4];
-    info.lla[9] = info.mac[1];
+    info.ip[3] = info.mac[5];
-    info.lla[10] = info.mac[2];
+    info.sn = {255, 255, 0, 0};
-    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];
 
     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);
 }

lib/client/client.go

@@ -92,3 +92,7 @@ func (c *Client) PICOBOOT() error {
 func (c *Client) Info() (*ResponseInfo, error) {
 	return roundTrip[ResponseInfo](c, &RequestInfo{})
 }
+
+func (c *Client) Test(name string) (*ResponseTest, error) {
+	return roundTrip[ResponseTest](c, &RequestTest{Name: name})
+}

lib/client/serial.go

@@ -3,6 +3,7 @@ package client
 import (
 	"fmt"
 	"io"
+	"strings"
 	"time"
 
 	"go.bug.st/serial"
@@ -16,7 +17,7 @@ func ListSerial() ([]string, error) {
 	}
 	var result []string
 	for _, p := range ports {
-		if p.IsUSB {
+		if p.IsUSB && p.VID == "2E8A" && strings.HasPrefix(p.Name, "/dev/cu.") {
 			result = append(result, p.Name)
 		}
 	}

lib/client/types.go

@@ -9,10 +9,19 @@ type RequestInfo struct{}
 type ResponseInfo struct {
 	BoardID      [8]byte
 	MAC          [6]byte
-	LinkLocal    [16]byte
+	IP           [4]byte
 	FirmwareName string
 }
 
+type RequestTest struct {
+	Name string
+}
+
+type ResponseTest struct {
+	Pass     bool
+	Messages []string
+}
+
 type DeviceError struct {
 	Code    uint32
 	Message string
@@ -35,4 +44,6 @@ func init() {
 	msgpack.RegisterExt(3, (*ResponsePICOBOOT)(nil))
 	msgpack.RegisterExt(4, (*RequestInfo)(nil))
 	msgpack.RegisterExt(5, (*ResponseInfo)(nil))
+	msgpack.RegisterExt(127, (*RequestTest)(nil))
+	msgpack.RegisterExt(126, (*ResponseTest)(nil))
 }