zephyr/subsys/net/lib/capture/cooked.c

/*
 * Copyright (c) 2024 Nordic Semiconductor
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(net_cooked, CONFIG_NET_CAPTURE_LOG_LEVEL);

#include <zephyr/kernel.h>
#include <stdlib.h>
#include <zephyr/net/net_core.h>
#include <zephyr/net/net_ip.h>
#include <zephyr/net/net_if.h>
#include <zephyr/net/net_pkt.h>
#include <zephyr/net/ethernet.h>
#include <zephyr/net/virtual.h>
#include <zephyr/net/virtual_mgmt.h>
#include <zephyr/net/capture.h>
#include <zephyr/net/net_l2.h>

#include "sll.h"

#define BUF_ALLOC_TIMEOUT 100 /* ms */

/* Use our own slabs for temporary pkts */
NET_PKT_SLAB_DEFINE(cooked_pkts, CONFIG_NET_CAPTURE_PKT_COUNT);

#if defined(CONFIG_NET_BUF_FIXED_DATA_SIZE)
NET_BUF_POOL_FIXED_DEFINE(cooked_bufs, CONFIG_NET_CAPTURE_BUF_COUNT,
			  CONFIG_NET_BUF_DATA_SIZE, 4, NULL);
#else
NET_BUF_POOL_VAR_DEFINE(cooked_bufs, CONFIG_NET_CAPTURE_BUF_COUNT,
			CONFIG_NET_PKT_BUF_RX_DATA_POOL_SIZE, 4, NULL);
#endif

#define COOKED_MTU 1024
#define COOKED_DEVICE "NET_COOKED"

struct cooked_context {
	struct net_if *iface;
	struct net_if *attached_to;

	/* -1 is used as a not configured link type */
	int link_types[CONFIG_NET_CAPTURE_COOKED_MODE_MAX_LINK_TYPES];
	int link_type_count;
	int mtu;
	bool init_done;
	bool status;
};

static void iface_init(struct net_if *iface)
{
	struct cooked_context *ctx = net_if_get_device(iface)->data;
	struct net_if *any_iface;
	int ifindex;
	int ret;

	ifindex = net_if_get_by_name("any");
	if (ifindex < 0) {
		NET_DBG("No such interface \"any\", cannot init interface %d",
			net_if_get_by_iface(iface));
		return;
	}

	if (net_if_l2(net_if_get_by_index(ifindex)) != &NET_L2_GET_NAME(DUMMY)) {
		NET_DBG("The \"any\" interface %d is wrong type", ifindex);
		return;
	}

	if (ctx->init_done) {
		return;
	}

	ctx->iface = iface;
	any_iface = net_if_get_by_index(ifindex);

	(void)net_if_set_name(iface,
			      CONFIG_NET_CAPTURE_COOKED_MODE_INTERFACE_NAME);
	(void)net_virtual_set_name(iface, "Cooked mode capture");

	net_if_flag_set(iface, NET_IF_NO_AUTO_START);
	net_if_flag_set(iface, NET_IF_POINTOPOINT);
	net_if_flag_clear(iface, NET_IF_IPV4);
	net_if_flag_clear(iface, NET_IF_IPV6);

	/* Hook into "any" interface so that we can receive the
	 * captured data.
	 */
	ret = net_virtual_interface_attach(ctx->iface, any_iface);
	if (ret < 0) {
		NET_DBG("Cannot hook into interface %d (%d)",
			net_if_get_by_iface(any_iface), ret);
		return;
	}

	NET_DBG("Interface %d attached on top of %d",
		net_if_get_by_iface(ctx->iface),
		net_if_get_by_iface(any_iface));

	ctx->init_done = true;
}

static int dev_init(const struct device *dev)
{
	struct cooked_context *ctx = dev->data;

	memset(ctx->link_types, -1, sizeof(ctx->link_types));

	return 0;
}

static int interface_start(const struct device *dev)
{
	struct cooked_context *ctx = dev->data;
	int ret = 0;

	if (ctx->status) {
		return -EALREADY;
	}

	ctx->status = true;

	NET_DBG("Starting iface %d", net_if_get_by_iface(ctx->iface));

	return ret;
}

static int interface_stop(const struct device *dev)
{
	struct cooked_context *ctx = dev->data;

	if (!ctx->status) {
		return -EALREADY;
	}

	ctx->status = false;

	NET_DBG("Stopping iface %d", net_if_get_by_iface(ctx->iface));

	return 0;
}

static enum net_verdict interface_recv(struct net_if *iface, struct net_pkt *pkt)
{
	struct cooked_context *ctx = net_if_get_device(iface)->data;
	bool found = false;
	uint16_t ptype;

	/* Feed the packet to capture system after verifying that we are capturing
	 * these types of packets.
	 * The packet will be freed by capture API after it has been processed.
	 */

	ptype = net_pkt_ll_proto_type(pkt);

