107 lines
2.9 KiB
C++
107 lines
2.9 KiB
C++
#include "igmp.h"
|
|
#include <vector>
|
|
#include "ipv4.h"
|
|
#include "net.h"
|
|
#include "parse_buffer.h"
|
|
#include "prepend_buffer.h"
|
|
|
|
namespace igmp {
|
|
|
|
struct group_entry {
|
|
ipv4::ip4_addr ip;
|
|
eth::mac_addr mac;
|
|
};
|
|
|
|
static std::vector<group_entry> groups;
|
|
|
|
eth::mac_addr mac_for_ip(const ipv4::ip4_addr& group) {
|
|
return {0x01, 0x00, 0x5E,
|
|
static_cast<uint8_t>(group[1] & 0x7F), group[2], group[3]};
|
|
}
|
|
|
|
bool is_member(const ipv4::ip4_addr& ip) {
|
|
if (ip == ALL_HOSTS) return true;
|
|
for (auto& g : groups)
|
|
if (g.ip == ip) return true;
|
|
return false;
|
|
}
|
|
|
|
bool is_member_mac(const eth::mac_addr& mac) {
|
|
static constexpr eth::mac_addr ALL_HOSTS_MAC = {0x01, 0x00, 0x5E, 0x00, 0x00, 0x01};
|
|
if (mac == ALL_HOSTS_MAC) return true;
|
|
for (auto& g : groups)
|
|
if (g.mac == mac) return true;
|
|
return false;
|
|
}
|
|
|
|
static void send_report(const ipv4::ip4_addr& group,
|
|
eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
|
|
prepend_buffer<4096> buf;
|
|
prepend_report(buf, our_mac, our_ip, group);
|
|
net_send_raw(buf.span());
|
|
}
|
|
|
|
void join(const ipv4::ip4_addr& group,
|
|
eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
|
|
for (auto& g : groups)
|
|
if (g.ip == group) return;
|
|
groups.push_back({group, mac_for_ip(group)});
|
|
send_report(group, our_mac, our_ip);
|
|
}
|
|
|
|
void send_all_reports(eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
|
|
for (auto& g : groups)
|
|
send_report(g.ip, our_mac, our_ip);
|
|
}
|
|
|
|
void handle(std::span<const uint8_t> frame, span_writer& tx,
|
|
eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
|
|
parse_buffer pb(frame);
|
|
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* msg = pb.consume<message>();
|
|
if (!msg) return;
|
|
if (msg->type != 0x11) return;
|
|
|
|
if (msg->group == ipv4::ip4_addr{0, 0, 0, 0}) {
|
|
for (auto& g : groups)
|
|
send_report(g.ip, our_mac, our_ip);
|
|
} else {
|
|
for (auto& g : groups) {
|
|
if (g.ip == msg->group) {
|
|
send_report(g.ip, our_mac, our_ip);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool parse_report(std::span<const uint8_t> frame, ipv4::ip4_addr& group) {
|
|
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 != 2) return false;
|
|
|
|
size_t options_len = ip->header_len() - sizeof(ipv4::header);
|
|
if (options_len > 0 && !pb.skip(options_len)) return false;
|
|
|
|
auto* msg = pb.consume<message>();
|
|
if (!msg) return false;
|
|
if (msg->type != 0x16) return false;
|
|
|
|
group = msg->group;
|
|
return true;
|
|
}
|
|
|
|
} // namespace igmp
|