From fd1cfc824aa780c784fbb001932b7c51842cc919 Mon Sep 17 00:00:00 2001
From: cinap_lenrek <cinap_lenrek@felloff.net>
Date: Sun, 27 Feb 2022 03:33:01 +0000
Subject: [PATCH] snoopy: add vlan protocol (802.1q)

---
 sys/src/cmd/ip/snoopy/ether.c |   9 +--
 sys/src/cmd/ip/snoopy/mkfile  |   1 +
 sys/src/cmd/ip/snoopy/vlan.c  | 104 ++++++++++++++++++++++++++++++++++
 3 files changed, 110 insertions(+), 4 deletions(-)
 create mode 100644 sys/src/cmd/ip/snoopy/vlan.c

diff --git a/sys/src/cmd/ip/snoopy/ether.c b/sys/src/cmd/ip/snoopy/ether.c
index daf51ee1d..807ab2d32 100644
--- a/sys/src/cmd/ip/snoopy/ether.c
+++ b/sys/src/cmd/ip/snoopy/ether.c
@@ -16,7 +16,7 @@ struct Hdr {
 #define	ETHERMAXTU	1514	/* maximum transmit size */
 #define ETHERHDRSIZE	14	/* size of an ethernet header */
 
-static Mux p_mux[] =
+Mux ethertypes[] =
 {
 	{"ip",		0x0800,	} ,
 	{"arp",		0x0806,	} ,
@@ -27,6 +27,7 @@ static Mux p_mux[] =
 	{"eapol",	0x888e, },
 	{"aoe",		0x88a2, } ,
 	{"cec",		0xbcbc, } ,
+	{"vlan",	0x8100, } ,
 	{0}
 };
 
@@ -57,7 +58,7 @@ p_compile(Filter *f)
 		compile_cmp(ether.name, f, p_fields);
 		return;
 	}
-	for(m = p_mux; m->name != nil; m++)
+	for(m = ethertypes; m->name != nil; m++)
 		if(strcmp(f->s, m->name) == 0){
 			f->pr = m->pr;
 			f->ulv = m->val;
@@ -106,7 +107,7 @@ p_seprint(Msg *m)
 	m->ps += ETHERHDRSIZE;
 
 	t = NetS(h->type);
-	demux(p_mux, t, t, m, &dump);
+	demux(ethertypes, t, t, m, &dump);
 
 	m->p = seprint(m->p, m->e, "s=%E d=%E pr=%4.4ux ln=%d", h->s, h->d,
 		t, len);
@@ -119,7 +120,7 @@ Proto ether =
 	p_compile,
 	p_filter,
 	p_seprint,
-	p_mux,
+	ethertypes,
 	"%#.4lux",
 	p_fields,
 	defaultframer
diff --git a/sys/src/cmd/ip/snoopy/mkfile b/sys/src/cmd/ip/snoopy/mkfile
index ab4e1b286..c4d42bb82 100644
--- a/sys/src/cmd/ip/snoopy/mkfile
+++ b/sys/src/cmd/ip/snoopy/mkfile
@@ -43,6 +43,7 @@ PROTOS=\
 	tcp\
 	ttls\
 	udp\
+	vlan\
 
 POBJS=${PROTOS:%=%.$O}
 
diff --git a/sys/src/cmd/ip/snoopy/vlan.c b/sys/src/cmd/ip/snoopy/vlan.c
new file mode 100644
index 000000000..6c3ed68d6
--- /dev/null
+++ b/sys/src/cmd/ip/snoopy/vlan.c
@@ -0,0 +1,104 @@
+#include <u.h>
+#include <libc.h>
+#include <ip.h>
+#include "dat.h"
+#include "protos.h"
+
+extern Mux ethertypes[];
+
+typedef struct Hdr	Hdr;
+struct Hdr {
+	uchar	tag[2];
+	uchar	type[2];
+};
+
+enum
+{
+	Ov,	/* vlan */
+	Oq,	/* qprio */
+	Ot,	/* type */
+};
+
+static Field p_fields[] =
+{
+	{"v",	Fnum,	Ov,	"vlan id" } ,
+	{"q",	Fnum,	Oq,	"queue prio" } ,
+	{"t",	Fnum,	Ot,	"type" } ,
+	{0}
+};
+
+static void
+p_compile(Filter *f)
+{
+	Mux *m;
+
+	if(f->op == '='){
+		compile_cmp(vlan.name, f, p_fields);
+		return;
+	}
+	for(m = ethertypes; m->name != nil; m++)
+		if(strcmp(f->s, m->name) == 0){
+			f->pr = m->pr;
+			f->ulv = m->val;
+			f->subop = Ot;
+			return;
+		}
+	sysfatal("unknown vlan field or protocol: %s", f->s);
+}
+
+static int
+p_filter(Filter *f, Msg *m)
+{
+	Hdr *h;
+
+	if(m->pe - m->ps < 4)
+		return 0;
+
+	h = (Hdr*)m->ps;
+	m->ps += 4;
+
+	switch(f->subop){
+	case Ov:
+		return (NetS(h->tag) & 0xFFF) == f->ulv;
+	case Oq:
+		return (NetS(h->tag) >> 12) == f->ulv;
+	case Ot:
+		return NetS(h->type) == f->ulv;
+	}
+	return 0;
+}
+
+static int
+p_seprint(Msg *m)
+{
+	uint v, q, t;
+	int len;
+	Hdr *h;
+
+	len = m->pe - m->ps;
+	if(len < 4)
+		return -1;
+
+	h = (Hdr*)m->ps;
+	m->ps += 4;
+
+	q = NetS(h->tag) >> 12;
+	v = NetS(h->tag) & 0xFFF;
+	t = NetS(h->type);
+	demux(ethertypes, t, t, m, &dump);
+
+	m->p = seprint(m->p, m->e, "v=%ud q=%ux pr=%4.4ux ln=%d", v, q, t, len);
+	return 0;
+}
+
+Proto vlan =
+{
+	"vlan",
+	p_compile,
+	p_filter,
+	p_seprint,
+	ethertypes,
+	"%#.4lux",
+	p_fields,
+	defaultframer
+};