lib/sockets/sockets_packet.c

/*
 * Copyright (c) 2019 Intel Corporation
 * Copyright (c) 2021 Nordic Semiconductor
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <stdbool.h>
#ifdef CONFIG_ARCH_POSIX
#include <fcntl.h>
#else
#include <zephyr/posix/fcntl.h>
#endif

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(net_sock_packet, CONFIG_NET_SOCKETS_LOG_LEVEL);

#include <zephyr/kernel.h>
#include <zephyr/drivers/entropy.h>
#include <zephyr/sys/util.h>
#include <zephyr/net/net_context.h>
#include <zephyr/net/net_pkt.h>
#include <zephyr/net/socket.h>
#include <zephyr/net/ethernet.h>
#include <zephyr/syscall_handler.h>
#include <zephyr/sys/fdtable.h>

#include "../../ip/net_stats.h"

#include "sockets_internal.h"

extern const struct socket_op_vtable sock_fd_op_vtable;

static const struct socket_op_vtable packet_sock_fd_op_vtable;

static inline int k_fifo_wait_non_empty(struct k_fifo *fifo,
					k_timeout_t timeout)
{
	struct k_poll_event events[] = {
		K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_FIFO_DATA_AVAILABLE,
					 K_POLL_MODE_NOTIFY_ONLY, fifo),
	};

	return k_poll(events, ARRAY_SIZE(events), timeout);
}

static int zpacket_socket(int family, int type, int proto)
{
	struct net_context *ctx;
	int fd;
	int ret;

	fd = z_reserve_fd();
	if (fd < 0) {
		return -1;
	}

	if (proto == 0) {
		if (type == SOCK_RAW) {
			proto = IPPROTO_RAW;
		}
	}

	ret = net_context_get(family, type, proto, &ctx);
	if (ret < 0) {
		z_free_fd(fd);
		errno = -ret;
		return -1;
	}

	/* Initialize user_data, all other calls will preserve it */
	ctx->user_data = NULL;

	/* recv_q and accept_q are in union */
	k_fifo_init(&ctx->recv_q);
	z_finalize_fd(fd, ctx,
		      (const struct fd_op_vtable *)&packet_sock_fd_op_vtable);

	return fd;
}

static void zpacket_received_cb(struct net_context *ctx,
				struct net_pkt *pkt,
				union net_ip_header *ip_hdr,
				union net_proto_header *proto_hdr,
				int status,
				void *user_data)
{
	NET_DBG("ctx=%p, pkt=%p, st=%d, user_data=%p", ctx, pkt, status,
		user_data);

	/* if pkt is NULL, EOF */
	if (!pkt) {
		struct net_pkt *last_pkt = k_fifo_peek_tail(&ctx->recv_q);

		if (!last_pkt) {
			/* If there're no packets in the queue, recv() may
			 * be blocked waiting on it to become non-empty,
			 * so cancel that wait.
			 */
			sock_set_eof(ctx);
			k_fifo_cancel_wait(&ctx->recv_q);
			NET_DBG("Marked socket %p as peer-closed", ctx);
		} else {
			net_pkt_set_eof(last_pkt, true);
			NET_DBG("Set EOF flag on pkt %p", ctx);
		}

		return;
	}

	/* Normal packet */
	net_pkt_set_eof(pkt, false);

	k_fifo_put(&ctx->recv_q, pkt);
}

static int zpacket_bind_ctx(struct net_context *ctx,
			    const struct sockaddr *addr,
			    socklen_t addrlen)
{
	int ret = 0;

	ret = net_context_bind(ctx, addr, addrlen);
	if (ret < 0) {
		errno = -ret;
		return -1;
	}

	/* For packet socket, we expect to receive packets after call
	 * to bind().
	 */
	ret = net_context_recv(ctx, zpacket_received_cb, K_NO_WAIT,
			       ctx->user_data);
	if (ret < 0) {
		errno = -ret;
		return -1;
	}

	return 0;
}

