android13/external/ltp/lib/tst_rtnetlink.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Copyright (c) 2021 Linux Test Project
 */

#include <stdlib.h>
#include <limits.h>
#include <asm/types.h>
#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/poll.h>
#define TST_NO_DEFAULT_MAIN
#include "tst_test.h"
#include "tst_rtnetlink.h"

struct tst_rtnl_context {
	int socket;
	pid_t pid;
	uint32_t seq;
	size_t bufsize, datalen;
	char *buffer;
	struct nlmsghdr *curmsg;
};

static int tst_rtnl_grow_buffer(const char *file, const int lineno,
	struct tst_rtnl_context *ctx, size_t size)
{
	size_t needed, offset, curlen = NLMSG_ALIGN(ctx->datalen);
	char *buf;

	if (ctx->bufsize - curlen >= size)
		return 1;

	needed = size - (ctx->bufsize - curlen);
	size = ctx->bufsize + (ctx->bufsize > needed ? ctx->bufsize : needed);
	size = NLMSG_ALIGN(size);
	buf = safe_realloc(file, lineno, ctx->buffer, size);

	if (!buf)
		return 0;

	memset(buf + ctx->bufsize, 0, size - ctx->bufsize);
	offset = ((char *)ctx->curmsg) - ctx->buffer;
	ctx->buffer = buf;
	ctx->curmsg = (struct nlmsghdr *)(buf + offset);
	ctx->bufsize = size;

	return 1;
}

void tst_rtnl_destroy_context(const char *file, const int lineno,
	struct tst_rtnl_context *ctx)
{
	safe_close(file, lineno, NULL, ctx->socket);
	free(ctx->buffer);
	free(ctx);
}

struct tst_rtnl_context *tst_rtnl_create_context(const char *file,
	const int lineno)
{
	struct tst_rtnl_context *ctx;
	struct sockaddr_nl addr = { .nl_family = AF_NETLINK };

	ctx = safe_malloc(file, lineno, NULL, sizeof(struct tst_rtnl_context));

	if (!ctx)
		return NULL;

	ctx->pid = 0;
	ctx->seq = 0;
	ctx->buffer = NULL;
	ctx->bufsize = 1024;
	ctx->datalen = 0;
	ctx->curmsg = NULL;
	ctx->socket = safe_socket(file, lineno, NULL, AF_NETLINK,
		SOCK_DGRAM | SOCK_CLOEXEC, NETLINK_ROUTE);

	if (ctx->socket < 0) {
		free(ctx);
		return NULL;
	}

	if (safe_bind(file, lineno, NULL, ctx->socket, (struct sockaddr *)&addr,
		sizeof(addr))) {
		tst_rtnl_destroy_context(file, lineno, ctx);
		return NULL;
	}

	ctx->buffer = safe_malloc(file, lineno, NULL, ctx->bufsize);

	if (!ctx->buffer) {
		tst_rtnl_destroy_context(file, lineno, ctx);
		return NULL;
	}

	memset(ctx->buffer, 0, ctx->bufsize);

	return ctx;
}

void tst_rtnl_free_message(struct tst_rtnl_message *msg)
{
	if (!msg)
		return;

	// all ptr->header and ptr->info pointers point to the same buffer
	// msg->header is the start of the buffer
	free(msg->header);
	free(msg);
}

int tst_rtnl_send(const char *file, const int lineno,
	struct tst_rtnl_context *ctx)
{
	int ret;
	struct sockaddr_nl addr = { .nl_family = AF_NETLINK };
	struct iovec iov;
	struct msghdr msg = {
		.msg_name = &addr,
		.msg_namelen = sizeof(addr),
		.msg_iov = &iov,
		.msg_iovlen = 1
	};

	if (!ctx->curmsg) {
		tst_brk_(file, lineno, TBROK, "%s(): No message to send",
			__func__);
		return 0;
	}

	if (ctx->curmsg->nlmsg_flags & NLM_F_MULTI) {
		struct nlmsghdr eom = { .nlmsg_type = NLMSG_DONE };

		if (!tst_rtnl_add_message(file, lineno, ctx, &eom, NULL, 0))
			return 0;

		/* NLMSG_DONE message must not have NLM_F_MULTI flag */
		ctx->curmsg->nlmsg_flags = 0;
	}

