/* * 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 { /** 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, };