[PATCH 3/4] ctrl_iface: Add vendor command support

[Ilan Peer]

From: Beni Lev <beni.lev at intel.com>

Add the support of vendor command to 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: Beni Lev <beni.lev at intel.com>
---
 wpa_supplicant/ctrl_iface.c |   59 +++++++++++++++++++++++++++++++++++++++++++
 wpa_supplicant/driver_i.h   |   10 ++++++++
 2 files changed, 69 insertions(+)

diff --git a/wpa_supplicant/ctrl_iface.c b/wpa_supplicant/ctrl_iface.c
index 793faec..68defb2 100644
--- a/wpa_supplicant/ctrl_iface.c
+++ b/wpa_supplicant/ctrl_iface.c
@@ -5459,6 +5459,62 @@ 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> <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;
+		else
+			data_len = 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!");
+			free(data);
+			return -EINVAL;
+		}
+	}
+
+	reply = wpabuf_alloc((buflen - 1) / 2);
+	if (!reply) {
+		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)
 {
@@ -6345,6 +6401,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 0691b6c..b336afb 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 */
-- 
1.7.10.4