zephyr/subsys/net/l2/ethernet/lldp/lldp.c

/** @file
 * @brief LLDP related functions
 */

/*
 * Copyright (c) 2018 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(net_lldp, CONFIG_NET_LLDP_LOG_LEVEL);

#include <errno.h>
#include <stdlib.h>

#include <zephyr/net/net_core.h>
#include <zephyr/net/ethernet.h>
#include <zephyr/net/net_mgmt.h>
#include <zephyr/net/lldp.h>

static struct net_mgmt_event_callback cb;

/* Have only one timer in order to save memory */
static struct k_work_delayable lldp_tx_timer;

/* Track currently active timers */
static sys_slist_t lldp_ifaces;

#define BUF_ALLOC_TIMEOUT K_MSEC(50)

static int lldp_find(struct ethernet_context *ctx, struct net_if *iface)
{
	int i, found = -1;

	for (i = 0; i < ARRAY_SIZE(ctx->lldp); i++) {
		if (ctx->lldp[i].iface == iface) {
			return i;
		}

		if (found < 0 && ctx->lldp[i].iface == NULL) {
			found = i;
		}
	}

	if (found >= 0) {
		ctx->lldp[found].iface = iface;
		return found;
	}

	return -ENOENT;
}

static void lldp_submit_work(uint32_t timeout)
{
	k_work_cancel_delayable(&lldp_tx_timer);
	k_work_reschedule(&lldp_tx_timer, K_MSEC(timeout));

	NET_DBG("Next wakeup in %d ms",
		k_ticks_to_ms_ceil32(
			k_work_delayable_remaining_get(&lldp_tx_timer)));
}

static bool lldp_check_timeout(int64_t start, uint32_t time, int64_t timeout)
{
	start += time;
	start = llabs(start);

	if (start > timeout) {
		return false;
	}

	return true;
}

static bool lldp_timedout(struct ethernet_lldp *lldp, int64_t timeout)
{
	return lldp_check_timeout(lldp->tx_timer_start,
				  lldp->tx_timer_timeout,
				  timeout);
}

static int lldp_send(struct ethernet_lldp *lldp)
{
	static const struct net_eth_addr lldp_multicast_eth_addr = {
		{ 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e }
	};
	int ret = 0;
	struct net_pkt *pkt;
	size_t len;

	if (!lldp->lldpdu) {
		/* The ethernet driver has not set the lldpdu pointer */
		NET_DBG("The LLDPDU is not set for lldp %p", lldp);
		ret = -EINVAL;
		goto out;
	}

	if (lldp->optional_du && lldp->optional_len) {
		len = sizeof(struct net_lldpdu) + lldp->optional_len;
	} else {
		len = sizeof(struct net_lldpdu);
	}

	if (IS_ENABLED(CONFIG_NET_LLDP_END_LLDPDU_TLV_ENABLED)) {
		len += sizeof(uint16_t);
	}

	pkt = net_pkt_alloc_with_buffer(lldp->iface, len, AF_UNSPEC, 0,
					BUF_ALLOC_TIMEOUT);
	if (!pkt) {
		ret = -ENOMEM;
		goto out;
	}

	net_pkt_set_lldp(pkt, true);
	net_pkt_set_ll_proto_type(pkt, NET_ETH_PTYPE_LLDP);

	ret = net_pkt_write(pkt, (uint8_t *)lldp->lldpdu,
			    sizeof(struct net_lldpdu));
	if (ret < 0) {
		net_pkt_unref(pkt);
		goto out;
	}

	if (lldp->optional_du && lldp->optional_len) {
		ret = net_pkt_write(pkt, (uint8_t *)lldp->optional_du,
				    lldp->optional_len);
		if (ret < 0) {
			net_pkt_unref(pkt);
			goto out;
		}
	}

	if (IS_ENABLED(CONFIG_NET_LLDP_END_LLDPDU_TLV_ENABLED)) {
		uint16_t tlv_end = htons(NET_LLDP_END_LLDPDU_VALUE);

		ret = net_pkt_write(pkt, (uint8_t *)&tlv_end, sizeof(tlv_end));
		if (ret < 0) {
			net_pkt_unref(pkt);
			goto out;
		}
	}

	net_pkt_lladdr_src(pkt)->addr = net_if_get_link_addr(lldp->iface)->addr;
	net_pkt_lladdr_src(pkt)->len = sizeof(struct net_eth_addr);
	net_pkt_lladdr_dst(pkt)->addr = (uint8_t *)lldp_multicast_eth_addr.addr;
	net_pkt_lladdr_dst(pkt)->len = sizeof(struct net_eth_addr);

	if (net_if_send_data(lldp->iface, pkt) == NET_DROP) {
		net_pkt_unref(pkt);
		ret = -EIO;
	}

out:
	lldp->tx_timer_start = k_uptime_get();

	return ret;
}

