Fix p2p service discovery

- Fix listen timing to improve SD reliability
- Fix SD packet scheduling

Signed-off-by: Eyal Shapira <eyal@wizery.com>

3 files changed, 100 insertions, 3 deletions

diff --git a/src/p2p/p2p.c b/src/p2p/p2p.c
index e434116..f55fa6d 100644
--- a/src/p2p/p2p.c
+++ b/src/p2p/p2p.c
@@ -84,6 +84,11 @@ static void p2p_expire_peers(struct p2p_data *p2p)
 
 		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Expiring old peer "
 			"entry " MACSTR, MAC2STR(dev->info.p2p_device_addr));
+#ifdef ANDROID_P2P
+		/* SD_FAIR_POLICY: Update the current sd_dev_list pointer to next device */
+		if(&dev->list == p2p->sd_dev_list)
+			p2p->sd_dev_list = dev->list.next;
+#endif
 		dl_list_del(&dev->list);
 		p2p_device_free(p2p, dev);
 	}
@@ -376,6 +381,11 @@ static struct p2p_device * p2p_create_device(struct p2p_data *p2p,
 		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 			"P2P: Remove oldest peer entry to make room for a new "
 			"peer");
+#ifdef ANDROID_P2P
+		/* SD_FAIR_POLICY: Update the current sd_dev_list pointer to next device */
+		if(&oldest->list == p2p->sd_dev_list)
+			p2p->sd_dev_list = oldest->list.next;
+#endif
 		dl_list_del(&oldest->list);
 		p2p_device_free(p2p, oldest);
 	}
@@ -553,7 +563,6 @@ static void p2p_copy_wps_info(struct p2p_device *dev, int probe_req,
 		dev->ext_listen_interval =
 			WPA_GET_LE16(msg->ext_listen_timing + 2);
 	}
-
 	if (!probe_req) {
 		dev->info.config_methods = msg->config_methods ?
 			msg->config_methods : msg->wps_config_methods;
@@ -891,7 +900,15 @@ static int p2p_run_after_scan(struct p2p_data *p2p)
 				      p2p->after_scan_tx->wait_time);
 		os_free(p2p->after_scan_tx);
 		p2p->after_scan_tx = NULL;
+#ifdef ANDROID_P2P
+		/* For SD frames, there is a scenario, where we can receive a SD request frame during p2p_scan.
+		 * At that moment, we will send the SD response from this context. After sending the SD response,
+		 * we need to continue p2p_find. But if we return 1 from here, p2p_find is going to be stopped.
+		 */
+		return 0;
+#else
 		return 1;
+#endif
 	}
 
 	op = p2p->start_after_scan;
@@ -2290,7 +2307,16 @@ struct p2p_data * p2p_init(const struct p2p_config *cfg)
 			p2p->cfg->num_pref_chan = 0;
 	}
 
+#ifdef ANDROID_P2P
+	/* 100ms listen time is too less to receive the response frames in some scenarios
+	 * increasing min listen time to 200ms.
+	 */
+	p2p->min_disc_int = 2;
+	/* SD_FAIR_POLICY: Initializing the SD current serviced pointer to NULL */
+	p2p->sd_dev_list = NULL;
+#else
 	p2p->min_disc_int = 1;
+#endif
 	p2p->max_disc_int = 3;
 
 	os_get_random(&p2p->next_tie_breaker, 1);
@@ -2341,6 +2367,10 @@ void p2p_flush(struct p2p_data *p2p)
 		dl_list_del(&dev->list);
 		p2p_device_free(p2p, dev);
 	}
+#ifdef ANDROID_P2P
+	/* SD_FAIR_POLICY: Initializing the SD current serviced pointer to NULL */
+	p2p->sd_dev_list = NULL;
+#endif
 	p2p_free_sd_queries(p2p);
 	os_free(p2p->after_scan_tx);
 	p2p->after_scan_tx = NULL;
