From 4969720835baabda824966967cbf259fdbdd2655 Mon Sep 17 00:00:00 2001
From: Vincent Bernat <bernat@luffy.cx>
Date: Tue, 8 Jun 2010 18:39:18 +0200
Subject: [PATCH] Listen on VLAN using an appropriate BPF filter and VLAN
 decapsulation.

Three cases may happen:
 - You have a non VLAN accelerated network card.
   The BPF filter will catch the frames in the VLAN and the frame will
   be decapsulated.
 - You have a VLAN accelerated network card and a kernel < 2.6.27.
   lldpd won't see any frame encapsulated into a VLAN.
 - You have a VLAN accelerated network card and a kernel >= 2.6.27.
   lldpd will see the frames encapsulated into a VLAN as untagged
   frames and won't need to decapsulate them.
---
 CHANGELOG        |  3 +++
 README           | 10 ++++++++++
 src/interfaces.c | 12 +++++++-----
 src/lldpd.c      | 12 +++++++++---
 4 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/CHANGELOG b/CHANGELOG
index 702579fa..bb293320 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -8,6 +8,9 @@ lldpd (0.5.1)
     + Add a summary of available options in "lldpd -h" and "lldpctl -h",
       thanks to a patch from Jorge Boncompte.
     + Add a new output (keyvalue) for lldpctl.
+    + Listen on VLAN using an appropriate BPF filter, VLAN
+      decapsulation. Older "listen on vlan" feature is discarded. See
+      README for more information on the new feature.
 
   * Fixes:
     + Ignore interface with no queue. It should filter out interfaces
diff --git a/README b/README
index 27e47266..754d602f 100644
--- a/README
+++ b/README
@@ -78,6 +78,16 @@ inactive slaves. Here are the patchs (thanks to Joe Eykholt):
  http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=commit;h=cc9bd5cebc0825e0fabc0186ab85806a0891104f
  http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=commit;h=f982307f22db96201e41540295f24e8dcc10c78f
 
+Some devices (notably Cisco IOS) send frames on the native VLAN while
+they should send them untagged. If your network card does not support
+accelerated VLAN, you will receive those frames as well. However, if
+your network card handles VLAN encapsulation/decapsulation, you need a
+recent kernel to be able to receive those frames without listening on
+all available VLAN. Starting from Linux 2.6.27, lldpd is able to
+capture VLAN frames when VLAN acceleration is supported by the network
+card. Here is the patch:
+ http://git.kernel.org/?p=linux/kernel/git/torvalds/linux-2.6.git;a=commit;h=bc1d0411b804ad190cdadabac48a10067f17b9e6
+
 More information:
  http://en.wikipedia.org/wiki/LLDP
  http://standards.ieee.org/getieee802/download/802.1AB-2005.pdf
diff --git a/src/interfaces.c b/src/interfaces.c
index 3ec2646f..30cc185d 100644
--- a/src/interfaces.c
+++ b/src/interfaces.c
@@ -47,13 +47,15 @@
 /* SONMP: "ether dst 01:00:81:00:01:00" */
 /* EDP: "ether dst 00:e0:2b:00:00:00" */
 #define LLDPD_FILTER_F			\
-	{ 0x28, 0, 0, 0x0000000c },	\
-	{ 0x15, 0, 4, 0x000088cc },	\
 	{ 0x20, 0, 0, 0x00000002 },	\
-	{ 0x15, 0, 2, 0xc200000e },	\
+	{ 0x15, 0, 7, 0xc200000e },	\
 	{ 0x28, 0, 0, 0x00000000 },	\
-	{ 0x15, 11, 12, 0x00000180 },	\
-	{ 0x20, 0, 0, 0x00000002 },	\
+	{ 0x15, 0, 16, 0x00000180 },	\
+	{ 0x28, 0, 0, 0x0000000c },	\
+	{ 0x15, 13, 0, 0x000088cc },	\
+	{ 0x15, 0, 13, 0x00008100 },	\
+	{ 0x28, 0, 0, 0x00000010 },	\
+	{ 0x15, 10, 11, 0x000088cc },	\
 	{ 0x15, 0, 2, 0x2b000000 },	\
 	{ 0x28, 0, 0, 0x00000000 },	\
 	{ 0x15, 7, 8, 0x000000e0 },	\
diff --git a/src/lldpd.c b/src/lldpd.c
index d9a64228..c3607f0a 100644
--- a/src/lldpd.c
+++ b/src/lldpd.c
@@ -341,10 +341,16 @@ lldpd_decode(struct lldpd *cfg, char *frame, int s,
 	struct lldpd_port *port, *oport = NULL;
 	int guess = LLDPD_MODE_LLDP;
 
-	/* Discard VLAN frames */
-	if ((s >= sizeof(struct ethhdr)) &&
-	    (((struct ethhdr*)frame)->h_proto == htons(ETHERTYPE_VLAN)))
+	if (s < sizeof(struct ethhdr) + 4)
+		/* Too short, just discard it */
 		return;
+	/* Decapsulate VLAN frames */
+	if (((struct ethhdr*)frame)->h_proto == htons(ETHERTYPE_VLAN)) {
+		/* VLAN decapsulation means to shift 4 bytes left the frame from
+		 * offset 2*ETH_ALEN */
+		memmove(frame + 2*ETH_ALEN, frame + 2*ETH_ALEN + 4, s - 2*ETH_ALEN);
+		s -= 4;
+	}
 
 	TAILQ_FOREACH(oport, &hardware->h_rports, p_entries) {
 		if ((oport->p_lastframe != NULL) &&
-- 
2.39.2