driver/bmi323: Update calibration code

First upload of code from Bosh to simplify IMU calibration.

BUG=b:240404661
TEST=in progress
BRANCH=None

Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Change-Id: I20c479e7ad54ce6054ecf43e0655e7a9314d6879
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3807165
Reviewed-by: HUANG Leo <leo.huang@bosch.corp-partner.google.com>
Tested-by: Mike Lee <mike5@huaqin.corp-partner.google.com>
Commit-Queue: Knox Chiou <knoxchiou@chromium.org>
Tested-by: HUANG Leo <leo.huang@bosch.corp-partner.google.com>

2 files changed, 43 insertions, 228 deletions

diff --git a/driver/accelgyro_bmi3xx.c b/driver/accelgyro_bmi3xx.c
index cda5623daa..4fc893c1ee 100644
--- a/driver/accelgyro_bmi3xx.c
+++ b/driver/accelgyro_bmi3xx.c
@@ -382,57 +382,13 @@ static int read_temp(const struct motion_sensor_t *s, int *temp_ptr)
 	return EC_ERROR_UNIMPLEMENTED;
 }
 
-static int poll_offset(const struct motion_sensor_t *s, uint8_t *reg_data)
-{
-	/* Delay time for offset update */
-	for (int i = 0; i < OFFSET_UPDATE_DELAY; i += OFFSET_UPDATE_PER_TRY) {
-		msleep(OFFSET_UPDATE_PER_TRY);
-
-		/* Read the configuration from the feature engine register */
-		RETURN_ERROR(bmi3_read_n(s, BMI3_FEATURE_IO_1, reg_data, 4));
-
-		if ((reg_data[3] & BMI3_UGAIN_OFFS_UPD_COMPLETE) &&
-		    ((reg_data[2] & BMI3_FEATURE_IO_1_ERROR_MASK) ==
-		     BMI3_FEATURE_IO_1_NO_ERROR)) {
-			return EC_SUCCESS;
-		}
-	}
-	return EC_ERROR_NOT_CALIBRATED;
-}
-
-static int reset_offset(const struct motion_sensor_t *s, uint8_t offset_en)
-{
-	uint8_t offset_sel[2] = { BMI3_REG_UGAIN_OFF_SEL, 0 };
-	uint8_t reg_data[4] = { 0 };
-
-	/* Reset the existing offset values by setting the bits in DMA*/
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, offset_sel, 2));
-
-	reg_data[0] = offset_en;
-	reg_data[1] = 0;
-
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, 2));
-
-	/* Update the offset change to the sensor engine */
-	reg_data[0] =
-		(uint8_t)(BMI3_CMD_USR_GAIN_OFFS_UPDATE & BMI3_SET_LOW_BYTE);
-	reg_data[1] = (uint8_t)((BMI3_CMD_USR_GAIN_OFFS_UPDATE &
-				 BMI3_SET_HIGH_BYTE) >>
-				8);
-	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2));
-
-	return poll_offset(s, reg_data);
-}
-
 int get_gyro_offset(const struct motion_sensor_t *s, intv3_t v)
 {
 	int i;
 	uint8_t reg_data[14] = { 0 };
 
 	/* Get the accel offset values */
-	RETURN_ERROR(bmi3_read_n(s, GYR_DP_OFF_X, reg_data, 14));
+	RETURN_ERROR(bmi3_read_n(s, BMI3_GYR_DP_OFF_X, reg_data, 14));
 
 	v[0] = ((uint16_t)(reg_data[3] << 8) | reg_data[2]) & 0x03FF;
 	v[1] = ((uint16_t)(reg_data[7] << 8) | reg_data[6]) & 0x03FF;
@@ -452,42 +408,23 @@ int get_gyro_offset(const struct motion_sensor_t *s, intv3_t v)
 static int write_gyro_offset(const struct motion_sensor_t *s, int *val)
 {
 	uint8_t reg_data[6] = { 0 };
-	uint8_t base_addr[2] = { BMI3_GYRO_OFFSET_ADDR, 0 };
-	uint8_t offset_sel[2] = { BMI3_REG_UGAIN_OFF_SEL, 0 };
-
-	/* Enable user gain/offset update*/
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, offset_sel, 2));
-	reg_data[0] = 0;
-	reg_data[1] = 0;
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, 2));
-	/*
-	 * Set the user gyro offset base address to feature engine
-	 * transmission address to start DMA transaction
-	 */
-	RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, base_addr, 2));
 
