diff options
Diffstat (limited to 'src/daemon/protocols/lldp.c')
| -rw-r--r-- | src/daemon/protocols/lldp.c | 1315 |
1 files changed, 1315 insertions, 0 deletions
diff --git a/src/daemon/protocols/lldp.c b/src/daemon/protocols/lldp.c new file mode 100644 index 0000000..6e73237 --- /dev/null +++ b/src/daemon/protocols/lldp.c @@ -0,0 +1,1315 @@ +/* -*- mode: c; c-file-style: "openbsd" -*- */ +/* + * Copyright (c) 2008 Vincent Bernat <bernat@luffy.cx> + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#include "../lldpd.h" +#include "../frame.h" + +#include <unistd.h> +#include <errno.h> +#include <time.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <sys/ioctl.h> + +static int +lldpd_af_to_lldp_proto(int af) +{ + switch (af) { + case LLDPD_AF_IPV4: + return LLDP_MGMT_ADDR_IP4; + case LLDPD_AF_IPV6: + return LLDP_MGMT_ADDR_IP6; + default: + return LLDP_MGMT_ADDR_NONE; + } +} + +static int +lldpd_af_from_lldp_proto(int proto) +{ + switch (proto) { + case LLDP_MGMT_ADDR_IP4: + return LLDPD_AF_IPV4; + case LLDP_MGMT_ADDR_IP6: + return LLDPD_AF_IPV6; + default: + return LLDPD_AF_UNSPEC; + } +} + +static int +_lldp_send(struct lldpd *global, struct lldpd_hardware *hardware, u_int8_t c_id_subtype, + char *c_id, int c_id_len, u_int8_t p_id_subtype, char *p_id, int p_id_len, + int shutdown, int without_vlans) +{ + struct lldpd_port *port; + struct lldpd_chassis *chassis; + struct lldpd_frame *frame; + int length; + u_int8_t *packet, *pos, *tlv; + struct lldpd_mgmt *mgmt; + int proto; + int vlans = 0; + + u_int8_t mcastaddr_regular[] = LLDP_ADDR_NEAREST_BRIDGE; + u_int8_t mcastaddr_nontpmr[] = LLDP_ADDR_NEAREST_NONTPMR_BRIDGE; + u_int8_t mcastaddr_customer[] = LLDP_ADDR_NEAREST_CUSTOMER_BRIDGE; + u_int8_t *mcastaddr; +#ifdef ENABLE_DOT1 + const u_int8_t dot1[] = LLDP_TLV_ORG_DOT1; + struct lldpd_vlan *vlan; + struct lldpd_ppvid *ppvid; + struct lldpd_pi *pi; +#endif +#ifdef ENABLE_DOT3 + const u_int8_t dot3[] = LLDP_TLV_ORG_DOT3; +#endif +#ifdef ENABLE_LLDPMED + int i; + const u_int8_t med[] = LLDP_TLV_ORG_MED; +#endif +#ifdef ENABLE_CUSTOM + struct lldpd_custom *custom; +#endif + port = &hardware->h_lport; + chassis = port->p_chassis; + length = hardware->h_mtu; + if ((packet = (u_int8_t *)calloc(1, length)) == NULL) return ENOMEM; + pos = packet; + + /* Ethernet header */ + switch (global->g_config.c_lldp_agent_type) { + case LLDP_AGENT_TYPE_NEAREST_NONTPMR_BRIDGE: + mcastaddr = mcastaddr_nontpmr; + break; + case LLDP_AGENT_TYPE_NEAREST_CUSTOMER_BRIDGE: + mcastaddr = mcastaddr_customer; + break; + case LLDP_AGENT_TYPE_NEAREST_BRIDGE: + default: + mcastaddr = mcastaddr_regular; + break; + } + if (!( + /* LLDP multicast address */ + POKE_BYTES(mcastaddr, ETHER_ADDR_LEN) && + /* Source MAC address */ + POKE_BYTES(&hardware->h_lladdr, ETHER_ADDR_LEN))) + goto toobig; + + /* Insert VLAN tag if needed */ + if (port->p_vlan_tx_enabled) { + if (!( + /* VLAN ethertype */ + POKE_UINT16(ETHERTYPE_VLAN) && + /* VLAN Tag Control Information (TCI) */ + /* Priority(3bits) | DEI(1bit) | VID(12bit) */ + POKE_UINT16(port->p_vlan_tx_tag))) + goto toobig; + } + + if (!( + /* LLDP frame */ + POKE_UINT16(ETH_P_LLDP))) + goto toobig; + + /* Chassis ID */ + if (!(POKE_START_LLDP_TLV(LLDP_TLV_CHASSIS_ID) && POKE_UINT8(c_id_subtype) && + POKE_BYTES(c_id, c_id_len) && POKE_END_LLDP_TLV)) + goto toobig; + + /* Port ID */ + if (!(POKE_START_LLDP_TLV(LLDP_TLV_PORT_ID) && POKE_UINT8(p_id_subtype) && + POKE_BYTES(p_id, p_id_len) && POKE_END_LLDP_TLV)) + goto toobig; + + /* Time to live */ + if (!(POKE_START_LLDP_TLV(LLDP_TLV_TTL) && + POKE_UINT16(shutdown ? 0 : (global ? global->g_config.c_ttl : 180)) && + POKE_END_LLDP_TLV)) + goto toobig; + + if (shutdown) goto end; + + /* System name */ + if (chassis->c_name && *chassis->c_name != '\0') { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_SYSTEM_NAME) && + POKE_BYTES(chassis->c_name, strlen(chassis->c_name)) && + POKE_END_LLDP_TLV)) + goto toobig; + } + + /* System description (skip it if empty) */ + if (chassis->c_descr && *chassis->c_descr != '\0') { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_SYSTEM_DESCR) && + POKE_BYTES(chassis->c_descr, strlen(chassis->c_descr)) && + POKE_END_LLDP_TLV)) + goto toobig; + } + + /* System capabilities */ + if (global->g_config.c_cap_advertise && chassis->c_cap_available) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_SYSTEM_CAP) && + POKE_UINT16(chassis->c_cap_available) && + POKE_UINT16(chassis->c_cap_enabled) && POKE_END_LLDP_TLV)) + goto toobig; + } + + /* Management addresses */ + TAILQ_FOREACH (mgmt, &chassis->c_mgmt, m_entries) { + proto = lldpd_af_to_lldp_proto(mgmt->m_family); + if (proto == LLDP_MGMT_ADDR_NONE) continue; + if (!(POKE_START_LLDP_TLV(LLDP_TLV_MGMT_ADDR) && + /* Size of the address, including its type */ + POKE_UINT8(mgmt->m_addrsize + 1) && POKE_UINT8(proto) && + POKE_BYTES(&mgmt->m_addr, mgmt->m_addrsize))) + goto toobig; + + /* Interface port type, OID */ + if (mgmt->m_iface == 0) { + if (!( + /* We don't know the management interface */ + POKE_UINT8(LLDP_MGMT_IFACE_UNKNOWN) && POKE_UINT32(0))) + goto toobig; + } else { + if (!( + /* We have the index of the management interface */ + POKE_UINT8(LLDP_MGMT_IFACE_IFINDEX) && + POKE_UINT32(mgmt->m_iface))) + goto toobig; + } + if (!( + /* We don't provide an OID for management */ + POKE_UINT8(0) && POKE_END_LLDP_TLV)) + goto toobig; + } + + /* Port description */ + if (port->p_descr && *port->p_descr != '\0') { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_PORT_DESCR) && + POKE_BYTES(port->p_descr, strlen(port->p_descr)) && + POKE_END_LLDP_TLV)) + goto toobig; + } + +#ifdef ENABLE_DOT1 + /* Port VLAN ID */ + if (port->p_pvid != 0) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(dot1, sizeof(dot1)) && + POKE_UINT8(LLDP_TLV_DOT1_PVID) && POKE_UINT16(port->p_pvid) && + POKE_END_LLDP_TLV)) { + goto toobig; + } + } + /* Port and Protocol VLAN IDs */ + TAILQ_FOREACH (ppvid, &port->p_ppvids, p_entries) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(dot1, sizeof(dot1)) && + POKE_UINT8(LLDP_TLV_DOT1_PPVID) && + POKE_UINT8(ppvid->p_cap_status) && + POKE_UINT16(ppvid->p_ppvid) && POKE_END_LLDP_TLV)) { + goto toobig; + } + } + /* VLANs */ + if (!without_vlans) { + TAILQ_FOREACH (vlan, &port->p_vlans, v_entries) { + vlans++; + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(dot1, sizeof(dot1)) && + POKE_UINT8(LLDP_TLV_DOT1_VLANNAME) && + POKE_UINT16(vlan->v_vid) && + POKE_UINT8(strlen(vlan->v_name)) && + POKE_BYTES(vlan->v_name, strlen(vlan->v_name)) && + POKE_END_LLDP_TLV)) + goto toobig; + } + } + /* Protocol Identities */ + TAILQ_FOREACH (pi, &port->p_pids, p_entries) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(dot1, sizeof(dot1)) && + POKE_UINT8(LLDP_TLV_DOT1_PI) && POKE_UINT8(pi->p_pi_len) && + POKE_BYTES(pi->p_pi, pi->p_pi_len) && POKE_END_LLDP_TLV)) + goto toobig; + } +#endif + +#ifdef ENABLE_DOT3 + /* Aggregation status */ + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && POKE_BYTES(dot3, sizeof(dot3)) && + POKE_UINT8(LLDP_TLV_DOT3_LA) && + /* Bit 0 = capability ; Bit 1 = status */ + POKE_UINT8((port->p_aggregid) ? 3 : 1) && + POKE_UINT32(port->p_aggregid) && POKE_END_LLDP_TLV)) + goto toobig; + + /* MAC/PHY */ + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && POKE_BYTES(dot3, sizeof(dot3)) && + POKE_UINT8(LLDP_TLV_DOT3_MAC) && + POKE_UINT8(port->p_macphy.autoneg_support | + (port->p_macphy.autoneg_enabled << 1)) && + POKE_UINT16(port->p_macphy.autoneg_advertised) && + POKE_UINT16(port->p_macphy.mau_type) && POKE_END_LLDP_TLV)) + goto toobig; + + /* MFS */ + if (port->p_mfs) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(dot3, sizeof(dot3)) && + POKE_UINT8(LLDP_TLV_DOT3_MFS) && POKE_UINT16(port->p_mfs) && + POKE_END_LLDP_TLV)) + goto toobig; + } + /* Power */ + if (port->p_power.devicetype) { + if (!((POKE_START_LLDP_TLV(LLDP_TLV_ORG)) && + POKE_BYTES(dot3, sizeof(dot3)) && + POKE_UINT8(LLDP_TLV_DOT3_POWER) && + POKE_UINT8(((((2 - port->p_power.devicetype) % (1 << 1)) << 0) | + ((port->p_power.supported % (1 << 1)) << 1) | + ((port->p_power.enabled % (1 << 1)) << 2) | + ((port->p_power.paircontrol % (1 << 1)) << 3))) && + POKE_UINT8(port->p_power.pairs) && + POKE_UINT8(port->p_power.class))) + goto toobig; + /* 802.3at */ + if (port->p_power.powertype != LLDP_DOT3_POWER_8023AT_OFF) { + if (!(POKE_UINT8(((((port->p_power.powertype == + LLDP_DOT3_POWER_8023AT_TYPE1) ? + 1 : + 0) + << 7) | + (((port->p_power.devicetype == LLDP_DOT3_POWER_PSE) ? + 0 : + 1) + << 6) | + ((port->p_power.source % (1 << 2)) << 4) | + ((port->p_power.priority % (1 << 2)) << 0))) && + POKE_UINT16(port->p_power.requested) && + POKE_UINT16(port->p_power.allocated))) + goto toobig; + } + if (!(POKE_END_LLDP_TLV)) goto toobig; + } +#endif + +#ifdef ENABLE_LLDPMED + if (port->p_med_cap_enabled) { + /* LLDP-MED cap */ + if (port->p_med_cap_enabled & LLDP_MED_CAP_CAP) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(med, sizeof(med)) && + POKE_UINT8(LLDP_TLV_MED_CAP) && + POKE_UINT16(chassis->c_med_cap_available) && + POKE_UINT8(chassis->c_med_type) && POKE_END_LLDP_TLV)) + goto toobig; + } + + /* LLDP-MED inventory */ +# define LLDP_INVENTORY(value, subtype) \ + if (value) { \ + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && POKE_BYTES(med, sizeof(med)) && \ + POKE_UINT8(subtype) && \ + POKE_BYTES(value, (strlen(value) > 32) ? 32 : strlen(value)) && \ + POKE_END_LLDP_TLV)) \ + goto toobig; \ + } + + if (port->p_med_cap_enabled & LLDP_MED_CAP_IV) { + LLDP_INVENTORY(chassis->c_med_hw, LLDP_TLV_MED_IV_HW); + LLDP_INVENTORY(chassis->c_med_fw, LLDP_TLV_MED_IV_FW); + LLDP_INVENTORY(chassis->c_med_sw, LLDP_TLV_MED_IV_SW); + LLDP_INVENTORY(chassis->c_med_sn, LLDP_TLV_MED_IV_SN); + LLDP_INVENTORY(chassis->c_med_manuf, LLDP_TLV_MED_IV_MANUF); + LLDP_INVENTORY(chassis->c_med_model, LLDP_TLV_MED_IV_MODEL); + LLDP_INVENTORY(chassis->c_med_asset, LLDP_TLV_MED_IV_ASSET); + } + + /* LLDP-MED location */ + for (i = 0; i < LLDP_MED_LOCFORMAT_LAST; i++) { + if (port->p_med_location[i].format == i + 1) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(med, sizeof(med)) && + POKE_UINT8(LLDP_TLV_MED_LOCATION) && + POKE_UINT8(port->p_med_location[i].format) && + POKE_BYTES(port->p_med_location[i].data, + port->p_med_location[i].data_len) && + POKE_END_LLDP_TLV)) + goto toobig; + } + } + + /* LLDP-MED network policy */ + for (i = 0; i < LLDP_MED_APPTYPE_LAST; i++) { + if (port->p_med_policy[i].type == i + 1) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(med, sizeof(med)) && + POKE_UINT8(LLDP_TLV_MED_POLICY) && + POKE_UINT32(( + ((port->p_med_policy[i].type % (1 << 8)) + << 24) | + ((port->p_med_policy[i].unknown % (1 << 1)) + << 23) | + ((port->p_med_policy[i].tagged % (1 << 1)) + << 22) | + /*((0 %(1<< + 1))<<21) |*/ + ((port->p_med_policy[i].vid % (1 << 12)) + << 9) | + ((port->p_med_policy[i].priority % (1 << 3)) + << 6) | + ((port->p_med_policy[i].dscp % (1 << 6)) + << 0))) && + POKE_END_LLDP_TLV)) + goto toobig; + } + } + + /* LLDP-MED POE-MDI */ + if ((port->p_med_power.devicetype == LLDP_MED_POW_TYPE_PSE) || + (port->p_med_power.devicetype == LLDP_MED_POW_TYPE_PD)) { + int devicetype = 0, source = 0; + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(med, sizeof(med)) && + POKE_UINT8(LLDP_TLV_MED_MDI))) + goto toobig; + switch (port->p_med_power.devicetype) { + case LLDP_MED_POW_TYPE_PSE: + devicetype = 0; + switch (port->p_med_power.source) { + case LLDP_MED_POW_SOURCE_PRIMARY: + source = 1; + break; + case LLDP_MED_POW_SOURCE_BACKUP: + source = 2; + break; + case LLDP_MED_POW_SOURCE_RESERVED: + source = 3; + break; + default: + source = 0; + break; + } + break; + case LLDP_MED_POW_TYPE_PD: + devicetype = 1; + switch (port->p_med_power.source) { + case LLDP_MED_POW_SOURCE_PSE: + source = 1; + break; + case LLDP_MED_POW_SOURCE_LOCAL: + source = 2; + break; + case LLDP_MED_POW_SOURCE_BOTH: + source = 3; + break; + default: + source = 0; + break; + } + break; + } + if (!(POKE_UINT8((((devicetype % (1 << 2)) << 6) | + ((source % (1 << 2)) << 4) | + ((port->p_med_power.priority % (1 << 4)) << 0))) && + POKE_UINT16(port->p_med_power.val) && + POKE_END_LLDP_TLV)) + goto toobig; + } + } +#endif + +#ifdef ENABLE_CUSTOM + TAILQ_FOREACH (custom, &port->p_custom_list, next) { + if (!(POKE_START_LLDP_TLV(LLDP_TLV_ORG) && + POKE_BYTES(custom->oui, sizeof(custom->oui)) && + POKE_UINT8(custom->subtype) && + POKE_BYTES(custom->oui_info, custom->oui_info_len) && + POKE_END_LLDP_TLV)) + goto toobig; + } +#endif + +end: + /* END */ + if (!(POKE_START_LLDP_TLV(LLDP_TLV_END) && POKE_END_LLDP_TLV)) goto toobig; + + if (interfaces_send_helper(global, hardware, (char *)packet, pos - packet) == + -1) { + log_warn("lldp", "unable to send packet on real device for %s", + hardware->h_ifname); + free(packet); + return ENETDOWN; + } + + hardware->h_tx_cnt++; + + /* We assume that LLDP frame is the reference */ + if (!shutdown && + (frame = (struct lldpd_frame *)malloc(sizeof(int) + pos - packet)) != + NULL) { + frame->size = pos - packet; + memcpy(&frame->frame, packet, frame->size); + if ((hardware->h_lport.p_lastframe == NULL) || + (hardware->h_lport.p_lastframe->size != frame->size) || + (memcmp(hardware->h_lport.p_lastframe->frame, frame->frame, + frame->size) != 0)) { + free(hardware->h_lport.p_lastframe); + hardware->h_lport.p_lastframe = frame; + hardware->h_lport.p_lastchange = time(NULL); + } else + free(frame); + } + + free(packet); + return 0; + +toobig: + free(packet); + if (vlans > 0 && !without_vlans) { + /* Retry without VLANs */ + return _lldp_send(global, hardware, c_id_subtype, c_id, c_id_len, + p_id_subtype, p_id, p_id_len, shutdown, 1); + } + log_info("lldp", "Cannot send LLDP packet for %s, too big message", + hardware->h_ifname); + return E2BIG; +} + +/* Send a shutdown LLDPDU. */ +int +lldp_send_shutdown(struct lldpd *global, struct lldpd_hardware *hardware) +{ + if (hardware->h_lchassis_previous_id == NULL || + hardware->h_lport_previous_id == NULL) + return 0; + return _lldp_send(global, hardware, hardware->h_lchassis_previous_id_subtype, + hardware->h_lchassis_previous_id, hardware->h_lchassis_previous_id_len, + hardware->h_lport_previous_id_subtype, hardware->h_lport_previous_id, + hardware->h_lport_previous_id_len, 1, 0); +} + +int +lldp_send(struct lldpd *global, struct lldpd_hardware *hardware) +{ + struct lldpd_port *port = &hardware->h_lport; + struct lldpd_chassis *chassis = port->p_chassis; + int ret; + + /* Check if we have a change. */ + if (hardware->h_lchassis_previous_id != NULL && + hardware->h_lport_previous_id != NULL && + (hardware->h_lchassis_previous_id_subtype != chassis->c_id_subtype || + hardware->h_lchassis_previous_id_len != chassis->c_id_len || + hardware->h_lport_previous_id_subtype != port->p_id_subtype || + hardware->h_lport_previous_id_len != port->p_id_len || + memcmp(hardware->h_lchassis_previous_id, chassis->c_id, + chassis->c_id_len) || + memcmp(hardware->h_lport_previous_id, port->p_id, port->p_id_len))) { + log_info("lldp", + "MSAP has changed for port %s, sending a shutdown LLDPDU", + hardware->h_ifname); + if ((ret = lldp_send_shutdown(global, hardware)) != 0) return ret; + } + + log_debug("lldp", "send LLDP PDU to %s", hardware->h_ifname); + + if ((ret = _lldp_send(global, hardware, chassis->c_id_subtype, chassis->c_id, + chassis->c_id_len, port->p_id_subtype, port->p_id, port->p_id_len, 0, + 0)) != 0) + return ret; + + /* Record current chassis and port ID */ + free(hardware->h_lchassis_previous_id); + hardware->h_lchassis_previous_id_subtype = chassis->c_id_subtype; + hardware->h_lchassis_previous_id_len = chassis->c_id_len; + if ((hardware->h_lchassis_previous_id = malloc(chassis->c_id_len)) != NULL) + memcpy(hardware->h_lchassis_previous_id, chassis->c_id, + chassis->c_id_len); + free(hardware->h_lport_previous_id); + hardware->h_lport_previous_id_subtype = port->p_id_subtype; + hardware->h_lport_previous_id_len = port->p_id_len; + if ((hardware->h_lport_previous_id = malloc(port->p_id_len)) != NULL) + memcpy(hardware->h_lport_previous_id, port->p_id, port->p_id_len); + + return 0; +} + +#define CHECK_TLV_SIZE(x, name) \ + do { \ + if (tlv_size < (x)) { \ + log_warnx("lldp", name " TLV too short received on %s", hardware->h_ifname); \ + goto malformed; \ + } \ + } while (0) +#define CHECK_TLV_MAX_SIZE(x, name) \ + do { \ + if (tlv_size > (x)) { \ + log_warnx("lldp", name " TLV too large received on %s", hardware->h_ifname); \ + goto malformed; \ + } \ + } while (0) + +int +lldp_decode(struct lldpd *cfg, char *frame, int s, struct lldpd_hardware *hardware, + struct lldpd_chassis **newchassis, struct lldpd_port **newport) +{ + struct lldpd_chassis *chassis; + struct lldpd_port *port; + char lldpaddr[ETHER_ADDR_LEN]; + const char dot1[] = LLDP_TLV_ORG_DOT1; + const char dot3[] = LLDP_TLV_ORG_DOT3; + const char med[] = LLDP_TLV_ORG_MED; + const char dcbx[] = LLDP_TLV_ORG_DCBX; + unsigned char orgid[3]; + int length, gotend = 0, ttl_received = 0; + int tlv_size, tlv_type, tlv_subtype, tlv_count = 0; + u_int8_t *pos, *tlv; + char *b; +#ifdef ENABLE_DOT1 + struct lldpd_vlan *vlan = NULL; + int vlan_len; + struct lldpd_ppvid *ppvid; + struct lldpd_pi *pi = NULL; +#endif + struct lldpd_mgmt *mgmt; + int af; + u_int8_t addr_str_length, addr_str_buffer[32] = { 0 }; + u_int8_t addr_family, addr_length, *addr_ptr, iface_subtype; + u_int32_t iface_number, iface; + int unrecognized; +#ifdef ENABLE_CUSTOM + struct lldpd_custom *custom = NULL; +#endif + + log_debug("lldp", "receive LLDP PDU on %s", hardware->h_ifname); + + if ((chassis = calloc(1, sizeof(struct lldpd_chassis))) == NULL) { + log_warn("lldp", "failed to allocate remote chassis"); + return -1; + } + TAILQ_INIT(&chassis->c_mgmt); + if ((port = calloc(1, sizeof(struct lldpd_port))) == NULL) { + log_warn("lldp", "failed to allocate remote port"); + free(chassis); + return -1; + } +#ifdef ENABLE_DOT1 + TAILQ_INIT(&port->p_vlans); + TAILQ_INIT(&port->p_ppvids); + TAILQ_INIT(&port->p_pids); +#endif +#ifdef ENABLE_CUSTOM + TAILQ_INIT(&port->p_custom_list); +#endif + + length = s; + pos = (u_int8_t *)frame; + + if (length < 2 * ETHER_ADDR_LEN + sizeof(u_int16_t)) { + log_warnx("lldp", "too short frame received on %s", hardware->h_ifname); + goto malformed; + } + PEEK_BYTES(lldpaddr, ETHER_ADDR_LEN); + if (memcmp(lldpaddr, (const char[])LLDP_ADDR_NEAREST_BRIDGE, ETHER_ADDR_LEN) && + memcmp(lldpaddr, (const char[])LLDP_ADDR_NEAREST_NONTPMR_BRIDGE, + ETHER_ADDR_LEN) && + memcmp(lldpaddr, (const char[])LLDP_ADDR_NEAREST_CUSTOMER_BRIDGE, + ETHER_ADDR_LEN)) { + log_info("lldp", + "frame not targeted at LLDP multicast address received on %s", + hardware->h_ifname); + goto malformed; + } + PEEK_DISCARD(ETHER_ADDR_LEN); /* Skip source address */ + if (PEEK_UINT16 != ETH_P_LLDP) { + log_info("lldp", "non LLDP frame received on %s", hardware->h_ifname); + goto malformed; + } + + while (length && (!gotend)) { + if (length < 2) { + log_warnx("lldp", "tlv header too short received on %s", + hardware->h_ifname); + goto malformed; + } + tlv_size = PEEK_UINT16; + tlv_type = tlv_size >> 9; + tlv_size = tlv_size & 0x1ff; + (void)PEEK_SAVE(tlv); + if (length < tlv_size) { + log_warnx("lldp", "frame too short for tlv received on %s", + hardware->h_ifname); + goto malformed; + } + /* Check order for mandatory TLVs */ + tlv_count++; + switch (tlv_type) { + case LLDP_TLV_CHASSIS_ID: + if (tlv_count != 1) { + log_warnx("lldp", + "Chassis ID TLV should be first on %s, but it is on position %d", + hardware->h_ifname, tlv_count); + goto malformed; + } + break; + case LLDP_TLV_PORT_ID: + if (tlv_count != 2) { + log_warnx("lldp", + "Port ID TLV should be second on %s, but it is on position %d", + hardware->h_ifname, tlv_count); + goto malformed; + } + break; + case LLDP_TLV_TTL: + if (tlv_count != 3) { + log_warnx("lldp", + "TTL TLV should be third on %s, but it is on position %d", + hardware->h_ifname, tlv_count); + goto malformed; + } + break; + } + + switch (tlv_type) { + case LLDP_TLV_END: + if (tlv_size != 0) { + log_warnx("lldp", + "lldp end received with size not null on %s", + hardware->h_ifname); + goto malformed; + } + if (length) + log_debug("lldp", "extra data after lldp end on %s", + hardware->h_ifname); + gotend = 1; + break; + case LLDP_TLV_CHASSIS_ID: + case LLDP_TLV_PORT_ID: + CHECK_TLV_SIZE(2, "Port/Chassis Id"); + CHECK_TLV_MAX_SIZE(256, "Port/Chassis Id"); + tlv_subtype = PEEK_UINT8; + if ((tlv_subtype == 0) || (tlv_subtype > 7)) { + log_warnx("lldp", + "unknown subtype for tlv id received on %s", + hardware->h_ifname); + goto malformed; + } + if ((b = (char *)calloc(1, tlv_size - 1)) == NULL) { + log_warn("lldp", + "unable to allocate memory for id tlv " + "received on %s", + hardware->h_ifname); + goto malformed; + } + PEEK_BYTES(b, tlv_size - 1); + if (tlv_type == LLDP_TLV_PORT_ID) { + if (port->p_id != NULL) { + log_warnx("lldp", + "Port ID TLV received twice on %s", + hardware->h_ifname); + free(b); + goto malformed; + } + port->p_id_subtype = tlv_subtype; + port->p_id = b; + port->p_id_len = tlv_size - 1; + } else { + if (chassis->c_id != NULL) { + log_warnx("lldp", + "Chassis ID TLV received twice on %s", + hardware->h_ifname); + free(b); + goto malformed; + } + chassis->c_id_subtype = tlv_subtype; + chassis->c_id = b; + chassis->c_id_len = tlv_size - 1; + } + break; + case LLDP_TLV_TTL: + if (ttl_received) { + log_warnx("lldp", "TTL TLV received twice on %s", + hardware->h_ifname); + goto malformed; + } + CHECK_TLV_SIZE(2, "TTL"); + port->p_ttl = PEEK_UINT16; + ttl_received = 1; + break; + case LLDP_TLV_PORT_DESCR: + case LLDP_TLV_SYSTEM_NAME: + case LLDP_TLV_SYSTEM_DESCR: + if (tlv_size < 1) { + log_debug("lldp", "empty tlv received on %s", + hardware->h_ifname); + break; + } + if ((b = (char *)calloc(1, tlv_size + 1)) == NULL) { + log_warn("lldp", + "unable to allocate memory for string tlv " + "received on %s", + hardware->h_ifname); + goto malformed; + } + PEEK_BYTES(b, tlv_size); + switch (tlv_type) { + case LLDP_TLV_PORT_DESCR: + free(port->p_descr); + port->p_descr = b; + break; + case LLDP_TLV_SYSTEM_NAME: + free(chassis->c_name); + chassis->c_name = b; + break; + case LLDP_TLV_SYSTEM_DESCR: + free(chassis->c_descr); + chassis->c_descr = b; + break; + default: + /* unreachable */ + free(b); + break; + } + break; + case LLDP_TLV_SYSTEM_CAP: + CHECK_TLV_SIZE(4, "System capabilities"); + chassis->c_cap_available = PEEK_UINT16; + chassis->c_cap_enabled = PEEK_UINT16; + break; + case LLDP_TLV_MGMT_ADDR: + CHECK_TLV_SIZE(1, "Management address"); + addr_str_length = PEEK_UINT8; + if (addr_str_length > sizeof(addr_str_buffer)) { + log_warnx("lldp", "too large management address on %s", + hardware->h_ifname); + goto malformed; + } + CHECK_TLV_SIZE(1 + addr_str_length, "Management address"); + PEEK_BYTES(addr_str_buffer, addr_str_length); + addr_length = addr_str_length - 1; + addr_family = addr_str_buffer[0]; + addr_ptr = &addr_str_buffer[1]; + CHECK_TLV_SIZE(1 + addr_str_length + 5, "Management address"); + iface_subtype = PEEK_UINT8; + iface_number = PEEK_UINT32; + + af = lldpd_af_from_lldp_proto(addr_family); + if (af == LLDPD_AF_UNSPEC) break; + if (iface_subtype == LLDP_MGMT_IFACE_IFINDEX) + iface = iface_number; + else + iface = 0; + mgmt = lldpd_alloc_mgmt(af, addr_ptr, addr_length, iface); + if (mgmt == NULL) { + if (errno == ENOMEM) + log_warn("lldp", + "unable to allocate memory " + "for management address"); + else + log_warn("lldp", + "too large management address " + "received on %s", + hardware->h_ifname); + goto malformed; + } + TAILQ_INSERT_TAIL(&chassis->c_mgmt, mgmt, m_entries); + break; + case LLDP_TLV_ORG: + CHECK_TLV_SIZE(1 + (int)sizeof(orgid), "Organisational"); + PEEK_BYTES(orgid, sizeof(orgid)); + unrecognized = 0; + tlv_subtype = PEEK_UINT8; + if (memcmp(dot1, orgid, sizeof(orgid)) == 0) { +#ifndef ENABLE_DOT1 + unrecognized = 1; +#else + /* Dot1 */ + switch (tlv_subtype) { + case LLDP_TLV_DOT1_VLANNAME: + CHECK_TLV_SIZE(7, "VLAN"); + if ((vlan = (struct lldpd_vlan *)calloc(1, + sizeof(struct lldpd_vlan))) == NULL) { + log_warn("lldp", + "unable to alloc vlan " + "structure for " + "tlv received on %s", + hardware->h_ifname); + goto malformed; + } + vlan->v_vid = PEEK_UINT16; + vlan_len = PEEK_UINT8; + CHECK_TLV_SIZE(7 + vlan_len, "VLAN"); + if ((vlan->v_name = (char *)calloc(1, + vlan_len + 1)) == NULL) { + log_warn("lldp", + "unable to alloc vlan name for " + "tlv received on %s", + hardware->h_ifname); + goto malformed; + } + PEEK_BYTES(vlan->v_name, vlan_len); + TAILQ_INSERT_TAIL(&port->p_vlans, vlan, + v_entries); + vlan = NULL; + break; + case LLDP_TLV_DOT1_PVID: + CHECK_TLV_SIZE(6, "PVID"); + port->p_pvid = PEEK_UINT16; + break; + case LLDP_TLV_DOT1_PPVID: + CHECK_TLV_SIZE(7, "PPVID"); + /* validation needed */ + /* PPVID has to be unique if more than + one PPVID TLVs are received - + discard if duplicate */ + /* if support bit is not set and + enabled bit is set - PPVID TLV is + considered error and discarded */ + /* if PPVID > 4096 - bad and discard */ + if ((ppvid = (struct lldpd_ppvid *)calloc(1, + sizeof(struct lldpd_ppvid))) == NULL) { + log_warn("lldp", + "unable to alloc ppvid " + "structure for " + "tlv received on %s", + hardware->h_ifname); + goto malformed; + } + ppvid->p_cap_status = PEEK_UINT8; + ppvid->p_ppvid = PEEK_UINT16; + TAILQ_INSERT_TAIL(&port->p_ppvids, ppvid, + p_entries); + break; + case LLDP_TLV_DOT1_PI: + /* validation needed */ + /* PI has to be unique if more than + one PI TLVs are received - discard + if duplicate ?? */ + CHECK_TLV_SIZE(5, "PI"); + if ((pi = (struct lldpd_pi *)calloc(1, + sizeof(struct lldpd_pi))) == NULL) { + log_warn("lldp", + "unable to alloc PI " + "structure for " + "tlv received on %s", + hardware->h_ifname); + goto malformed; + } + pi->p_pi_len = PEEK_UINT8; + CHECK_TLV_SIZE(5 + pi->p_pi_len, "PI"); + if ((pi->p_pi = (char *)calloc(1, + pi->p_pi_len)) == NULL) { + log_warn("lldp", + "unable to alloc pid name for " + "tlv received on %s", + hardware->h_ifname); + goto malformed; + } + PEEK_BYTES(pi->p_pi, pi->p_pi_len); + TAILQ_INSERT_TAIL(&port->p_pids, pi, p_entries); + pi = NULL; + break; + default: + /* Unknown Dot1 TLV, ignore it */ + unrecognized = 1; + } +#endif + } else if (memcmp(dot3, orgid, sizeof(orgid)) == 0) { +#ifndef ENABLE_DOT3 + unrecognized = 1; +#else + /* Dot3 */ + switch (tlv_subtype) { + case LLDP_TLV_DOT3_MAC: + CHECK_TLV_SIZE(9, "MAC/PHY"); + port->p_macphy.autoneg_support = PEEK_UINT8; + port->p_macphy.autoneg_enabled = + (port->p_macphy.autoneg_support & 0x2) >> 1; + port->p_macphy.autoneg_support = + port->p_macphy.autoneg_support & 0x1; + port->p_macphy.autoneg_advertised = PEEK_UINT16; + port->p_macphy.mau_type = PEEK_UINT16; + break; + case LLDP_TLV_DOT3_LA: + CHECK_TLV_SIZE(9, "Link aggregation"); + PEEK_DISCARD_UINT8; + port->p_aggregid = PEEK_UINT32; + break; + case LLDP_TLV_DOT3_MFS: + CHECK_TLV_SIZE(6, "MFS"); + port->p_mfs = PEEK_UINT16; + break; + case LLDP_TLV_DOT3_POWER: + CHECK_TLV_SIZE(7, "Power"); + port->p_power.devicetype = PEEK_UINT8; + port->p_power.supported = + (port->p_power.devicetype & 0x2) >> 1; + port->p_power.enabled = + (port->p_power.devicetype & 0x4) >> 2; + port->p_power.paircontrol = + (port->p_power.devicetype & 0x8) >> 3; + port->p_power.devicetype = + (port->p_power.devicetype & 0x1) ? + LLDP_DOT3_POWER_PSE : + LLDP_DOT3_POWER_PD; + port->p_power.pairs = PEEK_UINT8; + port->p_power.class = PEEK_UINT8; + /* 802.3at? */ + if (tlv_size >= 12) { + port->p_power.powertype = PEEK_UINT8; + port->p_power.source = + (port->p_power.powertype & + (1 << 5 | 1 << 4)) >> + 4; + port->p_power.priority = + (port->p_power.powertype & + (1 << 1 | 1 << 0)); + port->p_power.powertype = + (port->p_power.powertype & + (1 << 7)) ? + LLDP_DOT3_POWER_8023AT_TYPE1 : + LLDP_DOT3_POWER_8023AT_TYPE2; + port->p_power.requested = PEEK_UINT16; + port->p_power.allocated = PEEK_UINT16; + } else + port->p_power.powertype = + LLDP_DOT3_POWER_8023AT_OFF; + /* 802.3bt? */ + if (tlv_size >= 29) { + port->p_power.requested_a = PEEK_UINT16; + port->p_power.requested_b = PEEK_UINT16; + port->p_power.allocated_a = PEEK_UINT16; + port->p_power.allocated_b = PEEK_UINT16; + port->p_power.pse_status = PEEK_UINT16; + port->p_power.pd_status = + (port->p_power.pse_status & + (1 << 13 | 1 << 12)) >> + 12; + port->p_power.pse_pairs_ext = + (port->p_power.pse_status & + (1 << 11 | 1 << 10)) >> + 10; + port->p_power.class_a = + (port->p_power.pse_status & + (1 << 9 | 1 << 8 | 1 << 7)) >> + 7; + port->p_power.class_b = + (port->p_power.pse_status & + (1 << 6 | 1 << 5 | 1 << 4)) >> + 4; + port->p_power.class_ext = + (port->p_power.pse_status & 0xf); + port->p_power.pse_status = + (port->p_power.pse_status & + (1 << 15 | 1 << 14)) >> + 14; + port->p_power.type_ext = PEEK_UINT8; + port->p_power.pd_load = + (port->p_power.type_ext & 0x1); + port->p_power.type_ext = + ((port->p_power.type_ext & + (1 << 3 | 1 << 2 | 1 << 1)) + + 1); + port->p_power.pse_max = PEEK_UINT16; + } else { + port->p_power.type_ext = + LLDP_DOT3_POWER_8023BT_OFF; + } + break; + default: + /* Unknown Dot3 TLV, ignore it */ + unrecognized = 1; + } +#endif + } else if (memcmp(med, orgid, sizeof(orgid)) == 0) { + /* LLDP-MED */ +#ifndef ENABLE_LLDPMED + unrecognized = 1; +#else + u_int32_t policy; + unsigned loctype; + unsigned power; + + switch (tlv_subtype) { + case LLDP_TLV_MED_CAP: + CHECK_TLV_SIZE(7, "LLDP-MED capabilities"); + chassis->c_med_cap_available = PEEK_UINT16; + chassis->c_med_type = PEEK_UINT8; + port->p_med_cap_enabled |= LLDP_MED_CAP_CAP; + break; + case LLDP_TLV_MED_POLICY: + CHECK_TLV_SIZE(8, "LLDP-MED policy"); + policy = PEEK_UINT32; + if (((policy >> 24) < 1) || + ((policy >> 24) > LLDP_MED_APPTYPE_LAST)) { + log_info("lldp", + "unknown policy field %d " + "received on %s", + policy, hardware->h_ifname); + break; + } + port->p_med_policy[(policy >> 24) - 1].type = + (policy >> 24); + port->p_med_policy[(policy >> 24) - 1].unknown = + ((policy & 0x800000) != 0); + port->p_med_policy[(policy >> 24) - 1].tagged = + ((policy & 0x400000) != 0); + port->p_med_policy[(policy >> 24) - 1].vid = + (policy & 0x001FFE00) >> 9; + port->p_med_policy[(policy >> 24) - 1] + .priority = (policy & 0x1C0) >> 6; + port->p_med_policy[(policy >> 24) - 1].dscp = + policy & 0x3F; + port->p_med_cap_enabled |= LLDP_MED_CAP_POLICY; + break; + case LLDP_TLV_MED_LOCATION: + CHECK_TLV_SIZE(5, "LLDP-MED Location"); + loctype = PEEK_UINT8; + if ((loctype < 1) || + (loctype > LLDP_MED_LOCFORMAT_LAST)) { + log_info("lldp", + "unknown location type " + "received on %s", + hardware->h_ifname); + break; + } + free(port->p_med_location[loctype - 1].data); + if ((port->p_med_location[loctype - 1].data = + (char *)malloc(tlv_size - 5)) == + NULL) { + log_warn("lldp", + "unable to allocate memory " + "for LLDP-MED location for " + "frame received on %s", + hardware->h_ifname); + goto malformed; + } + PEEK_BYTES( + port->p_med_location[loctype - 1].data, + tlv_size - 5); + port->p_med_location[loctype - 1].data_len = + tlv_size - 5; + port->p_med_location[loctype - 1].format = + loctype; + port->p_med_cap_enabled |= + LLDP_MED_CAP_LOCATION; + break; + case LLDP_TLV_MED_MDI: + CHECK_TLV_SIZE(7, "LLDP-MED PoE-MDI"); + power = PEEK_UINT8; + switch (power & 0xC0) { + case 0x0: + port->p_med_power.devicetype = + LLDP_MED_POW_TYPE_PSE; + port->p_med_cap_enabled |= + LLDP_MED_CAP_MDI_PSE; + switch (power & 0x30) { + case 0x0: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_UNKNOWN; + break; + case 0x10: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_PRIMARY; + break; + case 0x20: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_BACKUP; + break; + default: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_RESERVED; + } + break; + case 0x40: + port->p_med_power.devicetype = + LLDP_MED_POW_TYPE_PD; + port->p_med_cap_enabled |= + LLDP_MED_CAP_MDI_PD; + switch (power & 0x30) { + case 0x0: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_UNKNOWN; + break; + case 0x10: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_PSE; + break; + case 0x20: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_LOCAL; + break; + default: + port->p_med_power.source = + LLDP_MED_POW_SOURCE_BOTH; + } + break; + default: + port->p_med_power.devicetype = + LLDP_MED_POW_TYPE_RESERVED; + } + if ((power & 0x0F) > LLDP_MED_POW_PRIO_LOW) + port->p_med_power.priority = + LLDP_MED_POW_PRIO_UNKNOWN; + else + port->p_med_power.priority = + power & 0x0F; + port->p_med_power.val = PEEK_UINT16; + break; + case LLDP_TLV_MED_IV_HW: + case LLDP_TLV_MED_IV_SW: + case LLDP_TLV_MED_IV_FW: + case LLDP_TLV_MED_IV_SN: + case LLDP_TLV_MED_IV_MANUF: + case LLDP_TLV_MED_IV_MODEL: + case LLDP_TLV_MED_IV_ASSET: + if (tlv_size <= 4) + b = NULL; + else { + if ((b = (char *)malloc( + tlv_size - 3)) == NULL) { + log_warn("lldp", + "unable to allocate " + "memory for LLDP-MED " + "inventory for frame " + "received on %s", + hardware->h_ifname); + goto malformed; + } + PEEK_BYTES(b, tlv_size - 4); + b[tlv_size - 4] = '\0'; + } + switch (tlv_subtype) { + case LLDP_TLV_MED_IV_HW: + free(chassis->c_med_hw); + chassis->c_med_hw = b; + break; + case LLDP_TLV_MED_IV_FW: + free(chassis->c_med_fw); + chassis->c_med_fw = b; + break; + case LLDP_TLV_MED_IV_SW: + free(chassis->c_med_sw); + chassis->c_med_sw = b; + break; + case LLDP_TLV_MED_IV_SN: + free(chassis->c_med_sn); + chassis->c_med_sn = b; + break; + case LLDP_TLV_MED_IV_MANUF: + free(chassis->c_med_manuf); + chassis->c_med_manuf = b; + break; + case LLDP_TLV_MED_IV_MODEL: + free(chassis->c_med_model); + chassis->c_med_model = b; + break; + case LLDP_TLV_MED_IV_ASSET: + free(chassis->c_med_asset); + chassis->c_med_asset = b; + break; + default: + /* unreachable */ + free(b); + break; + } + port->p_med_cap_enabled |= LLDP_MED_CAP_IV; + break; + default: + /* Unknown LLDP MED, ignore it */ + hardware->h_rx_unrecognized_cnt++; + } +#endif /* ENABLE_LLDPMED */ + } else if (memcmp(dcbx, orgid, sizeof(orgid)) == 0) { + log_debug("lldp", + "unsupported DCBX tlv received on %s - ignore", + hardware->h_ifname); + unrecognized = 1; + } else { + log_debug("lldp", + "unknown org tlv [%02x:%02x:%02x] received on %s", + orgid[0], orgid[1], orgid[2], hardware->h_ifname); + unrecognized = 1; + } + if (unrecognized) { + hardware->h_rx_unrecognized_cnt++; +#ifdef ENABLE_CUSTOM + custom = (struct lldpd_custom *)calloc(1, + sizeof(struct lldpd_custom)); + if (!custom) { + log_warn("lldp", + "unable to allocate memory for custom TLV"); + goto malformed; + } + custom->oui_info_len = tlv_size > 4 ? tlv_size - 4 : 0; + memcpy(custom->oui, orgid, sizeof(custom->oui)); + custom->subtype = tlv_subtype; + if (custom->oui_info_len > 0) { + custom->oui_info = malloc(custom->oui_info_len); + if (!custom->oui_info) { + log_warn("lldp", + "unable to allocate memory for custom TLV data"); + goto malformed; + } + PEEK_BYTES(custom->oui_info, + custom->oui_info_len); + } + TAILQ_INSERT_TAIL(&port->p_custom_list, custom, next); + custom = NULL; +#endif + } + break; + default: + log_warnx("lldp", "unknown tlv (%d) received on %s", tlv_type, + hardware->h_ifname); + hardware->h_rx_unrecognized_cnt++; + break; + } + if (pos > tlv + tlv_size) { + log_warnx("lldp", "BUG: already past TLV!"); + goto malformed; + } + PEEK_DISCARD(tlv + tlv_size - pos); + } + + /* Some random check */ + if ((chassis->c_id == NULL) || (port->p_id == NULL) || (!ttl_received) || + (gotend == 0)) { + log_warnx("lldp", + "some mandatory tlv are missing for frame received on %s", + hardware->h_ifname); + goto malformed; + } + *newchassis = chassis; + *newport = port; + return 1; +malformed: +#ifdef ENABLE_CUSTOM + free(custom); +#endif +#ifdef ENABLE_DOT1 + free(vlan); + free(pi); +#endif + lldpd_chassis_cleanup(chassis, 1); + lldpd_port_cleanup(port, 1); + free(port); + return -1; +} |