146 lines
4.3 KiB
C++
146 lines
4.3 KiB
C++
#include "net.h"
|
|
#include "pico/unique_id.h"
|
|
#include "pico/time.h"
|
|
#include "eth.h"
|
|
#include "arp.h"
|
|
#include "ipv4.h"
|
|
#include "udp.h"
|
|
#include "igmp.h"
|
|
#include "parse_buffer.h"
|
|
#include "prepend_buffer.h"
|
|
#include "w6300.h"
|
|
#include "debug_log.h"
|
|
|
|
static constexpr ipv4::ip4_addr IP_BROADCAST_SUBNET = {169, 254, 255, 255};
|
|
static constexpr uint16_t PICOMAP_PORT = __builtin_bswap16(28781);
|
|
|
|
static net_state state;
|
|
static w6300::socket_id raw_socket{0};
|
|
static net_handler msg_handler;
|
|
static frame_cb_list frame_callbacks;
|
|
|
|
void net_send_raw(std::span<const uint8_t> data) {
|
|
dlog_if_slow("net_send_raw", 1000, [&]{
|
|
auto result = w6300::send(raw_socket, data);
|
|
if (!result)
|
|
dlogf("w6300 send failed: %zu bytes, err %d",
|
|
data.size(), static_cast<int>(result.error()));
|
|
});
|
|
}
|
|
|
|
static void handle_udp(std::span<const uint8_t> frame, span_writer& tx) {
|
|
parse_buffer pb(frame);
|
|
auto* eth_hdr = pb.consume<eth::header>();
|
|
auto* ip = pb.consume<ipv4::header>();
|
|
if (!ip) return;
|
|
|
|
size_t options_len = ip->header_len() - sizeof(ipv4::header);
|
|
if (options_len > 0 && !pb.skip(options_len)) return;
|
|
|
|
auto* uhdr = pb.consume<udp::header>();
|
|
if (!uhdr) return;
|
|
if (uhdr->dst_port != PICOMAP_PORT) return;
|
|
if (!msg_handler) return;
|
|
|
|
size_t udp_len = __builtin_bswap16(uhdr->length);
|
|
if (udp_len < sizeof(udp::header)) return;
|
|
size_t payload_len = udp_len - sizeof(udp::header);
|
|
if (pb.remaining_size() < payload_len) return;
|
|
|
|
eth::mac_addr dst_mac = eth_hdr->src;
|
|
ipv4::ip4_addr dst_ip = ip->src;
|
|
uint16_t dst_port = uhdr->src_port;
|
|
|
|
msg_handler(pb.remaining().subspan(0, payload_len),
|
|
[dst_mac, dst_ip, dst_port](std::span<const uint8_t> resp_data) {
|
|
prepend_buffer<4096> buf;
|
|
buf.append_copy(resp_data);
|
|
udp::prepend(buf, dst_mac, state.mac, state.ip, dst_ip,
|
|
PICOMAP_PORT, dst_port, resp_data.size());
|
|
net_send_raw(buf.span());
|
|
});
|
|
}
|
|
|
|
static bool mac_match(const eth::mac_addr& dst) {
|
|
return dst == state.mac || dst == eth::MAC_BROADCAST || igmp::is_member_mac(dst);
|
|
}
|
|
|
|
static void process_frame(std::span<const uint8_t> frame, span_writer& tx) {
|
|
if (frame.size() < sizeof(eth::header)) return;
|
|
auto& eth_hdr = *reinterpret_cast<const eth::header*>(frame.data());
|
|
|
|
if (!mac_match(eth_hdr.dst)) return;
|
|
|
|
frame_callbacks.for_each([&](frame_cb_list::node* n) {
|
|
if (n->value().fn(frame))
|
|
frame_callbacks.remove(n);
|
|
});
|
|
|
|
switch (eth_hdr.ethertype) {
|
|
case eth::ETH_ARP:
|
|
arp::handle(frame, tx, state.mac, state.ip, net_send_raw);
|
|
break;
|
|
case eth::ETH_IPV4:
|
|
ipv4::handle(frame, tx, state.mac, state.ip, IP_BROADCAST_SUBNET, net_send_raw, handle_udp);
|
|
break;
|
|
}
|
|
}
|
|
|
|
bool net_init() {
|
|
w6300::init_spi();
|
|
w6300::reset();
|
|
w6300::init();
|
|
if (!w6300::check()) return false;
|
|
|
|
pico_unique_board_id_t uid;
|
|
pico_get_unique_board_id(&uid);
|
|
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];
|
|
|
|
state.ip[0] = 169;
|
|
state.ip[1] = 254;
|
|
state.ip[2] = state.mac[4];
|
|
state.ip[3] = state.mac[5];
|
|
|
|
w6300::open_socket(raw_socket, w6300::protocol::macraw, w6300::sock_flag::none);
|
|
w6300::set_interrupt_mask(w6300::ik_sock_0);
|
|
|
|
igmp::join(igmp::PICOMAP_DISCOVERY_GROUP, state.mac, state.ip, net_send_raw);
|
|
|
|
return true;
|
|
}
|
|
|
|
const net_state& net_get_state() {
|
|
return state;
|
|
}
|
|
|
|
void net_set_handler(net_handler handler) {
|
|
msg_handler = std::move(handler);
|
|
}
|
|
|
|
frame_cb_handle net_add_frame_callback(net_frame_callback cb) {
|
|
return frame_callbacks.insert({std::move(cb)});
|
|
}
|
|
|
|
void net_remove_frame_callback(frame_cb_handle h) {
|
|
frame_callbacks.remove(h);
|
|
}
|
|
|
|
void net_poll(std::span<uint8_t> tx) {
|
|
if (!w6300::irq_pending) return;
|
|
w6300::irq_pending = false;
|
|
w6300::clear_interrupt(w6300::ik_int_all);
|
|
static uint8_t rx_buf[1518];
|
|
int budget = 2;
|
|
while (budget-- > 0 && w6300::get_socket_recv_buf(raw_socket) > 0) {
|
|
auto result = w6300::recv(raw_socket, std::span{rx_buf});
|
|
if (!result) break;
|
|
span_writer tx_writer(tx);
|
|
process_frame({rx_buf, *result}, tx_writer);
|
|
}
|
|
}
|