[PATCH 7/7] wifi: rtw89: add EVM for antenna diversity

[Ping-Ke Shih <pkshih@xxxxxxxxxxx>]

From: Eric Huang <echuang@xxxxxxxxxxx>

Take EVM into consideration when doing antenna diversity, and the priority
is higher than RSSI. Since EVM is more relevant to performance than RSSI,
especially in OTA environment.

Signed-off-by: Eric Huang <echuang@xxxxxxxxxxx>
Signed-off-by: Ping-Ke Shih <pkshih@xxxxxxxxxxx>
---
 drivers/net/wireless/realtek/rtw89/core.h |  1 +
 drivers/net/wireless/realtek/rtw89/phy.c  | 17 ++++++++++++++++-
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index cc84b5512e28e..02dc44cab31c7 100644
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -3131,6 +3131,7 @@ struct rtw89_antdiv_stats {
 	u16 pkt_cnt_cck;
 	u16 pkt_cnt_ofdm;
 	u16 pkt_cnt_non_legacy;
+	u32 evm;
 };
 
 struct rtw89_antdiv_info {
diff --git a/drivers/net/wireless/realtek/rtw89/phy.c b/drivers/net/wireless/realtek/rtw89/phy.c
index b9a3ebc2e7901..5eac00cd51881 100644
--- a/drivers/net/wireless/realtek/rtw89/phy.c
+++ b/drivers/net/wireless/realtek/rtw89/phy.c
@@ -2956,6 +2956,7 @@ void rtw89_phy_antdiv_sts_instance_reset(struct rtw89_antdiv_stats *antdiv_sts)
 	antdiv_sts->pkt_cnt_cck = 0;
 	antdiv_sts->pkt_cnt_ofdm = 0;
 	antdiv_sts->pkt_cnt_non_legacy = 0;
+	antdiv_sts->evm = 0;
 }
 
 static void rtw89_phy_antdiv_sts_instance_add(struct rtw89_dev *rtwdev,
@@ -2969,10 +2970,12 @@ static void rtw89_phy_antdiv_sts_instance_add(struct rtw89_dev *rtwdev,
 		} else {
 			ewma_rssi_add(&stats->ofdm_rssi_avg, phy_ppdu->rssi_avg);
 			stats->pkt_cnt_ofdm++;
+			stats->evm += phy_ppdu->ofdm.evm_min;
 		}
 	} else {
 		ewma_rssi_add(&stats->non_legacy_rssi_avg, phy_ppdu->rssi_avg);
 		stats->pkt_cnt_non_legacy++;
+		stats->evm += phy_ppdu->ofdm.evm_min;
 	}
 }
 
@@ -2988,6 +2991,11 @@ static u8 rtw89_phy_antdiv_sts_instance_get_rssi(struct rtw89_antdiv_stats *stat
 		return ewma_rssi_read(&stats->cck_rssi_avg);
 }
 
+static u8 rtw89_phy_antdiv_sts_instance_get_evm(struct rtw89_antdiv_stats *stats)
+{
+	return phy_div(stats->evm, stats->pkt_cnt_non_legacy + stats->pkt_cnt_ofdm);
+}
+
 void rtw89_phy_antdiv_parse(struct rtw89_dev *rtwdev,
 			    struct rtw89_rx_phy_ppdu *phy_ppdu)
 {
@@ -4270,15 +4278,22 @@ static void rtw89_phy_antdiv_decision_state(struct rtw89_dev *rtwdev)
 	struct rtw89_hal *hal = &rtwdev->hal;
 	bool no_change = false;
 	u8 main_rssi, aux_rssi;
+	u8 main_evm, aux_evm;
 	u32 candidate;
 
 	antdiv->get_stats = false;
 	antdiv->training_count = 0;
 
 	main_rssi = rtw89_phy_antdiv_sts_instance_get_rssi(&antdiv->main_stats);
+	main_evm = rtw89_phy_antdiv_sts_instance_get_evm(&antdiv->main_stats);
 	aux_rssi = rtw89_phy_antdiv_sts_instance_get_rssi(&antdiv->aux_stats);
+	aux_evm = rtw89_phy_antdiv_sts_instance_get_evm(&antdiv->aux_stats);
 
-	if (main_rssi > aux_rssi + RTW89_TX_DIV_RSSI_RAW_TH)
+	if (main_evm > aux_evm + ANTDIV_EVM_DIFF_TH)
+		candidate = RF_A;
+	else if (aux_evm > main_evm + ANTDIV_EVM_DIFF_TH)
+		candidate = RF_B;
+	else if (main_rssi > aux_rssi + RTW89_TX_DIV_RSSI_RAW_TH)
 		candidate = RF_A;
 	else if (aux_rssi > main_rssi + RTW89_TX_DIV_RSSI_RAW_TH)
 		candidate = RF_B;
-- 
2.25.1