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

Add IGMP, prepend_buffer/parse_buffer, split UDP header, discovery tests, test all

Browse Source
This commit is contained in:
Ian Gulliver
2026-04-11 09:04:55 +09:00
parent e486f6501a
commit a6225faa2b
17 changed files with 582 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
firmware/lib/icmp.cpp
View File

@@ -1,96 +1,62 @@
#include "icmp.h"
#include <cstring>
#include "ipv4.h"
#include "parse_buffer.h"
#include "prepend_buffer.h"
namespace icmp {
void handle(std::span<const uint8_t> frame, span_writer& tx,
eth::mac_addr our_mac, ipv4::ip4_addr our_ip,
std::function<void(std::span<const uint8_t>)> send_raw) {
auto& ip = *reinterpret_cast<const ipv4::header*>(frame.data());
size_t ip_hdr_len = ip.ip_header_len();
size_t ip_total = ip.ip_total_len();
parse_buffer pb(frame);
auto* eth_hdr = pb.consume<eth::header>();
auto* ip = pb.consume<ipv4::header>();
if (!ip) return;
if (ip->protocol != 1) return;
if (sizeof(eth::header) + ip_total > frame.size()) return;
if (ip.protocol != 1) return;
size_t options_len = ip->header_len() - sizeof(ipv4::header);
if (options_len > 0 && !pb.skip(options_len)) return;
auto& icmp_pkt = *reinterpret_cast<const echo*>(frame.data() + sizeof(eth::header) + ip_hdr_len);
size_t icmp_len = ip_total - ip_hdr_len;
if (icmp_len < sizeof(echo)) return;
if (icmp_pkt.type != 8) return;
size_t icmp_len = ip->total() - ip->header_len();
if (pb.remaining_size() < icmp_len) return;
size_t reply_len = sizeof(eth::header) + ip_total;
if (reply_len > tx.capacity()) return;
auto* icmp_pkt = pb.consume<echo>();
if (!icmp_pkt) return;
if (icmp_pkt->type != 8) return;
memcpy(tx.data(), frame.data(), reply_len);
auto& rip = *reinterpret_cast<ipv4::header*>(tx.data());
rip.eth.dst = ip.eth.src;
rip.eth.src = our_mac;
rip.src = our_ip;
rip.dst = ip.src;
rip.ttl = 64;
rip.checksum = 0;
rip.checksum = ipv4::checksum(rip.ip_start(), ip_hdr_len);
prepend_buffer<1514> buf;
memcpy(buf.append(icmp_len), pb.remaining().data() - sizeof(echo), icmp_len);
auto& ricmp = *reinterpret_cast<echo*>(tx.data() + sizeof(eth::header) + ip_hdr_len);
ricmp.type = 0;
ricmp.checksum = 0;
ricmp.checksum = ipv4::checksum(&ricmp, icmp_len);
auto* reply = reinterpret_cast<echo*>(buf.data());
reply->type = 0;
reply->checksum = 0;
reply->checksum = ipv4::checksum(reply, icmp_len);
send_raw({tx.data(), reply_len});
}
size_t build_echo_request(std::span<uint8_t> buf,
eth::mac_addr src_mac, ipv4::ip4_addr src_ip,
eth::mac_addr dst_mac, ipv4::ip4_addr dst_ip,
uint16_t id, uint16_t seq) {
size_t total = sizeof(ipv4::header) + sizeof(echo);
if (buf.size() < total) return 0;
memset(buf.data(), 0, total);
auto& ip = *reinterpret_cast<ipv4::header*>(buf.data());
ip.eth.dst = dst_mac;
ip.eth.src = src_mac;
ip.eth.ethertype = eth::ETH_IPV4;
ip.ver_ihl = 0x45;
ip.dscp_ecn = 0;
ip.total_len = __builtin_bswap16(20 + sizeof(echo));
ip.identification = 0;
ip.flags_frag = 0;
ip.ttl = 64;
ip.protocol = 1;
ip.checksum = 0;
ip.src = src_ip;
ip.dst = dst_ip;
ip.checksum = ipv4::checksum(ip.ip_start(), 20);
auto& icmp_pkt = *reinterpret_cast<echo*>(buf.data() + sizeof(ipv4::header));
icmp_pkt.type = 8;
icmp_pkt.code = 0;
icmp_pkt.checksum = 0;
icmp_pkt.id = id;
icmp_pkt.seq = seq;
icmp_pkt.checksum = ipv4::checksum(&icmp_pkt, sizeof(echo));
return total;
ipv4::prepend(buf, eth_hdr->src, our_mac, our_ip, ip->src, 1, icmp_len);
send_raw(buf.span());
}
bool parse_echo_reply(std::span<const uint8_t> frame, ipv4::ip4_addr& src_ip, uint16_t expected_id) {
if (frame.size() < sizeof(ipv4::header)) return false;
auto& ip = *reinterpret_cast<const ipv4::header*>(frame.data());
if ((ip.ver_ihl >> 4) != 4) return false;
if (ip.eth.ethertype != eth::ETH_IPV4) return false;
if (ip.protocol != 1) return false;
parse_buffer pb(frame);
auto* eth_hdr = pb.consume<eth::header>();
if (!eth_hdr) return false;
if (eth_hdr->ethertype != eth::ETH_IPV4) return false;
size_t ip_hdr_len = ip.ip_header_len();
if (sizeof(eth::header) + ip_hdr_len + sizeof(echo) > frame.size()) return false;
auto* ip = pb.consume<ipv4::header>();
if (!ip) return false;
if ((ip->ver_ihl >> 4) != 4) return false;
if (ip->protocol != 1) return false;
auto& icmp_pkt = *reinterpret_cast<const echo*>(frame.data() + sizeof(eth::header) + ip_hdr_len);
if (icmp_pkt.type != 0) return false;
if (icmp_pkt.id != expected_id) return false;
size_t options_len = ip->header_len() - sizeof(ipv4::header);
if (options_len > 0 && !pb.skip(options_len)) return false;
src_ip = ip.src;
auto* icmp_pkt = pb.consume<echo>();
if (!icmp_pkt) return false;
if (icmp_pkt->type != 0) return false;
if (icmp_pkt->id != expected_id) return false;
src_ip = ip->src;
return true;
}