From 6219efda5b74baf62086236cc7f368f4307b9869 Mon Sep 17 00:00:00 2001
From: Michael-CY Lee <michael-cy.lee@mediatek.com>
Date: Mon, 11 Sep 2023 10:16:35 +0800
Subject: [PATCH] hostapd: mtk: synchronize bandwidth in AP/STA support

Signed-off-by: Michael Lee <michael-cy.lee@mediatek.com>
---
 src/ap/ucode.c         | 39 ++++++++++++++++++--
 src/utils/ucode.c      | 12 +++++--
 wpa_supplicant/ucode.c | 82 ++++++++++++++++++++++++++++++++++--------
 3 files changed, 115 insertions(+), 18 deletions(-)

diff --git a/src/ap/ucode.c b/src/ap/ucode.c
index 7bc797a..a3716ea 100644
--- a/src/ap/ucode.c
+++ b/src/ap/ucode.c
@@ -382,6 +382,9 @@ uc_hostapd_iface_stop(uc_vm_t *vm, size_t nargs)
 	struct hostapd_iface *iface = uc_fn_thisval("hostapd.iface");
 	int i;
 
+	wpa_printf(MSG_INFO, "ucode: mtk: stop iface for %s in state %s\n",
+			iface->phy, hostapd_state_text(iface->state));
+
 	switch (iface->state) {
 	case HAPD_IFACE_ENABLED:
 	case HAPD_IFACE_DISABLED:
@@ -420,6 +423,7 @@ uc_hostapd_iface_start(uc_vm_t *vm, size_t nargs)
 	uint64_t intval;
 	int i;
 
+	wpa_printf(MSG_INFO, "ucode: mtk: start iface for %s\n", iface->phy);
 	if (!iface)
 		return NULL;
 
@@ -442,7 +446,13 @@ uc_hostapd_iface_start(uc_vm_t *vm, size_t nargs)
 	UPDATE_VAL(op_class, "op_class");
 	UPDATE_VAL(hw_mode, "hw_mode");
 	UPDATE_VAL(channel, "channel");
-	UPDATE_VAL(secondary_channel, "sec_channel");
+
+	intval = ucv_int64_get(ucv_object_get(info, "sec_channel", NULL));
+	if (!errno) {
+		conf->secondary_channel = intval;
+		changed = true;
+	}
+
 	if (!changed &&
 	    (iface->bss[0]->beacon_set_done ||
 	     iface->state == HAPD_IFACE_DFS))
@@ -490,6 +500,18 @@ out:
 		return ucv_boolean_new(true);
 	}
 
