From cd5de7a2ea1321b760f92d36aaa5a394d3069705 Mon Sep 17 00:00:00 2001 From: Alexandru Ardelean Date: Wed, 15 Apr 2015 10:19:44 +0300 Subject: [PATCH] daemon: add basic custom TLV send & receive Signed-off-by: Alexandru Ardelean --- src/daemon/lldpd.c | 1 + src/daemon/protocols/lldp.c | 22 ++++++++++++++++++++++ src/lldp-const.h | 2 ++ src/lldpd-structs.c | 14 ++++++++++++++ src/lldpd-structs.h | 19 +++++++++++++++++++ 5 files changed, 58 insertions(+) diff --git a/src/daemon/lldpd.c b/src/daemon/lldpd.c index 8ba20710..782e9232 100644 --- a/src/daemon/lldpd.c +++ b/src/daemon/lldpd.c @@ -169,6 +169,7 @@ lldpd_alloc_hardware(struct lldpd *cfg, char *name, int index) TAILQ_INIT(&hardware->h_lport.p_ppvids); TAILQ_INIT(&hardware->h_lport.p_pids); #endif + TAILQ_INIT(&hardware->h_lport.p_custom_list); levent_hardware_init(hardware); return hardware; diff --git a/src/daemon/protocols/lldp.c b/src/daemon/protocols/lldp.c index 18055995..c4a30a41 100644 --- a/src/daemon/protocols/lldp.c +++ b/src/daemon/protocols/lldp.c @@ -84,6 +84,7 @@ static int _lldp_send(struct lldpd *global, int i; const u_int8_t med[] = LLDP_TLV_ORG_MED; #endif + struct lldpd_custom *custom; port = &hardware->h_lport; chassis = port->p_chassis; length = hardware->h_mtu; @@ -431,6 +432,16 @@ static int _lldp_send(struct lldpd *global, } #endif + 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; + } + end: /* END */ if (!( @@ -579,6 +590,7 @@ lldp_decode(struct lldpd *cfg, char *frame, int s, u_int8_t addr_str_length, addr_str_buffer[32]; u_int8_t addr_family, addr_length, *addr_ptr, iface_subtype; u_int32_t iface_number, iface; + struct lldpd_custom *custom; log_debug("lldp", "receive LLDP PDU on %s", hardware->h_ifname); @@ -598,6 +610,7 @@ lldp_decode(struct lldpd *cfg, char *frame, int s, TAILQ_INIT(&port->p_ppvids); TAILQ_INIT(&port->p_pids); #endif + TAILQ_INIT(&port->p_custom_list); length = s; pos = (u_int8_t*)frame; @@ -1078,6 +1091,15 @@ lldp_decode(struct lldpd *cfg, char *frame, int s, orgid[0], orgid[1], orgid[2], hardware->h_ifname); hardware->h_rx_unrecognized_cnt++; + custom = (struct lldpd_custom*)calloc(1, sizeof(struct lldpd_custom)); + if (!custom) + return ENOMEM; + 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) + PEEK_BYTES(custom->oui_info, custom->oui_info_len); + TAILQ_INSERT_TAIL(&port->p_custom_list, custom, next); } break; default: diff --git a/src/lldp-const.h b/src/lldp-const.h index 81a227a4..509b08fd 100644 --- a/src/lldp-const.h +++ b/src/lldp-const.h @@ -24,6 +24,8 @@ */ #define LLDP_TLV_ORG 127 +#define LLDP_TLV_ORG_OUI_LEN 3 +#define LLDP_TLV_ORG_OUI_INFO_MAXLEN 507 /* Chassis ID subtype */ #define LLDP_CHASSISID_SUBTYPE_CHASSIS 1 diff --git a/src/lldpd-structs.c b/src/lldpd-structs.c index e0642877..f713abdf 100644 --- a/src/lldpd-structs.c +++ b/src/lldpd-structs.c @@ -103,6 +103,19 @@ lldpd_pi_cleanup(struct lldpd_port *port) } #endif +void +lldpd_custom_list_cleanup(struct lldpd_port *port) +{ + struct lldpd_custom *custom, *custom_next; + for (custom = TAILQ_FIRST(&port->p_custom_list); + custom != NULL; + custom = custom_next) { + custom_next = TAILQ_NEXT(custom, next); + free(custom); + } + TAILQ_INIT(&port->p_custom_list); +} + /* Cleanup a remote port. The before last argument, `expire` is a function that * should be called when a remote port is removed. If the last argument is 1, * all remote ports are removed. @@ -171,6 +184,7 @@ lldpd_port_cleanup(struct lldpd_port *port, int all) port->p_chassis->c_refcount--; port->p_chassis = NULL; } + lldpd_custom_list_cleanup(port); } } diff --git a/src/lldpd-structs.h b/src/lldpd-structs.h index 43795de7..8c91d2d1 100644 --- a/src/lldpd-structs.h +++ b/src/lldpd-structs.h @@ -214,6 +214,22 @@ MARSHAL_STR(lldpd_chassis, c_med_asset) #endif MARSHAL_END(lldpd_chassis); +/* Custom TLV struct as defined on page 35 of IEEE 802.1AB-2005 */ +struct lldpd_custom { + TAILQ_ENTRY(lldpd_custom) next; /* Pointer to next custom TLV */ + + /* Organizationally Unique Identifier */ + u_int8_t oui[LLDP_TLV_ORG_OUI_LEN]; + /* Organizationally Defined Subtype */ + u_int8_t subtype; + /* Organizationally Defined Information String; for now/simplicity static array */ + u_int8_t oui_info[LLDP_TLV_ORG_OUI_INFO_MAXLEN]; + /* Organizationally Defined Information String length */ + int oui_info_len; +}; +MARSHAL_BEGIN(lldpd_custom) +MARSHAL_TQE(lldpd_custom, next) +MARSHAL_END(lldpd_custom); struct lldpd_port { TAILQ_ENTRY(lldpd_port) p_entries; @@ -253,6 +269,7 @@ struct lldpd_port { TAILQ_HEAD(, lldpd_ppvid) p_ppvids; TAILQ_HEAD(, lldpd_pi) p_pids; #endif + TAILQ_HEAD(, lldpd_custom) p_custom_list; }; MARSHAL_BEGIN(lldpd_port) MARSHAL_TQE(lldpd_port, p_entries) @@ -270,6 +287,7 @@ MARSHAL_SUBTQ(lldpd_port, lldpd_vlan, p_vlans) MARSHAL_SUBTQ(lldpd_port, lldpd_ppvid, p_ppvids) MARSHAL_SUBTQ(lldpd_port, lldpd_pi, p_pids) #endif +MARSHAL_SUBTQ(lldpd_port, lldpd_custom, p_custom_list) MARSHAL_END(lldpd_port); /* Used to modify some port related settings */ @@ -483,5 +501,6 @@ void lldpd_ppvid_cleanup(struct lldpd_port *); void lldpd_vlan_cleanup(struct lldpd_port *); void lldpd_pi_cleanup(struct lldpd_port *); #endif +void lldpd_custom_list_cleanup(struct lldpd_port *); #endif -- 2.39.5