From f725254cc1297532a553de9fa5af8484ec95cda4 Mon Sep 17 00:00:00 2001
From: Sreeramya Soratkal <ssramya@codeaurora.org>
Date: Thu, 22 Jul 2021 18:07:47 +0530
Subject: [PATCH] P2P: Enhance determination of secondary offset to support 6
 GHz channels

Current definition of wpas_p2p_get_ht40_mode() determines secondary
offset in the 5 GHz band. Enhance the functionality of this function to
determine offset to support 6 GHz channels also.

Signed-off-by: Sreeramya Soratkal <ssramya@codeaurora.org>
---
 src/common/ieee802_11_common.c  | 24 ++++++++++++++++++++++++
 src/common/ieee802_11_common.h  |  1 +
 wpa_supplicant/ap.c             |  4 ++--
 wpa_supplicant/p2p_supplicant.c | 23 +++++++++++++++++------
 wpa_supplicant/p2p_supplicant.h |  5 +++--
 5 files changed, 47 insertions(+), 10 deletions(-)

diff --git a/src/common/ieee802_11_common.c b/src/common/ieee802_11_common.c
index 3e5cfb01d..ef68abacd 100644
--- a/src/common/ieee802_11_common.c
+++ b/src/common/ieee802_11_common.c
@@ -2293,6 +2293,30 @@ bool is_6ghz_psc_frequency(int freq)
 }
 
 
+/**
+ * get_6ghz_sec_channel - Get the relative position of the secondary channel
+ * to the primary channel in 6 GHz
+ * @channel: Primary channel to be checked for (in global op class 131)
+ * Returns: 1 = secondary channel above, -1 = secondary channel below
+ */
+
+int get_6ghz_sec_channel(int channel)
+{
+	/*
+	 * In the 6 GHz band, primary channels are numbered as 1, 5, 9, 13.., so
+	 * the 40 MHz channels are formed with the channel pairs as (1,5),
+	 * (9,13), (17,21)..
+	 * The secondary channel for a given primary channel is below the
+	 * primary channel for the channels 5, 13, 21.. and it is above the
+	 * primary channel for the channels 1, 9, 17..
+	 */
+
+	if (((channel - 1) / 4) % 2)
+		return -1;
+	return 1;
+}
+
+
 int ieee802_11_parse_candidate_list(const char *pos, u8 *nei_rep,
 				    size_t nei_rep_len)
 {
diff --git a/src/common/ieee802_11_common.h b/src/common/ieee802_11_common.h
index fe2b1bca6..e9d7293e7 100644
--- a/src/common/ieee802_11_common.h
+++ b/src/common/ieee802_11_common.h
@@ -264,6 +264,7 @@ int center_idx_to_bw_6ghz(u8 idx);
 bool is_6ghz_freq(int freq);
 bool is_6ghz_op_class(u8 op_class);
 bool is_6ghz_psc_frequency(int freq);
+int get_6ghz_sec_channel(int channel);
 
 int ieee802_11_parse_candidate_list(const char *pos, u8 *nei_rep,
 				    size_t nei_rep_len);
diff --git a/wpa_supplicant/ap.c b/wpa_supplicant/ap.c
index 6c0f68ac1..d3fb239c8 100644
--- a/wpa_supplicant/ap.c
+++ b/wpa_supplicant/ap.c
@@ -280,8 +280,8 @@ int wpa_supplicant_conf_ap_ht(struct wpa_supplicant *wpa_s,
 			     HT_CAP_INFO_SUPP_CHANNEL_WIDTH_SET) &&
 			    ssid->ht40) {
 				conf->secondary_channel =
-					wpas_p2p_get_ht40_mode(wpa_s, mode,
-							       conf->channel);
+					wpas_p2p_get_sec_channel_offset_40mhz(
+						wpa_s, mode, conf->channel);
 				wpa_printf(MSG_DEBUG,
 					   "HT secondary channel offset %d for P2P group",
 					   conf->secondary_channel);
diff --git a/wpa_supplicant/p2p_supplicant.c b/wpa_supplicant/p2p_supplicant.c
index 92b696699..51d351a48 100644
--- a/wpa_supplicant/p2p_supplicant.c
+++ b/wpa_supplicant/p2p_supplicant.c
@@ -3893,30 +3893,41 @@ static int wpas_p2p_setup_channels(struct wpa_supplicant *wpa_s,
 }
 
 
-int wpas_p2p_get_ht40_mode(struct wpa_supplicant *wpa_s,
-			   struct hostapd_hw_modes *mode, u8 channel)
+int wpas_p2p_get_sec_channel_offset_40mhz(struct wpa_supplicant *wpa_s,
+					  struct hostapd_hw_modes *mode,
+					  u8 channel)
 {
 	int op;
 	enum chan_allowed ret;
 
 	for (op = 0; global_op_class[op].op_class; op++) {
 		const struct oper_class_map *o = &global_op_class[op];
-		u8 ch;
+		u16 ch;
+		int chan = channel;
 
 		if (o->p2p == NO_P2P_SUPP ||
 		    (is_6ghz_op_class(o->op_class) &&
 		     wpa_s->conf->p2p_6ghz_disable))
 			continue;
 
+		if (is_6ghz_op_class(o->op_class) && o->bw == BW40 &&
+		    get_6ghz_sec_channel(channel) < 0)
+			chan = channel - 4;
+
 		for (ch = o->min_chan; ch <= o->max_chan; ch += o->inc) {
 			if (o->mode != HOSTAPD_MODE_IEEE80211A ||
-			    (o->bw != BW40PLUS && o->bw != BW40MINUS) ||
-			    ch != channel)
+			    (o->bw != BW40PLUS && o->bw != BW40MINUS &&
+			     o->bw != BW40) ||
+			    ch != chan)
 				continue;
 			ret = wpas_p2p_verify_channel(wpa_s, mode, o->op_class,
 						      ch, o->bw);
-			if (ret == ALLOWED)
+			if (ret == ALLOWED) {
+				if (is_6ghz_op_class(o->op_class) &&
+				    o->bw == BW40)
+					return get_6ghz_sec_channel(channel);
 				return (o->bw == BW40MINUS) ? -1 : 1;
+			}
 		}
 	}
 	return 0;
diff --git a/wpa_supplicant/p2p_supplicant.h b/wpa_supplicant/p2p_supplicant.h
index dee9c1bcc..5a869e730 100644
--- a/wpa_supplicant/p2p_supplicant.h
+++ b/wpa_supplicant/p2p_supplicant.h
@@ -146,8 +146,9 @@ struct wpa_ssid * wpas_p2p_get_persistent(struct wpa_supplicant *wpa_s,
 void wpas_p2p_notify_ap_sta_authorized(struct wpa_supplicant *wpa_s,
 				       const u8 *addr);
 int wpas_p2p_scan_no_go_seen(struct wpa_supplicant *wpa_s);
-int wpas_p2p_get_ht40_mode(struct wpa_supplicant *wpa_s,
-			   struct hostapd_hw_modes *mode, u8 channel);
+int wpas_p2p_get_sec_channel_offset_40mhz(struct wpa_supplicant *wpa_s,
+					  struct hostapd_hw_modes *mode,
+					  u8 channel);
 int wpas_p2p_get_vht80_center(struct wpa_supplicant *wpa_s,
 			      struct hostapd_hw_modes *mode, u8 channel,
 			      u8 op_class);