From dc16de3204d1956d4fd17808e6d34ac926bbe932 Mon Sep 17 00:00:00 2001 From: Michael Brown Date: Sun, 5 Feb 2023 13:07:30 +0000 Subject: [lldp] Add support for the Link Layer Discovery Protocol Add support for recording LLDP packets and exposing TLV values via the settings mechanism. LLDP settings are encoded as ${netX.lldp/....} where is the TLV type is the starting offset within the TLV value is the length (or zero to read the from to the end) , if it has a non-zero value, is the subtype byte string of length to match at the start of the TLV value, up to a maximum matched length of 4 bytes is the index of the entry matching and to be accessed, with zero indicating the first matching entry The 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 Signed-off-by: Michael Brown --- src/net/lldp.c | 340 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 340 insertions(+) create mode 100644 src/net/lldp.c (limited to 'src/net') diff --git a/src/net/lldp.c b/src/net/lldp.c new file mode 100644 index 000000000..72e3ecdf6 --- /dev/null +++ b/src/net/lldp.c @@ -0,0 +1,340 @@ +/* + * Copyright (C) 2023 Michael Brown . + * + * 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 +#include +#include +#include +#include +#include +#include +#include + +/** 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, +}; -- cgit v1.2.3-55-g7522