Prefer channel from P2P Preferred channel list for P2P Group formation

In a STA connected case, the P2P prefers STA channel over the
channel from Preferred frequency list mentioned in config file
during P2P group formation. The current change prioritizes usage
of preferred frequency list over STA channel.

Bug: 197360994
Test: Tested P2P connection in STA disconnected,
      STA connected to 2G and STA connected to 5G cases.

Signed-off-by: Vinayak Yadawad <vinayak.yadawad@broadcom.com>
Change-Id: I3325a060b5975aedc247ee5eb8c01831a245efc4
Merged-In: I3325a060b5975aedc247ee5eb8c01831a245efc4
(cherry picked from commit 8db34573537efdb1c811e19c7b93ed4f3bb01d4a)
diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index a7e7587..9215794 100644
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -104,6 +104,9 @@
  */
 #define P2P_RECONSIDER_GO_MOVE_DELAY 30
 
+/* Check if frequency is 2GHz */
+#define	IS_2GHZ(n)	(n >= 2412 && n <= 2484)
+
 enum p2p_group_removal_reason {
 	P2P_GROUP_REMOVAL_UNKNOWN,
 	P2P_GROUP_REMOVAL_SILENT,
@@ -5564,6 +5567,7 @@
 	struct wpa_used_freq_data *freqs;
 	int res, best_freq, num_unused;
 	unsigned int freq_in_use = 0, num, i, max_pref_freq;
+	int p2p_pref_freq;
 
 	max_pref_freq = *num_pref_freq;
 	*num_pref_freq = 0;
@@ -5632,46 +5636,68 @@
 
 	best_freq = wpas_p2p_pick_best_used_freq(wpa_s, freqs, num);
 
-	if (!wpa_s->conf->num_p2p_pref_chan && *pref_freq == 0) {
-		enum wpa_driver_if_type iface_type;
-
-		if (go)
-			iface_type = WPA_IF_P2P_GO;
-		else
-			iface_type = WPA_IF_P2P_CLIENT;
-
-		wpa_printf(MSG_DEBUG, "P2P: best_freq=%d, go=%d",
-			   best_freq, go);
-
-		res = wpa_drv_get_pref_freq_list(wpa_s, iface_type,
-						 &max_pref_freq,
-						 pref_freq_list);
-		if (!res && max_pref_freq > 0) {
-			*num_pref_freq = max_pref_freq;
+	if (*pref_freq == 0) {
+		if (wpa_s->conf->num_p2p_pref_chan && IS_2GHZ(best_freq)) {
 			i = 0;
-			while (i < *num_pref_freq &&
-			       (!p2p_supported_freq(wpa_s->global->p2p,
-						    pref_freq_list[i]) ||
-				wpas_p2p_disallowed_freq(wpa_s->global,
-							 pref_freq_list[i]))) {
-				wpa_printf(MSG_DEBUG,
-					   "P2P: preferred_freq_list[%d]=%d is disallowed",
-					   i, pref_freq_list[i]);
+			while (i < wpa_s->conf->num_p2p_pref_chan) {
+				p2p_pref_freq = ieee80211_chan_to_freq(NULL,
+					wpa_s->conf->p2p_pref_chan[i].op_class,
+					wpa_s->conf->p2p_pref_chan[i].chan);
+
+				if (p2p_supported_freq(wpa_s->global->p2p, p2p_pref_freq) &&
+					!wpas_p2p_disallowed_freq(wpa_s->global, p2p_pref_freq)) {
+					best_freq = p2p_pref_freq;
+					wpa_printf(MSG_DEBUG, "P2P: Using frequency (%u MHz) "
+						"from P2P preferred channel list", best_freq);
+					break;
+				} else {
+					wpa_printf(MSG_MSGDUMP, "P2P: Skipping preferred "
+						"frequency (%u MHz) ", p2p_pref_freq);
+				}
 				i++;
 			}
-			if (i != *num_pref_freq) {
-				best_freq = pref_freq_list[i];
-				wpa_printf(MSG_DEBUG,
-					   "P2P: Using preferred_freq_list[%d]=%d",
-					   i, best_freq);
+		} else {
+			enum wpa_driver_if_type iface_type;
+
+			if (go)
+				iface_type = WPA_IF_P2P_GO;
+			else
+				iface_type = WPA_IF_P2P_CLIENT;
+
+			wpa_printf(MSG_DEBUG, "P2P: best_freq=%d, go=%d",
+				best_freq, go);
+
+			res = wpa_drv_get_pref_freq_list(wpa_s, iface_type,
+						 &max_pref_freq,
+						 pref_freq_list);
+			if (!res && max_pref_freq > 0) {
+				*num_pref_freq = max_pref_freq;
+				i = 0;
+				while (i < *num_pref_freq &&
+					(!p2p_supported_freq(wpa_s->global->p2p,
+						pref_freq_list[i]) ||
+					wpas_p2p_disallowed_freq(wpa_s->global,
+							pref_freq_list[i]))) {
+					wpa_printf(MSG_DEBUG,
+						"P2P: preferred_freq_list[%d]=%d is disallowed",
+						i, pref_freq_list[i]);
+					i++;
+				}
+				if (i != *num_pref_freq) {
+					best_freq = pref_freq_list[i];
+					wpa_printf(MSG_DEBUG,
+						"P2P: Using preferred_freq_list[%d]=%d",
+						i, best_freq);
+				} else {
+					wpa_printf(MSG_DEBUG,
+						"P2P: All driver preferred frequencies are "
+						"disallowed for P2P use");
+					*num_pref_freq = 0;
+				}
 			} else {
 				wpa_printf(MSG_DEBUG,
-					   "P2P: All driver preferred frequencies are disallowed for P2P use");
-				*num_pref_freq = 0;
+					"P2P: No preferred frequency list available");
 			}
-		} else {
-			wpa_printf(MSG_DEBUG,
-				   "P2P: No preferred frequency list available");
 		}
 	}