From: Vincent Bernat <bernat@luffy.cx>
Date: Sun, 19 Oct 2008 01:49:15 +0000 (+0200)
Subject: Sometimes, CDP are transmitted over administration VLAN. We try to
X-Git-Tag: 0.2~35
X-Git-Url: http://git.ipfire.org/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=dc1539a44534a36fa8683ecfaddbbd6b34466b96;p=thirdparty%2Flldpd.git

Sometimes, CDP are transmitted over administration VLAN. We try to
handle this case by handling this additional encapsulation on
reception of CDP frame. Frames are still sent outside of the VLAN.
---

diff --git a/src/cdp.c b/src/cdp.c
index d2fb90e3..bd2d6fae 100644
--- a/src/cdp.c
+++ b/src/cdp.c
@@ -48,10 +48,10 @@ cdp_send(struct lldpd *global, struct lldpd_chassis *chassis,
 	    sizeof(llc.ether.shost));
 	memcpy(&llc.ether.dhost, &mcastaddr,
 	    sizeof(llc.ether.dhost));
-	llc.dsap = llc.ssap = 0xaa;
-	llc.control = 0x03;
-	memcpy(llc.org, llcorg, sizeof(llc.org));
-	llc.protoid = htons(LLC_PID_CDP);
+	llc.llc.dsap = llc.llc.ssap = 0xaa;
+	llc.llc.control = 0x03;
+	memcpy(llc.llc.org, llcorg, sizeof(llc.llc.org));
+	llc.llc.protoid = htons(LLC_PID_CDP);
 	IOV_NEW;
 	iov[c].iov_base = &llc;
 	iov[c].iov_len = sizeof(llc);
