From: Viktor Barna <viktor.barna@xxxxxxxxxx>
(Part of the split. Please, take a look at the cover letter for more
details).
Signed-off-by: Viktor Barna <viktor.barna@xxxxxxxxxx>
---
drivers/net/wireless/celeno/cl8k/vns.c | 505 +++++++++++++++++++++++++
1 file changed, 505 insertions(+)
create mode 100644 drivers/net/wireless/celeno/cl8k/vns.c
diff --git a/drivers/net/wireless/celeno/cl8k/vns.c b/drivers/net/wireless/celeno/cl8k/vns.c
new file mode 100644
index 000000000000..75c3c0374793
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/vns.c
@@ -0,0 +1,505 @@
+// SPDX-License-Identifier: MIT
+/* Copyright(c) 2019-2021, Celeno Communications Ltd. */
+
+#include "vns.h"
+#include "rssi.h"
+#include "fw/msg_tx.h"
+#include "maintenance.h"
+#include "mac_addr.h"
+
+#define CL_VNS_HASH_IDX (ETH_ALEN - 2)
+#define CL_VNS_MGMT_AGEOUT 200
+
+#define CL_VNS_DBG(...) \
+ do { \
+ if (unlikely(cl_hw->vns_db.dbg)) \
+ pr_debug(__VA_ARGS__); \
+ } while (0)
+
+#define CL_VNS_DBG_PER_PACKET(...) \
+ do { \
+ if (unlikely(cl_hw->vns_db.dbg_per_packet)) \
+ pr_debug(__VA_ARGS__); \
+ } while (0)
+
+static void cl_vns_mgmt_list_add(struct cl_hw *cl_hw, u8 *addr, s8 strongset_rssi)
+{
+ /* Add entry to mgmt list */
+ struct cl_vns_rssi_entry *entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
+
+ if (!entry)
+ return;
+
+ /* Fill entry parameters */
+ INIT_LIST_HEAD(&entry->list_all);
+ INIT_LIST_HEAD(&entry->list_addr);
+ cl_mac_addr_copy(entry->addr, addr);
+ entry->strongset_rssi = strongset_rssi;
+ entry->timestamp = jiffies;
+
+ /* Add to list */
+ cl_hw->vns_db.mgmt_db.num_entries++;
+ list_add(&entry->list_all, &cl_hw->vns_db.mgmt_db.list_all);
+ list_add(&entry->list_addr, &cl_hw->vns_db.mgmt_db.list_addr[addr[CL_VNS_HASH_IDX]]);
+}
+
+static void cl_vns_mgmt_list_remove(struct cl_hw *cl_hw, struct cl_vns_rssi_entry *entry)
+{
+ /* Remove entry from mgmt list */
+ cl_hw->vns_db.mgmt_db.num_entries--;
+ list_del(&entry->list_all);
+ list_del(&entry->list_addr);
+ kfree(entry);
+}
+
+static void cl_vns_mgmt_list_flush(struct cl_hw *cl_hw)
+{
+ /* Flush all mgmt list */
+ if (cl_hw->vns_db.mgmt_db.num_entries > 0) {
+ struct cl_vns_rssi_entry *entry = NULL, *tmp = NULL;
+
+ list_for_each_entry_safe(entry, tmp, &cl_hw->vns_db.mgmt_db.list_all, list_all)
+ cl_vns_mgmt_list_remove(cl_hw, entry);
+ }
+}
+
+static struct cl_vns_rssi_entry *cl_vns_mgmt_list_find(struct cl_hw *cl_hw, u8 *addr)
+{
+ /* Search for entry in mgmt list */
+ struct cl_vns_mgmt_db *mgmt_db = &cl_hw->vns_db.mgmt_db;
+
+ if (mgmt_db->num_entries > 0) {
+ struct cl_vns_rssi_entry *entry = NULL;
+
+ list_for_each_entry(entry, &mgmt_db->list_addr[addr[CL_VNS_HASH_IDX]], list_addr)
+ if (ether_addr_equal(entry->addr, addr))
+ return entry;
+ }
+
+ return NULL;
+}
+
+static bool cl_vns_mgmt_list_find_and_remove(struct cl_hw *cl_hw, u8 *addr)
+{
+ /*
+ * Search for entry in mgmt list
+ * If entry found remove it and return true
+ */
+ struct cl_vns_rssi_entry *entry = cl_vns_mgmt_list_find(cl_hw, addr);
+
+ if (entry) {
+ cl_vns_mgmt_list_remove(cl_hw, entry);
+ return true;
+ }
+
+ return false;
+}
+
+static void cl_vns_mgmt_list_ageout(struct cl_hw *cl_hw)
+{
+ /* Remove old entries from mgmt list */
+ struct cl_vns_mgmt_db *mgmt_db = &cl_hw->vns_db.mgmt_db;
+
+ if (mgmt_db->num_entries > 0) {
+ struct cl_vns_rssi_entry *entry = NULL, *tmp = NULL;
+ unsigned long delta_time;
+
+ list_for_each_entry_safe(entry, tmp, &mgmt_db->list_all, list_all) {
+ delta_time = jiffies_to_msecs(jiffies - entry->timestamp);
+
+ if (delta_time > CL_VNS_MGMT_AGEOUT) {
+ CL_VNS_DBG("[VNS] sta %pM removed from list because of ageout\n",
+ entry->addr);
+ cl_vns_mgmt_list_remove(cl_hw, entry);
+ }
+ }
+ }
+}
+
+static s8 cl_vns_get_strongest_rssi(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ struct cl_vns_sta_db *vns_db = &cl_sta->vns_db;
+ s32 rssi_samples = vns_db->rssi_samples;
+
+ if (rssi_samples > 0) {
+ u8 i;
+ s32 strongest_rssi = S32_MIN;
+
+ for (i = 0; i < cl_hw->num_antennas; i++)
+ if (vns_db->rssi_sum[i] > strongest_rssi)
+ strongest_rssi = vns_db->rssi_sum[i];
+
+ /* Reset rssi for next time that cl_vns_get_strongest_rssi() will be called */
+ memset(vns_db->rssi_sum, 0, sizeof(vns_db->rssi_sum));
+ vns_db->rssi_samples = 0;
+
+ return (s8)(strongest_rssi / rssi_samples);
+ }
+
+ return 0;
+}
+
+static void cl_vns_monitor_rssi(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ /* Monitor RSSI of associated stations and update state if necessary */
+ struct cl_vns_sta_db *vns_db = &cl_sta->vns_db;
+ s8 strongset_rssi = cl_vns_get_strongest_rssi(cl_hw, cl_sta);
+ s8 rssi_thr = 0;
+ bool is_vns = false;
+
+ if (strongset_rssi == 0)
+ return;
+
+ /*
+ * Calculate RSSI threshold (take hystersis into
+ * consideration according to current state)
+ */
+ if (vns_db->is_very_near)
+ rssi_thr = cl_hw->conf->ci_vns_rssi_thr - cl_hw->conf->ci_vns_rssi_hys;
+ else
+ rssi_thr = cl_hw->conf->ci_vns_rssi_thr + cl_hw->conf->ci_vns_rssi_hys;
+
+ is_vns = (strongset_rssi > rssi_thr) ? true : false;
+
+ /* Avoid toggling of VNS state - require two consecutive same decisions */
+ if (is_vns != vns_db->prev_decision) {
+ vns_db->prev_decision = is_vns;
+ return;
+ }
+
+ if (is_vns != vns_db->is_very_near) {
+ CL_VNS_DBG("[VNS] sta %pM changed state, strongset_rssi = %d, is_vns = %s\n",
+ cl_sta->addr, strongset_rssi, is_vns ? "TRUE" : "FALSE");
+ vns_db->is_very_near = is_vns;
+ cl_msg_tx_set_vns(cl_hw, cl_sta->sta_idx, is_vns);
+ }
+}
+
+static void cl_vns_recovery_sta(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ if (cl_sta->vns_db.is_very_near)
+ cl_msg_tx_set_vns(cl_hw, cl_sta->sta_idx, true);
+}
+
+static int cl_vns_print_sta_state(struct cl_hw *cl_hw)
+{
+ struct cl_sta *cl_sta = NULL;
+ char *buf = NULL;
+ ssize_t buf_size;
+ int err = 0;
+ int len = 0;
+
+ /* 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)
+ cl_snprintf(&buf, &len, &buf_size,
+ "sta_idx = %u, mac = %pM, is_very_near = %s\n",
+ cl_sta->sta_idx, cl_sta->addr,
+ cl_sta->vns_db.is_very_near ?
+ "true" : "false");
+
+ read_unlock_bh(&cl_hw->cl_sta_db.lock);
+ err = cl_vendor_reply(cl_hw, buf, len);
+ kfree(buf);
+
+ return err;
+}
+
+static int cl_vns_cli_help(struct cl_hw *cl_hw)
+{
+ char *buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
+ int err = 0;
+
+ if (!buf)
+ return -ENOMEM;
+
+ snprintf(buf, PAGE_SIZE,
+ "vns usage\n"
+ "-a: Set rssi auto response threshold [thr]\n"
+ "-d: Set debug [0/1]\n"
+ "-h: Set rssi hystersis [hyst]\n"
+ "-l: Set power limit [limit]\n"
+ "-m: Set power mode [mode]\n"
+ "-p: Set debug per packet [0/1]\n"
+ "-s: Print all stations state\n"
+ "-t: Set rssi threshold [thr]\n");
+
+ err = cl_vendor_reply(cl_hw, buf, strlen(buf));
+ kfree(buf);
+
+ return err;
+}
+
+void cl_vns_init(struct cl_hw *cl_hw)
+{
+ int i = 0;
+ u8 vns_pwr_mode = cl_hw->conf->ci_vns_pwr_mode;
+
+ if (vns_pwr_mode == VNS_MODE_DATA || vns_pwr_mode == VNS_MODE_ALL)
+ cl_hw->vns_db.enable = true;
+
+ spin_lock_init(&cl_hw->vns_db.lock);
+
+ INIT_LIST_HEAD(&cl_hw->vns_db.mgmt_db.list_all);
+
+ for (i = 0; i < STA_HASH_SIZE; i++)
+ INIT_LIST_HEAD(&cl_hw->vns_db.mgmt_db.list_addr[i]);
+}
+
+void cl_vns_close(struct cl_hw *cl_hw)
+{
+ if (cl_hw->vns_db.enable) {
+ spin_lock_bh(&cl_hw->vns_db.lock);
+ cl_vns_mgmt_list_flush(cl_hw);
+ spin_unlock_bh(&cl_hw->vns_db.lock);
+
+ cl_hw->vns_db.enable = false;
+ }
+}
+
+void cl_vns_maintenance(struct cl_hw *cl_hw)
+{
+ /*
+ * Maintenance:
+ * 1) Remove old entries from mgmt list
+ * 2) Update state for associated clients
+ */
+ if (!cl_hw->vns_db.enable)
+ return;
+
+ cl_hw->vns_db.interval_period += CL_MAINTENANCE_PERIOD_SLOW_MS;
+
+ if (cl_hw->vns_db.interval_period < cl_hw->conf->ci_vns_maintenance_time)
+ return;
+
+ cl_hw->vns_db.interval_period = 0;
+
+ spin_lock_bh(&cl_hw->vns_db.lock);
+ cl_vns_mgmt_list_ageout(cl_hw);
+ spin_unlock_bh(&cl_hw->vns_db.lock);
+
+ /* Check RSSI of associated stations */
+ cl_sta_loop(cl_hw, cl_vns_monitor_rssi);
+}
+
+void cl_vns_mgmt_handler(struct cl_hw *cl_hw, u8 *addr, s8 rssi[MAX_ANTENNAS])
+{
+ /*
+ * Handle management frames of non-associated stations,
+ * and save the very-near ones in the mgmt list
+ */
+ s8 strongset_rssi = 0;
+ struct cl_vns_rssi_entry *entry = NULL;
+
+ if (!cl_hw->vns_db.enable)
+ return;
+
+ strongset_rssi = cl_rssi_get_strongest(cl_hw, rssi);
+
+ spin_lock_bh(&cl_hw->vns_db.lock);
+
+ entry = cl_vns_mgmt_list_find(cl_hw, addr);
+
+ if (entry) {
+ if (strongset_rssi > cl_hw->conf->ci_vns_rssi_thr) {
+ /* Update existing entry */
+ entry->strongset_rssi = strongset_rssi;
+ entry->timestamp = jiffies;
+ CL_VNS_DBG("[VNS] sta %pM updated in list (rssi=%d)\n",
+ addr, strongset_rssi);
+ } else {
+ /* Remove existing entry */
+ cl_vns_mgmt_list_remove(cl_hw, entry);
+ CL_VNS_DBG("[VNS] sta %pM removed from list (rssi=%d)\n",
+ addr, strongset_rssi);
+ }
+ } else {
+ if (strongset_rssi > cl_hw->conf->ci_vns_rssi_thr) {
+ /* Add new entry */
+ cl_vns_mgmt_list_add(cl_hw, addr, strongset_rssi);
+ CL_VNS_DBG("[VNS] sta %pM added to list (rssi=%d)\n",
+ addr, strongset_rssi);
+ }
+ }
+
+ spin_unlock_bh(&cl_hw->vns_db.lock);
+}
+
+bool cl_vns_is_very_near(struct cl_hw *cl_hw, struct cl_sta *cl_sta, struct sk_buff *skb)
+{
+ bool is_vns = false;
+ /* This function checks for every TX packet whether it's VNS or not */
+ if (!cl_hw->vns_db.enable)
+ return false;
+
+ if (unlikely(!cl_sta)) {
+ struct ieee80211_hdr *mac_hdr = (struct ieee80211_hdr *)skb->data;
+
+ spin_lock_bh(&cl_hw->vns_db.lock);
+ is_vns = cl_vns_mgmt_list_find(cl_hw, mac_hdr->addr1) ? true : false;
+ spin_unlock_bh(&cl_hw->vns_db.lock);
+
+ CL_VNS_DBG_PER_PACKET("[VNS] mgmt-sta %pM, is_vns = %s\n",
+ mac_hdr->addr1, is_vns ? "TRUE" : "FALSE");
+
+ return is_vns;
+ }
+ is_vns = cl_sta->vns_db.is_very_near;
+
+ CL_VNS_DBG_PER_PACKET("[VNS] assoc-sta %pM, is_vns = %s\n",
+ cl_sta->addr, is_vns ? "TRUE" : "FALSE");
+
+ return is_vns;
+}
+
+void cl_vns_sta_add(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ /* Update is_very_near according to mgmt list */
+ bool is_vns = false;
+
+ if (!cl_hw->vns_db.enable)
+ return;
+
+ spin_lock_bh(&cl_hw->vns_db.lock);
+ is_vns = cl_vns_mgmt_list_find_and_remove(cl_hw, cl_sta->addr);
+ spin_unlock_bh(&cl_hw->vns_db.lock);
+
+ if (is_vns) {
+ CL_VNS_DBG("[VNS] sta %pM connected - is_vns = TRUE\n", cl_sta->addr);
+ cl_sta->vns_db.is_very_near = true;
+ cl_sta->vns_db.prev_decision = true;
+ cl_msg_tx_set_vns(cl_hw, cl_sta->sta_idx, true);
+ } else {
+ CL_VNS_DBG("[VNS] sta %pM connected - is_vns = FALSE\n", cl_sta->addr);
+ }
+}
+
+void cl_vns_handle_rssi(struct cl_hw *cl_hw, struct cl_sta *cl_sta, s8 rssi[MAX_ANTENNAS])
+{
+ /* Collect rssi samples */
+ int i;
+
+ if (!cl_hw->vns_db.enable)
+ return;
+
+ for (i = 0; i < cl_hw->num_antennas; i++)
+ cl_sta->vns_db.rssi_sum[i] += rssi[i];
+
+ cl_sta->vns_db.rssi_samples++;
+}
+
+void cl_vns_recovery(struct cl_hw *cl_hw)
+{
+ CL_VNS_DBG("[VNS] Recovery\n");
+ cl_sta_loop_bh(cl_hw, cl_vns_recovery_sta);
+}
+
+int cl_vns_cli(struct cl_hw *cl_hw, struct cli_params *cli_params)
+{
+ u32 expected_params = 0;
+ bool set_rssi_auto_rsp_thr = false;
+ bool set_dbg = false;
+ bool set_rssi_hyst = false;
+ bool set_pwr_limit = false;
+ bool set_pwr_mode = false;
+ bool set_dbg_per_packet = false;
+ bool print_sta_state = false;
+ bool set_rssi_thr = false;
+
+ switch (cli_params->option) {
+ case 'a':
+ set_rssi_auto_rsp_thr = true;
+ expected_params = 1;
+ break;
+ case 'd':
+ set_dbg = true;
+ expected_params = 1;
+ break;
+ case 'h':
+ set_rssi_hyst = true;
+ expected_params = 1;
+ break;
+ case 'l':
+ set_pwr_limit = true;
+ expected_params = 1;
+ break;
+ case 'm':
+ set_pwr_mode = true;
+ expected_params = 1;
+ break;
+ case 'p':
+ set_dbg_per_packet = true;
+ expected_params = 1;
+ break;
+ case 's':
+ print_sta_state = true;
+ expected_params = 0;
+ break;
+ case 't':
+ set_rssi_thr = true;
+ expected_params = 1;
+ break;
+ case '?':
+ return cl_vns_cli_help(cl_hw);
+ default:
+ cl_dbg_err(cl_hw, "Illegal option (%c) - try '?' for help\n", cli_params->option);
+ goto out_err;
+ }
+
+ if (expected_params != cli_params->num_params) {
+ cl_dbg_err(cl_hw, "Wrong number of arguments (expected %u) (actual %u)\n",
+ expected_params, cli_params->num_params);
+ goto out_err;
+ }
+
+ if (set_rssi_auto_rsp_thr) {
+ cl_hw->conf->ci_vns_rssi_auto_resp_thr = (s8)cli_params->params[0];
+ pr_debug("[VNS] rssi auto response threshold = %d\n",
+ cl_hw->conf->ci_vns_rssi_auto_resp_thr);
+ return 0;
+ }
+
+ if (set_dbg) {
+ cl_hw->vns_db.dbg = (bool)cli_params->params[0];
+ pr_debug("[VNS] debug = %s\n", cl_hw->vns_db.dbg ? "enable" : "disable");
+ return 0;
+ }
+
+ if (set_pwr_limit) {
+ cl_hw->conf->ci_vns_pwr_limit = (s8)cli_params->params[0];
+ pr_debug("[VNS] power limit = %d\n", cl_hw->conf->ci_vns_pwr_limit);
+ return 0;
+ }
+
+ if (set_rssi_hyst) {
+ cl_hw->conf->ci_vns_rssi_hys = (s8)cli_params->params[0];
+ pr_debug("[VNS] rssi hystersis = %d\n", cl_hw->conf->ci_vns_rssi_hys);
+ return 0;
+ }
+
+ if (set_pwr_mode) {
+ cl_hw->conf->ci_vns_pwr_mode = (u8)cli_params->params[0];
+ pr_debug("[VNS] power mode = %u\n", cl_hw->conf->ci_vns_pwr_mode);
+ return 0;
+ }
+
+ if (set_dbg_per_packet) {
+ cl_hw->vns_db.dbg_per_packet = (bool)cli_params->params[0];
+ pr_debug("[VNS] debug per packet = %s\n",
+ cl_hw->vns_db.dbg_per_packet ? "enable" : "disable");
+ return 0;
+ }
+
+ if (set_rssi_thr) {
+ cl_hw->conf->ci_vns_rssi_thr = (s8)cli_params->params[0];
+ pr_debug("[VNS] rssi threshold = %d\n", cl_hw->conf->ci_vns_rssi_thr);
+ return 0;
+ }
+
+ if (print_sta_state)
+ return cl_vns_print_sta_state(cl_hw);
+
+out_err:
+ return -EIO;
+}
--
2.30.0
________________________________
The information transmitted is intended only for the person or entity to which it is addressed and may contain confidential and/or privileged material. Any retransmission, dissemination, copying or other use of, or taking of any action in reliance upon this information is prohibited. If you received this in error, please contact the sender and delete the material from any computer. Nothing contained herein shall be deemed as a representation, warranty or a commitment by Celeno. No warranties are expressed or implied, including, but not limited to, any implied warranties of non-infringement, merchantability and fitness for a particular purpose.
________________________________
