[RFC,v2,74/96] cl8k: add sta.c

diff --git a/drivers/net/wireless/celeno/cl8k/sta.c b/drivers/net/wireless/celeno/cl8k/sta.c
new file mode 100644
index 000000000000..26c280b05266
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/sta.c
@@ -0,0 +1,507 @@ 
+// SPDX-License-Identifier: GPL-2.0 OR BSD-2-Clause
+/* Copyright(c) 2019-2022, Celeno Communications Ltd. */
+
+#include "chip.h"
+#include "phy.h"
+#include "bf.h"
+#include "vns.h"
+#include "tx.h"
+#include "radio.h"
+#include "motion_sense.h"
+#include "mac_addr.h"
+#include "recovery.h"
+#include "dfs.h"
+#include "stats.h"
+#include "utils.h"
+#include "sta.h"
+
+void cl_sta_init(struct cl_hw *cl_hw)
+{
+	u32 i;
+
+	rwlock_init(&cl_hw->cl_sta_db.lock);
+	INIT_LIST_HEAD(&cl_hw->cl_sta_db.head);
+
+	for (i = 0; i < CL_STA_HASH_SIZE; i++)
+		INIT_LIST_HEAD(&cl_hw->cl_sta_db.hash[i]);
+}
+
+void cl_sta_init_sta(struct cl_hw *cl_hw, struct ieee80211_sta *sta)
+{
+	if (!cl_recovery_in_progress(cl_hw)) {
+		struct cl_sta *cl_sta = IEEE80211_STA_TO_CL_STA(sta);
+
+		/* Reset all cl_sta strcture */
+		memset(cl_sta, 0, sizeof(struct cl_sta));
+		cl_sta->sta = sta;
+		/*
+		 * Set sta_idx to 0xFF since FW expects this value as long as
+		 * the STA is not fully connected
+		 */
+		cl_sta->sta_idx = STA_IDX_INVALID;
+	}
+}
+
+static void cl_sta_add_to_lut(struct cl_hw *cl_hw, struct cl_vif *cl_vif, struct cl_sta *cl_sta)
+{
+	write_lock_bh(&cl_hw->cl_sta_db.lock);
+
+	cl_hw->cl_sta_db.num++;
+	cl_vif->num_sta++;
+	cl_hw->cl_sta_db.lut[cl_sta->sta_idx] = cl_sta;
+
+	/* Done here inside the lock because it allocates cl_stats */
+	cl_stats_sta_add(cl_hw, cl_sta);
+
+	write_unlock_bh(&cl_hw->cl_sta_db.lock);
+
+	cl_dbg_verbose(cl_hw, "mac=%pM, sta_idx=%u, vif_index=%u\n",
+		       cl_sta->addr, cl_sta->sta_idx, cl_sta->cl_vif->vif_index);
+}
+
+static void cl_sta_add_to_list(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+	u8 hash_idx = CL_STA_HASH_IDX(cl_sta->addr[5]);
+
+	write_lock_bh(&cl_hw->cl_sta_db.lock);
+
+	/* Add to hash table */
+	list_add(&cl_sta->list_hash, &cl_hw->cl_sta_db.hash[hash_idx]);
+
+	/* Make sure that cl_sta's are stored in the list according to their sta_idx. */
+	if (list_empty(&cl_hw->cl_sta_db.head)) {
+		list_add(&cl_sta->list, &cl_hw->cl_sta_db.head);
+	} else if (list_is_singular(&cl_hw->cl_sta_db.head)) {
+		struct cl_sta *cl_sta_singular =
+			list_first_entry(&cl_hw->cl_sta_db.head, struct cl_sta, list);
+
+		if (cl_sta_singular->sta_idx < cl_sta->sta_idx)
+			list_add_tail(&cl_sta->list, &cl_hw->cl_sta_db.head);
+		else
+			list_add(&cl_sta->list, &cl_hw->cl_sta_db.head);
+	} else {
+		struct cl_sta *cl_sta_last =
+			list_last_entry(&cl_hw->cl_sta_db.head, struct cl_sta, list);
+
+		if (cl_sta->sta_idx > cl_sta_last->sta_idx) {
+			list_add_tail(&cl_sta->list, &cl_hw->cl_sta_db.head);
+		} else {
+			struct cl_sta *cl_sta_next = NULL;
+			struct cl_sta *cl_sta_prev = NULL;
+
+			list_for_each_entry(cl_sta_next, &cl_hw->cl_sta_db.head, list) {
+				if (cl_sta_next->sta_idx < cl_sta->sta_idx)
+					continue;
+
+				cl_sta_prev = list_prev_entry(cl_sta_next, list);
+				__list_add(&cl_sta->list, &cl_sta_prev->list, &cl_sta_next->list);
+				break;
+			}
+		}
+	}
+
+	write_unlock_bh(&cl_hw->cl_sta_db.lock);
+
+	cl_sta->add_complete = true;
+}
+
+static void cl_connection_data_add(struct cl_vif *cl_vif)
+{
+	struct cl_connection_data *conn_data = cl_vif->conn_data;
+
+	if (cl_vif->num_sta > conn_data->max_client) {
+		conn_data->max_client = cl_vif->num_sta;
+		conn_data->max_client_timestamp = ktime_get_real_seconds();
+	}
+
+	if (cl_vif->num_sta == conn_data->watermark_threshold)
+		conn_data->watermark_reached_cnt++;
+}
+
+static void cl_connection_data_remove(struct cl_vif *cl_vif)
+{
+	struct cl_connection_data *conn_data = cl_vif->conn_data;
+
+	if (cl_vif->num_sta == conn_data->watermark_threshold)
+		conn_data->watermark_reached_cnt++;
+}
+
+static void _cl_sta_add(struct cl_hw *cl_hw, struct ieee80211_vif *vif, struct ieee80211_sta *sta)
+{
+	struct cl_vif *cl_vif = (struct cl_vif *)vif->drv_priv;
+	struct cl_sta *cl_sta = IEEE80211_STA_TO_CL_STA(sta);
+
+	/* !!! Must be first !!! */
+	cl_sta_add_to_lut(cl_hw, cl_vif, cl_sta);
+
+	cl_baw_init(cl_sta);
+	cl_txq_sta_add(cl_hw, cl_sta);
+	cl_vns_sta_add(cl_hw, cl_sta);
+	cl_connection_data_add(cl_vif);
+
+	/*
+	 * Add rssi of association request to rssi pool
+	 * Make sure to call it before cl_wrs_api_sta_add()
+	 */
+	cl_rssi_assoc_find(cl_hw, cl_sta, cl_hw->cl_sta_db.num);
+
+	cl_motion_sense_sta_add(cl_hw, cl_sta);
+	cl_bf_sta_add(cl_hw, cl_sta, sta);
+	cl_wrs_api_sta_add(cl_hw, sta);
+	cl_wrs_api_set_smps_mode(cl_hw, sta, sta->bandwidth);
+#ifdef CONFIG_CL8K_DYN_MCAST_RATE
+	/* Should be called after cl_wrs_api_sta_add() */
+	cl_dyn_mcast_rate_update_upon_assoc(cl_hw, cl_sta->wrs_sta.mode,
+					    cl_hw->cl_sta_db.num);
+#endif /* CONFIG_CL8K_DYN_MCAST_RATE */
+#ifdef CONFIG_CL8K_DYN_BCAST_RATE
+	cl_dyn_bcast_rate_update_upon_assoc(cl_hw,
+					    cl_sta->wrs_sta.tx_su_params.rate_params.mcs,
+					    cl_hw->cl_sta_db.num);
+#endif /* CONFIG_CL8K_DYN_BCAST_RATE */
+
+	/* !!! Must be last !!! */
+	cl_sta_add_to_list(cl_hw, cl_sta);
+}
+
+/*
+ * Parse the ampdu density to retrieve the value in usec, according to
+ * the values defined in ieee80211.h
+ */
+static u8 cl_sta_density2usec(u8 ampdu_density)
+{
+	switch (ampdu_density) {
+	case IEEE80211_HT_MPDU_DENSITY_NONE:
+		return 0;
+		/* 1 microsecond is our granularity */
+	case IEEE80211_HT_MPDU_DENSITY_0_25:
+	case IEEE80211_HT_MPDU_DENSITY_0_5:
+	case IEEE80211_HT_MPDU_DENSITY_1:
+		return 1;
+	case IEEE80211_HT_MPDU_DENSITY_2:
+		return 2;
+	case IEEE80211_HT_MPDU_DENSITY_4:
+		return 4;
+	case IEEE80211_HT_MPDU_DENSITY_8:
+		return 8;
+	case IEEE80211_HT_MPDU_DENSITY_16:
+		return 16;
+	default:
+		return 0;
+	}
+}
+
+static void cl_sta_set_min_spacing(struct cl_hw *cl_hw,
+				   struct ieee80211_sta *sta)
+{
+	bool is_6g = cl_band_is_6g(cl_hw);
+	u8 sta_min_spacing = 0;
+	struct cl_sta *cl_sta = IEEE80211_STA_TO_CL_STA(sta);
+
+	if (is_6g)
+		sta_min_spacing =
+			cl_sta_density2usec(le16_get_bits(sta->he_6ghz_capa.capa,
+							  IEEE80211_HE_6GHZ_CAP_MIN_MPDU_START));
+	else if (sta->ht_cap.ht_supported)
+		sta_min_spacing =
+			cl_sta_density2usec(sta->ht_cap.ampdu_density);
+	else
+		cl_dbg_err(cl_hw, "HT is not supported - cannot set sta_min_spacing\n");
+
+	cl_sta->ampdu_min_spacing =
+		max(cl_sta_density2usec(IEEE80211_HT_MPDU_DENSITY_1), sta_min_spacing);
+}
+
+u32 cl_sta_num(struct cl_hw *cl_hw)
+{
+	u32 num = 0;
+
+	read_lock(&cl_hw->cl_sta_db.lock);
+	num = cl_hw->cl_sta_db.num;
+	read_unlock(&cl_hw->cl_sta_db.lock);
+
+	return num;
+}
+
+u32 cl_sta_num_bh(struct cl_hw *cl_hw)
+{
+	u32 num = 0;
+
+	read_lock_bh(&cl_hw->cl_sta_db.lock);
+	num = cl_hw->cl_sta_db.num;
+	read_unlock_bh(&cl_hw->cl_sta_db.lock);
+
+	return num;
+}
+
+struct cl_sta *cl_sta_get(struct cl_hw *cl_hw, u8 sta_idx)
+{
+	if (sta_idx < CL_MAX_NUM_STA)
+		return cl_hw->cl_sta_db.lut[sta_idx];
+
+	return NULL;
+}
+
+struct cl_sta *cl_sta_get_by_addr(struct cl_hw *cl_hw, u8 *addr)
+{
+	struct cl_sta *cl_sta = NULL;
+	u8 hash_idx = CL_STA_HASH_IDX(addr[5]);
+
+	if (is_multicast_ether_addr(addr))
+		return NULL;
+
+	list_for_each_entry(cl_sta, &cl_hw->cl_sta_db.hash[hash_idx], list_hash)
+		if (cl_mac_addr_compare(cl_sta->addr, addr))
+			return cl_sta;
+
+	return NULL;
+}
+
+void cl_sta_loop(struct cl_hw *cl_hw, sta_callback callback)
+{
+	struct cl_sta *cl_sta = NULL;
+
+	/* Go over all stations */
+	read_lock(&cl_hw->cl_sta_db.lock);
+
+	list_for_each_entry(cl_sta, &cl_hw->cl_sta_db.head, list)
+		callback(cl_hw, cl_sta);
+
+	read_unlock(&cl_hw->cl_sta_db.lock);
+}
+
+void cl_sta_loop_bh(struct cl_hw *cl_hw, sta_callback callback)
+{
+	struct cl_sta *cl_sta = NULL;
+
+	/* Go over all stations - use bottom-half lock */
+	read_lock_bh(&cl_hw->cl_sta_db.lock);
+
+	list_for_each_entry(cl_sta, &cl_hw->cl_sta_db.head, list)
+		callback(cl_hw, cl_sta);
+
+	read_unlock_bh(&cl_hw->cl_sta_db.lock);
+}
+
+static int cl_sta_add_to_firmware(struct cl_hw *cl_hw, struct ieee80211_vif *vif,
+				  struct ieee80211_sta *sta)
+{
+	struct cl_sta *cl_sta = (struct cl_sta *)sta->drv_priv;
+	struct cl_vif *cl_vif = (struct cl_vif *)vif->drv_priv;
+	struct mm_sta_add_cfm *sta_add_cfm;
+	int error = 0;
+	u8 recovery_sta_idx = 0;
+	u32 rate_ctrl_info = 0;
+
+	if (cl_recovery_in_progress(cl_hw)) {
+		struct cl_wrs_rate_params *rate_params = &cl_sta->wrs_sta.tx_su_params.rate_params;
+
+		/*
+		 * If station is added to firmware during recovery, the driver passes to firmware
+		 * the station index to be used instead of firmware selecting a free index
+		 */
+		recovery_sta_idx = cl_sta->sta_idx;
+
+		/* Keep current rate value */
+		rate_ctrl_info = cl_rate_ctrl_generate(cl_hw, cl_sta, rate_params->mode,
+						       rate_params->bw, rate_params->nss,
+						       rate_params->mcs, rate_params->gi,
+						       false, false);
+	} else {
+		bool is_cck = cl_band_is_24g(cl_hw) && cl_hw_mode_is_b_or_bg(cl_hw);
+		u8 mode = is_cck ? WRS_MODE_CCK : WRS_MODE_OFDM;
+
+		/*
+		 * Not in recovery:
+		 * firmware will set sta_idx and will return in confirmation message
+		 */
+		recovery_sta_idx = STA_IDX_INVALID;
+
+		/* Default rate value */
+		rate_ctrl_info = cl_rate_ctrl_generate(cl_hw, cl_sta, mode,
+						       0, 0, 0, 0, false, false);
+	}
+
+	/* Must be called before cl_msg_tx_sta_add() */
+	cl_sta_set_min_spacing(cl_hw, sta);
+
+	/* Send message to firmware */
+	error = cl_msg_tx_sta_add(cl_hw, sta, cl_vif, recovery_sta_idx, rate_ctrl_info);
+	if (error)
+		return error;
+
+	sta_add_cfm = (struct mm_sta_add_cfm *)(cl_hw->msg_cfm_params[MM_STA_ADD_CFM]);
+	if (!sta_add_cfm)
+		return -ENOMSG;
+
+	if (sta_add_cfm->status != 0) {
+		cl_dbg_verbose(cl_hw, "Status Error (%u)\n", sta_add_cfm->status);
+		cl_msg_tx_free_cfm_params(cl_hw, MM_STA_ADD_CFM);
+		return -EIO;
+	}
+
+	/* Save the index retrieved from firmware */
+	cl_sta->sta_idx = sta_add_cfm->sta_idx;
+
+	/* Release cfm msg */
+	cl_msg_tx_free_cfm_params(cl_hw, MM_STA_ADD_CFM);
+
+	return 0;
+}
+
+int cl_sta_add(struct cl_hw *cl_hw, struct ieee80211_vif *vif,
+	       struct ieee80211_sta *sta)
+{
+	struct cl_sta *cl_sta = (struct cl_sta *)sta->drv_priv;
+	struct cl_vif *cl_vif = (struct cl_vif *)vif->drv_priv;
+	int error = 0;
+
+	if (cl_radio_is_going_down(cl_hw))
+		return -EPERM;
+
+	if (vif->type == NL80211_IFTYPE_MESH_POINT &&
+	    cl_vif->num_sta >= CL_MAX_NUM_MESH_POINT)
+		return -EPERM;
+
+	cl_sta->cl_vif = cl_vif;
+	cl_mac_addr_copy(cl_sta->addr, sta->addr);
+
+	error = cl_sta_add_to_firmware(cl_hw, vif, sta);
+	if (error)
+		return error;
+
+	if (!cl_recovery_in_progress(cl_hw))
+		if (vif->type != NL80211_IFTYPE_STATION ||
+		    cl_hw->chip->conf->ce_production_mode)
+			_cl_sta_add(cl_hw, vif, sta);
+
+	if (vif->type == NL80211_IFTYPE_MESH_POINT && cl_vif->num_sta == 1)
+		set_bit(CL_DEV_MESH_AP, &cl_hw->drv_flags);
+
+	return 0;
+}
+
+void cl_sta_mgd_add(struct cl_hw *cl_hw, struct cl_vif *cl_vif, struct ieee80211_sta *sta)
+{
+	/* Should be called in station mode */
+	struct cl_sta *cl_sta = (struct cl_sta *)sta->drv_priv;
+
+	/* !!! Must be first !!! */
+	cl_sta_add_to_lut(cl_hw, cl_vif, cl_sta);
+
+	cl_baw_init(cl_sta);
+	cl_txq_sta_add(cl_hw, cl_sta);
+	cl_vns_sta_add(cl_hw, cl_sta);
+
+	/*
+	 * Add rssi of association response to rssi pool
+	 * Make sure to call it before cl_wrs_api_sta_add()
+	 */
+	cl_rssi_assoc_find(cl_hw, cl_sta, cl_hw->cl_sta_db.num);
+
+	cl_connection_data_add(cl_vif);
+
+	/* In station mode we assume that the AP we connect to is static */
+	cl_motion_sense_sta_add(cl_hw, cl_sta);
+	cl_bf_sta_add(cl_hw, cl_sta, sta);
+	cl_wrs_api_sta_add(cl_hw, sta);
+#ifdef CONFIG_CL8K_DYN_MCAST_RATE
+	/* Should be called after cl_wrs_api_sta_add() */
+	cl_dyn_mcast_rate_update_upon_assoc(cl_hw, cl_sta->wrs_sta.mode,
+					    cl_hw->cl_sta_db.num);
+#endif /* CONFIG_CL8K_DYN_MCAST_RATE */
+#ifdef CONFIG_CL8K_DYN_BCAST_RATE
+	cl_dyn_bcast_rate_update_upon_assoc(cl_hw,
+					    cl_sta->wrs_sta.tx_su_params.rate_params.mcs,
+					    cl_hw->cl_sta_db.num);
+
+#endif /* CONFIG_CL8K_DYN_BCAST_RATE */
+	/* !!! Must be last !!! */
+	cl_sta_add_to_list(cl_hw, cl_sta);
+}
+
+static void _cl_sta_remove(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+	write_lock_bh(&cl_hw->cl_sta_db.lock);
+
+	list_del(&cl_sta->list);
+	list_del(&cl_sta->list_hash);
+
+	cl_hw->cl_sta_db.lut[cl_sta->sta_idx] = NULL;
+	cl_hw->cl_sta_db.num--;
+	cl_sta->cl_vif->num_sta--;
+
+	cl_dbg_verbose(cl_hw, "mac=%pM, sta_idx=%u, vif_index=%u\n",
+		       cl_sta->addr, cl_sta->sta_idx, cl_sta->cl_vif->vif_index);
+
+	write_unlock_bh(&cl_hw->cl_sta_db.lock);
+}
+
+void cl_sta_remove(struct cl_hw *cl_hw, struct ieee80211_vif *vif, struct ieee80211_sta *sta)
+{
+	struct cl_vif *cl_vif = (struct cl_vif *)vif->drv_priv;
+	struct cl_sta *cl_sta = (struct cl_sta *)sta->drv_priv;
+	u8 sta_idx = cl_sta->sta_idx;
+
+	cl_sta->remove_start = true;
+
+	/* !!! Must be first - remove from list and LUT !!! */
+	_cl_sta_remove(cl_hw, cl_sta);
+
+	cl_traffic_sta_remove(cl_hw, cl_sta);
+	cl_bf_sta_remove(cl_hw, cl_sta);
+	cl_connection_data_remove(cl_vif);
+#ifdef CONFIG_CL8K_DYN_MCAST_RATE
+	cl_dyn_mcast_rate_update_upon_disassoc(cl_hw,
+					       cl_sta->wrs_sta.mode,
+					       cl_hw->cl_sta_db.num);
+#endif /* CONFIG_CL8K_DYN_MCAST_RATE */
+#ifdef CONFIG_CL8K_DYN_BCAST_RATE
+	cl_dyn_bcast_rate_update_upon_disassoc(cl_hw,
+					       cl_sta->wrs_sta.tx_su_params.rate_params.mcs,
+					       cl_hw->cl_sta_db.num);
+#endif /* CONFIG_CL8K_DYN_BCAST_RATE */
+	cl_wrs_api_sta_remove(cl_hw, cl_sta);
+	cl_stats_sta_remove(cl_hw, cl_sta);
+
+	/*
+	 * TX stop flow:
+	 * 1) Flush TX queues
+	 * 2) Poll confirmation queue and clear enhanced TIM
+	 * 3) Send MM_STA_DEL_REQ message to firmware
+	 * 4) Flush confirmation queue
+	 * 5) Reset write index
+	 */
+
+	cl_txq_flush_sta(cl_hw, cl_sta);
+	cl_single_cfm_poll_empty_sta(cl_hw, sta_idx);
+	cl_txq_sta_remove(cl_hw, sta_idx);
+
+	if (cl_vif->vif->type == NL80211_IFTYPE_MESH_POINT &&
+	    cl_vif->num_sta == 0) {
+		clear_bit(CL_DEV_MESH_AP, &cl_hw->drv_flags);
+	}
+
+	cl_single_cfm_flush_sta(cl_hw, sta_idx);
+
+	cl_msg_tx_sta_del(cl_hw, sta_idx);
+
+	if (cl_vif->num_sta == 0)
+		cl_radio_off_wake_up(cl_hw);
+}
+
+void cl_sta_ps_notify(struct cl_hw *cl_hw, struct cl_sta *cl_sta, bool is_ps)
+{
+	struct sta_info *stainfo = IEEE80211_STA_TO_STAINFO(cl_sta->sta);
+
+	/*
+	 * PS-Poll & UAPSD are handled by FW, by setting
+	 * WLAN_STA_SP we ensure mac80211 does not re-handle.
+	 * flag is unset at ieee80211_sta_ps_deliver_wakeup
+	 */
+	if (is_ps)
+		set_sta_flag(stainfo, WLAN_STA_SP);
+
+	cl_stats_update_ps(cl_hw, cl_sta, is_ps);
+}
+