@@ -172,7 +172,8 @@ cdp_decode(struct lldpd *cfg, char *frame, int s,
 {
 	struct lldpd_chassis *chassis;
 	struct lldpd_port *port;
-	struct ethllc *llc;
+	struct ieee8023 *ether;
+	struct llc *llc;
 	struct cdp_header *ch;
 	struct cdp_tlv_head *tlv;
 	struct cdp_tlv_address_head *ah;
@@ -195,21 +196,46 @@ cdp_decode(struct lldpd *cfg, char *frame, int s,
 	}
 	TAILQ_INIT(&port->p_vlans);
 
-	if (s < sizeof(struct ethllc) + sizeof(struct cdp_header)) {
+	if (s < sizeof(struct ieee8023)) {
 		LLOG_WARNX("too short frame received on %s", hardware->h_ifname);
 		goto malformed;
 	}
 
-	llc = (struct ethllc *)frame;
-	if (memcmp(&llc->ether.dhost, cdpaddr, sizeof(cdpaddr)) != 0) {
+	ether = (struct ieee8023*)frame;
+	if (memcmp(&ether->dhost, cdpaddr, sizeof(cdpaddr)) != 0) {
 		LLOG_INFO("frame not targeted at CDP multicast address received on %s",
 		    hardware->h_ifname);
 		goto malformed;
 	}
-	if (ntohs(llc->ether.size) > s - sizeof(struct ieee8023)) {
-		LLOG_WARNX("incorrect 802.3 frame size reported on %s",
-		    hardware->h_ifname);
-		goto malformed;
+
+	/* Is it a CDP frame encapsulated into a VLAN? */
+	if (ether->size == htons(ETHERTYPE_VLAN)) {
+		if (s < sizeof(struct ieee8023) + sizeof(struct ieee8021q) +
+		    sizeof(struct llc) + sizeof(struct cdp_header)) {
+			LLOG_WARNX("too short frame received on %s", hardware->h_ifname);
+			goto malformed;
+		}
+		if (ntohs(((struct ieee8021q*)(frame +
+				sizeof(struct ieee8023)))->size) >
+		    s - sizeof(struct ieee8023) - sizeof(struct ieee8021q)) {
+			LLOG_WARNX("incorrect 802.3/802.1q frame size reported on %s",
+			    hardware->h_ifname);
+			goto malformed;
+		}
+		llc = (struct llc*)(frame + sizeof(struct ieee8023) +
+		    sizeof(struct ieee8021q));
+	} else {
+		if (s < sizeof(struct ethllc) + sizeof(struct cdp_header)) {
+			LLOG_WARNX("too short frame received on %s", hardware->h_ifname);
+			goto malformed;
+		}
+
+		if (ntohs(ether->size) > s - sizeof(struct ieee8023)) {
+			LLOG_WARNX("incorrect 802.3 frame size reported on %s",
+			    hardware->h_ifname);
+			goto malformed;
+		}
+		llc = (struct llc*)(frame + sizeof(struct ieee8023));
 	}
 	if (llc->protoid != htons(LLC_PID_CDP)) {
 		if ((llc->protoid != htons(LLC_PID_DRIP)) &&
@@ -223,7 +249,7 @@ cdp_decode(struct lldpd *cfg, char *frame, int s,
 			    hardware->h_ifname);
 		goto malformed;
 	}
-	f = sizeof(struct ethllc);
+	f = (void*)llc - (void*)frame + sizeof(struct llc);
 	ch = (struct cdp_header *)(frame + f);
 	if ((ch->version != 1) && (ch->version != 2)) {
 		LLOG_WARNX("incorrect CDP version (%d) for frame received on %s",
@@ -327,7 +353,7 @@ cdp_decode(struct lldpd *cfg, char *frame, int s,
 				LLOG_WARN("unable to allocate memory for port ID");
 				goto malformed;
 			}
-			memcpy(port->p_id, llc->ether.shost, ETH_ALEN);
+			memcpy(port->p_id, ether->shost, ETH_ALEN);
 			port->p_id_len = ETH_ALEN;
 			f += len;
 			break;
@@ -440,11 +466,25 @@ cdp_guess(char *frame, int len, int version)
 {
 	const u_int8_t mcastaddr[] = CDP_MULTICAST_ADDR;
 	struct cdp_header *ch;
-	if (len < sizeof(struct ethllc) + sizeof(struct cdp_header))
+	if (len < sizeof(struct ieee8023))
 		return 0;
 	if (memcmp(frame, mcastaddr, ETH_ALEN) != 0)
 		return 0;
-	ch = (struct cdp_header *)(frame + sizeof(struct ethllc));
+	/* Maybe this is encapsulated into a VLAN */
+	if (((struct ieee8023*)frame)->size != htons(ETHERTYPE_VLAN)) {
+		/* Not a 802.1q frame */
+		if (len < sizeof(struct ethllc) +
+		    sizeof(struct cdp_header))
+			return 0;
+		ch = (struct cdp_header *)(frame + sizeof(struct ethllc));
+	} else {
+		/* 802.1q frame */
+		if (len < sizeof(struct ieee8023) + sizeof(struct ieee8021q) +
+		    sizeof(struct llc) + sizeof(struct cdp_header))
+			return 0;
+		ch = (struct cdp_header *)(frame + sizeof(struct ieee8023) +
+		    sizeof(struct ieee8021q) + sizeof(struct llc));
+	}
 	return (ch->version == version);
 }
 
diff --git a/src/edp.c b/src/edp.c
index 0275074d..ecc8546d 100644
--- a/src/edp.c
+++ b/src/edp.c
@@ -60,10 +60,10 @@ edp_send(struct lldpd *global, struct lldpd_chassis *chassis,
 		    sizeof(llc.ether.shost));
 		memcpy(&llc.ether.dhost, &mcastaddr,
 		    sizeof(llc.ether.dhost));
-		llc.dsap = llc.ssap = 0xaa;
-		llc.control = 0x03;
-		memcpy(llc.org, llcorg, sizeof(llc.org));
-		llc.protoid = htons(LLC_PID_EDP);
+		llc.llc.dsap = llc.llc.ssap = 0xaa;
+		llc.llc.control = 0x03;
+		memcpy(llc.llc.org, llcorg, sizeof(llc.llc.org));
+		llc.llc.protoid = htons(LLC_PID_EDP);
 		IOV_NEW;
 		iov[c].iov_base = &llc;
 		iov[c].iov_len = sizeof(llc);
@@ -241,7 +241,7 @@ edp_decode(struct lldpd *cfg, char *frame, int s,
 		    hardware->h_ifname);
 		goto malformed;
 	}
-	if (llc->protoid != htons(LLC_PID_EDP)) {
+	if (llc->llc.protoid != htons(LLC_PID_EDP)) {
 		LLOG_DEBUG("incorrect LLC protocol ID received on %s",
 		    hardware->h_ifname);
 		goto malformed;
diff --git a/src/llc.h b/src/llc.h
index 002ccf18..0ca0651f 100644
--- a/src/llc.h
+++ b/src/llc.h
@@ -23,8 +23,12 @@ struct ieee8023 {
 	u_int16_t size;	        /* packet type ID field	*/
 } __attribute__ ((__packed__));
 
-struct ethllc {
-	struct ieee8023 ether;
+struct ieee8021q {
+	u_int16_t vid;
+	u_int16_t size;
+} __attribute__ ((__packed__));
+
+struct llc {
 	u_int8_t  dsap;		/* destination SAP */
 	u_int8_t  ssap;		/* source SAP */
 	u_int8_t  control;		/* LLC control field */
@@ -32,4 +36,10 @@ struct ethllc {
 	u_int16_t protoid;
 } __attribute__ ((__packed__));
 
+/* IEEE 802.3 + LLC */
+struct ethllc {
+	struct ieee8023 ether;
+	struct llc llc;
+} __attribute__ ((__packed__));
+
 #endif
diff --git a/src/lldpd.c b/src/lldpd.c
index e4161b5f..c4e48fdd 100644
--- a/src/lldpd.c
+++ b/src/lldpd.c
@@ -64,14 +64,20 @@ void			 lldpd_iface_multicast(struct lldpd *, const char *, int);
         { 0x6, 0, 0, 0x0000ffff },					\
         { 0x6, 0, 0, 0x00000000 },
 struct sock_filter lldpd_filter_lldp_f[] = { LLDPD_FILTER_LLDP_F };
-/* "ether dst 01:00:0c:cc:cc:cc" */
+/* "(ether dst 01:00:0c:cc:cc:cc) or (vlan and ether dst 01:00:0c:cc:cc:cc)" */
 #define LLDPD_FILTER_CDP_F			\
-        { 0x20, 0, 0, 0x00000002 },		\
-        { 0x15, 0, 3, 0x0ccccccc },		\
-        { 0x28, 0, 0, 0x00000000 },		\
-        { 0x15, 0, 1, 0x00000100 },		\
-        { 0x6, 0, 0, 0x0000ffff },		\
-        { 0x6, 0, 0, 0x00000000 },
+	{ 0x20, 0, 0, 0x00000002 },             \
+	{ 0x15, 0, 2, 0x0ccccccc },		\
+	{ 0x28, 0, 0, 0x00000000 },		\
+	{ 0x15, 6, 0, 0x00000100 },		\
+	{ 0x28, 0, 0, 0x0000000c },		\
+	{ 0x15, 0, 5, 0x00008100 },		\
+	{ 0x20, 0, 0, 0x00000002 },		\
+	{ 0x15, 0, 3, 0x0ccccccc },		\
+	{ 0x28, 0, 0, 0x00000000 },		\
+	{ 0x15, 0, 1, 0x00000100 },		\
+	{ 0x6, 0, 0, 0x0000ffff },		\
+	{ 0x6, 0, 0, 0x00000000 },
 struct sock_filter lldpd_filter_cdp_f[] = { LLDPD_FILTER_CDP_F };
 /* "ether dst 01:00:81:00:01:00" */
 #define LLDPD_FILTER_SONMP_F			\
diff --git a/src/sonmp.c b/src/sonmp.c
index 19067c70..19f9caa4 100644
--- a/src/sonmp.c
+++ b/src/sonmp.c
@@ -190,10 +190,10 @@ sonmp_send(struct lldpd *global, struct lldpd_chassis *chassis,
 	    sizeof(frame.llc.ether.dhost));
 	frame.llc.ether.size = htons(sizeof(struct sonmp) -
 	    sizeof(struct ieee8023));
-	frame.llc.dsap = frame.llc.ssap = 0xaa;
-	frame.llc.control = 0x03;
-	memcpy(frame.llc.org, llcorg, sizeof(frame.llc.org));
-	frame.llc.protoid = htons(LLC_PID_SONMP_HELLO);
+	frame.llc.llc.dsap = frame.llc.llc.ssap = 0xaa;
+	frame.llc.llc.control = 0x03;
+	memcpy(frame.llc.llc.org, llcorg, sizeof(frame.llc.llc.org));
+	frame.llc.llc.protoid = htons(LLC_PID_SONMP_HELLO);
 	memcpy(&frame.addr, &chassis->c_mgmt, sizeof(struct in_addr));
 	frame.seg[2] = if_nametoindex(hardware->h_ifname);
 	frame.chassis = 1;	/* Other */
@@ -208,7 +208,7 @@ sonmp_send(struct lldpd *global, struct lldpd_chassis *chassis,
 		return ENETDOWN;
 	}
 
-	frame.llc.protoid = htons(LLC_PID_SONMP_FLATNET);
+	frame.llc.llc.protoid = htons(LLC_PID_SONMP_FLATNET);
 	frame.llc.ether.dhost[ETH_ALEN-1] = 1;
 
 	if (write((hardware->h_raw_real > 0) ? hardware->h_raw_real :
@@ -255,7 +255,7 @@ sonmp_decode(struct lldpd *cfg, char *frame, int s,
 		 * them. */
 		goto malformed;
 	}
-	if (f->llc.protoid != htons(LLC_PID_SONMP_HELLO)) {
+	if (f->llc.llc.protoid != htons(LLC_PID_SONMP_HELLO)) {
 		LLOG_DEBUG("incorrect LLC protocol ID received on %s",
 		    hardware->h_ifname);
 		goto malformed;