Adding upstream version 255.4.upstream/255.4

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 488 insertions, 0 deletions

diff --git a/src/libsystemd/sd-netlink/netlink-genl.c b/src/libsystemd/sd-netlink/netlink-genl.c
new file mode 100644
index 0000000..1dc62e8
--- /dev/null
+++ b/src/libsystemd/sd-netlink/netlink-genl.c
@@ -0,0 +1,488 @@
+/* SPDX-License-Identifier: LGPL-2.1-or-later */
+
+#include <linux/genetlink.h>
+
+#include "sd-netlink.h"
+
+#include "alloc-util.h"
+#include "netlink-genl.h"
+#include "netlink-internal.h"
+#include "netlink-types.h"
+
+typedef struct GenericNetlinkFamily {
+        sd_netlink *genl;
+
+        const NLAPolicySet *policy_set;
+
+        uint16_t id; /* a.k.a nlmsg_type */
+        char *name;
+        uint32_t version;
+        uint32_t additional_header_size;
+        Hashmap *multicast_group_by_name;
+} GenericNetlinkFamily;
+
+static const GenericNetlinkFamily nlctrl_static = {
+        .id = GENL_ID_CTRL,
+        .name = (char*) CTRL_GENL_NAME,
+        .version = 0x01,
+};
+
+static GenericNetlinkFamily *genl_family_free(GenericNetlinkFamily *f) {
+        if (!f)
+                return NULL;
+
+        if (f->genl) {
+                if (f->id > 0)
+                        hashmap_remove(f->genl->genl_family_by_id, UINT_TO_PTR(f->id));
+                if (f->name)
+                        hashmap_remove(f->genl->genl_family_by_name, f->name);
+        }
+
+        free(f->name);
+        hashmap_free(f->multicast_group_by_name);
+
+        return mfree(f);
+}
+
+DEFINE_TRIVIAL_CLEANUP_FUNC(GenericNetlinkFamily*, genl_family_free);
+
+void genl_clear_family(sd_netlink *nl) {
+        assert(nl);
+
+        nl->genl_family_by_name = hashmap_free_with_destructor(nl->genl_family_by_name, genl_family_free);
+        nl->genl_family_by_id = hashmap_free_with_destructor(nl->genl_family_by_id, genl_family_free);
+}
+
+static int genl_family_new_unsupported(
+                sd_netlink *nl,
+                const char *family_name,
+                const NLAPolicySet *policy_set) {
+
+        _cleanup_(genl_family_freep) GenericNetlinkFamily *f = NULL;
+        int r;
+
+        assert(nl);
+        assert(family_name);
+        assert(policy_set);
+
+        /* Kernel does not support the genl family? To prevent from resolving the family name again,
+         * let's store the family with zero id to indicate that. */
+
+        f = new(GenericNetlinkFamily, 1);
+        if (!f)
+                return -ENOMEM;
+
+        *f = (GenericNetlinkFamily) {
+                .policy_set = policy_set,
+        };
+
+        f->name = strdup(family_name);
+        if (!f->name)
+                return -ENOMEM;
+
+        r = hashmap_ensure_put(&nl->genl_family_by_name, &string_hash_ops, f->name, f);
+        if (r < 0)
+                return r;
+
+        f->genl = nl;
+        TAKE_PTR(f);
+        return 0;
+}
+
+static int genl_family_new(
+                sd_netlink *nl,
+                const char *expected_family_name,
+                const NLAPolicySet *policy_set,
+                sd_netlink_message *message,
+                const GenericNetlinkFamily **ret) {
+
+        _cleanup_(genl_family_freep) GenericNetlinkFamily *f = NULL;
+        const char *family_name;
+        uint8_t cmd;
+        int r;
+
+        assert(nl);
+        assert(expected_family_name);
+        assert(policy_set);
+        assert(message);
+        assert(ret);
+
+        f = new(GenericNetlinkFamily, 1);
+        if (!f)
+                return -ENOMEM;
+
+        *f = (GenericNetlinkFamily) {
+                .policy_set = policy_set,
+        };
+
+        r = sd_genl_message_get_family_name(nl, message, &family_name);
+        if (r < 0)
+                return r;
+
+        if (!streq(family_name, CTRL_GENL_NAME))
+                return -EINVAL;
+
+        r = sd_genl_message_get_command(nl, message, &cmd);
+        if (r < 0)
+                return r;
+
+        if (cmd != CTRL_CMD_NEWFAMILY)
+                return -EINVAL;
+
+        r = sd_netlink_message_read_u16(message, CTRL_ATTR_FAMILY_ID, &f->id);
+        if (r < 0)
+                return r;
+
+        r = sd_netlink_message_read_string_strdup(message, CTRL_ATTR_FAMILY_NAME, &f->name);
+        if (r < 0)
+                return r;
+
+        if (!streq(f->name, expected_family_name))
+                return -EINVAL;
+
+        r = sd_netlink_message_read_u32(message, CTRL_ATTR_VERSION, &f->version);
+        if (r < 0)
+                return r;
+
+        r = sd_netlink_message_read_u32(message, CTRL_ATTR_HDRSIZE, &f->additional_header_size);
+        if (r < 0)
+                return r;
+
+        r = sd_netlink_message_enter_container(message, CTRL_ATTR_MCAST_GROUPS);
+        if (r >= 0) {
+                for (uint16_t i = 0; i < UINT16_MAX; i++) {
+                        _cleanup_free_ char *group_name = NULL;
+                        uint32_t group_id;
+
+                        r = sd_netlink_message_enter_array(message, i + 1);
+                        if (r == -ENODATA)
+                                break;
+                        if (r < 0)
+                                return r;
+
+                        r = sd_netlink_message_read_u32(message, CTRL_ATTR_MCAST_GRP_ID, &group_id);
+                        if (r < 0)
+                                return r;
+
+                        r = sd_netlink_message_read_string_strdup(message, CTRL_ATTR_MCAST_GRP_NAME, &group_name);
+                        if (r < 0)
+                                return r;
+
+                        r = sd_netlink_message_exit_container(message);
+                        if (r < 0)
+                                return r;
+
+                        if (group_id == 0) {
+                                log_debug("sd-netlink: received multicast group '%s' for generic netlink family '%s' with id == 0, ignoring",
+                                          group_name, f->name);
+                                continue;
+                        }
+
+                        r = hashmap_ensure_put(&f->multicast_group_by_name, &string_hash_ops_free, group_name, UINT32_TO_PTR(group_id));
+                        if (r < 0)
+                                return r;
+
+                        TAKE_PTR(group_name);
+                }
+
+                r = sd_netlink_message_exit_container(message);
+                if (r < 0)
+                        return r;
+        }
+
+        r = hashmap_ensure_put(&nl->genl_family_by_id, NULL, UINT_TO_PTR(f->id), f);
+        if (r < 0)
+                return r;
+
+        r = hashmap_ensure_put(&nl->genl_family_by_name, &string_hash_ops, f->name, f);
+        if (r < 0) {
+                hashmap_remove(nl->genl_family_by_id, UINT_TO_PTR(f->id));
+                return r;
+        }
+
+        f->genl = nl;
+        *ret = TAKE_PTR(f);
+        return 0;
+}
+
+static const NLAPolicySet *genl_family_get_policy_set(const GenericNetlinkFamily *family) {
+        assert(family);
+
+        if (family->policy_set)
+                return family->policy_set;
+
+        return genl_get_policy_set_by_name(family->name);
+}
+
+static int genl_message_new(
+                sd_netlink *nl,
+                const GenericNetlinkFamily *family,
+                uint8_t cmd,
+                sd_netlink_message **ret) {
+
+        _cleanup_(sd_netlink_message_unrefp) sd_netlink_message *m = NULL;
+        const NLAPolicySet *policy_set;
+        int r;
+
+        assert(nl);
+        assert(nl->protocol == NETLINK_GENERIC);
+        assert(family);
+        assert(ret);
+
+        policy_set = genl_family_get_policy_set(family);
+        if (!policy_set)
+                return -EOPNOTSUPP;
+
+        r = message_new_full(nl, family->id, policy_set,
+                             sizeof(struct genlmsghdr) + family->additional_header_size, &m);
+        if (r < 0)
+                return r;
+
+        *(struct genlmsghdr *) NLMSG_DATA(m->hdr) = (struct genlmsghdr) {
+                .cmd = cmd,
+                .version = family->version,
+        };
+
+        *ret = TAKE_PTR(m);
+        return 0;
+}
+
+static int genl_family_get_by_name_internal(
+                sd_netlink *nl,
+                const GenericNetlinkFamily *ctrl,
+                const char *name,
+                const GenericNetlinkFamily **ret) {
+
+        _cleanup_(sd_netlink_message_unrefp) sd_netlink_message *req = NULL, *reply = NULL;
+        const NLAPolicySet *policy_set;
+        int r;
+
+        assert(nl);
+        assert(nl->protocol == NETLINK_GENERIC);
+        assert(ctrl);
+        assert(name);
+        assert(ret);
+
+        policy_set = genl_get_policy_set_by_name(name);
+        if (!policy_set)
+                return -EOPNOTSUPP;
+
+        r = genl_message_new(nl, ctrl, CTRL_CMD_GETFAMILY, &req);
+        if (r < 0)
+                return r;
+
+        r = sd_netlink_message_append_string(req, CTRL_ATTR_FAMILY_NAME, name);
+        if (r < 0)
+                return r;
+
+        if (sd_netlink_call(nl, req, 0, &reply) < 0) {
+                (void) genl_family_new_unsupported(nl, name, policy_set);
+                return -EOPNOTSUPP;
+        }
+
+        return genl_family_new(nl, name, policy_set, reply, ret);
+}
+
+static int genl_family_get_by_name(sd_netlink *nl, const char *name, const GenericNetlinkFamily **ret) {
+        const GenericNetlinkFamily *f, *ctrl;
+        int r;
+
+        assert(nl);
+        assert(nl->protocol == NETLINK_GENERIC);
+        assert(name);
+        assert(ret);
+
+        f = hashmap_get(nl->genl_family_by_name, name);
+        if (f) {
+                if (f->id == 0) /* kernel does not support the family. */
+                        return -EOPNOTSUPP;
+
+                *ret = f;
+                return 0;
+        }
+
+        if (streq(name, CTRL_GENL_NAME))
+                return genl_family_get_by_name_internal(nl, &nlctrl_static, CTRL_GENL_NAME, ret);
+
+        ctrl = hashmap_get(nl->genl_family_by_name, CTRL_GENL_NAME);
+        if (!ctrl) {
+                r = genl_family_get_by_name_internal(nl, &nlctrl_static, CTRL_GENL_NAME, &ctrl);
+                if (r < 0)
+                        return r;
+        }
+
+        return genl_family_get_by_name_internal(nl, ctrl, name, ret);
+}
+
+static int genl_family_get_by_id(sd_netlink *nl, uint16_t id, const GenericNetlinkFamily **ret) {
+        const GenericNetlinkFamily *f;
+
+        assert(nl);
+        assert(nl->protocol == NETLINK_GENERIC);
+        assert(ret);
+
+        f = hashmap_get(nl->genl_family_by_id, UINT_TO_PTR(id));
+        if (f) {
+                *ret = f;
+                return 0;
+        }
+
+        if (id == GENL_ID_CTRL) {
+                *ret = &nlctrl_static;
+                return 0;
+        }
+
+        return -ENOENT;
+}
+
+int genl_get_policy_set_and_header_size(
+                sd_netlink *nl,
+                uint16_t id,
+                const NLAPolicySet **ret_policy_set,
+                size_t *ret_header_size) {
+
+        const GenericNetlinkFamily *f;
+        int r;
+
+        assert(nl);
+        assert(nl->protocol == NETLINK_GENERIC);
+
+        r = genl_family_get_by_id(nl, id, &f);
+        if (r < 0)
+                return r;
+
+        if (ret_policy_set) {
+                const NLAPolicySet *p;
+
+                p = genl_family_get_policy_set(f);
+                if (!p)
+                        return -EOPNOTSUPP;
+
+                *ret_policy_set = p;
+        }
+        if (ret_header_size)
+                *ret_header_size = sizeof(struct genlmsghdr) + f->additional_header_size;
+        return 0;
+}
+
+int sd_genl_message_new(sd_netlink *nl, const char *family_name, uint8_t cmd, sd_netlink_message **ret) {
+        const GenericNetlinkFamily *family;
+        int r;
+
+        assert_return(nl, -EINVAL);
+        assert_return(nl->protocol == NETLINK_GENERIC, -EINVAL);
+        assert_return(family_name, -EINVAL);
+        assert_return(ret, -EINVAL);
+
+        r = genl_family_get_by_name(nl, family_name, &family);
+        if (r < 0)
+                return r;
+
+        return genl_message_new(nl, family, cmd, ret);
+}
+
+int sd_genl_message_get_family_name(sd_netlink *nl, sd_netlink_message *m, const char **ret) {
+        const GenericNetlinkFamily *family;
+        uint16_t nlmsg_type;
+        int r;
+
+        assert_return(nl, -EINVAL);
+        assert_return(nl->protocol == NETLINK_GENERIC, -EINVAL);
+        assert_return(m, -EINVAL);
+        assert_return(ret, -EINVAL);
+
+        r = sd_netlink_message_get_type(m, &nlmsg_type);
+        if (r < 0)
+                return r;
+
+        r = genl_family_get_by_id(nl, nlmsg_type, &family);
+        if (r < 0)
+                return r;
+
+        *ret = family->name;
+        return 0;
+}
+
+int sd_genl_message_get_command(sd_netlink *nl, sd_netlink_message *m, uint8_t *ret) {
+        struct genlmsghdr *h;
+        uint16_t nlmsg_type;
+        size_t size;
+        int r;
+
+        assert_return(nl, -EINVAL);
+        assert_return(nl->protocol == NETLINK_GENERIC, -EINVAL);
+        assert_return(m, -EINVAL);
+        assert_return(m->protocol == NETLINK_GENERIC, -EINVAL);
+        assert_return(m->hdr, -EINVAL);
+        assert_return(ret, -EINVAL);
+
+        r = sd_netlink_message_get_type(m, &nlmsg_type);
+        if (r < 0)
+                return r;
+
+        r = genl_get_policy_set_and_header_size(nl, nlmsg_type, NULL, &size);
+        if (r < 0)
+                return r;
+
+        if (m->hdr->nlmsg_len < NLMSG_LENGTH(size))
+                return -EBADMSG;
+
+        h = NLMSG_DATA(m->hdr);
+
+        *ret = h->cmd;
+        return 0;
+}
+
+static int genl_family_get_multicast_group_id_by_name(const GenericNetlinkFamily *f, const char *name, uint32_t *ret) {
+        void *p;
+
+        assert(f);
+        assert(name);
+
+        p = hashmap_get(f->multicast_group_by_name, name);
+        if (!p)
+                return -ENOENT;
+
+        if (ret)
+                *ret = PTR_TO_UINT32(p);
+        return 0;
+}
+
+int sd_genl_add_match(
+                sd_netlink *nl,
+                sd_netlink_slot **ret_slot,
+                const char *family_name,
+                const char *multicast_group_name,
+                uint8_t command,
+                sd_netlink_message_handler_t callback,
+                sd_netlink_destroy_t destroy_callback,
+                void *userdata,
+                const char *description) {
+
+        const GenericNetlinkFamily *f;
+        uint32_t multicast_group_id;
+        int r;
+
+        assert_return(nl, -EINVAL);
+        assert_return(nl->protocol == NETLINK_GENERIC, -EINVAL);
+        assert_return(callback, -EINVAL);
+        assert_return(family_name, -EINVAL);
+        assert_return(multicast_group_name, -EINVAL);
+
+        /* If command == 0, then all commands belonging to the multicast group trigger the callback. */
+
+        r = genl_family_get_by_name(nl, family_name, &f);
+        if (r < 0)
+                return r;
+
+        r = genl_family_get_multicast_group_id_by_name(f, multicast_group_name, &multicast_group_id);
+        if (r < 0)
+                return r;
+
+        return netlink_add_match_internal(nl, ret_slot, &multicast_group_id, 1, f->id, command,
+                                          callback, destroy_callback, userdata, description);
+}
+
+int sd_genl_socket_open(sd_netlink **ret) {
+        return netlink_open_family(ret, NETLINK_GENERIC);
+}