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 | 458 ++++++++++++++++++
1 file changed, 458 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..bd8b1c6df08e
--- /dev/null
+++ b/drivers/net/wireless/celeno/cl8k/motion_sense.c
@@ -0,0 +1,458 @@
+// SPDX-License-Identifier: MIT
+/* Copyright(c) 2019-2021, Celeno Communications Ltd. */
+
+#include "motion_sense.h"
+#include "rssi.h"
+#include "chip.h"
+
+#define MOTION_PRINT(...) \
+ do { \
+ if (cl_hw->motion_sense_dbg) \
+ pr_debug(__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
+
+#define MOTION_STATE_STR(state) \
+ (((state) == STATE_NULL) ? "NULL" : \
+ (((state) == STATE_MOVING) ? "MOVING" : "STATIC")) \
+
+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);
+
+ 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;
+
+ 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_PRINT("[MOTION_SENSE] %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_PRINT("[MOTION_SENSE] %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_dump(char **buf, int *len, ssize_t *buf_size,
+ struct cl_motion_rssi *motion_rssi,
+ const s8 *type)
+{
+ int delta = motion_rssi->max - motion_rssi->min;
+ int i;
+
+ cl_snprintf(buf, len, buf_size, "\n");
+ cl_snprintf(buf, len, buf_size, "type = %s\n", type);
+ cl_snprintf(buf, len, buf_size,
+ "state = %s\n", MOTION_STATE_STR(motion_rssi->state));
+ cl_snprintf(buf, len, buf_size,
+ "min = %d\n", motion_rssi->min);
+ cl_snprintf(buf, len, buf_size,
+ "max = %d\n", motion_rssi->max);
+ cl_snprintf(buf, len, buf_size,
+ "delta = %d\n", delta);
+ cl_snprintf(buf, len, buf_size,
+ "idx = %u\n", motion_rssi->idx);
+
+ for (i = 0; i < MOTION_SENSE_SIZE; i++) {
+ if (motion_rssi->history[i])
+ cl_snprintf(buf, len, buf_size,
+ "%2i) = %3d, ", i, motion_rssi->history[i]);
+ else
+ break;
+
+ if ((i % 8) == 7)
+ cl_snprintf(buf, len, buf_size, "\n");
+ }
+
+ cl_snprintf(buf, len, buf_size, "\n");
+}
+
+static int cl_motion_sense_dump(struct cl_hw *cl_hw, u8 sta_idx)
+{
+ struct cl_sta *cl_sta = NULL;
+ struct cl_motion_sense *motion_sense = NULL;
+ char *buf = NULL;
+ ssize_t buf_size;
+ int err = 0;
+ int len = 0;
+
+ cl_sta_lock_bh(cl_hw);
+ cl_sta = cl_sta_get(cl_hw, sta_idx);
+
+ if (!cl_sta) {
+ pr_err("[MS] Invalid sta_idx = %u\n", sta_idx);
+ goto out;
+ }
+
+ motion_sense = &cl_sta->motion_sense;
+
+ cl_snprintf(&buf, &len, &buf_size,
+ "sta_idx = %u\n", sta_idx);
+
+ if (motion_sense->forced_state != STATE_NULL) {
+ cl_snprintf(&buf, &len, &buf_size,
+ "forced_state = %s\n",
+ MOTION_STATE_STR(motion_sense->forced_state));
+ goto out;
+ }
+
+ cl_snprintf(&buf, &len, &buf_size,
+ "combined_state = %s\n",
+ MOTION_STATE_STR(motion_sense->combined_state));
+
+ _cl_motion_sense_dump(&buf, &len, &buf_size, &motion_sense->rssi_mgmt_ctl, "mgmt/ctl");
+ _cl_motion_sense_dump(&buf, &len, &buf_size, &motion_sense->rssi_ba, "ba");
+ _cl_motion_sense_dump(&buf, &len, &buf_size, &motion_sense->rssi_data, "data");
+
+out:
+ cl_sta_unlock_bh(cl_hw);
+
+ err = cl_vendor_reply(cl_hw, buf, len);
+ kfree(buf);
+
+ return err;
+}
+
+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_PRINT("[MOTION_SENSE] 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_PRINT("[MOTION_SENSE] 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);
+}
+
+static void cl_motion_sense_force_state(struct cl_hw *cl_hw, u8 sta_idx, u8 state)
+{
+ struct cl_sta *cl_sta = NULL;
+ struct cl_motion_sense *motion_sense = NULL;
+
+ cl_sta_lock_bh(cl_hw);
+ cl_sta = cl_sta_get(cl_hw, sta_idx);
+
+ if (!cl_sta) {
+ pr_err("[MS] Invalid station (%u)\n", sta_idx);
+ goto out;
+ }
+
+ motion_sense = &cl_sta->motion_sense;
+
+ switch (state) {
+ case STATE_NULL:
+ pr_debug("[MS] Disable force state\n");
+ break;
+ case STATE_MOVING:
+ pr_debug("[MS] Force state - MOVING\n");
+ cl_motion_sense_moving(cl_hw, cl_sta, motion_sense);
+ break;
+ case STATE_STATIC:
+ pr_debug("[MS] Force state - STATIC\n");
+ cl_motion_sense_static(cl_hw, cl_sta, motion_sense);
+ break;
+ default:
+ pr_warn("[MS] Invalid state (%u)\n", state);
+ goto out;
+ }
+
+ motion_sense->forced_state = state;
+
+out:
+ cl_sta_unlock_bh(cl_hw);
+}
+
+static int cl_motion_sense_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,
+ "motion usage:\n"
+ "-d: Set debug [0/1]\n"
+ "-e: Set enable [0/1]\n"
+ "-f: Force state [sta_idx].[0-null, 1-moving, 2-static]\n"
+ "-i: Dump info [sta_idx]\n"
+ "-r: Set rssi threshold [rssi]\n");
+
+ err = cl_vendor_reply(cl_hw, buf, strlen(buf));
+ kfree(buf);
+
+ return err;
+}
+
+int cl_motion_sense_cli(struct cl_hw *cl_hw, struct cli_params *cli_params)
+{
+ u32 expected_params = 0;
+ bool set_debug = false;
+ bool set_enable = false;
+ bool force_state = false;
+ bool dump_info = false;
+ bool set_rssi_thr = false;
+
+ switch (cli_params->option) {
+ case 'd':
+ set_debug = true;
+ expected_params = 1;
+ break;
+ case 'e':
+ set_enable = true;
+ expected_params = 1;
+ break;
+ case 'f':
+ force_state = true;
+ expected_params = 2;
+ break;
+ case 'i':
+ dump_info = true;
+ expected_params = 1;
+ break;
+ case 'r':
+ set_rssi_thr = true;
+ expected_params = 1;
+ break;
+ case '?':
+ return cl_motion_sense_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_debug) {
+ cl_hw->motion_sense_dbg = (bool)cli_params->params[0];
+ pr_debug("[MS] debug = %u\n", cl_hw->motion_sense_dbg);
+ return 0;
+ }
+
+ if (dump_info) {
+ u8 sta_idx = (u8)cli_params->params[0];
+
+ return cl_motion_sense_dump(cl_hw, sta_idx);
+ }
+
+ if (set_enable) {
+ cl_hw->conf->ci_motion_sense_en = (bool)cli_params->params[0];
+ pr_debug("[MS] ci_motion_sense_en = %s\n",
+ cl_hw->conf->ci_motion_sense_en ? "true" : "false");
+ return 0;
+ }
+
+ if (force_state) {
+ u8 sta_idx = (u8)cli_params->params[0];
+ u8 state = (u8)cli_params->params[1];
+
+ cl_motion_sense_force_state(cl_hw, sta_idx, state);
+ return 0;
+ }
+
+ if (set_rssi_thr) {
+ cl_hw->conf->ci_motion_sense_rssi_thr = (s8)cli_params->params[0];
+ pr_debug("[MS] ci_motion_sense_rssi_thr = %d\n",
+ cl_hw->conf->ci_motion_sense_rssi_thr);
+ return 0;
+ }
+
+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.
________________________________
