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

@@ -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))

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.

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
+}

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

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)

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

package/gluon-radv-priorityd/src/gluon-radv-priorityd.c

@@ -0,0 +1,199 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+
+#include <netinet/in.h>
+#include <netinet/ip6.h>
+#include <netinet/icmp6.h>
+
+#include <linux/netfilter.h>
+
+#include <libnfnetlink/libnfnetlink.h>
+#include <libnetfilter_queue/libnetfilter_queue.h>
+
+#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;
+}