+	/* x-axis offset */
 	reg_data[0] = (uint8_t)(val[0] & BMI3_SET_LOW_BYTE);
 	reg_data[1] = (uint8_t)((val[0] & 0x0300) >> 8);
+	/* y-axis offset */
 	reg_data[2] = (uint8_t)(val[1] & BMI3_SET_LOW_BYTE);
 	reg_data[3] = (uint8_t)((val[1] & 0x0300) >> 8);
+	/* z-axis offset */
 	reg_data[4] = (uint8_t)(val[2] & BMI3_SET_LOW_BYTE);
 	reg_data[5] = (uint8_t)((val[2] & 0x0300) >> 8);
 
-	/* Set the configuration to the feature engine register */
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, 6));
-
-	/* Update the offset to the sensor engine */
-	reg_data[0] =
-		(uint8_t)(BMI3_CMD_USR_GAIN_OFFS_UPDATE & BMI3_SET_LOW_BYTE);
-	reg_data[1] = (uint8_t)((BMI3_CMD_USR_GAIN_OFFS_UPDATE &
-				 BMI3_SET_HIGH_BYTE) >>
-				8);
-	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2));
+	/* Update the gyro data path offset registers */
+	RETURN_ERROR(bmi3_write_n(s, BMI3_GYR_DP_OFF_X, &reg_data[0], 2));
+	RETURN_ERROR(bmi3_write_n(s, BMI3_GYR_DP_OFF_Y, &reg_data[2], 2));
+	RETURN_ERROR(bmi3_write_n(s, BMI3_GYR_DP_OFF_Z, &reg_data[4], 2));
 
-	return poll_offset(s, reg_data);
+	return EC_SUCCESS;
 }
 
 int set_gyro_offset(const struct motion_sensor_t *s, intv3_t v)
@@ -498,7 +435,7 @@ int set_gyro_offset(const struct motion_sensor_t *s, intv3_t v)
 
 	for (i = X; i <= Z; ++i) {
 		val[i] = round_divide((int64_t)v[i] * BMI_OFFSET_GYRO_DIV_MDS,
-				      BMI_OFFSET_GYRO_MULTI_MDS);
+				      BMI3_OFFSET_GYR_MDPS);
 		if (val[i] > 511)
 			val[i] = 511;
 		if (val[i] < -512)
@@ -517,9 +454,6 @@ int set_gyro_offset(const struct motion_sensor_t *s, intv3_t v)
 	reg_data[3] = 0x00;
 	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_ACC_CONF, reg_data, 4));
 
-	/* Reset the existing offset values */
-	RETURN_ERROR(reset_offset(s, 2));
-
 	/* Set the gyro offset in the sensor registers */
 	RETURN_ERROR(write_gyro_offset(s, val));
 
@@ -535,15 +469,15 @@ int get_accel_offset(const struct motion_sensor_t *s, intv3_t v)
 	uint8_t reg_data[14] = { 0 };
 
 	/* Get the accel offset values from user registers */
-	RETURN_ERROR(bmi3_read_n(s, ACC_DP_OFF_X, reg_data, 14));
+	RETURN_ERROR(bmi3_read_n(s, BMI3_ACC_DP_OFF_X, reg_data, 14));
 
