Add support for B.A.T.M.A.N. Advanced

This adds support for the layer 2 mesh routing protocol
B.A.T.M.A.N. Advanced. "batadv" can be used to filter on batman-adv
packets. It also allows later filters to look at frames inside the
tunnel when both "version" and "type" are specified.

Documentation for the batman-adv protocol can be found at the following
locations:

https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/tree/Documentation/networking/batman-adv.rst
https://www.open-mesh.org/

Signed-off-by: Linus Lüssing <linus.luessing@c0d3.blue>

Author: T-X

ethertype.h

@@ -49,6 +49,9 @@
 #ifndef ETHERTYPE_TRAIL
 #define ETHERTYPE_TRAIL		0x1000
 #endif
+#ifndef ETHERTYPE_BATMAN
+#define ETHERTYPE_BATMAN	0x4305 /* B.A.T.M.A.N. Advanced */
+#endif
 #ifndef	ETHERTYPE_MOPDL
 #define ETHERTYPE_MOPDL		0x6001
 #endif

gencode.c

@@ -60,6 +60,8 @@
 #include "atmuni31.h"
 #include "sunatmpos.h"
 #include "ppp.h"
+#include "pcap/batadv_packet.h"
+#include "pcap/batadv_legacy_packet.h"
 #include "pcap/sll.h"
 #include "pcap/ipnet.h"
 #include "arcnet.h"
@@ -9483,6 +9485,169 @@ gen_geneve(compiler_state_t *cstate, bpf_u_int32 vni, int has_vni)
 	return b1;
 }
 
+struct block *
+gen_batadv_check_version(compiler_state_t *cstate, struct block *b0, bpf_u_int32 version)
+{
+	struct block *b1;
+
+	if (version > UINT8_MAX)
+		bpf_error(cstate,
+			  "batman-adv compatibility version number %u unsupported",
+			  version);
+
+	b1 = gen_cmp(cstate, OR_LINKPL, 1, BPF_B, version);
+	gen_and(b0, b1);
+
+	return b1;
+}
+
+struct block *
+gen_batadv_check_type(compiler_state_t *cstate, struct block *b0,
+		      bpf_u_int32 version, bpf_u_int32 type)
+{
+	struct block *b1;
+
+	switch (version) {
+	case 14:
+	case 15:
+		if (type > UINT8_MAX)
+			bpf_error(cstate,
+				  "batman-adv packet type %u unsupported for compatibility version %u",
+				  type, version);
+
+		b1 = gen_cmp(cstate, OR_LINKPL, 0, BPF_B, type);
+		gen_and(b0, b1);
+		b0 = b1;
+
+		break;
+	default:
+		bpf_error(cstate,
+			  "batman-adv compatibility version number %u unsupported",
+			  version);
+	}
+
+	return b0;
+}
+
+
+static void gen_batadv_push_offset(compiler_state_t *cstate, size_t offset)
+{
+	PUSH_LINKHDR(cstate, DLT_EN10MB, cstate->off_linkpl.is_variable,
+		     cstate->off_linkpl.constant_part + cstate->off_nl + offset,
+		     cstate->off_linkpl.reg);
+
+	cstate->off_linktype.constant_part += cstate->off_linkhdr.constant_part;
+	cstate->off_linkpl.constant_part += cstate->off_linkhdr.constant_part;
+
+	cstate->off_nl = 0;
+	cstate->off_nl_nosnap = 0;	/* no 802.2 LLC */
+}
+
+static void
+gen_batadv_offsets_v14(compiler_state_t *cstate, bpf_u_int32 type)
+{
+	size_t offset;
+
+	switch (type) {
+	case BATADV_LEGACY_UNICAST:		/* 0x03 */
+		offset = sizeof(struct batadv_legacy_unicast_packet);
+		break;
+	case BATADV_LEGACY_BCAST:		/* 0x04 */
+		offset = sizeof(struct batadv_legacy_bcast_packet);
+		break;
+	case BATADV_LEGACY_UNICAST_FRAG:	/* 0x06 */
+		offset = sizeof(struct batadv_legacy_unicast_frag_packet);
+		break;
+	case BATADV_LEGACY_UNICAST_4ADDR:	/* 0x09 */
+		offset = sizeof(struct batadv_legacy_unicast_4addr_packet);
+		break;
+	case BATADV_LEGACY_CODED:		/* 0x0a */
+		offset = sizeof(struct batadv_legacy_coded_packet);
+		break;
+	default:
+		offset = 0;
+	}
+
+	if (offset)
+		gen_batadv_push_offset(cstate, offset);
+}
+
+static void
+gen_batadv_offsets_v15(compiler_state_t *cstate, bpf_u_int32 type)
+{
+	size_t offset;
+
+	switch (type) {
+	case BATADV_BCAST:		/* 0x01 */
+		offset = sizeof(struct batadv_bcast_packet);
+		break;
+	case BATADV_CODED:		/* 0x02 */
+		offset = sizeof(struct batadv_coded_packet);
+		break;
+	case BATADV_UNICAST:		/* 0x40 */
+		offset = sizeof(struct batadv_unicast_packet);
+		break;
+	case BATADV_UNICAST_FRAG:	/* 0x41 */
+		offset = sizeof(struct batadv_frag_packet);
+		break;
+	case BATADV_UNICAST_4ADDR:	/* 0x42 */
+		offset = sizeof(struct batadv_unicast_4addr_packet);
+		break;
+	case BATADV_UNICAST_TVLV:
+		/* unsupported for now, needs variable offset to
+		 * take tvlv_len into account
+		 */
+		/* fall through */
+	default:
+		offset = 0;
+	}
+
+	if (offset)
+		gen_batadv_push_offset(cstate, offset);
+}
+
+static void
+gen_batadv_offsets(compiler_state_t *cstate, bpf_u_int32 version, bpf_u_int32 type)
+{
+	switch (version) {
+	case 14:
+		gen_batadv_offsets_v14(cstate, type);
+		break;
+	case 15:
+		gen_batadv_offsets_v15(cstate, type);
+		break;
+	default:
+		break;
+	}
+}
+
+struct block *
+gen_batadv(compiler_state_t *cstate, bpf_u_int32 version, int has_version,
+	   bpf_u_int32 type, int has_type)
+{
+	struct block *b0, *b1;
+	struct slist *s;
+
+	/*
+	 * Catch errors reported by us and routines below us, and return NULL
+	 * on an error.
+	 */
+	if (setjmp(cstate->top_ctx))
+		return (NULL);
+
+	b0 = gen_linktype(cstate, ETHERTYPE_BATMAN);
+
+	if (has_version)
+		b0 = gen_batadv_check_version(cstate, b0, version);
+
+	if (has_type) {
+		b0 = gen_batadv_check_type(cstate, b0, version, type);
+		gen_batadv_offsets(cstate, version, type);
+	}
+
+	return b0;
+}
+
 /* Check that the encapsulated frame has a link layer header
  * for Ethernet filters. */
 static struct block *

