src/net/lldp.c - ipxe - Git at Google

 /*
  * 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 {
 	/** Settings interface */
 	struct settings settings;
 	/** Name */
 	const char *name;
 	/** LLDP data */
 	void *data;
 	/** Length of LLDP data */
 	size_t len;
 };

 /* Forward declaration */
 struct net_driver lldp_driver __net_driver;

 /** LLDP settings scope */
 static const struct settings_scope lldp_settings_scope;

 /**
  * 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 = netdev_priv ( netdev, &lldp_driver );

 	/* 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:
 	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
  * @v priv		Private data
  * @ret rc		Return status code
  */
 static int lldp_probe ( struct net_device *netdev, void *priv ) {
 	struct lldp_settings *lldpset = priv;
 	int rc;

 	/* Initialise LLDP settings block */
 	settings_init ( &lldpset->settings, &lldp_settings_operations,
 			&netdev->refcnt, &lldp_settings_scope );
 	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 );

 	return 0;

 	unregister_settings ( &lldpset->settings );
  err_register:
 	assert ( lldpset->data == NULL );
 	return rc;
 }

 /**
  * Remove LLDP settings block
  *
  * @v netdev		Network device
  * @v priv		Private data
  */
 static void lldp_remove ( struct net_device *netdev __unused, void *priv ) {
 	struct lldp_settings *lldpset = priv;

 	/* Unregister settings */
 	unregister_settings ( &lldpset->settings );
 	DBGC ( lldpset, "LLDP %s unregistered\n", lldpset->name );

 	/* Free any LLDP data */
 	free ( lldpset->data );
 	lldpset->data = NULL;
 }

 /** LLDP driver */
 struct net_driver lldp_driver __net_driver = {
 	.name = "LLDP",
 	.priv_len = sizeof ( struct lldp_settings ),
 	.probe = lldp_probe,
 	.remove = lldp_remove,
 };
	/*
	* 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 {
	/** Settings interface */
	struct settings settings;
	/** Name */
	const char *name;
	/** LLDP data */
	void *data;
	/** Length of LLDP data */
	size_t len;
	};

	/* Forward declaration */
	struct net_driver lldp_driver __net_driver;

	/** LLDP settings scope */
	static const struct settings_scope lldp_settings_scope;

	/**
	* 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 = netdev_priv ( netdev, &lldp_driver );

	/* 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:
	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
	* @v priv Private data
	* @ret rc Return status code
	*/
	static int lldp_probe ( struct net_device netdev, void priv ) {
	struct lldp_settings *lldpset = priv;
	int rc;

	/* Initialise LLDP settings block */
	settings_init ( &lldpset->settings, &lldp_settings_operations,
	&netdev->refcnt, &lldp_settings_scope );
	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 );

	return 0;

	unregister_settings ( &lldpset->settings );
	err_register:
	assert ( lldpset->data == NULL );
	return rc;
	}

	/**
	* Remove LLDP settings block
	*
	* @v netdev Network device
	* @v priv Private data
	*/
	static void lldp_remove ( struct net_device netdev __unused, void priv ) {
	struct lldp_settings *lldpset = priv;

	/* Unregister settings */
	unregister_settings ( &lldpset->settings );
	DBGC ( lldpset, "LLDP %s unregistered\n", lldpset->name );

	/* Free any LLDP data */
	free ( lldpset->data );
	lldpset->data = NULL;
	}

	/** LLDP driver */
	struct net_driver lldp_driver __net_driver = {
	.name = "LLDP",
	.priv_len = sizeof ( struct lldp_settings ),
	.probe = lldp_probe,
	.remove = lldp_remove,
	};