static void zpacket_set_eth_pkttype(struct net_if *iface,
				    struct sockaddr_ll *addr,
				    struct net_linkaddr *lladdr)
{
	if (net_eth_is_addr_broadcast((struct net_eth_addr *)lladdr->addr)) {
		addr->sll_pkttype = PACKET_BROADCAST;
	} else if (net_eth_is_addr_multicast(
			   (struct net_eth_addr *)lladdr->addr)) {
		addr->sll_pkttype = PACKET_MULTICAST;
	} else if (!net_linkaddr_cmp(net_if_get_link_addr(iface), lladdr)) {
		addr->sll_pkttype = PACKET_HOST;
	} else {
		addr->sll_pkttype = PACKET_OTHERHOST;
	}
}

static void zpacket_set_source_addr(struct net_context *ctx,
				    struct net_pkt *pkt,
				    struct sockaddr *src_addr,
				    socklen_t *addrlen)
{
	struct sockaddr_ll addr = {0};
	struct net_if *iface = net_context_get_iface(ctx);

	if (iface == NULL) {
		return;
	}

	addr.sll_family = AF_PACKET;
	addr.sll_ifindex = net_if_get_by_iface(iface);

	if (net_pkt_is_l2_processed(pkt)) {
		/* L2 has already processed the packet - can copy information
		 * directly from the net_pkt structure
		 */
		addr.sll_halen = pkt->lladdr_src.len;
		memcpy(addr.sll_addr, pkt->lladdr_src.addr,
		       MIN(sizeof(addr.sll_addr), pkt->lladdr_src.len));

		addr.sll_protocol = net_pkt_ll_proto_type(pkt);

		if (net_if_get_link_addr(iface)->type == NET_LINK_ETHERNET) {
			addr.sll_hatype = ARPHRD_ETHER;
			zpacket_set_eth_pkttype(iface, &addr,
						net_pkt_lladdr_dst(pkt));
		}
	} else if (net_if_get_link_addr(iface)->type == NET_LINK_ETHERNET) {
		/* Need to extract information from the L2 header. Only
		 * Ethernet L2 supported currently.
		 */
		struct net_eth_hdr *hdr;
		struct net_linkaddr dst_addr;
		struct net_pkt_cursor cur;

		net_pkt_cursor_backup(pkt, &cur);
		net_pkt_cursor_init(pkt);

		hdr = NET_ETH_HDR(pkt);
		if (hdr == NULL ||
		    pkt->buffer->len < sizeof(struct net_eth_hdr)) {
			net_pkt_cursor_restore(pkt, &cur);
			return;
		}

		addr.sll_halen = sizeof(struct net_eth_addr);
		memcpy(addr.sll_addr, hdr->src.addr,
		       sizeof(struct net_eth_addr));

		addr.sll_protocol = ntohs(hdr->type);
		addr.sll_hatype = ARPHRD_ETHER;

		dst_addr.addr = hdr->dst.addr;
		dst_addr.len = sizeof(struct net_eth_addr);
		dst_addr.type = NET_LINK_ETHERNET;

		zpacket_set_eth_pkttype(iface, &addr, &dst_addr);
		net_pkt_cursor_restore(pkt, &cur);
	}

	/* Copy the result sockaddr_ll structure into provided buffer. If the
	 * buffer is smaller than the structure size, it will be truncated.
	 */
	memcpy(src_addr, &addr, MIN(sizeof(struct sockaddr_ll), *addrlen));
	*addrlen = sizeof(struct sockaddr_ll);
}

ssize_t zpacket_sendto_ctx(struct net_context *ctx, const void *buf, size_t len,
			   int flags, const struct sockaddr *dest_addr,
			   socklen_t addrlen)
{
	k_timeout_t timeout = K_FOREVER;
	int status;

	if (!dest_addr) {
		errno = EDESTADDRREQ;
		return -1;
	}

	if ((flags & ZSOCK_MSG_DONTWAIT) || sock_is_nonblock(ctx)) {
		timeout = K_NO_WAIT;
	} else {
		net_context_get_option(ctx, NET_OPT_SNDTIMEO, &timeout, NULL);
	}

	/* Register the callback before sending in order to receive the response
	 * from the peer.
	 */

	status = net_context_recv(ctx, zpacket_received_cb, K_NO_WAIT,
				  ctx->user_data);
	if (status < 0) {
		errno = -status;
		return -1;
	}

	status = net_context_sendto(ctx, buf, len, dest_addr, addrlen,
				    NULL, timeout, ctx->user_data);
	if (status < 0) {
		errno = -status;
		return -1;
	}

	return status;
}

