diff --git a/rtl837x_lldp.c b/rtl837x_lldp.c index 4ea73156..ebf8387b 100644 --- a/rtl837x_lldp.c +++ b/rtl837x_lldp.c @@ -63,6 +63,7 @@ struct lldp_pkt { static void lldp_send_ports(uint16_t ttl, uint16_t ports) __reentrant { __xdata uint8_t *p; + __xdata uint8_t *tail; uint8_t port_position; lldp_set_addresses(); @@ -120,25 +121,7 @@ static void lldp_send_ports(uint16_t ttl, uint16_t ports) __reentrant *p++ = 0; //OID length } - // End of LLDPDU TLV - *p++ = 0x00; //padding - *p++ = 0x00; //padding - - while (p < (LLDP_O->payload + LLDP_MIN_ETHERNET_PAYLOAD_LENGTH)) - *p++ = 0x00; - - /* - * uip_len is the Ethernet frame length excluding FCS. - * - * The frame consists of: - * - * dst mac - * src mac - * rtl_tag sizeof(struct rtl_tag) - * EtherType (2 bytes) - * payload (how far we've moved p pointer) - */ - uip_len = MAC_ADDR_LEN + MAC_ADDR_LEN + sizeof(struct rtl_tag) + LLDP_ETHERTYPE_LENGTH + (p - LLDP_O->payload); + tail = p; for (uint8_t port = MACHINE_MIN_PORT; port <= MACHINE_MAX_PORT; port++) { @@ -146,6 +129,29 @@ static void lldp_send_ports(uint16_t ttl, uint16_t ports) __reentrant LLDP_O->payload[port_position] = '0' + machine.log_to_phys_port[port]; LLDP_O->rtl_tag.pmask = HTONS((uint16_t)1 << port); + p = tail; + p += lldp_portdesc(p, port); + + // End of LLDPDU TLV + *p++ = 0x00; //padding + *p++ = 0x00; //padding + + while (p < (LLDP_O->payload + LLDP_MIN_ETHERNET_PAYLOAD_LENGTH)) + *p++ = 0x00; + + /* + * uip_len is the Ethernet frame length excluding FCS. + * + * The frame consists of: + * + * dst mac + * src mac + * rtl_tag sizeof(struct rtl_tag) + * EtherType (2 bytes) + * payload (how far we've moved p pointer) + */ + uip_len = MAC_ADDR_LEN + MAC_ADDR_LEN + sizeof(struct rtl_tag) + LLDP_ETHERTYPE_LENGTH + (p - LLDP_O->payload); + tcpip_output(); } } @@ -205,3 +211,15 @@ uint8_t lldp_sysdesc(__xdata uint8_t *p) __banked memcpyc(p, VERSION_SW, version_length); return machine_name_length + 1 + version_length + 2; } + +uint8_t lldp_portdesc(__xdata uint8_t *p, uint8_t port) __banked +{ + uint8_t name_length = strlen_x(port_names[port]); + + if (!name_length) + return 0; + *p++ = LLDP_TYPE(LLDP_PORT_DESC_TLV_TYPE); + *p++ = name_length; + memcpy(p, port_names[port], name_length); + return name_length + 2; +} diff --git a/rtl837x_lldp.h b/rtl837x_lldp.h index e79143c3..f7b86615 100644 --- a/rtl837x_lldp.h +++ b/rtl837x_lldp.h @@ -24,6 +24,7 @@ #define LLDP_CHASSIS_ID_TLV_LENGTH 7 #define LLDP_CHASSIS_ID_TLV_SUBTYPE 4 +#define LLDP_PORT_DESC_TLV_TYPE 4 #define LLDP_SYSNAME_TLV_TYPE 5 #define LLDP_SYSDESC_TLV_TYPE 6 @@ -47,5 +48,6 @@ void lldp_set_addresses(void) __banked; void lldp_set_rtl_wrapper(void) __banked; uint8_t lldp_sysname(__xdata uint8_t *p) __banked; uint8_t lldp_sysdesc(__xdata uint8_t *p) __banked; +uint8_t lldp_portdesc(__xdata uint8_t *p, uint8_t port) __banked; #endif