new file mode 100644
@@ -0,0 +1,1121 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-2-Clause
+/* Copyright(c) 2019-2022, Celeno Communications Ltd. */
+
+#include "debug.h"
+#include "bf.h"
+#include "chip.h"
+#include "utils.h"
+#include "recovery.h"
+#include "debug.h"
+#include "hw.h"
+#include "sounding.h"
+
+#define DBG_PREFIX_MAX_LENGTH 64
+#define sounding_pr(level, format, ...) \
+ do { \
+ if ((level) <= cl_hw->sounding.dbg_level) { \
+ char __dbg_prefix[DBG_PREFIX_MAX_LENGTH] = {0}; \
+ if ((level) >= DBG_LVL_TRACE) \
+ snprintf(__dbg_prefix, DBG_PREFIX_MAX_LENGTH, "[%s][%d]", \
+ __func__, __LINE__); \
+ pr_debug("%s [Sounding] " format, __dbg_prefix, ##__VA_ARGS__); \
+ } \
+ } while (0)
+
+#define sounding_pr_verbose(...) sounding_pr(DBG_LVL_VERBOSE, ##__VA_ARGS__)
+#define sounding_pr_err(...) sounding_pr(DBG_LVL_ERROR, ##__VA_ARGS__)
+#define sounding_pr_warn(...) sounding_pr(DBG_LVL_WARNING, ##__VA_ARGS__)
+#define sounding_pr_trace(...) sounding_pr(DBG_LVL_TRACE, ##__VA_ARGS__)
+#define sounding_pr_info(...) sounding_pr(DBG_LVL_INFO, ##__VA_ARGS__)
+
+#define CL_SOUNDING_ALL_STA 0xff
+#define CL_SOUNDING_LIFETIME_MAX 4095
+#define CL_SOUNDING_LIFETIME_FACTOR 5
+#define CL_SOUNDING_V_MATRIX_PADDING 32
+#define CL_V_MATRIX_MAC_OVERHEAD 41
+#define CL_Q_MATRIX_BITMAP_MASK 0xf
+
+enum cl_sounding_feedback_type {
+ CL_SOUNDING_FEEDBACK_TYPE_SU = 0,
+ CL_SOUNDING_FEEDBACK_TYPE_MU,
+};
+
+enum cl_sounding_ng {
+ CL_SOUNDING_NG_4 = 0,
+ CL_SOUNDING_NG_16,
+ CL_SOUNDING_NG_MAX
+};
+
+struct sounding_work_data {
+ struct work_struct work;
+ struct cl_hw *cl_hw;
+ bool start;
+ bool is_recovery;
+ struct cl_sounding_info *elem; /* For stop and recovery cases */
+ enum sounding_type sounding_type;
+ u8 gid;
+ u8 sta_num;
+ u8 bw;
+ u8 q_matrix_bitmap;
+ u8 sta_indices[CL_MU_MAX_STA_PER_GROUP];
+};
+
+static u16 ng_bw_to_nsc[CL_SOUNDING_NG_MAX][CHNL_BW_MAX_HE] = {
+ {64, 128, 256, 512},
+ {32, 32, 64, 128}
+};
+
+static int cl_sounding_check_response(struct cl_hw *cl_hw, u8 param_err)
+{
+ int ret = -1;
+
+ switch (param_err) {
+ case CL_SOUNDING_RSP_OK:
+ sounding_pr_trace("param OK!\n");
+ ret = 0;
+ break;
+ case CL_SOUNDING_RSP_ERR_RLIMIT:
+ sounding_pr_err("error, resource limit reached\n");
+ break;
+ case CL_SOUNDING_RSP_ERR_BW:
+ sounding_pr_err("error, unsupported BW tx requested\n");
+ break;
+ case CL_SOUNDING_RSP_ERR_NSS:
+ sounding_pr_err("error, unsupported ndp NSS tx requested\n");
+ break;
+ case CL_SOUNDING_RSP_ERR_INTERVAL:
+ sounding_pr_err("error, interval value is invalid\n");
+ break;
+ case CL_SOUNDING_RSP_ERR_ALREADY:
+ sounding_pr_err("error, station already associated/disassociated with sounding\n");
+ break;
+ case CL_SOUNDING_RSP_ERR_STA:
+ sounding_pr_err("error, station is inactive/active\n");
+ break;
+ case CL_SOUNDING_RSP_ERR_TYPE:
+ sounding_pr_err("error, invalid sounding type\n");
+ break;
+ default:
+ sounding_pr_err("error status unknown, BUG\n");
+ break;
+ }
+
+ return ret;
+}
+
+static u32 cl_sounding_get_lifetime(struct cl_hw *cl_hw, u32 interval)
+{
+ u32 lifetime = (interval * CL_SOUNDING_LIFETIME_FACTOR) >> 1;
+
+ if (lifetime > CL_SOUNDING_LIFETIME_MAX) {
+ sounding_pr_err("lifetime (%u) exceeds 4095\n", lifetime);
+ lifetime = CL_SOUNDING_LIFETIME_MAX;
+ }
+
+ return lifetime;
+}
+
+static bool cl_sounding_is_sta_ng_16_capable(struct cl_hw *cl_hw, struct cl_sta *cl_sta,
+ bool mu_cap)
+{
+ struct ieee80211_sta_he_cap *he_cap = &cl_sta->sta->he_cap;
+
+ if (he_cap->has_he) {
+ if (mu_cap)
+ return (he_cap->he_cap_elem.phy_cap_info[5] &
+ IEEE80211_HE_PHY_CAP5_NG16_MU_FEEDBACK);
+ else
+ return (he_cap->he_cap_elem.phy_cap_info[5] &
+ IEEE80211_HE_PHY_CAP5_NG16_SU_FEEDBACK);
+ }
+
+ return false;
+}
+
+static bool cl_sounding_is_sta_codebook_size_75_capable(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ struct ieee80211_sta_he_cap *he_cap = &cl_sta->sta->he_cap;
+
+ if (he_cap->has_he)
+ return (he_cap->he_cap_elem.phy_cap_info[6] &
+ IEEE80211_HE_PHY_CAP6_CODEBOOK_SIZE_75_MU);
+
+ return false;
+}
+
+static void cl_sounding_extract_ng_cb_size(struct cl_hw *cl_hw, u8 fb_type_ng_cb_size,
+ enum cl_sounding_ng *ng, u8 *phi_psi_sum)
+{
+ enum cl_sounding_feedback_type fb_type =
+ SOUNDING_FEEDBACK_TYPE_VAL(fb_type_ng_cb_size);
+ u8 cb_size = SOUNDING_CODEBOOK_SIZE_VAL(fb_type_ng_cb_size);
+
+ *ng = SOUNDING_NG_VAL(fb_type_ng_cb_size);
+
+ switch (fb_type) {
+ case CL_SOUNDING_FEEDBACK_TYPE_SU:
+ *phi_psi_sum = (cb_size ? 10 : 6);
+ break;
+ case CL_SOUNDING_FEEDBACK_TYPE_MU:
+ *phi_psi_sum = (cb_size ? 16 : 12);
+ break;
+ default:
+ sounding_pr_err("Invalid feedback_type %d\n", fb_type);
+ break;
+ }
+}
+
+static u32 cl_sounding_get_v_matrix_size(struct cl_hw *cl_hw, u8 sta_idx, u8 bw, u8 nc, u8 nr,
+ u8 fb_type_ng_cb_size)
+{
+ enum cl_sounding_ng ng = 0;
+ u8 phi_psi_sum = 0;
+ u8 nsc;
+ u32 v_size;
+
+ cl_sounding_extract_ng_cb_size(cl_hw, fb_type_ng_cb_size, &ng, &phi_psi_sum);
+ nsc = ng_bw_to_nsc[ng][bw];
+
+ /* NC and NR should start from 1 and not 0 for the below calculation */
+ nc++;
+ nr++;
+
+ /* v_size = [8*41 + 8*nc + (phi + psi)/2 * nsc*(nc * (2*nr-nc-1))] / 8 + extra padding */
+ v_size = CL_V_MATRIX_MAC_OVERHEAD + nc +
+ ((phi_psi_sum * nsc * nc * (2 * nr - nc - 1)) >> 4) +
+ CL_SOUNDING_V_MATRIX_PADDING;
+
+ sounding_pr_info("sta %u, nc %u, nr %u, ng %d, phi_psi_sum %u, nsc %u, v_size %u\n",
+ sta_idx, nc, nr, ng, phi_psi_sum, nsc, v_size);
+
+ return v_size;
+}
+
+static u32 cl_sounding_get_v_matrices_data_size(struct cl_hw *cl_hw,
+ struct sounding_info_per_sta *info_per_sta,
+ u8 sta_num, u8 bw, u8 nr)
+{
+ u8 i;
+ u32 v_size = 0;
+
+ for (i = 0; i < sta_num; i++)
+ v_size += cl_sounding_get_v_matrix_size(cl_hw, info_per_sta[i].sta_idx,
+ bw, info_per_sta[i].nc,
+ nr, info_per_sta[i].fb_type_ng_cb_size);
+
+ sounding_pr_info("v_matrices data size %u, sta_num %u\n", v_size, sta_num);
+
+ return v_size;
+}
+
+static u32 cl_sounding_get_q_matrix_size(struct cl_hw *cl_hw,
+ const struct sounding_info_per_sta *info_per_sta,
+ u8 sta_num, u8 bw, u8 nr)
+{
+ u8 i;
+ u8 nc = 0;
+ enum cl_sounding_ng ng = 0;
+ u8 nsc, phi_psi_sum = 0;
+ u32 q_size = 0;
+
+ /*
+ * NC and NR should start from 1 and not 0 for the below calculation
+ * In MU-MIMO case, when sta_num > 1, we should take the sum of all nc's
+ */
+ for (i = 0; i < sta_num; i++)
+ nc += info_per_sta[i].nc + 1;
+
+ nr++;
+
+ cl_sounding_extract_ng_cb_size(cl_hw, info_per_sta[0].fb_type_ng_cb_size, &ng,
+ &phi_psi_sum);
+ nsc = ng_bw_to_nsc[ng][bw];
+ q_size = (nr * nc * nsc) << 2;
+
+ sounding_pr_info("q_matrix size %u, sta_num %u\n", q_size, sta_num);
+
+ return q_size;
+}
+
+static u32 cl_sounding_get_required_xmem_size(struct cl_hw *cl_hw,
+ const struct mm_sounding_req *sounding_req,
+ const struct sounding_info_per_sta *info_per_sta)
+{
+ u8 i;
+ u8 sta_num = sounding_req->sta_num;
+ u8 q_matrix_bitmap = sounding_req->q_matrix_bitmap;
+ u8 bw = sounding_req->req_txbw;
+ u8 nr = sounding_req->ndp_nsts;
+ u32 total_size = 0;
+
+ /*
+ * In case of MU sounding only one Q matrix is generated.
+ * Otherwise, the number of Q matrices equals to te number of stations
+ */
+ if (sta_num > 1 &&
+ sounding_req->sounding_type != SOUNDING_TYPE_HE_MU &&
+ sounding_req->sounding_type != SOUNDING_TYPE_VHT_MU)
+ for (i = 0; i < sta_num; i++)
+ total_size +=
+ cl_sounding_get_q_matrix_size(cl_hw, &info_per_sta[i], 1, bw, nr);
+ else
+ total_size =
+ cl_sounding_get_q_matrix_size(cl_hw, info_per_sta, sta_num, bw, nr);
+
+ /*
+ * If additional SU Q matrices should be generated - consider them also when calculating
+ * the required XMEM space
+ */
+ if (q_matrix_bitmap) {
+ for (i = 0; i < CL_MU_MIMO_MAX_STA_PER_GRP; i++)
+ if (q_matrix_bitmap & BIT(i))
+ total_size +=
+ cl_sounding_get_q_matrix_size(cl_hw, &info_per_sta[i], 1, bw, nr);
+ }
+
+ return total_size;
+}
+
+static bool cl_sounding_is_enough_xmem_space(struct cl_hw *cl_hw,
+ const struct mm_sounding_req *sounding_req,
+ const struct sounding_info_per_sta *info_per_sta,
+ u32 *required_size)
+{
+ struct cl_xmem *xmem_db = &cl_hw->chip->xmem_db;
+ u32 req_mem = cl_sounding_get_required_xmem_size(cl_hw, sounding_req, info_per_sta);
+
+ if (required_size)
+ *required_size = req_mem;
+
+ return ((xmem_db->size - xmem_db->total_used) >= req_mem);
+}
+
+static void cl_sounding_fill_info_per_sta(struct cl_hw *cl_hw, u8 sounding_type, u8 bw, u8 sta_num,
+ struct cl_sta **cl_sta_arr,
+ struct sounding_info_per_sta *info_per_sta,
+ u8 *n_sts)
+{
+ u8 i;
+ u8 min_sts = cl_hw->num_antennas;
+ struct cl_sta *cl_sta = NULL;
+ u8 mu_fb_type_ng_cb_size = FEEDBACK_TYPE_MU_NG_4_CODEBOOK_SIZE_9_7;
+ u8 curr_fb_type_ng_cb_size;
+ bool should_update_fb_type_ng_cb_size = false;
+
+ for (i = 0; i < sta_num; i++) {
+ cl_sta = cl_sta_arr[i];
+
+ if (!cl_sta)
+ continue;
+
+ info_per_sta[i].sta_idx = cl_sta->sta_idx;
+ info_per_sta[i].nc = cl_sta->bf_db.nc;
+
+ min_sts = min(min_sts, cl_sta->bf_db.beamformee_sts);
+
+ switch (sounding_type) {
+ case SOUNDING_TYPE_HE_CQI:
+ case SOUNDING_TYPE_HE_SU:
+ case SOUNDING_TYPE_VHT_SU:
+ case SOUNDING_TYPE_VHT_MU:
+ info_per_sta[i].fb_type_ng_cb_size =
+ FEEDBACK_TYPE_SU_NG_4_CODEBOOK_SIZE_4_2;
+ break;
+ case SOUNDING_TYPE_HE_SU_TB:
+ info_per_sta[i].fb_type_ng_cb_size =
+ FEEDBACK_TYPE_SU_NG_4_CODEBOOK_SIZE_6_4;
+ break;
+ case SOUNDING_TYPE_HE_CQI_TB:
+ info_per_sta[i].fb_type_ng_cb_size =
+ FEEDBACK_TYPE_CQI_TB;
+ break;
+ case SOUNDING_TYPE_HE_MU:
+ if (bw == CHNL_BW_160 &&
+ info_per_sta[i].nc >= WRS_SS_3 &&
+ min_sts == MAX_ANTENNAS) {
+ should_update_fb_type_ng_cb_size = true;
+
+ if (cl_sounding_is_sta_codebook_size_75_capable(cl_hw, cl_sta)) {
+ curr_fb_type_ng_cb_size =
+ FEEDBACK_TYPE_MU_NG_4_CODEBOOK_SIZE_7_5;
+ } else if (cl_sounding_is_sta_ng_16_capable(cl_hw, cl_sta, true)) {
+ curr_fb_type_ng_cb_size =
+ FEEDBACK_TYPE_MU_NG_16_CODEBOOK_SIZE_9_7;
+ } else {
+ curr_fb_type_ng_cb_size =
+ FEEDBACK_TYPE_MU_NG_4_CODEBOOK_SIZE_9_7;
+ mu_fb_type_ng_cb_size =
+ FEEDBACK_TYPE_MU_NG_4_CODEBOOK_SIZE_9_7;
+ min_sts--;
+ }
+
+ if ((SOUNDING_NG_VAL(curr_fb_type_ng_cb_size) >
+ SOUNDING_NG_VAL(mu_fb_type_ng_cb_size)) ||
+ (SOUNDING_CODEBOOK_SIZE_VAL(curr_fb_type_ng_cb_size) <
+ SOUNDING_CODEBOOK_SIZE_VAL(mu_fb_type_ng_cb_size)))
+ mu_fb_type_ng_cb_size = curr_fb_type_ng_cb_size;
+ }
+
+ info_per_sta[i].fb_type_ng_cb_size = mu_fb_type_ng_cb_size;
+ break;
+ default:
+ sounding_pr_trace("Invalid sounding type %u\n", sounding_type);
+ break;
+ }
+ }
+
+ *n_sts = min_sts;
+
+ if (should_update_fb_type_ng_cb_size)
+ for (i = 0; i < sta_num; i++)
+ info_per_sta[i].fb_type_ng_cb_size = mu_fb_type_ng_cb_size;
+}
+
+static struct cl_sounding_info *cl_sounding_elem_alloc(struct cl_hw *cl_hw, u32 v_mat_len)
+{
+ struct cl_sounding_info *elem = NULL;
+ dma_addr_t phys_dma_addr;
+ struct v_matrix_header *buf = NULL;
+
+ elem = kzalloc(sizeof(*elem), GFP_KERNEL);
+
+ if (!elem) {
+ CL_DBG(cl_hw, DBG_LVL_ERROR, "kzalloc failed\n");
+ return NULL;
+ }
+
+ buf = dma_alloc_coherent(cl_hw->chip->dev, v_mat_len, &phys_dma_addr, GFP_KERNEL);
+
+ if (!buf) {
+ CL_DBG(cl_hw, DBG_LVL_ERROR, "dma_alloc_coherent failed. size=%u\n", v_mat_len);
+ kfree(elem);
+ return NULL;
+ }
+
+ elem->v_matrices_data = buf;
+ elem->v_matrices_dma_addr = phys_dma_addr;
+ elem->v_matrices_data_len = v_mat_len;
+
+ return elem;
+}
+
+static void cl_sounding_elem_free(struct cl_hw *cl_hw, struct cl_sounding_info *elem)
+{
+ struct v_matrix_header *v_data = elem->v_matrices_data;
+
+ if (v_data) {
+ dma_free_coherent(cl_hw->chip->dev, elem->v_matrices_data_len, (void *)v_data,
+ elem->v_matrices_dma_addr);
+ } else {
+ sounding_pr_err("%s: v_matrices_data is NULL for sid %u\n",
+ __func__, elem->sounding_id);
+ }
+
+ elem->v_matrices_data = NULL;
+ kfree(elem);
+}
+
+static void cl_sounding_req_success(struct cl_hw *cl_hw, struct cl_sounding_info *elem)
+{
+ cl_bf_sounding_req_success(cl_hw, elem);
+}
+
+static void cl_sounding_req_failure(struct cl_hw *cl_hw, struct cl_sounding_info *elem)
+{
+ cl_bf_sounding_req_failure(cl_hw, elem);
+}
+
+static void cl_sounding_increase_num_profiles(struct cl_hw *cl_hw, u8 sounding_type, u8 sta_num)
+{
+ if (SOUNDING_TYPE_IS_CQI(sounding_type))
+ cl_hw->sounding.cqi_profiles += sta_num;
+ else
+ cl_hw->sounding.active_profiles += sta_num;
+}
+
+static void cl_sounding_decrease_num_profiles(struct cl_hw *cl_hw, u8 sounding_type, u8 sta_num)
+{
+ if (SOUNDING_TYPE_IS_CQI(sounding_type))
+ cl_hw->sounding.cqi_profiles -= sta_num;
+ else
+ cl_hw->sounding.active_profiles -= sta_num;
+}
+
+static void _cl_sounding_add(struct cl_hw *cl_hw, struct cl_sounding_info *elem, u8 sounding_id,
+ u32 req_xmem)
+{
+ write_lock_bh(&cl_hw->sounding.list_lock);
+ elem->sounding_id = sounding_id;
+ elem->xmem_space = req_xmem;
+ cl_hw->chip->xmem_db.total_used += req_xmem;
+ cl_hw->sounding.num_soundings++;
+ list_add_tail(&elem->list, &cl_hw->sounding.head);
+ cl_sounding_increase_num_profiles(cl_hw, elem->type, elem->sta_num);
+ write_unlock_bh(&cl_hw->sounding.list_lock);
+}
+
+static void cl_sounding_remove_from_list(struct cl_hw *cl_hw, struct cl_sounding_info *elem)
+{
+ /* Remove the sounding sequence from the list and update the XMEM and profile counters */
+ write_lock_bh(&cl_hw->sounding.list_lock);
+ list_del(&elem->list);
+ cl_hw->chip->xmem_db.total_used -= elem->xmem_space;
+ cl_hw->sounding.num_soundings--;
+ cl_sounding_decrease_num_profiles(cl_hw, elem->type, elem->sta_num);
+ write_unlock_bh(&cl_hw->sounding.list_lock);
+}
+
+static void cl_sounding_remove_recovery(struct cl_hw *cl_hw, struct cl_sounding_info *elem)
+{
+ u8 i;
+
+ cl_sounding_remove_from_list(cl_hw, elem);
+
+ {
+ /* Set invalid sid for all STAs related to this sounding sequence */
+ for (i = 0; i < elem->sta_num; i++) {
+ struct cl_sta *cl_sta = elem->su_cl_sta_arr[i];
+
+ if (cl_sta)
+ cl_sta->su_sid = INVALID_SID;
+ }
+ }
+
+ /* Free the deleted sounding element */
+ cl_sounding_elem_free(cl_hw, elem);
+}
+
+static void cl_sounding_start_handler(struct cl_hw *cl_hw, struct sounding_work_data *data)
+{
+ struct mm_sounding_req sounding_req;
+ struct mm_sounding_cfm *cfm = NULL;
+ int ret = 0;
+ u32 len = 0;
+ u32 req_xmem = 0;
+ struct cl_sounding_info *elem = NULL;
+ u8 sounding_type = data->sounding_type;
+ u8 bw = data->bw;
+ u8 i, sta_num = 0;
+ u8 q_matrix_bitmap = data->q_matrix_bitmap;
+ u8 min_nsts = 0;
+ struct cl_sta *cl_sta_arr[CL_MU_MAX_STA_PER_GROUP] = {NULL};
+
+ cl_sta_lock_bh(cl_hw);
+
+ for (i = 0; i < data->sta_num; i++) {
+ u8 sta_idx = data->sta_indices[i];
+ struct cl_sta *cl_sta;
+
+ cl_sta = cl_sta_get(cl_hw, sta_idx);
+
+ if (!cl_sta)
+ continue;
+
+ cl_sta_arr[sta_num] = cl_sta;
+ sta_num++;
+ }
+
+ if (!sta_num) {
+ cl_sta_unlock_bh(cl_hw);
+ sounding_pr_err("%s: No STA found!\n", __func__);
+ return;
+ }
+
+ q_matrix_bitmap &= CL_Q_MATRIX_BITMAP_MASK;
+
+ /* Configure sounding request parameters */
+ sounding_req.start = true;
+ sounding_req.sounding_type = sounding_type;
+ sounding_req.req_txbw = bw;
+ sounding_req.sta_num = sta_num;
+ sounding_req.interval = cl_sounding_get_interval(cl_hw);
+ sounding_req.lifetime = cl_sounding_get_lifetime(cl_hw, sounding_req.interval);
+ sounding_req.q_matrix_bitmap = q_matrix_bitmap;
+ cl_sounding_fill_info_per_sta(cl_hw, sounding_type, bw, sta_num, cl_sta_arr,
+ sounding_req.info_per_sta, &min_nsts);
+ cl_sta_unlock_bh(cl_hw);
+
+ sounding_req.ndp_nsts = min_nsts;
+
+ if (data->is_recovery) {
+ elem = data->elem;
+ } else {
+ /*
+ * Check if there is enough XMEM space.
+ * Should be called after filling sounding req struct
+ */
+ if (!cl_sounding_is_enough_xmem_space(cl_hw, &sounding_req,
+ sounding_req.info_per_sta, &req_xmem)) {
+ sounding_pr_err("There is not enough space in XMEM!\n");
+ return;
+ }
+
+ /* Should be called after filling info per STA */
+ len = cl_sounding_get_v_matrices_data_size(cl_hw, sounding_req.info_per_sta,
+ sta_num, bw, min_nsts);
+ elem = cl_sounding_elem_alloc(cl_hw, len);
+
+ if (!elem)
+ return;
+
+ elem->type = sounding_type;
+ elem->bw = bw;
+ elem->sta_num = sta_num;
+ elem->q_matrix_bitmap = q_matrix_bitmap;
+
+ if (data->gid)
+ elem->gid = data->gid;
+ else
+ memcpy(elem->su_cl_sta_arr, cl_sta_arr,
+ sta_num * sizeof(cl_sta_arr[0]));
+ }
+
+ sounding_req.host_address = cpu_to_le32(elem->v_matrices_dma_addr);
+
+ /* Print request parameters */
+ sounding_pr_trace("Request: start=%u, bfr_lifetime=%u, interval=%u, "
+ "req_txbw=%u, ndp_nsts=%u, sounding_type=%u\n",
+ sounding_req.start,
+ sounding_req.lifetime,
+ sounding_req.interval,
+ sounding_req.req_txbw,
+ sounding_req.ndp_nsts,
+ sounding_req.sounding_type);
+
+ /* Send message to firmware */
+ ret = cl_msg_tx_sounding(cl_hw, &sounding_req);
+
+ /* Check firmware response */
+ cfm = cl_hw->msg_cfm_params[MM_SOUNDING_CFM];
+ if (ret == 0 && cfm) {
+ ret = cl_sounding_check_response(cl_hw, cfm->param_err);
+
+ if (ret == 0) {
+ if (!data->is_recovery)
+ _cl_sounding_add(cl_hw, elem, cfm->sounding_id, req_xmem);
+
+ cl_sounding_req_success(cl_hw, elem);
+
+ sounding_pr_trace("Sounding %u was enabled successfully\n",
+ cfm->sounding_id);
+ } else {
+ cl_sounding_req_failure(cl_hw, elem);
+
+ if (data->is_recovery)
+ cl_sounding_remove_recovery(cl_hw, elem);
+ }
+ } else {
+ sounding_pr_err("%s: failed to send message (%d)\n", __func__, ret);
+ cl_sounding_req_failure(cl_hw, elem);
+
+ if (data->is_recovery)
+ cl_sounding_remove_recovery(cl_hw, elem);
+ else
+ cl_sounding_elem_free(cl_hw, elem);
+ }
+
+ /* Free message confirmation */
+ cl_msg_tx_free_cfm_params(cl_hw, MM_SOUNDING_CFM);
+}
+
+static void _cl_sounding_remove(struct cl_hw *cl_hw, struct cl_sounding_info *elem)
+{
+ u8 i;
+ struct sounding_work_data data = {
+ .cl_hw = cl_hw,
+ .bw = elem->bw,
+ .start = true,
+ .is_recovery = false,
+ .sta_num = 0
+ };
+
+ cl_sounding_remove_from_list(cl_hw, elem);
+
+ /* Update invalid sid for all STAs related to this sounding sequence.
+ * Also start sounding for STAs that didn't request to stop sounding.
+ */
+
+ for (i = 0; i < elem->sta_num; i++) {
+ struct cl_sta *cl_sta = elem->su_cl_sta_arr[i];
+
+ if (!cl_sta)
+ continue;
+
+ cl_sta->su_sid = INVALID_SID;
+
+ /* After stopping the multi STA sounding - check if a new sounding is needed */
+ if (elem->sounding_restart_required) {
+ if (!cl_sta->bf_db.sounding_remove_required) {
+ data.sta_indices[data.sta_num] = cl_sta->sta_idx;
+ data.sta_num++;
+ } else {
+ cl_sta->bf_db.sounding_remove_required = false;
+ }
+ }
+ }
+
+ /* Start a new sounding for the remaining stations only when needed */
+ if (data.sta_num) {
+ /* Determine new sounding type */
+ if (SOUNDING_TYPE_IS_CQI(elem->type)) {
+ if (data.sta_num > 1)
+ data.sounding_type = elem->type;
+ else
+ data.sounding_type = SOUNDING_TYPE_HE_CQI;
+ } else if (SOUNDING_TYPE_IS_VHT(elem->type)) {
+ data.sounding_type = SOUNDING_TYPE_VHT_SU;
+ } else {
+ if (data.sta_num > 1)
+ data.sounding_type = SOUNDING_TYPE_HE_SU_TB;
+ else
+ data.sounding_type = SOUNDING_TYPE_HE_SU;
+ }
+
+ cl_sounding_start_handler(cl_hw, &data);
+ }
+
+ /* Free the deleted sounding element */
+ cl_sounding_elem_free(cl_hw, elem);
+}
+
+static void cl_sounding_stop_handler(struct cl_hw *cl_hw, struct cl_sounding_info *elem)
+{
+ struct mm_sounding_req sounding_req;
+ int ret = 0;
+
+ if (!elem) {
+ sounding_pr_err("elem is NULL!!\n");
+ return;
+ }
+
+ /* Configure sounding request parameters */
+ sounding_req.start = false;
+ sounding_req.sounding_type = elem->type;
+ sounding_req.sid = elem->sounding_id;
+
+ /* Print request parameters */
+ sounding_pr_trace("Delete request: sid=%u, sounding_type=%u\n",
+ elem->sounding_id, elem->type);
+
+ /* Send message to firmware */
+ ret = cl_msg_tx_sounding(cl_hw, &sounding_req);
+
+ /* Check firmware response */
+ if (ret)
+ sounding_pr_err("%s: failed to send message (%d)\n", __func__, ret);
+ else
+ /* Free message confirmation */
+ cl_msg_tx_free_cfm_params(cl_hw, MM_SOUNDING_CFM);
+
+ /* Remove the sounding sequence from the list and update the used XMEM counter.
+ * Notice that elem is freed and shouldn't be accessed after the call to this function.
+ */
+ _cl_sounding_remove(cl_hw, elem);
+}
+
+static void cl_sounding_handler_send_request(struct work_struct *work)
+{
+ struct sounding_work_data *data = (struct sounding_work_data *)work;
+ struct cl_hw *cl_hw = data->cl_hw;
+
+ if (data->start) {
+ cl_sounding_start_handler(cl_hw, data);
+ } else {
+ u8 sid;
+
+ {
+ u8 sta_idx = data->sta_indices[0];
+ struct cl_sta *cl_sta;
+
+ cl_sta_lock_bh(cl_hw);
+ cl_sta = cl_sta_get(cl_hw, sta_idx);
+ sid = cl_sta ? cl_sta->su_sid : U8_MAX;
+ cl_sta_unlock_bh(cl_hw);
+ }
+
+ if (data->elem)
+ cl_sounding_stop_handler(cl_hw, data->elem);
+ else
+ cl_sounding_stop_by_sid(cl_hw, sid, false);
+ }
+
+ kfree(data);
+}
+
+static u16 cl_sounding_calc_interval(struct cl_hw *cl_hw, u8 active_profiles)
+{
+ /* Sounding interval = min interval + [(active_profiles - 1) / STA step] * interval step */
+
+ u16 *coefs = cl_hw->conf->ce_sounding_interval_coefs;
+ u16 min_interval = coefs[SOUNDING_INTERVAL_COEF_MIN_INTERVAL];
+ u16 max_interval = coefs[SOUNDING_INTERVAL_COEF_MAX_INTERVAL];
+ u8 sta_step = coefs[SOUNDING_INTERVAL_COEF_STA_STEP];
+ u8 interval_step = coefs[SOUNDING_INTERVAL_COEF_INTERVAL_STEP];
+ u16 ret = min_interval;
+
+ if (active_profiles <= sta_step)
+ return ret;
+
+ active_profiles--;
+ ret += (active_profiles / sta_step) * interval_step;
+
+ return min(ret, max_interval);
+}
+
+static void cl_sounding_handler_change_interval(struct work_struct *work)
+{
+ struct sounding_work_data *data = (struct sounding_work_data *)work;
+ struct cl_hw *cl_hw = data->cl_hw;
+ struct mm_sounding_interval_cfm *sounding_interval_cfm = NULL;
+ int ret = 0;
+ /* Configure sounding request parameters */
+ u16 interval = cl_sounding_get_interval(cl_hw);
+ u16 lifetime = cl_sounding_get_lifetime(cl_hw, interval);
+
+ sounding_pr_trace("Sounding interval request: sta_idx=%d, interval=%u, "
+ "lifetime=%u, sounding_type=%u\n",
+ CL_SOUNDING_ALL_STA, interval, lifetime, data->sounding_type);
+
+ /* Start/Stop synchronize sounding request periodically */
+ ret = cl_msg_tx_sounding_interval(cl_hw, interval, lifetime, data->sounding_type,
+ CL_SOUNDING_ALL_STA);
+ sounding_interval_cfm = cl_hw->msg_cfm_params[MM_SOUNDING_INTERVAL_CFM];
+ if (ret == 0 && sounding_interval_cfm)
+ cl_sounding_check_response(cl_hw, sounding_interval_cfm->param_err);
+ else
+ sounding_pr_err("%s: failed to send message (%d)\n", __func__, ret);
+
+ cl_msg_tx_free_cfm_params(cl_hw, MM_SOUNDING_INTERVAL_CFM);
+ kfree(data);
+}
+
+static void cl_sounding_recovery_reset(struct cl_hw *cl_hw)
+{
+ struct cl_sounding_db *sounding_db = &cl_hw->sounding;
+
+ memset(sounding_db->active_profiles_prev, 0, sizeof(u8) * CL_SOUNDING_STABILITY_TIME);
+ sounding_db->active_profiles_idx = 0;
+ cl_sta_loop(cl_hw, cl_bf_reset_sounding_ind);
+}
+
+void cl_sounding_init(struct cl_hw *cl_hw)
+{
+ struct cl_sounding_db *sounding_db = &cl_hw->sounding;
+
+ memset(sounding_db, 0, sizeof(*sounding_db));
+ sounding_db->sounding_wq = create_workqueue("cl_sounding_wq");
+ sounding_db->current_interval =
+ cl_hw->conf->ce_sounding_interval_coefs[SOUNDING_INTERVAL_COEF_MIN_INTERVAL];
+ sounding_db->dbg_level = 1;
+ cl_hw->chip->xmem_db.size = XMEM_SIZE;
+ INIT_LIST_HEAD(&sounding_db->head);
+ rwlock_init(&sounding_db->list_lock);
+}
+
+void cl_sounding_close(struct cl_hw *cl_hw)
+{
+ struct cl_sounding_info *elem, *tmp;
+
+ if (cl_hw->sounding.sounding_wq)
+ destroy_workqueue(cl_hw->sounding.sounding_wq);
+
+ list_for_each_entry_safe(elem, tmp, &cl_hw->sounding.head, list) {
+ /* Don't try to start a new sounding sequence after stopping this one */
+ elem->sounding_restart_required = false;
+ cl_sounding_stop_handler(cl_hw, elem);
+ }
+}
+
+struct cl_sounding_info *cl_sounding_get_elem(struct cl_hw *cl_hw, u8 sounding_id)
+{
+ struct cl_sounding_info *elem = NULL;
+
+ read_lock_bh(&cl_hw->sounding.list_lock);
+
+ list_for_each_entry(elem, &cl_hw->sounding.head, list) {
+ if (elem->sounding_id == sounding_id) {
+ read_unlock_bh(&cl_hw->sounding.list_lock);
+ return elem;
+ }
+ }
+
+ read_unlock_bh(&cl_hw->sounding.list_lock);
+
+ return NULL;
+}
+
+void cl_sounding_send_request(struct cl_hw *cl_hw, struct cl_sta **cl_sta_arr,
+ u8 sta_num, bool enable, u8 sounding_type, u8 bw,
+ void *mu_grp,
+ u8 q_matrix_bitmap, struct cl_sounding_info *recovery_elem)
+{
+ struct sounding_work_data *data;
+ struct cl_sounding_info *elem = NULL;
+ u8 i;
+ struct cl_sta *cl_sta = NULL;
+ bool background = (preempt_count() != 0);
+
+ if (cl_band_is_24g(cl_hw) && SOUNDING_TYPE_IS_VHT(sounding_type)) {
+ sounding_pr_err("A VHT sounding type (%u) is not supported in 2.4g band\n",
+ sounding_type);
+ return;
+ }
+
+ /*
+ * When Multiple STAs are members of a single sounding process, that is about to be stopped,
+ * we want to schedule the stopping work only once and possibly start another sounding
+ * sequence for STAs that still want to use it.
+ */
+ if (enable)
+ goto next;
+
+ cl_sta = cl_sta_arr[0];
+ if (cl_sta) {
+ u8 sid = cl_sta->su_sid;
+
+ if (sid != INVALID_SID) {
+ elem = cl_sounding_get_elem(cl_hw, sid);
+
+ if (!elem) {
+ sounding_pr_trace("Sounding %u not found\n", sid);
+ return;
+ }
+
+ cl_sta->bf_db.sounding_remove_required = true;
+
+ if (elem->sounding_restart_required)
+ return;
+ elem->sounding_restart_required = true;
+ }
+ }
+
+next:
+ /* data will be freed in work handler */
+ data = kzalloc(sizeof(*data), GFP_ATOMIC);
+
+ if (!data)
+ return;
+
+ data->cl_hw = cl_hw;
+ data->start = enable;
+ data->sounding_type = sounding_type;
+ data->bw = bw;
+ data->is_recovery = cl_recovery_in_progress(cl_hw);
+ data->elem = recovery_elem ? recovery_elem : elem;
+
+ /* Fill cl_sta_arr */
+ {
+ for (i = 0; i < sta_num; i++)
+ data->sta_indices[i] = cl_sta_arr[i]->sta_idx;
+
+ data->sta_num = sta_num;
+ }
+
+ if (background) {
+ INIT_WORK(&data->work, cl_sounding_handler_send_request);
+ queue_work(cl_hw->sounding.sounding_wq, &data->work);
+ } else {
+ cl_sounding_handler_send_request((struct work_struct *)data);
+ }
+}
+
+static void cl_sounding_change_interval(struct cl_hw *cl_hw, u8 sounding_type)
+{
+ /* Data will be freed in work handler */
+ struct sounding_work_data *data = kzalloc(sizeof(*data), GFP_ATOMIC);
+
+ if (!data)
+ return;
+
+ INIT_WORK(&data->work, cl_sounding_handler_change_interval);
+ data->cl_hw = cl_hw;
+ data->sounding_type = sounding_type;
+ queue_work(cl_hw->sounding.sounding_wq, &data->work);
+}
+
+void cl_sounding_stop_by_sid(struct cl_hw *cl_hw, u8 sid, bool sounding_restart_check)
+{
+ struct cl_sounding_info *elem = cl_sounding_get_elem(cl_hw, sid);
+
+ if (!elem) {
+ sounding_pr_trace("Sounding with id %u not found or is in the middle of removal\n",
+ sid);
+ return;
+ }
+
+ elem->sounding_restart_required = sounding_restart_check;
+ cl_sounding_stop_handler(cl_hw, elem);
+}
+
+void cl_sounding_maintenance(struct cl_hw *cl_hw)
+{
+ /*
+ * Change sounding_index accoording to number of active_profiles.
+ * sounding_index is modified only if number of active_profiles is stable for 5 seconds.
+ *
+ * Examples:
+ * e.g #1: active_profiles=2, active_profiles_prev=3,3,3,3,3 - stabilised on 3
+ * e.g #3: active_profiles=2, active_profiles_prev=1,1,1,1,1 - stabilised on 1
+ * e.g #2: active_profiles=5, active_profiles_prev=6,7,7,6,6 - stabilised on 6
+ * e.g #4: active_profiles=5, active_profiles_prev=4,3,3,2,4 - stabilised on 4
+ */
+
+ int i = 0;
+ u8 active_profiles_min = 255;
+ u8 active_profiles_max = 0;
+ u8 active_profiles = cl_hw->sounding.last_conf_active_profiles;
+ u8 active_profiles_new = 0;
+ u16 interval;
+ u16 interval_new;
+
+ /* Add to last 5 sec buffer */
+ cl_hw->sounding.active_profiles_prev[cl_hw->sounding.active_profiles_idx] =
+ cl_hw->sounding.active_profiles;
+
+ /* Increase cyclic index */
+ cl_hw->sounding.active_profiles_idx++;
+ if (cl_hw->sounding.active_profiles_idx == CL_SOUNDING_STABILITY_TIME)
+ cl_hw->sounding.active_profiles_idx = 0;
+
+ /* Find active_profiles min/max in last 5 seconds */
+ for (i = 0; i < CL_SOUNDING_STABILITY_TIME; i++) {
+ if (cl_hw->sounding.active_profiles_prev[i] < active_profiles_min)
+ active_profiles_min = cl_hw->sounding.active_profiles_prev[i];
+
+ if (cl_hw->sounding.active_profiles_prev[i] > active_profiles_max)
+ active_profiles_max = cl_hw->sounding.active_profiles_prev[i];
+ }
+
+ if (active_profiles < active_profiles_min)
+ active_profiles_new = active_profiles_min;
+ else if (active_profiles > active_profiles_max)
+ active_profiles_new = active_profiles_max;
+ else /* Active_profiles in last 5 seconds did not change or is not stable */
+ return;
+
+ interval = cl_sounding_calc_interval(cl_hw, active_profiles);
+ interval_new = cl_sounding_calc_interval(cl_hw, active_profiles_new);
+
+ /* Check if sounding interval changed */
+ if (interval != interval_new) {
+ cl_hw->sounding.last_conf_active_profiles = active_profiles_new;
+ cl_hw->sounding.current_interval = interval_new;
+ cl_sounding_change_interval(cl_hw, SOUNDING_TYPE_MAX);
+ sounding_pr_trace("Interval: current = %u, new = %u\n",
+ interval, interval_new);
+ }
+}
+
+u16 cl_sounding_get_interval(struct cl_hw *cl_hw)
+{
+ return cl_hw->sounding.current_interval;
+}
+
+static void cl_sounding_indication_pr(struct cl_hw *cl_hw,
+ struct mm_sounding_ind *ind,
+ struct v_matrix_header *v_matrix,
+ u8 *avg_snr)
+{
+ sounding_pr_info("Sounding indication: nc index = %u, BFR BW = %u, "
+ "SNR1 = %u, SNR2 = %u, SNR3 = %u, SNR4 = %u, SNR5 = %u, SNR6 = %u,"
+ "feedback type = %u, sta index = %u\n",
+ v_matrix->nc_index, v_matrix->bw,
+ avg_snr[0], avg_snr[1], avg_snr[2], avg_snr[3], avg_snr[4],
+ avg_snr[5], ind->sounding_type, ind->sta_idx);
+}
+
+static void cl_sounding_indication_su(struct cl_hw *cl_hw,
+ struct mm_sounding_ind *ind,
+ struct cl_sounding_info *sounding_elem)
+{
+ struct cl_sta *cl_sta;
+ struct v_matrix_header *v_matrix = NULL;
+ u8 *avg_snr = NULL;
+ bool pairing = false;
+
+ v_matrix = sounding_elem->v_matrices_data + ind->v_matrix_offset[0];
+ avg_snr = (u8 *)v_matrix + v_matrix->padding;
+
+ cl_sta_lock(cl_hw);
+ cl_sta = cl_sta_get(cl_hw, ind->sta_idx);
+
+ if (cl_sta) {
+ struct cl_bf_sta_db *bf_db = &cl_sta->bf_db;
+
+ /* Update Nc for the current STA */
+ bf_db->nc = v_matrix->nc_index;
+ bf_db->sounding_indications++;
+
+ if (bf_db->sounding_indications == 1) {
+ /*
+ * After getting first indication disable the timer and set the BF
+ * bit in the firmware rate flags.
+ */
+ del_timer(&bf_db->timer);
+ cl_bf_update_rate(cl_hw, cl_sta);
+ pairing = true;
+ }
+ }
+
+ cl_sta_unlock(cl_hw);
+
+ /* Send a msg to fw to pair the STA with sounding ID */
+ if (pairing)
+ cl_msg_tx_sounding_pairing(cl_hw, ind->sid, ind->sounding_type, 0, ind->sta_idx);
+
+ cl_sounding_indication_pr(cl_hw, ind, v_matrix, avg_snr);
+}
+
+void cl_sounding_indication(struct cl_hw *cl_hw, struct mm_sounding_ind *ind)
+{
+ struct cl_sounding_info *sounding_elem = NULL;
+
+ sounding_elem = cl_sounding_get_elem(cl_hw, ind->sid);
+
+ if (!sounding_elem) {
+ sounding_pr_err("[%s]: sounding id %u not found!\n", __func__, ind->sid);
+ return;
+ }
+
+ switch (ind->sounding_type) {
+ case SOUNDING_TYPE_HE_SU:
+ case SOUNDING_TYPE_HE_SU_TB:
+ case SOUNDING_TYPE_VHT_SU:
+ case SOUNDING_TYPE_HE_CQI:
+ case SOUNDING_TYPE_HE_CQI_TB:
+ break;
+ default:
+ sounding_pr_err("[%s]: Invalid sounding type %u\n", __func__,
+ ind->sounding_type);
+ return;
+ }
+
+ cl_sounding_indication_su(cl_hw, ind, sounding_elem);
+}
+
+void cl_sounding_recovery(struct cl_hw *cl_hw)
+{
+ /*
+ * After recovery process we need to update sounding requests and
+ * sounding interval in firmware
+ */
+ struct cl_sounding_info *elem;
+
+ /* No sounding is active */
+ if (!cl_hw->sounding.num_soundings)
+ return;
+
+ /* Reset sounding parameters */
+ cl_sounding_recovery_reset(cl_hw);
+
+ /*
+ * Go over all clients that had sounding before recovery,
+ * and send a new sounding request to firmware.
+ */
+
+ sounding_pr_trace("Start sounding recovery\n");
+
+ list_for_each_entry(elem, &cl_hw->sounding.head, list)
+ cl_bf_sounding_start(cl_hw, elem->type, elem->su_cl_sta_arr, elem->sta_num, elem);
+}
+