ssize_t zpacket_sendmsg_ctx(struct net_context *ctx, const struct msghdr *msg,
			    int flags)
{
	k_timeout_t timeout = K_FOREVER;
	int status;

	if ((flags & ZSOCK_MSG_DONTWAIT) || sock_is_nonblock(ctx)) {
		timeout = K_NO_WAIT;
	} else {
		net_context_get_option(ctx, NET_OPT_SNDTIMEO, &timeout, NULL);
	}

	status = net_context_sendmsg(ctx, msg, flags, NULL, timeout, NULL);
	if (status < 0) {
		errno = -status;
		return -1;
	}

	return status;
}

ssize_t zpacket_recvfrom_ctx(struct net_context *ctx, void *buf, size_t max_len,
			     int flags, struct sockaddr *src_addr,
			     socklen_t *addrlen)
{
	size_t recv_len = 0;
	k_timeout_t timeout = K_FOREVER;
	struct net_pkt *pkt;

	if ((flags & ZSOCK_MSG_DONTWAIT) || sock_is_nonblock(ctx)) {
		timeout = K_NO_WAIT;
	} else {
		net_context_get_option(ctx, NET_OPT_RCVTIMEO, &timeout, NULL);
	}

	if (flags & ZSOCK_MSG_PEEK) {
		int res;

		res = k_fifo_wait_non_empty(&ctx->recv_q, timeout);
		/* EAGAIN when timeout expired, EINTR when cancelled */
		if (res && res != -EAGAIN && res != -EINTR) {
			errno = -res;
			return -1;
		}

		pkt = k_fifo_peek_head(&ctx->recv_q);
	} else {
		pkt = k_fifo_get(&ctx->recv_q, timeout);
	}

	if (!pkt) {
		errno = EAGAIN;
		return -1;
	}

	/* We do not handle any headers here,
	 * just pass the whole packet to caller.
	 */
	recv_len = net_pkt_get_len(pkt);
	if (recv_len > max_len) {
		recv_len = max_len;
	}

	if (net_pkt_read(pkt, buf, recv_len)) {
		errno = ENOBUFS;
		return -1;
	}

	if (src_addr && addrlen) {
		zpacket_set_source_addr(ctx, pkt, src_addr, addrlen);
	}

	if (IS_ENABLED(CONFIG_NET_PKT_RXTIME_STATS) &&
	    !(flags & ZSOCK_MSG_PEEK)) {
		net_socket_update_tc_rx_time(pkt, k_cycle_get_32());
	}

	if (!(flags & ZSOCK_MSG_PEEK)) {
		net_pkt_unref(pkt);
	} else {
		net_pkt_cursor_init(pkt);
	}

	return recv_len;
}

int zpacket_getsockopt_ctx(struct net_context *ctx, int level, int optname,
			   void *optval, socklen_t *optlen)
{
	if (!optval || !optlen) {
		errno = EINVAL;
		return -1;
	}

	return sock_fd_op_vtable.getsockopt(ctx, level, optname,
					    optval, optlen);
}

int zpacket_setsockopt_ctx(struct net_context *ctx, int level, int optname,
			const void *optval, socklen_t optlen)
{
	return sock_fd_op_vtable.setsockopt(ctx, level, optname,
					    optval, optlen);
}

static ssize_t packet_sock_read_vmeth(void *obj, void *buffer, size_t count)
{
	return zpacket_recvfrom_ctx(obj, buffer, count, 0, NULL, 0);
}