-	v[0] = ((uint16_t)(reg_data[3] << 8) | reg_data[2]) & 0x1FFF;
-	v[1] = ((uint16_t)(reg_data[7] << 8) | reg_data[6]) & 0x1FFF;
-	v[2] = ((uint16_t)(reg_data[11] << 8) | reg_data[10]) & 0x1FFF;
+	v[0] = ((uint16_t)(reg_data[3] << 8) | reg_data[2]) & 0x3FFF;
+	v[1] = ((uint16_t)(reg_data[7] << 8) | reg_data[6]) & 0x3FFF;
+	v[2] = ((uint16_t)(reg_data[11] << 8) | reg_data[10]) & 0x3FFF;
 
 	for (i = X; i <= Z; ++i) {
-		if (v[i] > 0x0FFF)
-			v[i] = -8192 + v[i];
+		if (v[i] > 0x1FFF)
+			v[i] = -16384 + v[i];
 
 		v[i] = round_divide((int64_t)v[i] * BMI3_OFFSET_ACC_MULTI_MG,
 				    BMI_OFFSET_ACC_DIV_MG);
@@ -554,49 +488,27 @@ int get_accel_offset(const struct motion_sensor_t *s, intv3_t v)
 
 static int write_accel_offsets(const struct motion_sensor_t *s, int *val)
 {
-	uint8_t base_addr[2] = { BMI3_ACC_OFFSET_ADDR, 0 };
-	uint8_t offset_sel[2] = { BMI3_REG_UGAIN_OFF_SEL, 0 };
 	uint8_t reg_data[6] = { 0 };
 
-	/* Enable user gain/offset update*/
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, offset_sel, 2));
-	reg_data[0] = 0;
-	reg_data[1] = 0;
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, 2));
-	/*
-	 * Set the user accel offset base address to feature engine
-	 * transmission address to start DMA transaction
-	 */
-	RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX, base_addr, 2));
-
+	/* x-axis offset */
 	reg_data[0] = (uint8_t)(val[0] & BMI3_SET_LOW_BYTE);
-	reg_data[1] = (uint8_t)((val[0] & 0x1F00) >> 8);
+	reg_data[1] = (uint8_t)((val[0] & 0x3F00) >> 8);
+	/* y-axis offset */
 	reg_data[2] = (uint8_t)(val[1] & BMI3_SET_LOW_BYTE);
-	reg_data[3] = (uint8_t)((val[1] & 0x1F00) >> 8);
+	reg_data[3] = (uint8_t)((val[1] & 0x3F00) >> 8);
+	/* z-axis offset */
 	reg_data[4] = (uint8_t)(val[2] & BMI3_SET_LOW_BYTE);
-	reg_data[5] = (uint8_t)((val[2] & 0x1F00) >> 8);
-
-	/* Set the configuration to the feature engine register */
-	RETURN_ERROR(
-		bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data, 6));
-
-	/* Update the offset to the sensor engine */
-	reg_data[0] =
-		(uint8_t)(BMI3_CMD_USR_GAIN_OFFS_UPDATE & BMI3_SET_LOW_BYTE);
+	reg_data[5] = (uint8_t)((val[2] & 0x3F00) >> 8);
 
-	reg_data[1] = (uint8_t)((BMI3_CMD_USR_GAIN_OFFS_UPDATE &
-				 BMI3_SET_HIGH_BYTE) >>
-				8);
+	/* Update the acc data path offset registers */
+	RETURN_ERROR(bmi3_write_n(s, BMI3_ACC_DP_OFF_X, &reg_data[0], 2));
+	RETURN_ERROR(bmi3_write_n(s, BMI3_ACC_DP_OFF_Y, &reg_data[2], 2));
+	RETURN_ERROR(bmi3_write_n(s, BMI3_ACC_DP_OFF_Z, &reg_data[4], 2));
 
-	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2));
-
-	return poll_offset(s, reg_data);
+	return EC_SUCCESS;
 }
 
