67 lines
1.9 KiB
C++
67 lines
1.9 KiB
C++
#include "icmp.h"
|
|
#include <cstring>
|
|
#include "eth.h"
|
|
#include "ipv4.h"
|
|
#include "parse_buffer.h"
|
|
#include "prepend_buffer.h"
|
|
|
|
namespace icmp {
|
|
|
|
void handle(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;
|
|
if (!ipv4::addressed_to_us(ip->dst)) return;
|
|
|
|
size_t options_len = ip->header_len() - sizeof(ipv4::header);
|
|
if (options_len > 0 && !pb.skip(options_len)) return;
|
|
|
|
size_t icmp_len = ip->total() - ip->header_len();
|
|
if (pb.remaining_size() < icmp_len) return;
|
|
|
|
auto* icmp_pkt = pb.consume<echo>();
|
|
if (!icmp_pkt) return;
|
|
if (icmp_pkt->type != 8) return;
|
|
|
|
prepend_buffer<4096> buf;
|
|
memcpy(buf.append(icmp_len), pb.remaining().data() - sizeof(echo), icmp_len);
|
|
|
|
auto* reply = reinterpret_cast<echo*>(buf.data());
|
|
reply->type = 0;
|
|
reply->checksum = 0;
|
|
reply->checksum = ipv4::checksum(reply, icmp_len);
|
|
|
|
ipv4::prepend(buf, eth_hdr->src, eth::get_mac(), ipv4::get_ip(), ip->src, 1, icmp_len);
|
|
eth::send_raw(buf.span());
|
|
}
|
|
|
|
void init() {
|
|
ipv4::register_protocol(1, handle);
|
|
}
|
|
|
|
bool parse_echo_reply(std::span<const uint8_t> frame, ipv4::ip4_addr& src_ip, uint16_t expected_id) {
|
|
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;
|
|
|
|
auto* ip = pb.consume<ipv4::header>();
|
|
if (!ip) return false;
|
|
if ((ip->ver_ihl >> 4) != 4) return false;
|
|
if (ip->protocol != 1) return false;
|
|
|
|
size_t options_len = ip->header_len() - sizeof(ipv4::header);
|
|
if (options_len > 0 && !pb.skip(options_len)) return false;
|
|
|
|
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;
|
|
}
|
|
|
|
} // namespace icmp
|