static uint32_t lldp_manage_timeouts(struct ethernet_lldp *lldp, int64_t timeout)
{
	int32_t next_timeout;

	if (lldp_timedout(lldp, timeout)) {
		lldp_send(lldp);
	}

	next_timeout = timeout - (lldp->tx_timer_start +
				  lldp->tx_timer_timeout);

	return abs(next_timeout);
}

static void lldp_tx_timeout(struct k_work *work)
{
	uint32_t timeout_update = UINT32_MAX - 1;
	int64_t timeout = k_uptime_get();
	struct ethernet_lldp *current, *next;

	ARG_UNUSED(work);

	SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&lldp_ifaces, current, next, node) {
		uint32_t next_timeout;

		next_timeout = lldp_manage_timeouts(current, timeout);
		if (next_timeout < timeout_update) {
			timeout_update = next_timeout;
		}
	}

	if (timeout_update < (UINT32_MAX - 1)) {
		NET_DBG("Waiting for %u ms", timeout_update);

		k_work_reschedule(&lldp_tx_timer, K_MSEC(timeout_update));
	}
}

static void lldp_start_timer(struct ethernet_context *ctx,
			     struct net_if *iface,
			     int slot)
{
	/* exit if started */
	if (ctx->lldp[slot].tx_timer_start != 0) {
		return;
	}

	ctx->lldp[slot].iface = iface;

	sys_slist_append(&lldp_ifaces, &ctx->lldp[slot].node);

	ctx->lldp[slot].tx_timer_start = k_uptime_get();
	ctx->lldp[slot].tx_timer_timeout =
				CONFIG_NET_LLDP_TX_INTERVAL * MSEC_PER_SEC;

	lldp_submit_work(ctx->lldp[slot].tx_timer_timeout);
}

static int lldp_check_iface(struct net_if *iface)
{
	if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
		return -ENOENT;
	}

	if (!(net_eth_get_hw_capabilities(iface) & ETHERNET_LLDP)) {
		return -ESRCH;
	}

	return 0;
}

static int lldp_start(struct net_if *iface, uint32_t mgmt_event)
{
	struct ethernet_context *ctx;
	int ret, slot;

	ret = lldp_check_iface(iface);
	if (ret < 0) {
		return ret;
	}

	ctx = net_if_l2_data(iface);

	ret = lldp_find(ctx, iface);
	if (ret < 0) {
		return ret;
	}

	slot = ret;

	if (mgmt_event == NET_EVENT_IF_DOWN) {
		if (sys_slist_find_and_remove(&lldp_ifaces,
					      &ctx->lldp[slot].node)) {
			ctx->lldp[slot].tx_timer_start = 0;
		}

		if (sys_slist_is_empty(&lldp_ifaces)) {
			k_work_cancel_delayable(&lldp_tx_timer);
		}
	} else if (mgmt_event == NET_EVENT_IF_UP) {
		NET_DBG("Starting timer for iface %p", iface);
		lldp_start_timer(ctx, iface, slot);
	}

	return 0;
}

static enum net_verdict net_lldp_recv(struct net_if *iface, uint16_t ptype, struct net_pkt *pkt)
{
	struct ethernet_context *ctx;
	net_lldp_recv_cb_t recv_cb;
	int ret;

	ARG_UNUSED(ptype);

