1 files changed, 90 insertions, 12 deletions

diff --git a/src/libsystemd/sd-netlink/generic-netlink.c b/src/libsystemd/sd-netlink/generic-netlink.c
index 5467f62ffa..ad35b143f8 100644
--- a/src/libsystemd/sd-netlink/generic-netlink.c
+++ b/src/libsystemd/sd-netlink/generic-netlink.c
@@ -1,8 +1,12 @@
+/* SPDX-License-Identifier: LGPL-2.1+ */
+
 #include <linux/genetlink.h>
 
 #include "sd-netlink.h"
-#include "netlink-internal.h"
+
 #include "alloc-util.h"
+#include "generic-netlink.h"
+#include "netlink-internal.h"
 
 typedef struct {
         const char* name;
@@ -15,6 +19,7 @@ static const genl_family genl_families[] = {
         [SD_GENL_FOU]       = { .name = "fou",       .version = 1 },
         [SD_GENL_L2TP]      = { .name = "l2tp",      .version = 1 },
         [SD_GENL_MACSEC]    = { .name = "macsec",    .version = 1 },
+        [SD_GENL_NL80211]   = { .name = "nl80211",   .version = 1 },
 };
 
 int sd_genl_socket_open(sd_netlink **ret) {
@@ -23,12 +28,12 @@ int sd_genl_socket_open(sd_netlink **ret) {
 static int lookup_id(sd_netlink *nl, sd_genl_family family, uint16_t *id);
 
 static int genl_message_new(sd_netlink *nl, sd_genl_family family, uint16_t nlmsg_type, uint8_t cmd, sd_netlink_message **ret) {
-        int r;
-        struct genlmsghdr *genl;
+        _cleanup_(sd_netlink_message_unrefp) sd_netlink_message *m = NULL;
         const NLType *genl_cmd_type, *nl_type;
         const NLTypeSystem *type_system;
+        struct genlmsghdr *genl;
         size_t size;
-        _cleanup_(sd_netlink_message_unrefp) sd_netlink_message *m = NULL;
+        int r;
 
         assert_return(nl->protocol == NETLINK_GENERIC, -EINVAL);
 
@@ -67,21 +72,33 @@ static int genl_message_new(sd_netlink *nl, sd_genl_family family, uint16_t nlms
 }
 
 int sd_genl_message_new(sd_netlink *nl, sd_genl_family family, uint8_t cmd, sd_netlink_message **ret) {
+        uint16_t id;
         int r;
-        uint16_t id = GENL_ID_CTRL;
 
-        if (family != SD_GENL_ID_CTRL) {
-                r = lookup_id(nl, family, &id);
-                if (r < 0)
-                        return r;
-        }
+        r = lookup_id(nl, family, &id);
+        if (r < 0)
+                return r;
 
         return genl_message_new(nl, family, id, cmd, ret);
 }
 
 static int lookup_id(sd_netlink *nl, sd_genl_family family, uint16_t *id) {
-        int r;
         _cleanup_(sd_netlink_message_unrefp) sd_netlink_message *req = NULL, *reply = NULL;
+        uint16_t u;
+        void *v;
+        int r;
+
+        if (family == SD_GENL_ID_CTRL) {
+                *id = GENL_ID_CTRL;
+                return 0;
+        }
+
+        v = hashmap_get(nl->genl_family_to_nlmsg_type, INT_TO_PTR(family));
+        if (v) {
+                *id = PTR_TO_UINT(v);
+                return 0;
+        }
+
 
         r = sd_genl_message_new(nl, SD_GENL_ID_CTRL, CTRL_CMD_GETFAMILY, &req);
         if (r < 0)
@@ -95,5 +112,66 @@ static int lookup_id(sd_netlink *nl, sd_genl_family family, uint16_t *id) {
         if (r < 0)
                 return r;
 
-        return sd_netlink_message_read_u16(reply, CTRL_ATTR_FAMILY_ID, id);
+        r = sd_netlink_message_read_u16(reply, CTRL_ATTR_FAMILY_ID, &u);
+        if (r < 0)
+                return r;
+
+        r = hashmap_ensure_allocated(&nl->genl_family_to_nlmsg_type, NULL);
+        if (r < 0)
+                return r;
+
+        r = hashmap_ensure_allocated(&nl->nlmsg_type_to_genl_family, NULL);
+        if (r < 0)
+                return r;
+
+        r = hashmap_put(nl->genl_family_to_nlmsg_type, INT_TO_PTR(family), UINT_TO_PTR(u));
+        if (r < 0)
+                return r;
+
+        r = hashmap_put(nl->nlmsg_type_to_genl_family, UINT_TO_PTR(u), INT_TO_PTR(family));
+        if (r < 0)
+                return r;
+
+        *id = u;
+        return 0;
+}
+
+int nlmsg_type_to_genl_family(sd_netlink *nl, uint16_t type, sd_genl_family *ret) {
+        void *p;
+
+        assert_return(nl, -EINVAL);
+        assert_return(nl->protocol == NETLINK_GENERIC, -EINVAL);
+        assert(ret);
+
+        if (type == NLMSG_ERROR)
+                *ret = SD_GENL_ERROR;
+        else if (type == NLMSG_DONE)
+                *ret = SD_GENL_DONE;
+        else if (type == GENL_ID_CTRL)
+                *ret = SD_GENL_ID_CTRL;
+        else {
+                p = hashmap_get(nl->nlmsg_type_to_genl_family, UINT_TO_PTR(type));
+                if (!p)
+                        return -EOPNOTSUPP;
+
+                *ret = PTR_TO_INT(p);
+        }
+
+        return 0;
+}
+
+int sd_genl_message_get_family(sd_netlink *nl, sd_netlink_message *m, sd_genl_family *family) {
+        uint16_t type;
+        int r;
+
+        assert_return(m, -EINVAL);
+        assert_return(nl, -EINVAL);
+        assert_return(nl->protocol == NETLINK_GENERIC, -EINVAL);
+        assert_return(family, -EINVAL);
+
+        r = sd_netlink_message_get_type(m, &type);
+        if (r < 0)
+                return r;
+
+        return nlmsg_type_to_genl_family(nl, type, family);
 }