qcacld-3.0: Fix 2.4 GHz SAP can't be started on 40 MHz

If SAP is initially started on 40 MHz bandwidth, keep using it.
But still keep the old logic 20 MHz for internal CSA for SAP.
This is to fix the SAP can't bring up on 2.4 GHz 40 MHz bw if SAP
OBSS scan disabled and configured as HT40 in hostapd conf.

Change-Id: Ib01be7fe594130f1eee35e3bfb773b1e913ec9c1
CRs-Fixed: 3234564

core/sap/src/sap_module.c

@@ -1217,6 +1217,39 @@ wlansap_5g_original_bw_validate(
 	return ch_width;
 }
 
+/**
+ * wlansap_2g_original_bw_validate() - validate bw for sap on 2.4 GHz
+ * @sap_context: sap context
+ * @chan_freq: channel frequency
+ * @ch_width: band width
+ * @sec_ch_freq: secondary channel frequency
+ *
+ * If initial SAP starts on 2.4 GHz HT40/HT20 mode, driver honors it.
+ *
+ * Return: new bandwidth
+ */
+static enum phy_ch_width
+wlansap_2g_original_bw_validate(struct sap_context *sap_context,
+				uint32_t chan_freq,
+				enum phy_ch_width ch_width,
+				qdf_freq_t *sec_ch_freq)
+{
+	if (sap_context->csa_reason == CSA_REASON_UNKNOWN &&
+	    WLAN_REG_IS_24GHZ_CH_FREQ(chan_freq) &&
+	    sap_context->ch_width_orig == CH_WIDTH_40MHZ) {
+		ch_width = CH_WIDTH_40MHZ;
+		if (sap_context->ch_params.sec_ch_offset == LOW_PRIMARY_CH)
+			*sec_ch_freq = chan_freq + 20;
+		else if (sap_context->ch_params.sec_ch_offset ==
+						HIGH_PRIMARY_CH)
+			*sec_ch_freq = chan_freq - 20;
+		else
+			*sec_ch_freq = 0;
+	}
+
+	return ch_width;
+}
+
 enum phy_ch_width
 wlansap_get_csa_chanwidth_from_phymode(struct sap_context *sap_context,
 				       uint32_t chan_freq,
@@ -1226,6 +1259,7 @@ wlansap_get_csa_chanwidth_from_phymode(struct sap_context *sap_context,
 	struct mac_context *mac;
 	struct ch_params ch_params = {0};
 	uint32_t channel_bonding_mode = 0;
+	qdf_freq_t sec_ch_freq = 0;
 
 	mac = sap_get_mac_context();
 	if (!mac) {
@@ -1239,7 +1273,9 @@ wlansap_get_csa_chanwidth_from_phymode(struct sap_context *sap_context,
 		 * SAP coming up in HT40 on channel switch we are
 		 * disabling channel bonding in 2.4Ghz.
 		 */
-		ch_width = CH_WIDTH_20MHZ;
+		ch_width = wlansap_2g_original_bw_validate(
+				sap_context, chan_freq, CH_WIDTH_20MHZ,
+				&sec_ch_freq);
 	} else {
 		wlan_mlme_get_channel_bonding_5ghz(mac->psoc,
 						   &channel_bonding_mode);
@@ -1261,7 +1297,7 @@ wlansap_get_csa_chanwidth_from_phymode(struct sap_context *sap_context,
 	ch_params.ch_width = ch_width;
 	if (sap_phymode_is_eht(sap_context->phyMode))
 		wlan_reg_set_create_punc_bitmap(&ch_params, true);
-	wlan_reg_set_channel_params_for_freq(mac->pdev, chan_freq, 0,
+	wlan_reg_set_channel_params_for_freq(mac->pdev, chan_freq, sec_ch_freq,
 					     &ch_params);
 	ch_width = ch_params.ch_width;
 	if (tgt_ch_params)