	if (!net_eth_is_addr_lldp_multicast(
		    (struct net_eth_addr *)net_pkt_lladdr_dst(pkt)->addr)) {
		return NET_DROP;
	}

	ret = lldp_check_iface(iface);
	if (ret < 0) {
		return NET_DROP;
	}

	ctx = net_if_l2_data(iface);

	ret = lldp_find(ctx, iface);
	if (ret < 0) {
		return NET_DROP;
	}

	recv_cb = ctx->lldp[ret].cb;
	if (recv_cb) {
		return recv_cb(iface, pkt);
	}

	return NET_DROP;
}

ETH_NET_L3_REGISTER(LLDP, NET_ETH_PTYPE_LLDP, net_lldp_recv);

int net_lldp_register_callback(struct net_if *iface, net_lldp_recv_cb_t recv_cb)
{
	struct ethernet_context *ctx;
	int ret;

	ret = lldp_check_iface(iface);
	if (ret < 0) {
		return ret;
	}

	ctx = net_if_l2_data(iface);

	ret = lldp_find(ctx, iface);
	if (ret < 0) {
		return ret;
	}

	ctx->lldp[ret].cb = recv_cb;

	return 0;
}

static void iface_event_handler(struct net_mgmt_event_callback *evt_cb,
				uint32_t mgmt_event, struct net_if *iface)
{
	lldp_start(iface, mgmt_event);
}

static void iface_cb(struct net_if *iface, void *user_data)
{
	/* If the network interface is already up, then call the sender
	 * immediately. If the interface is not ethernet one, then
	 * lldp_start() will return immediately.
	 */
	if (net_if_flag_is_set(iface, NET_IF_UP)) {
		lldp_start(iface, NET_EVENT_IF_UP);
	}
}

int net_lldp_config(struct net_if *iface, const struct net_lldpdu *lldpdu)
{
	struct ethernet_context *ctx = net_if_l2_data(iface);
	int i;

	i = lldp_find(ctx, iface);
	if (i < 0) {
		return i;
	}

	ctx->lldp[i].lldpdu = lldpdu;

	return 0;
}

int net_lldp_config_optional(struct net_if *iface, const uint8_t *tlv, size_t len)
{
	struct ethernet_context *ctx = net_if_l2_data(iface);
	int i;

	i = lldp_find(ctx, iface);
	if (i < 0) {
		return i;
	}

	ctx->lldp[i].optional_du = tlv;
	ctx->lldp[i].optional_len = len;

	return 0;
}

static const struct net_lldpdu lldpdu = {
	.chassis_id = {
		.type_length = htons((LLDP_TLV_CHASSIS_ID << 9) |
			NET_LLDP_CHASSIS_ID_TLV_LEN),
		.subtype = CONFIG_NET_LLDP_CHASSIS_ID_SUBTYPE,
		.value = NET_LLDP_CHASSIS_ID_VALUE
	},
	.port_id = {
		.type_length = htons((LLDP_TLV_PORT_ID << 9) |
			NET_LLDP_PORT_ID_TLV_LEN),
		.subtype = CONFIG_NET_LLDP_PORT_ID_SUBTYPE,
		.value = NET_LLDP_PORT_ID_VALUE
	},
	.ttl = {
		.type_length = htons((LLDP_TLV_TTL << 9) |
			NET_LLDP_TTL_TLV_LEN),
		.ttl = htons(NET_LLDP_TTL)
	},
};

int net_lldp_set_lldpdu(struct net_if *iface)
{
	return net_lldp_config(iface, &lldpdu);
}

void net_lldp_unset_lldpdu(struct net_if *iface)
{
	net_lldp_config(iface, NULL);
	net_lldp_config_optional(iface, NULL, 0);
}

void net_lldp_init(void)
{
	k_work_init_delayable(&lldp_tx_timer, lldp_tx_timeout);

	net_if_foreach(iface_cb, NULL);

	net_mgmt_init_event_callback(&cb, iface_event_handler,
				     NET_EVENT_IF_UP | NET_EVENT_IF_DOWN);
	net_mgmt_add_event_callback(&cb);
}