64 lines
1.7 KiB
C++
64 lines
1.7 KiB
C++
#include "ipv4.h"
|
|
#include <array>
|
|
#include "igmp.h"
|
|
#include "net.h"
|
|
#include "parse_buffer.h"
|
|
|
|
namespace ipv4 {
|
|
|
|
static constexpr ip4_addr IP_BROADCAST_ALL = {255, 255, 255, 255};
|
|
|
|
uint16_t checksum(const void* data, size_t len) {
|
|
auto p = static_cast<const uint8_t*>(data);
|
|
uint32_t sum = 0;
|
|
for (size_t i = 0; i < len - 1; i += 2)
|
|
sum += (p[i] << 8) | p[i + 1];
|
|
if (len & 1)
|
|
sum += p[len - 1] << 8;
|
|
while (sum >> 16)
|
|
sum = (sum & 0xFFFF) + (sum >> 16);
|
|
return __builtin_bswap16(~sum);
|
|
}
|
|
|
|
bool addressed_to_us(ip4_addr dst) {
|
|
const auto& ns = net_get_state();
|
|
return dst == ns.ip || dst == IP_BROADCAST_ALL || dst == SUBNET_BROADCAST || igmp::is_member(dst);
|
|
}
|
|
|
|
struct protocol_entry {
|
|
uint8_t protocol;
|
|
protocol_handler fn;
|
|
};
|
|
static std::array<protocol_entry, 8> protocol_handlers;
|
|
static size_t protocol_handler_count = 0;
|
|
|
|
void register_protocol(uint8_t protocol, protocol_handler fn) {
|
|
if (protocol_handler_count < protocol_handlers.size())
|
|
protocol_handlers[protocol_handler_count++] = {protocol, fn};
|
|
}
|
|
|
|
void handle(std::span<const uint8_t> frame, span_writer& tx) {
|
|
parse_buffer pb(frame);
|
|
pb.consume<eth::header>();
|
|
auto* ip = pb.consume<header>();
|
|
if (!ip) return;
|
|
if ((ip->ver_ihl >> 4) != 4) return;
|
|
|
|
size_t options_len = ip->header_len() - sizeof(header);
|
|
if (options_len > 0 && !pb.skip(options_len)) return;
|
|
|
|
for (size_t i = 0; i < protocol_handler_count; i++) {
|
|
if (protocol_handlers[i].protocol == ip->protocol) {
|
|
protocol_handlers[i].fn(frame, tx);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
__attribute__((constructor))
|
|
static void register_ethertype() {
|
|
net_register_ethertype(eth::ETH_IPV4, handle);
|
|
}
|
|
|
|
} // namespace ipv4
|