diff --git a/Makefile b/Makefile index bb914904..15d3f10c 100644 --- a/Makefile +++ b/Makefile @@ -83,6 +83,7 @@ SRCS += \ rtl837x_pins.c\ rtl837x_port.c \ rtl837x_stp.c \ + rtl837x_lldp.c \ rtl837x_storm.c SRCS += \ httpd/httpd.c \ diff --git a/cmd_parser.c b/cmd_parser.c index c493c9e9..de7e1d49 100644 --- a/cmd_parser.c +++ b/cmd_parser.c @@ -16,6 +16,7 @@ #include "rtl837x_stp.h" #include "rtl837x_igmp.h" #include "rtl837x_bandwidth.h" +#include "rtl837x_lldp.h" #include "rtl837x_storm.h" #include "sfp.h" #include "dhcp.h" @@ -32,6 +33,7 @@ extern __code const struct machine machine; extern __xdata bool stp_enabled; extern __code const uint8_t log_to_phys_port[9]; +extern __xdata uint8_t lldp_enabled; extern volatile __xdata uint32_t ticks; extern volatile __xdata uint8_t sfr_data[4]; @@ -2024,6 +2026,14 @@ void cmd_parser(void) __banked parse_ingress(); } else if (cmd_compare(0, "session")) { parse_session(); + } else if (cmd_compare(0, "lldp")) { + if (cmd_compare(1, "on")) { + print_string("LLDP enabled\n"); + lldp_enabled = 1; + } else { + print_string("LLDP disabled\n"); + lldp_enabled = 0; + } } else { cmd_error("Unknown command\n"); diff --git a/cmd_parser.h b/cmd_parser.h index f96c72ae..2359eb16 100644 --- a/cmd_parser.h +++ b/cmd_parser.h @@ -20,6 +20,7 @@ extern __xdata uint8_t err_status; #define HEALTH_PH_STP 4 #define HEALTH_PH_CMD 5 #define HEALTH_PHASES 6 +#define HEALTH_LLDP 7 extern __xdata uint16_t health_rx_frames; #define health_rx_frame() health_rx_frames++ diff --git a/rtl837x_lldp.c b/rtl837x_lldp.c new file mode 100644 index 00000000..24e774c6 --- /dev/null +++ b/rtl837x_lldp.c @@ -0,0 +1,181 @@ +#pragma codeseg BANK2 +#pragma constseg BANK2 + +#include "rtl837x_common.h" +#include "rtl837x_lldp.h" +#include "rtl837x_sfr.h" +#include "rtl837x_common.h" +#include "dhcp.h" +#include "uip.h" +#include "uip/uip.h" +#include "machine.h" + +extern __xdata uint8_t lldp_enabled; +extern __code const struct machine machine; + +uint8_t lldp_seconds; + +void lldp_init(void) __banked +{ + lldp_seconds = 0; +} + +void lldp_tick(void) __banked +{ + lldp_seconds++; + + if (lldp_seconds < LLDP_TX_INTERVAL_SEC) + return; + + lldp_seconds = 0; + + if(lldp_enabled == 1) + lldp_send(); +} + +//Outgoing LLDP packet. + +struct lldp_pkt { + struct uip_eth_addr dst; + struct uip_eth_addr src; + struct rtl_tag rtl_tag; + uint16_t ether_type; + + uint8_t payload[64]; +}; + +#define LLDP_O ((__xdata struct lldp_pkt *)&uip_buf[RTL_FRAME_DESC_SIZE]) + +// The type is 7 bits and the length is 9 bits +// we can represent this as the 7 most significant +// bits of the type bit and leave the least significant +// bit as 0 always as we're not going to need 2^9 for +// the length at any point +#define LLDP_TYPE(value) (value << 1) + +void lldp_send(void) __banked __reentrant +{ + uint8_t *p; + uint16_t len = 0; + uint8_t port_position; + + lldp_set_addresses(); + lldp_set_rtl_wrapper(); + + p = LLDP_O->payload; + + //Chassis ID + p[len++] = LLDP_TYPE(LLDP_CHASSIS_ID_TLV_TYPE); + p[len++] = LLDP_CHASSIS_ID_TLV_LENGTH; + p[len++] = LLDP_CHASSIS_ID_TLV_SUBTYPE; + + for (uint8_t i = 0; i < LLDP_MAC_ADDR_LEN; i++) + p[len + i] = uip_ethaddr.addr[i]; + + len += LLDP_MAC_ADDR_LEN; + + //Port ID + p[len++] = LLDP_TYPE(LLDP_PORT_ID_TLV_TYPE); + p[len++] = LLDP_PORT_ID_TLV_LENGTH; + p[len++] = LLDP_PORT_ID_TLV_SUBTYPE; + port_position = len; + p[len++] = '0'; // filled in per port below + + //TTL + p[len++] = LLDP_TYPE(LLDP_TTL_TLV_TYPE); + p[len++] = LLDP_TTL_TLV_LENGTH; + p[len++] = 0x00; //padding + p[len++] = LLDP_TTL_TTL_SECONDS; + + lldp_sysname(p, &len); + lldp_sysdesc(p, &len); + + // End of LLDPDU TLV + p[len++] = 0x00; //padding + p[len++] = 0x00; //padding + + while (len < LLDP_MIN_ETHERNET_PAYLOAD_LENGTH) + p[len++] = 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 len + */ + uip_len = LLDP_MAC_ADDR_LEN + LLDP_MAC_ADDR_LEN + sizeof(struct rtl_tag) + LLDP_ETHERTYPE_LENGTH + len; + + for (uint8_t port = machine.min_port; port <= machine.max_port; port++) { + + LLDP_O->payload[port_position] = '1' + port; + LLDP_O->rtl_tag.pmask = HTONS((uint16_t)1 << port); + + tcpip_output(); + } +} + +void lldp_set_addresses(void) __banked { + // LLDP destination multicast addr: 01:80:c2:00:00:0e + LLDP_O->dst.addr[0] = 0x01; + LLDP_O->dst.addr[1] = 0x80; + LLDP_O->dst.addr[2] = 0xc2; + LLDP_O->dst.addr[3] = 0x00; + LLDP_O->dst.addr[4] = 0x00; + LLDP_O->dst.addr[5] = 0x0e; + + for (uint8_t i = 0; i < LLDP_MAC_ADDR_LEN; i++) + LLDP_O->src.addr[i] = uip_ethaddr.addr[i]; +} + +void lldp_set_rtl_wrapper(void) __banked { + /* + * This is the RTL CPU tag, not the Ethernet EtherType. + * port 0 -> 0x0001 + * port 1 -> 0x0002 + * port 2 -> 0x0004 + */ + LLDP_O->rtl_tag.tag = HTONS(RTL_FRAME_TAG_ID); + LLDP_O->rtl_tag.version = RTL_FRAME_TAG_VERSION; + LLDP_O->rtl_tag.reason = 0x00; + LLDP_O->rtl_tag.flags = HTONS(RTL_TAG_LEARN_DIS); + + LLDP_O->ether_type = HTONS(LLDP_ETHERTYPE); +} + +void lldp_sysname(uint8_t *p, uint16_t *len) __banked +{ + uint16_t hostname_length = 0; + __xdata char *hp = hostname; + + p[(*len)++] = LLDP_TYPE(LLDP_SYSNAME_TLV_TYPE); + p[(*len)++] = 0; + + while (*hp) + { + hostname_length++; + p[(*len)++] = *hp++; + } + p[(*len)-hostname_length-1] = hostname_length; + +} + +void lldp_sysdesc(uint8_t *p, uint16_t *len) __banked +{ + uint16_t machine_name_length = 0; + const char *mp = machine.machine_name; + + p[(*len)++] = LLDP_TYPE(LLDP_SYSDESC_TLV_TYPE); + p[(*len)++] = 0; + + while (*mp) + { + machine_name_length++; + p[(*len)++] = *mp++; + } + p[(*len)-machine_name_length-1] = machine_name_length; +} \ No newline at end of file diff --git a/rtl837x_lldp.h b/rtl837x_lldp.h new file mode 100644 index 00000000..038d005d --- /dev/null +++ b/rtl837x_lldp.h @@ -0,0 +1,36 @@ +#ifndef _LLDP_H_ +#define _LLDP_H_ + +#include + +#define LLDP_MIN_ETHERNET_PAYLOAD_LENGTH 46 +#define LLDP_ETHERTYPE 0x88cc +#define LLDP_ETHERTYPE_LENGTH 2 +#define LLDP_MAX_FRAME 256 +#define LLDP_MAC_ADDR_LEN 6 +#define LLDP_TX_INTERVAL_SEC 30 + +#define LLDP_PORT_ID_TLV_TYPE 2 +#define LLDP_PORT_ID_TLV_LENGTH 0x02 +#define LLDP_PORT_ID_TLV_SUBTYPE 0x07 + +#define LLDP_TTL_TLV_TYPE 3 +#define LLDP_TTL_TLV_LENGTH 2 +#define LLDP_TTL_TTL_SECONDS 120 + +#define LLDP_CHASSIS_ID_TLV_TYPE 1 +#define LLDP_CHASSIS_ID_TLV_LENGTH 7 +#define LLDP_CHASSIS_ID_TLV_SUBTYPE 4 + +#define LLDP_SYSNAME_TLV_TYPE 5 +#define LLDP_SYSDESC_TLV_TYPE 6 + +void lldp_init(void) __banked; +void lldp_tick(void) __banked; +void lldp_send(void) __banked; +void lldp_set_addresses(void) __banked; +void lldp_set_rtl_wrapper(void) __banked; +void lldp_sysname(uint8_t *p, uint16_t *len) __banked; +void lldp_sysdesc(uint8_t *p, uint16_t *len) __banked; + +#endif diff --git a/rtlplayground.c b/rtlplayground.c index 916984f1..738b6ab2 100644 --- a/rtlplayground.c +++ b/rtlplayground.c @@ -16,6 +16,7 @@ #include "rtl837x_leds.h" #include "rtl837x_bandwidth.h" #include "rtl837x_init.h" +#include "rtl837x_lldp.h" #include "dhcp.h" #include "cmd_parser.h" #include "cmd_editor.h" @@ -127,6 +128,7 @@ __xdata uint8_t tx_seq; __xdata bool stp_enabled; __xdata uint8_t igmpEnabled; +__xdata uint8_t lldp_enabled; __xdata char hostname[24]; /* device hostname, default set at boot, see rtl837x_common.h */ __code const uint16_t bit_mask[16] = { @@ -1066,6 +1068,13 @@ void handle_rx(void) if (uip_len) { tcpip_output(); } + } else if (lldp_enabled && uip_buf[0] == 0x01 && uip_buf[1] == 0x80 && uip_buf[2] == 0xc2 + && uip_buf[3] == 0x00 && uip_buf[4] == 0x00 && uip_buf[5] == 0x0e && uip_buf[12] == 0x88 + && uip_buf[13] == 0xcc) { //LLDP Mac + EtherType check + // LLDP packets shouldn't be passed elsewhere + if(uip_len){ + print_string("LLDP received\n"); + } } else if (ETH_IN->ether_type == HTONS(0x0806)) { // ARP uip_arp_arpin(); if (uip_len) { @@ -1286,6 +1295,8 @@ static void handle_tick(void) } } health_phase(HEALTH_PH_STP); + lldp_tick(); + health_phase(HEALTH_LLDP); } @@ -1794,6 +1805,7 @@ void main(void) uip_init(); uip_arp_init(); httpd_init(); + lldp_init(); management_vlan = 1; // Default management VLAN is 1