static ssize_t packet_sock_write_vmeth(void *obj, const void *buffer,
				       size_t count)
{
	return zpacket_sendto_ctx(obj, buffer, count, 0, NULL, 0);
}

static int packet_sock_ioctl_vmeth(void *obj, unsigned int request,
				   va_list args)
{
	return sock_fd_op_vtable.fd_vtable.ioctl(obj, request, args);
}

/*
 * TODO: A packet socket can be bound to a network device using SO_BINDTODEVICE.
 */
static int packet_sock_bind_vmeth(void *obj, const struct sockaddr *addr,
				  socklen_t addrlen)
{
	return zpacket_bind_ctx(obj, addr, addrlen);
}

/* The connect() function is no longer necessary. */
static int packet_sock_connect_vmeth(void *obj, const struct sockaddr *addr,
				     socklen_t addrlen)
{
	return -EOPNOTSUPP;
}

/*
 * The listen() and accept() functions are without any functionality,
 * since the client-Server-Semantic is no longer present.
 * When we use packet sockets we are sending unconnected packets.
 */
static int packet_sock_listen_vmeth(void *obj, int backlog)
{
	return -EOPNOTSUPP;
}

static int packet_sock_accept_vmeth(void *obj, struct sockaddr *addr,
				    socklen_t *addrlen)
{
	return -EOPNOTSUPP;
}

static ssize_t packet_sock_sendto_vmeth(void *obj, const void *buf, size_t len,
					int flags,
					const struct sockaddr *dest_addr,
					socklen_t addrlen)
{
	return zpacket_sendto_ctx(obj, buf, len, flags, dest_addr, addrlen);
}

static ssize_t packet_sock_sendmsg_vmeth(void *obj, const struct msghdr *msg,
					 int flags)
{
	return zpacket_sendmsg_ctx(obj, msg, flags);
}

static ssize_t packet_sock_recvfrom_vmeth(void *obj, void *buf, size_t max_len,
					  int flags, struct sockaddr *src_addr,
					  socklen_t *addrlen)
{
	return zpacket_recvfrom_ctx(obj, buf, max_len, flags,
				    src_addr, addrlen);
}

static int packet_sock_getsockopt_vmeth(void *obj, int level, int optname,
					void *optval, socklen_t *optlen)
{
	return zpacket_getsockopt_ctx(obj, level, optname, optval, optlen);
}

static int packet_sock_setsockopt_vmeth(void *obj, int level, int optname,
					const void *optval, socklen_t optlen)
{
	return zpacket_setsockopt_ctx(obj, level, optname, optval, optlen);
}

static int packet_sock_close_vmeth(void *obj)
{
	return zsock_close_ctx(obj);
}

static const struct socket_op_vtable packet_sock_fd_op_vtable = {
	.fd_vtable = {
		.read = packet_sock_read_vmeth,
		.write = packet_sock_write_vmeth,
		.close = packet_sock_close_vmeth,
		.ioctl = packet_sock_ioctl_vmeth,
	},
	.bind = packet_sock_bind_vmeth,
	.connect = packet_sock_connect_vmeth,
	.listen = packet_sock_listen_vmeth,
	.accept = packet_sock_accept_vmeth,
	.sendto = packet_sock_sendto_vmeth,
	.sendmsg = packet_sock_sendmsg_vmeth,
	.recvfrom = packet_sock_recvfrom_vmeth,
	.getsockopt = packet_sock_getsockopt_vmeth,
	.setsockopt = packet_sock_setsockopt_vmeth,
};

static bool packet_is_supported(int family, int type, int proto)
{
	switch (type) {
	case SOCK_RAW:
		return proto == ETH_P_ALL
		  || proto == ETH_P_ECAT
		  || proto == ETH_P_IEEE802154
		  || proto == IPPROTO_RAW;

	case SOCK_DGRAM:
		return proto > 0;

	default:
		return false;
	}
}

NET_SOCKET_REGISTER(af_packet, NET_SOCKET_DEFAULT_PRIO, AF_PACKET,
		    packet_is_supported, zpacket_socket);