diff --git a/sys/netlink/netlink_message_parser.c b/sys/netlink/netlink_message_parser.c index 8caa13fbf3ce..eafc2d83cbfb 100644 --- a/sys/netlink/netlink_message_parser.c +++ b/sys/netlink/netlink_message_parser.c @@ -1,585 +1,596 @@ /*- * SPDX-License-Identifier: BSD-2-Clause * * 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 #include "opt_inet.h" #include "opt_inet6.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define DEBUG_MOD_NAME nl_parser #define DEBUG_MAX_LEVEL LOG_DEBUG3 #include _DECLARE_DEBUG(LOG_INFO); bool nlmsg_report_err_msg(struct nl_pstate *npt, const char *fmt, ...) { va_list ap; if (npt->err_msg != NULL) return (false); char *buf = npt_alloc(npt, NL_MAX_ERROR_BUF); if (buf == NULL) return (false); va_start(ap, fmt); vsnprintf(buf, NL_MAX_ERROR_BUF, fmt, ap); va_end(ap); npt->err_msg = buf; return (true); } bool nlmsg_report_err_offset(struct nl_pstate *npt, uint32_t off) { if (npt->err_off != 0) return (false); npt->err_off = off; return (true); } void nlmsg_report_cookie(struct nl_pstate *npt, struct nlattr *nla) { MPASS(nla->nla_type == NLMSGERR_ATTR_COOKIE); MPASS(nla->nla_len >= sizeof(struct nlattr)); npt->cookie = nla; } void nlmsg_report_cookie_u32(struct nl_pstate *npt, uint32_t val) { struct nlattr *nla = npt_alloc(npt, sizeof(*nla) + sizeof(uint32_t)); nla->nla_type = NLMSGERR_ATTR_COOKIE; nla->nla_len = sizeof(*nla) + sizeof(uint32_t); memcpy(nla + 1, &val, sizeof(uint32_t)); nlmsg_report_cookie(npt, nla); } 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, const struct nlattr_parser *ps, int pslen, struct nl_pstate *npt, void *target) { struct nlattr *nla = NULL; int error = 0; NL_LOG(LOG_DEBUG3, "parse %p remaining_len %d", nla_head, len); int orig_len = len; NLA_FOREACH(nla, nla_head, len) { NL_LOG(LOG_DEBUG3, ">> parsing %p attr_type %d len %d (rem %d)", nla, nla->nla_type, nla->nla_len, len); if (nla->nla_len < sizeof(struct nlattr)) { NLMSG_REPORT_ERR_MSG(npt, "Invalid attr %p type %d len: %d", nla, nla->nla_type, nla->nla_len); uint32_t off = (char *)nla - (char *)npt->hdr; nlmsg_report_err_offset(npt, off); 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, s->arg, ptr); if (error != 0) { uint32_t off = (char *)nla - (char *)npt->hdr; nlmsg_report_err_offset(npt, off); NL_LOG(LOG_DEBUG3, "parse failed at offset %u", off); return (error); } } else { /* Ignore non-specified attributes */ NL_LOG(LOG_DEBUG3, "ignoring attr %d", nla->nla_type); } } if (len >= sizeof(struct nlattr)) { nla = (struct nlattr *)((char *)nla_head + (orig_len - len)); NL_LOG(LOG_DEBUG3, " >>> end %p attr_type %d len %d", nla, nla->nla_type, nla->nla_len); } NL_LOG(LOG_DEBUG3, "end parse: %p remaining_len %d", nla, len); return (0); } void nl_get_attrs_bmask_raw(struct nlattr *nla_head, int len, struct nlattr_bmask *bm) { struct nlattr *nla = NULL; BIT_ZERO(NL_ATTR_BMASK_SIZE, bm); NLA_FOREACH(nla, nla_head, len) { if (nla->nla_len < sizeof(struct nlattr)) return; int nla_type = nla->nla_type & NLA_TYPE_MASK; if (nla_type < NL_ATTR_BMASK_SIZE) BIT_SET(NL_ATTR_BMASK_SIZE, nla_type, bm); else NL_LOG(LOG_DEBUG2, "Skipping type %d in the mask: too short", nla_type); } } bool nl_has_attr(const struct nlattr_bmask *bm, unsigned int nla_type) { MPASS(nla_type < NL_ATTR_BMASK_SIZE); return (BIT_ISSET(NL_ATTR_BMASK_SIZE, nla_type, bm)); } int nlattr_get_flag(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { if (__predict_false(NLA_DATA_LEN(nla) != 0)) { NLMSG_REPORT_ERR_MSG(npt, "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 nl_pstate *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 nl_pstate *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 in6_addr)); return ((struct sockaddr *)sin6); } static struct sockaddr * parse_rta_ip(struct rtattr *rta, struct nl_pstate *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 { NLMSG_REPORT_ERR_MSG(npt, "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 nl_pstate *npt, const void *arg, 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 nl_pstate *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)) { NLMSG_REPORT_ERR_MSG(npt, "undersized RTA_VIA(%d) attr: len %d", rta->rta_type, data_len); *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 nl_pstate *npt, const void *arg, 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_bool(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) +{ + if (__predict_false(NLA_DATA_LEN(nla) != sizeof(bool))) { + NLMSG_REPORT_ERR_MSG(npt, "nla type %d size(%u) is not bool", + nla->nla_type, NLA_DATA_LEN(nla)); + return (EINVAL); + } + *((bool *)target) = *((const bool *)NL_RTA_DATA_CONST(nla)); + return (0); +} int nlattr_get_uint8(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { if (__predict_false(NLA_DATA_LEN(nla) != sizeof(uint8_t))) { NLMSG_REPORT_ERR_MSG(npt, "nla type %d size(%u) is not uint8", nla->nla_type, NLA_DATA_LEN(nla)); return (EINVAL); } *((uint8_t *)target) = *((const uint8_t *)NL_RTA_DATA_CONST(nla)); return (0); } int nlattr_get_uint16(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { if (__predict_false(NLA_DATA_LEN(nla) != sizeof(uint16_t))) { NLMSG_REPORT_ERR_MSG(npt, "nla type %d size(%u) is not uint16", nla->nla_type, NLA_DATA_LEN(nla)); return (EINVAL); } *((uint16_t *)target) = *((const uint16_t *)NL_RTA_DATA_CONST(nla)); return (0); } int nlattr_get_uint32(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { if (__predict_false(NLA_DATA_LEN(nla) != sizeof(uint32_t))) { NLMSG_REPORT_ERR_MSG(npt, "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_uint64(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { if (__predict_false(NLA_DATA_LEN(nla) != sizeof(uint64_t))) { NLMSG_REPORT_ERR_MSG(npt, "nla type %d size(%u) is not uint64", nla->nla_type, NLA_DATA_LEN(nla)); return (EINVAL); } memcpy(target, NL_RTA_DATA_CONST(nla), sizeof(uint64_t)); return (0); } int nlattr_get_in_addr(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { if (__predict_false(NLA_DATA_LEN(nla) != sizeof(in_addr_t))) { NLMSG_REPORT_ERR_MSG(npt, "nla type %d size(%u) is not in_addr_t", nla->nla_type, NLA_DATA_LEN(nla)); return (EINVAL); } memcpy(target, NLA_DATA_CONST(nla), sizeof(in_addr_t)); return (0); } int nlattr_get_in6_addr(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { if (__predict_false(NLA_DATA_LEN(nla) != sizeof(struct in6_addr))) { NLMSG_REPORT_ERR_MSG(npt, "nla type %d size(%u) is not struct in6_addr", nla->nla_type, NLA_DATA_LEN(nla)); return (EINVAL); } memcpy(target, NLA_DATA_CONST(nla), sizeof(struct in6_addr)); return (0); } static int nlattr_get_ifp_internal(struct nlattr *nla, struct nl_pstate *npt, void *target, bool zero_ok) { if (__predict_false(NLA_DATA_LEN(nla) != sizeof(uint32_t))) { NLMSG_REPORT_ERR_MSG(npt, "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)); if (ifindex == 0 && zero_ok) { *((struct ifnet **)target) = NULL; return (0); } NET_EPOCH_ASSERT(); struct ifnet *ifp = ifnet_byindex(ifindex); if (__predict_false(ifp == NULL)) { NLMSG_REPORT_ERR_MSG(npt, "nla type %d: ifindex %u invalid", nla->nla_type, ifindex); return (ENOENT); } *((struct ifnet **)target) = ifp; NL_LOG(LOG_DEBUG3, "nla type %d: ifindex %u -> %s", nla->nla_type, ifindex, if_name(ifp)); return (0); } int nlattr_get_ifp(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { return (nlattr_get_ifp_internal(nla, npt, target, false)); } int nlattr_get_ifpz(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { return (nlattr_get_ifp_internal(nla, npt, target, true)); } int nlattr_get_chara(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { int maxlen = NLA_DATA_LEN(nla); int target_size = (size_t)arg; int len = strnlen((char *)NLA_DATA(nla), maxlen); if (__predict_false(len >= maxlen) || __predict_false(len >= target_size)) { NLMSG_REPORT_ERR_MSG(npt, "nla type %d size(%u) is not NULL-terminated or longer than %u", nla->nla_type, maxlen, target_size); return (EINVAL); } strncpy((char *)target, (char *)NLA_DATA(nla), target_size); return (0); } int nlattr_get_string(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { int maxlen = NLA_DATA_LEN(nla); if (__predict_false(strnlen((char *)NLA_DATA(nla), maxlen) >= maxlen)) { NLMSG_REPORT_ERR_MSG(npt, "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_stringn(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { int maxlen = NLA_DATA_LEN(nla); char *buf = npt_alloc(npt, maxlen + 1); if (buf == NULL) return (ENOMEM); buf[maxlen] = '\0'; memcpy(buf, NLA_DATA(nla), maxlen); *((char **)target) = buf; return (0); } int nlattr_get_bytes(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { size_t size = (size_t)arg; if (NLA_DATA_LEN(nla) != size) return (EINVAL); memcpy(target, NLA_DATA(nla), size); return (0); } int nlattr_get_nla(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { NL_LOG(LOG_DEBUG3, "STORING %p len %d", nla, nla->nla_len); *((struct nlattr **)target) = nla; return (0); } int nlattr_get_nested(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { const struct nlhdr_parser *p = (const struct nlhdr_parser *)arg; int error; /* Assumes target points to the beginning of the structure */ error = nl_parse_header(NLA_DATA(nla), NLA_DATA_LEN(nla), p, npt, target); return (error); } int nlattr_get_nested_ptr(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target) { const struct nlhdr_parser *p = (const struct nlhdr_parser *)arg; int error; /* Assumes target points to the beginning of the structure */ error = nl_parse_header(NLA_DATA(nla), NLA_DATA_LEN(nla), p, npt, *(void **)target); return (error); } int nlf_get_ifp(void *src, struct nl_pstate *npt, void *target) { int ifindex = *((const int *)src); NET_EPOCH_ASSERT(); struct ifnet *ifp = ifnet_byindex(ifindex); if (ifp == NULL) { NL_LOG(LOG_DEBUG, "ifindex %u invalid", ifindex); return (ENOENT); } *((struct ifnet **)target) = ifp; return (0); } int nlf_get_ifpz(void *src, struct nl_pstate *npt, void *target) { int ifindex = *((const int *)src); NET_EPOCH_ASSERT(); struct ifnet *ifp = ifnet_byindex(ifindex); if (ifindex != 0 && ifp == NULL) { NL_LOG(LOG_DEBUG, "ifindex %u invalid", ifindex); return (ENOENT); } *((struct ifnet **)target) = ifp; return (0); } int nlf_get_u8(void *src, struct nl_pstate *npt, void *target) { uint8_t val = *((const uint8_t *)src); *((uint8_t *)target) = val; return (0); } int nlf_get_u8_u32(void *src, struct nl_pstate *npt, void *target) { *((uint32_t *)target) = *((const uint8_t *)src); return (0); } int nlf_get_u16(void *src, struct nl_pstate *npt, void *target) { *((uint16_t *)target) = *((const uint16_t *)src); return (0); } int nlf_get_u32(void *src, struct nl_pstate *npt, void *target) { *((uint32_t *)target) = *((const uint32_t *)src); return (0); } diff --git a/sys/netlink/netlink_message_parser.h b/sys/netlink/netlink_message_parser.h index 7a417c00fc46..49bec646b29e 100644 --- a/sys/netlink/netlink_message_parser.h +++ b/sys/netlink/netlink_message_parser.h @@ -1,331 +1,333 @@ /*- * SPDX-License-Identifier: BSD-2-Clause * * 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. */ #ifndef _NETLINK_NETLINK_MESSAGE_PARSER_H_ #define _NETLINK_NETLINK_MESSAGE_PARSER_H_ #ifdef _KERNEL #include /* * It is not meant to be included directly */ /* Parsing state */ struct linear_buffer { char *base; /* Base allocated memory pointer */ uint32_t offset; /* Currently used offset */ uint32_t size; /* Total buffer size */ } __aligned(_Alignof(__max_align_t)); static inline void * lb_alloc(struct linear_buffer *lb, int len) { len = roundup2(len, _Alignof(__max_align_t)); if (lb->offset + len > lb->size) return (NULL); void *data = (void *)(lb->base + lb->offset); lb->offset += len; return (data); } static inline void lb_clear(struct linear_buffer *lb) { memset(lb->base, 0, lb->size); lb->offset = 0; } #define NL_MAX_ERROR_BUF 128 #define SCRATCH_BUFFER_SIZE (1024 + NL_MAX_ERROR_BUF) struct nl_pstate { struct linear_buffer lb; /* Per-message scratch buffer */ struct nlpcb *nlp; /* Originator socket */ struct nl_writer *nw; /* Message writer to use */ struct nlmsghdr *hdr; /* Current parsed message header */ uint32_t err_off; /* error offset from hdr start */ int error; /* last operation error */ char *err_msg; /* Description of last error */ struct nlattr *cookie; /* NLA to return to the userspace */ bool strict; /* Strict parsing required */ }; static inline void * npt_alloc(struct nl_pstate *npt, int len) { return (lb_alloc(&npt->lb, len)); } #define npt_alloc_sockaddr(_npt, _len) ((struct sockaddr *)(npt_alloc(_npt, _len))) typedef int parse_field_f(void *hdr, struct nl_pstate *npt, void *target); struct nlfield_parser { uint16_t off_in; uint16_t off_out; parse_field_f *cb; }; static const struct nlfield_parser nlf_p_empty[] = {}; int nlf_get_ifp(void *src, struct nl_pstate *npt, void *target); int nlf_get_ifpz(void *src, struct nl_pstate *npt, void *target); int nlf_get_u8(void *src, struct nl_pstate *npt, void *target); int nlf_get_u16(void *src, struct nl_pstate *npt, void *target); int nlf_get_u32(void *src, struct nl_pstate *npt, void *target); int nlf_get_u8_u32(void *src, struct nl_pstate *npt, void *target); struct nlattr_parser; typedef int parse_attr_f(struct nlattr *attr, struct nl_pstate *npt, const void *arg, void *target); struct nlattr_parser { uint16_t type; /* Attribute type */ uint16_t off; /* field offset in the target structure */ parse_attr_f *cb; /* parser function to call */ const void *arg; }; typedef bool strict_parser_f(void *hdr, struct nl_pstate *npt); typedef bool post_parser_f(void *parsed_attrs, struct nl_pstate *npt); struct nlhdr_parser { int nl_hdr_off; /* aligned netlink header size */ int out_hdr_off; /* target header size */ int fp_size; int np_size; const struct nlfield_parser *fp; /* array of header field parsers */ const struct nlattr_parser *np; /* array of attribute parsers */ strict_parser_f *sp; /* Pre-parse strict validation function */ post_parser_f *post_parse; }; #define NL_DECLARE_PARSER_EXT(_name, _t, _sp, _fp, _np, _pp) \ static const struct nlhdr_parser _name = { \ .nl_hdr_off = sizeof(_t), \ .fp = &((_fp)[0]), \ .np = &((_np)[0]), \ .fp_size = NL_ARRAY_LEN(_fp), \ .np_size = NL_ARRAY_LEN(_np), \ .sp = _sp, \ .post_parse = _pp, \ } #define NL_DECLARE_PARSER(_name, _t, _fp, _np) \ NL_DECLARE_PARSER_EXT(_name, _t, NULL, _fp, _np, NULL) #define NL_DECLARE_STRICT_PARSER(_name, _t, _sp, _fp, _np) \ NL_DECLARE_PARSER_EXT(_name, _t, _sp, _fp, _np, NULL) #define NL_DECLARE_ARR_PARSER(_name, _t, _o, _fp, _np) \ static const struct nlhdr_parser _name = { \ .nl_hdr_off = sizeof(_t), \ .out_hdr_off = sizeof(_o), \ .fp = &((_fp)[0]), \ .np = &((_np)[0]), \ .fp_size = NL_ARRAY_LEN(_fp), \ .np_size = NL_ARRAY_LEN(_np), \ } #define NL_DECLARE_ATTR_PARSER_EXT(_name, _np, _pp) \ static const struct nlhdr_parser _name = { \ .np = &((_np)[0]), \ .np_size = NL_ARRAY_LEN(_np), \ .post_parse = (_pp) \ } #define NL_DECLARE_ATTR_PARSER(_name, _np) \ NL_DECLARE_ATTR_PARSER_EXT(_name, _np, NULL) #define NL_ATTR_BMASK_SIZE 128 BITSET_DEFINE(nlattr_bmask, NL_ATTR_BMASK_SIZE); void nl_get_attrs_bmask_raw(struct nlattr *nla_head, int len, struct nlattr_bmask *bm); bool nl_has_attr(const struct nlattr_bmask *bm, unsigned int nla_type); int nl_parse_attrs_raw(struct nlattr *nla_head, int len, const struct nlattr_parser *ps, int pslen, struct nl_pstate *npt, void *target); int nlattr_get_flag(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_ip(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); +int nlattr_get_bool(struct nlattr *nla, struct nl_pstate *npt, + const void *arg, void *target); int nlattr_get_uint8(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_uint16(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_uint32(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_uint64(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_in_addr(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_in6_addr(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_ifp(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_ifpz(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_ipvia(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_chara(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_string(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_stringn(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_bytes(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_nla(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_nested(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); int nlattr_get_nested_ptr(struct nlattr *nla, struct nl_pstate *npt, const void *arg, void *target); bool nlmsg_report_err_msg(struct nl_pstate *npt, const char *fmt, ...); #define NLMSG_REPORT_ERR_MSG(_npt, _fmt, ...) { \ nlmsg_report_err_msg(_npt, _fmt, ## __VA_ARGS__); \ NLP_LOG(LOG_DEBUG, (_npt)->nlp, _fmt, ## __VA_ARGS__); \ } bool nlmsg_report_err_offset(struct nl_pstate *npt, uint32_t off); void nlmsg_report_cookie(struct nl_pstate *npt, struct nlattr *nla); void nlmsg_report_cookie_u32(struct nl_pstate *npt, uint32_t val); /* * Have it inline so compiler can optimize field accesses into * the list of direct function calls without iteration. */ static inline int nl_parse_header(void *hdr, int len, const struct nlhdr_parser *parser, struct nl_pstate *npt, void *target) { int error; if (__predict_false(len < parser->nl_hdr_off)) { if (npt->strict) { nlmsg_report_err_msg(npt, "header too short: expected %d, got %d", parser->nl_hdr_off, len); return (EINVAL); } /* Compat with older applications: pretend there's a full header */ void *tmp_hdr = npt_alloc(npt, parser->nl_hdr_off); if (tmp_hdr == NULL) return (EINVAL); memcpy(tmp_hdr, hdr, len); hdr = tmp_hdr; len = parser->nl_hdr_off; } if (npt->strict && parser->sp != NULL && !parser->sp(hdr, npt)) return (EINVAL); /* Extract fields first */ for (int i = 0; i < parser->fp_size; i++) { const struct nlfield_parser *fp = &parser->fp[i]; void *src = (char *)hdr + fp->off_in; void *dst = (char *)target + fp->off_out; error = fp->cb(src, npt, dst); if (error != 0) return (error); } struct nlattr *nla_head = (struct nlattr *)((char *)hdr + parser->nl_hdr_off); error = nl_parse_attrs_raw(nla_head, len - parser->nl_hdr_off, parser->np, parser->np_size, npt, target); if (parser->post_parse != NULL && error == 0) { if (!parser->post_parse(target, npt)) return (EINVAL); } return (error); } static inline int nl_parse_nested(struct nlattr *nla, const struct nlhdr_parser *parser, struct nl_pstate *npt, void *target) { struct nlattr *nla_head = (struct nlattr *)NLA_DATA(nla); return (nl_parse_attrs_raw(nla_head, NLA_DATA_LEN(nla), parser->np, parser->np_size, npt, target)); } /* * Checks that attributes are sorted by attribute type. */ static inline void nl_verify_parsers(const struct nlhdr_parser **parser, int count) { #ifdef INVARIANTS for (int i = 0; i < count; i++) { const struct nlhdr_parser *p = parser[i]; int attr_type = 0; for (int j = 0; j < p->np_size; j++) { MPASS(p->np[j].type > attr_type); attr_type = p->np[j].type; /* Recurse into nested objects. */ if (p->np[j].cb == nlattr_get_nested || p->np[j].cb == nlattr_get_nested_ptr) { const struct nlhdr_parser *np = (const struct nlhdr_parser *)p->np[j].arg; nl_verify_parsers(&np, 1); } } } #endif } void nl_verify_parsers(const struct nlhdr_parser **parser, int count); #define NL_VERIFY_PARSERS(_p) nl_verify_parsers((_p), NL_ARRAY_LEN(_p)) static inline int nl_parse_nlmsg(struct nlmsghdr *hdr, const struct nlhdr_parser *parser, struct nl_pstate *npt, void *target) { return (nl_parse_header(hdr + 1, hdr->nlmsg_len - sizeof(*hdr), parser, npt, target)); } static inline void nl_get_attrs_bmask_nlmsg(struct nlmsghdr *hdr, const struct nlhdr_parser *parser, struct nlattr_bmask *bm) { struct nlattr *nla_head; nla_head = (struct nlattr *)((char *)(hdr + 1) + parser->nl_hdr_off); int len = hdr->nlmsg_len - sizeof(*hdr) - parser->nl_hdr_off; nl_get_attrs_bmask_raw(nla_head, len, bm); } #endif #endif diff --git a/sys/netlink/netlink_snl.h b/sys/netlink/netlink_snl.h index 89ba5b5aef5d..6553a391a8f5 100644 --- a/sys/netlink/netlink_snl.h +++ b/sys/netlink/netlink_snl.h @@ -1,1302 +1,1308 @@ /*- * SPDX-License-Identifier: BSD-2-Clause * * 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. */ #ifndef _NETLINK_NETLINK_SNL_H_ #define _NETLINK_NETLINK_SNL_H_ /* * Simple Netlink Library */ #include #include #include #include #include #include #include #include #include #include #include #include #include #define _roundup2(x, y) (((x)+((y)-1))&(~((y)-1))) #define NETLINK_ALIGN_SIZE sizeof(uint32_t) #define NETLINK_ALIGN(_len) _roundup2(_len, NETLINK_ALIGN_SIZE) #define NLA_ALIGN_SIZE sizeof(uint32_t) #define NLA_HDRLEN ((int)sizeof(struct nlattr)) #define NLA_DATA_LEN(_nla) ((int)((_nla)->nla_len - NLA_HDRLEN)) #define NLA_DATA(_nla) NL_ITEM_DATA(_nla, NLA_HDRLEN) #define NLA_DATA_CONST(_nla) NL_ITEM_DATA_CONST(_nla, NLA_HDRLEN) #define NLA_TYPE(_nla) ((_nla)->nla_type & 0x3FFF) #define NLA_NEXT(_attr) (struct nlattr *)(void *)((char *)_attr + NLA_ALIGN(_attr->nla_len)) #define _NLA_END(_start, _len) ((char *)(_start) + (_len)) #define NLA_FOREACH(_attr, _start, _len) \ for (_attr = (_start); \ ((char *)_attr < _NLA_END(_start, _len)) && \ ((char *)NLA_NEXT(_attr) <= _NLA_END(_start, _len)); \ _attr = NLA_NEXT(_attr)) #define NL_ARRAY_LEN(_a) (sizeof(_a) / sizeof((_a)[0])) struct linear_buffer { char *base; /* Base allocated memory pointer */ uint32_t offset; /* Currently used offset */ uint32_t size; /* Total buffer size */ struct linear_buffer *next; /* Buffer chaining */ } __aligned(alignof(__max_align_t)); static inline struct linear_buffer * lb_init(uint32_t size) { struct linear_buffer *lb = calloc(1, size); if (lb != NULL) { lb->base = (char *)(lb + 1); lb->size = size - sizeof(*lb); } return (lb); } static inline void lb_free(struct linear_buffer *lb) { free(lb); } static inline char * lb_allocz(struct linear_buffer *lb, int len) { len = roundup2(len, alignof(__max_align_t)); if (lb->offset + len > lb->size) return (NULL); void *data = (void *)(lb->base + lb->offset); lb->offset += len; return (data); } static inline void lb_clear(struct linear_buffer *lb) { memset(lb->base, 0, lb->offset); lb->offset = 0; } struct snl_state { int fd; char *buf; size_t off; size_t bufsize; size_t datalen; uint32_t seq; bool init_done; struct linear_buffer *lb; }; #define SCRATCH_BUFFER_SIZE 1024 #define SNL_WRITER_BUFFER_SIZE 256 typedef void snl_parse_field_f(struct snl_state *ss, void *hdr, void *target); struct snl_field_parser { uint16_t off_in; uint16_t off_out; snl_parse_field_f *cb; }; typedef bool snl_parse_attr_f(struct snl_state *ss, struct nlattr *attr, const void *arg, void *target); struct snl_attr_parser { uint16_t type; /* Attribute type */ uint16_t off; /* field offset in the target structure */ snl_parse_attr_f *cb; /* parser function to call */ /* Optional parser argument */ union { const void *arg; const uint32_t arg_u32; }; }; typedef bool snl_parse_post_f(struct snl_state *ss, void *target); struct snl_hdr_parser { uint16_t in_hdr_size; /* Input header size */ uint16_t out_size; /* Output structure size */ uint16_t fp_size; /* Number of items in field parser */ uint16_t np_size; /* Number of items in attribute parser */ const struct snl_field_parser *fp; /* array of header field parsers */ const struct snl_attr_parser *np; /* array of attribute parsers */ snl_parse_post_f *cb_post; /* post-parse callback */ }; #define SNL_DECLARE_PARSER_EXT(_name, _sz_h_in, _sz_out, _fp, _np, _cb) \ static const struct snl_hdr_parser _name = { \ .in_hdr_size = _sz_h_in, \ .out_size = _sz_out, \ .fp = &((_fp)[0]), \ .np = &((_np)[0]), \ .fp_size = NL_ARRAY_LEN(_fp), \ .np_size = NL_ARRAY_LEN(_np), \ .cb_post = _cb, \ } #define SNL_DECLARE_PARSER(_name, _t, _fp, _np) \ SNL_DECLARE_PARSER_EXT(_name, sizeof(_t), 0, _fp, _np, NULL) #define SNL_DECLARE_FIELD_PARSER_EXT(_name, _sz_h_in, _sz_out, _fp, _cb) \ static const struct snl_hdr_parser _name = { \ .in_hdr_size = _sz_h_in, \ .out_size = _sz_out, \ .fp = &((_fp)[0]), \ .fp_size = NL_ARRAY_LEN(_fp), \ .cb_post = _cb, \ } #define SNL_DECLARE_FIELD_PARSER(_name, _t, _fp) \ SNL_DECLARE_FIELD_PARSER_EXT(_name, sizeof(_t), 0, _fp, NULL) #define SNL_DECLARE_ATTR_PARSER_EXT(_name, _sz_out, _np, _cb) \ static const struct snl_hdr_parser _name = { \ .out_size = _sz_out, \ .np = &((_np)[0]), \ .np_size = NL_ARRAY_LEN(_np), \ .cb_post = _cb, \ } #define SNL_DECLARE_ATTR_PARSER(_name, _np) \ SNL_DECLARE_ATTR_PARSER_EXT(_name, 0, _np, NULL) static inline void * snl_allocz(struct snl_state *ss, int len) { void *data = lb_allocz(ss->lb, len); if (data == NULL) { uint32_t size = ss->lb->size * 2; while (size < len + sizeof(struct linear_buffer)) size *= 2; struct linear_buffer *lb = lb_init(size); if (lb != NULL) { lb->next = ss->lb; ss->lb = lb; data = lb_allocz(ss->lb, len); } } return (data); } static inline void snl_clear_lb(struct snl_state *ss) { struct linear_buffer *lb = ss->lb; lb_clear(lb); lb = lb->next; ss->lb->next = NULL; /* Remove all linear bufs except the largest one */ while (lb != NULL) { struct linear_buffer *lb_next = lb->next; lb_free(lb); lb = lb_next; } } static void snl_free(struct snl_state *ss) { if (ss->init_done) { close(ss->fd); if (ss->buf != NULL) free(ss->buf); if (ss->lb != NULL) { snl_clear_lb(ss); lb_free(ss->lb); } } } static inline bool snl_init(struct snl_state *ss, int netlink_family) { memset(ss, 0, sizeof(*ss)); ss->fd = socket(AF_NETLINK, SOCK_RAW, netlink_family); if (ss->fd == -1) return (false); ss->init_done = true; int val = 1; socklen_t optlen = sizeof(val); if (setsockopt(ss->fd, SOL_NETLINK, NETLINK_EXT_ACK, &val, optlen) == -1) { snl_free(ss); return (false); } int rcvbuf; if (getsockopt(ss->fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf, &optlen) == -1) { snl_free(ss); return (false); } ss->bufsize = rcvbuf; ss->buf = malloc(ss->bufsize); if (ss->buf == NULL) { snl_free(ss); return (false); } ss->lb = lb_init(SCRATCH_BUFFER_SIZE); if (ss->lb == NULL) { snl_free(ss); return (false); } return (true); } static inline bool snl_send(struct snl_state *ss, void *data, int sz) { return (send(ss->fd, data, sz, 0) == sz); } static inline bool snl_send_message(struct snl_state *ss, struct nlmsghdr *hdr) { ssize_t sz = NLMSG_ALIGN(hdr->nlmsg_len); return (send(ss->fd, hdr, sz, 0) == sz); } static inline uint32_t snl_get_seq(struct snl_state *ss) { return (++ss->seq); } struct snl_msg_info { int cmsg_type; int cmsg_level; uint32_t process_id; uint8_t port_id; uint8_t seq_id; }; static inline bool parse_cmsg(struct snl_state *ss, const struct msghdr *msg, struct snl_msg_info *attrs); static inline struct nlmsghdr * snl_read_message_dbg(struct snl_state *ss, struct snl_msg_info *cinfo) { memset(cinfo, 0, sizeof(*cinfo)); if (ss->off == ss->datalen) { struct sockaddr_nl nladdr; char cbuf[64]; struct iovec iov = { .iov_base = ss->buf, .iov_len = ss->bufsize, }; struct msghdr msg = { .msg_name = &nladdr, .msg_namelen = sizeof(nladdr), .msg_iov = &iov, .msg_iovlen = 1, .msg_control = cbuf, .msg_controllen = sizeof(cbuf), }; ss->off = 0; ss->datalen = 0; for (;;) { ssize_t datalen = recvmsg(ss->fd, &msg, 0); if (datalen > 0) { ss->datalen = datalen; parse_cmsg(ss, &msg, cinfo); break; } else if (errno != EINTR) return (NULL); } } struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off]; ss->off += NLMSG_ALIGN(hdr->nlmsg_len); return (hdr); } static inline struct nlmsghdr * snl_read_message(struct snl_state *ss) { if (ss->off == ss->datalen) { struct sockaddr_nl nladdr; struct iovec iov = { .iov_base = ss->buf, .iov_len = ss->bufsize, }; struct msghdr msg = { .msg_name = &nladdr, .msg_namelen = sizeof(nladdr), .msg_iov = &iov, .msg_iovlen = 1, }; ss->off = 0; ss->datalen = 0; for (;;) { ssize_t datalen = recvmsg(ss->fd, &msg, 0); if (datalen > 0) { ss->datalen = datalen; break; } else if (errno != EINTR) return (NULL); } } struct nlmsghdr *hdr = (struct nlmsghdr *)(void *)&ss->buf[ss->off]; ss->off += NLMSG_ALIGN(hdr->nlmsg_len); return (hdr); } static inline struct nlmsghdr * snl_read_reply(struct snl_state *ss, uint32_t nlmsg_seq) { struct nlmsghdr *hdr; while ((hdr = snl_read_message(ss)) != NULL) { if (hdr->nlmsg_seq == nlmsg_seq) return (hdr); } return (NULL); } /* * Checks that attributes are sorted by attribute type. */ static inline void snl_verify_parsers(const struct snl_hdr_parser **parser, int count) { for (int i = 0; i < count; i++) { const struct snl_hdr_parser *p = parser[i]; int attr_type = 0; for (int j = 0; j < p->np_size; j++) { assert(p->np[j].type > attr_type); attr_type = p->np[j].type; } } } #define SNL_VERIFY_PARSERS(_p) snl_verify_parsers((_p), NL_ARRAY_LEN(_p)) static const struct snl_attr_parser * find_parser(const struct snl_attr_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); } static inline bool snl_parse_attrs_raw(struct snl_state *ss, struct nlattr *nla_head, int len, const struct snl_attr_parser *ps, int pslen, void *target) { struct nlattr *nla; NLA_FOREACH(nla, nla_head, len) { if (nla->nla_len < sizeof(struct nlattr)) return (false); int nla_type = nla->nla_type & NLA_TYPE_MASK; const struct snl_attr_parser *s = find_parser(ps, pslen, nla_type); if (s != NULL) { void *ptr = (void *)((char *)target + s->off); if (!s->cb(ss, nla, s->arg, ptr)) return (false); } } return (true); } static inline bool snl_parse_attrs(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen, const struct snl_attr_parser *ps, int pslen, void *target) { int off = NLMSG_HDRLEN + NETLINK_ALIGN(hdrlen); int len = hdr->nlmsg_len - off; struct nlattr *nla_head = (struct nlattr *)(void *)((char *)hdr + off); return (snl_parse_attrs_raw(ss, nla_head, len, ps, pslen, target)); } static inline void snl_parse_fields(struct snl_state *ss, struct nlmsghdr *hdr, int hdrlen __unused, const struct snl_field_parser *ps, int pslen, void *target) { for (int i = 0; i < pslen; i++) { const struct snl_field_parser *fp = &ps[i]; void *src = (char *)hdr + fp->off_in; void *dst = (char *)target + fp->off_out; fp->cb(ss, src, dst); } } static inline bool snl_parse_header(struct snl_state *ss, void *hdr, int len, const struct snl_hdr_parser *parser, void *target) { struct nlattr *nla_head; /* Extract fields first (if any) */ snl_parse_fields(ss, hdr, parser->in_hdr_size, parser->fp, parser->fp_size, target); nla_head = (struct nlattr *)(void *)((char *)hdr + parser->in_hdr_size); bool result = snl_parse_attrs_raw(ss, nla_head, len - parser->in_hdr_size, parser->np, parser->np_size, target); if (result && parser->cb_post != NULL) result = parser->cb_post(ss, target); return (result); } static inline bool snl_parse_nlmsg(struct snl_state *ss, struct nlmsghdr *hdr, const struct snl_hdr_parser *parser, void *target) { return (snl_parse_header(ss, hdr + 1, hdr->nlmsg_len - sizeof(*hdr), parser, target)); } static inline bool snl_attr_get_flag(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { if (NLA_DATA_LEN(nla) == 0) { *((uint8_t *)target) = 1; return (true); } return (false); } static inline bool snl_attr_get_bytes(struct snl_state *ss __unused, struct nlattr *nla, const void *arg, void *target) { if ((size_t)NLA_DATA_LEN(nla) != (size_t)arg) return (false); memcpy(target, NLA_DATA_CONST(nla), (size_t)arg); return (true); } static inline bool snl_attr_get_uint8(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { if (NLA_DATA_LEN(nla) == sizeof(uint8_t)) { *((uint8_t *)target) = *((const uint8_t *)NLA_DATA_CONST(nla)); return (true); } return (false); } static inline bool snl_attr_get_uint16(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { if (NLA_DATA_LEN(nla) == sizeof(uint16_t)) { *((uint16_t *)target) = *((const uint16_t *)NLA_DATA_CONST(nla)); return (true); } return (false); } static inline bool snl_attr_get_uint32(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { if (NLA_DATA_LEN(nla) == sizeof(uint32_t)) { *((uint32_t *)target) = *((const uint32_t *)NLA_DATA_CONST(nla)); return (true); } return (false); } static inline bool snl_attr_get_uint64(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { if (NLA_DATA_LEN(nla) == sizeof(uint64_t)) { memcpy(target, NLA_DATA_CONST(nla), sizeof(uint64_t)); return (true); } return (false); } static inline bool snl_attr_get_int8(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) { return (snl_attr_get_uint8(ss, nla, arg, target)); } static inline bool snl_attr_get_int16(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) { return (snl_attr_get_uint16(ss, nla, arg, target)); } static inline bool snl_attr_get_int32(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) { return (snl_attr_get_uint32(ss, nla, arg, target)); } static inline bool snl_attr_get_int64(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) { return (snl_attr_get_uint64(ss, nla, arg, target)); } static inline bool snl_attr_get_string(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { size_t maxlen = NLA_DATA_LEN(nla); if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) { *((char **)target) = (char *)NLA_DATA(nla); return (true); } return (false); } static inline bool snl_attr_get_stringn(struct snl_state *ss, struct nlattr *nla, const void *arg __unused, void *target) { int maxlen = NLA_DATA_LEN(nla); char *buf = snl_allocz(ss, maxlen + 1); if (buf == NULL) return (false); buf[maxlen] = '\0'; memcpy(buf, NLA_DATA(nla), maxlen); *((char **)target) = buf; return (true); } static inline bool snl_attr_copy_string(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) { char *tmp; if (snl_attr_get_string(ss, nla, NULL, &tmp)) { strlcpy(target, tmp, (size_t)arg); return (true); } return (false); } static inline bool snl_attr_dup_string(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { size_t maxlen = NLA_DATA_LEN(nla); if (strnlen((char *)NLA_DATA(nla), maxlen) < maxlen) { char *buf = snl_allocz(ss, maxlen); if (buf == NULL) return (false); memcpy(buf, NLA_DATA(nla), maxlen); *((char **)target) = buf; return (true); } return (false); } static inline bool snl_attr_get_nested(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) { const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg; /* Assumes target points to the beginning of the structure */ return (snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, target)); } struct snl_parray { uint32_t count; void **items; }; static inline bool snl_attr_get_parray_sz(struct snl_state *ss, struct nlattr *container_nla, uint32_t start_size, const void *arg, void *target) { const struct snl_hdr_parser *p = (const struct snl_hdr_parser *)arg; struct snl_parray *array = target; struct nlattr *nla; uint32_t count = 0, size = start_size; if (p->out_size == 0) return (false); array->items = snl_allocz(ss, size * sizeof(void *)); if (array->items == NULL) return (false); /* * If the provided parser is an attribute parser, assume that each * nla in the container nla is the container nla itself and parse * the contents of this nla. * Otherwise, run the parser on raw data, assuming the header of this * data has u16 field with total size in the beginning. */ uint32_t data_off = 0; if (p->in_hdr_size == 0) data_off = sizeof(struct nlattr); NLA_FOREACH(nla, NLA_DATA(container_nla), NLA_DATA_LEN(container_nla)) { void *item = snl_allocz(ss, p->out_size); if (item == NULL) return (false); void *data = (char *)(void *)nla + data_off; int data_len = nla->nla_len - data_off; if (!(snl_parse_header(ss, data, data_len, p, item))) return (false); if (count == size) { uint32_t new_size = size * 2; void **new_array = snl_allocz(ss, new_size *sizeof(void *)); memcpy(new_array, array->items, size * sizeof(void *)); array->items = new_array; size = new_size; } array->items[count++] = item; } array->count = count; return (true); } /* * Parses and stores the unknown-size array. * Assumes each array item is a container and the NLAs in the container are parsable * by the parser provided in @arg. * Assumes @target is struct snl_parray */ static inline bool snl_attr_get_parray(struct snl_state *ss, struct nlattr *nla, const void *arg, void *target) { return (snl_attr_get_parray_sz(ss, nla, 8, arg, target)); } static inline bool snl_attr_get_nla(struct snl_state *ss __unused, struct nlattr *nla, const void *arg __unused, void *target) { *((struct nlattr **)target) = nla; return (true); } static inline bool snl_attr_dup_nla(struct snl_state *ss, struct nlattr *nla, const void *arg __unused, void *target) { void *ptr = snl_allocz(ss, nla->nla_len); if (ptr != NULL) { memcpy(ptr, nla, nla->nla_len); *((void **)target) = ptr; return (true); } return (false); } static inline bool snl_attr_copy_struct(struct snl_state *ss, struct nlattr *nla, const void *arg __unused, void *target) { void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla)); if (ptr != NULL) { memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla)); *((void **)target) = ptr; return (true); } return (false); } static inline bool snl_attr_dup_struct(struct snl_state *ss, struct nlattr *nla, const void *arg __unused, void *target) { void *ptr = snl_allocz(ss, NLA_DATA_LEN(nla)); if (ptr != NULL) { memcpy(ptr, NLA_DATA(nla), NLA_DATA_LEN(nla)); *((void **)target) = ptr; return (true); } return (false); } struct snl_attr_bit { uint32_t bit_index; char *bit_name; int bit_value; }; struct snl_attr_bits { uint32_t num_bits; struct snl_attr_bit **bits; }; #define _OUT(_field) offsetof(struct snl_attr_bit, _field) static const struct snl_attr_parser _nla_p_bit[] = { { .type = NLA_BITSET_BIT_INDEX, .off = _OUT(bit_index), .cb = snl_attr_get_uint32 }, { .type = NLA_BITSET_BIT_NAME, .off = _OUT(bit_name), .cb = snl_attr_dup_string }, { .type = NLA_BITSET_BIT_VALUE, .off = _OUT(bit_value), .cb = snl_attr_get_flag }, }; #undef _OUT SNL_DECLARE_ATTR_PARSER_EXT(_nla_bit_parser, sizeof(struct snl_attr_bit), _nla_p_bit, NULL); struct snl_attr_bitset { uint32_t nla_bitset_size; uint32_t *nla_bitset_mask; uint32_t *nla_bitset_value; struct snl_attr_bits bits; }; #define _OUT(_field) offsetof(struct snl_attr_bitset, _field) static const struct snl_attr_parser _nla_p_bitset[] = { { .type = NLA_BITSET_SIZE, .off = _OUT(nla_bitset_size), .cb = snl_attr_get_uint32 }, { .type = NLA_BITSET_BITS, .off = _OUT(bits), .cb = snl_attr_get_parray, .arg = &_nla_bit_parser }, { .type = NLA_BITSET_VALUE, .off = _OUT(nla_bitset_mask), .cb = snl_attr_dup_nla }, { .type = NLA_BITSET_MASK, .off = _OUT(nla_bitset_value), .cb = snl_attr_dup_nla }, }; static inline bool _cb_p_bitset(struct snl_state *ss __unused, void *_target) { struct snl_attr_bitset *target = _target; uint32_t sz_bytes = _roundup2(target->nla_bitset_size, 32) / 8; if (target->nla_bitset_mask != NULL) { struct nlattr *nla = (struct nlattr *)target->nla_bitset_mask; uint32_t data_len = NLA_DATA_LEN(nla); if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len) return (false); target->nla_bitset_mask = (uint32_t *)NLA_DATA(nla); } if (target->nla_bitset_value != NULL) { struct nlattr *nla = (struct nlattr *)target->nla_bitset_value; uint32_t data_len = NLA_DATA_LEN(nla); if (data_len != sz_bytes || _roundup2(data_len, 4) != data_len) return (false); target->nla_bitset_value = (uint32_t *)NLA_DATA(nla); } return (true); } #undef _OUT SNL_DECLARE_ATTR_PARSER_EXT(_nla_bitset_parser, sizeof(struct snl_attr_bitset), _nla_p_bitset, _cb_p_bitset); /* * Parses the compact bitset representation. */ static inline bool snl_attr_get_bitset_c(struct snl_state *ss, struct nlattr *nla, const void *arg __unused, void *_target) { const struct snl_hdr_parser *p = &_nla_bitset_parser; struct snl_attr_bitset *target = _target; /* Assumes target points to the beginning of the structure */ if (!snl_parse_header(ss, NLA_DATA(nla), NLA_DATA_LEN(nla), p, _target)) return (false); if (target->nla_bitset_mask == NULL || target->nla_bitset_value == NULL) return (false); return (true); } static inline void snl_field_get_uint8(struct snl_state *ss __unused, void *src, void *target) { *((uint8_t *)target) = *((uint8_t *)src); } static inline void snl_field_get_uint16(struct snl_state *ss __unused, void *src, void *target) { *((uint16_t *)target) = *((uint16_t *)src); } static inline void snl_field_get_uint32(struct snl_state *ss __unused, void *src, void *target) { *((uint32_t *)target) = *((uint32_t *)src); } static inline void snl_field_get_ptr(struct snl_state *ss __unused, void *src, void *target) { *((void **)target) = src; } struct snl_errmsg_data { struct nlmsghdr *orig_hdr; int error; uint32_t error_offs; char *error_str; struct nlattr *cookie; }; #define _IN(_field) offsetof(struct nlmsgerr, _field) #define _OUT(_field) offsetof(struct snl_errmsg_data, _field) static const struct snl_attr_parser nla_p_errmsg[] = { { .type = NLMSGERR_ATTR_MSG, .off = _OUT(error_str), .cb = snl_attr_get_string }, { .type = NLMSGERR_ATTR_OFFS, .off = _OUT(error_offs), .cb = snl_attr_get_uint32 }, { .type = NLMSGERR_ATTR_COOKIE, .off = _OUT(cookie), .cb = snl_attr_get_nla }, }; static const struct snl_field_parser nlf_p_errmsg[] = { { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 }, { .off_in = _IN(msg), .off_out = _OUT(orig_hdr), .cb = snl_field_get_ptr }, }; #undef _IN #undef _OUT SNL_DECLARE_PARSER(snl_errmsg_parser, struct nlmsgerr, nlf_p_errmsg, nla_p_errmsg); #define _IN(_field) offsetof(struct nlmsgerr, _field) #define _OUT(_field) offsetof(struct snl_errmsg_data, _field) static const struct snl_field_parser nlf_p_donemsg[] = { { .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 }, }; #undef _IN #undef _OUT SNL_DECLARE_FIELD_PARSER(snl_donemsg_parser, struct nlmsgerr, nlf_p_donemsg); static inline bool snl_parse_errmsg(struct snl_state *ss, struct nlmsghdr *hdr, struct snl_errmsg_data *e) { if ((hdr->nlmsg_flags & NLM_F_CAPPED) != 0) return (snl_parse_nlmsg(ss, hdr, &snl_errmsg_parser, e)); const struct snl_hdr_parser *ps = &snl_errmsg_parser; struct nlmsgerr *errmsg = (struct nlmsgerr *)(hdr + 1); int hdrlen = sizeof(int) + NLMSG_ALIGN(errmsg->msg.nlmsg_len); struct nlattr *attr_head = (struct nlattr *)(void *)((char *)errmsg + hdrlen); int attr_len = hdr->nlmsg_len - sizeof(struct nlmsghdr) - hdrlen; snl_parse_fields(ss, (struct nlmsghdr *)errmsg, hdrlen, ps->fp, ps->fp_size, e); return (snl_parse_attrs_raw(ss, attr_head, attr_len, ps->np, ps->np_size, e)); } static inline bool snl_read_reply_code(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e) { struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq); if (hdr == NULL) { e->error = EINVAL; } else if (hdr->nlmsg_type == NLMSG_ERROR) { if (!snl_parse_errmsg(ss, hdr, e)) e->error = EINVAL; return (e->error == 0); } return (false); } #define _OUT(_field) offsetof(struct snl_msg_info, _field) static const struct snl_attr_parser _nla_p_cinfo[] = { { .type = NLMSGINFO_ATTR_PROCESS_ID, .off = _OUT(process_id), .cb = snl_attr_get_uint32 }, { .type = NLMSGINFO_ATTR_PORT_ID, .off = _OUT(port_id), .cb = snl_attr_get_uint32 }, { .type = NLMSGINFO_ATTR_SEQ_ID, .off = _OUT(seq_id), .cb = snl_attr_get_uint32 }, }; #undef _OUT SNL_DECLARE_ATTR_PARSER(snl_msg_info_parser, _nla_p_cinfo); static inline bool parse_cmsg(struct snl_state *ss, const struct msghdr *msg, struct snl_msg_info *attrs) { for (struct cmsghdr *cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL; cmsg = CMSG_NXTHDR(msg, cmsg)) { if (cmsg->cmsg_level != SOL_NETLINK || cmsg->cmsg_type != NETLINK_MSG_INFO) continue; void *data = CMSG_DATA(cmsg); int len = cmsg->cmsg_len - ((char *)data - (char *)cmsg); const struct snl_hdr_parser *ps = &snl_msg_info_parser; return (snl_parse_attrs_raw(ss, data, len, ps->np, ps->np_size, attrs)); } return (false); } /* * Assumes e is zeroed */ static inline struct nlmsghdr * snl_read_reply_multi(struct snl_state *ss, uint32_t nlmsg_seq, struct snl_errmsg_data *e) { struct nlmsghdr *hdr = snl_read_reply(ss, nlmsg_seq); if (hdr == NULL) { e->error = EINVAL; } else if (hdr->nlmsg_type == NLMSG_ERROR) { if (!snl_parse_errmsg(ss, hdr, e)) e->error = EINVAL; } else if (hdr->nlmsg_type == NLMSG_DONE) { snl_parse_nlmsg(ss, hdr, &snl_donemsg_parser, e); } else return (hdr); return (NULL); } /* writer logic */ struct snl_writer { char *base; uint32_t offset; uint32_t size; struct nlmsghdr *hdr; struct snl_state *ss; bool error; }; static inline void snl_init_writer(struct snl_state *ss, struct snl_writer *nw) { nw->size = SNL_WRITER_BUFFER_SIZE; nw->base = snl_allocz(ss, nw->size); if (nw->base == NULL) { nw->error = true; nw->size = 0; } nw->offset = 0; nw->hdr = NULL; nw->error = false; nw->ss = ss; } static inline bool snl_realloc_msg_buffer(struct snl_writer *nw, size_t sz) { uint32_t new_size = nw->size * 2; while (new_size < nw->size + sz) new_size *= 2; if (nw->error) return (false); if (snl_allocz(nw->ss, new_size) == NULL) { nw->error = true; return (false); } nw->size = new_size; void *new_base = nw->ss->lb->base; if (new_base != nw->base) { memcpy(new_base, nw->base, nw->offset); if (nw->hdr != NULL) { int hdr_off = (char *)(nw->hdr) - nw->base; nw->hdr = (struct nlmsghdr *) (void *)((char *)new_base + hdr_off); } nw->base = new_base; } return (true); } static inline void * snl_reserve_msg_data_raw(struct snl_writer *nw, size_t sz) { sz = NETLINK_ALIGN(sz); if (__predict_false(nw->offset + sz > nw->size)) { if (!snl_realloc_msg_buffer(nw, sz)) return (NULL); } void *data_ptr = &nw->base[nw->offset]; nw->offset += sz; return (data_ptr); } #define snl_reserve_msg_object(_ns, _t) ((_t *)snl_reserve_msg_data_raw(_ns, sizeof(_t))) #define snl_reserve_msg_data(_ns, _sz, _t) ((_t *)snl_reserve_msg_data_raw(_ns, _sz)) static inline void * _snl_reserve_msg_attr(struct snl_writer *nw, uint16_t nla_type, uint16_t sz) { sz += sizeof(struct nlattr); struct nlattr *nla = snl_reserve_msg_data(nw, sz, struct nlattr); if (__predict_false(nla == NULL)) return (NULL); nla->nla_type = nla_type; nla->nla_len = sz; return ((void *)(nla + 1)); } #define snl_reserve_msg_attr(_ns, _at, _t) ((_t *)_snl_reserve_msg_attr(_ns, _at, sizeof(_t))) static inline bool snl_add_msg_attr(struct snl_writer *nw, int attr_type, int attr_len, const void *data) { int required_len = NLA_ALIGN(attr_len + sizeof(struct nlattr)); if (__predict_false(nw->offset + required_len > nw->size)) { if (!snl_realloc_msg_buffer(nw, required_len)) return (false); } struct nlattr *nla = (struct nlattr *)(void *)(&nw->base[nw->offset]); nla->nla_len = attr_len + sizeof(struct nlattr); nla->nla_type = attr_type; if (attr_len > 0) { if ((attr_len % 4) != 0) { /* clear padding bytes */ bzero((char *)nla + required_len - 4, 4); } memcpy((nla + 1), data, attr_len); } nw->offset += required_len; return (true); } static inline bool snl_add_msg_attr_raw(struct snl_writer *nw, const struct nlattr *nla_src) { int attr_len = nla_src->nla_len - sizeof(struct nlattr); assert(attr_len >= 0); return (snl_add_msg_attr(nw, nla_src->nla_type, attr_len, (const void *)(nla_src + 1))); } +static inline bool +snl_add_msg_attr_bool(struct snl_writer *nw, int attrtype, bool value) +{ + return (snl_add_msg_attr(nw, attrtype, sizeof(bool), &value)); +} + static inline bool snl_add_msg_attr_u8(struct snl_writer *nw, int attrtype, uint8_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(uint8_t), &value)); } static inline bool snl_add_msg_attr_u16(struct snl_writer *nw, int attrtype, uint16_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(uint16_t), &value)); } static inline bool snl_add_msg_attr_u32(struct snl_writer *nw, int attrtype, uint32_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(uint32_t), &value)); } static inline bool snl_add_msg_attr_u64(struct snl_writer *nw, int attrtype, uint64_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(uint64_t), &value)); } static inline bool snl_add_msg_attr_s8(struct snl_writer *nw, int attrtype, int8_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(int8_t), &value)); } static inline bool snl_add_msg_attr_s16(struct snl_writer *nw, int attrtype, int16_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(int16_t), &value)); } static inline bool snl_add_msg_attr_s32(struct snl_writer *nw, int attrtype, int32_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(int32_t), &value)); } static inline bool snl_add_msg_attr_s64(struct snl_writer *nw, int attrtype, int64_t value) { return (snl_add_msg_attr(nw, attrtype, sizeof(int64_t), &value)); } static inline bool snl_add_msg_attr_flag(struct snl_writer *nw, int attrtype) { return (snl_add_msg_attr(nw, attrtype, 0, NULL)); } static inline bool snl_add_msg_attr_string(struct snl_writer *nw, int attrtype, const char *str) { return (snl_add_msg_attr(nw, attrtype, strlen(str) + 1, str)); } static inline int snl_get_msg_offset(const struct snl_writer *nw) { return (nw->offset - ((char *)nw->hdr - nw->base)); } static inline void * _snl_restore_msg_offset(const struct snl_writer *nw, int off) { return ((void *)((char *)nw->hdr + off)); } #define snl_restore_msg_offset(_ns, _off, _t) ((_t *)_snl_restore_msg_offset(_ns, _off)) static inline int snl_add_msg_attr_nested(struct snl_writer *nw, int attrtype) { int off = snl_get_msg_offset(nw); struct nlattr *nla = snl_reserve_msg_data(nw, sizeof(struct nlattr), struct nlattr); if (__predict_false(nla == NULL)) return (0); nla->nla_type = attrtype; return (off); } static inline void snl_end_attr_nested(const struct snl_writer *nw, int off) { if (!nw->error) { struct nlattr *nla = snl_restore_msg_offset(nw, off, struct nlattr); nla->nla_len = NETLINK_ALIGN(snl_get_msg_offset(nw) - off); } } static inline struct nlmsghdr * snl_create_msg_request(struct snl_writer *nw, int nlmsg_type) { assert(nw->hdr == NULL); struct nlmsghdr *hdr = snl_reserve_msg_object(nw, struct nlmsghdr); hdr->nlmsg_type = nlmsg_type; hdr->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; nw->hdr = hdr; return (hdr); } static void snl_abort_msg(struct snl_writer *nw) { if (nw->hdr != NULL) { int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr); nw->offset -= offset; nw->hdr = NULL; } } static inline struct nlmsghdr * snl_finalize_msg(struct snl_writer *nw) { if (nw->error) snl_abort_msg(nw); if (nw->hdr != NULL) { struct nlmsghdr *hdr = nw->hdr; int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr); hdr->nlmsg_len = offset; hdr->nlmsg_seq = snl_get_seq(nw->ss); nw->hdr = NULL; return (hdr); } return (NULL); } static inline bool snl_send_msgs(struct snl_writer *nw) { int offset = nw->offset; assert(nw->hdr == NULL); nw->offset = 0; return (snl_send(nw->ss, nw->base, offset)); } static const struct snl_hdr_parser *snl_all_core_parsers[] = { &snl_errmsg_parser, &snl_donemsg_parser, &_nla_bit_parser, &_nla_bitset_parser, }; #endif