gencode.h

@@ -346,6 +346,9 @@ struct block *gen_pppoes(compiler_state_t *, bpf_u_int32, int);
 
 struct block *gen_geneve(compiler_state_t *, bpf_u_int32, int);
 
+struct block *gen_batadv(compiler_state_t *, bpf_u_int32, int,
+			 bpf_u_int32, int);
+
 struct block *gen_atmfield_code(compiler_state_t *, int, bpf_u_int32,
     int, int);
 struct block *gen_atmtype_abbrev(compiler_state_t *, int);

grammar.y.in

@@ -347,6 +347,7 @@ DIAG_OFF_BISON_BYACC
 %type	<i>	mtp2type
 %type	<blk>	mtp3field
 %type	<blk>	mtp3fieldvalue mtp3value mtp3listvalue
+%type	<rblk>	pbatadv
 
 
 %token  DST SRC HOST GATEWAY
@@ -365,7 +366,7 @@ DIAG_OFF_BISON_BYACC
 %token  LEN
 %token  IPV6 ICMPV6 AH ESP
 %token	VLAN MPLS
-%token	PPPOED PPPOES GENEVE
+%token	PPPOED PPPOES GENEVE BATADV
 %token  ISO ESIS CLNP ISIS L1 L2 IIH LSP SNP CSNP PSNP
 %token  STP
 %token  IPX
@@ -592,11 +593,41 @@ other:	  pqual TK_BROADCAST	{ CHECK_PTR_VAL(($$ = gen_broadcast(cstate, $1))); }
 	| PPPOES		{ CHECK_PTR_VAL(($$ = gen_pppoes(cstate, 0, 0))); }
 	| GENEVE pnum		{ CHECK_PTR_VAL(($$ = gen_geneve(cstate, $2, 1))); }
 	| GENEVE		{ CHECK_PTR_VAL(($$ = gen_geneve(cstate, 0, 0))); }
+	| BATADV pbatadv	{ $$ = $2; }
 	| pfvar			{ $$ = $1; }
 	| pqual p80211		{ $$ = $2; }
 	| pllc			{ $$ = $1; }
 	;
 
