driver: bma2x2: Add perform_calib

Add fast compensation automatic calibration, like bmi160.
Use timestamp_expired for timeout measurement for both perform_calib
functions.
Remove driver offset field, remove private bma2x2 structure.

BUG=b:73205042
BRANCH=master
TEST=echo 1 > calibrate perform calibration.
Reading in_accel_*_calibbias is within range.
Check on Lami for both bma2x2 and bmi160.

Change-Id: I3472865287fa4769a05e6f872b92d7c3f933cb4e
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/957872
Reviewed-by: Daisuke Nojiri <dnojiri@chromium.org>

6 files changed, 92 insertions, 48 deletions

diff --git a/board/kevin/board.c b/board/kevin/board.c
index d2fd228723..1ec3e6bf1e 100644
--- a/board/kevin/board.c
+++ b/board/kevin/board.c
@@ -422,7 +422,7 @@ static struct bmi160_drv_data_t g_bmi160_data;
 
 #ifdef BOARD_KEVIN
 /* BMA255 private data */
-static struct bma2x2_accel_data g_bma255_data;
+static struct accelgyro_saved_data_t g_bma255_data;
 
 /* Matrix to rotate accelrator into standard reference frame */
 const matrix_3x3_t base_standard_ref = {
diff --git a/board/nami/board.c b/board/nami/board.c
index dd9fa16cee..438dfc824c 100644
--- a/board/nami/board.c
+++ b/board/nami/board.c
@@ -551,7 +551,7 @@ static struct mutex g_base_mutex;
 static struct bmi160_drv_data_t g_bmi160_data;
 
 /* BMA255 private data */
-static struct bma2x2_accel_data g_bma255_data;
+static struct accelgyro_saved_data_t g_bma255_data;
 
 static struct opt3001_drv_data_t g_opt3001_data = {
 	.scale = 1,
diff --git a/board/nautilus/board.c b/board/nautilus/board.c
index 654b8e50d2..215f0213a8 100644
--- a/board/nautilus/board.c
+++ b/board/nautilus/board.c
@@ -576,7 +576,7 @@ static struct mutex g_base_mutex;
 static struct bmi160_drv_data_t g_bmi160_data;
 
 /* BMA255 private data */
-static struct bma2x2_accel_data g_bma255_data;
+static struct accelgyro_saved_data_t g_bma255_data;
 
 /* Matrix to rotate accelrator into standard reference frame */
 const matrix_3x3_t base_standard_ref = {
diff --git a/driver/accel_bma2x2.c b/driver/accel_bma2x2.c
index eecae02357..fcb9cf478d 100644
--- a/driver/accel_bma2x2.c
+++ b/driver/accel_bma2x2.c
@@ -59,7 +59,7 @@ static int raw_read_multi(const int port, int addr, uint8_t reg,
 static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 {
 	int ret,  range_val, reg_val, range_reg_val;
-	struct bma2x2_accel_data *data = s->drv_data;
+	struct accelgyro_saved_data_t *data = s->drv_data;
 
 	range_val = BMA2x2_RANGE_TO_REG(range);
 	if ((BMA2x2_RANGE_TO_REG(range_val) < range) && rnd)
@@ -79,7 +79,7 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 
 	/* If successfully written, then save the range. */
 	if (ret == EC_SUCCESS)
-		data->sensor_range = BMA2x2_REG_TO_RANGE(range_val);
+		data->range = BMA2x2_REG_TO_RANGE(range_val);
 
 	mutex_unlock(s->mutex);
 
@@ -88,9 +88,9 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 
 static int get_range(const struct motion_sensor_t *s)
 {
-	struct bma2x2_accel_data *data = s->drv_data;
+	struct accelgyro_saved_data_t *data = s->drv_data;
 
-	return data->sensor_range;
+	return data->range;
 }
 
 static int get_resolution(const struct motion_sensor_t *s)
@@ -101,7 +101,7 @@ static int get_resolution(const struct motion_sensor_t *s)
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
 	int ret, odr_val, odr_reg_val, reg_val;
-	struct bma2x2_accel_data *data = s->drv_data;
+	struct accelgyro_saved_data_t *data = s->drv_data;
 
 	odr_val = BMA2x2_BW_TO_REG(rate);
 	if ((BMA2x2_REG_TO_BW(odr_val) < rate) && rnd)
@@ -121,7 +121,7 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 
 	/* If successfully written, then save the new data rate. */
 	if (ret == EC_SUCCESS)
-		data->sensor_datarate = BMA2x2_REG_TO_BW(odr_val);
+		data->odr = BMA2x2_REG_TO_BW(odr_val);
 
 	mutex_unlock(s->mutex);
 	return ret;
@@ -129,42 +129,47 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 
 static int get_data_rate(const struct motion_sensor_t *s)
 {
-	struct bma2x2_accel_data *data = s->drv_data;
+	struct accelgyro_saved_data_t *data = s->drv_data;
 
-	return data->sensor_datarate;
+	return data->odr;
 }
 
 static int set_offset(const struct motion_sensor_t *s, const int16_t *offset,
 		      int16_t temp)
 {
-	/* temperature is ignored */
-	struct bma2x2_accel_data *data = s->drv_data;
-
-	data->offset[X] = offset[X];
-	data->offset[Y] = offset[Y];
-	data->offset[Z] = offset[Z];
+	int i, ret;
 
+	/* temperature is ignored */
+	/* Offset from host is in 1/1024g, 1/128g internally. */
+	for (i = X; i <= Z; i++) {
+		ret = raw_write8(s->port, s->addr,
+				 BMA2x2_OFFSET_X_AXIS_ADDR + i, offset[i] / 8);
+		if (ret)
+			return ret;
+	}
 	return EC_SUCCESS;
 }
 
 static int get_offset(const struct motion_sensor_t *s, int16_t *offset,
 		      int16_t *temp)
 {
-	struct bma2x2_accel_data *data = s->drv_data;
+	int i, val, ret;
 
-	offset[X] = data->offset[X];
-	offset[Y] = data->offset[Y];
-	offset[Z] = data->offset[Z];
+	for (i = X; i <= Z; i++) {
+		ret = raw_read8(s->port, s->addr, BMA2x2_OFFSET_X_AXIS_ADDR + i,
+				&val);
+		if (ret)
+			return ret;
+		offset[i] = (int8_t)val * 8;
+	}
 	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
-
 	return EC_SUCCESS;
 }
 
 static int read(const struct motion_sensor_t *s, vector_3_t v)
 {
 	uint8_t acc[6];
-	int ret, i, range;
-	struct bma2x2_accel_data *data = s->drv_data;
+	int ret, i;
 
 	/* Read 6 bytes starting at X_AXIS_LSB. */
 	mutex_lock(s->mutex);
@@ -184,19 +189,70 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 	 * acc[3] = Y_AXIS_MSB
 	 * acc[4] = Z_AXIS_LSB -> bit 7~4 for value, bit 0 for new data bit
 	 * acc[5] = Z_AXIS_MSB
-	 *
-	 * Add calibration offset before returning the data.
 	 */
 	for (i = X; i <= Z; i++)
 		v[i] = (((int8_t)acc[i * 2 + 1]) << 8) | (acc[i * 2] & 0xf0);
 	rotate(v, *s->rot_standard_ref, v);
 
-	/* apply offset in the device coordinates */
+	return EC_SUCCESS;
+}
+
+static int perform_calib(const struct motion_sensor_t *s)
+{
+	int ret, val, status, rate, range, i;
+	timestamp_t deadline;
+
+	ret = raw_read8(s->port, s->addr, BMA2x2_OFFSET_CTRL_ADDR, &val);
+	if (ret)
+		return ret;
+	if (!(val & BMA2x2_OFFSET_CAL_READY))
+		return EC_ERROR_ACCESS_DENIED;
+
+	rate = get_data_rate(s);
 	range = get_range(s);
-	for (i = X; i <= Z; i++)
-		v[i] += (data->offset[i] << 5) / range;
+	/*
+	 * Temporary set frequency to 100Hz to get enough data in a short
+	 * period of time.
+	 */
+	set_data_rate(s, 100000, 0);
+	set_range(s, 2, 0);
+
+	/* We assume the device is laying flat for calibration */
+	if (s->rot_standard_ref == NULL ||
+	    (*s->rot_standard_ref)[2][2] > INT_TO_FP(0))
+		val = BMA2x2_OFC_TARGET_PLUS_1G;
+	else
+		val = BMA2x2_OFC_TARGET_MINUS_1G;
+	val = ((BMA2x2_OFC_TARGET_0G << BMA2x2_OFC_TARGET_AXIS(X)) |
+	       (BMA2x2_OFC_TARGET_0G << BMA2x2_OFC_TARGET_AXIS(Y)) |
+	       (val << BMA2x2_OFC_TARGET_AXIS(Z)));
+	raw_write8(s->port, s->addr, BMA2x2_OFC_SETTING_ADDR, val);
+
+	for (i = X; i <= Z; i++) {
+		val = (i + 1) << BMA2x2_OFFSET_TRIGGER_OFF;
+		raw_write8(s->port, s->addr, BMA2x2_OFFSET_CTRL_ADDR, val);
+		/*
+		 * The sensor needs 16 samples. At 100Hz/10ms, it needs 160ms to
+		 * complete. Set 400ms to have some margin.
+		 */
+		deadline.val = get_time().val + 400;
+		do {
+			if (timestamp_expired(deadline, NULL)) {
+				ret = EC_RES_TIMEOUT;
+				goto end_perform_calib;
+			}
+			msleep(50);
+			ret = raw_read8(s->port, s->addr,
+					BMA2x2_OFFSET_CTRL_ADDR, &status);
+			if (ret != EC_SUCCESS)
+				goto end_perform_calib;
+		} while ((status & BMA2x2_OFFSET_CAL_READY) == 0);
+	}
 
-	return EC_SUCCESS;
+end_perform_calib:
+	set_range(s, range, 0);
+	set_data_rate(s, rate, 0);
+	return ret;
 }
 
 static int init(const struct motion_sensor_t *s)
@@ -259,5 +315,5 @@ const struct accelgyro_drv bma2x2_accel_drv = {
 	.get_data_rate = get_data_rate,
 	.set_offset = set_offset,
 	.get_offset = get_offset,
-	.perform_calib = NULL,
+	.perform_calib = perform_calib,
 };
diff --git a/driver/accel_bma2x2.h b/driver/accel_bma2x2.h
index d26782e9d4..2fcbfadfdc 100644
--- a/driver/accel_bma2x2.h
+++ b/driver/accel_bma2x2.h
@@ -8,19 +8,6 @@
 #ifndef __CROS_EC_ACCEL_BMA2x2_H
 #define __CROS_EC_ACCEL_BMA2x2_H
 
-enum bma2x2_accel {
-	BMA255,
-	SUPPORTED_BOSCH_ACCELS,
-};
-
-struct bma2x2_accel_data {
-	/* Current range of accelerometer. */
-	int sensor_range;
-	/* Current output data rate of accelerometer. */
-	int sensor_datarate;
-	int16_t offset[3];
-};
-
 extern const struct accelgyro_drv bma2x2_accel_drv;
 
 /* I2C ADDRESS DEFINITIONS    */
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 20a6a65a79..74689b6ae9 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -599,9 +599,10 @@ static int set_offset(const struct motion_sensor_t *s,
 	return ret;
 }
 
-int perform_calib(const struct motion_sensor_t *s)
+static int perform_calib(const struct motion_sensor_t *s)
 {
-	int ret, val, en_flag, status, timeout = 0, rate;
+	int ret, val, en_flag, status, rate;
+	timestamp_t deadline;
 
 	rate = get_data_rate(s);
 	/*
@@ -635,8 +636,9 @@ int perform_calib(const struct motion_sensor_t *s)
 	ret = raw_write8(s->port, s->addr, BMI160_FOC_CONF, val);
 	ret = raw_write8(s->port, s->addr, BMI160_CMD_REG,
 			 BMI160_CMD_START_FOC);
+	deadline.val = get_time().val + 400;
 	do {
-		if (timeout > 400) {
+		if (timestamp_expired(deadline, NULL)) {
 			ret = EC_RES_TIMEOUT;
 			goto end_perform_calib;
 		}
@@ -644,7 +646,6 @@ int perform_calib(const struct motion_sensor_t *s)
 		ret = raw_read8(s->port, s->addr, BMI160_STATUS, &status);
 		if (ret != EC_SUCCESS)
 			goto end_perform_calib;
-		timeout += 50;
 	} while ((status & BMI160_FOC_RDY) == 0);
 
 	/* Calibration is successful, and loaded, use the result */