hostapd: Add vendor command support

Add support of vendor command to hostapd ctrl_iface.
Vendor command's format:
VENDOR <vendor id> <sub command id> [<hex formatted data>]

The 3rd argument will be converted to binary data and then passed as
argument to the sub command.

Signed-off-by: Avraham Stern <avraham.stern@intel.com>

2 files changed, 83 insertions, 0 deletions

diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index 29d5d8b..4dddf80 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -1281,6 +1281,63 @@ static int hostapd_ctrl_iface_mib(struct hostapd_data *hapd, char *reply,
 }
 
 
+static int hostapd_ctrl_iface_vendor(struct hostapd_data *hapd, 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> <subcommand id> [<hex formatted data>] */
+	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 = hostapd_drv_vendor_cmd(hapd, 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 hostapd_ctrl_iface_receive(int sock, void *eloop_ctx,
 				       void *sock_ctx)
 {
@@ -1486,6 +1543,10 @@ static void hostapd_ctrl_iface_receive(int sock, void *eloop_ctx,
 	} else if (os_strncmp(buf, "CHAN_SWITCH ", 12) == 0) {
 		if (hostapd_ctrl_iface_chan_switch(hapd, buf + 12))
 			reply_len = -1;
+	} else if (os_strncmp(buf, "VENDOR ", 7) == 0) {
+		reply_len = hostapd_ctrl_iface_vendor(hapd, buf + 7, reply,
+						      reply_size);
+
 	} else {
 		os_memcpy(reply, "UNKNOWN COMMAND\n", 16);
 		reply_len = 16;
diff --git a/hostapd/hostapd_cli.c b/hostapd/hostapd_cli.c
index 8caca4f..c488b4f 100644
--- a/hostapd/hostapd_cli.c
+++ b/hostapd/hostapd_cli.c
@@ -940,6 +940,27 @@ static int hostapd_cli_cmd_chan_switch(struct wpa_ctrl *ctrl,
 }
 
 
+static int hostapd_cli_cmd_vendor(struct wpa_ctrl *ctrl, int argc, char *argv[])
+{
+	char cmd[256];
+	int res;
+
+	if (argc < 2 || argc > 3) {
+		printf("Invalid vendor command\n"
+		       "usage: <vendor id> <command id> [<hex formatted command argument>]\n");
+		return -1;
+	}
+
+	res = os_snprintf(cmd, sizeof(cmd), "VENDOR %s %s %s", argv[0], argv[1],
+			  argc == 3 ? argv[2] : "");
+	if (res < 0 || (size_t) res >= sizeof(cmd) - 1) {
+		printf("Too long VENDOR command.\n");
+		return -1;
+	}
+	return wpa_ctrl_command(ctrl, cmd);
+}
+
+
 struct hostapd_cli_cmd {
 	const char *cmd;
 	int (*handler)(struct wpa_ctrl *ctrl, int argc, char *argv[]);
@@ -988,6 +1009,7 @@ static struct hostapd_cli_cmd hostapd_cli_commands[] = {
 	{ "chan_switch", hostapd_cli_cmd_chan_switch },
 	{ "hs20_wnm_notif", hostapd_cli_cmd_hs20_wnm_notif },
 	{ "hs20_deauth_req", hostapd_cli_cmd_hs20_deauth_req },
+	{ "vendor", hostapd_cli_cmd_vendor },
 	{ NULL, NULL }
 };