Differential D36002 Diff 109873 sys/netlink/netlink_helpers.c

Changeset View

Standalone View

sys/netlink/netlink_helpers.c

This file was added.

				/*-
				* SPDX-License-Identifier: BSD-2-Clause-FreeBSD
				*
				* Copyright (c) 2022 Alexander V. Chernikov
				*
				* Redistribution and use in source and binary forms, with or without
				* modification, are permitted provided that the following conditions
				* are met:
				* 1. Redistributions of source code must retain the above copyright
				* notice, this list of conditions and the following disclaimer.
				* 2. Redistributions in binary form must reproduce the above copyright
				* notice, this list of conditions and the following disclaimer in the
				* documentation and/or other materials provided with the distribution.
				*
				* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
				* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
				* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
				* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
				* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
				* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
				* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
				* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
				* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
				* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
				* SUCH DAMAGE.
				*/

				#include <sys/cdefs.h>
				__FBSDID("$FreeBSD$");
				#include "opt_inet.h"
				#include "opt_inet6.h"
				#include <sys/types.h>
				#include <sys/malloc.h>
				#include <sys/rmlock.h>
				#include <sys/socket.h>

				#include <net/if.h>
				#include <net/route.h>
				#include <net/route/nhop.h>

				#include <net/route/route_ctl.h>
				#include <netlink/netlink.h>
				#include <netlink/netlink_ctl.h>
				#include <netlink/netlink_var.h>
				#include <netlink/netlink_route.h>

				#define DEBUG_MOD_NAME nl_helpers
				#define DEBUG_MAX_LEVEL LOG_DEBUG3
				#include <netlink/netlink_debug.h>
				_DECLARE_DEBUG(LOG_DEBUG);

				/*
				* Sends an ack message
				*/
				void
				nlmsg_ack(struct nlpcb nlp, int error, struct nlmsghdr hdr)
				{
				struct nlmsgerr *errmsg;
				int payload_len;
				uint32_t flags = nlp->nl_flags;
				struct nlmsg_state ns;
				bool cap_ack;

				payload_len = sizeof(struct nlmsgerr);

				/*
				* The only case when we send the full message in the
				* reply is when there is an error and NETLINK_CAP_ACK
				* is not set.
				*/
				cap_ack = (error == 0) \|\| (flags & NLF_CAP_ACK);
				if (!cap_ack)
				payload_len += hdr->nlmsg_len - sizeof(struct nlmsghdr);

				/*
				* TODO: handle NETLINK_F_EXT_ACK sockopt
				* TODO: handle cookies
				*/

				int sz = payload_len + sizeof(struct nlmsghdr);
				if (!nlmsg_get_socket_writer(sz, nlp, &ns))
				goto enomem;
				nlmsg_ignore_limit(&ns);

				RT_LOG(LOG_DEBUG3, "acknowledging message type %d seq %d",
				hdr->nlmsg_type, hdr->nlmsg_seq);

				if (!nlmsg_add(&ns, nlp->nl_port, hdr->nlmsg_seq, NLMSG_ERROR, 0, payload_len))
				goto enomem;

				errmsg = nlmsg_reserve_data(&ns, payload_len, struct nlmsgerr);
				errmsg->error = error;
				/* In case of error copy the whole message, else just the header */
				memcpy(&errmsg->msg, hdr, cap_ack ? sizeof(*hdr) : hdr->nlmsg_len);

				nlmsg_end(&ns);
				nlmsg_flush(&ns);
				return;
				enomem:
				NLP_LOG(LOG_INFO, nlp, "error allocating ack data for message %d seq %u",
				hdr->nlmsg_type, hdr->nlmsg_seq);
				}

				bool
				nlmsg_end_dump(struct nlmsg_state ns, int error, struct nlmsghdr hdr)
				{
				if (!nlmsg_add(ns, hdr->nlmsg_pid, hdr->nlmsg_seq, NLMSG_DONE, 0, sizeof(int))) {
				RT_LOG(LOG_DEBUG, "Error finalizing table dump");
				return (false);
				}
				/* Save operation result */
				int *perror = nlmsg_reserve_object(ns, int);
				RT_LOG(LOG_DEBUG2, "record error=%d at off %d (%p)", error,
				ns->offset, perror);
				*perror = error;
				nlmsg_end(ns);

				return (true);
				}


				static const struct nlattr_parser *
				search_states(const struct nlattr_parser *ps, int pslen, int key)
				{
				int left_i = 0, right_i = pslen - 1;

				if (key < ps[0].type \|\| key > ps[pslen - 1].type)
				return (NULL);

				while (left_i + 1 < right_i) {
				int mid_i = (left_i + right_i) / 2;
				if (key < ps[mid_i].type)
				right_i = mid_i;
				else if (key > ps[mid_i].type)
				left_i = mid_i + 1;
				else
				return (&ps[mid_i]);
				}
				if (ps[left_i].type == key)
				return (&ps[left_i]);
				else if (ps[right_i].type == key)
				return (&ps[right_i]);
				return (NULL);
				}

				int
				nl_parse_attrs_raw(struct nlattr nla_head, int len, struct nlattr_parser ps, int pslen,
				struct netlink_parse_tracker npt, void target)
				{
				struct nlattr *nla;
				int error = 0;

				RT_LOG(LOG_DEBUG3, "parse %p remaining_len %d", nla_head, len);
				NLA_FOREACH(nla, nla_head, len) {
				if (nla->nla_len < sizeof(struct nlattr)) {
				RT_LOG(LOG_DEBUG, "Invalid attr len: %d", nla->nla_len);
				return (EINVAL);
				}

				int nla_type = nla->nla_type & NLA_TYPE_MASK;
				const struct nlattr_parser *s = search_states(ps, pslen, nla_type);
				if (s != NULL) {
				void ptr = (void )((char *)target + s->off);
				error = s->cb(nla, npt, ptr);
				if (error != 0)
				return (error);
				} else {
				/* Default policy is to ignore unknown attrs */
				}

				if (s == NULL) {
				/* Default policy is to ignore */
				continue;
				}
				}

				return (0);
				}

				int
				nl_parse_attrs(struct nlmsghdr hdr, int hdrlen, struct nlattr_parser ps, int pslen,
				struct netlink_parse_tracker npt, void target)
				{
				int off = NLMSG_HDRLEN + NETLINK_ALIGN(hdrlen);
				int len = hdr->nlmsg_len - off;
				struct nlattr nla_head = (struct nlattr )((char *)hdr + off);

				return (nl_parse_attrs_raw(nla_head, len, ps, pslen, npt, target));
				}

				int
				nlattr_get_flag(struct nlattr nla, struct netlink_parse_tracker npt, void *target)
				{
				if (__predict_false(NLA_DATA_LEN(nla) != 0)) {
				RT_LOG(LOG_DEBUG, "nla type %d size(%u) is not a flag",
				nla->nla_type, NLA_DATA_LEN(nla));
				return (EINVAL);
				}

				((uint8_t )target) = 1;
				return (0);
				}

				static struct sockaddr *
				parse_rta_ip4(void rta_data, struct netlink_parse_tracker npt, int *perror)
				{
				struct sockaddr_in *sin;

				sin = (struct sockaddr_in *)npt_alloc_sockaddr(npt, sizeof(struct sockaddr_in));
				if (__predict_false(sin == NULL)) {
				*perror = ENOBUFS;
				return (NULL);
				}
				sin->sin_len = sizeof(struct sockaddr_in);
				sin->sin_family = AF_INET;
				memcpy(&sin->sin_addr, rta_data, sizeof(struct in_addr));
				return ((struct sockaddr *)sin);
				}

				static struct sockaddr *
				parse_rta_ip6(void rta_data, struct netlink_parse_tracker npt, int *perror)
				{
				struct sockaddr_in6 *sin6;

				sin6 = (struct sockaddr_in6 *)npt_alloc_sockaddr(npt, sizeof(struct sockaddr_in6));
				if (__predict_false(sin6 == NULL)) {
				*perror = ENOBUFS;
				return (NULL);
				}
				sin6->sin6_len = sizeof(struct sockaddr_in6);
				sin6->sin6_family = AF_INET6;
				memcpy(&sin6->sin6_addr, rta_data, sizeof(struct in_addr));
				return ((struct sockaddr *)sin6);
				}

				static struct sockaddr *
				parse_rta_ip(struct rtattr rta, struct netlink_parse_tracker npt, int *perror)
				{
				void *rta_data = NL_RTA_DATA(rta);
				int rta_len = NL_RTA_DATA_LEN(rta);

				if (rta_len == sizeof(struct in_addr)) {
				return (parse_rta_ip4(rta_data, npt, perror));
				} else if (rta_len == sizeof(struct in6_addr)) {
				return (parse_rta_ip6(rta_data, npt, perror));
				} else {
				RT_LOG(LOG_NOTICE, "unknown IP len: %d for rta type %d",
				rta_len, rta->rta_type);
				*perror = ENOTSUP;
				return (NULL);
				}
				return (NULL);
				}

				int
				nlattr_get_ip(struct nlattr nla, struct netlink_parse_tracker npt, void *target)
				{
				int error = 0;

				struct sockaddr sa = parse_rta_ip((struct rtattr )nla, npt, &error);

				((struct sockaddr *)target) = sa;
				return (error);
				}

				static struct sockaddr *
				parse_rta_via(struct rtattr rta, struct netlink_parse_tracker npt, int *perror)
				{
				struct rtvia *via = NL_RTA_DATA(rta);
				int data_len = NL_RTA_DATA_LEN(rta);

				if (__predict_false(data_len) < sizeof(struct rtvia)) {
				*perror = EINVAL;
				return (NULL);
				}
				data_len -= offsetof(struct rtvia, rtvia_addr);

				switch (via->rtvia_family) {
				case AF_INET:
				if (__predict_false(data_len < sizeof(struct in_addr))) {
				*perror = EINVAL;
				return (NULL);
				}
				return (parse_rta_ip4(via->rtvia_addr, npt, perror));
				case AF_INET6:
				if (__predict_false(data_len < sizeof(struct in6_addr))) {
				*perror = EINVAL;
				return (NULL);
				}
				return (parse_rta_ip6(via->rtvia_addr, npt, perror));
				default:
				*perror = ENOTSUP;
				return (NULL);
				}
				}

				int
				nlattr_get_ipvia(struct nlattr nla, struct netlink_parse_tracker npt, void *target)
				{
				int error = 0;

				struct sockaddr sa = parse_rta_via((struct rtattr )nla, npt, &error);

				((struct sockaddr *)target) = sa;
				return (error);
				}


				int
				nlattr_get_uint32(struct nlattr nla, struct netlink_parse_tracker npt, void *target)
				{
				if (__predict_false(NLA_DATA_LEN(nla) != sizeof(uint32_t))) {
				RT_LOG(LOG_DEBUG, "nla type %d size(%u) is not uint32",
				nla->nla_type, NLA_DATA_LEN(nla));
				return (EINVAL);
				}
				((uint32_t )target) = ((const uint32_t )NL_RTA_DATA_CONST(nla));
				return (0);
				}

				int
				nlattr_get_ifindex(struct nlattr nla, struct netlink_parse_tracker npt, void *target)
				{
				if (__predict_false(NLA_DATA_LEN(nla) != sizeof(uint32_t))) {
				RT_LOG(LOG_DEBUG, "nla type %d size(%u) is not uint32",
				nla->nla_type, NLA_DATA_LEN(nla));
				return (EINVAL);
				}
				uint32_t ifindex = ((const uint32_t )NLA_DATA_CONST(nla));

				NET_EPOCH_ASSERT();

				struct ifnet *ifp = ifnet_byindex(ifindex);
				if (__predict_false(ifp == NULL)) {
				RT_LOG(LOG_DEBUG, "nla type %d: ifindex %u invalid",
				nla->nla_type, ifindex);
				return (ENOENT);
				}
				((struct ifnet *)target) = ifp;
				RT_LOG(LOG_DEBUG3, "nla type %d: ifindex %u -> %s", nla->nla_type,
				ifindex, if_name(ifp));

				return (0);
				}

				int
				nlattr_get_string(struct nlattr nla, struct netlink_parse_tracker npt, void *target)
				{
				int maxlen = NLA_DATA_LEN(nla);

				if (__predict_false(strnlen((char *)NLA_DATA(nla), maxlen) >= maxlen)) {
				RT_LOG(LOG_DEBUG, "nla type %d size(%u) is not NULL-terminated",
				nla->nla_type, maxlen);
				return (EINVAL);
				}

				((char )target) = (char )NLA_DATA(nla);
				return (0);
				}

				int
				nlattr_get_nla(struct nlattr nla, struct netlink_parse_tracker npt, void *target)
				{
				((struct nlattr *)target) = nla;
				return (0);
				}