From 0721fce1983732b73f7f40bba90162f30d5ddb2b Mon Sep 17 00:00:00 2001
From: Suresh Sanaboina <Suresh.Sanaboina@infineon.com>
Date: Fri, 17 Jun 2022 10:43:18 -0500
Subject: [PATCH] brcmfmac: report cqm rssi event based on rssi change for
 wpa_supplicant bgscan

FW reports rssi change indication using BRCM_E_RSSI event. Register for
same event and report rssi change indication to supplicant using
CQM event.

Signed-off-by: Suresh Sanaboina <Suresh.Sanaboina@infineon.com>
---
 .../broadcom/brcm80211/brcmfmac/cfg80211.c    | 100 +++++++++++-------
 .../broadcom/brcm80211/brcmfmac/fwil_types.h  |  17 +++
 2 files changed, 81 insertions(+), 36 deletions(-)

diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
index aa6b9cdb5..82f85a86f 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
@@ -3605,41 +3605,6 @@ next_bss_le(struct brcmf_scan_results *list, struct brcmf_bss_info_le *bss)
 					    le32_to_cpu(bss->length));
 }
 
-static s32 brcmf_singal_monitor(struct brcmf_cfg80211_info *cfg,
-					struct brcmf_if *ifp)
-{
-	s32 err = 0, rssi = 0;
-	int i;
-	u8 bssid[ETH_ALEN] = {0};
-	struct brcmf_scan_results *bss_list;
-	struct brcmf_bss_info_le *bi = NULL;
-
-	if(!cfg->cqm_info.enable)
-		return 0;
-
-	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSSID, bssid, ETH_ALEN);
-	bss_list = (struct brcmf_scan_results *)cfg->escan_info.escan_buf;
-
-	/* wpa bgscan feature: wpa will send scan request after associated
-	 * to the target AP, when we get the scan result of this associated
-	 * AP, send the rssi threshold event upward */
-	for (i = 0; i < bss_list->count; i++) {
-		bi = next_bss_le(bss_list, bi);
-		if (err == 0 && memcmp(bi->BSSID, bssid, ETH_ALEN) == 0) {
-			rssi = (s16)le16_to_cpu(bi->RSSI);
-			brcmf_dbg(TRACE, "%s(%pM), rssi: %d, threshold: %d, send event(%s)\n",
-				bi->SSID, bi->BSSID, rssi, cfg->cqm_info.rssi_threshold,
-				rssi > cfg->cqm_info.rssi_threshold ? "HIGH" : "LOW");
-			cfg80211_cqm_rssi_notify(cfg_to_ndev(cfg),
-				(rssi > cfg->cqm_info.rssi_threshold ?
-					NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH :
-					NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW),
-				rssi, GFP_KERNEL);
-		}
-	}
-	return 0;
-}
-
 static s32 brcmf_inform_bss(struct brcmf_cfg80211_info *cfg)
 {
 	struct brcmf_pub *drvr = cfg->pub;
@@ -3947,7 +3912,6 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 			goto exit;
 		if (cfg->int_escan_map || cfg->scan_request) {
 			brcmf_inform_bss(cfg);
-			brcmf_singal_monitor(cfg, ifp);
 			aborted = status != BRCMF_E_STATUS_SUCCESS;
 			brcmf_notify_escan_complete(cfg, ifp, aborted, false);
 		} else
@@ -6397,6 +6361,39 @@ brcmf_cfg80211_external_auth(struct wiphy *wiphy, struct net_device *dev,
 	return ret;
 }
 
+static int
+brcmf_cfg80211_set_cqm_rssi_config(struct wiphy *wiphy, struct net_device *dev,
+				   s32 rssi_thold, u32 rssi_hyst)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_if *ifp;
+	struct wl_rssi_event rssi = {};
+	int err = 0;
+
+	ifp = netdev_priv(dev);
+	if (rssi_thold == 0) {
+		cfg->cqm_info.enable = 0;
+		cfg->cqm_info.rssi_threshold = 0;
+	} else {
+		cfg->cqm_info.enable = 1;
+		cfg->cqm_info.rssi_threshold = rssi_thold;
+
+		rssi.rate_limit_msec = 0;
+		rssi.rssi_levels[rssi.num_rssi_levels++] = S8_MIN;
+		rssi.rssi_levels[rssi.num_rssi_levels++] =
+				cfg->cqm_info.rssi_threshold;
+		rssi.rssi_levels[rssi.num_rssi_levels++] = S8_MAX;
+	}
+
+	err = brcmf_fil_iovar_data_set(ifp, "rssi_event", &rssi, sizeof(rssi));
+	if (err < 0)
+		brcmf_err("set rssi_event iovar failed (%d)\n", err);
+
+	brcmf_dbg(TRACE, "enable = %d, rssi_threshold = %d\n",
+		cfg->cqm_info.enable, cfg->cqm_info.rssi_threshold);
+	return err;
+}
+
 static struct cfg80211_ops brcmf_cfg80211_ops = {
 	.add_virtual_intf = brcmf_cfg80211_add_iface,
 	.del_virtual_intf = brcmf_cfg80211_del_iface,
@@ -6446,6 +6443,7 @@ static struct cfg80211_ops brcmf_cfg80211_ops = {
 	.del_pmk = brcmf_cfg80211_del_pmk,
 	.change_bss = brcmf_cfg80211_change_bss,
 	.external_auth = brcmf_cfg80211_external_auth,
+	.set_cqm_rssi_config = brcmf_cfg80211_set_cqm_rssi_config,
 };
 
 struct cfg80211_ops *brcmf_cfg80211_get_ops(struct brcmf_mp_device *settings)
@@ -7385,6 +7383,34 @@ brcmf_notify_mgmt_tx_status(struct brcmf_if *ifp,
 	return 0;
 }
 
+static s32
+brcmf_notify_rssi_change_ind(struct brcmf_if *ifp,
+			     const struct brcmf_event_msg *e, void *data)
+{
+	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
+	struct wl_event_data_rssi *value = (struct wl_event_data_rssi *)data;
+	s32 rssi = 0;
+
+	brcmf_dbg(INFO, "Enter: event %s (%d), status=%d\n",
+		  brcmf_fweh_event_name(e->event_code), e->event_code,
+		  e->status);
+
+	if (!cfg->cqm_info.enable)
+		return 0;
+
+	rssi = ntohl(value->rssi);
+	brcmf_dbg(TRACE, "rssi: %d, threshold: %d, send event(%s)\n",
+		  rssi, cfg->cqm_info.rssi_threshold,
+		  rssi > cfg->cqm_info.rssi_threshold ? "HIGH" : "LOW");
+	cfg80211_cqm_rssi_notify(cfg_to_ndev(cfg),
+				 (rssi > cfg->cqm_info.rssi_threshold ?
+					NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH :
+					NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW),
+				 rssi, GFP_KERNEL);
+
+	return 0;
+}
+
 static void brcmf_init_conf(struct brcmf_cfg80211_conf *conf)
 {
 	conf->frag_threshold = (u32)-1;
@@ -7439,6 +7465,8 @@ static void brcmf_register_event_handlers(struct brcmf_cfg80211_info *cfg)
 			    brcmf_notify_mgmt_tx_status);
 	brcmf_fweh_register(cfg->pub, BRCMF_E_MGMT_FRAME_OFF_CHAN_COMPLETE,
 			    brcmf_notify_mgmt_tx_status);
+	brcmf_fweh_register(cfg->pub, BRCMF_E_RSSI,
+			    brcmf_notify_rssi_change_ind);
 }
 
 static void brcmf_deinit_priv_mem(struct brcmf_cfg80211_info *cfg)
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
index 86539bef6..874cb4919 100644
--- a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
@@ -198,6 +198,8 @@
 
 #define DL_TYPE_CLM			2
 
+#define MAX_RSSI_LEVELS			8
+
 /* join preference types for join_pref iovar */
 enum brcmf_join_pref_types {
 	BRCMF_JOIN_PREF_RSSI = 1,
@@ -1282,4 +1284,19 @@ struct brcmf_mkeep_alive_pkt_le {
 	u8   data[];
 } __packed;
 
+/* BRCM_E_RSSI event data */
+struct wl_event_data_rssi {
+	s32 rssi;
+	s32 snr;
+	s32 noise;
+};
+
+/** RSSI event notification configuration. */
+struct wl_rssi_event {
+	u32 rate_limit_msec;
+	u8 num_rssi_levels;
+	s8 rssi_levels[MAX_RSSI_LEVELS];
+	s8 pad[3];
+};
+
 #endif /* FWIL_TYPES_H_ */
-- 
2.39.5 (Apple Git-154)