	iov.iov_base = ctx->buffer;
	iov.iov_len = ctx->datalen;
	ret = safe_sendmsg(file, lineno, ctx->datalen, ctx->socket, &msg, 0);

	if (ret > 0)
		ctx->curmsg = NULL;

	return ret;
}

int tst_rtnl_wait(struct tst_rtnl_context *ctx)
{
	struct pollfd fdinfo = {
		.fd = ctx->socket,
		.events = POLLIN
	};

	return poll(&fdinfo, 1, 1000);
}

struct tst_rtnl_message *tst_rtnl_recv(const char *file, const int lineno,
	struct tst_rtnl_context *ctx)
{
	char tmp, *tmpbuf, *buffer = NULL;
	struct tst_rtnl_message *ret;
	struct nlmsghdr *ptr;
	size_t retsize, bufsize = 0;
	ssize_t size;
	int i, size_left, msgcount;

	/* Each recv() call returns one message, read all pending messages */
	while (1) {
		errno = 0;
		size = recv(ctx->socket, &tmp, 1,
			MSG_DONTWAIT | MSG_PEEK | MSG_TRUNC);

		if (size < 0) {
			if (errno != EAGAIN) {
				tst_brk_(file, lineno, TBROK | TERRNO,
					"recv() failed");
			}

			break;
		}

		tmpbuf = safe_realloc(file, lineno, buffer, bufsize + size);

		if (!tmpbuf)
			break;

		buffer = tmpbuf;
		size = safe_recv(file, lineno, size, ctx->socket,
			buffer + bufsize, size, 0);

		if (size < 0)
			break;

		bufsize += size;
	}

	if (!bufsize) {
		free(buffer);
		return NULL;
	}

	ptr = (struct nlmsghdr *)buffer;
	size_left = bufsize;
	msgcount = 0;

	for (; size_left > 0 && NLMSG_OK(ptr, size_left); msgcount++)
		ptr = NLMSG_NEXT(ptr, size_left);

	retsize = (msgcount + 1) * sizeof(struct tst_rtnl_message);
	ret = safe_malloc(file, lineno, NULL, retsize);

	if (!ret) {
		free(buffer);
		return NULL;
	}

	memset(ret, 0, retsize);
	ptr = (struct nlmsghdr *)buffer;
	size_left = bufsize;

	for (i = 0; i < msgcount; i++, ptr = NLMSG_NEXT(ptr, size_left)) {
		ret[i].header = ptr;
		ret[i].payload = NLMSG_DATA(ptr);
		ret[i].payload_size = NLMSG_PAYLOAD(ptr, 0);

		if (ptr->nlmsg_type == NLMSG_ERROR)
			ret[i].err = NLMSG_DATA(ptr);
	}

	return ret;
}

int tst_rtnl_add_message(const char *file, const int lineno,
	struct tst_rtnl_context *ctx, const struct nlmsghdr *header,
	const void *payload, size_t payload_size)
{
	size_t size;
	unsigned int extra_flags = 0;

	if (!tst_rtnl_grow_buffer(file, lineno, ctx, NLMSG_SPACE(payload_size)))
		return 0;

	if (!ctx->curmsg) {
		/*
		 * datalen may hold the size of last sent message for ACK
		 * checking, reset it back to 0 here
		 */
		ctx->datalen = 0;
		ctx->curmsg = (struct nlmsghdr *)ctx->buffer;
	} else {
		size = NLMSG_ALIGN(ctx->curmsg->nlmsg_len);

		extra_flags = NLM_F_MULTI;
		ctx->curmsg->nlmsg_flags |= extra_flags;
		ctx->curmsg = NLMSG_NEXT(ctx->curmsg, size);
		ctx->datalen = NLMSG_ALIGN(ctx->datalen);
	}

	*ctx->curmsg = *header;
	ctx->curmsg->nlmsg_len = NLMSG_LENGTH(payload_size);
	ctx->curmsg->nlmsg_flags |= extra_flags;
	ctx->curmsg->nlmsg_seq = ctx->seq++;
	ctx->curmsg->nlmsg_pid = ctx->pid;

	if (payload_size)
		memcpy(NLMSG_DATA(ctx->curmsg), payload, payload_size);

	ctx->datalen += ctx->curmsg->nlmsg_len;

	return 1;
}

