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>
---
.../net/wireless/celeno/cl8k/motion_sense.c | 244 ++++++++++++++++++
1 file changed, 244 insertions(+)
create mode 100644 drivers/net/wireless/celeno/cl8k/motion_sense.c
diff --git a/drivers/net/wireless/celeno/cl8k/motion_sense.c b/drivers/net/wireless/celeno/cl8k/motion_sense.c
new file mode 100644
index 000000000000..2e4c05564900
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/motion_sense.c
@@ -0,0 +1,244 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-2-Clause
+/* Copyright(c) 2019-2022, Celeno Communications Ltd. */
+
+#include "chip.h"
+#include "hw.h"
+#include "debug.h"
+#include "motion_sense.h"
+
+#define motion_pr(...) \
+ do { \
+ if (cl_hw->motion_sense_dbg) \
+ pr_debug("[MOTION SENSE]" __VA_ARGS__); \
+ } while (0)
+
+/* Minimum time (+1) for taking a decison */
+#define MOTION_SENSE_MIN_DECISION_MGMT_CTL 4
+#define MOTION_SENSE_MIN_DECISION_DATA 9
+#define MOTION_SENSE_MIN_DECISION_BA 9
+
+static void _cl_motion_sense_sta_add(struct cl_motion_rssi *motion_rssi)
+{
+ motion_rssi->max = S8_MIN;
+ motion_rssi->min = S8_MAX;
+}
+
+void cl_motion_sense_sta_add(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ _cl_motion_sense_sta_add(&cl_sta->motion_sense.rssi_mgmt_ctl);
+ _cl_motion_sense_sta_add(&cl_sta->motion_sense.rssi_data);
+ _cl_motion_sense_sta_add(&cl_sta->motion_sense.rssi_ba);
+}
+
+static void cl_motion_sense_rssi_handler(struct cl_hw *cl_hw,
+ struct cl_motion_rssi *motion_rssi,
+ s8 rssi[MAX_ANTENNAS])
+{
+ u8 i;
+
+ motion_rssi->cnt++;
+
+ for (i = 0; i < cl_hw->num_antennas; i++)
+ motion_rssi->sum[i] += rssi[i];
+}
+
+void cl_motion_sense_rssi_mgmt_ctl(struct cl_hw *cl_hw, struct cl_sta *cl_sta,
+ struct hw_rxhdr *rxhdr)
+{
+ /* RSSI of mgmt and ctl packets */
+ if (cl_hw->conf->ci_motion_sense_en) {
+ s8 rssi[MAX_ANTENNAS] = RX_HDR_RSSI(rxhdr);
+ u8 i;
+
+ if (!IS_REAL_PHY(cl_hw->chip))
+ for (i = 0; i < cl_hw->num_antennas; i++)
+ if (rssi[i] == 0)
+ return;
+
+ cl_motion_sense_rssi_handler(cl_hw, &cl_sta->motion_sense.rssi_mgmt_ctl, rssi);
+ }
+}
+
+void cl_motion_sense_rssi_data(struct cl_hw *cl_hw, struct cl_sta *cl_sta,
+ struct hw_rxhdr *rxhdr)
+{
+ /* RSSI of data packets */
+ s8 rssi[MAX_ANTENNAS] = RX_HDR_RSSI(rxhdr);
+
+ if (!cl_hw->conf->ci_motion_sense_en)
+ return;
+
+ if (!IS_REAL_PHY(cl_hw->chip) && rssi[0] == 0)
+ return;
+
+ cl_motion_sense_rssi_handler(cl_hw, &cl_sta->motion_sense.rssi_data, rssi);
+}
+
+void cl_motion_sense_rssi_ba(struct cl_hw *cl_hw, struct cl_sta *cl_sta, s8 rssi[MAX_ANTENNAS])
+{
+ /* RSSI of block-acks */
+ if (cl_hw->conf->ci_motion_sense_en)
+ cl_motion_sense_rssi_handler(cl_hw, &cl_sta->motion_sense.rssi_ba, rssi);
+}
+
+static s8 cl_motion_sense_calc_new_rssi(struct cl_hw *cl_hw, struct cl_motion_rssi *motion_rssi)
+{
+ u8 i = 0;
+ s8 rssi_avg[MAX_ANTENNAS] = {0};
+
+ /* Calculate average rssi */
+ for (i = 0; i < cl_hw->num_antennas; i++)
+ rssi_avg[i] = (s8)(motion_rssi->sum[i] / motion_rssi->cnt);
+
+ /* Reset rssi sum for next maintenance cycle */
+ memset(motion_rssi->sum, 0, sizeof(motion_rssi->sum));
+ motion_rssi->cnt = 0;
+
+ return cl_rssi_calc_equivalent(cl_hw, rssi_avg);
+}
+
+static void cl_motion_sense_state(struct cl_hw *cl_hw, struct cl_motion_rssi *motion_rssi,
+ u8 sta_idx, u8 min_history, const s8 *type)
+{
+ u8 i = 0;
+ s8 rssi_new = 0, rssi_old = 0;
+
+ if (motion_rssi->cnt == 0)
+ return;
+
+ /* Get new and old rssi */
+ rssi_new = cl_motion_sense_calc_new_rssi(cl_hw, motion_rssi);
+ rssi_old = motion_rssi->history[motion_rssi->idx];
+
+ /* Add new rssi to history and increase history index */
+ motion_rssi->history[motion_rssi->idx] = rssi_new;
+
+ motion_rssi->idx++;
+ if (motion_rssi->idx == MOTION_SENSE_SIZE)
+ motion_rssi->idx = 0;
+
+ /* Check if new rssi is max or min */
+ if (rssi_new > motion_rssi->max) {
+ motion_rssi->max = rssi_new;
+ goto out;
+ } else if (rssi_new < motion_rssi->min) {
+ motion_rssi->min = rssi_new;
+ goto out;
+ }
+
+ /*
+ * Check if old rssi was max or min.
+ * If so, go over history and find new max/min
+ */
+ if (rssi_old == motion_rssi->max) {
+ motion_rssi->max = S8_MIN;
+
+ for (i = 0; i < MOTION_SENSE_SIZE; i++) {
+ if (motion_rssi->history[i] == 0)
+ break;
+
+ if (motion_rssi->history[i] > motion_rssi->max)
+ motion_rssi->max = motion_rssi->history[i];
+ }
+ } else if (rssi_old == motion_rssi->min) {
+ motion_rssi->min = S8_MAX;
+
+ for (i = 0; i < MOTION_SENSE_SIZE; i++) {
+ if (motion_rssi->history[i] == 0)
+ break;
+
+ if (motion_rssi->history[i] < motion_rssi->min)
+ motion_rssi->min = motion_rssi->history[i];
+ }
+ }
+
+out:
+ /* Wait X second after connection, before making first decision */
+ if (motion_rssi->history[min_history] == 0)
+ return;
+
+ /* According to delta decide if station is STATIC or in MOTION */
+ if ((motion_rssi->max - motion_rssi->min) < cl_hw->conf->ci_motion_sense_rssi_thr) {
+ if (motion_rssi->state == STATE_STATIC)
+ return;
+
+ motion_rssi->state = STATE_STATIC;
+
+ motion_pr("%s - sta_idx=%u, min=%d, max=%d, state=STATIC\n",
+ type, sta_idx, motion_rssi->min, motion_rssi->max);
+ } else {
+ if (motion_rssi->state == STATE_MOVING)
+ return;
+
+ motion_rssi->state = STATE_MOVING;
+
+ motion_pr("%s - sta_idx=%u, min=%d, max=%d, state=MOVING\n",
+ type, sta_idx, motion_rssi->min, motion_rssi->max);
+ }
+}
+
+static void cl_motion_sense_moving(struct cl_hw *cl_hw, struct cl_sta *cl_sta,
+ struct cl_motion_sense *motion_sense)
+{
+ if (motion_sense->combined_state != STATE_MOVING) {
+ motion_sense->combined_state = STATE_MOVING;
+ motion_pr("sta_idx = %u, combined_state = MOVING\n",
+ cl_sta->sta_idx);
+ }
+}
+
+static void cl_motion_sense_static(struct cl_hw *cl_hw, struct cl_sta *cl_sta,
+ struct cl_motion_sense *motion_sense)
+{
+ if (motion_sense->combined_state != STATE_STATIC) {
+ motion_sense->combined_state = STATE_STATIC;
+ motion_pr("sta_idx = %u, combined_state = STATIC\n",
+ cl_sta->sta_idx);
+ }
+}
+
+static void cl_motion_sense_combined_state(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ struct cl_motion_sense *motion_sense = &cl_sta->motion_sense;
+
+ if (motion_sense->rssi_mgmt_ctl.history[MOTION_SENSE_MIN_DECISION_MGMT_CTL] == 0 &&
+ motion_sense->rssi_data.history[MOTION_SENSE_MIN_DECISION_DATA] == 0 &&
+ motion_sense->rssi_ba.history[MOTION_SENSE_MIN_DECISION_BA] == 0)
+ return;
+
+ if (motion_sense->rssi_mgmt_ctl.state == STATE_MOVING ||
+ motion_sense->rssi_data.state == STATE_MOVING ||
+ motion_sense->rssi_ba.state == STATE_MOVING)
+ cl_motion_sense_moving(cl_hw, cl_sta, motion_sense);
+ else
+ cl_motion_sense_static(cl_hw, cl_sta, motion_sense);
+}
+
+static void cl_motion_sense_maintenance_sta(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ u8 sta_idx = cl_sta->sta_idx;
+ struct cl_motion_sense *motion_sense = &cl_sta->motion_sense;
+
+ cl_motion_sense_state(cl_hw, &motion_sense->rssi_mgmt_ctl, sta_idx,
+ MOTION_SENSE_MIN_DECISION_MGMT_CTL, "mgmt/ctl");
+ cl_motion_sense_state(cl_hw, &motion_sense->rssi_data, sta_idx,
+ MOTION_SENSE_MIN_DECISION_DATA, "data");
+ cl_motion_sense_state(cl_hw, &motion_sense->rssi_ba, sta_idx,
+ MOTION_SENSE_MIN_DECISION_BA, "ba");
+
+ if (motion_sense->forced_state != STATE_NULL)
+ return;
+
+ cl_motion_sense_combined_state(cl_hw, cl_sta);
+}
+
+void cl_motion_sense_maintenance(struct cl_hw *cl_hw)
+{
+ cl_sta_loop(cl_hw, cl_motion_sense_maintenance_sta);
+}
+
+bool cl_motion_sense_is_static(struct cl_hw *cl_hw, struct cl_sta *cl_sta)
+{
+ return (cl_sta->motion_sense.combined_state == STATE_STATIC);
+}
+
--
2.36.1
