modules: introduce LLDP support

Interfaces must accept multicast frames:
grcli set interface port <port0> allmulti on

LLDP is configured globally
grcli set lldp [rx|tx|both|off] [ttl 10..600] [sysname Groot ] [sysdesc Router]

grcli show lldp config
grcli show lldp neighbors [iface IFACE] [brief]

Signed-off-by: Christophe Fontaine <[email protected]>

modules/lldp/announce.c

@@ -0,0 +1,203 @@
+// SPDX-License-Identifier: BSD-3-Clause
+// Copyright (c) 2024 Christophe Fontaine
+
+#include "gr_lldp.h"
+#include "lldp.h"
+#include "lldp_priv.h"
+
+#include <gr_eth_input.h>
+#include <gr_eth_output.h>
+#include <gr_graph.h>
+#include <gr_ip4_control.h>
+#include <gr_log.h>
+#include <gr_mbuf.h>
+
+#include <rte_ether.h>
+#include <rte_graph_worker.h>
+#include <rte_malloc.h>
+
+enum {
+	OUTPUT = 0,
+	ERROR,
+	EDGE_COUNT,
+};
+
+static struct rte_ring *lldp_ring;
+static struct rte_mempool *lldp_pool;
+int lldp_output_request_solicit(const struct iface *);
+
+struct gr_lldp_node_data lldp_ctx = {
+	.rx = 1,
+	.tx = 1,
+	.ttl = 60,
+	.sys_name = SYSNAME,
+	.sys_descr = SYSDESC
+};
+
+const struct rte_ether_addr lldp_dst = {
+	.addr_bytes = {0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e},
+};
+
+static uint8_t *lldp_tlv_append(
+	struct rte_mbuf *mbuf,
+	uint8_t type,
+	uint8_t subtype,
+	uint16_t value_len,
+	void *value
+) {
+	uint8_t offset = 2;
+	uint8_t *data = (uint8_t *)
+		rte_pktmbuf_append(mbuf, 2 + value_len + (subtype != T_NO_SUBTYPE ? 1 : 0));
+	if (data) {
+		uint16_t total_len = value_len;
+		if (subtype != T_NO_SUBTYPE) {
+			total_len += 1;
+			offset += 1;
+			data[2] = subtype;
+		}
+		// First 7 bits sets the type, the 9 following bits for the length
+		data[0] = type << 1 | total_len >> 7;
+		data[1] = total_len & 0xff;
+		memcpy(&data[offset], value, value_len);
+	}
+	return data;
+}
+
+int lldp_output_request_solicit(const struct iface *iface) {
+	int ret;
+	if (iface == NULL)
+		return errno_set(EINVAL);
+	ret = rte_ring_enqueue(lldp_ring, (void *)iface);
+	if (ret < 0) {
+		return errno_set(-ret);
+	}
+	return 0;
+}
+
+static bool build_lldp_frame(struct rte_mbuf *mbuf, const struct iface *iface) {
+	struct lldp_ip4 lldp_ip4 = {.afi = AFI_IP_4};
+	uint16_t ttl_value = rte_cpu_to_be_16(lldp_ctx.ttl);
+	struct rte_ether_addr src_addr;
+	struct nexthop *local_ip;
+
+	iface_get_eth_addr(iface->id, &src_addr);
+	rte_pktmbuf_reset(mbuf);
+
+	lldp_tlv_append(
+		mbuf, T_CHASSIS_ID, T_CHASSIS_MAC_ADDRESS, RTE_ETHER_ADDR_LEN, src_addr.addr_bytes
+	);
+
+	lldp_tlv_append(
+		mbuf, T_CHASSIS_ID, T_CHASSIS_IF_ALIAS, strlen(lldp_ctx.sys_name), lldp_ctx.sys_name
+	);
+	lldp_tlv_append(
+		mbuf, T_PORT_ID, T_PORT_MAC_ADDRESS, RTE_ETHER_ADDR_LEN, src_addr.addr_bytes
+	);
+	lldp_tlv_append(mbuf, T_PORT_ID, T_PORT_IF_ALIAS, strlen(iface->name), iface->name);
+	lldp_tlv_append(mbuf, T_TTL, T_NO_SUBTYPE, sizeof(ttl_value), &ttl_value);
+	lldp_tlv_append(mbuf, T_PORT_DESC, T_NO_SUBTYPE, strlen(iface->name), iface->name);
+	lldp_tlv_append(
+		mbuf, T_SYSTEM_NAME, T_NO_SUBTYPE, strlen(lldp_ctx.sys_name), lldp_ctx.sys_name
+	);
+	lldp_tlv_append(
+		mbuf, T_SYSTEM_DESC, T_NO_SUBTYPE, strlen(lldp_ctx.sys_descr), lldp_ctx.sys_descr
+	);
+
+	local_ip = ip4_addr_get(iface->id);
+	if (local_ip) {
+		lldp_ip4.ip4_addr = local_ip->ip;
+		lldp_tlv_append(
+			mbuf,
+			T_CHASSIS_ID,
+			T_CHASSIS_NET_ADDRESS,
+			sizeof(struct lldp_ip4),
+			&lldp_ip4
+		);
+	}
+	// TOOD: ip6_addr_get(iface->id)
+
+	return lldp_tlv_append(mbuf, T_END, T_NO_SUBTYPE, 0, NULL) != NULL;
+}
+
+uint64_t last_sent = 0;
+
+static uint16_t
+lldp_announce_process(struct rte_graph *graph, struct rte_node *node, void **, uint16_t) {
+	struct eth_output_mbuf_data *eth_data;
+	const struct iface *iface;
+	struct rte_mbuf *mbuf;
+	uint16_t sent;
+	uint64_t now;
+	uint16_t n;
+
+	sent = 0;
+
+	now = rte_get_tsc_cycles();
+	n = rte_ring_dequeue_burst(lldp_ring, node->objs, RTE_GRAPH_BURST_SIZE, NULL);
+
+	for (unsigned i = 0; i < n; i++) {
+		iface = node->objs[i];
+		mbuf = rte_pktmbuf_alloc(lldp_pool);
+
+		if (build_lldp_frame(mbuf, iface) == false) {
+			rte_node_enqueue_x1(graph, node, ERROR, mbuf);
+		}
+
+		// Prepare ethernet layer info.
+		eth_data = eth_output_mbuf_data(mbuf);
+		rte_ether_addr_copy(&lldp_dst, &eth_data->dst);
+		eth_data->ether_type = rte_cpu_to_be_16(RTE_ETHER_TYPE_LLDP);
+		eth_data->iface = iface;
+
+		rte_node_enqueue_x1(graph, node, OUTPUT, mbuf);
+		sent++;
+	}
+	last_sent = now;
+
+	return sent;
+}
+
+static void lldp_announce_register(void) {
+	lldp_ring = rte_ring_create(
+		"lldp_announce",
+		RTE_GRAPH_BURST_SIZE,
+		SOCKET_ID_ANY,
+		RING_F_MP_RTS_ENQ | RING_F_MC_RTS_DEQ
+	);
+	if (lldp_ring == NULL)
+		ABORT("rte_ring_create(arp_output_request): %s", rte_strerror(rte_errno));
+
+	lldp_pool = rte_pktmbuf_pool_create(
+		"lldp_announce",
+		RTE_GRAPH_BURST_SIZE,
+		64, // cache_size
+		0, // priv_size
+		RTE_MBUF_DEFAULT_BUF_SIZE,
+		SOCKET_ID_ANY
+	);
+	if (lldp_pool == NULL)
+		ABORT("rte_pktmbuf_pool_create(lldp_announce): %s", rte_strerror(rte_errno));
+}
+
+static void lldp_announce_unregister(void) {
+	rte_ring_free(lldp_ring);
+	rte_mempool_free(lldp_pool);
+}
+
+static struct rte_node_register lldp_announce_node = {
+	.flags = RTE_NODE_SOURCE_F,
+	.name = "lldp_announce",
+	.process = lldp_announce_process,
+	.nb_edges = EDGE_COUNT,
+	.next_nodes = {[OUTPUT] = "eth_output", [ERROR] = "lldp_announce_error"},
+};
+
+static struct gr_node_info info = {
+	.node = &lldp_announce_node,
+	.register_callback = lldp_announce_register,
+	.unregister_callback = lldp_announce_unregister,
+};
+
+GR_NODE_REGISTER(info);
+
+GR_DROP_REGISTER(lldp_announce_error);