new file mode 100644
@@ -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;
+}