driver: bmi: Enable On/Off body detection on all BMI IMU

Define a common function for setting the rms noise floor.
Use static function per sensor to set the proper floor.

BUG=b:236799854
BRANCH=dedede
TEST=Compile

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

6 files changed, 36 insertions, 32 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 18ed0ed0a4..2de7381399 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -365,6 +365,13 @@ static int manage_activity(const struct motion_sensor_t *s,
 }
 #endif
 
+#ifdef CONFIG_BODY_DETECTION
+static int get_rms_noise(const struct motion_sensor_t *s)
+{
+	return bmi_get_rms_noise(s, BMI160_ACCEL_RMS_NOISE_100HZ);
+}
+#endif
+
 /** Requires that the passed sensor `*s` is an accelerometer */
 static __maybe_unused int
 config_accel_interrupt(const struct motion_sensor_t *s)
@@ -718,7 +725,7 @@ const struct accelgyro_drv bmi160_drv = {
 	.list_activities = bmi_list_activities,
 #endif
 #ifdef CONFIG_BODY_DETECTION
-	.get_rms_noise = bmi_get_rms_noise,
+	.get_rms_noise = get_rms_noise,
 #endif
 };
 
diff --git a/driver/accelgyro_bmi260.c b/driver/accelgyro_bmi260.c
index db1ac41fca..cc19355d24 100644
--- a/driver/accelgyro_bmi260.c
+++ b/driver/accelgyro_bmi260.c
@@ -166,6 +166,13 @@ static int set_offset(const struct motion_sensor_t *s, const int16_t *offset,
 	return ret;
 }
 
+#ifdef CONFIG_BODY_DETECTION
+static int get_rms_noise(const struct motion_sensor_t *s)
+{
+	return bmi_get_rms_noise(s, BMI260_ACCEL_RMS_NOISE_100HZ);
+}
+#endif
+
 static int wait_and_read_data(const struct motion_sensor_t *s, intv3_t v,
 			      int try_cnt, int msec)
 {
@@ -595,7 +602,7 @@ const struct accelgyro_drv bmi260_drv = {
 	.list_activities = bmi_list_activities,
 #endif
 #ifdef CONFIG_BODY_DETECTION
-	.get_rms_noise = bmi_get_rms_noise,
+	.get_rms_noise = get_rms_noise,
 #endif
 };
 
diff --git a/driver/accelgyro_bmi3xx.c b/driver/accelgyro_bmi3xx.c
index 4fc893c1ee..091cf5e2a8 100644
--- a/driver/accelgyro_bmi3xx.c
+++ b/driver/accelgyro_bmi3xx.c
@@ -711,9 +711,9 @@ static int set_offset(const struct motion_sensor_t *s, const int16_t *offset,
 }
 
 #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)
 {
-	return EC_ERROR_UNIMPLEMENTED;
+	return bmi_get_rms_noise(s, BMI3_ACCEL_RMS_NOISE_100HZ);
 }
 #endif
 
diff --git a/driver/accelgyro_bmi3xx.h b/driver/accelgyro_bmi3xx.h
index eb46d3bc9a..b5395cbce3 100644
--- a/driver/accelgyro_bmi3xx.h
+++ b/driver/accelgyro_bmi3xx.h
@@ -214,6 +214,9 @@
  */
 #define BMI3_FIFO_FILL_LVL_MASK 0x07FF
 
+/* Root mean square noise of 100 Hz accelerometer, units: ug */
+#define BMI3_ACCEL_RMS_NOISE_100HZ 1200
+
 /* Enum to define interrupt lines */
 enum bmi3_hw_int_pin {
 	BMI3_INT_NONE,
diff --git a/driver/accelgyro_bmi_common.c b/driver/accelgyro_bmi_common.c
index 7ab202d515..7fb5ff1e51 100644
--- a/driver/accelgyro_bmi_common.c
+++ b/driver/accelgyro_bmi_common.c
@@ -624,30 +624,21 @@ int bmi_get_offset(const struct motion_sensor_t *s, int16_t *offset,
 }
 
 #ifdef CONFIG_BODY_DETECTION
-int bmi_get_rms_noise(const struct motion_sensor_t *s)
+int bmi_get_rms_noise(const struct motion_sensor_t *accel,
+		      int rms_noise_100hz_mg)
 {
-	int ret;
-	fp_t noise_100hz, rate, sqrt_rate_ratio;
+	fp_t rate, sqrt_rate_ratio;
 
-	switch (s->type) {
-	case MOTIONSENSE_TYPE_ACCEL:
-		/* change unit of ODR to Hz to prevent INT_TO_FP() overflow */
-		rate = INT_TO_FP(bmi_get_data_rate(s) / 1000);
-		/*
-		 * Since the noise is proportional to sqrt(ODR) in BMI, and we
-		 * have rms noise in 100 Hz, we multiply it with the sqrt(ratio
-		 * of ODR to 100Hz) to get current noise.
-		 */
-		noise_100hz = INT_TO_FP(BMI_ACCEL_RMS_NOISE_100HZ(V(s)));
-		sqrt_rate_ratio =
-			fp_sqrtf(fp_div(rate, INT_TO_FP(BMI_ACCEL_100HZ)));
-		ret = FP_TO_INT(fp_mul(noise_100hz, sqrt_rate_ratio));
-		break;
-	default:
-		CPRINTS("%s with gyro/mag is not implemented", __func__);
-		return 0;
-	}
-	return ret;
+	/* change unit of ODR to Hz to prevent INT_TO_FP() overflow */
+	rate = INT_TO_FP(bmi_get_data_rate(accel) / 1000);
+	/*
+	 * Since the noise is proportional to sqrt(ODR) in BMI, and we
+	 * have rms noise in 100 Hz, we multiply it with the sqrt(ratio
+	 * of ODR to 100Hz) to get current noise.
+	 */
+	sqrt_rate_ratio = fp_sqrtf(fp_div(rate, INT_TO_FP(BMI_ACCEL_100HZ)));
+	return FP_TO_INT(
+		fp_mul(INT_TO_FP(rms_noise_100hz_mg), sqrt_rate_ratio));
 }
 #endif
 
diff --git a/include/driver/accelgyro_bmi_common.h b/include/driver/accelgyro_bmi_common.h
index 5647d8776d..bc82e8aba2 100644
--- a/include/driver/accelgyro_bmi_common.h
+++ b/include/driver/accelgyro_bmi_common.h
@@ -131,9 +131,6 @@ enum bmi_running_mode {
 	(BMI160_CMD_REG + (v) * (BMI260_CMD_REG - BMI160_CMD_REG))
 #define BMI_CMD_FIFO_FLUSH 0xb0
 
-#define BMI_ACCEL_RMS_NOISE_100HZ(v)    \
-	(BMI160_ACCEL_RMS_NOISE_100HZ + \
-	 (v) * (BMI260_ACCEL_RMS_NOISE_100HZ - BMI160_ACCEL_RMS_NOISE_100HZ))
 #define BMI_ACCEL_100HZ 100
 
 /*
@@ -272,9 +269,8 @@ int bmi_get_offset(const struct motion_sensor_t *s, int16_t *offset,
 
 int bmi_get_resolution(const struct motion_sensor_t *s);
 
-#ifdef CONFIG_BODY_DETECTION
-int bmi_get_rms_noise(const struct motion_sensor_t *s);
-#endif
+int bmi_get_rms_noise(const struct motion_sensor_t *accel,
+		      int rms_noise_100hz_mg);
 
 int bmi_set_scale(const struct motion_sensor_t *s, const uint16_t *scale,
 		  int16_t temp);