[PATCH 08/10] nl80211_driver: implement init and deinit P2P device functions

[dvdspndl at gmail.com]

From: David Spinadel <david.spinadel at intel.com>

Signed-off-by: David Spinadel <david.spinadel at intel.com>
---
 src/drivers/driver_nl80211.c |  138 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 138 insertions(+)

diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index bf53ce4..4a2c87d 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -9404,6 +9404,140 @@ static int android_pno_stop(struct i802_bss *bss)
 
 #endif /* ANDROID */
 
+#ifdef CONFIG_P2P
+static int nl80211_create_p2p_dev_handler(struct nl_msg *msg, void *arg)
+{
+	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
+	struct nlattr *tb[NL80211_ATTR_MAX + 1];
+	struct i802_bss *p2p_dev = arg;
+
+	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+			genlmsg_attrlen(gnlh, 0), NULL);
+	if (tb[NL80211_ATTR_WDEV])
+		p2p_dev->wdev_id =
+			nla_get_u64(tb[NL80211_ATTR_WDEV]);
+
+	if (tb[NL80211_ATTR_MAC])
+		os_memcpy(p2p_dev->addr,
+			  nla_data(tb[NL80211_ATTR_MAC]), ETH_ALEN);
+
+	return NL_SKIP;
+}
+
+static int nl80211_p2p_device_exec_cmd(struct wpa_driver_nl80211_data *drv,
+					const char *ifname,
+					enum nl80211_commands cmd,
+					int64_t wdev_id)
+{
+	struct nl_msg *msg;
+	int ret = -1;
+
+	if (wdev_id < 0) {
+		wpa_printf(MSG_DEBUG, "nl80211: Cannot execute p2p device"
+				"cmd %d without wdev_id", cmd);
+		return -1;
+	}
+	wpa_printf(MSG_DEBUG, "Executing p2p device cmd %d", cmd);
+	msg = nlmsg_alloc();
+	if (!msg)
+		return -1;
+
+	nl80211_cmd(drv, msg, 0, cmd);
+	NLA_PUT_U64(msg, NL80211_ATTR_WDEV, wdev_id);
+
+	ret = send_and_recv_msgs(drv, msg, NULL, NULL);
+	msg = NULL;
+	if (ret) {
+nla_put_failure:
+		nlmsg_free(msg);
+		wpa_printf(MSG_ERROR, "nl80211: Failed to execute CMD %d on "
+				"%s: error =%d:%s", cmd, ifname, ret,
+				strerror(-ret));
+	}
+	return ret;
+}
+
+static void nl80211_deinit_p2p_dev(void *priv)
+{
+	struct i802_bss *p2p_dev = priv;
+	struct wpa_driver_nl80211_data *drv = p2p_dev->drv;
+	struct i802_bss *last;
+
+	if (p2p_dev->wdev_id == -1)
+		return;
+
+	/*
+	 * Not stopping the P2P device here as the kernel does that when
+	 * deinit is called. Need to execute STOP here if that changes.
+	 */
+	nl80211_p2p_device_exec_cmd(drv, p2p_dev->ifname,
+				    NL80211_CMD_DEL_INTERFACE,
+				    p2p_dev->wdev_id);
+
+	for (last = &drv->first_bss; last->next && last->next != p2p_dev;
+	     last = last->next)
+		;
+	if (last->next && last->next == p2p_dev)
+		last->next = p2p_dev->next;
+
+	os_free(p2p_dev);
+}
+
+static void *nl80211_init_p2p_dev(void *priv, const char *ifname, u8 *addr)
+{
+	struct i802_bss *bss = priv;
+	struct i802_bss *last;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	int ret;
+	int type;
+	struct i802_bss *p2p_dev = os_zalloc(sizeof(struct i802_bss));
+	if (!p2p_dev)
+		return NULL;
+
+	*p2p_dev = *bss;
+	os_strncpy(p2p_dev->ifname, ifname, IFNAMSIZ);
+
+	p2p_dev->ifindex = nl80211_create_iface(drv, ifname,
+						NL80211_IFTYPE_P2P_DEVICE,
+						NULL, 0,
+						nl80211_create_p2p_dev_handler,
+						p2p_dev);
+	if (p2p_dev->wdev_id == -1 || p2p_dev->ifindex != 0 ||
+	    is_zero_ether_addr((const u8 *)&p2p_dev->addr)) {
+		wpa_printf(MSG_ERROR, "nl80211: Failed to create a p2p device"
+				" interface %s", ifname);
+		os_free(p2p_dev);
+		return NULL;
+	}
+
+	ret = nl80211_p2p_device_exec_cmd(drv, ifname,
+					NL80211_CMD_START_P2P_DEVICE,
+					p2p_dev->wdev_id);
+
+	if (ret < 0) {
+		wpa_printf(MSG_ERROR, "nl80211: Failed to start p2p device"
+				" interface %s", ifname);
+		nl80211_deinit_p2p_dev(p2p_dev);
+		return NULL;
+	}
+
+	for (last = bss; last->next; last = last->next)
+		;
+	last->next = p2p_dev;
+
+	os_memcpy(addr, p2p_dev->addr, ETH_ALEN);
+
+	/* Register P2P Public Action frame on the device interface */
+	type = (WLAN_FC_TYPE_MGMT << 2) | (WLAN_FC_STYPE_ACTION << 4);
+	nl80211_register_frame(p2p_dev, bss->nl_mgmt, type,
+			       (u8 *) "\x04\x09\x50\x6f\x9a\x09", 6);
+	/* Register P2P Action frame on the device interface */
+	nl80211_register_frame(p2p_dev, bss->nl_mgmt, type,
+			       (u8 *) "\x7f\x50\x6f\x9a\x09", 5);
+
+	return p2p_dev;
+}
+#endif /* CONFIG_P2P */
 
 const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.name = "nl80211",
@@ -9480,4 +9614,8 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.send_tdls_mgmt = nl80211_send_tdls_mgmt,
 	.tdls_oper = nl80211_tdls_oper,
 #endif /* CONFIG_TDLS */
+#ifdef CONFIG_P2P
+	.init_p2p_dev = nl80211_init_p2p_dev,
+	.deinit_p2p_dev = nl80211_deinit_p2p_dev,
+#endif /* CONFIG_P2P */
 };
-- 
1.7.10.msysgit.1