[RFC v2 72/96] cl8k: add sounding.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/sounding.c | 1121 +++++++++++++++++++
 1 file changed, 1121 insertions(+)
 create mode 100644 drivers/net/wireless/celeno/cl8k/sounding.c

diff --git a/drivers/net/wireless/celeno/cl8k/sounding.c b/drivers/net/wireless/celeno/cl8k/sounding.c
new file mode 100644
index 000000000000..09d43a01bb70
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/sounding.c
@@ -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);
+}
+
-- 
2.36.1