[lldp] Add support for the Link Layer Discovery Protocollldp

Add support for recording LLDP packets and exposing TLV values via the
settings mechanism.  LLDP settings are encoded as

  ${netX.lldp/<prefix>.<type>.<index>.<offset>.<length>}

where

  <type> is the TLV type

  <offset> is the starting offset within the TLV value

  <length> is the length (or zero to read the from <offset> to the end)

  <prefix>, if it has a non-zero value, is the subtype byte string of
  length <offset> to match at the start of the TLV value, up to a
  maximum matched length of 4 bytes

  <index> is the index of the entry matching <type> and <prefix> to be
  accessed, with zero indicating the first matching entry

The <prefix> is designed to accommodate both matching of the OUI
within an organization-specific TLV (e.g. 0x0080c2 for IEEE 802.1
TLVs) and of a subtype byte as found within many TLVs.

This encoding allows most LLDP values to be extracted easily.  For
example

  System name: ${netX.lldp/5.0.0.0:string}

  System description: ${netX.lldp/6.0.0.0:string}

  Port description: ${netX.lldp/4.0.0.0:string}

  Port interface name: ${netX.lldp/5.2.0.1.0:string}

  Chassis MAC address: ${netX.lldp/4.1.0.1.0:hex}

  Management IPv4 address: ${netX.lldp/5.1.8.0.2.4:ipv4}

  Port VLAN ID: ${netX.lldp/0x0080c2.1.127.0.4.2:int16}

  Port VLAN name: ${netX.lldp/0x0080c2.3.127.0.7.0:string}

  Maximum frame size: ${netX.lldp/0x00120f.4.127.0.4.2:uint16}

Originally-implemented-by: Marin Hannache <git@mareo.fr>
Signed-off-by: Michael Brown <mcb30@ipxe.org>

6 files changed, 443 insertions, 0 deletions

diff --git a/src/config/config_ethernet.c b/src/config/config_ethernet.c
index 8a663c92..c1b35bfe 100644
--- a/src/config/config_ethernet.c
+++ b/src/config/config_ethernet.c
@@ -49,3 +49,6 @@ REQUIRE_OBJECT ( eth_slow );
 #ifdef NET_PROTO_EAPOL
 REQUIRE_OBJECT ( eapol );
 #endif
+#ifdef NET_PROTO_LLDP
+REQUIRE_OBJECT ( lldp );
+#endif
diff --git a/src/config/general.h b/src/config/general.h
index 2d15f500..e9ceaff5 100644
--- a/src/config/general.h
+++ b/src/config/general.h
@@ -40,6 +40,7 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #define	NET_PROTO_STP		/* Spanning Tree protocol */
 #define	NET_PROTO_LACP		/* Link Aggregation control protocol */
 #define	NET_PROTO_EAPOL		/* EAP over LAN protocol */
+#undef	NET_PROTO_LLDP		/* Link Layer Discovery protocol */
 
 /*
  * PXE support
diff --git a/src/include/ipxe/errfile.h b/src/include/ipxe/errfile.h
index 7c3b0c43..d7b6ea1b 100644
--- a/src/include/ipxe/errfile.h
+++ b/src/include/ipxe/errfile.h
@@ -295,6 +295,7 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #define ERRFILE_ntp			( ERRFILE_NET | 0x00490000 )
 #define ERRFILE_httpntlm		( ERRFILE_NET | 0x004a0000 )
 #define ERRFILE_eap			( ERRFILE_NET | 0x004b0000 )
+#define ERRFILE_lldp			( ERRFILE_NET | 0x004c0000 )
 
 #define ERRFILE_image		      ( ERRFILE_IMAGE | 0x00000000 )
 #define ERRFILE_elf		      ( ERRFILE_IMAGE | 0x00010000 )
diff --git a/src/include/ipxe/if_ether.h b/src/include/ipxe/if_ether.h
index 58d91b97..c1168b10 100644
--- a/src/include/ipxe/if_ether.h
+++ b/src/include/ipxe/if_ether.h
@@ -23,6 +23,7 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #define ETH_P_SLOW	0x8809	/* Ethernet slow protocols */
 #define ETH_P_EAPOL	0x888E	/* 802.1X EAP over LANs */
 #define ETH_P_AOE	0x88A2	/* ATA over Ethernet */
+#define ETH_P_LLDP	0x88CC	/* Link Layer Discovery Protocol */
 #define ETH_P_FCOE	0x8906	/* Fibre Channel over Ethernet */
 #define ETH_P_FIP	0x8914	/* FCoE Initialization Protocol */
 
