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

Add UDP transport with picomap port 28781, info -udp flag

Browse Source
This commit is contained in:
Ian Gulliver
2026-04-07 21:36:50 +09:00
parent 3d749add7d
commit 9989d8c66a
5 changed files with 185 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
firmware/lib/dispatch.cpp
View File

@@ -30,6 +30,14 @@ void dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
static usb_cdc usb;
static static_vector<uint8_t, 256> usb_rx_buf;
net_set_handler([&](std::span<const uint8_t> payload) -> std::vector<std::vector<uint8_t>> {
auto msg = try_decode(payload.data(), payload.size());
if (!msg) return {};
auto it = handler_map.find(msg->type_id);
if (it == handler_map.end()) return {};
return it->second(msg->message_id, msg->payload);
});
while (true) {
dlog_if_slow("tud_task", 1000, [&]{ tud_task(); });
dlog_if_slow("drain", 1000, [&]{ usb.drain(); });

70
firmware/lib/net.cpp
View File

@@ -49,6 +49,15 @@ struct __attribute__((packed)) ipv4_header {
};
static_assert(sizeof(ipv4_header) == 34);
struct __attribute__((packed)) udp_header {
ipv4_header ip;
uint16_t src_port;
uint16_t dst_port;
uint16_t length;
uint16_t checksum;
};
static_assert(sizeof(udp_header) == 42);
struct __attribute__((packed)) icmp_echo {
uint8_t type;
uint8_t code;
@@ -64,12 +73,14 @@ static constexpr uint16_t ARP_HTYPE_ETH = __builtin_bswap16(1);
static constexpr uint16_t ARP_PTYPE_IPV4 = __builtin_bswap16(0x0800);
static constexpr uint16_t ARP_OP_REQUEST = __builtin_bswap16(1);
static constexpr uint16_t ARP_OP_REPLY = __builtin_bswap16(2);
static constexpr uint16_t PICOMAP_PORT = __builtin_bswap16(28781);
static constexpr mac_addr MAC_BROADCAST = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
static constexpr ip4_addr IP_BROADCAST_ALL = {255, 255, 255, 255};
static constexpr ip4_addr IP_BROADCAST_SUBNET = {169, 254, 255, 255};
static net_state state;
static w6300::socket_id raw_socket{0};
static net_handler msg_handler;
static uint16_t ip_checksum(const void* data, size_t len) {
auto p = static_cast<const uint8_t*>(data);
@@ -130,6 +141,58 @@ static void handle_arp(const uint8_t* frame, size_t len) {
send_raw(&reply, sizeof(reply));
}
static void handle_udp(const uint8_t* frame, size_t len) {
if (len < sizeof(udp_header)) return;
auto& pkt = *reinterpret_cast<const udp_header*>(frame);
if (pkt.dst_port != PICOMAP_PORT) return;
if (!ip_match_or_broadcast(pkt.ip.dst)) return;
if (!msg_handler) return;
size_t udp_len = __builtin_bswap16(pkt.length);
if (udp_len < 8) return;
if (sizeof(eth_header) + pkt.ip.ip_total_len() < sizeof(udp_header) + udp_len - 8) return;
auto* payload = frame + sizeof(udp_header);
size_t payload_len = udp_len - 8;
auto responses = msg_handler(std::span<const uint8_t>{payload, payload_len});
for (auto& resp : responses) {
uint8_t reply_buf[1514];
size_t udp_data_len = resp.size();
size_t ip_total = 20 + 8 + udp_data_len;
size_t reply_len = sizeof(eth_header) + ip_total;
if (reply_len > sizeof(reply_buf)) continue;
auto& rip = *reinterpret_cast<ipv4_header*>(reply_buf);
rip.eth.dst = pkt.ip.eth.src;
rip.eth.src = state.mac;
rip.eth.ethertype = ETH_IPV4;
rip.ver_ihl = 0x45;
rip.dscp_ecn = 0;
rip.total_len = __builtin_bswap16(ip_total);
rip.identification = 0;
rip.flags_frag = 0;
rip.ttl = 64;
rip.protocol = 17;
rip.checksum = 0;
rip.src = state.ip;
rip.dst = pkt.ip.src;
rip.checksum = ip_checksum(rip.ip_start(), 20);
auto& rudp = *reinterpret_cast<udp_header*>(reply_buf);
rudp.src_port = PICOMAP_PORT;
rudp.dst_port = pkt.src_port;
rudp.length = __builtin_bswap16(8 + udp_data_len);
rudp.checksum = 0;
memcpy(reply_buf + sizeof(udp_header), resp.data(), udp_data_len);
send_raw(reply_buf, reply_len);
}
}
static void handle_icmp(const uint8_t* frame, size_t len) {
auto& ip = *reinterpret_cast<const ipv4_header*>(frame);
size_t ip_hdr_len = ip.ip_header_len();
@@ -175,6 +238,9 @@ static void handle_ipv4(const uint8_t* frame, size_t len) {
case 1:
handle_icmp(frame, len);
break;
case 17:
handle_udp(frame, len);
break;
}
}
@@ -225,6 +291,10 @@ const net_state& net_get_state() {
return state;
}
void net_set_handler(net_handler handler) {
msg_handler = std::move(handler);
}
void net_poll() {
if (w6300::get_socket_recv_buf(raw_socket) == 0) return;
static uint8_t rx_buf[1518];