+	wpa_printf(MSG_INFO, "ucode: mtk: updated channel information:\n");
+	wpa_printf(MSG_INFO, "    * channel: %d\n", conf->channel);
+	wpa_printf(MSG_INFO, "    * op_class: %d\n", conf->op_class);
+	wpa_printf(MSG_INFO, "    * secondary channel: %d\n",
+			conf->secondary_channel);
+	wpa_printf(MSG_INFO, "    * seg0: %d\n",
+			hostapd_get_oper_centr_freq_seg0_idx(conf));
+	wpa_printf(MSG_INFO, "    * seg1: %d\n",
+			hostapd_get_oper_centr_freq_seg0_idx(conf));
+	wpa_printf(MSG_INFO, "    * oper_chwidth: %d\n",
+			hostapd_get_oper_chwidth(conf));
+
 	for (i = 0; i < iface->num_bss; i++) {
 		struct hostapd_data *hapd = iface->bss[i];
 		int ret;
@@ -524,6 +546,7 @@ uc_hostapd_iface_switch_channel(uc_vm_t *vm, size_t nargs)
 	uint64_t intval;
 	int i, ret = 0;
 
+	wpa_printf(MSG_INFO, "ucode: mtk: channel switch for %s\n", iface->phy);
 	if (!iface || ucv_type(info) != UC_OBJECT)
 		return NULL;
 
@@ -543,7 +566,8 @@ uc_hostapd_iface_switch_channel(uc_vm_t *vm, size_t nargs)
 	if (errno)
 		intval = hostapd_get_oper_chwidth(conf);
 	if (intval)
-		csa.freq_params.bandwidth = 40 << intval;
+		csa.freq_params.bandwidth = 40 <<
+			(intval == CONF_OPER_CHWIDTH_320MHZ ? 3 : intval);
 	else
 		csa.freq_params.bandwidth = csa.freq_params.sec_channel_offset ? 40 : 20;
 
@@ -554,6 +578,17 @@ uc_hostapd_iface_switch_channel(uc_vm_t *vm, size_t nargs)
 	if ((intval = ucv_int64_get(ucv_object_get(info, "center_freq2", NULL))) && !errno)
 		csa.freq_params.center_freq2 = intval;
 
+	wpa_printf(MSG_INFO, "ucode: mtk: switch channel information:\n");
+	wpa_printf(MSG_INFO, "    * freq is %d\n", csa.freq_params.freq);
+	wpa_printf(MSG_INFO, "    * bandwidth is %d\n",
+			csa.freq_params.bandwidth);
+	wpa_printf(MSG_INFO, "    * sec_chan_offset is %d\n",
+			csa.freq_params.sec_channel_offset);
+	wpa_printf(MSG_INFO, "    * center_freq1 is %d\n",
+			csa.freq_params.center_freq1);
+	wpa_printf(MSG_INFO, "    * center_freq2 is %d\n",
+			csa.freq_params.center_freq2);
+
 	for (i = 0; i < iface->num_bss; i++)
 		ret = hostapd_switch_channel(iface->bss[i], &csa);
 
diff --git a/src/utils/ucode.c b/src/utils/ucode.c
index 122c619..0990e7b 100644
--- a/src/utils/ucode.c
+++ b/src/utils/ucode.c
@@ -51,6 +51,7 @@ uc_value_t *uc_wpa_freq_info(uc_vm_t *vm, size_t nargs)
 	uc_value_t *freq = uc_fn_arg(0);
 	uc_value_t *sec = uc_fn_arg(1);
 	int width = ucv_uint64_get(uc_fn_arg(2));
+	int bw320_offset = 1;
 	int freq_val, center_idx, center_ofs;
 	enum oper_chan_width chanwidth;
 	enum hostapd_hw_mode hw_mode;
@@ -88,6 +89,9 @@ uc_value_t *uc_wpa_freq_info(uc_vm_t *vm, size_t nargs)
 	case 9:
 		width = 3;
 		chanwidth = CONF_OPER_CHWIDTH_320MHZ;
+
+		/* bw320_offset is 1 for 320 MHz-1, and 2 for 320 MHz-2 */
+		bw320_offset = ucv_uint64_get(uc_fn_arg(3));
 		break;
 	default:
 		return NULL;
@@ -119,12 +123,16 @@ uc_value_t *uc_wpa_freq_info(uc_vm_t *vm, size_t nargs)
 	ucv_object_add(ret, "hw_mode_str", ucv_get(ucv_string_new(modestr)));
 	ucv_object_add(ret, "sec_channel", ucv_int64_new(sec_channel));
 	ucv_object_add(ret, "frequency", ucv_int64_new(freq_val));
+	ucv_object_add(ret, "oper_chwidth", ucv_int64_new(chanwidth));
 
-	if (!sec_channel)
+	if (chanwidth == CONF_OPER_CHWIDTH_USE_HT && !sec_channel) {
+		ucv_object_add(ret, "center_seg0_idx", ucv_int64_new(channel));
+		ucv_object_add(ret, "center_freq1", ucv_int64_new(freq_val));
 		return ret;
+	}
 
 	if (freq_val >= 5900)
-		center_ofs = 0;
+		center_ofs = 32 * (1 - bw320_offset);
 	else if (freq_val >= 5745)
 		center_ofs = 20;
 	else
diff --git a/wpa_supplicant/ucode.c b/wpa_supplicant/ucode.c
index 55d2258..d8a19b8 100644
--- a/wpa_supplicant/ucode.c
+++ b/wpa_supplicant/ucode.c
@@ -7,6 +7,8 @@
 #include "wps_supplicant.h"
 #include "bss.h"
 #include "ucode.h"
+#include "driver_i.h"
+#include "common/ieee802_11_common.h"
 
 static struct wpa_global *wpa_global;
 static uc_resource_type_t *global_type, *iface_type;
@@ -96,6 +98,8 @@ void wpas_ucode_event(struct wpa_supplicant *wpa_s, int event, union wpa_event_d
 {
 	const char *state;
 	uc_value_t *val;
+	enum oper_chan_width ch_width;
+	int center_freq1, bw320_offset = 1;
 
 	if (event != EVENT_CH_SWITCH_STARTED)
 		return;
@@ -114,11 +118,42 @@ void wpas_ucode_event(struct wpa_supplicant *wpa_s, int event, union wpa_event_d
 	uc_value_push(ucv_get(val));
 
 	if (event == EVENT_CH_SWITCH_STARTED) {
+		center_freq1 = data->ch_switch.cf1;
+
+		switch (data->ch_switch.ch_width) {
+		case CHAN_WIDTH_80:
+			ch_width = CONF_OPER_CHWIDTH_80MHZ;
+			break;
+		case CHAN_WIDTH_80P80:
+			ch_width = CONF_OPER_CHWIDTH_80P80MHZ;
+			break;
+		case CHAN_WIDTH_160:
+			ch_width = CONF_OPER_CHWIDTH_160MHZ;
+			break;
+		case CHAN_WIDTH_320:
+			ch_width = CONF_OPER_CHWIDTH_320MHZ;
+			break;
+		case CHAN_WIDTH_20_NOHT:
+		case CHAN_WIDTH_20:
+		case CHAN_WIDTH_40:
+		default:
+			ch_width = CONF_OPER_CHWIDTH_USE_HT;
+			break;
+		}
+
+		/* Check bandwidth 320 MHz-2 */
+		if (ch_width == CONF_OPER_CHWIDTH_320MHZ &&
+		    (center_freq1 == 6265) || center_freq1 == 6585 ||
+		     center_freq1 == 6905)
+			bw320_offset = 2;
+
 		ucv_object_add(val, "csa_count", ucv_int64_new(data->ch_switch.count));
 		ucv_object_add(val, "frequency", ucv_int64_new(data->ch_switch.freq));
 		ucv_object_add(val, "sec_chan_offset", ucv_int64_new(data->ch_switch.ch_offset));
-		ucv_object_add(val, "center_freq1", ucv_int64_new(data->ch_switch.cf1));
+		ucv_object_add(val, "center_freq1", ucv_int64_new(center_freq1));
 		ucv_object_add(val, "center_freq2", ucv_int64_new(data->ch_switch.cf2));
+		ucv_object_add(val, "ch_width", ucv_int64_new(ch_width));
+		ucv_object_add(val, "bw320_offset", ucv_int64_new(bw320_offset));
 	}
 
 	ucv_put(wpa_ucode_call(4));
@@ -195,6 +230,11 @@ uc_wpas_iface_status(uc_vm_t *vm, size_t nargs)
 	struct wpa_supplicant *wpa_s = uc_fn_thisval("wpas.iface");
 	struct wpa_bss *bss;
 	uc_value_t *ret, *val;
+	struct wpa_channel_info ci;
+	u8 op_class, channel;
+	enum oper_chan_width ch_width;
+	int center_freq1, bw320_offset = 1, is_24ghz;
+	enum hostapd_hw_mode hw_mode;
 
 	if (!wpa_s)
 		return NULL;
@@ -207,23 +247,37 @@ uc_wpas_iface_status(uc_vm_t *vm, size_t nargs)
 	bss = wpa_s->current_bss;
 	if (bss) {
 		int sec_chan = 0;
-		const u8 *ie;
-
-		ie = wpa_bss_get_ie(bss, WLAN_EID_HT_OPERATION);
-		if (ie && ie[1] >= 2) {
-			const struct ieee80211_ht_operation *ht_oper;
-			int sec;
-
-			ht_oper = (const void *) (ie + 2);
-			sec = ht_oper->ht_param & HT_INFO_HT_PARAM_SECONDARY_CHNL_OFF_MASK;
-			if (sec == HT_INFO_HT_PARAM_SECONDARY_CHNL_ABOVE)
-				sec_chan = 1;
-			else if (sec == HT_INFO_HT_PARAM_SECONDARY_CHNL_BELOW)
-				sec_chan = -1;
+
+		hw_mode = ieee80211_freq_to_chan(bss->freq, &channel);
+		is_24ghz = hw_mode == HOSTAPD_MODE_IEEE80211G ||
+			hw_mode == HOSTAPD_MODE_IEEE80211B;
+
+		wpa_drv_channel_info(wpa_s, &ci);
+		center_freq1 = ci.center_frq1;
+
+		if (bss->freq != center_freq1) {
+			if (is_24ghz)
+				sec_chan = (bss->freq < center_freq1) ? 1 : -1;
+			else
+				sec_chan = (bss->freq / 20) & 1 ? 1 : -1;
+		}
+
+		if (ieee80211_chaninfo_to_channel(ci.frequency, ci.chanwidth,
+						  sec_chan, &op_class, &channel))
+			return NULL;
+
+		ch_width = op_class_to_ch_width(op_class);
+		if (ch_width == CONF_OPER_CHWIDTH_320MHZ &&
+		    (center_freq1 == 6265) || center_freq1 == 6585 ||
+		     center_freq1 == 6905) {
+			/* Bandwidth 320 MHz-2 */
+			bw320_offset = 2;
 		}
 
 		ucv_object_add(ret, "sec_chan_offset", ucv_int64_new(sec_chan));
 		ucv_object_add(ret, "frequency", ucv_int64_new(bss->freq));
+		ucv_object_add(ret, "ch_width", ucv_int64_new(ch_width));
+		ucv_object_add(ret, "bw320_offset", ucv_int64_new(bw320_offset));
 	}
 
 #ifdef CONFIG_MESH
-- 
2.25.1