-int set_accel_offset(const struct motion_sensor_t *s, intv3_t v,
-		     uint8_t reset_en)
+int set_accel_offset(const struct motion_sensor_t *s, intv3_t v)
 {
 	uint8_t reg_data[4] = { 0 };
 	uint8_t saved_conf[6] = { 0 };
@@ -605,12 +517,12 @@ int set_accel_offset(const struct motion_sensor_t *s, intv3_t v,
 	for (i = X; i <= Z; ++i) {
 		val[i] = round_divide((int64_t)v[i] * BMI_OFFSET_ACC_DIV_MG,
 				      BMI3_OFFSET_ACC_MULTI_MG);
-		if (val[i] > 4095)
-			val[i] = 4095;
-		if (val[i] < -4096)
-			val[i] = -4096;
+		if (val[i] > 8191)
+			val[i] = 8191;
+		if (val[i] < -8192)
+			val[i] = -8192;
 		if (val[i] < 0)
-			val[i] += 8192;
+			val[i] += 16384;
 	}
 
 	/* Set the power mode as suspend */
@@ -623,12 +535,6 @@ int set_accel_offset(const struct motion_sensor_t *s, intv3_t v,
 	reg_data[3] = 0x00;
 	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_ACC_CONF, reg_data, 4));
 
-	/* Reset the existing offset values */
-	if (reset_en) {
-		/* Reset is only done for writing offset and not for FOC */
-		RETURN_ERROR(reset_offset(s, 1));
-	}
-
 	/* Set the accel offset in the sensor registers */
 	RETURN_ERROR(write_accel_offsets(s, val));
 
@@ -638,80 +544,6 @@ int set_accel_offset(const struct motion_sensor_t *s, intv3_t v,
 	return EC_SUCCESS;
 }
 
-static int wait_and_read_data(const struct motion_sensor_t *s,
-			      intv3_t accel_data)
-{
-	uint8_t reg_data[8] = { 0 };
-
-	/* Retry 5 times */
-	uint8_t try_cnt = FOC_TRY_COUNT;
-
-	/* Check if data is ready */
-	while (try_cnt && (!(reg_data[2] & BMI3_STAT_DATA_RDY_ACCEL_MSK))) {
-		/* 20ms delay for 50Hz ODR */
-		msleep(FOC_DELAY);
-
-		/* Read the status register */
-		RETURN_ERROR(bmi3_read_n(s, BMI3_REG_STATUS, reg_data, 4));
-		try_cnt--;
-	}
-
-	if (!(reg_data[2] & BMI3_STAT_DATA_RDY_ACCEL_MSK))
-		return EC_ERROR_TIMEOUT;
-
-	/* Read the sensor data */
-	RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_DATA_X, reg_data, 8));
-
-	accel_data[0] = ((int16_t)((reg_data[3] << 8) | reg_data[2]));
-	accel_data[1] = ((int16_t)((reg_data[5] << 8) | reg_data[4]));
-	accel_data[2] = ((int16_t)((reg_data[7] << 8) | reg_data[6]));
-
-	rotate(accel_data, *s->rot_standard_ref, accel_data);
-
-	return EC_SUCCESS;
-}
-
-/*!
- * @brief This internal API performs Fast Offset Compensation for accelerometer.
- */
-static int8_t perform_accel_foc(struct motion_sensor_t *s, int *target,
-				int sens_range)
-{
-	intv3_t accel_data, offset;
-	int32_t delta_value[3] = { 0, 0, 0 };
-
-	/* Variable to define count */
-	uint8_t i, loop, sample_count = 0;
-
-	for (loop = 0; loop < BMI3_FOC_SAMPLE_LIMIT; loop++) {
-		RETURN_ERROR(wait_and_read_data(s, accel_data));
-
-		sample_count++;
-
-		/* Store the data in a temporary structure */
-		delta_value[0] += accel_data[0] - target[X];
-		delta_value[1] += accel_data[1] - target[Y];
-		delta_value[2] += accel_data[2] - target[Z];
-	}
-
-	/* The data is in LSB so -> [(LSB)*1000*range/2^15] (mdps | mg) */
-	for (i = X; i <= Z; ++i) {
-		offset[i] = (((int64_t)(delta_value[i] * 1000 * sens_range /
-					sample_count) >>
-			      15) *
-			     -1);
-	}
-
-	rotate_inv(offset, *s->rot_standard_ref, offset);
-
-	/* Set accel offset without resetting the existing offsets
-	 * since we calculated the bias with the existing offsets
-	 */
-	RETURN_ERROR(set_accel_offset(s, offset, BMI3_DISABLE));
-
-	return EC_SUCCESS;
-}
-
 static int set_gyro_foc_config(struct motion_sensor_t *s)
 {
 	uint8_t reg_data[4] = { 0 };
@@ -757,13 +589,6 @@ static int get_calib_result(struct motion_sensor_t *s)
 		RETURN_ERROR(bmi3_read_n(s, BMI3_FEATURE_IO_1, reg_data, 4));
 
 		switch (s->type) {
-		case MOTIONSENSE_TYPE_ACCEL:
-			if ((reg_data[3] & BMI3_UGAIN_OFFS_UPD_COMPLETE) &&
-			    ((reg_data[2] & BMI3_FEATURE_IO_1_ERROR_MASK) ==
-			     BMI3_FEATURE_IO_1_NO_ERROR)) {
-				return EC_SUCCESS;
-			}
-			break;
 		case MOTIONSENSE_TYPE_GYRO:
 			if (reg_data[2] & BMI3_SC_ST_STATUS_MASK) {
 				/* Check calibration result */
@@ -782,12 +607,8 @@ static int get_calib_result(struct motion_sensor_t *s)
 static int perform_calib(struct motion_sensor_t *s, int enable)
 {
 	int ret;
-	intv3_t target = { 0, 0, 0 };
 	uint8_t saved_conf[6] = { 0 };
 
-	/* Sensor is configured to be in 16G range */
-	int sens_range = 16;
-
 	/* Variable to set the accelerometer configuration value 50Hz for FOC */
 	uint8_t acc_conf_data[2] = { BMI3_FOC_ACC_CONF_VAL_LSB,
 				     BMI3_FOC_ACC_CONF_VAL_MSB };
@@ -806,19 +627,8 @@ static int perform_calib(struct motion_sensor_t *s, int enable)
 
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
-		target[Z] = BMI3_ACC_DATA_PLUS_1G(sens_range);
-
-		/* Perform accel calibration */
-		ret = perform_accel_foc(s, target, sens_range);
-		if (ret)
-			goto end_calib;
-
-		/* Get caliration results */
-		ret = get_calib_result(s);
-		if (ret)
-			goto end_calib;
-
-		break;
+		ret = EC_RES_INVALID_COMMAND;
+		goto end_calib;
 	case MOTIONSENSE_TYPE_GYRO:
 		ret = set_gyro_foc_config(s);
 		if (ret)
@@ -887,7 +697,7 @@ static int set_offset(const struct motion_sensor_t *s, const int16_t *offset,
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
 		/* Offset should be in units of mg */
-		RETURN_ERROR(set_accel_offset(s, v, BMI3_ENABLE));
+		RETURN_ERROR(set_accel_offset(s, v));
 		break;
 	case MOTIONSENSE_TYPE_GYRO:
 		/* Offset should be in units of mdps */
diff --git a/driver/accelgyro_bmi3xx.h b/driver/accelgyro_bmi3xx.h
index 32381aab24..eb46d3bc9a 100644
--- a/driver/accelgyro_bmi3xx.h
+++ b/driver/accelgyro_bmi3xx.h
@@ -84,8 +84,13 @@
 #define BMI3_PORTRAIT_INVERT 2
 #define BMI3_LANDSCAPE_INVERT 3
 
-#define ACC_DP_OFF_X 0x60
-#define GYR_DP_OFF_X 0x66
+#define BMI3_ACC_DP_OFF_X 0x60
+#define BMI3_ACC_DP_OFF_Y 0x62
+#define BMI3_ACC_DP_OFF_Z 0x64
+
+#define BMI3_GYR_DP_OFF_X 0x66
+#define BMI3_GYR_DP_OFF_Y 0x68
+#define BMI3_GYR_DP_OFF_Z 0x6A
 
 #define BMI3_REG_CMD 0x7E
 #define BMI3_CMD_SOFT_RESET 0xDEAF