[RFC v2 07/96] cl8k: add bf.c

[viktor.barna@celeno.com]

From: Viktor Barna <viktor.barna@celeno.com>

(Part of the split. Please, take a look at the cover letter for more
details).

Signed-off-by: Viktor Barna <viktor.barna@celeno.com>
---
 drivers/net/wireless/celeno/cl8k/bf.c | 346 ++++++++++++++++++++++++++
 1 file changed, 346 insertions(+)
 create mode 100644 drivers/net/wireless/celeno/cl8k/bf.c

diff --git a/drivers/net/wireless/celeno/cl8k/bf.c b/drivers/net/wireless/celeno/cl8k/bf.c
new file mode 100644
index 000000000000..49d16e13e6e4
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/bf.c
@@ -0,0 +1,346 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-2-Clause
+/* Copyright(c) 2019-2022, Celeno Communications Ltd. */
+
+#include "hw.h"
+#include "traffic.h"
+#include "sta.h"
+#include "sounding.h"
+#include "debug.h"
+#include "bf.h"
+
+#define CL_BF_MIN_SOUNDING_NR 3
+
+#define bf_pr(cl_hw, level, ...) \
+	do { \
+		if ((level) <= (cl_hw)->bf_db.dbg_level) \
+			pr_debug("[BF]" __VA_ARGS__); \
+	} while (0)
+
+#define bf_pr_verbose(cl_hw, ...) bf_pr((cl_hw), DBG_LVL_VERBOSE, ##__VA_ARGS__)
+#define bf_pr_err(cl_hw, ...)     bf_pr((cl_hw), DBG_LVL_ERROR, ##__VA_ARGS__)
+#define bf_pr_warn(cl_hw, ...)    bf_pr((cl_hw), DBG_LVL_WARNING, ##__VA_ARGS__)
+#define bf_pr_trace(cl_hw, ...)   bf_pr((cl_hw), DBG_LVL_TRACE, ##__VA_ARGS__)
+#define bf_pr_info(cl_hw, ...)    bf_pr((cl_hw), DBG_LVL_INFO, ##__VA_ARGS__)
+
+static bool cl_bf_is_beamformee_capable_he(struct ieee80211_sta *sta, bool mu_cap)
+{
+	u8 phy_cap_info4 = sta->he_cap.he_cap_elem.phy_cap_info[4];
+
+	if (mu_cap)
+		return (phy_cap_info4 & IEEE80211_HE_PHY_CAP4_MU_BEAMFORMER);
+	else
+		return (phy_cap_info4 & IEEE80211_HE_PHY_CAP4_SU_BEAMFORMEE);
+}
+
+static bool cl_bf_is_beamformee_capable_vht(struct ieee80211_sta *sta, bool mu_cap)
+{
+	u32 vht_cap = sta->vht_cap.cap;
+
+	if (mu_cap)
+		return (vht_cap & IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE);
+	else
+		return (vht_cap & IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE);
+}
+
+static bool cl_bf_is_beamformee_capable(struct cl_sta *cl_sta, bool mu_cap)
+{
+	struct ieee80211_sta *sta = cl_sta->sta;
+
+	if (sta->he_cap.has_he)
+		return cl_bf_is_beamformee_capable_he(sta, mu_cap);
+
+	if (sta->vht_cap.vht_supported)
+		return cl_bf_is_beamformee_capable_vht(sta, mu_cap);
+
+	return false;
+}
+
+void cl_bf_enable(struct cl_hw *cl_hw, bool enable, bool trigger_decision)
+{
+	struct cl_tcv_conf *conf = cl_hw->conf;
+
+	if (cl_hw->bf_db.enable == enable)
+		return;
+
+	if (!conf->ci_bf_en && enable) {
+		bf_pr_err(cl_hw, "Unable to enable - BF is globally disabled\n");
+		return;
+	}
+
+	cl_hw->bf_db.enable = enable;
+	bf_pr_verbose(cl_hw, "%s\n", enable ? "Enable" : "Disable");
+
+	if (trigger_decision)
+		cl_sta_loop_bh(cl_hw, cl_bf_sounding_decision);
+}
+
+static void cl_bf_timer_callback(struct timer_list *t)
+{
+	/*
+	 * If timer expired it means that we started sounding but didn't get any
+	 * indication for (10 * sounding_interval).
+	 * So we disable sounding for this station (even when in starts again traffic).
+	 */
+	struct cl_bf_sta_db *bf_db = from_timer(bf_db, t, timer);
+	struct cl_sta *cl_sta = container_of(bf_db, struct cl_sta, bf_db);
+	struct cl_hw *cl_hw = cl_sta->cl_vif->cl_hw;
+
+	bf_pr_trace(cl_hw, "Failed to get reply (%u)\n", cl_sta->sta_idx);
+	bf_db->indication_timeout = true;
+	cl_bf_sounding_decision(cl_hw, cl_sta);
+}
+
+static void cl_bf_reset_sounding_info(struct cl_sta *cl_sta)
+{
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	bf_db->synced = false;
+	bf_db->sounding_start = false;
+	bf_db->sounding_indications = 0;
+}
+
+void cl_bf_sounding_start(struct cl_hw *cl_hw, enum sounding_type type, struct cl_sta **cl_sta_arr,
+			  u8 sta_num, struct cl_sounding_info *recovery_elem)
+{
+#define STA_INDICES_STR_SIZE 64
+
+	/* Send request to start sounding */
+	u8 i, bw = CHNL_BW_MAX;
+	char sta_indices_str[STA_INDICES_STR_SIZE] = {0};
+	u8 str_len = 0;
+
+	for (i = 0; i < sta_num; i++) {
+		struct cl_sta *cl_sta = cl_sta_arr[i];
+		struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+		bw = cl_sta->wrs_sta.assoc_bw;
+		bf_db->synced = false;
+		bf_db->sounding_start = true;
+		bf_db->sounding_indications = 0;
+
+		str_len += snprintf(sta_indices_str, STA_INDICES_STR_SIZE - str_len, "%u%s",
+				    cl_sta->sta_idx, (i == sta_num - 1 ? ", " : ""));
+	}
+
+	bf_pr_trace(cl_hw, "Start sounding: Sta = %s\n", sta_indices_str);
+	cl_sounding_send_request(cl_hw, cl_sta_arr, sta_num, SOUNDING_ENABLE, type, bw, NULL, 0,
+				 recovery_elem);
+
+#undef STA_INDICES_STR_SIZE
+}
+
+void cl_bf_sounding_stop(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	if (bf_db->sounding_start) {
+		/* Send request to stop sounding */
+		cl_bf_reset_sounding_info(cl_sta);
+		bf_pr_trace(cl_hw, "Sta = %u, Stop sounding\n", cl_sta->sta_idx);
+		cl_sounding_send_request(cl_hw, &cl_sta, 1, SOUNDING_DISABLE, SOUNDING_TYPE_HE_SU,
+					 0, NULL, 0, NULL);
+		bf_pr_trace(cl_hw, "Sta: %u, Beamforming disabled\n", cl_sta->sta_idx);
+	}
+}
+
+void cl_bf_sounding_decision(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	if (cl_bf_is_enabled(cl_hw) &&
+	    cl_bf_is_beamformee_capable(cl_sta, false) &&
+	    !bf_db->indication_timeout &&
+	    ((bf_db->beamformee_sts + 1) >= CL_BF_MIN_SOUNDING_NR) &&
+	    (bf_db->traffic_active || cl_hw->bf_db.force)) {
+		if (!bf_db->sounding_start) {
+			if (cl_sta->su_sid == INVALID_SID)
+				cl_bf_sounding_start(cl_hw, SOUNDING_TYPE_HE_SU, &cl_sta, 1, NULL);
+			else
+				bf_pr_verbose(cl_hw, "[%s]: STA %u already belongs to sid %u\n",
+					      __func__, cl_sta->sta_idx, cl_sta->su_sid);
+		}
+	} else {
+		del_timer(&bf_db->timer);
+		cl_bf_sounding_stop(cl_hw, cl_sta);
+	}
+}
+
+static u8 cl_bf_get_sts_he(struct ieee80211_sta *sta)
+{
+	u8 *phy_cap_info = sta->he_cap.he_cap_elem.phy_cap_info;
+
+	if (phy_cap_info[0] & IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_160MHZ_IN_5G ||
+	    phy_cap_info[0] & IEEE80211_HE_PHY_CAP0_CHANNEL_WIDTH_SET_80PLUS80_MHZ_IN_5G)
+		return u8_get_bits(phy_cap_info[4],
+				   IEEE80211_HE_PHY_CAP4_BEAMFORMEE_MAX_STS_ABOVE_80MHZ_MASK);
+	else
+		return u8_get_bits(phy_cap_info[4],
+				   IEEE80211_HE_PHY_CAP4_BEAMFORMEE_MAX_STS_UNDER_80MHZ_MASK);
+}
+
+static u8 cl_bf_get_sts_vht(struct ieee80211_sta *sta)
+{
+	struct ieee80211_sta_vht_cap *vht_cap = &sta->vht_cap;
+
+	return u32_get_bits(vht_cap->cap, IEEE80211_VHT_CAP_BEAMFORMEE_STS_MASK);
+}
+
+static u8 cl_bf_get_sts(struct ieee80211_sta *sta)
+{
+	if (sta->he_cap.has_he)
+		return cl_bf_get_sts_he(sta);
+
+	return cl_bf_get_sts_vht(sta);
+}
+
+void cl_bf_update_rate(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	/* Old & new BF state for main rate */
+	bool bf_on_old = bf_db->is_on;
+	bool bf_on_new = cl_bf_is_on(cl_hw, cl_sta, bf_db->num_ss);
+
+	/* Old & new BF state for fallback rate */
+	bool bf_on_old_fbk = bf_db->is_on_fallback;
+	bool bf_on_new_fbk = cl_bf_is_on(cl_hw, cl_sta, bf_db->num_ss_fallback);
+
+	if (bf_on_old != bf_on_new || bf_on_old_fbk != bf_on_new_fbk) {
+		/* BF state for main rate or fallback rate changed */
+
+		/* Save the new state */
+		bf_db->is_on = bf_on_new;
+		bf_db->is_on_fallback = bf_on_new_fbk;
+
+		/* Update the firmware */
+		if (cl_msg_tx_set_tx_bf(cl_hw, cl_sta->sta_idx, bf_on_new, bf_on_new_fbk))
+			pr_err("%s: failed to set TX-BF\n", __func__);
+	}
+}
+
+void cl_bf_sta_add(struct cl_hw *cl_hw, struct cl_sta *cl_sta, struct ieee80211_sta *sta)
+{
+	/* Beamformee capabilities */
+	bool su_beamformee_capable = cl_bf_is_beamformee_capable(cl_sta, false);
+	bool mu_beamformee_capable = cl_bf_is_beamformee_capable(cl_sta, true);
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	WARN_ON_ONCE(sta->rx_nss == 0);
+	bf_db->beamformee_sts = cl_bf_get_sts(sta);
+	bf_db->nc = min_t(u8, sta->rx_nss, WRS_SS_MAX) - 1;
+	cl_sta->su_sid = INVALID_SID;
+
+	bf_pr_trace(cl_hw,
+		    "sta_idx: %u, su_beamformee_capable: %u, mu_beamformee_capable: %u, "
+		    "beamformee_sts: %u, nc = %u\n",
+		    cl_sta->sta_idx, su_beamformee_capable, mu_beamformee_capable,
+		    bf_db->beamformee_sts, bf_db->nc);
+
+	if (bf_db->beamformee_sts == 0)
+		bf_db->beamformee_sts = 3;
+
+	/*
+	 * Init the BF timer
+	 * Period is set to 0. It will be updated before enabling it.
+	 */
+	timer_setup(&bf_db->timer, cl_bf_timer_callback, 0);
+}
+
+void cl_bf_sta_remove(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	/* Disable timer before removing the station */
+	del_timer_sync(&bf_db->timer);
+
+	/*
+	 * Remove the sounding sequence associated with the STA and possibly start another sequence
+	 * for other stations that participate in the same sounding sequence with the STA
+	 */
+	if (cl_sta->su_sid != INVALID_SID) {
+		bf_db->sounding_remove_required = true;
+		cl_sounding_stop_by_sid(cl_hw, cl_sta->su_sid, true);
+	}
+}
+
+void cl_bf_sta_active(struct cl_hw *cl_hw, struct cl_sta *cl_sta, bool active)
+{
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	if (bf_db->traffic_active != active) {
+		bf_pr_trace(cl_hw, "Sta: %u, Active: %s\n",
+			    cl_sta->sta_idx, active ? "True" : " False");
+
+		bf_db->traffic_active = active;
+		cl_bf_sounding_decision(cl_hw, cl_sta);
+	}
+}
+
+void cl_bf_reset_sounding_ind(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+	cl_sta->bf_db.sounding_indications = 0;
+}
+
+bool cl_bf_is_enabled(struct cl_hw *cl_hw)
+{
+	return cl_hw->bf_db.enable;
+}
+
+bool cl_bf_is_on(struct cl_hw *cl_hw, struct cl_sta *cl_sta, u8 nss)
+{
+	struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+	return (cl_bf_is_enabled(cl_hw) &&
+		bf_db->sounding_start &&
+		bf_db->sounding_indications &&
+		(nss <= min(cl_hw->conf->ci_bf_max_nss, bf_db->nc)));
+}
+
+void cl_bf_sounding_req_success(struct cl_hw *cl_hw, struct cl_sounding_info *new_elem)
+{
+	/*
+	 * Start a timer to check that we are receiving indications from the station.
+	 * The period of the timer is set to 10 times the sounding-interval.
+	 */
+	u8 i;
+	struct cl_sta *cl_sta;
+	struct cl_bf_sta_db *bf_db;
+	unsigned long period = CL_SOUNDING_FACTOR * cl_sounding_get_interval(cl_hw);
+
+	for (i = 0; i < new_elem->sta_num; i++) {
+		cl_sta = new_elem->su_cl_sta_arr[i];
+		bf_db = &cl_sta->bf_db;
+
+		if (cl_sta) {
+			cl_sta->bf_db.sounding_start = true;
+			cl_sta->su_sid = new_elem->sounding_id;
+
+			/* Don't enable BF timer in case of force mode */
+			if (!cl_hw->bf_db.force)
+				mod_timer(&bf_db->timer, jiffies + msecs_to_jiffies(period));
+		}
+	}
+}
+
+void cl_bf_sounding_req_failure(struct cl_hw *cl_hw, struct cl_sounding_info *new_elem)
+{
+	u8 i;
+	struct cl_sta *cl_sta;
+	struct cl_bf_sta_db *bf_db;
+
+	for (i = 0; i < new_elem->sta_num; i++) {
+		cl_sta = new_elem->su_cl_sta_arr[i];
+
+		if (cl_sta) {
+			bf_db = &cl_sta->bf_db;
+			bf_db->sounding_start = false;
+			bf_db->sounding_indications = 0;
+		}
+	}
+}
+
+void cl_bf_init(struct cl_hw *cl_hw)
+{
+	cl_bf_enable(cl_hw, cl_hw->conf->ci_bf_en, false);
+}
+
-- 
2.36.1