+pbatadv:		{ CHECK_PTR_VAL(($$ = gen_batadv(cstate, 0, 0, 0, 0))); }
+	| pnum		{ CHECK_PTR_VAL(($$ = gen_batadv(cstate, $1, 1, 0, 0))); }
+	| pnum pnum	{ CHECK_PTR_VAL(($$ = gen_batadv(cstate, $1, 1, $2, 1))); }
+	| pnum ID
+		{
+			int type;
+			CHECK_INT_VAL($1);
+			CHECK_PTR_VAL($2);
+
+			switch ($1) {
+			case 14:
+				type = pcap_nametobatadvtype_v14($2);
+				break;
+			case 15:
+				type = pcap_nametobatadvtype_v15($2);
+				break;
+			default:
+				YYABORT;
+			}
+
+			if (type == PROTO_UNDEF) {
+				bpf_set_error(cstate, "invalid batman-adv packet type value \"%s\"", $2);
+				YYABORT;
+			}
+
+			CHECK_PTR_VAL(($$ = gen_batadv(cstate, $1, 1, type, 1)));
+		}
+	;
+
 pfvar:	  PF_IFNAME ID		{ CHECK_PTR_VAL($2); CHECK_PTR_VAL(($$ = gen_pf_ifname(cstate, $2))); }
 	| PF_RSET ID		{ CHECK_PTR_VAL($2); CHECK_PTR_VAL(($$ = gen_pf_ruleset(cstate, $2))); }
 	| PF_RNR NUM		{ CHECK_PTR_VAL(($$ = gen_pf_rnr(cstate, $2))); }

nametoaddr.c

@@ -137,7 +137,10 @@
 #include "diag-control.h"
 
 #include "gencode.h"
+#include <pcap/batadv_packet.h>
+#include <pcap/batadv_legacy_packet.h>
 #include <pcap/namedb.h>
+
 #include "nametoaddr.h"
 
 #ifdef HAVE_OS_PROTO_H
@@ -604,6 +607,7 @@ PCAP_API_DEF struct eproto eproto_db[] = {
 	{ "moprc", ETHERTYPE_MOPRC },
 	{ "rarp", ETHERTYPE_REVARP },
 	{ "sca", ETHERTYPE_SCA },
+	{ "batadv", ETHERTYPE_BATMAN },
 	{ (char *)0, 0 }
 };
 
@@ -644,6 +648,60 @@ pcap_nametollc(const char *s)
 	return PROTO_UNDEF;
 }
 
+/* Static data base of batman-adv v14 packet type values. */
+static struct eproto batadv_type_db_v14[] = {
+	{ "iv_ogm",		BATADV_LEGACY_IV_OGM },
+	{ "icmp",		BATADV_LEGACY_ICMP },
+	{ "unicast",		BATADV_LEGACY_UNICAST },
+	{ "bcast",		BATADV_LEGACY_BCAST },
+	{ "vis",		BATADV_LEGACY_VIS },
+	{ "unicast_frag",	BATADV_LEGACY_UNICAST_FRAG },
+	{ "tt_query",		BATADV_LEGACY_TT_QUERY },
+	{ "roam_adv",		BATADV_LEGACY_ROAM_ADV },
+	{ "unicast_4addr",	BATADV_LEGACY_UNICAST_4ADDR },
+	{ "coded",		BATADV_LEGACY_CODED },
+	{ (char *)0, 0 }
+};
+
+int pcap_nametobatadvtype_v14(const char *s)
+{
+	struct eproto *p = batadv_type_db_v14;
+
+	while (p->s != 0) {
+		if (strcmp(p->s, s) == 0)
+			return p->p;
+		p += 1;
+	}
+	return PROTO_UNDEF;
+}
+
+/* Static data base of batman-adv v15 packet type values. */
+static struct eproto batadv_type_db_v15[] = {
+	{ "iv_ogm",		BATADV_IV_OGM },
+	{ "bcast",		BATADV_BCAST },
+	{ "coded",		BATADV_CODED },
+	{ "elp",		BATADV_ELP },
+	{ "ogm2",		BATADV_OGM2 },
+	{ "unicast",		BATADV_UNICAST },
+	{ "unicast_frag",	BATADV_UNICAST_FRAG },
+	{ "unicast_4addr",	BATADV_UNICAST_4ADDR },
+	{ "icmp",		BATADV_ICMP },
+	{ "unicast_tvlv",	BATADV_UNICAST_TVLV },
+	{ (char *)0, 0 }
+};
+
+int pcap_nametobatadvtype_v15(const char *s)
+{
+	struct eproto *p = batadv_type_db_v15;
+
+	while (p->s != 0) {
+		if (strcmp(p->s, s) == 0)
+			return p->p;
+		p += 1;
+	}
+	return PROTO_UNDEF;
+}
+
 /* Hex digit to 8-bit unsigned integer. */
 static inline u_char
 xdtoi(u_char c)