diff --git a/src/include/ipxe/lldp.h b/src/include/ipxe/lldp.h
new file mode 100644
index 00000000..9951d3b8
--- /dev/null
+++ b/src/include/ipxe/lldp.h
@@ -0,0 +1,97 @@
+#ifndef _IPXE_LLDP_H
+#define _IPXE_LLDP_H
+
+/** @file
+ *
+ * Link Layer Discovery Protocol
+ *
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+#include <stdint.h>
+
+/** An LLDP TLV header */
+struct lldp_tlv {
+	/** Type and length */
+	uint16_t type_len;
+	/** Data */
+	uint8_t data[0];
+} __attribute__ (( packed ));
+
+/**
+ * Extract LLDP TLV type
+ *
+ * @v type_len		Type and length
+ * @ret type		Type
+ */
+#define LLDP_TLV_TYPE( type_len ) ( (type_len) >> 9 )
+
+/**
+ * Extract LLDP TLV length
+ *
+ * @v type_len		Type and length
+ * @ret len		Length
+ */
+#define LLDP_TLV_LEN( type_len ) ( (type_len) & 0x01ff )
+
+/** End of LLDP data unit */
+#define LLDP_TYPE_END 0x00
+
+/** LLDP settings block name */
+#define LLDP_SETTINGS_NAME "lldp"
+
+/**
+ * Construct LLDP setting tag
+ *
+ * LLDP settings are encoded as
+ *
+ *   ${netX.lldp/<prefix>.<type>.<index>.<offset>.<length>}
+ *
+ * where
+ *
+ *   <type> is the TLV type
+ *
+ *   <offset> is the starting offset within the TLV value
+ *
+ *   <length> is the length (or zero to read the from <offset> to the end)
+ *
+ *   <prefix>, if it has a non-zero value, is the subtype byte string
+ *   of length <offset> to match at the start of the TLV value, up to
+ *   a maximum matched length of 4 bytes
+ *
+ *   <index> is the index of the entry matching <type> and <prefix> to
+ *   be accessed, with zero indicating the first matching entry
+ *
+ * The <prefix> is designed to accommodate both matching of the OUI
+ * within an organization-specific TLV (e.g. 0x0080c2 for IEEE 802.1
+ * TLVs) and of a subtype byte as found within many TLVs.
+ *
+ * This encoding allows most LLDP values to be extracted easily.  For
+ * example
+ *
+ *   System name: ${netX.lldp/5.0.0.0:string}
+ *
+ *   System description: ${netX.lldp/6.0.0.0:string}
+ *
+ *   Port description: ${netX.lldp/4.0.0.0:string}
+ *
+ *   Port interface name: ${netX.lldp/5.2.0.1.0:string}
+ *
+ *   Chassis MAC address: ${netX.lldp/4.1.0.1.0:hex}
+ *
+ *   Management IPv4 address: ${netX.lldp/5.1.8.0.2.4:ipv4}
+ *
+ *   Port VLAN ID: ${netX.lldp/0x0080c2.1.127.0.4.2:int16}
+ *
+ *   Port VLAN name: ${netX.lldp/0x0080c2.3.127.0.7.0:string}
+ *
+ *   Maximum frame size: ${netX.lldp/0x00120f.4.127.0.4.2:uint16}
+ *
+ */
+#define LLDP_TAG( prefix, type, index, offset, length )			\
+	( ( ( ( uint64_t ) (prefix) ) << 32 ) |				\
+	  ( (type) << 24 ) | ( (index) << 16 ) |			\
+	  ( (offset) << 8 ) | ( (length) << 0 ) )
+
+#endif /* _IPXE_LLDP_H */
diff --git a/src/net/lldp.c b/src/net/lldp.c
new file mode 100644
index 00000000..72e3ecdf
--- /dev/null
+++ b/src/net/lldp.c
@@ -0,0 +1,340 @@
+/*
+ * Copyright (C) 2023 Michael Brown <mbrown@fensystems.co.uk>.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ *
+ * You can also choose to distribute this program under the terms of
+ * the Unmodified Binary Distribution Licence (as given in the file
+ * COPYING.UBDL), provided that you have satisfied its requirements.
+ */
+
+FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
+
+/** @file
+ *
+ * Link Layer Discovery Protocol
+ *
+ */
+
+#include <stdlib.h>
+#include <errno.h>
+#include <byteswap.h>
+#include <ipxe/iobuf.h>
+#include <ipxe/netdevice.h>
+#include <ipxe/if_ether.h>
+#include <ipxe/settings.h>
+#include <ipxe/lldp.h>
+
+/** An LLDP settings block */
+struct lldp_settings {
+	/** Reference counter */
+	struct refcnt refcnt;
+	/** Settings interface */
+	struct settings settings;
+	/** List of LLDP settings blocks */
+	struct list_head list;
+	/** Name */
+	const char *name;
+	/** LLDP data */
+	void *data;
+	/** Length of LLDP data */
+	size_t len;
+};
+
+/** LLDP settings scope */
+static const struct settings_scope lldp_settings_scope;
+
+/** List of LLDP settings blocks */
+static LIST_HEAD ( lldp_settings );
+
+/**
+ * Free LLDP settings block
+ *
+ * @v refcnt		Reference counter
+ */
+static void lldp_free ( struct refcnt *refcnt ) {
+	struct lldp_settings *lldpset =
+		container_of ( refcnt, struct lldp_settings, refcnt );
+
+	DBGC ( lldpset, "LLDP %s freed\n", lldpset->name );
+	list_del ( &lldpset->list );
+	free ( lldpset->data );
+	free ( lldpset );
+}
+
+/**
+ * Find LLDP settings block
+ *
+ * @v netdev		Network device
+ * @ret lldpset		LLDP settings block
+ */
+static struct lldp_settings * lldp_find ( struct net_device *netdev ) {
+	struct lldp_settings *lldpset;
+
+	/* Find matching LLDP settings block */
+	list_for_each_entry ( lldpset, &lldp_settings, list ) {
+		if ( netdev_settings ( netdev ) == lldpset->settings.parent )
+			return lldpset;
+	}
+
+	return NULL;
+}
+
+/**
+ * Check applicability of LLDP setting
+ *
+ * @v settings		Settings block
+ * @v setting		Setting to fetch
+ * @ret applies		Setting applies within this settings block
+ */
+static int lldp_applies ( struct settings *settings __unused,
+			  const struct setting *setting ) {
+
+	return ( setting->scope == &lldp_settings_scope );
+}
+
+/**
+ * Fetch value of LLDP setting
+ *
+ * @v settings		Settings block
+ * @v setting		Setting to fetch
+ * @v buf		Buffer to fill with setting data
+ * @v len		Length of buffer
+ * @ret len		Length of setting data, or negative error
+ */
+static int lldp_fetch ( struct settings *settings,
+			struct setting *setting,
+			void *buf, size_t len ) {
+	struct lldp_settings *lldpset =
+		container_of ( settings, struct lldp_settings, settings );
+	union {
+		uint32_t high;
+		uint8_t raw[4];
+	} tag_prefix;
+	uint32_t tag_low;
+	uint8_t tag_type;
+	uint8_t tag_index;
+	uint8_t tag_offset;
+	uint8_t tag_length;
+	const void *match;
+	const void *data;
+	size_t match_len;
+	size_t remaining;
+	const struct lldp_tlv *tlv;
+	unsigned int tlv_type_len;
+	unsigned int tlv_type;
+	unsigned int tlv_len;
+
+	/* Parse setting tag */
+	tag_prefix.high = htonl ( setting->tag >> 32 );
+	tag_low = setting->tag;
+	tag_type = ( tag_low >> 24 );
+	tag_index = ( tag_low >> 16 );
+	tag_offset = ( tag_low >> 8 );
+	tag_length = ( tag_low >> 0 );
+
+	/* Identify match prefix */
+	match_len = tag_offset;
+	if ( match_len > sizeof ( tag_prefix ) )
+		match_len = sizeof ( tag_prefix );
+	if ( ! tag_prefix.high )
+		match_len = 0;
+	match = &tag_prefix.raw[ sizeof ( tag_prefix ) - match_len ];
+
+	/* Locate matching TLV */
+	for ( data = lldpset->data, remaining = lldpset->len ; remaining ;
+	      data += tlv_len, remaining -= tlv_len ) {
+
+		/* Parse TLV header */
+		if ( remaining < sizeof ( *tlv ) ) {
+			DBGC ( lldpset, "LLDP %s underlength TLV header\n",
+			       lldpset->name );
+			DBGC_HDA ( lldpset, 0, data, remaining );
+			break;
+		}
+		tlv = data;
+		data += sizeof ( *tlv );
+		remaining -= sizeof ( *tlv );
+		tlv_type_len = ntohs ( tlv->type_len );
+		tlv_type = LLDP_TLV_TYPE ( tlv_type_len );
+		if ( tlv_type == LLDP_TYPE_END )
+			break;
+		tlv_len = LLDP_TLV_LEN ( tlv_type_len );
+		if ( remaining < tlv_len ) {
+			DBGC ( lldpset, "LLDP %s underlength TLV value\n",
+			       lldpset->name );
+			DBGC_HDA ( lldpset, 0, data, remaining );
+			break;
+		}
+		DBGC2 ( lldpset, "LLDP %s found type %d:\n",
+			lldpset->name, tlv_type );
+		DBGC2_HDA ( lldpset, 0, data, tlv_len );
+
+		/* Check for matching tag type */
+		if ( tlv_type != tag_type )
+			continue;
+
+		/* Check for matching prefix */
+		if ( tlv_len < match_len )
+			continue;
+		if ( memcmp ( data, match, match_len ) != 0 )
+			continue;
+
+		/* Check for matching index */
+		if ( tag_index-- )
+			continue;
+
+		/* Skip offset */
+		if ( tlv_len < tag_offset )
+			return 0;
+		data += tag_offset;
+		tlv_len -= tag_offset;
+
+		/* Set type if not already specified */
+		if ( ! setting->type ) {
+			setting->type = ( tag_length ? &setting_type_hex :
+					  &setting_type_string );
+		}
+
+		/* Extract value */
+		if ( tag_length && ( tlv_len > tag_length ) )
+			tlv_len = tag_length;
+		if ( len > tlv_len )
+			len = tlv_len;
+		memcpy ( buf, data, len );
+		return tlv_len;
+	}
+
+	return -ENOENT;
+}
+
+/** LLDP settings operations */
+static struct settings_operations lldp_settings_operations = {
+	.applies = lldp_applies,
+	.fetch = lldp_fetch,
+};
+
+/**
+ * Process LLDP packet
+ *
+ * @v iobuf		I/O buffer
+ * @v netdev		Network device
+ * @v ll_dest		Link-layer destination address
+ * @v ll_source		Link-layer source address
+ * @v flags		Packet flags
+ * @ret rc		Return status code
+ */
+static int lldp_rx ( struct io_buffer *iobuf, struct net_device *netdev,
+		     const void *ll_dest, const void *ll_source,
+		     unsigned int flags __unused ) {
+	struct lldp_settings *lldpset;
+	size_t len;
+	void *data;
+	int rc;
+
+	/* Find matching LLDP settings block */
+	lldpset = lldp_find ( netdev );
+	if ( ! lldpset ) {
+		DBGC ( netdev, "LLDP %s has no \"%s\" settings block\n",
+		       netdev->name, LLDP_SETTINGS_NAME );
+		rc = -ENOENT;
+		goto err_find;
+	}
+
+	/* Create trimmed copy of received LLDP data */
+	len = iob_len ( iobuf );
+	data = malloc ( len );
+	if ( ! data ) {
+		rc = -ENOMEM;
+		goto err_alloc;
+	}
+	memcpy ( data, iobuf->data, len );
+
+	/* Free any existing LLDP data */
+	free ( lldpset->data );
+
+	/* Transfer data to LLDP settings block */
+	lldpset->data = data;
+	lldpset->len = len;
+	data = NULL;
+	DBGC2 ( lldpset, "LLDP %s src %s ",
+		lldpset->name, netdev->ll_protocol->ntoa ( ll_source ) );
+	DBGC2 ( lldpset, "dst %s\n", netdev->ll_protocol->ntoa ( ll_dest ) );
+	DBGC2_HDA ( lldpset, 0, lldpset->data, lldpset->len );
+
+	/* Success */
+	rc = 0;
+
+	free ( data );
+ err_alloc:
+ err_find:
+	free_iob ( iobuf );
+	return rc;
+}
+
+/** LLDP protocol */
+struct net_protocol lldp_protocol __net_protocol = {
+	.name = "LLDP",
+	.net_proto = htons ( ETH_P_LLDP ),
+	.rx = lldp_rx,
+};
+
+/**
+ * Create LLDP settings block
+ *
+ * @v netdev		Network device
+ * @ret rc		Return status code
+ */
+static int lldp_probe ( struct net_device *netdev ) {
+	struct lldp_settings *lldpset;
+	int rc;
+
+	/* Allocate LLDP settings block */
+	lldpset = zalloc ( sizeof ( *lldpset ) );
+	if ( ! lldpset ) {
+		rc = -ENOMEM;
+		goto err_alloc;
+	}
+	ref_init ( &lldpset->refcnt, lldp_free );
+	settings_init ( &lldpset->settings, &lldp_settings_operations,
+			&lldpset->refcnt, &lldp_settings_scope );
+	list_add_tail ( &lldpset->list, &lldp_settings );
+	lldpset->name = netdev->name;
+
+	/* Register settings */
+	if ( ( rc = register_settings ( &lldpset->settings, netdev_settings ( netdev ),
+					LLDP_SETTINGS_NAME ) ) != 0 ) {
+		DBGC ( lldpset, "LLDP %s could not register settings: %s\n",
+		       lldpset->name, strerror ( rc ) );
+		goto err_register;
+	}
+	DBGC ( lldpset, "LLDP %s registered\n", lldpset->name );
+
+	ref_put ( &lldpset->refcnt );
+	return 0;
+
+	unregister_settings ( &lldpset->settings );
+ err_register:
+	ref_put ( &lldpset->refcnt );
+ err_alloc:
+	return rc;
+}
+
+/** LLDP driver */
+struct net_driver lldp_driver __net_driver = {
+	.name = "LLDP",
+	.probe = lldp_probe,
+};