new file mode 100644
@@ -0,0 +1,354 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-2-Clause
+/* Copyright(c) 2019-2022, Celeno Communications Ltd. */
+
+#include "maintenance.h"
+#include "mac_addr.h"
+#include "debug.h"
+#include "vns.h"
+
+#define CL_VNS_HASH_IDX (ETH_ALEN - 2)
+#define CL_VNS_MGMT_AGEOUT 200
+
+#define vns_pr(...) \
+ do { \
+ if (unlikely(cl_hw->vns_db->dbg)) \
+ pr_debug("[VNS]" __VA_ARGS__); \
+ } while (0)
+
+#define vns_pr_pkt(...) \
+ do { \
+ if (unlikely(cl_hw->vns_db->dbg_per_packet)) \
+ pr_debug("[VNS-pkt]" __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) {
+ vns_pr("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;
+ /* 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) {
+ vns_pr("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);
+}
+
+int cl_vns_init(struct cl_hw *cl_hw)
+{
+ int i = 0;
+ u8 vns_pwr_mode = cl_hw->conf->ci_vns_pwr_mode;
+
+ cl_hw->vns_db = vzalloc(sizeof(*cl_hw->vns_db));
+ if (!cl_hw->vns_db)
+ return -ENOMEM;
+
+ 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]);
+
+ return 0;
+}
+
+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;
+ }
+
+ vfree(cl_hw->vns_db);
+ cl_hw->vns_db = NULL;
+}
+
+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 || !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 exisiting entry */
+ entry->strongset_rssi = strongset_rssi;
+ entry->timestamp = jiffies;
+ vns_pr("sta %pM updated in list (rssi=%d)\n",
+ addr, strongset_rssi);
+ } else {
+ /* Remove exisiting entry */
+ cl_vns_mgmt_list_remove(cl_hw, entry);
+ vns_pr("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);
+ vns_pr("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);
+ spin_unlock_bh(&cl_hw->vns_db->lock);
+
+ vns_pr_pkt("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;
+
+ vns_pr_pkt("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) {
+ vns_pr("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 {
+ vns_pr("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)
+{
+ vns_pr("Recovery\n");
+ cl_sta_loop_bh(cl_hw, cl_vns_recovery_sta);
+}
+