driver: bmi160: Fix bugs found testing SPI

SPI being much faster, need to add right sleep to wait for
sensor to change state from suspend.

Set the ODR before the range, an issue I did not have with i2c.

Fix test used when FIFO is disabled.

BRANCH=smaug
BUG=chrome-os-partner:42304
TEST=Check sensors are coming and rate/range are correctly set.

Change-Id: I5bf655626f1f4232478a04d1d4e1a0d443efbf0f
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/288517
Reviewed-by: Vincent Palatin <vpalatin@chromium.org>

3 files changed, 26 insertions, 13 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 8da7c7a7a2..d55ff642e3 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -54,6 +54,12 @@ const struct accel_param_pair dps_ranges[] = {
 	{2000, BMI160_DPS_SEL_2000}
 };
 
+static int wakeup_time[] = {
+	[MOTIONSENSE_TYPE_ACCEL] = 4,
+	[MOTIONSENSE_TYPE_GYRO] = 80,
+	[MOTIONSENSE_TYPE_MAG] = 1
+};
+
 static inline const struct accel_param_pair *get_range_table(
 		enum motionsensor_type type, int *psize)
 {
@@ -321,12 +327,6 @@ static int get_resolution(const struct motion_sensor_t *s,
 	return EC_SUCCESS;
 }
 
-static int wakeup_time[] = {
-	[MOTIONSENSE_TYPE_ACCEL] = 4,
-	[MOTIONSENSE_TYPE_GYRO] = 60,
-	[MOTIONSENSE_TYPE_MAG] = 5
-};
-
 static int set_data_rate(const struct motion_sensor_t *s,
 				int rate,
 				int rnd)
@@ -852,7 +852,7 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 	 * Note: return success so that motion senor task can read again
 	 * to get the latest updated sensor data quickly.
 	 */
-	if (status & BMI160_DRDY_MASK(s->type)) {
+	if (!(status & BMI160_DRDY_MASK(s->type))) {
 		if (v != s->raw_xyz)
 			memcpy(v, s->raw_xyz, sizeof(s->raw_xyz));
 		return EC_SUCCESS;
@@ -896,7 +896,7 @@ static int init(const struct motion_sensor_t *s)
 
 	raw_write8(s->addr, BMI160_CMD_REG,
 			BMI160_CMD_MODE_NORMAL(s->type));
-	msleep(30);
+	msleep(wakeup_time[s->type]);
 
 #ifdef CONFIG_MAG_BMI160_BMM150
 	if (s->type == MOTIONSENSE_TYPE_MAG) {
@@ -969,12 +969,13 @@ static int init(const struct motion_sensor_t *s)
 		bmm150_mag_access_ctrl(s->addr, 0);
 	}
 #endif
+
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	if (s->type == MOTIONSENSE_TYPE_ACCEL)
 		ret = config_interrupt(s);
 #endif
-	set_range(s, s->runtime_config.range, 0);
 	set_data_rate(s, s->runtime_config.odr, 0);
+	set_range(s, s->runtime_config.range, 0);
 
 	CPRINTF("[%T %s: MS Done Init type:0x%X range:%d odr:%d]\n",
 			s->name, s->type, s->runtime_config.range,
diff --git a/driver/accelgyro_bmi160.h b/driver/accelgyro_bmi160.h
index 3e39e657ff..27df9c2c1f 100644
--- a/driver/accelgyro_bmi160.h
+++ b/driver/accelgyro_bmi160.h
@@ -24,6 +24,16 @@
 
 #define BMI160_ERR_REG         0x02
 #define BMI160_PMU_STATUS      0x03
+#define BMI160_PMU_MAG_OFFSET               0
+#define BMI160_PMU_GYR_OFFSET               2
+#define BMI160_PMU_ACC_OFFSET               4
+#define BMI160_PMU_SENSOR_STATUS(_sensor_type, _val) \
+	(((_val) >> (4 - 2 * (_sensor_type))) & 0x3)
+#define BMI160_PMU_SUSPEND                  0
+#define BMI160_PMU_NORMAL                   1
+#define BMI160_PMU_LOW_POWER                2
+#define BMI160_PMU_FAST_STARTUP             3
+
 #define BMI160_MAG_X_L_G       0x04
 #define BMI160_MAG_X_H_G       0x05
 #define BMI160_MAG_Y_L_G       0x06
@@ -314,6 +324,7 @@ enum fifo_header {
 #define BMI160_CMD_SOFT_RESET      0xb6
 #define BMI160_CMD_NOOP            0x00
 #define BMI160_CMD_START_FOC       0x03
+#define BMI160_CMD_ACC_MODE_OFFSET 0x10
 #define BMI160_CMD_ACC_MODE_SUSP   0x10
 #define BMI160_CMD_ACC_MODE_NORMAL 0x11
 #define BMI160_CMD_ACC_MODE_LOWPOWER 0x12
@@ -323,10 +334,10 @@ enum fifo_header {
 #define BMI160_CMD_MAG_MODE_SUSP   0x18
 #define BMI160_CMD_MAG_MODE_NORMAL 0x19
 #define BMI160_CMD_MAG_MODE_LOWPOWER 0x1a
-#define BMI160_CMD_MODE_SUSPEND(_sensor) \
-	(BMI160_CMD_ACC_MODE_SUSP + 4 * (_sensor))
-#define BMI160_CMD_MODE_NORMAL(_sensor) \
-	(BMI160_CMD_ACC_MODE_NORMAL + 4 * (_sensor))
+#define BMI160_CMD_MODE_SUSPEND(_sensor_type) \
+	(BMI160_CMD_ACC_MODE_OFFSET | (_sensor_type) << 2 | BMI160_PMU_SUSPEND)
+#define BMI160_CMD_MODE_NORMAL(_sensor_type) \
+	(BMI160_CMD_ACC_MODE_OFFSET | (_sensor_type) << 2 | BMI160_PMU_NORMAL)
 
 #define BMI160_CMD_FIFO_FLUSH      0xb0
 #define BMI160_CMD_INT_RESET       0xb1
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c
index 772b147d70..4bc0d7a8e7 100644
--- a/driver/mag_bmm150.c
+++ b/driver/mag_bmm150.c
@@ -82,6 +82,7 @@ int bmm150_init(const struct motion_sensor_t *s)
 
 	/* Set the compass from Suspend to Sleep */
 	ret = raw_mag_write8(s->addr, BMM150_PWR_CTRL, BMM150_PWR_ON);
+	msleep(4);
 	/* Now we can read the device id */
 	ret = raw_mag_read8(s->addr, BMM150_CHIP_ID, &val);
 	if (ret)