diff --git a/src/platform/wifi/wifi-utils-nl80211.c b/src/platform/wifi/wifi-utils-nl80211.c index 3879cb0b1a..e05104f374 100644 --- a/src/platform/wifi/wifi-utils-nl80211.c +++ b/src/platform/wifi/wifi-utils-nl80211.c @@ -28,12 +28,8 @@ #include #include #include - - -#include -#include -#include - +#include +#include #include #include "nm-default.h" @@ -42,6 +38,198 @@ #include "nm-platform.h" #include "nm-utils.h" + +/***************************************************************************** + * Copied from libnl3/genl: + *****************************************************************************/ + +static void * +genlmsg_put (struct nl_msg *msg, uint32_t port, uint32_t seq, int family, + int hdrlen, int flags, uint8_t cmd, uint8_t version) +{ + struct nlmsghdr *nlh; + struct genlmsghdr hdr = { + .cmd = cmd, + .version = version, + }; + + nlh = nlmsg_put (msg, port, seq, family, GENL_HDRLEN + hdrlen, flags); + if (nlh == NULL) + return NULL; + + memcpy (nlmsg_data (nlh), &hdr, sizeof (hdr)); + + return (char *) nlmsg_data (nlh) + GENL_HDRLEN; +} + +static void * +genlmsg_data (const struct genlmsghdr *gnlh) +{ + return ((unsigned char *) gnlh + GENL_HDRLEN); +} + +static void * +genlmsg_user_hdr (const struct genlmsghdr *gnlh) +{ + return genlmsg_data (gnlh); +} + +static struct genlmsghdr * +genlmsg_hdr (struct nlmsghdr *nlh) +{ + return nlmsg_data (nlh); +} + +static void * +genlmsg_user_data (const struct genlmsghdr *gnlh, const int hdrlen) +{ + return (char *) genlmsg_user_hdr (gnlh) + NLMSG_ALIGN (hdrlen); +} + +static struct nlattr * +genlmsg_attrdata (const struct genlmsghdr *gnlh, int hdrlen) +{ + return genlmsg_user_data (gnlh, hdrlen); +} + +static int +genlmsg_len (const struct genlmsghdr *gnlh) +{ + const struct nlmsghdr *nlh; + + nlh = (const struct nlmsghdr *) ((const unsigned char *) gnlh - NLMSG_HDRLEN); + return (nlh->nlmsg_len - GENL_HDRLEN - NLMSG_HDRLEN); +} + +static int +genlmsg_attrlen (const struct genlmsghdr *gnlh, int hdrlen) +{ + return genlmsg_len (gnlh) - NLMSG_ALIGN (hdrlen); +} + +static int +genlmsg_valid_hdr (struct nlmsghdr *nlh, int hdrlen) +{ + struct genlmsghdr *ghdr; + + if (!nlmsg_valid_hdr (nlh, GENL_HDRLEN)) + return 0; + + ghdr = nlmsg_data (nlh); + if (genlmsg_len (ghdr) < NLMSG_ALIGN (hdrlen)) + return 0; + + return 1; +} + +static int +genlmsg_parse (struct nlmsghdr *nlh, int hdrlen, struct nlattr *tb[], + int maxtype, struct nla_policy *policy) +{ + struct genlmsghdr *ghdr; + + if (!genlmsg_valid_hdr (nlh, hdrlen)) + return -NLE_MSG_TOOSHORT; + + ghdr = nlmsg_data (nlh); + return nla_parse (tb, maxtype, genlmsg_attrdata (ghdr, hdrlen), + genlmsg_attrlen (ghdr, hdrlen), policy); +} + +/***************************************************************************** + * Reimplementation of libnl3/genl functions: + *****************************************************************************/ + +static int +probe_response (struct nl_msg *msg, void *arg) +{ + static struct nla_policy ctrl_policy[CTRL_ATTR_MAX+1] = { + [CTRL_ATTR_FAMILY_ID] = { .type = NLA_U16 }, + [CTRL_ATTR_FAMILY_NAME] = { .type = NLA_STRING, + .maxlen = GENL_NAMSIZ }, + [CTRL_ATTR_VERSION] = { .type = NLA_U32 }, + [CTRL_ATTR_HDRSIZE] = { .type = NLA_U32 }, + [CTRL_ATTR_MAXATTR] = { .type = NLA_U32 }, + [CTRL_ATTR_OPS] = { .type = NLA_NESTED }, + [CTRL_ATTR_MCAST_GROUPS] = { .type = NLA_NESTED }, + }; + struct nlattr *tb[CTRL_ATTR_MAX+1]; + struct nlmsghdr *nlh = nlmsg_hdr (msg); + gint32 *response_data = arg; + + if (genlmsg_parse (nlh, 0, tb, CTRL_ATTR_MAX, ctrl_policy)) + return NL_SKIP; + + if (tb[CTRL_ATTR_FAMILY_ID]) + *response_data = nla_get_u16 (tb[CTRL_ATTR_FAMILY_ID]); + + return NL_STOP; +} + +static int +genl_ctrl_resolve (struct nl_sock *sk, const char *name) +{ + struct nl_msg *msg; + struct nl_cb *cb, *orig; + int rc; + int result = -NLE_OBJ_NOTFOUND; + gint32 response_data = -1; + + if (!(orig = nl_socket_get_cb (sk))) + goto out; + + cb = nl_cb_clone (orig); + nl_cb_put (orig); + if (!cb) + goto out; + + msg = nlmsg_alloc (); + if (!msg) + goto out_cb_free; + + if (!genlmsg_put (msg, NL_AUTO_PORT, NL_AUTO_SEQ, GENL_ID_CTRL, + 0, 0, CTRL_CMD_GETFAMILY, 1)) + goto out_msg_free; + + if (nla_put_string (msg, CTRL_ATTR_FAMILY_NAME, name) < 0) + goto out_msg_free; + + rc = nl_cb_set (cb, NL_CB_VALID, NL_CB_CUSTOM, probe_response, &response_data); + if (rc < 0) + goto out_msg_free; + + rc = nl_send_auto_complete (sk, msg); + if (rc < 0) + goto out_msg_free; + + rc = nl_recvmsgs (sk, cb); + if (rc < 0) + goto out_msg_free; + + /* If search was successful, request may be ACKed after data */ + rc = nl_wait_for_ack (sk); + if (rc < 0) + goto out_msg_free; + + if (response_data > 0) + result = response_data; + +out_msg_free: + nlmsg_free (msg); +out_cb_free: + nl_cb_put (cb); +out: + if (result >= 0) + nm_log_dbg (LOGD_WIFI, "genl_ctrl_resolve: resolved \"%s\" as 0x%x", name, result); + else + nm_log_err (LOGD_WIFI, "genl_ctrl_resolve: failed resolve \"%s\"", name); + return result; +} + +/***************************************************************************** + * + *****************************************************************************/ + typedef struct { WifiData parent; struct nl_sock *nl_sock; @@ -867,7 +1055,7 @@ wifi_nl80211_init (const char *iface, int ifindex) if (nl80211->nl_sock == NULL) goto error; - if (genl_connect (nl80211->nl_sock)) + if (nl_connect (nl80211->nl_sock, NETLINK_GENERIC)) goto error; nl80211->id = genl_ctrl_resolve (nl80211->nl_sock, "nl80211");