driver: bmi3xx: Fix perform calibration

BRANCH=none
BUG=b:193869438
TEST=Accel implementation tested on Guybrush
     $ ectool motionsense calibrate 1

Signed-off-by: Bhanu Prakash Maiya <bhanumaiya@chromium.org>
Signed-off-by: Latchiamaran Senram <latchiamaran.senram@bosch.corp-partner.google.com>
Change-Id: Ib52f5a70137a135b8be68115c705f004f932b3b4
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3036651
Tested-by: Bhanu Prakash Maiya <bhanumaiya@google.com>
Auto-Submit: Bhanu Prakash Maiya <bhanumaiya@google.com>
Commit-Queue: Rob Barnes <robbarnes@google.com>
Reviewed-by: Rob Barnes <robbarnes@google.com>

2 files changed, 62 insertions, 16 deletions

diff --git a/driver/accelgyro_bmi3xx.c b/driver/accelgyro_bmi3xx.c
index be6be066c1..08020f926a 100644
--- a/driver/accelgyro_bmi3xx.c
+++ b/driver/accelgyro_bmi3xx.c
@@ -595,6 +595,7 @@ int set_accel_offset(const struct motion_sensor_t *s, intv3_t v)
 {
 	uint8_t reg_data[6] = { 0 };
 	uint8_t base_addr[2] = { BMI3_ACC_OFFSET_ADDR, 0 };
+	uint8_t saved_conf[6] = { 0 };
 	int i, val[3];
 
 	for (i = X; i <= Z; ++i) {
@@ -608,6 +609,16 @@ int set_accel_offset(const struct motion_sensor_t *s, intv3_t v)
 			val[i] += 8192;
 	}
 
+	/* Set the power mode as suspend */
+	RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_CONF, saved_conf, 6));
+
+	/* Ignore two i2c sync bytes and store consecutive bytes in reg_data */
+	reg_data[0] = saved_conf[2];
+	reg_data[1] = 0x00;
+	reg_data[2] = saved_conf[4];
+	reg_data[3] = 0x00;
+	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_ACC_CONF, reg_data, 4));
+
 	/*
 	 * Set the user accel offset base address to feature engine
 	 * transmission address to start DMA transaction
@@ -623,14 +634,17 @@ int set_accel_offset(const struct motion_sensor_t *s, intv3_t v)
 
 	/* Set the configuration to the feature engine register */
 	RETURN_ERROR(bmi3_write_n(s, BMI3_FEATURE_ENGINE_DMA_TX_DATA, reg_data,
-		     6));
+				  6));
+
+	/* Restore ACC_CONF by storing saved_conf data */
+	RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_CONF, saved_conf, 6));
 
 	/* Update the offset to the sensor engine */
 	reg_data[0] = (uint8_t)(BMI3_CMD_USR_GAIN_OFFS_UPDATE &
-				 BMI3_SET_LOW_BYTE);
+				BMI3_SET_LOW_BYTE);
 
 	reg_data[1] = (uint8_t)((BMI3_CMD_USR_GAIN_OFFS_UPDATE &
-				BMI3_SET_HIGH_BYTE) >> 8);
+				 BMI3_SET_HIGH_BYTE) >> 8);
 
 	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_CMD, reg_data, 2));
 
@@ -751,11 +765,23 @@ static int get_calib_result(struct motion_sensor_t *s)
 		/* Read the configuration from the feature engine register */
 		RETURN_ERROR(bmi3_read_n(s, BMI3_FEATURE_IO_1, reg_data, 4));
 
-		/* Check calibration complete status */
-		if (reg_data[2] & BMI3_SC_ST_STATUS_MASK) {
-			/* Check cailbration result */
-			if (reg_data[2] & BMI3_SC_RESULT_MASK)
+		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 */
+				if (reg_data[2] & BMI3_SC_RESULT_MASK)
+					return EC_SUCCESS;
+			}
+			break;
+		default:
+			return EC_ERROR_UNIMPLEMENTED;
 		}
 	}
 
