theater/picomap
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Ian Gulliver
2026-04-07 07:34:24 +09:00
parent 642e2ff318
commit 46db2fd966
7 changed files with 213 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
firmware/include/dispatch.h
View File

@@ -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);

9
firmware/include/net.h
View File

@@ -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();

34
firmware/lib/dispatch.cpp
View File

@@ -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();
}
}

8
firmware/lib/handlers.cpp
View File

@@ -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)};
}

176
firmware/lib/net.cpp
View File

@@ -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);
}

14
firmware/test.cpp
View File

@@ -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);
}

16
lib/client/serial.go
View File

@@ -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
}