P2P: Add preliminary P2P Manager AP support for hostapd

hostapd/config_file.c

@@ -1988,6 +1988,19 @@ struct hostapd_config * hostapd_config_read(const char *fname)
 			os_free(bss->upc);
 			bss->upc = os_strdup(pos);
 #endif /* CONFIG_WPS */
+#ifdef CONFIG_P2P_MANAGER
+		} else if (os_strcmp(buf, "manage_p2p") == 0) {
+			int manage = atoi(pos);
+			if (manage)
+				bss->p2p |= P2P_MANAGE;
+			else
+				bss->p2p &= ~P2P_MANAGE;
+		} else if (os_strcmp(buf, "allow_cross_connection") == 0) {
+			if (atoi(pos))
+				bss->p2p |= P2P_ALLOW_CROSS_CONNECTION;
+			else
+				bss->p2p &= ~P2P_ALLOW_CROSS_CONNECTION;
+#endif /* CONFIG_P2P_MANAGER */
 		} else {
 			wpa_printf(MSG_ERROR, "Line %d: unknown configuration "
 				   "item '%s'", line, buf);

hostapd/ctrl_iface.c

@@ -155,6 +155,59 @@ static int hostapd_ctrl_iface_new_sta(struct hostapd_data *hapd,
 }
 
 
+#ifdef CONFIG_P2P_MANAGER
+static int p2p_manager_disconnect(struct hostapd_data *hapd, u16 stype,
+				  u8 minor_reason_code, const u8 *addr)
+{
+	struct ieee80211_mgmt *mgmt;
+	int ret;
+	u8 *pos;
+
+	if (hapd->driver->send_frame == NULL)
+		return -1;
+
+	mgmt = os_zalloc(sizeof(*mgmt) + 100);
+	if (mgmt == NULL)
+		return -1;
+
+	wpa_printf(MSG_DEBUG, "P2P: Disconnect STA " MACSTR " with minor "
+		   "reason code %u (stype=%u)",
+		   MAC2STR(addr), minor_reason_code, stype);
+
+	mgmt->frame_control = IEEE80211_FC(WLAN_FC_TYPE_MGMT, stype);
+	os_memcpy(mgmt->da, addr, ETH_ALEN);
+	os_memcpy(mgmt->sa, hapd->own_addr, ETH_ALEN);
+	os_memcpy(mgmt->bssid, hapd->own_addr, ETH_ALEN);
+	if (stype == WLAN_FC_STYPE_DEAUTH) {
+		mgmt->u.deauth.reason_code =
+			host_to_le16(WLAN_REASON_PREV_AUTH_NOT_VALID);
+		pos = (u8 *) (&mgmt->u.deauth.reason_code + 1);
+	} else {
+		mgmt->u.disassoc.reason_code =
+			host_to_le16(WLAN_REASON_PREV_AUTH_NOT_VALID);
+		pos = (u8 *) (&mgmt->u.disassoc.reason_code + 1);
+	}
+
+	*pos++ = WLAN_EID_VENDOR_SPECIFIC;
+	*pos++ = 4 + 3 + 1;
+	WPA_PUT_BE24(pos, OUI_WFA);
+	pos += 3;
+	*pos++ = P2P_OUI_TYPE;
+
+	*pos++ = P2P_ATTR_MINOR_REASON_CODE;
+	WPA_PUT_LE16(pos, 1);
+	pos += 2;
+	*pos++ = minor_reason_code;
+
+	ret = hapd->driver->send_frame(hapd->drv_priv, (u8 *) mgmt,
+				       pos - (u8 *) mgmt, 1);
+	os_free(mgmt);
+
+	return ret < 0 ? -1 : 0;
+}
+#endif /* CONFIG_P2P_MANAGER */
+
+
 static int hostapd_ctrl_iface_deauthenticate(struct hostapd_data *hapd,
 					     const char *txtaddr)
 {
@@ -191,6 +244,14 @@ static int hostapd_ctrl_iface_deauthenticate(struct hostapd_data *hapd,
 		return 0;
 	}
 
+#ifdef CONFIG_P2P_MANAGER
+	pos = os_strstr(txtaddr, " p2p=");
+	if (pos) {
+		return p2p_manager_disconnect(hapd, WLAN_FC_STYPE_DEAUTH,
+					      atoi(pos + 5), addr);
+	}
+#endif /* CONFIG_P2P_MANAGER */
+
 	hapd->drv.sta_deauth(hapd, addr, WLAN_REASON_PREV_AUTH_NOT_VALID);
 	sta = ap_get_sta(hapd, addr);
 	if (sta)
@@ -237,6 +298,14 @@ static int hostapd_ctrl_iface_disassociate(struct hostapd_data *hapd,
 		return 0;
 	}
 
+#ifdef CONFIG_P2P_MANAGER
+	pos = os_strstr(txtaddr, " p2p=");
+	if (pos) {
+		return p2p_manager_disconnect(hapd, WLAN_FC_STYPE_DISASSOC,
+					      atoi(pos + 5), addr);
+	}
+#endif /* CONFIG_P2P_MANAGER */
+
 	hapd->drv.sta_disassoc(hapd, addr, WLAN_REASON_PREV_AUTH_NOT_VALID);
 	sta = ap_get_sta(hapd, addr);
 	if (sta)

hostapd/hostapd.conf

@@ -985,6 +985,11 @@ own_ip_addr=127.0.0.1
 # 12-digit, all-numeric code that identifies the consumer package.
 #upc=123456789012
 
+##### Wi-Fi Direct (P2P) ######################################################
+
+# Enable P2P Device management
+#manage_p2p=1
+
 ##### Multiple BSSID support ##################################################
 #
 # Above configuration is using the default interface (wlan#, or multi-SSID VLAN

src/ap/ap_config.h

@@ -317,6 +317,8 @@ struct hostapd_bss_config {
 #define P2P_ENABLED BIT(0)
 #define P2P_GROUP_OWNER BIT(1)
 #define P2P_GROUP_FORMATION BIT(2)
+#define P2P_MANAGE BIT(3)
+#define P2P_ALLOW_CROSS_CONNECTION BIT(4)
 	int p2p;
 };
 

src/ap/beacon.c

@@ -195,6 +195,29 @@ static u8 * hostapd_eid_wpa(struct hostapd_data *hapd, u8 *eid, size_t len,
 }
 
 
+#ifdef CONFIG_P2P_MANAGER
+u8 * hostapd_eid_p2p_manage(struct hostapd_data *hapd, u8 *eid)
+{
+	u8 bitmap;
+	*eid++ = WLAN_EID_VENDOR_SPECIFIC;
+	*eid++ = 4 + 3 + 1;
+	WPA_PUT_BE24(eid, OUI_WFA);
+	eid += 3;
+	*eid++ = P2P_OUI_TYPE;
+
+	*eid++ = P2P_ATTR_MANAGEABILITY;
+	WPA_PUT_LE16(eid, 1);
+	eid += 2;
+	bitmap = BIT(0); /* P2P Device Management */
+	if (hapd->conf->p2p & P2P_ALLOW_CROSS_CONNECTION)
+		bitmap |= BIT(1); /* Cross Connection Permitted */
+	*eid++ = bitmap;
+
+	return eid;
+}
+#endif /* CONFIG_P2P_MANAGER */
+
+
 void handle_probe_req(struct hostapd_data *hapd,
 		      const struct ieee80211_mgmt *mgmt, size_t len)
 {
@@ -368,6 +391,11 @@ void handle_probe_req(struct hostapd_data *hapd,
 		pos += wpabuf_len(hapd->p2p_probe_resp_ie);
 	}
 #endif /* CONFIG_P2P */
+#ifdef CONFIG_P2P_MANAGER
+	if ((hapd->conf->p2p & (P2P_MANAGE | P2P_ENABLED | P2P_GROUP_OWNER)) ==
+	    P2P_MANAGE)
+		pos = hostapd_eid_p2p_manage(hapd, pos);
+#endif /* CONFIG_P2P_MANAGER */
 
 	if (hapd->drv.send_mgmt_frame(hapd, resp, pos - (u8 *) resp) < 0)
 		perror("handle_probe_req: send");
@@ -487,6 +515,11 @@ void ieee802_11_set_beacon(struct hostapd_data *hapd)
 		tailpos += wpabuf_len(hapd->p2p_beacon_ie);
 	}
 #endif /* CONFIG_P2P */
+#ifdef CONFIG_P2P_MANAGER
+	if ((hapd->conf->p2p & (P2P_MANAGE | P2P_ENABLED | P2P_GROUP_OWNER)) ==
+	    P2P_MANAGE)
+		tailpos = hostapd_eid_p2p_manage(hapd, tailpos);
+#endif /* CONFIG_P2P_MANAGER */
 
 	tail_len = tailpos > tail ? tailpos - tail : 0;
 

src/ap/beacon.h

@@ -20,6 +20,7 @@ struct ieee80211_mgmt;
 
 void handle_probe_req(struct hostapd_data *hapd,
 		      const struct ieee80211_mgmt *mgmt, size_t len);
+u8 * hostapd_eid_p2p_manage(struct hostapd_data *hapd, u8 *eid);
 #ifdef NEED_AP_MLME
 void ieee802_11_set_beacon(struct hostapd_data *hapd);
 void ieee802_11_set_beacons(struct hostapd_iface *iface);

src/ap/ieee802_11.c

@@ -905,6 +905,11 @@ static void send_assoc_resp(struct hostapd_data *hapd, struct sta_info *sta,
 	}
 #endif /* CONFIG_P2P */
 
+#ifdef CONFIG_P2P_MANAGER
+	if (hapd->conf->p2p & P2P_MANAGE)
+		p = hostapd_eid_p2p_manage(hapd, p);
+#endif /* CONFIG_P2P_MANAGER */
+
 	send_len += p - reply->u.assoc_resp.variable;
 
 	if (hapd->drv.send_mgmt_frame(hapd, reply, send_len) < 0)