int tst_rtnl_add_attr(const char *file, const int lineno,
	struct tst_rtnl_context *ctx, unsigned short type,
	const void *data, unsigned short len)
{
	size_t size;
	struct rtattr *attr;

	if (!ctx->curmsg) {
		tst_brk_(file, lineno, TBROK,
			"%s(): No message to add attributes to", __func__);
		return 0;
	}

	if (!tst_rtnl_grow_buffer(file, lineno, ctx, RTA_SPACE(len)))
		return 0;

	size = NLMSG_ALIGN(ctx->curmsg->nlmsg_len);
	attr = (struct rtattr *)(((char *)ctx->curmsg) + size);
	attr->rta_type = type;
	attr->rta_len = RTA_LENGTH(len);
	memcpy(RTA_DATA(attr), data, len);
	ctx->curmsg->nlmsg_len = size + attr->rta_len;
	ctx->datalen = NLMSG_ALIGN(ctx->datalen) + attr->rta_len;

	return 1;
}

int tst_rtnl_add_attr_string(const char *file, const int lineno,
	struct tst_rtnl_context *ctx, unsigned short type,
	const char *data)
{
	return tst_rtnl_add_attr(file, lineno, ctx, type, data,
		strlen(data) + 1);
}

int tst_rtnl_add_attr_list(const char *file, const int lineno,
	struct tst_rtnl_context *ctx,
	const struct tst_rtnl_attr_list *list)
{
	int i, ret;
	size_t offset;

	for (i = 0; list[i].len >= 0; i++) {
		if (list[i].len > USHRT_MAX) {
			tst_brk_(file, lineno, TBROK,
				"%s(): Attribute value too long", __func__);
			return -1;
		}

		offset = NLMSG_ALIGN(ctx->datalen);
		ret = tst_rtnl_add_attr(file, lineno, ctx, list[i].type,
			list[i].data, list[i].len);

		if (!ret)
			return -1;

		if (list[i].sublist) {
			struct rtattr *attr;

			ret = tst_rtnl_add_attr_list(file, lineno, ctx,
				list[i].sublist);

			if (ret < 0)
				return ret;

			attr = (struct rtattr *)(ctx->buffer + offset);

			if (ctx->datalen - offset > USHRT_MAX) {
				tst_brk_(file, lineno, TBROK,
					"%s(): Sublist too long", __func__);
				return -1;
			}

			attr->rta_len = ctx->datalen - offset;
		}
	}

	return i;
}

int tst_rtnl_check_acks(const char *file, const int lineno,
	struct tst_rtnl_context *ctx, struct tst_rtnl_message *res)
{
	struct nlmsghdr *msg = (struct nlmsghdr *)ctx->buffer;
	int size_left = ctx->datalen;

	for (; size_left > 0 && NLMSG_OK(msg, size_left);
		msg = NLMSG_NEXT(msg, size_left)) {

		if (!(msg->nlmsg_flags & NLM_F_ACK))
			continue;

		while (res->header && res->header->nlmsg_seq != msg->nlmsg_seq)
			res++;

		if (!res->err || res->header->nlmsg_seq != msg->nlmsg_seq) {
			tst_brk_(file, lineno, TBROK,
				"No ACK found for Netlink message %u",
				msg->nlmsg_seq);
			return 0;
		}

		if (res->err->error) {
			TST_ERR = -res->err->error;
			return 0;
		}
	}

	return 1;
}

int tst_rtnl_send_validate(const char *file, const int lineno,
	struct tst_rtnl_context *ctx)
{
	struct tst_rtnl_message *response;
	int ret;

	TST_ERR = 0;

	if (tst_rtnl_send(file, lineno, ctx) <= 0)
		return 0;

	tst_rtnl_wait(ctx);
	response = tst_rtnl_recv(file, lineno, ctx);

	if (!response)
		return 0;

	ret = tst_rtnl_check_acks(file, lineno, ctx, response);
	tst_rtnl_free_message(response);

	return ret;
}