	NET_DBG("Capture pkt %p for interface %d", pkt, net_if_get_by_iface(iface));

	for (int i = 0; i < ctx->link_type_count; i++) {
		if (ctx->link_types[i] == ptype) {
			found = true;
			break;
		}
	}

	if (found) {
		int ret;

		NET_DBG("Handler found for packet type 0x%04x", ptype);

		/* Normally capture API will clone the net_pkt and we would
		 * always need to unref it. But for cooked packets, we can avoid
		 * the cloning so need to unref only if there was an error with
		 * capturing.
		 */
		ret = net_capture_pkt_with_status(iface, pkt);
		if (ret < 0) {
			net_pkt_unref(pkt);
		}

		return NET_OK;
	}

	NET_DBG("No handler found for packet type 0x%04x", ptype);

	return NET_DROP;
}

static int interface_attach(struct net_if *iface, struct net_if *lower_iface)
{
	struct cooked_context *ctx;

	if (net_if_get_by_iface(iface) < 0) {
		return -ENOENT;
	}

	ctx = net_if_get_device(iface)->data;
	ctx->attached_to = lower_iface;

	return 0;
}

static int interface_set_config(struct net_if *iface,
				enum virtual_interface_config_type type,
				const struct virtual_interface_config *config)
{
	struct cooked_context *ctx = net_if_get_device(iface)->data;

	switch (type) {
	case VIRTUAL_INTERFACE_CONFIG_TYPE_LINK_TYPE:
		if (config->link_types.count > ARRAY_SIZE(ctx->link_types)) {
			return -ERANGE;
		}

		for (int i = 0; i < config->link_types.count; i++) {
			NET_DBG("Adding link type %u", config->link_types.type[i]);

			ctx->link_types[i] = (int)config->link_types.type[i];
		}

		ctx->link_type_count = config->link_types.count;

		/* Mark the rest of the types as invalid */
		for (int i = ctx->link_type_count; i < ARRAY_SIZE(ctx->link_types); i++) {
			ctx->link_types[i] = -1;
		}

		return 0;

	case VIRTUAL_INTERFACE_CONFIG_TYPE_MTU:
		NET_DBG("Interface %d MTU set to %d",
			net_if_get_by_iface(iface), config->mtu);
		net_if_set_mtu(iface, config->mtu);
		return 0;

	default:
		break;
	}

	return -ENOTSUP;
}

static int interface_get_config(struct net_if *iface,
				enum virtual_interface_config_type type,
				struct virtual_interface_config *config)
{
	struct cooked_context *ctx = net_if_get_device(iface)->data;
	int i;

	switch (type) {
	case VIRTUAL_INTERFACE_CONFIG_TYPE_LINK_TYPE:
		for (i = 0; i < ctx->link_type_count; i++) {
			if (ctx->link_types[i] < 0) {
				break;
			}

			config->link_types.type[i] = (uint16_t)ctx->link_types[i];
		}

		config->link_types.count = i;
		NET_ASSERT(config->link_types.count == ctx->link_type_count);
		return 0;

	case VIRTUAL_INTERFACE_CONFIG_TYPE_MTU:
		config->mtu = net_if_get_mtu(iface);
		return 0;

	default:
		break;
	}

	return -ENOTSUP;
}

static const struct virtual_interface_api cooked_api = {
	.iface_api.init = iface_init,

	.start = interface_start,
	.stop = interface_stop,
	.recv = interface_recv,
	.attach = interface_attach,
	.set_config = interface_set_config,
	.get_config = interface_get_config,
};

static struct cooked_context cooked_context_data;

NET_VIRTUAL_INTERFACE_INIT(cooked, COOKED_DEVICE, dev_init,
			   NULL, &cooked_context_data, NULL,
			   CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,
			   &cooked_api, COOKED_MTU);

int net_capture_cooked_setup(struct net_capture_cooked *ctx,
			     uint16_t hatype,
			     uint16_t halen,
			     uint8_t *addr)
{
	if (halen == 0 || halen > NET_CAPTURE_LL_ADDRLEN) {
		return -EINVAL;
	}

	memset(ctx, 0, sizeof(*ctx));

	ctx->hatype = hatype;
	ctx->halen = halen;

	memcpy(ctx->addr, addr, halen);

	return 0;
}

