sd-netlink: introduce sd_genl_add_match()

By using this, we can listen multicast messages for generic netlink.

src/libsystemd/sd-netlink/netlink-genl.c

@@ -18,6 +18,7 @@ typedef struct GenericNetlinkFamily {
         char *name;
         uint32_t version;
         uint32_t additional_header_size;
+        Hashmap *multicast_group_by_name;
 } GenericNetlinkFamily;
 
 static const GenericNetlinkFamily nlctrl_static = {
@@ -38,6 +39,7 @@ static GenericNetlinkFamily *genl_family_free(GenericNetlinkFamily *f) {
         }
 
         free(f->name);
+        hashmap_free(f->multicast_group_by_name);
 
         return mfree(f);
 }
@@ -132,6 +134,48 @@ static int genl_family_new(
         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;
@@ -373,6 +417,56 @@ int sd_genl_message_get_command(sd_netlink *nl, sd_netlink_message *m, uint8_t *
         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);
 }

src/libsystemd/sd-netlink/netlink-internal.h

@@ -28,6 +28,7 @@ struct match_callback {
         uint32_t *groups;
         size_t n_groups;
         uint16_t type;
+        uint8_t cmd; /* used by genl */
 
         LIST_FIELDS(struct match_callback, match_callbacks);
 };
