MACRAW IP stack with ARP/ICMP, stable device ordering, LED blink on test

firmware/include/dispatch.h

@@ -1,9 +1,9 @@
 #pragma once
 #include <cstdint>
+#include <functional>
 #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);
 
@@ -26,5 +26,5 @@ std::vector<std::vector<uint8_t>> typed_handler(uint32_t message_id, std::span<c
 }
 
 void dispatch_init();
-[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers,
-                               std::span<const w6300::socket_id> sockets = {});
+void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn);
+[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers);

firmware/include/net.h

@@ -1,3 +1,12 @@
 #pragma once
+#include <array>
+#include <cstdint>
+
+struct net_state {
+    std::array<uint8_t, 6> mac;
+    std::array<uint8_t, 4> ip;
+};
 
 bool net_init();
+const net_state& net_get_state();
+void net_poll();

firmware/lib/dispatch.cpp

@@ -8,31 +8,32 @@
 #include "timer_queue.h"
 #include "net.h"
 
+static timer_queue timers;
+
 void dispatch_init() {
     tusb_init();
     net_init();
 }
 
-[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers,
-                               std::span<const w6300::socket_id> sockets) {
+void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
+    timers.schedule_ms(ms, std::move(fn));
+}
+
+[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers) {
     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;
     }
 
     static usb_cdc usb;
-    static timer_queue timers;
     static static_vector<uint8_t, 256> usb_rx_buf;
 
-    for (auto sn : sockets) {
-        w6300::set_socket_io_mode(sn, w6300::sock_io_mode::nonblock);
-    }
-
     while (true) {
         tud_task();
 
         usb.drain();
         timers.run();
+        net_poll();
 
         while (tud_cdc_available()) {
             uint8_t byte;
@@ -60,25 +61,6 @@ void dispatch_init() {
             }
         }
 
-        for (auto sn : sockets) {
-            static uint8_t udp_rx_buf[512];
-            w6300::ip_address src_addr = {};
-            w6300::port_num src_port{0};
-
-            auto result = w6300::recvfrom(sn, std::span{udp_rx_buf}, src_addr, src_port);
-            if (!result) continue;
-
-            auto msg = try_decode(udp_rx_buf, *result);
-            if (!msg) continue;
-
-            auto it = handler_map.find(msg->type_id);
-            if (it != handler_map.end()) {
-                for (auto& response : it->second(msg->message_id, msg->payload)) {
-                    w6300::sendto(sn, std::span<const uint8_t>{response}, src_addr, src_port);
-                }
-            }
-        }
-
         __wfi();
     }
 }

firmware/lib/handlers.cpp

@@ -1,6 +1,6 @@
 #include "handlers.h"
 #include "pico/unique_id.h"