@@ -2519,8 +2549,37 @@ int p2p_set_country(struct p2p_data *p2p, const char *country)
 void p2p_continue_find(struct p2p_data *p2p)
 {
 	struct p2p_device *dev;
+#ifdef ANDROID_P2P
+	int skip=1;
+#endif
 	p2p_set_state(p2p, P2P_SEARCH);
 	dl_list_for_each(dev, &p2p->devices, struct p2p_device, list) {
+#ifdef ANDROID_P2P
+		/* SD_FAIR_POLICY: We need to give chance to all devices in the device list
+		 * There may be a scenario, where a particular peer device have
+		 * not registered any query response. When we send a SD request to such device,
+		 * no response will be received. And if we continue to get probe responses from that device,
+		 * and if that device happens to be on top in our device list,
+		 * we will always continue to send SD requests always to that peer only.
+		 * We will not be able to send SD requests to other devices in that case.
+		 * This implementation keeps track of last serviced peer device.
+		 * And then takes the next one from the device list, in the next iteration.
+		 */
+		if (p2p->sd_dev_list && p2p->sd_dev_list != &p2p->devices) {
+			if(skip) {
+				if ((&dev->list == p2p->sd_dev_list) ) {
+					skip = 0;
+					if (dev->list.next == &p2p->devices)
+						p2p->sd_dev_list = NULL;
+				}
+				continue;
+			}
+		}
+		p2p->sd_dev_list = &dev->list;
+		wpa_printf(MSG_DEBUG, "P2P: ### Servicing %p dev->flags 0x%x SD schedule %s devaddr " MACSTR,
+			p2p->sd_dev_list, dev->flags, dev->flags & P2P_DEV_SD_SCHEDULE ? "TRUE": "FALSE",
+			MAC2STR(dev->info.p2p_device_addr));
+#endif
 		if (dev->flags & P2P_DEV_SD_SCHEDULE) {
 			if (p2p_start_sd(p2p, dev) == 0)
 				return;
diff --git a/src/p2p/p2p_i.h b/src/p2p/p2p_i.h
index 254662e..ca5fc3a 100644
--- a/src/p2p/p2p_i.h
+++ b/src/p2p/p2p_i.h
@@ -224,6 +224,14 @@ struct p2p_data {
 	 */
 	struct dl_list devices;
 
+#ifdef ANDROID_P2P
+	/**
+	 * sd_dev_list - device pointer to be serviced next
+	 * for service discovery
+	 */
+	struct dl_list *sd_dev_list;
+#endif
+
 	/**
 	 * go_neg_peer - Pointer to GO Negotiation peer
 	 */
diff --git a/src/p2p/p2p_sd.c b/src/p2p/p2p_sd.c
index 5cf1cfe..4b027fa 100644
--- a/src/p2p/p2p_sd.c
+++ b/src/p2p/p2p_sd.c
@@ -411,7 +411,7 @@ void p2p_rx_gas_initial_resp(struct p2p_data *p2p, const u8 *sa,
 
 	if (p2p->state != P2P_SD_DURING_FIND || p2p->sd_peer == NULL ||
 	    os_memcmp(sa, p2p->sd_peer->info.p2p_device_addr, ETH_ALEN) != 0) {
-		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
+		wpa_msg(p2p->cfg->msg_ctx, MSG_ERROR,
 			"P2P: Ignore unexpected GAS Initial Response from "
 			MACSTR, MAC2STR(sa));
 		return;
@@ -846,6 +846,19 @@ void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
 		      const struct wpabuf *tlvs)
 {
 	struct p2p_sd_query *q;
+#ifdef ANDROID_P2P
+	/* Currently, supplicant doesn't support more than one pending broadcast SD request.
+	 * So reject if application is registering another one before cancelling the existing one.
+	 */
+	for (q = p2p->sd_queries; q; q = q->next) {
+		if( (q->for_all_peers == 1) && (!dst)) {
+				wpa_printf(MSG_ERROR, "P2P: Already one pending"
+					" Broadcast request. Please cancel the current one"
+					" before adding a new one");
+				return NULL;
+		}
+	}
+#endif
 
 	q = os_zalloc(sizeof(*q));
 	if (q == NULL)
@@ -864,7 +877,7 @@ void * p2p_sd_request(struct p2p_data *p2p, const u8 *dst,
 
 	q->next = p2p->sd_queries;
 	p2p->sd_queries = q;
-	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p", q);
+	wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG, "P2P: Added SD Query %p for_all_peers %d", q, q->for_all_peers);
 
 	if (dst == NULL) {
 		struct p2p_device *dev;
@@ -885,8 +898,25 @@ void p2p_sd_service_update(struct p2p_data *p2p)
 int p2p_sd_cancel_request(struct p2p_data *p2p, void *req)
 {
 	if (p2p_unlink_sd_query(p2p, req)) {
+#ifdef ANDROID_P2P
+	struct p2p_device *dev;
+	struct p2p_sd_query *q = (struct p2p_sd_query *)req;
+#endif
 		wpa_msg(p2p->cfg->msg_ctx, MSG_DEBUG,
 			"P2P: Cancel pending SD query %p", req);
+#ifdef ANDROID_P2P
+		/* If the request is a bcast query, then clear the
+		 * P2P_DEV_SD_INFO flag so that when new sd query is registered,
+		 * we will send the SD request frames to peer devices.
+		 */
+		if(q->for_all_peers) {
+			p2p->sd_dev_list = NULL;
+			dl_list_for_each(dev, &p2p->devices,
+							struct p2p_device, list) {
+				dev->flags &= ~P2P_DEV_SD_INFO;
+			}
+		}
+#endif
 		p2p_free_sd_query(req);
 		return 0;
 	}