From 46db2fd9666e9e724979da832e725fc8eac0c8a0 Mon Sep 17 00:00:00 2001 From: Ian Gulliver Date: Tue, 7 Apr 2026 07:34:24 +0900 Subject: [PATCH] MACRAW IP stack with ARP/ICMP, stable device ordering, LED blink on test --- firmware/include/dispatch.h | 6 +- firmware/include/net.h | 9 ++ firmware/lib/dispatch.cpp | 34 ++----- firmware/lib/handlers.cpp | 8 +- firmware/lib/net.cpp | 176 +++++++++++++++++++++++++++++++++--- firmware/test.cpp | 14 ++- lib/client/serial.go | 16 +++- 7 files changed, 213 insertions(+), 50 deletions(-) diff --git a/firmware/include/dispatch.h b/firmware/include/dispatch.h index 29c3141..099f93a 100644 --- a/firmware/include/dispatch.h +++ b/firmware/include/dispatch.h @@ -1,9 +1,9 @@ #pragma once #include +#include #include #include #include "wire.h" -#include "w6300.h" using handler_fn = std::vector> (*)(uint32_t message_id, std::span payload); @@ -26,5 +26,5 @@ std::vector> typed_handler(uint32_t message_id, std::span handlers, - std::span sockets = {}); +void dispatch_schedule_ms(uint32_t ms, std::function fn); +[[noreturn]] void dispatch_run(std::span handlers); diff --git a/firmware/include/net.h b/firmware/include/net.h index c688261..e2a2327 100644 --- a/firmware/include/net.h +++ b/firmware/include/net.h @@ -1,3 +1,12 @@ #pragma once +#include +#include + +struct net_state { + std::array mac; + std::array ip; +}; bool net_init(); +const net_state& net_get_state(); +void net_poll(); diff --git a/firmware/lib/dispatch.cpp b/firmware/lib/dispatch.cpp index c4c5e1a..111a642 100644 --- a/firmware/lib/dispatch.cpp +++ b/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 handlers, - std::span sockets) { +void dispatch_schedule_ms(uint32_t ms, std::function fn) { + timers.schedule_ms(ms, std::move(fn)); +} + +[[noreturn]] void dispatch_run(std::span handlers) { std::unordered_map> (*)(uint32_t, std::span)> handler_map; for (auto& entry : handlers) { handler_map[entry.type_id] = entry.handle; } static usb_cdc usb; - static timer_queue timers; static static_vector 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{response}, src_addr, src_port); - } - } - } - __wfi(); } } diff --git a/firmware/lib/handlers.cpp b/firmware/lib/handlers.cpp index 0e2e55c..faed53f 100644 --- a/firmware/lib/handlers.cpp +++ b/firmware/lib/handlers.cpp @@ -1,6 +1,6 @@ #include "handlers.h" #include "pico/unique_id.h" -#include "w6300.h" +#include "net.h" std::vector> handle_picoboot(uint32_t message_id, std::span) { return {encode_response(message_id, ResponsePICOBOOT{})}; @@ -11,9 +11,9 @@ std::vector> handle_info(uint32_t message_id, std::span #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{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); +} diff --git a/firmware/test.cpp b/firmware/test.cpp index 2db87a3..76defe4 100644 --- a/firmware/test.cpp +++ b/firmware/test.cpp @@ -1,9 +1,18 @@ #include +#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); } diff --git a/lib/client/serial.go b/lib/client/serial.go index a4a4ec3..570aa1c 100644 --- a/lib/client/serial.go +++ b/lib/client/serial.go @@ -3,6 +3,7 @@ package client import ( "fmt" "io" + "slices" "strings" "time" @@ -15,12 +16,23 @@ func ListSerial() ([]string, error) { if err != nil { return nil, fmt.Errorf("enumerating ports: %w", err) } - var result []string + type entry struct { + name string + serial string + } + var entries []entry for _, p := range ports { if p.IsUSB && p.VID == "2E8A" && strings.HasPrefix(p.Name, "/dev/cu.") { - result = append(result, p.Name) + entries = append(entries, entry{p.Name, p.SerialNumber}) } } + slices.SortFunc(entries, func(a, b entry) int { + return strings.Compare(a.serial, b.serial) + }) + var result []string + for _, e := range entries { + result = append(result, e.name) + } return result, nil }