@@ -159,6 +160,7 @@ int netlink_add_match_internal(
                 const uint32_t *groups,
                 size_t n_groups,
                 uint16_t type,
+                uint8_t cmd,
                 sd_netlink_message_handler_t callback,
                 sd_netlink_destroy_t destroy_callback,
                 void *userdata,

src/libsystemd/sd-netlink/sd-netlink.c

@@ -426,8 +426,8 @@ static int process_reply(sd_netlink *nl, sd_netlink_message *m) {
 
 static int process_match(sd_netlink *nl, sd_netlink_message *m) {
         struct match_callback *c;
-        sd_netlink_slot *slot;
         uint16_t type;
+        uint8_t cmd;
         int r;
 
         assert(nl);
@@ -437,8 +437,19 @@ static int process_match(sd_netlink *nl, sd_netlink_message *m) {
         if (r < 0)
                 return r;
 
+        if (m->protocol == NETLINK_GENERIC) {
+                r = sd_genl_message_get_command(nl, m, &cmd);
+                if (r < 0)
+                        return r;
+        } else
+                cmd = 0;
+
         LIST_FOREACH(match_callbacks, c, nl->match_callbacks) {
-                if (type != c->type)
+                sd_netlink_slot *slot;
+
+                if (c->type != type)
+                        continue;
+                if (c->cmd != 0 && c->cmd != cmd)
                         continue;
 
                 slot = container_of(c, sd_netlink_slot, match_callback);
@@ -901,6 +912,7 @@ int netlink_add_match_internal(
                 const uint32_t *groups,
                 size_t n_groups,
                 uint16_t type,
+                uint8_t cmd,
                 sd_netlink_message_handler_t callback,
                 sd_netlink_destroy_t destroy_callback,
                 void *userdata,
@@ -930,6 +942,7 @@ int netlink_add_match_internal(
         slot->match_callback.n_groups = n_groups;
         slot->match_callback.callback = callback;
         slot->match_callback.type = type;
+        slot->match_callback.cmd = cmd;
 
         LIST_PREPEND(match_callbacks, nl->match_callbacks, &slot->match_callback);
 
@@ -1001,6 +1014,6 @@ int sd_netlink_add_match(
                         return -EOPNOTSUPP;
         }
 
-        return netlink_add_match_internal(rtnl, ret_slot, groups, n_groups, type, callback,
+        return netlink_add_match_internal(rtnl, ret_slot, groups, n_groups, type, 0, callback,
                                           destroy_callback, userdata, description);
 }

src/libsystemd/sd-netlink/test-netlink.c

@@ -2,7 +2,12 @@
 
 #include <net/if.h>
 #include <netinet/ether.h>
+#include <netinet/in.h>
+#include <linux/fou.h>
 #include <linux/genetlink.h>
+#include <linux/if_macsec.h>
+#include <linux/l2tp.h>
+#include <linux/nl80211.h>
 
 #include "sd-netlink.h"
 
@@ -583,24 +588,82 @@ static void test_strv(sd_netlink *rtnl) {
         assert_se(sd_netlink_message_exit_container(m) >= 0);
 }
 
+static int genl_ctrl_match_callback(sd_netlink *genl, sd_netlink_message *m, void *userdata) {
+        const char *name;
+        uint16_t id;
+        uint8_t cmd;
+
+        assert(genl);
+        assert(m);
+
+        assert_se(sd_genl_message_get_family_name(genl, m, &name) >= 0);
+        assert_se(streq(name, CTRL_GENL_NAME));
+
+        assert_se(sd_genl_message_get_command(genl, m, &cmd) >= 0);
+
+        switch (cmd) {
+        case CTRL_CMD_NEWFAMILY:
+        case CTRL_CMD_DELFAMILY:
+                assert_se(sd_netlink_message_read_string(m, CTRL_ATTR_FAMILY_NAME, &name) >= 0);
+                assert_se(sd_netlink_message_read_u16(m, CTRL_ATTR_FAMILY_ID, &id) >= 0);
+                log_debug("%s: %s (id=%"PRIu16") family is %s.",
+                          __func__, name, id, cmd == CTRL_CMD_NEWFAMILY ? "added" : "removed");
+                break;
+        case CTRL_CMD_NEWMCAST_GRP:
+        case CTRL_CMD_DELMCAST_GRP:
+                assert_se(sd_netlink_message_read_string(m, CTRL_ATTR_FAMILY_NAME, &name) >= 0);
+                assert_se(sd_netlink_message_read_u16(m, CTRL_ATTR_FAMILY_ID, &id) >= 0);
+                log_debug("%s: multicast group for %s (id=%"PRIu16") family is %s.",
+                          __func__, name, id, cmd == CTRL_CMD_NEWMCAST_GRP ? "added" : "removed");
+                break;
+        default:
+                log_debug("%s: received nlctrl message with unknown command '%"PRIu8"'.", __func__, cmd);
+        }
+
+        return 0;
+}
+
 static void test_genl(void) {
+        _cleanup_(sd_event_unrefp) sd_event *event = NULL;
         _cleanup_(sd_netlink_unrefp) sd_netlink *genl = NULL;
         _cleanup_(sd_netlink_message_unrefp) sd_netlink_message *m = NULL;
         const char *name;
         uint8_t cmd;
+        int r;
 
         log_debug("/* %s */", __func__);
 
         assert_se(sd_genl_socket_open(&genl) >= 0);
+        assert_se(sd_event_default(&event) >= 0);
+        assert_se(sd_netlink_attach_event(genl, event, 0) >= 0);
+
         assert_se(sd_genl_message_new(genl, CTRL_GENL_NAME, CTRL_CMD_GETFAMILY, &m) >= 0);
         assert_se(sd_genl_message_get_family_name(genl, m, &name) >= 0);
         assert_se(streq(name, CTRL_GENL_NAME));
         assert_se(sd_genl_message_get_command(genl, m, &cmd) >= 0);
         assert_se(cmd == CTRL_CMD_GETFAMILY);
 
+        assert_se(sd_genl_add_match(genl, NULL, CTRL_GENL_NAME, "notify", 0, genl_ctrl_match_callback, NULL, NULL, "genl-ctrl-notify") >= 0);
+
         m = sd_netlink_message_unref(m);
         assert_se(sd_genl_message_new(genl, "should-not-exist", CTRL_CMD_GETFAMILY, &m) < 0);
         assert_se(sd_genl_message_new(genl, "should-not-exist", CTRL_CMD_GETFAMILY, &m) == -EOPNOTSUPP);
+
+        /* These families may not be supported by kernel. Hence, ignore results. */
+        (void) sd_genl_message_new(genl, FOU_GENL_NAME, 0, &m);
+        m = sd_netlink_message_unref(m);
+        (void) sd_genl_message_new(genl, L2TP_GENL_NAME, 0, &m);
+        m = sd_netlink_message_unref(m);
+        (void) sd_genl_message_new(genl, MACSEC_GENL_NAME, 0, &m);
+        m = sd_netlink_message_unref(m);
+        (void) sd_genl_message_new(genl, NL80211_GENL_NAME, 0, &m);
+
+        for (;;) {
+                r = sd_event_run(event, 500 * USEC_PER_MSEC);
+                assert_se(r >= 0);
+                if (r == 0)
+                        return;
+        }
 }
 
 int main(void) {

src/systemd/sd-netlink.h

@@ -238,6 +238,11 @@ int sd_genl_socket_open(sd_netlink **ret);
 int sd_genl_message_new(sd_netlink *genl, const char *family_name, uint8_t cmd, sd_netlink_message **ret);
 int sd_genl_message_get_family_name(sd_netlink *genl, sd_netlink_message *m, const char **ret);
 int sd_genl_message_get_command(sd_netlink *genl, sd_netlink_message *m, uint8_t *ret);
+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);
 
 /* slot */
 sd_netlink_slot *sd_netlink_slot_ref(sd_netlink_slot *slot);