@@ -766,7 +792,7 @@ static int perform_calib(struct motion_sensor_t *s, int enable)
 {
 	int ret;
 	intv3_t target = {0, 0, 0};
-	uint8_t saved_conf[4] = {0};
+	uint8_t saved_conf[6] = {0};
 
 	/* Sensor is configured to be in 16G range */
 	int sens_range = 16;
@@ -779,8 +805,8 @@ static int perform_calib(struct motion_sensor_t *s, int enable)
 		return EC_SUCCESS;
 
 	/* Get default configurations for the type of feature selected. */
-	RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_CONF + s->type, saved_conf,
-				4));
+	RETURN_ERROR(bmi3_read_n(s, BMI3_REG_ACC_CONF, saved_conf,
+				6));
 
 	ret = bmi3_write_n(s, BMI3_REG_ACC_CONF, acc_conf_data, 2);
 	if (ret)
@@ -796,6 +822,12 @@ static int perform_calib(struct motion_sensor_t *s, int enable)
 		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;
 	case MOTIONSENSE_TYPE_GYRO:
 		ret = set_gyro_foc_config(s);
@@ -805,6 +837,7 @@ static int perform_calib(struct motion_sensor_t *s, int enable)
 		ret = get_calib_result(s);
 		if (ret)
 			goto end_calib;
+
 		break;
 	default:
 		/* Not supported on Magnetometer */
@@ -814,7 +847,8 @@ static int perform_calib(struct motion_sensor_t *s, int enable)
 
 
 end_calib:
-	bmi3_write_n(s, BMI3_REG_ACC_CONF + s->type, &saved_conf[2], 2);
+	/* Restore ACC_CONF before exiting */
+	RETURN_ERROR(bmi3_write_n(s, BMI3_REG_ACC_CONF, &saved_conf[2], 4));
 
 	return ret;
 }
@@ -954,14 +988,18 @@ static int set_data_rate(const struct motion_sensor_t *s,
 			if (IS_ENABLED(CONFIG_ACCEL_FIFO))
 				ret = enable_fifo(s, 0);
 
-			/* Set the sensor in suspend mode */
+			/*
+			 * Disable accel to set rate equal to zero.
+			 * Accel does not have suspend mode.
+			 */
 			reg_data[3] = BMI3_SET_BITS(reg_data[3],
 				BMI3_POWER_MODE,
-				BMI3_ACC_MODE_LOW_PWR);
+				BMI3_ACC_MODE_DISABLE);
 
 			saved_data->odr = 0;
 		} else if (saved_data->odr == 0) {
-			/* Power mode changed from suspend to
+			/*
+			 * Power mode changed from suspend to
 			 * normal
 			 */
 			reg_data[3] = BMI3_SET_BITS(reg_data[3],
@@ -974,7 +1012,10 @@ static int set_data_rate(const struct motion_sensor_t *s,
 			if (IS_ENABLED(CONFIG_ACCEL_FIFO))
 				ret = enable_fifo(s, 0);
 
-			/* Set the sensor in suspend mode */
+			/*
+			 * Set gyro to suspend mode to disable gyro
+			 * however keep internal driver enabled
+			 */
 			reg_data[3] = BMI3_SET_BITS(reg_data[3],
 				      BMI3_POWER_MODE,
 				      BMI3_GYR_MODE_SUSPEND);
diff --git a/driver/accelgyro_bmi3xx.h b/driver/accelgyro_bmi3xx.h
index ad2bce3145..e892f1cbea 100644
--- a/driver/accelgyro_bmi3xx.h
+++ b/driver/accelgyro_bmi3xx.h
@@ -129,10 +129,15 @@
 /* Feature engine General purpose register 1. */
 #define BMI3_FEATURE_IO_0			0x10
 #define BMI3_ANY_MOTION_X_EN_MASK		0x08
+
 #define BMI3_FEATURE_IO_1			0x11
-#define BMI3_FEATURE_IO_STATUS			0x14
+#define BMI3_FEATURE_IO_1_ERROR_MASK		0x0F
+#define BMI3_FEATURE_IO_1_NO_ERROR		0x05
 #define BMI3_SC_ST_STATUS_MASK			0x10
 #define BMI3_SC_RESULT_MASK			0x20
+#define BMI3_UGAIN_OFFS_UPD_COMPLETE		0x01
+
+#define BMI3_FEATURE_IO_STATUS			0x14
 
 /*
  * The max positive value of accel data is 0x7FFF, equal to range(g)