-#include "w6300.h"
+#include "net.h"
 
 std::vector<std::vector<uint8_t>> handle_picoboot(uint32_t message_id, std::span<const uint8_t>) {
     return {encode_response(message_id, ResponsePICOBOOT{})};
@@ -11,9 +11,9 @@ std::vector<std::vector<uint8_t>> handle_info(uint32_t message_id, std::span<con
     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.ip = ninfo.ip;
+    auto& ns = net_get_state();
+    resp.mac = ns.mac;
+    resp.ip = ns.ip;
     resp.firmware_name = firmware_name;
     return {encode_response(message_id, resp)};
 }

firmware/lib/net.cpp

@@ -1,7 +1,145 @@
 #include "net.h"
+#include <cstring>
 #include "pico/unique_id.h"
 #include "w6300.h"
 
+static net_state state;
+static w6300::socket_id raw_socket{0};
+
+static constexpr uint16_t ETHERTYPE_ARP = 0x0806;
+static constexpr uint16_t ETHERTYPE_IPV4 = 0x0800;
+static constexpr uint8_t IP_PROTO_ICMP = 1;
+static constexpr uint8_t ICMP_ECHO_REQUEST = 8;
+static constexpr uint8_t ICMP_ECHO_REPLY = 0;
+static constexpr uint16_t ARP_OP_REQUEST = 1;
+static constexpr uint16_t ARP_OP_REPLY = 2;
+
+static uint16_t read_u16(const uint8_t* p) { return (p[0] << 8) | p[1]; }
+
+static void write_u16(uint8_t* p, uint16_t v) {
+    p[0] = v >> 8;
+    p[1] = v & 0xFF;
+}
+
+static uint16_t ip_checksum(const uint8_t* data, size_t len) {
+    uint32_t sum = 0;
+    for (size_t i = 0; i < len - 1; i += 2)
+        sum += read_u16(data + i);
+    if (len & 1)
+        sum += data[len - 1] << 8;
+    while (sum >> 16)
+        sum = (sum & 0xFFFF) + (sum >> 16);
+    return ~sum;
+}
+
+static bool mac_match(const uint8_t* dst) {
+    static constexpr uint8_t broadcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+    return memcmp(dst, state.mac.data(), 6) == 0 ||
+           memcmp(dst, broadcast, 6) == 0;
+}
+
+static bool ip_match(const uint8_t* dst) {
+    return memcmp(dst, state.ip.data(), 4) == 0;
+}
+
+static void send_raw(const uint8_t* data, size_t len) {
+    w6300::ip_address dummy = {};
+    w6300::sendto(raw_socket, std::span<const uint8_t>{data, len}, dummy, w6300::port_num{0});
+}
+
+static void handle_arp(const uint8_t* frame, size_t len) {
+    if (len < 42) return;
+    const uint8_t* arp = frame + 14;
+
+    if (read_u16(arp) != 1) return;
+    if (read_u16(arp + 2) != ETHERTYPE_IPV4) return;
+    if (arp[4] != 6 || arp[5] != 4) return;
+    if (read_u16(arp + 6) != ARP_OP_REQUEST) return;
+    if (!ip_match(arp + 24)) return;
+
+    uint8_t reply[42];
+    memcpy(reply, frame + 6, 6);
+    memcpy(reply + 6, state.mac.data(), 6);
+    write_u16(reply + 12, ETHERTYPE_ARP);
+
+    uint8_t* rarp = reply + 14;
+    write_u16(rarp, 1);
+    write_u16(rarp + 2, ETHERTYPE_IPV4);
+    rarp[4] = 6;
+    rarp[5] = 4;
+    write_u16(rarp + 6, ARP_OP_REPLY);
+    memcpy(rarp + 8, state.mac.data(), 6);
+    memcpy(rarp + 14, state.ip.data(), 4);
+    memcpy(rarp + 18, arp + 8, 6);
+    memcpy(rarp + 24, arp + 14, 4);
+
+    send_raw(reply, 42);
+}
+
+static void handle_icmp(const uint8_t* frame, size_t len) {
+    const uint8_t* ip = frame + 14;
+    size_t ip_hdr_len = (ip[0] & 0x0F) * 4;
+    size_t ip_total_len = read_u16(ip + 2);
+
+    if (14 + ip_total_len > len) return;
+    if (ip[9] != IP_PROTO_ICMP) return;
+    if (!ip_match(ip + 16)) return;
+
+    const uint8_t* icmp = ip + ip_hdr_len;
+    size_t icmp_len = ip_total_len - ip_hdr_len;
+    if (icmp_len < 8) return;
+    if (icmp[0] != ICMP_ECHO_REQUEST) return;
+
+    uint8_t reply[1514];
+    size_t reply_len = 14 + ip_total_len;
+    if (reply_len > sizeof(reply)) return;
+
+    memcpy(reply, frame + 6, 6);
+    memcpy(reply + 6, state.mac.data(), 6);
+    write_u16(reply + 12, ETHERTYPE_IPV4);
+
+    uint8_t* rip = reply + 14;
+    memcpy(rip, ip, ip_hdr_len);
+    memcpy(rip + 12, ip + 16, 4);
+    memcpy(rip + 16, ip + 12, 4);
+    rip[8] = 64;
+    memset(rip + 10, 0, 2);
+    uint16_t ip_cksum = ip_checksum(rip, ip_hdr_len);
+    write_u16(rip + 10, ip_cksum);
+
+    uint8_t* ricmp = rip + ip_hdr_len;
+    memcpy(ricmp, icmp, icmp_len);
+    ricmp[0] = ICMP_ECHO_REPLY;
+    memset(ricmp + 2, 0, 2);
+    uint16_t icmp_cksum = ip_checksum(ricmp, icmp_len);
+    write_u16(ricmp + 2, icmp_cksum);
+
+    send_raw(reply, reply_len);
+}
+
+static void handle_ipv4(const uint8_t* frame, size_t len) {
+    if (len < 34) return;
+    const uint8_t* ip = frame + 14;
+    if ((ip[0] >> 4) != 4) return;
+
+    handle_icmp(frame, len);
+}
+
+static void process_frame(const uint8_t* frame, size_t len) {
+    if (len < 14) return;
+    if (!mac_match(frame)) return;
+
+    uint16_t ethertype = read_u16(frame + 12);
+    switch (ethertype) {
+    case ETHERTYPE_ARP:
+        handle_arp(frame, len);
+        break;
+    case ETHERTYPE_IPV4:
+        handle_ipv4(frame, len);
+        break;
+    }
+}
+
 bool net_init() {
     w6300::init_spi();
     w6300::init_critical_section();
@@ -11,21 +149,33 @@ bool net_init() {
 
     pico_unique_board_id_t uid;
     pico_get_unique_board_id(&uid);
-    w6300::net_info info = {};
-    info.mac[0] = (uid.id[0] & 0xFC) | 0x02;
-    info.mac[1] = uid.id[1];
-    info.mac[2] = uid.id[2];
-    info.mac[3] = uid.id[3];
-    info.mac[4] = uid.id[4];
-    info.mac[5] = uid.id[5];
+    state.mac[0] = (uid.id[0] & 0xFC) | 0x02;
+    state.mac[1] = uid.id[1];
+    state.mac[2] = uid.id[2];
+    state.mac[3] = uid.id[3];
+    state.mac[4] = uid.id[4];
+    state.mac[5] = uid.id[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};
+    state.ip[0] = 169;
+    state.ip[1] = 254;
+    state.ip[2] = state.mac[4];
+    state.ip[3] = state.mac[5];
 
-    w6300::init_net(info);
+    w6300::open_socket(raw_socket, w6300::protocol::macraw, w6300::port_num{0}, w6300::sock_flag::none);
+    w6300::set_socket_io_mode(raw_socket, w6300::sock_io_mode::nonblock);
 
     return true;
 }
+
+const net_state& net_get_state() {
+    return state;
+}
+
+void net_poll() {
+    static uint8_t rx_buf[1518];
+    w6300::ip_address dummy_addr = {};
+    w6300::port_num dummy_port{0};
+    auto result = w6300::recvfrom(raw_socket, std::span{rx_buf}, dummy_addr, dummy_port);
+    if (!result) return;
+    process_frame(rx_buf, *result);
+}

firmware/test.cpp

@@ -1,9 +1,18 @@
 #include <unordered_map>
+#include "pico/stdlib.h"
 #include "pico/time.h"
+#include "hardware/gpio.h"
 #include "dispatch.h"
 #include "handlers.h"
 #include "w6300.h"
 
+static constexpr uint8_t LED_PIN = 25;
+
+static void led_toggle() {
+    gpio_xor_mask(1 << LED_PIN);
+    dispatch_schedule_ms(1000, led_toggle);
+}
+
 std::string_view firmware_name = "picomap_test";
 
 static constexpr uint16_t PICOMAP_DISCOVERY_PORT = 28777;
@@ -114,8 +123,9 @@ static constexpr handler_entry handlers[] = {
 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);
+    gpio_init(LED_PIN);
+    gpio_set_dir(LED_PIN, GPIO_OUT);
+    dispatch_schedule_ms(1000, led_toggle);
 
     dispatch_run(handlers);
 }