static int create_sll_header(struct net_if *iface,
			     struct net_pkt *pkt,
			     struct net_capture_cooked *ctx,
			     enum net_capture_packet_type type,
			     uint16_t ptype)
{
	struct sll2_header hdr2;
	struct sll_header hdr1;
	size_t hdr_len;
	uint8_t *hdr;
	int ret;

	if (IS_ENABLED(CONFIG_NET_CAPTURE_COOKED_MODE_SLLV1)) {
		hdr1.sll_pkttype = htons(type);
		hdr1.sll_hatype = htons(ctx->hatype);
		hdr1.sll_halen = htons(ctx->halen);
		memcpy(hdr1.sll_addr, ctx->addr, sizeof(ctx->addr));
		hdr1.sll_protocol = htons(ptype);

		hdr = (uint8_t *)&hdr1;
		hdr_len = sizeof(hdr1);

	} else {
		hdr2.sll2_protocol = htons(ptype);
		hdr2.sll2_reserved_mbz = 0;
		hdr2.sll2_if_index = net_if_get_by_iface(iface);
		hdr2.sll2_hatype = htons(ctx->hatype);
		hdr2.sll2_pkttype = htons(type);
		hdr2.sll2_halen = htons(ctx->halen);
		memcpy(hdr2.sll2_addr, ctx->addr, sizeof(ctx->addr));

		hdr = (uint8_t *)&hdr2;
		hdr_len = sizeof(hdr2);
	}

	ret = net_pkt_write(pkt, hdr, hdr_len);
	if (ret < 0) {
		NET_DBG("Cannot write sll%s header (%d)",
			IS_ENABLED(CONFIG_NET_CAPTURE_COOKED_MODE_SLLV1) ?
			"" : "2", ret);
	}

	return ret;
}

static struct k_mem_slab *get_net_pkt(void)
{
	return &cooked_pkts;
}

static struct net_buf_pool *get_net_buf(void)
{
	return &cooked_bufs;
}

void net_capture_data(struct net_capture_cooked *ctx,
		      const uint8_t *data, size_t data_len,
		      enum net_capture_packet_type type,
		      uint16_t ptype)
{
	static struct net_context context;
	const struct device *dev;
	struct net_if *iface;
	struct net_pkt *pkt;
	int ret;

	net_context_setup_pools(&context, get_net_pkt, get_net_buf);

	pkt = net_pkt_alloc_from_slab(&cooked_pkts, K_MSEC(BUF_ALLOC_TIMEOUT));
	if (pkt == NULL) {
		NET_DBG("Cannot allocate %s", "net_pkt");
		return;
	}

	net_pkt_set_context(pkt, &context);

	ret = net_pkt_alloc_buffer_raw(pkt,
			sizeof(COND_CODE_1(CONFIG_NET_CAPTURE_COOKED_MODE_SLLV1,
					   (struct sll_header),
					   (struct sll2_header))) + data_len,
				       K_MSEC(BUF_ALLOC_TIMEOUT));
	if (ret < 0) {
		NET_DBG("Cannot allocate %s %zd bytes (%d)", "net_buf for",
			sizeof(struct sll_header) + data_len, ret);
		net_pkt_unref(pkt);
		return;
	}

	/* Write the packet to "any" interface which will pass it to
	 * the virtual interface that does the actual capturing of the packet.
	 * The reason for this trickery is that we do not have a network
	 * interface in use in this API, and want to have the packet routed
	 * via the any interface which can then deliver it to registered
	 * virtual interfaces.
	 */
	dev = device_get_binding(COOKED_DEVICE);
	if (dev == NULL) {
		NET_DBG("No such device %s found, data not captured!",
			COOKED_DEVICE);
		net_pkt_unref(pkt);
		return;
	}

	/* Next we feed the data to the any interface, which
	 * will pass the data to the virtual interface.
	 * When using capture API or net-shell capture command, one
	 * should capture packets from the virtual interface.
	 */
	iface = ((struct cooked_context *)dev->data)->attached_to;

	ret = create_sll_header(iface, pkt, ctx, type, ptype);
	if (ret < 0) {
		NET_DBG("Cannot write %s %zd bytes (%d)", "header",
			sizeof(struct sll_header), ret);
		net_pkt_unref(pkt);
		return;
	}

	ret = net_pkt_write(pkt, data, data_len);
	if (ret < 0) {
		NET_DBG("Cannot write %s %zd bytes (%d)", "payload",
			data_len, ret);
		net_pkt_unref(pkt);
		return;
	}

	/* Mark that this packet came from cooked capture mode.
	 * This will prevent the capture API from cloning the packet,
	 * so that the net_pkt will be passed as is to capture interface.
	 */
	net_pkt_set_cooked_mode(pkt, true);

	/* The protocol type is used by virtual cooked interface to decide
	 * whether we capture the packet or not.
	 */
	net_pkt_set_ll_proto_type(pkt, ptype);

	memset(net_pkt_lladdr_src(pkt)->addr, 0, sizeof(net_pkt_lladdr_src(pkt)->addr));
	net_pkt_lladdr_src(pkt)->len = 0U;
	net_pkt_lladdr_src(pkt)->type = NET_LINK_DUMMY;
	memset(net_pkt_lladdr_dst(pkt)->addr, 0, sizeof(net_pkt_lladdr_dst(pkt)->addr));
	net_pkt_lladdr_dst(pkt)->len = 0U;
	net_pkt_lladdr_dst(pkt)->type = NET_LINK_DUMMY;

	ret = net_recv_data(iface, pkt);
	if (ret < 0) {
		net_pkt_unref(pkt);
	}
}