diff mbox series

[RFC,v2,46/96] cl8k: add motion_sense.c

Message ID 20220524113502.1094459-47-viktor.barna@celeno.com
State New
Headers show
Series wireless: cl8k driver for Celeno IEEE 802.11ax devices | expand

Commit Message

Viktor Barna May 24, 2022, 11:34 a.m. UTC 
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>
---
 .../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 mbox series

Patch

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);
+}
+