driver: st: Enable on/off body detection to lsm6dsm

Remove simple macro in lsm6dso and move the code
to report activities to common code.

BUG=b:236799854
BRANCH=dedede
TEST=Compile. Check on bugzzy the EC report on/off body detection.

Change-Id: I7a1713182b4ee7807a2f681a70e664d0f4eae563
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3860189
Reviewed-by: Diana Z <dzigterman@chromium.org>

5 files changed, 50 insertions, 18 deletions

diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 1c46c275bc..2663224658 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -726,6 +726,17 @@ static int read_temp(const struct motion_sensor_t *s, int *temp)
 	return EC_SUCCESS;
 }
 
+#ifdef CONFIG_BODY_DETECTION
+static int get_rms_noise(const struct motion_sensor_t *s)
+{
+	/*
+	 * RMS | Acceleration RMS noise in normal/low-power mode
+	 * FS = ±4 g | 2.0 mg(RMS)
+	 */
+	return 2000;
+}
+#endif
+
 const struct accelgyro_drv lsm6dsm_drv = {
 	.init = init,
 	.read = read,
@@ -739,4 +750,10 @@ const struct accelgyro_drv lsm6dsm_drv = {
 #ifdef ACCEL_LSM6DSM_INT_ENABLE
 	.irq_handler = irq_handler,
 #endif /* ACCEL_LSM6DSM_INT_ENABLE */
+#ifdef CONFIG_BODY_DETECTION
+	.get_rms_noise = get_rms_noise,
+#endif
+#ifdef CONFIG_GESTURE_HOST_DETECTION
+	.list_activities = st_list_activities,
+#endif
 };
diff --git a/driver/accelgyro_lsm6dso.c b/driver/accelgyro_lsm6dso.c
index 609583723b..0bc7009060 100644
--- a/driver/accelgyro_lsm6dso.c
+++ b/driver/accelgyro_lsm6dso.c
@@ -187,7 +187,7 @@ static inline int load_fifo(struct motion_sensor_t *main_s,
 static int accelgyro_config_fifo(const struct motion_sensor_t *s)
 {
 	int err;
-	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
+	struct stprivate_data *data = s->drv_data;
 	uint8_t reg_val;
 	uint8_t fifo_odr_mask;
 
@@ -311,7 +311,7 @@ static int set_range(struct motion_sensor_t *s, int range, int rnd)
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
 	int ret, normalized_rate = 0;
-	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
+	struct stprivate_data *data = s->drv_data;
 	uint8_t ctrl_reg, reg_val = 0;
 
 	ctrl_reg = LSM6DSO_ODR_REG(s->type);
@@ -406,7 +406,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 static int init(struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
-	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
+	struct stprivate_data *data = s->drv_data;
 
 	ret = st_raw_read8(s->port, s->i2c_spi_addr_flags, LSM6DSO_WHO_AM_I_REG,
 			   &tmp);
@@ -468,7 +468,7 @@ err_unlock:
 }
 
 #ifdef CONFIG_BODY_DETECTION
-int get_rms_noise(const struct motion_sensor_t *s)
+static int get_rms_noise(const struct motion_sensor_t *s)
 {
 	/*
 	 * RMS | Acceleration RMS noise in normal/low-power mode
@@ -478,17 +478,6 @@ int get_rms_noise(const struct motion_sensor_t *s)
 }
 #endif
 
-#ifdef CONFIG_GESTURE_HOST_DETECTION
-int lsm_list_activities(const struct motion_sensor_t *s, uint32_t *enabled,
-			uint32_t *disabled)
-{
-	struct stprivate_data *data = LSM6DSO_GET_DATA(s);
-	*enabled = data->enabled_activities;
-	*disabled = data->disabled_activities;
-	return EC_RES_SUCCESS;
-}
-#endif /* CONFIG_GESTURE_HOST_DETECTION */
-
 const struct accelgyro_drv lsm6dso_drv = {
 	.init = init,
 	.read = read,
@@ -504,7 +493,7 @@ const struct accelgyro_drv lsm6dso_drv = {
 	.get_rms_noise = get_rms_noise,
 #endif
 #ifdef CONFIG_GESTURE_HOST_DETECTION
-	.list_activities = lsm_list_activities,
+	.list_activities = st_list_activities,
 #endif
 #endif /* ACCEL_LSM6DSO_INT_ENABLE */
 };
diff --git a/driver/accelgyro_lsm6dso.h b/driver/accelgyro_lsm6dso.h
index 1730715665..f50b4b40d8 100644
--- a/driver/accelgyro_lsm6dso.h
+++ b/driver/accelgyro_lsm6dso.h
@@ -199,8 +199,6 @@ struct lsm6dso_data {
  */
 #define LSM6DSO_DISCARD_SAMPLES 3
 
-#define LSM6DSO_GET_DATA(_s) ((struct stprivate_data *)((_s)->drv_data))
-
 /* Macro to initialize motion_sensors structure */
 #define LSM6DSO_ST_DATA(g, type) (&((g).st_data[type]))
 
diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c
index 61de0dd92a..c5fa9a2cca 100644
--- a/driver/stm_mems_common.c
+++ b/driver/stm_mems_common.c
@@ -136,3 +136,20 @@ void st_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *data)
 	for (i = X; i <= Z; i++)
 		v[i] += (drvdata->offset[i] << 5) / s->current_range;
 }
+
+#ifdef CONFIG_GESTURE_HOST_DETECTION
+/**
+ * st_list_activities - Apply to LSB data sensitivity and rotation
+ * @s: Motion sensor pointer
+ * @enabled: Activities that are enabled (bitmap)
+ * @disabled: Activities that are disabled (bitmap)
+ */
+int st_list_activities(const struct motion_sensor_t *s, uint32_t *enabled,
+		       uint32_t *disabled)
+{
+	struct stprivate_data *data = s->drv_data;
+	*enabled = data->enabled_activities;
+	*disabled = data->disabled_activities;
+	return EC_RES_SUCCESS;
+}
+#endif /* CONFIG_GESTURE_HOST_DETECTION */
diff --git a/driver/stm_mems_common.h b/driver/stm_mems_common.h
index 75377cabbf..7191b368ac 100644
--- a/driver/stm_mems_common.h
+++ b/driver/stm_mems_common.h
@@ -110,11 +110,22 @@ int st_get_data_rate(const struct motion_sensor_t *s);
  */
 void st_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *data);
 
+/**
+ * st_list_activities - Apply to LSB data sensitivity and rotation
+ * @s: Motion sensor pointer
+ * @enabled: Activities that are enabled (bitmap)
+ * @disabled: Activities that are disabled (bitmap)
+ */
+int st_list_activities(const struct motion_sensor_t *s, uint32_t *enabled,
+		       uint32_t *disabled);
+
 /* Internal data structure for sensors */
 struct stprivate_data {
 	struct accelgyro_saved_data_t base;
+#ifdef CONFIG_GESTURE_HOST_DETECTION
 	uint8_t enabled_activities;
 	uint8_t disabled_activities;
+#endif /* CONFIG_GESTURE_HOST_DETECTION */
 	int16_t offset[3];
 	uint8_t resol;
 };