From 99ddcc4f4e5f2ed4bd47f14a960039b06f01d67f Mon Sep 17 00:00:00 2001 From: Jan-Philipp Litza Date: Sun, 3 Apr 2016 20:15:39 +0200 Subject: [PATCH] Add package gluon-radv-priorityd This package tries to prioritize the router advertisements of the gateway selected by the B.A.T.M.A.N. advanced gateway selection. It does this by inserting rules into the firewall to hand all router advertisements via the NFQUEUE mechanism to a userspace daemon, which then examines them and changes the preference field to "high" if appropriate. --- package/gluon-radv-priorityd/Makefile | 41 ++++ package/gluon-radv-priorityd/README.md | 8 + .../files/etc/init.d/gluon-radv-priorityd | 17 ++ .../lib/gluon/radv-priorityd/ip6tables.rules | 6 + .../lib/gluon/upgrade/400-radv-priorityd | 15 ++ package/gluon-radv-priorityd/src/Makefile | 8 + .../src/gluon-radv-priorityd.c | 199 ++++++++++++++++++ 7 files changed, 294 insertions(+) create mode 100644 package/gluon-radv-priorityd/Makefile create mode 100644 package/gluon-radv-priorityd/README.md create mode 100755 package/gluon-radv-priorityd/files/etc/init.d/gluon-radv-priorityd create mode 100644 package/gluon-radv-priorityd/files/lib/gluon/radv-priorityd/ip6tables.rules create mode 100755 package/gluon-radv-priorityd/files/lib/gluon/upgrade/400-radv-priorityd create mode 100644 package/gluon-radv-priorityd/src/Makefile create mode 100644 package/gluon-radv-priorityd/src/gluon-radv-priorityd.c diff --git a/package/gluon-radv-priorityd/Makefile b/package/gluon-radv-priorityd/Makefile new file mode 100644 index 00000000..676d9b3b --- /dev/null +++ b/package/gluon-radv-priorityd/Makefile @@ -0,0 +1,41 @@ +include $(TOPDIR)/rules.mk + +PKG_NAME:=gluon-radv-priorityd +PKG_VERSION:=1 +PKG_RELEASE:=1 + +PKG_BUILD_DIR := $(BUILD_DIR)/$(PKG_NAME) + +include $(INCLUDE_DIR)/package.mk + +define Package/gluon-radv-priorityd + SECTION:=gluon + CATEGORY:=Gluon + TITLE:=Prioritize router advertisements + DEPENDS:=+gluon-core +kmod-ipt-nfqueue +libnetfilter-queue +kmod-ipt-extra +iptables-mod-nfqueue +endef + +define Package/gluon-radv-priorityd/description + Gluon community wifi mesh firmware framework: prioritize router advertisements +endef + +define Build/Prepare + mkdir -p $(PKG_BUILD_DIR) + $(CP) ./src/* $(PKG_BUILD_DIR)/ +endef + +define Build/Configure +endef + +define Build/Compile + CFLAGS="$(TARGET_CFLAGS)" CPPFLAGS="$(TARGET_CPPFLAGS)" $(MAKE) -C $(PKG_BUILD_DIR) $(TARGET_CONFIGURE_OPTS) +endef + +define Package/gluon-radv-priorityd/install + $(CP) ./files/* $(1)/ + + $(INSTALL_DIR) $(1)/usr/sbin/ + $(INSTALL_BIN) $(PKG_BUILD_DIR)/gluon-radv-priorityd $(1)/usr/sbin/ +endef + +$(eval $(call BuildPackage,gluon-radv-priorityd)) diff --git a/package/gluon-radv-priorityd/README.md b/package/gluon-radv-priorityd/README.md new file mode 100644 index 00000000..50d24c13 --- /dev/null +++ b/package/gluon-radv-priorityd/README.md @@ -0,0 +1,8 @@ +gluon-radv-priorityd +==================== + +This package tries to prioritize the router advertisements of the gateway +selected by the B.A.T.M.A.N. advanced gateway selection. It does this by +inserting rules into the firewall to hand all router advertisements via the +NFQUEUE mechanism to a userspace daemon, which then examines them and changes +the preference field to "high" if appropriate. diff --git a/package/gluon-radv-priorityd/files/etc/init.d/gluon-radv-priorityd b/package/gluon-radv-priorityd/files/etc/init.d/gluon-radv-priorityd new file mode 100755 index 00000000..284554fe --- /dev/null +++ b/package/gluon-radv-priorityd/files/etc/init.d/gluon-radv-priorityd @@ -0,0 +1,17 @@ +#!/bin/sh /etc/rc.common + +START=50 + +SERVICE_WRITE_PID=1 +SERVICE_DAEMONIZE=1 + +DAEMON=/usr/sbin/gluon-radv-priorityd + + +start() { + service_start $DAEMON +} + +stop() { + service_stop $DAEMON +} diff --git a/package/gluon-radv-priorityd/files/lib/gluon/radv-priorityd/ip6tables.rules b/package/gluon-radv-priorityd/files/lib/gluon/radv-priorityd/ip6tables.rules new file mode 100644 index 00000000..27465dc7 --- /dev/null +++ b/package/gluon-radv-priorityd/files/lib/gluon/radv-priorityd/ip6tables.rules @@ -0,0 +1,6 @@ +*raw +-I PREROUTING -m physdev --physdev-is-in -p icmpv6 --icmpv6-type router-advertisement -j NFQUEUE --queue-num 0 --queue-bypass +COMMIT +*filter +-I FORWARD -m physdev --physdev-is-bridged -j ACCEPT +COMMIT diff --git a/package/gluon-radv-priorityd/files/lib/gluon/upgrade/400-radv-priorityd b/package/gluon-radv-priorityd/files/lib/gluon/upgrade/400-radv-priorityd new file mode 100755 index 00000000..ecbd0f87 --- /dev/null +++ b/package/gluon-radv-priorityd/files/lib/gluon/upgrade/400-radv-priorityd @@ -0,0 +1,15 @@ +#!/usr/bin/lua + +local sysctl = require 'gluon.sysctl' +local uci = require('luci.model.uci').cursor() + +uci:section('firewall', 'include', 'radv_priorityd', + { + type = 'restore', + path = '/lib/gluon/radv-priorityd/ip6tables.rules', + family = 'ipv6', + } +) +uci:save('firewall') + +sysctl.set('net.bridge.bridge-nf-call-ip6tables', 1) diff --git a/package/gluon-radv-priorityd/src/Makefile b/package/gluon-radv-priorityd/src/Makefile new file mode 100644 index 00000000..ce317b9c --- /dev/null +++ b/package/gluon-radv-priorityd/src/Makefile @@ -0,0 +1,8 @@ +all: gluon-radv-priorityd + +LDLIBS += $(shell pkg-config --libs libnetfilter_queue) + +gluon-radv-priorityd: gluon-radv-priorityd.c + +clean: + rm gluon-radv-priorityd diff --git a/package/gluon-radv-priorityd/src/gluon-radv-priorityd.c b/package/gluon-radv-priorityd/src/gluon-radv-priorityd.c new file mode 100644 index 00000000..240f4235 --- /dev/null +++ b/package/gluon-radv-priorityd/src/gluon-radv-priorityd.c @@ -0,0 +1,199 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#define QUEUE_NUMBER 0 +#define HWADDR_SIZE 6 +#define BUFSIZE 1500 +#define BATADV_GW_FILE "/sys/kernel/debug/batman_adv/bat0/gateways" +#define BATADV_REFRESH_INTERVAL 60 + +#ifdef DEBUG +#define ASSERT(stmt) \ + if(!(stmt)) { \ + fprintf(stderr, "Assertion failed: " #stmt "\n"); \ + goto fail; \ + } +#else +#define ASSERT(stmt) if(!(stmt)) goto fail; +#endif + +struct ip6_pseudo_hdr { + struct in6_addr src; + struct in6_addr dst; + uint32_t plen; + uint8_t pad[3]; + uint8_t nxt; +}; + +// cf. RFC 1071 section 4.1 +uint16_t checksum(void *buf, int count, uint16_t start) { + register uint32_t sum = start; + uint16_t *addr = buf; + + while (count > 1) { + sum += *addr++; + count -= 2; + } + + if (count > 0) + sum += htons(*(uint8_t *)addr); + + while (sum >> 16) + sum = (sum & 0xffff) + (sum >> 16); + + return ~sum; +} + +bool is_chosen_gateway(uint8_t *hw_addr) { + static uint8_t gateway[HWADDR_SIZE]; + static time_t t; + + if (time(NULL) - t >= BATADV_REFRESH_INTERVAL) { + FILE *f = fopen(BATADV_GW_FILE, "r"); + if (!f) + return false; + + char *line = NULL; + size_t len = 0; + + while (getline(&line, &len, f) >= 0) { + if (HWADDR_SIZE == sscanf(line, "=> %hhx:%hhx:%hhx:%hhx:%hhx:%hhx", + &gateway[0], + &gateway[1], + &gateway[2], + &gateway[3], + &gateway[4], + &gateway[5])) { + t = time(NULL); + break; + } + } + free(line); + fclose(f); + } + + return memcmp(gateway, hw_addr, HWADDR_SIZE) == 0; +} + +int process_packet(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg, struct nfq_data *nfad, void *data) { + int len; + struct nfqnl_msg_packet_hdr *pkt_hdr; + struct ip6_pseudo_hdr phdr = {}; + struct ip6_hdr *pkt; + struct ip6_ext *ext; + struct nd_router_advert *ra; + struct nfqnl_msg_packet_hw *source; + uint8_t ext_type; + uint16_t hdrchksum; + + pkt_hdr = nfq_get_msg_packet_hdr(nfad); + + source = nfq_get_packet_hw(nfad); + ASSERT(is_chosen_gateway(source->hw_addr)); + + len = nfq_get_payload(nfad, (unsigned char**)&pkt); + ASSERT(len > sizeof(struct ip6_hdr)); + ASSERT(len >= ntohs(pkt->ip6_plen) + sizeof(struct ip6_hdr)); + + ext_type = pkt->ip6_nxt; + ext = (void*)pkt + sizeof(struct ip6_hdr); + while (ext_type != IPPROTO_ICMPV6) { + ASSERT((void*)ext < (void*)pkt + sizeof(struct ip6_hdr) + len); + ext_type = ext->ip6e_nxt; + ext = (void*)ext + ext->ip6e_len; + } + ra = (struct nd_router_advert*)ext; + ASSERT(len >= (void*)ra - (void*)pkt + sizeof(struct nd_router_advert)); + ASSERT(ra->nd_ra_type == ND_ROUTER_ADVERT); + ASSERT(ra->nd_ra_code == 0); + ASSERT((ra->nd_ra_flags_reserved & (0x03 << 3)) == 0x00); + + phdr.nxt = IPPROTO_ICMPV6; + // original plen - length of IPv6 extension headers + phdr.plen = htonl(ntohs(pkt->ip6_plen) - ((void*)ra - (void*)pkt - sizeof(struct ip6_hdr))); + memcpy(&phdr.src, &pkt->ip6_src, sizeof(struct in6_addr)); + memcpy(&phdr.dst, &pkt->ip6_dst, sizeof(struct in6_addr)); + hdrchksum = ~checksum(&phdr, sizeof(struct ip6_pseudo_hdr), 0); + + ASSERT(checksum(ra, phdr.plen, hdrchksum) == 0); + + // Validation complete, set preference to high + ra->nd_ra_flags_reserved |= (0x01 << 3); + + ra->nd_ra_cksum = 0; + ra->nd_ra_cksum = htons(checksum(ra, phdr.plen, hdrchksum)); + +#ifdef DEBUG + printf("pkt modified\n"); +#endif + + nfq_set_verdict(qh, pkt_hdr->packet_id, NF_ACCEPT, len, (unsigned char*) pkt); + return 0; + + fail: + // accept packet in any case, but don't copy data around + nfq_set_verdict(qh, pkt_hdr->packet_id, NF_ACCEPT, 0, NULL); + return 1; +} + +int main(int argc, char* argv[]) { + int fd, rv, ret = 0; + char buf[BUFSIZE]; + struct nfq_q_handle *qh; + struct nfq_handle *h; + + h = nfq_open(); + if (!h) { + perror("nfq_open()"); + goto fail; + } + + if (nfq_unbind_pf(h, AF_INET6) < 0 ) { + perror("nfq_unbind_pf()"); + fprintf(stderr, "Probably we are missing CAP_NET_ADMIN\n"); + goto fail; + } + + if (nfq_bind_pf(h, AF_INET6) < 0) { + perror("nfq_bind_pf()"); + goto fail; + } + + qh = nfq_create_queue(h, QUEUE_NUMBER, &process_packet, NULL); + if (!qh) { + perror("nfq_create_queue()\n"); + goto fail; + } + + if (nfq_set_mode(qh, NFQNL_COPY_PACKET, BUFSIZE) < 0) { + perror("nfq_set_mode()\n"); + goto fail; + } + + fd = nfq_fd(h); + while ((rv = recv(fd, buf, sizeof(buf), 0)) >= 0) { +#ifdef DEBUG + printf("pkt received\n"); +#endif + nfq_handle_packet(h, buf, rv); + } + + goto cleanup; + fail: + ret = 1; + cleanup: + nfq_close(h); + return ret; +}