From adef89480d7fa3c5d18f4d680f665e3d42de9886 Mon Sep 17 00:00:00 2001 From: Beni Lev Date: Mon, 3 Mar 2014 13:09:50 +0200 Subject: [PATCH] nl80211: Add vendor command support Add a callback to the driver interface that allows vendor specific commands to be sent. In addition, a control interface command is added to expose this new interface outside wpa_supplicant: Vendor command's format: VENDOR [] The 3rd argument will be converted to binary data and then passed as argument to the sub command. This interface is driver independent, but for now, this is only implemented for the nl80211 driver interface using the cfg80211 vendor commands. Signed-off-by: Beni Lev --- src/drivers/driver.h | 24 +++++++++++++ src/drivers/driver_nl80211.c | 65 ++++++++++++++++++++++++++++++++++++ wpa_supplicant/ctrl_iface.c | 60 +++++++++++++++++++++++++++++++++ wpa_supplicant/driver_i.h | 10 ++++++ wpa_supplicant/wpa_cli.c | 9 +++++ 5 files changed, 168 insertions(+) diff --git a/src/drivers/driver.h b/src/drivers/driver.h index d2aad246b..b6434c294 100644 --- a/src/drivers/driver.h +++ b/src/drivers/driver.h @@ -2519,6 +2519,30 @@ struct wpa_driver_ops { int (*driver_cmd)(void *priv, char *cmd, char *buf, size_t buf_len); #endif /* ANDROID */ + /** + * vendor_cmd - Execute vendor specific command + * @priv: Private driver interface data + * @vendor_id: Vendor id + * @subcmd: Vendor command id + * @data: Vendor command parameters (%NULL if no parameters) + * @data_len: Data length + * @buf: Return buffer (%NULL to ignore reply) + * Returns: 0 on success, negative (<0) on failure + * + * This function handles vendor specific commands that are passed to + * the driver/device. The command is identified by vendor id and + * command id. Parameters can be passed as argument to the command + * in the data buffer. Reply (if any) will be filled in the supplied + * return buffer. + * + * The exact driver behavior is driver interface and vendor specific. As + * an example, this will be converted to a vendor specific cfg80211 + * command in case of the nl80211 driver interface. + */ + int (*vendor_cmd)(void *priv, unsigned int vendor_id, + unsigned int subcmd, const u8 *data, size_t data_len, + struct wpabuf *buf); + /** * set_rekey_info - Set rekey information * @priv: Private driver interface data diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c index d09f7b3b7..5439f81c4 100644 --- a/src/drivers/driver_nl80211.c +++ b/src/drivers/driver_nl80211.c @@ -11710,6 +11710,70 @@ error: } +static int vendor_reply_handler(struct nl_msg *msg, void *arg) +{ + struct nlattr *tb[NL80211_ATTR_MAX + 1]; + struct nlattr *nl_vendor_reply, *nl; + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wpabuf *buf = arg; + int rem; + + if (!buf) + return NL_SKIP; + + nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), NULL); + nl_vendor_reply = tb[NL80211_ATTR_VENDOR_DATA]; + + if (!nl_vendor_reply) + return NL_SKIP; + + if ((size_t) nla_len(nl_vendor_reply) > wpabuf_tailroom(buf)) { + wpa_printf(MSG_INFO, "nl80211: Vendor command: insufficient buffer space for reply"); + return NL_SKIP; + } + + nla_for_each_nested(nl, nl_vendor_reply, rem) { + wpabuf_put_data(buf, nla_data(nl), nla_len(nl)); + } + + return NL_SKIP; +} + + +static int nl80211_vendor_cmd(void *priv, unsigned int vendor_id, + unsigned int subcmd, const u8 *data, + size_t data_len, struct wpabuf *buf) +{ + struct i802_bss *bss = priv; + struct wpa_driver_nl80211_data *drv = bss->drv; + struct nl_msg *msg; + int ret; + + msg = nlmsg_alloc(); + if (!msg) + return -ENOMEM; + + nl80211_cmd(drv, msg, 0, NL80211_CMD_VENDOR); + if (nl80211_set_iface_id(msg, bss) < 0) + goto nla_put_failure; + NLA_PUT_U32(msg, NL80211_ATTR_VENDOR_ID, vendor_id); + NLA_PUT_U32(msg, NL80211_ATTR_VENDOR_SUBCMD, subcmd); + if (data) + NLA_PUT(msg, NL80211_ATTR_VENDOR_DATA, data_len, data); + + ret = send_and_recv_msgs(drv, msg, vendor_reply_handler, buf); + if (ret) + wpa_printf(MSG_DEBUG, "nl80211: vendor command failed err=%d", + ret); + return ret; + +nla_put_failure: + nlmsg_free(msg); + return -ENOBUFS; +} + + static int nl80211_set_qos_map(void *priv, const u8 *qos_map_set, u8 qos_map_set_len) { @@ -11829,5 +11893,6 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = { #ifdef ANDROID .driver_cmd = wpa_driver_nl80211_driver_cmd, #endif /* ANDROID */ + .vendor_cmd = nl80211_vendor_cmd, .set_qos_map = nl80211_set_qos_map, }; diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c index 793faec58..2935ce739 100644 --- a/wpa_supplicant/ctrl_iface.c +++ b/wpa_supplicant/ctrl_iface.c @@ -5460,6 +5460,63 @@ static int wpa_supplicant_driver_cmd(struct wpa_supplicant *wpa_s, char *cmd, #endif /* ANDROID */ +static int wpa_supplicant_vendor_cmd(struct wpa_supplicant *wpa_s, char *cmd, + char *buf, size_t buflen) +{ + int ret; + char *pos; + u8 *data = NULL; + unsigned int vendor_id, subcmd; + struct wpabuf *reply; + size_t data_len = 0; + + /* cmd: [] */ + vendor_id = strtoul(cmd, &pos, 16); + if (!isblank(*pos)) + return -EINVAL; + + subcmd = strtoul(pos, &pos, 10); + + if (*pos != '\0') { + if (!isblank(*pos++)) + return -EINVAL; + data_len = os_strlen(pos); + } + + if (data_len) { + data_len /= 2; + data = os_malloc(data_len); + if (!data) + return -ENOBUFS; + + if (hexstr2bin(pos, data, data_len)) { + wpa_printf(MSG_DEBUG, + "Vendor command: wrong parameter format"); + os_free(data); + return -EINVAL; + } + } + + reply = wpabuf_alloc((buflen - 1) / 2); + if (!reply) { + os_free(data); + return -ENOBUFS; + } + + ret = wpa_drv_vendor_cmd(wpa_s, vendor_id, subcmd, data, data_len, + reply); + + if (ret == 0) + ret = wpa_snprintf_hex(buf, buflen, wpabuf_head_u8(reply), + wpabuf_len(reply)); + + wpabuf_free(reply); + os_free(data); + + return ret; +} + + static void wpa_supplicant_ctrl_iface_flush(struct wpa_supplicant *wpa_s) { wpa_dbg(wpa_s, MSG_DEBUG, "Flush all wpa_supplicant state"); @@ -6345,6 +6402,9 @@ char * wpa_supplicant_ctrl_iface_process(struct wpa_supplicant *wpa_s, reply_len = wpa_supplicant_driver_cmd(wpa_s, buf + 7, reply, reply_size); #endif /* ANDROID */ + } else if (os_strncmp(buf, "VENDOR ", 7) == 0) { + reply_len = wpa_supplicant_vendor_cmd(wpa_s, buf + 7, reply, + reply_size); } else if (os_strcmp(buf, "REAUTHENTICATE") == 0) { pmksa_cache_clear_current(wpa_s->wpa); eapol_sm_request_reauth(wpa_s->eapol); diff --git a/wpa_supplicant/driver_i.h b/wpa_supplicant/driver_i.h index 0691b6caa..b336afb05 100644 --- a/wpa_supplicant/driver_i.h +++ b/wpa_supplicant/driver_i.h @@ -604,4 +604,14 @@ static inline int wpa_drv_set_qos_map(struct wpa_supplicant *wpa_s, qos_map_set_len); } +static inline int wpa_drv_vendor_cmd(struct wpa_supplicant *wpa_s, + int vendor_id, int subcmd, const u8 *data, + size_t data_len, struct wpabuf *buf) +{ + if (!wpa_s->driver->vendor_cmd) + return -1; + return wpa_s->driver->vendor_cmd(wpa_s->drv_priv, vendor_id, subcmd, + data, data_len, buf); +} + #endif /* DRIVER_I_H */ diff --git a/wpa_supplicant/wpa_cli.c b/wpa_supplicant/wpa_cli.c index 069118304..63ea1df93 100644 --- a/wpa_supplicant/wpa_cli.c +++ b/wpa_supplicant/wpa_cli.c @@ -2426,6 +2426,12 @@ static int wpa_cli_cmd_driver(struct wpa_ctrl *ctrl, int argc, char *argv[]) #endif /* ANDROID */ +static int wpa_cli_cmd_vendor(struct wpa_ctrl *ctrl, int argc, char *argv[]) +{ + return wpa_cli_cmd(ctrl, "VENDOR", 1, argc, argv); +} + + static int wpa_cli_cmd_flush(struct wpa_ctrl *ctrl, int argc, char *argv[]) { return wpa_ctrl_command(ctrl, "FLUSH"); @@ -2912,6 +2918,9 @@ static struct wpa_cli_cmd wpa_cli_commands[] = { #endif /* ANDROID */ { "radio_work", wpa_cli_cmd_radio_work, NULL, cli_cmd_flag_none, "= radio_work " }, + { "vendor", wpa_cli_cmd_vendor, NULL, cli_cmd_flag_none, + " [] = Send vendor command" + }, { NULL, NULL, NULL, cli_cmd_flag_none, NULL } };