new file mode 100644
@@ -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);
+}
+