driver: Add BMI common code

Add accelgyro_bmi_common.(c/h) for the code shared among the BMI.

BRANCH=None
BUG=b:146144827
TEST=make -j4 BOARD=ampton
     Then, flash the ec.bin into DUT, check sensor data is available on DUT
     by "ectool motionsense".
TEST=make buildall -j

Change-Id: Ia56ffb0896943b08640b3d6ca96324ac5e7d93a0
Signed-off-by: Ching-Kang Yen <chingkang@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2130146
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3631903
Commit-Queue: Henry Sun <henrysun@google.com>
Tested-by: Zick Wei <zick.wei@quanta.corp-partner.google.com>
Reviewed-by: Henry Sun <henrysun@google.com>

7 files changed, 1298 insertions, 863 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 61461ff222..8aa5af4098 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -11,6 +11,7 @@
 #include "accelgyro.h"
 #include "common.h"
 #include "console.h"
+#include "driver/accelgyro_bmi_common.h"
 #include "driver/accelgyro_bmi160.h"
 #include "driver/mag_bmm150.h"
 #include "hooks.h"
@@ -30,216 +31,13 @@
 static volatile uint32_t last_interrupt_timestamp;
 #endif
 
-/*
- * Struct for pairing an engineering value with the register value for a
- * parameter.
- */
-struct accel_param_pair {
-	int val; /* Value in engineering units. */
-	int reg_val; /* Corresponding register value. */
-};
-
-/* List of range values in +/-G's and their associated register values. */
-static const struct accel_param_pair g_ranges[] = {
-	{2, BMI160_GSEL_2G},
-	{4, BMI160_GSEL_4G},
-	{8, BMI160_GSEL_8G},
-	{16, BMI160_GSEL_16G}
-};
-
-/*
- * List of angular rate range values in +/-dps's
- * and their associated register values.
- */
-const struct accel_param_pair dps_ranges[] = {
-	{125, BMI160_DPS_SEL_125},
-	{250, BMI160_DPS_SEL_250},
-	{500, BMI160_DPS_SEL_500},
-	{1000, BMI160_DPS_SEL_1000},
-	{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)
-{
-	if (MOTIONSENSE_TYPE_ACCEL == type) {
-		if (psize)
-			*psize = ARRAY_SIZE(g_ranges);
-		return g_ranges;
-	} else {
-		if (psize)
-			*psize = ARRAY_SIZE(dps_ranges);
-		return dps_ranges;
-	}
-}
-
-static inline int get_xyz_reg(enum motionsensor_type type)
-{
-	switch (type) {
-	case MOTIONSENSE_TYPE_ACCEL:
-		return BMI160_ACC_X_L_G;
-	case MOTIONSENSE_TYPE_GYRO:
-		return BMI160_GYR_X_L_G;
-	case MOTIONSENSE_TYPE_MAG:
-		return BMI160_MAG_X_L_G;
-	default:
-		return -1;
-	}
-}
-
-/**
- * @return reg value that matches the given engineering value passed in.
- * The round_up flag is used to specify whether to round up or down.
- * Note, this function always returns a valid reg value. If the request is
- * outside the range of values, it returns the closest valid reg value.
- */
-static int get_reg_val(const int eng_val, const int round_up,
-		const struct accel_param_pair *pairs, const int size)
-{
-	int i;
-
-	for (i = 0; i < size - 1; i++) {
-		if (eng_val <= pairs[i].val)
-			break;
-
-		if (eng_val < pairs[i+1].val) {
-			if (round_up)
-				i += 1;
-			break;
-		}
-	}
-	return pairs[i].reg_val;
-}
-
-/**
- * @return engineering value that matches the given reg val
- */
-static int get_engineering_val(const int reg_val,
-		const struct accel_param_pair *pairs, const int size)
-{
-	int i;
-
-	for (i = 0; i < size; i++) {
-		if (reg_val == pairs[i].reg_val)
-			break;
-	}
-	return pairs[i].val;
-}
-
-#ifdef CONFIG_SPI_ACCEL_PORT
-static inline int spi_raw_read(const int addr, const uint8_t reg,
-			       uint8_t *data, const int len)
-{
-	uint8_t cmd = 0x80 | reg;
-
-	return spi_transaction(&spi_devices[addr], &cmd, 1, data, len);
-}
-#endif
-/**
- * Read 8bit register from accelerometer.
- */
-static int raw_read8(const int port, const int addr, const int reg,
-					 int *data_ptr)
-{
-	int rv = -EC_ERROR_PARAM1;
-
-	if (BMI160_IS_SPI(addr)) {
-#ifdef CONFIG_SPI_ACCEL_PORT
-		uint8_t val;
-		rv = spi_raw_read(BMI160_SPI_ADDRESS(addr), reg, &val, 1);
-		if (rv == EC_SUCCESS)
-			*data_ptr = val;
-#endif
-	} else {
-#ifdef I2C_PORT_ACCEL
-		rv = i2c_read8(port, BMI160_I2C_ADDRESS(addr),
-			       reg, data_ptr);
-#endif
-	}
-	return rv;
-}
-
-/**
- * Write 8bit register from accelerometer.
- */
-static int raw_write8(const int port, const int addr, const int reg,
-					  int data)
-{
-	int rv = -EC_ERROR_PARAM1;
-
-	if (BMI160_IS_SPI(addr)) {
-#ifdef CONFIG_SPI_ACCEL_PORT
-		uint8_t cmd[2] = { reg, data };
-		rv = spi_transaction(&spi_devices[BMI160_SPI_ADDRESS(addr)],
-				     cmd, 2, NULL, 0);
-#endif
-	} else {
-#ifdef I2C_PORT_ACCEL
-		rv = i2c_write8(port, BMI160_I2C_ADDRESS(addr),
-				reg, data);
-#endif
-	}
-	/*
-	 * From Bosch:  BMI160 needs a delay of 450us after each write if it
-	 * is in suspend mode, otherwise the operation may be ignored by
-	 * the sensor. Given we are only doing write during init, add
-	 * the delay inconditionally.
-	 */
-	msleep(1);
-	return rv;
-}
-
-#ifdef CONFIG_ACCEL_INTERRUPTS
-/**
- * Read 32bit register from accelerometer.
- */
-static int raw_read32(const int port, const int addr, const uint8_t reg,
-					  int *data_ptr)
-{
-	int rv = -EC_ERROR_PARAM1;
-	if (BMI160_IS_SPI(addr)) {
-#ifdef CONFIG_SPI_ACCEL_PORT
-		rv = spi_raw_read(BMI160_SPI_ADDRESS(addr), reg,
-				  (uint8_t *)data_ptr, 4);
-#endif
-	} else {
-#ifdef I2C_PORT_ACCEL
-		rv = i2c_read32(port, BMI160_I2C_ADDRESS(addr),
-				reg, data_ptr);
-#endif
-	}
-	return rv;
-}
-#endif /* defined(CONFIG_ACCEL_INTERRUPTS) */
-
-/**
- * Read n bytes from accelerometer.
- */
-static int raw_read_n(const int port, const int addr, const uint8_t reg,
-		uint8_t *data_ptr, const int len)
-{
-	int rv = -EC_ERROR_PARAM1;
-
-	if (BMI160_IS_SPI(addr)) {
-#ifdef CONFIG_SPI_ACCEL_PORT
-		rv = spi_raw_read(BMI160_SPI_ADDRESS(addr), reg, data_ptr, len);
-#endif
-	} else {
-#ifdef I2C_PORT_ACCEL
-		rv = i2c_read_block(port, BMI160_I2C_ADDRESS(addr), reg,
-				data_ptr, len);
-#endif
-	}
-	return rv;
-}
-
-#ifdef CONFIG_BMI160_SEC_I2C
+#ifdef CONFIG_BMI_SEC_I2C
 /**
  * Control access to the compass on the secondary i2c interface:
  * enable values are:
@@ -250,7 +48,7 @@ static int bmi160_sec_access_ctrl(const int port, const int addr,
 				  const int enable)
 {
 	int mag_if_ctrl;
-	raw_read8(port, addr, BMI160_MAG_IF_1, &mag_if_ctrl);
+	bmi_read8(port, addr, BMI160_MAG_IF_1, &mag_if_ctrl);
 	if (enable) {
 		mag_if_ctrl |= BMI160_MAG_MANUAL_EN;
 		mag_if_ctrl &= ~BMI160_MAG_READ_BURST_MASK;
@@ -260,162 +58,69 @@ static int bmi160_sec_access_ctrl(const int port, const int addr,
 		mag_if_ctrl &= ~BMI160_MAG_READ_BURST_MASK;
 		mag_if_ctrl |= BMI160_MAG_READ_BURST_8;
 	}
-	return raw_write8(port, addr, BMI160_MAG_IF_1, mag_if_ctrl);
+	return bmi_write8(port, addr, BMI160_MAG_IF_1, mag_if_ctrl);
 }
 
 /**
  * Read register from compass.
  * Assuming we are in manual access mode, read compass i2c register.
  */
-int bmi160_sec_raw_read8(const int port, const int addr, const uint8_t reg,
+int bmi160_sec_bmi_read8(const int port, const int addr, const uint8_t reg,
 				  int *data_ptr)
 {
 	/* Only read 1 bytes */
-	raw_write8(port, addr, BMI160_MAG_I2C_READ_ADDR, reg);
-	return raw_read8(port, addr, BMI160_MAG_I2C_READ_DATA, data_ptr);
+	bmi_write8(port, addr, BMI160_MAG_I2C_READ_ADDR, reg);
+	return bmi_read8(port, addr, BMI160_MAG_I2C_READ_DATA, data_ptr);
 }
 
 /**
  * Write register from compass.
  * Assuming we are in manual access mode, write to compass i2c register.
  */
-int bmi160_sec_raw_write8(const int port, const int addr, const uint8_t reg,
+int bmi160_sec_bmi_write8(const int port, const int addr, const uint8_t reg,
 			  int data)
 {
-	raw_write8(port, addr, BMI160_MAG_I2C_WRITE_DATA, data);
-	return raw_write8(port, addr, BMI160_MAG_I2C_WRITE_ADDR, reg);
+	bmi_write8(port, addr, BMI160_MAG_I2C_WRITE_DATA, data);
+	return bmi_write8(port, addr, BMI160_MAG_I2C_WRITE_ADDR, reg);
 }
 #endif
 
-#ifdef CONFIG_ACCEL_FIFO
-static int enable_fifo(const struct motion_sensor_t *s, int enable)
-{
-	struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
-	int ret, val;
-
-	if (enable) {
-		/* FIFO start collecting events */
-		ret = raw_read8(s->port, s->addr, BMI160_FIFO_CONFIG_1, &val);
-		val |= BMI160_FIFO_SENSOR_EN(s->type);
-		ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1, val);
-		if (ret == EC_SUCCESS)
-			data->flags |= 1 << (s->type + BMI160_FIFO_FLAG_OFFSET);
-
-	} else {
-		/* FIFO stop collecting events */
-		ret = raw_read8(s->port, s->addr, BMI160_FIFO_CONFIG_1, &val);
-		val &= ~BMI160_FIFO_SENSOR_EN(s->type);
-		ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1, val);
-		if (ret == EC_SUCCESS)
-			data->flags &=
-				~(1 << (s->type + BMI160_FIFO_FLAG_OFFSET));
-	}
-	return ret;
-}
-#endif
-
-static int set_range(const struct motion_sensor_t *s,
-				int range,
-				int rnd)
-{
-	int ret, range_tbl_size;
-	uint8_t reg_val, ctrl_reg;
-	const struct accel_param_pair *ranges;
-	struct accelgyro_saved_data_t *data = BMI160_GET_SAVED_DATA(s);
-
-	if (s->type == MOTIONSENSE_TYPE_MAG) {
-		data->range = range;
-		return EC_SUCCESS;
-	}
-
-	ctrl_reg = BMI160_RANGE_REG(s->type);
-	ranges = get_range_table(s->type, &range_tbl_size);
-	reg_val = get_reg_val(range, rnd, ranges, range_tbl_size);
-
-	ret = raw_write8(s->port, s->addr, ctrl_reg, reg_val);
-	/* Now that we have set the range, update the driver's value. */
-	if (ret == EC_SUCCESS)
-		data->range = get_engineering_val(reg_val, ranges,
-				range_tbl_size);
-	return ret;
-}
-
-static int get_range(const struct motion_sensor_t *s)
-{
-	struct accelgyro_saved_data_t *data = BMI160_GET_SAVED_DATA(s);
-
-	return data->range;
-}
-
-static int get_resolution(const struct motion_sensor_t *s)
-{
-	return BMI160_RESOLUTION;
-}
-
 static int set_data_rate(const struct motion_sensor_t *s,
 				int rate,
 				int rnd)
 {
-	int ret, val, normalized_rate;
-	uint8_t ctrl_reg, reg_val;
-	struct accelgyro_saved_data_t *data = BMI160_GET_SAVED_DATA(s);
-#ifdef CONFIG_MAG_BMI160_BMM150
+	int ret, normalized_rate;
+	uint8_t reg_val;
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
+#ifdef CONFIG_MAG_BMI_BMM150
 	struct mag_cal_t              *moc = BMM150_CAL(s);
 #endif
 
 	if (rate == 0) {
 #ifdef CONFIG_ACCEL_FIFO
 		/* FIFO stop collecting events */
-		enable_fifo(s, 0);
+		bmi_enable_fifo(s, 0);
 #endif
 		/* go to suspend mode */
-		ret = raw_write8(s->port, s->addr, BMI160_CMD_REG,
+		ret = bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 				 BMI160_CMD_MODE_SUSPEND(s->type));
 		msleep(3);
 		data->odr = 0;
-#ifdef CONFIG_MAG_BMI160_BMM150
+#ifdef CONFIG_MAG_BMI_BMM150
 		if (s->type == MOTIONSENSE_TYPE_MAG)
 			moc->batch_size = 0;
 #endif
 		return ret;
 	} else if (data->odr == 0) {
 		/* back from suspend mode. */
-		ret = raw_write8(s->port, s->addr, BMI160_CMD_REG,
+		ret = bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 				 BMI160_CMD_MODE_NORMAL(s->type));
 		msleep(wakeup_time[s->type]);
 	}
-	ctrl_reg = BMI160_CONF_REG(s->type);
-	reg_val = BMI160_ODR_TO_REG(rate);
-	normalized_rate = BMI160_REG_TO_ODR(reg_val);
-	if (rnd && (normalized_rate < rate)) {
-		reg_val++;
-		normalized_rate = BMI160_REG_TO_ODR(reg_val);
-	}
 
-	switch (s->type) {
-	case MOTIONSENSE_TYPE_ACCEL:
-		if (normalized_rate > MIN(BMI160_ACCEL_MAX_FREQ,
-					CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ) ||
-		    normalized_rate < BMI160_ACCEL_MIN_FREQ)
-			return EC_RES_INVALID_PARAM;
-		break;
-	case MOTIONSENSE_TYPE_GYRO:
-		if (normalized_rate > MIN(BMI160_GYRO_MAX_FREQ,
-					CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ) ||
-		    normalized_rate < BMI160_GYRO_MIN_FREQ)
-			return EC_RES_INVALID_PARAM;
-		break;
-#ifdef CONFIG_MAG_BMI160_BMM150
-	case MOTIONSENSE_TYPE_MAG:
-		/* We use the regular preset we can go about 100Hz */
-		if (reg_val > BMI160_ODR_100HZ || reg_val < BMI160_ODR_0_78HZ)
-			return EC_RES_INVALID_PARAM;
-		break;
-#endif
-
-	default:
-		return EC_RES_INVALID_PARAM;
-	}
+	ret = bmi_get_normalized_rate(s, rate, rnd, &normalized_rate, &reg_val);
+	if (ret)
+		return ret;
 
 	/*
 	 * Lock accel resource to prevent another task from attempting
@@ -423,19 +128,15 @@ static int set_data_rate(const struct motion_sensor_t *s,
 	 */
 	mutex_lock(s->mutex);
 
-	ret = raw_read8(s->port, s->addr, ctrl_reg, &val);
-	if (ret != EC_SUCCESS)
-		goto accel_cleanup;
-
-	val = (val & ~BMI160_ODR_MASK) | reg_val;
-	ret = raw_write8(s->port, s->addr, ctrl_reg, val);
+	ret = bmi_set_reg8(s, BMI_CONF_REG(s->type),
+			   reg_val, BMI_ODR_MASK);
 	if (ret != EC_SUCCESS)
 		goto accel_cleanup;
 
 	/* Now that we have set the odr, update the driver's value. */
 	data->odr = normalized_rate;
 
-#ifdef CONFIG_MAG_BMI160_BMM150
+#ifdef CONFIG_MAG_BMI_BMM150
 	if (s->type == MOTIONSENSE_TYPE_MAG) {
 		/* Reset the calibration */
 		init_mag_cal(moc);
@@ -456,7 +157,7 @@ static int set_data_rate(const struct motion_sensor_t *s,
 	 * FIFO start collecting events.
 	 * They will be discarded if AP does not want them.
 	 */
-	enable_fifo(s, 1);
+	bmi_enable_fifo(s, 1);
 #endif
 
 accel_cleanup:
@@ -464,126 +165,31 @@ accel_cleanup:
 	return ret;
 }
 
-static int get_data_rate(const struct motion_sensor_t *s)
-{
-	struct accelgyro_saved_data_t *data = BMI160_GET_SAVED_DATA(s);
-
-	return data->odr;
-}
-static int get_offset(const struct motion_sensor_t *s,
-			int16_t    *offset,
-			int16_t    *temp)
-{
-	int i, val, val98;
-	intv3_t v;
-
-	switch (s->type) {
-	case MOTIONSENSE_TYPE_ACCEL:
-		/*
-		 * The offset of the accelerometer off_acc_[xyz] is a 8 bit
-		 * two-complement number in units of 3.9 mg independent of the
-		 * range selected for the accelerometer.
-		 */
-		for (i = X; i <= Z; i++) {
-			raw_read8(s->port, s->addr, BMI160_OFFSET_ACC70 + i,
-				  &val);
-			if (val > 0x7f)
-				val = -256 + val;
-			v[i] = round_divide(
-				(int64_t)val * BMI160_OFFSET_ACC_MULTI_MG,
-				BMI160_OFFSET_ACC_DIV_MG);
-
-		}
-		break;
-	case MOTIONSENSE_TYPE_GYRO:
-		/* Read the MSB first */
-		raw_read8(s->port, s->addr, BMI160_OFFSET_EN_GYR98, &val98);
-		/*
-		 * The offset of the gyroscope off_gyr_[xyz] is a 10 bit
-		 * two-complement number in units of 0.061 °/s.
-		 * Therefore a maximum range that can be compensated is
-		 * -31.25 °/s to +31.25 °/s
-		 */
-		for (i = X; i <= Z; i++) {
-			raw_read8(s->port, s->addr, BMI160_OFFSET_GYR70 + i,
-				  &val);
-			val |= ((val98 >> (2 * i)) & 0x3) << 8;
-			if (val > 0x1ff)
-				val = -1024 + val;
-			v[i] = round_divide(
-				(int64_t)val * BMI160_OFFSET_GYRO_MULTI_MDS,
-				BMI160_OFFSET_GYRO_DIV_MDS);
-		}
-		break;
-#ifdef CONFIG_MAG_BMI160_BMM150
-	case MOTIONSENSE_TYPE_MAG:
-		bmm150_get_offset(s, v);
-		break;
-#endif /* defined(CONFIG_MAG_BMI160_BMM150) */
-	default:
-		for (i = X; i <= Z; i++)
-			v[i] = 0;
-	}
-	rotate(v, *s->rot_standard_ref, v);
-	offset[X] = v[X];
-	offset[Y] = v[Y];
-	offset[Z] = v[Z];
-	/* Saving temperature at calibration not supported yet */
-	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
-	return EC_SUCCESS;
-}
-
 static int set_offset(const struct motion_sensor_t *s,
 			const int16_t *offset,
 			int16_t    temp)
 {
-	int ret, i, val, val98;
+	int ret, val98;
 	intv3_t v = { offset[X], offset[Y], offset[Z] };
 
 	rotate_inv(v, *s->rot_standard_ref, v);
 
-	ret = raw_read8(s->port, s->addr, BMI160_OFFSET_EN_GYR98, &val98);
+	ret = bmi_read8(s->port, s->addr, BMI160_OFFSET_EN_GYR98, &val98);
 	if (ret != 0)
 		return ret;
 
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
-		for (i = X; i <= Z; i++) {
-			val = round_divide(
-				(int64_t)v[i] * BMI160_OFFSET_ACC_DIV_MG,
-				BMI160_OFFSET_ACC_MULTI_MG);
-			if (val > 127)
-				val = 127;
-			if (val < -128)
-				val = -128;
-			if (val < 0)
-				val = 256 + val;
-			raw_write8(s->port, s->addr, BMI160_OFFSET_ACC70 + i,
-				   val);
-		}
-		ret = raw_write8(s->port, s->addr, BMI160_OFFSET_EN_GYR98,
+		bmi_set_accel_offset(s, v);
+		ret = bmi_write8(s->port, s->addr, BMI160_OFFSET_EN_GYR98,
 				 val98 | BMI160_OFFSET_ACC_EN);
 		break;
 	case MOTIONSENSE_TYPE_GYRO:
-		for (i = X; i <= Z; i++) {
-			val = round_divide(
-				(int64_t)v[i] * BMI160_OFFSET_GYRO_DIV_MDS,
-				BMI160_OFFSET_GYRO_MULTI_MDS);
-			if (val > 511)
-				val = 511;
-			if (val < -512)
-				val = -512;
-			if (val < 0)
-				val = 1024 + val;
-			raw_write8(s->port, s->addr, BMI160_OFFSET_GYR70 + i,
-					val & 0xFF);
-			val98 &= ~(0x3 << (2 * i));
-			val98 |= (val >> 8) << (2 * i);
-		}
-		ret = raw_write8(s->port, s->addr, BMI160_OFFSET_EN_GYR98,
+		bmi_set_gyro_offset(s, v, &val98);
+		ret = bmi_write8(s->port, s->addr, BMI160_OFFSET_EN_GYR98,
 				 val98 | BMI160_OFFSET_GYRO_EN);
 		break;
-#ifdef CONFIG_MAG_BMI160_BMM150
+#ifdef CONFIG_MAG_BMI_BMM150
 	case MOTIONSENSE_TYPE_MAG:
 		ret = bmm150_set_offset(s, v);
 		break;
@@ -599,7 +205,7 @@ static int perform_calib(const struct motion_sensor_t *s)
 	int ret, val, en_flag, status, rate;
 	timestamp_t deadline;
 
-	rate = get_data_rate(s);
+	rate = bmi_get_data_rate(s);
 	/*
 	 * Temporary set frequency to 100Hz to get enough data in a short
 	 * period of time.
@@ -628,8 +234,8 @@ static int perform_calib(const struct motion_sensor_t *s)
 		ret = EC_RES_INVALID_PARAM;
 		goto end_perform_calib;
 	}
-	ret = raw_write8(s->port, s->addr, BMI160_FOC_CONF, val);
-	ret = raw_write8(s->port, s->addr, BMI160_CMD_REG,
+	ret = bmi_write8(s->port, s->addr, BMI160_FOC_CONF, val);
+	ret = bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 			 BMI160_CMD_START_FOC);
 	deadline.val = get_time().val + 400 * MSEC;
 	do {
@@ -638,40 +244,18 @@ static int perform_calib(const struct motion_sensor_t *s)
 			goto end_perform_calib;
 		}
 		msleep(50);
-		ret = raw_read8(s->port, s->addr, BMI160_STATUS, &status);
+		ret = bmi_read8(s->port, s->addr, BMI160_STATUS, &status);
 		if (ret != EC_SUCCESS)
 			goto end_perform_calib;
 	} while ((status & BMI160_FOC_RDY) == 0);
 
 	/* Calibration is successful, and loaded, use the result */
-	ret = raw_read8(s->port, s->addr, BMI160_OFFSET_EN_GYR98, &val);
-	ret = raw_write8(s->port, s->addr, BMI160_OFFSET_EN_GYR98,
-			 val | en_flag);
+	ret = bmi_enable_reg8(s, BMI160_OFFSET_EN_GYR98, en_flag, 1);
 end_perform_calib:
 	set_data_rate(s, rate, 0);
 	return ret;
 }
 
-void normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *data)
-{
-#ifdef CONFIG_MAG_BMI160_BMM150
-	if (s->type == MOTIONSENSE_TYPE_MAG)
-		bmm150_normalize(s, v, data);
-	else
-#endif
-#ifdef CONFIG_MAG_BMI160_LIS2MDL
-	if (s->type == MOTIONSENSE_TYPE_MAG)
-		lis2mdl_normalize(s, v, data);
-	else
-#endif
-	{
-		v[0] = ((int16_t)((data[1] << 8) | data[0]));
-		v[1] = ((int16_t)((data[3] << 8) | data[2]));
-		v[2] = ((int16_t)((data[5] << 8) | data[4]));
-	}
-	rotate(v, *s->rot_standard_ref, v);
-}
-
 /*
  * Manage gesture recognition.
  * Defined even if host interface is not defined, to enable double tap even
@@ -683,18 +267,14 @@ int manage_activity(const struct motion_sensor_t *s,
 		    const struct ec_motion_sense_activity *param)
 {
 	int ret;
-	struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
+	struct bmi_drv_data_t *data = BMI_GET_DATA(s);
 
 	switch (activity) {
 #ifdef CONFIG_GESTURE_SIGMO
 	case MOTIONSENSE_ACTIVITY_SIG_MOTION: {
-		int tmp;
-		ret = raw_read8(s->port, s->addr, BMI160_INT_EN_0, &tmp);
-		if (ret)
-			return ret;
 		if (enable) {
 			/* We should use parameters from caller */
-			raw_write8(s->port, s->addr, BMI160_INT_MOTION_3,
+			bmi_write8(s->port, s->addr, BMI160_INT_MOTION_3,
 				BMI160_MOTION_PROOF_TIME(
 					CONFIG_GESTURE_SIGMO_PROOF_MS) <<
 				BMI160_MOTION_PROOF_OFF |
@@ -702,18 +282,15 @@ int manage_activity(const struct motion_sensor_t *s,
 					CONFIG_GESTURE_SIGMO_SKIP_MS) <<
 				BMI160_MOTION_SKIP_OFF |
 				BMI160_MOTION_SIG_MOT_SEL);
-			raw_write8(s->port, s->addr, BMI160_INT_MOTION_1,
+			bmi_write8(s->port, s->addr, BMI160_INT_MOTION_1,
 				BMI160_MOTION_TH(s,
 					CONFIG_GESTURE_SIGMO_THRES_MG));
-			tmp |= BMI160_INT_ANYMO_X_EN |
-				BMI160_INT_ANYMO_Y_EN |
-				BMI160_INT_ANYMO_Z_EN;
-		} else {
-			tmp &= ~(BMI160_INT_ANYMO_X_EN |
-				 BMI160_INT_ANYMO_Y_EN |
-				 BMI160_INT_ANYMO_Z_EN);
-		}
-		ret = raw_write8(s->port, s->addr, BMI160_INT_EN_0, tmp);
+					}
+		ret = bmi_enable_reg8(s, BMI160_INT_EN_0,
+				      BMI160_INT_ANYMO_X_EN |
+				      BMI160_INT_ANYMO_Y_EN |
+				      BMI160_INT_ANYMO_Z_EN,
+				      enable);
 		if (ret)
 			ret = EC_RES_UNAVAILABLE;
 		break;
@@ -721,16 +298,10 @@ int manage_activity(const struct motion_sensor_t *s,
 #endif
 #ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
 	case MOTIONSENSE_ACTIVITY_DOUBLE_TAP: {
-		int tmp;
 		/* Set double tap interrupt */
-		ret = raw_read8(s->port, s->addr, BMI160_INT_EN_0, &tmp);
-		if (ret)
-			return ret;
-		if (enable)
-			tmp |= BMI160_INT_D_TAP_EN;
-		else
-			tmp &= ~BMI160_INT_D_TAP_EN;
-		ret = raw_write8(s->port, s->addr, BMI160_INT_EN_0, tmp);
+		ret = bmi_enable_reg8(s, BMI160_INT_EN_0,
+				      BMI160_INT_D_TAP_EN,
+				      enable);
 		if (ret)
 			ret = EC_RES_UNAVAILABLE;
 		break;
@@ -756,7 +327,7 @@ int list_activities(const struct motion_sensor_t *s,
 		    uint32_t *enabled,
 		    uint32_t *disabled)
 {
-	struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
+	struct bmi_drv_data_t *data = BMI_GET_DATA(s);
 	*enabled = data->enabled_activities;
 	*disabled = data->disabled_activities;
 	return EC_RES_SUCCESS;
@@ -765,218 +336,6 @@ int list_activities(const struct motion_sensor_t *s,
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
 
-#ifdef CONFIG_ACCEL_FIFO
-enum fifo_state {
-	FIFO_HEADER,
-	FIFO_DATA_SKIP,
-	FIFO_DATA_TIME,
-	FIFO_DATA_CONFIG,
-};
-
-
-#define BMI160_FIFO_BUFFER 64
-static uint8_t bmi160_buffer[BMI160_FIFO_BUFFER];
-/*
- * Decode the header from the fifo.
- * Return 0 if we need further processing.
- * Sensor mutex must be held during processing, to protect the fifos.
- *
- * @s: base sensor
- * @hdr: the header to decode
- * @bp: current pointer in the buffer, updated when processing the header.
- * @ep: pointer to the end of the valid data in the buffer.
- */
-static int bmi160_decode_header(struct motion_sensor_t *accel,
-		enum fifo_header hdr, uint32_t last_ts,
-		uint8_t **bp, uint8_t *ep)
-{
-	if ((hdr & BMI160_FH_MODE_MASK) == BMI160_EMPTY &&
-			(hdr & BMI160_FH_PARM_MASK) != 0) {
-		int i, size = 0;
-		/* Check if there is enough space for the data frame */
-		for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL;
-		     i--) {
-			if (hdr & (1 << (i + BMI160_FH_PARM_OFFSET)))
-				size += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
-		}
-		if (*bp + size > ep) {
-			/* frame is not complete, it will be retransmitted. */
-			*bp = ep;
-			return 1;
-		}
-		for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL;
-		     i--) {
-			struct motion_sensor_t *s = accel + i;
-
-			if (hdr & (1 << (i + BMI160_FH_PARM_OFFSET))) {
-				struct ec_response_motion_sensor_data vector;
-				int *v = s->raw_xyz;
-				vector.flags = 0;
-				normalize(s, v, *bp);
-#ifdef CONFIG_ACCEL_SPOOF_MODE
-				if (s->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE)
-					v = s->spoof_xyz;
-#endif  /* defined(CONFIG_ACCEL_SPOOF_MODE) */
-				vector.data[X] = v[X];
-				vector.data[Y] = v[Y];
-				vector.data[Z] = v[Z];
-				vector.sensor_num = s - motion_sensors;
-				motion_sense_fifo_add_data(&vector, s, 3,
-						last_ts);
-				*bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
-			}
-		}
-		return 1;
-	} else {
-		return 0;
-	}
-}
-
-/**
- * Retrieve hardware FIFO from sensor,
- * - put data in Sensor Hub fifo.
- * - update sensor raw_xyz vector with the last information.
- * We put raw data in hub fifo and process data from there.
- * @s Pointer to sensor data.
- *
- * Read only up to  bmi160_buffer. If more reads are needed, we will be called
- * again by the interrupt routine.
- *
- * NOTE: If a new driver supports this function, be sure to add a check
- * for spoof_mode in order to load the sensor stack with the spoofed
- * data.  See accelgyro_bmi160.c::load_fifo for an example.
- */
-static int load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
-{
-	struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
-	uint16_t length;
-	enum fifo_state state = FIFO_HEADER;
-	uint8_t *bp = bmi160_buffer;
-	uint8_t *ep;
-	uint32_t beginning;
-
-
-	if (s->type != MOTIONSENSE_TYPE_ACCEL)
-		return EC_SUCCESS;
-
-	if (!(data->flags &
-	     (BMI160_FIFO_ALL_MASK << BMI160_FIFO_FLAG_OFFSET))) {
-		/*
-		 * The FIFO was disabled while we were processing it.
-		 *
-		 * Flush potential left over:
-		 * When sensor is resumed, we won't read old data.
-		 */
-		raw_write8(s->port, s->addr, BMI160_CMD_REG,
-				BMI160_CMD_FIFO_FLUSH);
-		return EC_SUCCESS;
-	}
-
-	raw_read_n(s->port, s->addr, BMI160_FIFO_LENGTH_0,
-			(uint8_t *)&length, sizeof(length));
-	length &= BMI160_FIFO_LENGTH_MASK;
-
-	/*
-	 * We have not requested timestamp, no extra frame to read.
-	 * if we have too much to read, read the whole buffer.
-	 */
-	if (length == 0) {
-		CPRINTS("unexpected empty FIFO");
-		return EC_SUCCESS;
-	}
-
-	/* Add one byte to get an empty FIFO frame.*/
-	length++;
-
-	if (length > sizeof(bmi160_buffer))
-		CPRINTS("unexpected large FIFO: %d", length);
-	length = MIN(length, sizeof(bmi160_buffer));
-
-
-	raw_read_n(s->port, s->addr, BMI160_FIFO_DATA, bmi160_buffer,
-			length);
-	beginning = *(uint32_t *)bmi160_buffer;
-	ep = bmi160_buffer + length;
-	/*
-	 * FIFO is invalid when reading while the sensors are all
-	 * suspended.
-	 * Instead of returning the empty frame, it can return a
-	 * pattern that looks like a valid header: 84 or 40.
-	 * If we see those, assume the sensors have been disabled
-	 * while this thread was running.
-	 */
-	if (beginning == 0x84848484 ||
-			(beginning & 0xdcdcdcdc) == 0x40404040) {
-		CPRINTS("Suspended FIFO: accel ODR/rate: %d/%d: 0x%08x",
-				BASE_ODR(s->config[SENSOR_CONFIG_AP].odr),
-				get_data_rate(s),
-				beginning);
-		return EC_SUCCESS;
-	}
-
-	while (bp < ep) {
-		switch (state) {
-		case FIFO_HEADER: {
-			enum fifo_header hdr = *bp++;
-
-			if (bmi160_decode_header(s, hdr, last_ts, &bp, ep))
-				continue;
-			/* Other cases */
-			hdr &= 0xdc;
-			switch (hdr) {
-			case BMI160_EMPTY:
-				return EC_SUCCESS;
-			case BMI160_SKIP:
-				state = FIFO_DATA_SKIP;
-				break;
-			case BMI160_TIME:
-				state = FIFO_DATA_TIME;
-				break;
-			case BMI160_CONFIG:
-				state = FIFO_DATA_CONFIG;
-				break;
-			default:
-				CPRINTS("Unknown header: 0x%02x @ %d",
-						hdr, bp - bmi160_buffer);
-				raw_write8(s->port, s->addr,
-						BMI160_CMD_REG,
-						BMI160_CMD_FIFO_FLUSH);
-				return EC_ERROR_NOT_HANDLED;
-			}
-			break;
-		}
-		case FIFO_DATA_SKIP:
-			CPRINTS("@ %d - %d, skipped %d frames",
-					bp - bmi160_buffer, length, *bp);
-			bp++;
-			state = FIFO_HEADER;
-			break;
-		case FIFO_DATA_CONFIG:
-			CPRINTS("@ %d - %d, config change: 0x%02x",
-					bp - bmi160_buffer, length, *bp);
-			bp++;
-			state = FIFO_HEADER;
-			break;
-		case FIFO_DATA_TIME:
-			if (bp + 3 > ep) {
-				bp = ep;
-				continue;
-			}
-			/* We are not requesting timestamp */
-			CPRINTS("timestamp %d", (bp[2] << 16) |
-					(bp[1] << 8) | bp[0]);
-			state = FIFO_HEADER;
-			bp += 3;
-			break;
-		default:
-			CPRINTS("Unknown data: 0x%02x", *bp++);
-			state = FIFO_HEADER;
-		}
-	}
-	return EC_SUCCESS;
-}
-#endif  /* CONFIG_ACCEL_FIFO */
-
 /**
  * bmi160_interrupt - called when the sensor activates the interrupt line.
  *
@@ -992,7 +351,6 @@ void bmi160_interrupt(enum gpio_signal signal)
 		       CONFIG_ACCELGYRO_BMI160_INT_EVENT, 0);
 }
 
-
 static int config_interrupt(const struct motion_sensor_t *s)
 {
 	int ret, tmp;
@@ -1001,35 +359,35 @@ static int config_interrupt(const struct motion_sensor_t *s)
 		return EC_SUCCESS;
 
 	mutex_lock(s->mutex);
-	raw_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_FIFO_FLUSH);
-	raw_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_INT_RESET);
+	bmi_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_FIFO_FLUSH);
+	bmi_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_INT_RESET);
 
 #ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
-	raw_write8(s->port, s->addr, BMI160_INT_TAP_0,
+	bmi_write8(s->port, s->addr, BMI160_INT_TAP_0,
 		BMI160_TAP_DUR(s, CONFIG_GESTURE_TAP_MAX_INTERSTICE_T));
-	ret = raw_write8(s->port, s->addr, BMI160_INT_TAP_1,
+	ret = bmi_write8(s->port, s->addr, BMI160_INT_TAP_1,
 		BMI160_TAP_TH(s, CONFIG_GESTURE_TAP_THRES_MG));
 #endif
 #ifdef CONFIG_BMI160_ORIENTATION_SENSOR
 	/* only use orientation sensor on the lid sensor */
 	if (s->location == MOTIONSENSE_LOC_LID) {
-		ret = raw_write8(s->port, s->addr, BMI160_INT_ORIENT_0,
+		ret = bmi_write8(s->port, s->addr, BMI160_INT_ORIENT_0,
 			BMI160_INT_ORIENT_0_INIT_VAL);
-		ret = raw_write8(s->port, s->addr, BMI160_INT_ORIENT_1,
+		ret = bmi_write8(s->port, s->addr, BMI160_INT_ORIENT_1,
 			BMI160_INT_ORIENT_1_INIT_VAL);
 	}
 #endif
 
 #ifdef CONFIG_ACCELGYRO_BMI160_INT2_OUTPUT
-	ret = raw_write8(s->port, s->addr, BMI160_INT_LATCH, BMI160_LATCH_5MS);
+	ret = bmi_write8(s->port, s->addr, BMI160_INT_LATCH, BMI160_LATCH_5MS);
 #else
 	/* Also, configure int2 as an external input. */
-	ret = raw_write8(s->port, s->addr, BMI160_INT_LATCH,
+	ret = bmi_write8(s->port, s->addr, BMI160_INT_LATCH,
 		BMI160_INT2_INPUT_EN | BMI160_LATCH_5MS);
 #endif
 
 	/* configure int1 as an interrupt */
-	ret = raw_write8(s->port, s->addr, BMI160_INT_OUT_CTRL,
+	ret = bmi_write8(s->port, s->addr, BMI160_INT_OUT_CTRL,
 		BMI160_INT_CTRL(1, OUTPUT_EN));
 
 	/* Map activity interrupt to int 1 */
@@ -1045,29 +403,28 @@ static int config_interrupt(const struct motion_sensor_t *s)
 	if (s->location == MOTIONSENSE_LOC_LID)
 		tmp |= BMI160_INT_ORIENT;
 #endif
-	ret = raw_write8(s->port, s->addr, BMI160_INT_MAP_REG(1), tmp);
+	ret = bmi_write8(s->port, s->addr, BMI160_INT_MAP_REG(1), tmp);
 
 #ifdef CONFIG_ACCEL_FIFO
 	/* map fifo water mark to int 1 */
-	ret = raw_write8(s->port, s->addr, BMI160_INT_FIFO_MAP,
+	ret = bmi_write8(s->port, s->addr, BMI160_INT_FIFO_MAP,
 			BMI160_INT_MAP(1, FWM) |
 			BMI160_INT_MAP(1, FFULL));
 
 	/* configure fifo watermark to int whenever there's any data in there */
-	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_0, 1);
+	ret = bmi_write8(s->port, s->addr, BMI160_FIFO_CONFIG_0, 1);
 #ifdef CONFIG_ACCELGYRO_BMI160_INT2_OUTPUT
-	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
+	ret = bmi_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
 			BMI160_FIFO_HEADER_EN);
 #else
-	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
+	ret = bmi_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
 			BMI160_FIFO_TAG_INT2_EN |
 			BMI160_FIFO_HEADER_EN);
 #endif
 
-	/* Set fifo*/
-	ret = raw_read8(s->port, s->addr, BMI160_INT_EN_1, &tmp);
-	tmp |= BMI160_INT_FWM_EN | BMI160_INT_FFUL_EN;
-	ret = raw_write8(s->port, s->addr, BMI160_INT_EN_1, tmp);
+		/* Set fifo*/
+		bmi_enable_reg8(s, BMI160_INT_EN_1,
+				BMI160_INT_FWM_EN | BMI160_INT_FFUL_EN, 1);
 #endif
 	mutex_unlock(s->mutex);
 	return ret;
@@ -1079,11 +436,11 @@ static void irq_set_orientation(struct motion_sensor_t *s,
 {
 	int shifted_masked_orientation =
 		(interrupt >> 24) & BMI160_ORIENT_XY_MASK;
-	if (BMI160_GET_DATA(s)->raw_orientation != shifted_masked_orientation) {
+	if (BMI_GET_DATA(s)->bmi_orientation != shifted_masked_orientation) {
 		enum motionsensor_orientation orientation =
 			MOTIONSENSE_ORIENTATION_UNKNOWN;
 
-		BMI160_GET_DATA(s)->raw_orientation =
+		BMI_GET_DATA(s)->bmi_orientation =
 			shifted_masked_orientation;
 
 		switch (shifted_masked_orientation) {
@@ -1126,7 +483,7 @@ static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
 		return EC_ERROR_NOT_HANDLED;
 
 	do {
-		rv = raw_read32(s->port, s->addr, BMI160_INT_STATUS_0,
+		rv = bmi_read32(s->port, s->addr, BMI160_INT_STATUS_0,
 				&interrupt);
 		/*
 		 * Bail out of this loop there was an error reading the register
@@ -1146,7 +503,7 @@ static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
 #endif
 #ifdef CONFIG_ACCEL_FIFO
 		if (interrupt & (BMI160_FWM_INT | BMI160_FFULL_INT))
-			load_fifo(s, last_interrupt_timestamp);
+			bmi_load_fifo(s, last_interrupt_timestamp);
 #endif
 #ifdef CONFIG_BMI160_ORIENTATION_SENSOR
 		irq_set_orientation(s, interrupt);
@@ -1157,67 +514,35 @@ static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
 }
 #endif  /* CONFIG_ACCEL_INTERRUPTS */
 
-
-static int read(const struct motion_sensor_t *s, intv3_t v)
-{
-	uint8_t data[6];
-	int ret, status = 0;
-
-	ret = raw_read8(s->port, s->addr, BMI160_STATUS, &status);
-	if (ret != EC_SUCCESS)
-		return ret;
-
-	/*
-	 * If sensor data is not ready, return the previous read data.
-	 * 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 (v != s->raw_xyz)
-			memcpy(v, s->raw_xyz, sizeof(s->raw_xyz));
-		return EC_SUCCESS;
-	}
-
-	/* Read 6 bytes starting at xyz_reg */
-	ret = raw_read_n(s->port, s->addr, get_xyz_reg(s->type), data, 6);
-
-	if (ret != EC_SUCCESS) {
-		CPRINTS("%s: type:0x%X RD XYZ Error %d", s->name, s->type, ret);
-		return ret;
-	}
-	normalize(s, v, data);
-	return EC_SUCCESS;
-}
-
 static int init(const struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
-	struct accelgyro_saved_data_t *data = BMI160_GET_SAVED_DATA(s);
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
 
-	ret = raw_read8(s->port, s->addr, BMI160_CHIP_ID, &tmp);
+	ret = bmi_read8(s->port, s->addr, BMI160_CHIP_ID, &tmp);
 	if (ret)
 		return EC_ERROR_UNKNOWN;
 
 	if (tmp != BMI160_CHIP_ID_MAJOR && tmp != BMI168_CHIP_ID_MAJOR) {
 		/* The device may be lock on paging mode. Try to unlock it. */
-		raw_write8(s->port, s->addr, BMI160_CMD_REG,
+		bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 				BMI160_CMD_EXT_MODE_EN_B0);
-		raw_write8(s->port, s->addr, BMI160_CMD_REG,
+		bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 				BMI160_CMD_EXT_MODE_EN_B1);
-		raw_write8(s->port, s->addr, BMI160_CMD_REG,
+		bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 				BMI160_CMD_EXT_MODE_EN_B2);
-		raw_write8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
+		bmi_write8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
 				BMI160_CMD_PAGING_EN);
-		raw_write8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR, 0);
+		bmi_write8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR, 0);
 		return EC_ERROR_ACCESS_DENIED;
 	}
 
 
 	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
-		struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
+		struct bmi_drv_data_t *data = BMI_GET_DATA(s);
 
 		/* Reset the chip to be in a good state */
-		raw_write8(s->port, s->addr, BMI160_CMD_REG,
+		bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 				BMI160_CMD_SOFT_RESET);
 		msleep(1);
 		data->flags &= ~(BMI160_FLAG_SEC_I2C_ENABLED |
@@ -1236,18 +561,18 @@ static int init(const struct motion_sensor_t *s)
 #endif
 #endif
 		/* To avoid gyro wakeup */
-		raw_write8(s->port, s->addr, BMI160_PMU_TRIGGER, 0);
+		bmi_write8(s->port, s->addr, BMI160_PMU_TRIGGER, 0);
 	}
 
 #ifdef CONFIG_BMI160_SEC_I2C
 	if (s->type == MOTIONSENSE_TYPE_MAG) {
-		struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
+		struct bmi_drv_data_t *data = BMI_GET_DATA(s);
 
 		/*
 		 * To be able to configure the real magnetometer, we must set
 		 * the BMI160 magnetometer part (a pass through) in normal mode.
 		 */
-		raw_write8(s->port, s->addr, BMI160_CMD_REG,
+		bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 				BMI160_CMD_MODE_NORMAL(s->type));
 		msleep(wakeup_time[s->type]);
 
@@ -1261,42 +586,34 @@ static int init(const struct motion_sensor_t *s)
 			 *
 			 * Magic command sequences
 			 */
-			raw_write8(s->port, s->addr, BMI160_CMD_REG,
+			bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 					BMI160_CMD_EXT_MODE_EN_B0);
-			raw_write8(s->port, s->addr, BMI160_CMD_REG,
+			bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 					BMI160_CMD_EXT_MODE_EN_B1);
-			raw_write8(s->port, s->addr, BMI160_CMD_REG,
+			bmi_write8(s->port, s->addr, BMI160_CMD_REG,
 					BMI160_CMD_EXT_MODE_EN_B2);
 
 			/*
 			 * Change the register page to target mode, to change
 			 * the internal pull ups of the secondary interface.
 			 */
-			raw_read8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
-					&ext_page_reg);
-			raw_write8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
-					ext_page_reg | BMI160_CMD_TARGET_PAGE);
-			raw_read8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
-					&ext_page_reg);
-			raw_write8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
-					ext_page_reg | BMI160_CMD_PAGING_EN);
-			raw_read8(s->port, s->addr, BMI160_COM_C_TRIM_ADDR,
-					&pullup_reg);
-			raw_write8(s->port, s->addr, BMI160_COM_C_TRIM_ADDR,
-					pullup_reg | BMI160_COM_C_TRIM);
-			raw_read8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
-					&ext_page_reg);
-			raw_write8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
-					ext_page_reg & ~BMI160_CMD_TARGET_PAGE);
-			raw_read8(s->port, s->addr, BMI160_CMD_EXT_MODE_ADDR,
-					&ext_page_reg);
+			bmi_enable_reg8(s, BMI160_CMD_EXT_MODE_ADDR,
+					BMI160_CMD_TARGET_PAGE, 1);
+			bmi_enable_reg8(s, BMI160_CMD_EXT_MODE_ADDR,
+					BMI160_CMD_PAGING_EN, 1);
+			bmi_enable_reg8(s, BMI160_COM_C_TRIM_ADDR,
+					BMI160_COM_C_TRIM, 1);
+			bmi_enable_reg8(s, BMI160_CMD_EXT_MODE_ADDR,
+					BMI160_CMD_TARGET_PAGE, 0);
+			bmi_read8(s->port, s->i2c_spi_addr_flags,
+					BMI160_CMD_EXT_MODE_ADDR, &ext_page_reg);
 
 			/* Set the i2c address of the compass */
-			ret = raw_write8(s->port, s->addr, BMI160_MAG_IF_0,
+			ret = bmi_write8(s->port, s->addr, BMI160_MAG_IF_0,
 					CONFIG_ACCELGYRO_SEC_ADDR);
 
 			/* Enable the secondary interface as I2C */
-			ret = raw_write8(s->port, s->addr, BMI160_IF_CONF,
+			ret = bmi_write8(s->port, s->addr, BMI160_IF_CONF,
 				BMI160_IF_MODE_AUTO_I2C << BMI160_IF_MODE_OFF);
 			data->flags |= BMI160_FLAG_SEC_I2C_ENABLED;
 		}
@@ -1310,7 +627,7 @@ static int init(const struct motion_sensor_t *s)
 			return ret;
 
 		/* Leave the address for reading the data */
-		raw_write8(s->port, s->addr, BMI160_MAG_I2C_READ_ADDR,
+		bmi_write8(s->port, s->addr, BMI160_MAG_I2C_READ_ADDR,
 				BMM150_BASE_DATA);
 		/*
 		 * Put back the secondary interface in normal mode.
@@ -1325,7 +642,7 @@ static int init(const struct motion_sensor_t *s)
 	 * so set data rate to 0.
 	 */
 	data->odr = 0;
-	set_range(s, s->default_range, 0);
+	bmi_set_range(s, s->default_range, 0);
 
 	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
 #ifdef CONFIG_ACCEL_INTERRUPTS
@@ -1338,14 +655,16 @@ static int init(const struct motion_sensor_t *s)
 
 const struct accelgyro_drv bmi160_drv = {
 	.init = init,
-	.read = read,
-	.set_range = set_range,
-	.get_range = get_range,
-	.get_resolution = get_resolution,
+	.read = bmi_read,
+	.set_range = bmi_set_range,
+	.get_range = bmi_get_range,
+	.get_resolution = bmi_get_resolution,
 	.set_data_rate = set_data_rate,
-	.get_data_rate = get_data_rate,
+	.get_data_rate = bmi_get_data_rate,
 	.set_offset = set_offset,
-	.get_offset = get_offset,
+	.get_scale = bmi_get_scale,
+	.set_scale = bmi_set_scale,
+	.get_offset = bmi_get_offset,
 	.perform_calib = perform_calib,
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	.irq_handler = irq_handler,
@@ -1363,23 +682,12 @@ struct i2c_stress_test_dev bmi160_i2c_stress_test_dev = {
 		.read_val = BMI160_CHIP_ID_MAJOR,
 		.write_reg = BMI160_PMU_TRIGGER,
 	},
-	.i2c_read = &raw_read8,
-	.i2c_write = &raw_write8,
+	.i2c_read = &bmi_read8,
+	.i2c_write = &bmi_write8,
 };
 #endif /* CONFIG_CMD_I2C_STRESS_TEST_ACCEL */
 
 int bmi160_get_sensor_temp(int idx, int *temp_ptr)
 {
-	struct motion_sensor_t *s = &motion_sensors[idx];
-	int16_t temp;
-	int ret;
-
-	ret = raw_read_n(s->port, s->addr, BMI160_TEMPERATURE_0,
-			 (uint8_t *)&temp, sizeof(temp));
-
-	if (ret || temp == BMI160_INVALID_TEMP)
-		return EC_ERROR_NOT_POWERED;
-
-	*temp_ptr = C_TO_K(23 + ((temp + 256) >> 9));
-	return 0;
+	return bmi_get_sensor_temp(idx, temp_ptr);
 }
diff --git a/driver/accelgyro_bmi160.h b/driver/accelgyro_bmi160.h
index 5b934b57a2..5bf12d636e 100644
--- a/driver/accelgyro_bmi160.h
+++ b/driver/accelgyro_bmi160.h
@@ -11,22 +11,6 @@
 #include "accelgyro.h"
 #include "mag_bmm150.h"
 
-/*
- * The addr field of motion_sensor support both SPI and I2C:
- *
- * +-------------------------------+---+
- * |    7 bit i2c address          | 0 |
- * +-------------------------------+---+
- * Or
- * +-------------------------------+---+
- * |    SPI device ID              | 1 |
- * +-------------------------------+---+
- */
-#define BMI160_SET_SPI_ADDRESS(_addr) (((_addr) << 1) | 1)
-#define BMI160_IS_SPI(_addr)        ((_addr) & 1)
-#define BMI160_SPI_ADDRESS(_addr)   ((_addr) >> 1)
-#define BMI160_I2C_ADDRESS(_addr)   (_addr)
-
 /* I2C addresses */
 #define BMI160_ADDR0             0xd0
 #define BMI160_ADDR1             0xd2
@@ -140,12 +124,7 @@
 #define BMI160_FIFO_LENGTH_1   0x23
 #define BMI160_FIFO_LENGTH_MASK    ((1 << 11) - 1)
 #define BMI160_FIFO_DATA       0x24
-enum fifo_header {
-	BMI160_EMPTY = 0x80,
-	BMI160_SKIP = 0x40,
-	BMI160_TIME = 0x44,
-	BMI160_CONFIG = 0x48
-};
+
 
 #define BMI160_FH_MODE_MASK    0xc0
 #define BMI160_FH_PARM_OFFSET    2
@@ -427,8 +406,6 @@ enum fifo_header {
 #define BMI160_FF_DATA_LEN_GYR          6
 #define BMI160_FF_DATA_LEN_MAG          8
 
-/* Sensor resolution in number of bits. This sensor has fixed resolution. */
-#define BMI160_RESOLUTION      16
 
 /* Min and Max sampling frequency in mHz */
 #define BMI160_ACCEL_MIN_FREQ 12500
@@ -438,18 +415,6 @@ enum fifo_header {
 
 extern const struct accelgyro_drv bmi160_drv;
 
-enum bmi160_running_mode {
-	STANDARD_UI_9DOF_FIFO          = 0,
-	STANDARD_UI_IMU_FIFO           = 1,
-	STANDARD_UI_IMU                = 2,
-	STANDARD_UI_ADVANCEPOWERSAVE   = 3,
-	ACCEL_PEDOMETER                = 4,
-	APPLICATION_HEAD_TRACKING      = 5,
-	APPLICATION_NAVIGATION         = 6,
-	APPLICATION_REMOTE_CONTROL     = 7,
-	APPLICATION_INDOOR_NAVIGATION  = 8,
-};
-
 #define BMI160_FLAG_SEC_I2C_ENABLED    (1 << 0)
 #define BMI160_FIFO_FLAG_OFFSET        4
 #define BMI160_FIFO_ALL_MASK           7
@@ -470,27 +435,6 @@ struct bmi160_drv_data_t {
 
 };
 
-#define BMI160_GET_DATA(_s) \
-	((struct bmi160_drv_data_t *)(_s)->drv_data)
-#define BMI160_GET_SAVED_DATA(_s) \
-	(&BMI160_GET_DATA(_s)->saved_data[(_s)->type])
-
-#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
-#define ORIENTATION_CHANGED(_sensor) \
-	(BMI160_GET_DATA(_sensor)->orientation != \
-	BMI160_GET_DATA(_sensor)->last_orientation)
-
-#define GET_ORIENTATION(_sensor) \
-	(BMI160_GET_DATA(_sensor)->orientation)
-
-#define SET_ORIENTATION(_sensor, _val) \
-	(BMI160_GET_DATA(_sensor)->orientation = _val)
-
-#define SET_ORIENTATION_UPDATED(_sensor) \
-	(BMI160_GET_DATA(_sensor)->last_orientation = \
-	BMI160_GET_DATA(_sensor)->orientation)
-#endif
-
 void bmi160_interrupt(enum gpio_signal signal);
 
 #ifdef CONFIG_BMI160_SEC_I2C
diff --git a/driver/accelgyro_bmi_common.c b/driver/accelgyro_bmi_common.c
new file mode 100644
index 0000000000..fa5d4492c1
--- /dev/null
+++ b/driver/accelgyro_bmi_common.c
@@ -0,0 +1,831 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/**
+ * BMI accelerometer and gyro module for Chrome EC
+ * 3D digital accelerometer & 3D digital gyroscope
+ */
+
+
+#include "accelgyro.h"
+#include "console.h"
+#include "driver/accelgyro_bmi_common.h"
+#include "i2c.h"
+#include "math_util.h"
+#include "spi.h"
+
+#define CPUTS(outstr) cputs(CC_ACCEL, outstr)
+#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
+
+/* List of range values in +/-G's and their associated register values. */
+const struct bmi_accel_param_pair g_ranges[4] = {
+	{2, BMI160_GSEL_2G},
+	{4, BMI160_GSEL_4G},
+	{8, BMI160_GSEL_8G},
+	{16, BMI160_GSEL_16G}
+};
+
+/*
+ * List of angular rate range values in +/-dps's
+ * and their associated register values.
+ */
+const struct bmi_accel_param_pair dps_ranges[5] = {
+	{125, BMI160_DPS_SEL_125},
+	{250, BMI160_DPS_SEL_250},
+	{500, BMI160_DPS_SEL_500},
+	{1000, BMI160_DPS_SEL_1000},
+	{2000, BMI160_DPS_SEL_2000}
+};
+
+int bmi_get_xyz_reg(const struct motion_sensor_t *s)
+{
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		return BMI_ACC_DATA;
+	case MOTIONSENSE_TYPE_GYRO:
+		return BMI_GYR_DATA;
+	case MOTIONSENSE_TYPE_MAG:
+		return BMI_AUX_DATA;
+	default:
+		return -1;
+	}
+}
+
+const struct bmi_accel_param_pair *bmi_get_range_table(
+		const struct motion_sensor_t *s, int *psize)
+{
+	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
+		if (psize)
+			*psize = ARRAY_SIZE(g_ranges);
+		return g_ranges;
+	}
+	if (psize)
+		*psize = ARRAY_SIZE(dps_ranges);
+	return dps_ranges;
+
+}
+
+/**
+ * @return reg value that matches the given engineering value passed in.
+ * The round_up flag is used to specify whether to round up or down.
+ * Note, this function always returns a valid reg value. If the request is
+ * outside the range of values, it returns the closest valid reg value.
+ */
+int bmi_get_reg_val(const int eng_val, const int round_up,
+		const struct bmi_accel_param_pair *pairs, const int size)
+{
+	int i;
+
+	for (i = 0; i < size - 1; i++) {
+		if (eng_val <= pairs[i].val)
+			break;
+
+		if (eng_val < pairs[i+1].val) {
+			if (round_up)
+				i += 1;
+			break;
+		}
+	}
+	return pairs[i].reg_val;
+}
+
+/**
+ * @return engineering value that matches the given reg val
+ */
+int bmi_get_engineering_val(const int reg_val,
+		const struct bmi_accel_param_pair *pairs, const int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		if (reg_val == pairs[i].reg_val)
+			break;
+	}
+	return pairs[i].val;
+}
+
+#ifdef CONFIG_SPI_ACCEL_PORT
+int bmi_spi_raw_read(const int addr, const uint8_t reg,
+		     uint8_t *data, const int len)
+{
+	uint8_t cmd = 0x80 | reg;
+
+	return spi_transaction(&spi_devices[addr], &cmd, 1, data, len);
+}
+#endif
+
+/**
+ * Read 8bit register from accelerometer.
+ */
+int bmi_read8(const int port, const uint16_t addr,
+	      const int reg, int *data_ptr)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	if (BMI_IS_SPI(addr)) {
+#ifdef CONFIG_SPI_ACCEL_PORT
+		uint8_t val;
+
+		rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr),
+				  reg, &val, 1);
+		if (rv == EC_SUCCESS)
+			*data_ptr = val;
+#endif
+	} else {
+#ifdef I2C_PORT_ACCEL
+		rv = i2c_read8(port, addr,
+			       reg, data_ptr);
+#endif
+	}
+	return rv;
+}
+
+/**
+ * Write 8bit register from accelerometer.
+ */
+int bmi_write8(const int port, const uint16_t addr,
+	       const int reg, int data)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	if (BMI_IS_SPI(addr)) {
+#ifdef CONFIG_SPI_ACCEL_PORT
+		uint8_t cmd[2] = { reg, data };
+
+		rv = spi_transaction(
+			&spi_devices[SLAVE_GET_SPI_ADDR(addr)],
+			cmd, 2, NULL, 0);
+#endif
+	} else {
+#ifdef I2C_PORT_ACCEL
+		rv = i2c_write8(port, addr,
+				reg, data);
+#endif
+	}
+	/*
+	 * From Bosch:  BMI needs a delay of 450us after each write if it
+	 * is in suspend mode, otherwise the operation may be ignored by
+	 * the sensor. Given we are only doing write during init, add
+	 * the delay unconditionally.
+	 */
+	msleep(1);
+	return rv;
+}
+
+/**
+ * Read 16bit register from accelerometer.
+ */
+int bmi_read16(const int port, const uint16_t addr,
+	       const uint8_t reg, int *data_ptr)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	if (BMI_IS_SPI(addr)) {
+#ifdef CONFIG_SPI_ACCEL_PORT
+		rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr),
+				  reg, (uint8_t *)data_ptr, 2);
+#endif
+	} else {
+#ifdef I2C_PORT_ACCEL
+		rv = i2c_read16(port, addr,
+				reg, data_ptr);
+#endif
+	}
+	return rv;
+}
+
+/**
+ * Write 16bit register from accelerometer.
+ */
+int bmi_write16(const int port, const uint16_t addr,
+		const int reg, int data)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	if (BMI_IS_SPI(addr)) {
+#ifdef CONFIG_SPI_ACCEL_PORT
+		CPRINTS("%s() spi part is not implemented", __func__);
+#endif
+	} else {
+#ifdef I2C_PORT_ACCEL
+		rv = i2c_write16(port, addr,
+				 reg, data);
+#endif
+	}
+	/*
+	 * From Bosch:  BMI needs a delay of 450us after each write if it
+	 * is in suspend mode, otherwise the operation may be ignored by
+	 * the sensor. Given we are only doing write during init, add
+	 * the delay unconditionally.
+	 */
+	msleep(1);
+	return rv;
+}
+
+/**
+ * Read 32bit register from accelerometer.
+ */
+int bmi_read32(const int port, const uint16_t addr,
+	       const uint8_t reg, int *data_ptr)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	if (BMI_IS_SPI(addr)) {
+#ifdef CONFIG_SPI_ACCEL_PORT
+		rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr),
+				  reg, (uint8_t *)data_ptr, 4);
+#endif
+	} else {
+#ifdef I2C_PORT_ACCEL
+		rv = i2c_read32(port, addr,
+				reg, data_ptr);
+#endif
+	}
+	return rv;
+}
+
+/**
+ * Read n bytes from accelerometer.
+ */
+int bmi_read_n(const int port, const uint16_t addr,
+	       const uint8_t reg, uint8_t *data_ptr, const int len)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	if (BMI_IS_SPI(addr)) {
+#ifdef CONFIG_SPI_ACCEL_PORT
+		rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(addr),
+				  reg, data_ptr, len);
+#endif
+	} else {
+#ifdef I2C_PORT_ACCEL
+		rv = i2c_read_block(port, addr,
+				    reg, data_ptr, len);
+#endif
+	}
+	return rv;
+}
+
+/**
+ * Write n bytes from accelerometer.
+ */
+int bmi_write_n(const int port, const uint16_t addr,
+		const uint8_t reg, const uint8_t *data_ptr, const int len)
+{
+	int rv = -EC_ERROR_PARAM1;
+
+	if (BMI_IS_SPI(addr)) {
+#ifdef CONFIG_SPI_ACCEL_PORT
+		CPRINTS("%s() spi part is not implemented", __func__);
+#endif
+	} else {
+#ifdef I2C_PORT_ACCEL
+		rv = i2c_write_block(port, addr,
+				     reg, data_ptr, len);
+#endif
+	}
+	/*
+	 * From Bosch:  BMI needs a delay of 450us after each write if it
+	 * is in suspend mode, otherwise the operation may be ignored by
+	 * the sensor. Given we are only doing write during init, add
+	 * the delay unconditionally.
+	 */
+	msleep(1);
+	return rv;
+}
+/*
+ * Enable/Disable specific bit set of a 8-bit reg.
+ */
+int bmi_enable_reg8(const struct motion_sensor_t *s,
+		    int reg, uint8_t bits, int enable)
+{
+	if (enable)
+		return bmi_set_reg8(s, reg, bits, 0);
+	return bmi_set_reg8(s, reg, 0, bits);
+}
+
+/*
+ * Set specific bit set to certain value of a 8-bit reg.
+ */
+int bmi_set_reg8(const struct motion_sensor_t *s,
+		 int reg, uint8_t bits, int mask)
+{
+	int ret, val;
+
+	ret = bmi_read8(s->port, s->addr, reg, &val);
+	if (ret)
+		return ret;
+	val = (val & ~mask) | bits;
+	ret = bmi_write8(s->port, s->addr, reg, val);
+	return ret;
+}
+
+void bmi_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *data)
+{
+
+#ifdef CONFIG_MAG_BMI_BMM150
+	if (s->type == MOTIONSENSE_TYPE_MAG)
+		bmm150_normalize(s, v, data);
+	else
+#endif
+#ifdef CONFIG_MAG_BMI_LIS2MDL
+	if (s->type == MOTIONSENSE_TYPE_MAG)
+		lis2mdl_normalize(s, v, data);
+	else
+#endif
+	{
+		v[0] = ((int16_t)((data[1] << 8) | data[0]));
+		v[1] = ((int16_t)((data[3] << 8) | data[2]));
+		v[2] = ((int16_t)((data[5] << 8) | data[4]));
+	}
+	rotate(v, *s->rot_standard_ref, v);
+}
+
+int bmi_decode_header(struct motion_sensor_t *accel,
+		enum fifo_header hdr, uint32_t last_ts,
+		uint8_t **bp, uint8_t *ep)
+{
+	if ((hdr & BMI_FH_MODE_MASK) == BMI_FH_EMPTY &&
+			(hdr & BMI_FH_PARM_MASK) != 0) {
+		int i, size = 0;
+		/* Check if there is enough space for the data frame */
+		for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL;
+		     i--) {
+			if (hdr & (1 << (i + BMI_FH_PARM_OFFSET)))
+				size += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
+		}
+		if (*bp + size > ep) {
+			/* frame is not complete, it will be retransmitted. */
+			*bp = ep;
+			return 1;
+		}
+		for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL;
+		     i--) {
+			struct motion_sensor_t *s = accel + i;
+
+			if (hdr & (1 << (i + BMI_FH_PARM_OFFSET))) {
+				struct ec_response_motion_sensor_data vector;
+				int *v = s->raw_xyz;
+
+				vector.flags = 0;
+				bmi_normalize(s, v, *bp);
+#ifdef CONFIG_ACCEL_SPOOF_MODE
+				if (s->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE)
+					v = s->spoof_xyz;
+#endif  /* defined(CONFIG_ACCEL_SPOOF_MODE) */
+				vector.data[X] = v[X];
+				vector.data[Y] = v[Y];
+				vector.data[Z] = v[Z];
+				vector.sensor_num = s - motion_sensors;
+				motion_sense_fifo_add_data(&vector, s, 3,
+						last_ts);
+				*bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
+			}
+		}
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+enum fifo_state {
+	FIFO_HEADER,
+	FIFO_DATA_SKIP,
+	FIFO_DATA_TIME,
+	FIFO_DATA_CONFIG,
+};
+
+#define BMI_FIFO_BUFFER 64
+static uint8_t bmi_buffer[BMI_FIFO_BUFFER];
+
+int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
+{
+	struct bmi_drv_data_t *data = BMI_GET_DATA(s);
+	uint16_t length;
+	enum fifo_state state = FIFO_HEADER;
+	uint8_t *bp = bmi_buffer;
+	uint8_t *ep;
+	uint32_t beginning;
+
+
+	if (s->type != MOTIONSENSE_TYPE_ACCEL)
+		return EC_SUCCESS;
+
+	if (!(data->flags &
+	     (BMI_FIFO_ALL_MASK << BMI_FIFO_FLAG_OFFSET))) {
+		/*
+		 * The FIFO was disabled while we were processing it.
+		 *
+		 * Flush potential left over:
+		 * When sensor is resumed, we won't read old data.
+		 */
+		bmi_write8(s->port, s->addr, BMI_CMD_REG,
+				BMI_CMD_FIFO_FLUSH);
+		return EC_SUCCESS;
+	}
+
+	bmi_read_n(s->port, s->addr, BMI_FIFO_LENGTH_0,
+			(uint8_t *)&length, sizeof(length));
+	length &= BMI_FIFO_LENGTH_MASK;
+
+	/*
+	 * We have not requested timestamp, no extra frame to read.
+	 * if we have too much to read, read the whole buffer.
+	 */
+	if (length == 0) {
+		CPRINTS("unexpected empty FIFO");
+		return EC_SUCCESS;
+	}
+
+	/* Add one byte to get an empty FIFO frame.*/
+	length++;
+
+	if (length > sizeof(bmi_buffer))
+		CPRINTS("unexpected large FIFO: %d", length);
+	length = MIN(length, sizeof(bmi_buffer));
+
+
+	bmi_read_n(s->port, s->addr, BMI_FIFO_DATA, bmi_buffer,
+			length);
+	beginning = *(uint32_t *)bmi_buffer;
+	ep = bmi_buffer + length;
+	/*
+	 * FIFO is invalid when reading while the sensors are all
+	 * suspended.
+	 * Instead of returning the empty frame, it can return a
+	 * pattern that looks like a valid header: 84 or 40.
+	 * If we see those, assume the sensors have been disabled
+	 * while this thread was running.
+	 */
+	if (beginning == 0x84848484 ||
+			(beginning & 0xdcdcdcdc) == 0x40404040) {
+		CPRINTS("Suspended FIFO: accel ODR/rate: %d/%d: 0x%08x",
+				BASE_ODR(s->config[SENSOR_CONFIG_AP].odr),
+				bmi_get_data_rate(s),
+				beginning);
+		return EC_SUCCESS;
+	}
+
+	while (bp < ep) {
+		switch (state) {
+		case FIFO_HEADER: {
+			enum fifo_header hdr = *bp++;
+
+			if (bmi_decode_header(s, hdr, last_ts, &bp, ep))
+				continue;
+			/* Other cases */
+			hdr &= 0xdc;
+			switch (hdr) {
+			case BMI_FH_EMPTY:
+				return EC_SUCCESS;
+			case BMI_FH_SKIP:
+				state = FIFO_DATA_SKIP;
+				break;
+			case BMI_FH_TIME:
+				state = FIFO_DATA_TIME;
+				break;
+			case BMI_FH_CONFIG:
+				state = FIFO_DATA_CONFIG;
+				break;
+			default:
+				CPRINTS("Unknown header: 0x%02x @ %d",
+						hdr, bp - bmi_buffer);
+				bmi_write8(s->port, s->addr,
+						BMI_CMD_REG,
+						BMI_CMD_FIFO_FLUSH);
+				return EC_ERROR_NOT_HANDLED;
+			}
+			break;
+		}
+		case FIFO_DATA_SKIP:
+			CPRINTS("@ %d - %d, skipped %d frames",
+					bp - bmi_buffer, length, *bp);
+			bp++;
+			state = FIFO_HEADER;
+			break;
+		case FIFO_DATA_CONFIG:
+			CPRINTS("@ %d - %d, config change: 0x%02x",
+					bp - bmi_buffer, length, *bp);
+			bp++;
+			state = FIFO_HEADER;
+			break;
+		case FIFO_DATA_TIME:
+			if (bp + 3 > ep) {
+				bp = ep;
+				continue;
+			}
+			/* We are not requesting timestamp */
+			CPRINTS("timestamp %d", (bp[2] << 16) |
+					(bp[1] << 8) | bp[0]);
+			state = FIFO_HEADER;
+			bp += 3;
+			break;
+		default:
+			CPRINTS("Unknown data: 0x%02x", *bp++);
+			state = FIFO_HEADER;
+		}
+	}
+	return EC_SUCCESS;
+}
+
+int bmi_set_range(const struct motion_sensor_t *s,
+				int range,
+				int rnd)
+{
+	int ret, range_tbl_size;
+	uint8_t reg_val, ctrl_reg;
+	const struct bmi_accel_param_pair *ranges;
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
+
+	if (s->type == MOTIONSENSE_TYPE_MAG) {
+		data->range = range;
+		return EC_SUCCESS;
+	}
+
+	ctrl_reg = BMI_RANGE_REG(s->type);
+	ranges = bmi_get_range_table(s, &range_tbl_size);
+	reg_val = bmi_get_reg_val(range, rnd, ranges, range_tbl_size);
+
+	ret = bmi_write8(s->port, s->addr, ctrl_reg, reg_val);
+	/* Now that we have set the range, update the driver's value. */
+	if (ret == EC_SUCCESS)
+		data->range = bmi_get_engineering_val(reg_val, ranges,
+				range_tbl_size);
+	return ret;
+}
+
+
+int bmi_get_range(const struct motion_sensor_t *s)
+{
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
+
+	return data->range;
+}
+
+int bmi_get_data_rate(const struct motion_sensor_t *s)
+{
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
+
+	return data->odr;
+}
+
+int bmi_get_offset(const struct motion_sensor_t *s,
+			int16_t    *offset,
+			int16_t    *temp)
+{
+	int i, val98;
+	intv3_t v;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/*
+		 * The offset of the accelerometer off_acc_[xyz] is a 8 bit
+		 * two-complement number in units of 3.9 mg independent of the
+		 * range selected for the accelerometer.
+		 */
+		bmi_accel_get_offset(s, v);
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		/* Read the MSB first */
+		bmi_read8(s->port, s->addr, BMI_OFFSET_EN_GYR98, &val98);
+		/*
+		 * The offset of the gyroscope off_gyr_[xyz] is a 10 bit
+		 * two-complement number in units of 0.061 °/s.
+		 * Therefore a maximum range that can be compensated is
+		 * -31.25 °/s to +31.25 °/s
+		 */
+		bmi_gyro_get_offset(s, v);
+		break;
+#ifdef CONFIG_MAG_BMI_BMM150
+	case MOTIONSENSE_TYPE_MAG:
+		bmm150_get_offset(s, v);
+		break;
+#endif /* defined(CONFIG_MAG_BMI_BMM150) */
+	default:
+		for (i = X; i <= Z; i++)
+			v[i] = 0;
+	}
+	rotate(v, *s->rot_standard_ref, v);
+	offset[X] = v[X];
+	offset[Y] = v[Y];
+	offset[Z] = v[Z];
+	/* Saving temperature at calibration not supported yet */
+	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
+	return EC_SUCCESS;
+}
+
+int bmi_get_resolution(const struct motion_sensor_t *s)
+{
+	return BMI_RESOLUTION;
+}
+
+int bmi_set_scale(const struct motion_sensor_t *s,
+	      const uint16_t *scale, int16_t temp)
+{
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
+
+	data->scale[X] = scale[X];
+	data->scale[Y] = scale[Y];
+	data->scale[Z] = scale[Z];
+	return EC_SUCCESS;
+}
+
+int bmi_get_scale(const struct motion_sensor_t *s,
+	      uint16_t *scale, int16_t *temp)
+{
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
+
+	scale[X] = data->scale[X];
+	scale[Y] = data->scale[Y];
+	scale[Z] = data->scale[Z];
+	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
+	return EC_SUCCESS;
+}
+
+int bmi_enable_fifo(const struct motion_sensor_t *s, int enable)
+{
+	struct bmi_drv_data_t *data = BMI_GET_DATA(s);
+	int ret;
+
+	/* FIFO start/stop collecting events */
+	ret = bmi_enable_reg8(s, BMI_FIFO_CONFIG_1,
+			      BMI_FIFO_SENSOR_EN(s->type), enable);
+	if (ret)
+		return ret;
+
+	if (enable)
+		data->flags |= 1 << (s->type + BMI_FIFO_FLAG_OFFSET);
+	else
+		data->flags &= ~(1 << (s->type + BMI_FIFO_FLAG_OFFSET));
+
+	return ret;
+}
+
+int bmi_read(const struct motion_sensor_t *s, intv3_t v)
+{
+	uint8_t data[6];
+	int ret, status = 0;
+
+	ret = bmi_read8(s->port, s->addr, BMI_STATUS, &status);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/*
+	 * If sensor data is not ready, return the previous read data.
+	 * Note: return success so that motion senor task can read again
+	 * to get the latest updated sensor data quickly.
+	 */
+	if (!(status & BMI_DRDY_MASK(s->type))) {
+		if (v != s->raw_xyz)
+			memcpy(v, s->raw_xyz, sizeof(s->raw_xyz));
+		return EC_SUCCESS;
+	}
+
+	/* Read 6 bytes starting at xyz_reg */
+	ret = bmi_read_n(s->port, s->addr, bmi_get_xyz_reg(s), data, 6);
+
+	if (ret != EC_SUCCESS) {
+		CPRINTS("%s: type:0x%X RD XYZ Error %d", s->name, s->type, ret);
+		return ret;
+	}
+	bmi_normalize(s, v, data);
+	return EC_SUCCESS;
+}
+
+int bmi_read_temp(const struct motion_sensor_t *s, int *temp_ptr)
+{
+	return bmi_get_sensor_temp(s - motion_sensors, temp_ptr);
+}
+
+int bmi_get_sensor_temp(int idx, int *temp_ptr)
+{
+	struct motion_sensor_t *s = &motion_sensors[idx];
+	int16_t temp;
+	int ret;
+
+	ret = bmi_read_n(s->port, s->addr,
+			 BMI_TEMPERATURE_0,
+			 (uint8_t *)&temp, sizeof(temp));
+
+	if (ret || temp == BMI_INVALID_TEMP)
+		return EC_ERROR_NOT_POWERED;
+
+	*temp_ptr = C_TO_K(23 + ((temp + 256) >> 9));
+	return 0;
+}
+
+int bmi_get_normalized_rate(const struct motion_sensor_t *s, int rate, int rnd,
+			    int *normalized_rate_ptr, uint8_t *reg_val_ptr)
+{
+	*reg_val_ptr = BMI_ODR_TO_REG(rate);
+	*normalized_rate_ptr = BMI_REG_TO_ODR(*reg_val_ptr);
+	if (rnd && (*normalized_rate_ptr < rate)) {
+		(*reg_val_ptr)++;
+		*normalized_rate_ptr = BMI_REG_TO_ODR(*reg_val_ptr);
+	}
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		if (*normalized_rate_ptr > BMI_ACCEL_MAX_FREQ ||
+		    *normalized_rate_ptr < BMI_ACCEL_MIN_FREQ)
+			return EC_RES_INVALID_PARAM;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		if (*normalized_rate_ptr > BMI_GYRO_MAX_FREQ ||
+		    *normalized_rate_ptr < BMI_GYRO_MIN_FREQ)
+			return EC_RES_INVALID_PARAM;
+		break;
+#ifdef CONFIG_MAG_BMI_BMM150
+	case MOTIONSENSE_TYPE_MAG:
+		/* We use the regular preset we can go about 100Hz */
+		if (*reg_val_ptr > BMI_ODR_100HZ ||
+		    *reg_val_ptr < BMI_ODR_0_78HZ)
+			return EC_RES_INVALID_PARAM;
+		break;
+#endif
+
+	default:
+		return EC_RES_INVALID_PARAM;
+	}
+	return EC_SUCCESS;
+}
+
+void bmi_accel_get_offset(const struct motion_sensor_t *accel, intv3_t v)
+{
+	int i, val;
+
+	for (i = X; i <= Z; i++) {
+		bmi_read8(accel->port, accel->addr,
+			  BMI_OFFSET_ACC70 + i, &val);
+		if (val > 0x7f)
+			val = -256 + val;
+		v[i] = round_divide(
+			(int64_t)val * BMI_OFFSET_ACC_MULTI_MG,
+			BMI_OFFSET_ACC_DIV_MG);
+
+	}
+}
+
+void bmi_gyro_get_offset(const struct motion_sensor_t *gyro, intv3_t v)
+{
+	int i, val, val98;
+
+	/* Read the MSB first */
+	bmi_read8(gyro->port, gyro->addr,
+		  BMI_OFFSET_EN_GYR98, &val98);
+	for (i = X; i <= Z; i++) {
+		bmi_read8(gyro->port, gyro->addr,
+			  BMI_OFFSET_GYR70 + i, &val);
+		val |= ((val98 >> (2 * i)) & 0x3) << 8;
+		if (val > 0x1ff)
+			val = -1024 + val;
+		v[i] = round_divide(
+			(int64_t)val * BMI_OFFSET_GYRO_MULTI_MDS,
+			BMI_OFFSET_GYRO_DIV_MDS);
+	}
+}
+
+void bmi_set_accel_offset(const struct motion_sensor_t *accel, intv3_t v)
+{
+	int i, val;
+
+	for (i = X; i <= Z; ++i) {
+		val = round_divide(
+			(int64_t)v[i] * BMI_OFFSET_ACC_DIV_MG,
+			BMI_OFFSET_ACC_MULTI_MG);
+		if (val > 127)
+			val = 127;
+		if (val < -128)
+			val = -128;
+		if (val < 0)
+			val = 256 + val;
+		bmi_write8(accel->port, accel->addr,
+			   BMI_OFFSET_ACC70 + i, val);
+	}
+}
+
+void bmi_set_gyro_offset(const struct motion_sensor_t *gyro, intv3_t v,
+			 int *val98_ptr)
+{
+	int i, val;
+
+	for (i = X; i <= Z; i++) {
+		val = round_divide(
+			(int64_t)v[i] * BMI_OFFSET_GYRO_DIV_MDS,
+			BMI_OFFSET_GYRO_MULTI_MDS);
+		if (val > 511)
+			val = 511;
+		if (val < -512)
+			val = -512;
+		if (val < 0)
+			val = 1024 + val;
+		bmi_write8(gyro->port, gyro->addr,
+			   BMI_OFFSET_GYR70 + i, val & 0xFF);
+		*val98_ptr &= ~(0x3 << (2 * i));
+		*val98_ptr |= (val >> 8) << (2 * i);
+	}
+}
+
diff --git a/driver/accelgyro_bmi_common.h b/driver/accelgyro_bmi_common.h
new file mode 100644
index 0000000000..27e4978ab5
--- /dev/null
+++ b/driver/accelgyro_bmi_common.h
@@ -0,0 +1,341 @@
+/* Copyright 2020 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+/* BMI accelerometer and gyro common definitions for Chrome EC */
+
+#ifndef __CROS_EC_ACCELGYRO_BMI_COMMON_H
+#define __CROS_EC_ACCELGYRO_BMI_COMMON_H
+
+#include "accelgyro.h"
+#include "driver/accelgyro_bmi160.h"
+#include "mag_bmm150.h"
+
+/*
+ * The addr field of motion_sensor support both SPI and I2C:
+ *
+ * +-------------------------------+---+
+ * |    7 bit i2c address          | 0 |
+ * +-------------------------------+---+
+ * Or
+ * +-------------------------------+---+
+ * |    SPI device ID              | 1 |
+ * +-------------------------------+---+
+ */
+#define BMI_SET_SPI_ADDRESS(_addr) (((_addr) << 1) | 1)
+#define BMI_IS_SPI(_addr)        ((_addr) & 1)
+#define BMI_SPI_ADDRESS(_addr)   ((_addr) >> 1)
+#define BMI_I2C_ADDRESS(_addr)   (_addr)
+
+#define BMI_CONF_REG(_sensor)      (0x40 + 2 * (_sensor))
+#define BMI_RANGE_REG(_sensor)     (0x41 + 2 * (_sensor))
+
+#define BMI_ODR_MASK         0x0F
+/* odr = 100 / (1 << (8 - reg)) , within limit */
+#define BMI_ODR_0_78HZ       0x01
+#define BMI_ODR_100HZ        0x08
+
+#define BMI_REG_TO_ODR(_regval) \
+	((_regval) < BMI_ODR_100HZ ? 100000 / (1 << (8 - (_regval))) : \
+					100000 * (1 << ((_regval) - 8)))
+#define BMI_ODR_TO_REG(_odr) \
+	((_odr) < 100000 ? (__builtin_clz(100000 / ((_odr) + 1)) - 24) : \
+			   (39 - __builtin_clz((_odr) / 100000)))
+
+enum fifo_header {
+	BMI_FH_EMPTY = 0x80,
+	BMI_FH_SKIP = 0x40,
+	BMI_FH_TIME = 0x44,
+	BMI_FH_CONFIG = 0x48
+};
+
+#define BMI_FH_MODE_MASK     0xc0
+#define BMI_FH_PARM_OFFSET       2
+#define BMI_FH_PARM_MASK         (0x7 << BMI_FH_PARM_OFFSET)
+#define BMI_FH_EXT_MASK      0x03
+
+/* Sensor resolution in number of bits. This sensor has fixed resolution. */
+#define BMI_RESOLUTION      16
+/* Min and Max sampling frequency in mHz */
+#define BMI_ACCEL_MIN_FREQ 12500
+#define BMI_ACCEL_MAX_FREQ MOTION_MAX_SENSOR_FREQUENCY(1600000, 100000)
+#define BMI_GYRO_MIN_FREQ  25000
+#define BMI_GYRO_MAX_FREQ MOTION_MAX_SENSOR_FREQUENCY(3200000, 100000)
+
+enum bmi_running_mode {
+	STANDARD_UI_9DOF_FIFO          = 0,
+	STANDARD_UI_IMU_FIFO           = 1,
+	STANDARD_UI_IMU                = 2,
+	STANDARD_UI_ADVANCEPOWERSAVE   = 3,
+	ACCEL_PEDOMETER                = 4,
+	APPLICATION_HEAD_TRACKING      = 5,
+	APPLICATION_NAVIGATION         = 6,
+	APPLICATION_REMOTE_CONTROL     = 7,
+	APPLICATION_INDOOR_NAVIGATION  = 8,
+};
+
+#define BMI_FLAG_SEC_I2C_ENABLED    (1 << 0)
+#define BMI_FIFO_FLAG_OFFSET        4
+#define BMI_FIFO_ALL_MASK           7
+
+struct bmi_drv_data_t {
+	struct accelgyro_saved_data_t saved_data[3];
+	uint8_t              flags;
+	uint8_t              enabled_activities;
+	uint8_t              disabled_activities;
+#ifdef CONFIG_MAG_BMI_BMM150
+	struct bmm150_private_data compass;
+#endif
+#ifdef CONFIG_BMI_ORIENTATION_SENSOR
+	uint8_t raw_orientation;
+	enum motionsensor_orientation orientation;
+	enum motionsensor_orientation last_orientation;
+#endif
+
+};
+
+#define BMI_GET_DATA(_s) \
+	((struct bmi_drv_data_t *)(_s)->drv_data)
+#define BMI_GET_SAVED_DATA(_s) \
+	(&BMI_GET_DATA(_s)->saved_data[(_s)->type])
+
+#ifdef CONFIG_BMI_ORIENTATION_SENSOR
+#define ORIENTATION_CHANGED(_sensor) \
+	(BMI_GET_DATA(_sensor)->orientation != \
+	BMI_GET_DATA(_sensor)->last_orientation)
+
+#define GET_ORIENTATION(_sensor) \
+	(BMI_GET_DATA(_sensor)->orientation)
+
+#define SET_ORIENTATION(_sensor, _val) \
+	(BMI_GET_DATA(_sensor)->orientation = _val)
+
+#define SET_ORIENTATION_UPDATED(_sensor) \
+	(BMI_GET_DATA(_sensor)->last_orientation = \
+	BMI_GET_DATA(_sensor)->orientation)
+#endif
+
+#define BMI_ACC_DATA BMI160_ACC_X_L_G
+#define BMI_GYR_DATA BMI160_GYR_X_L_G
+#define BMI_AUX_DATA BMI160_MAG_X_L_G
+
+#define BMI_FIFO_CONFIG_0 BMI160_FIFO_CONFIG_0
+#define BMI_FIFO_CONFIG_1 BMI160_FIFO_CONFIG_1
+#define BMI_FIFO_SENSOR_EN(_sensor) BMI160_FIFO_SENSOR_EN(_sensor)
+
+#define BMI_TEMPERATURE_0 BMI160_TEMPERATURE_0
+#define BMI_INVALID_TEMP     0x8000
+
+#define BMI_STATUS BMI160_STATUS
+#define BMI_DRDY_OFF(_sensor)   (7 - (_sensor))
+#define BMI_DRDY_MASK(_sensor)  (1 << BMI160_DRDY_OFF(_sensor))
+
+#define BMI_OFFSET_ACC70 BMI160_OFFSET_ACC70
+#define BMI_OFFSET_GYR70 BMI160_OFFSET_GYR70
+/*
+ * There is some bits in this register that differ between BMI160 and BMI260
+ * Only use this macro for gyro offset 9:8 (BMI_OFFSET_EN_GYR98 5:0).
+ */
+#define BMI_OFFSET_EN_GYR98 BMI160_OFFSET_EN_GYR98
+#define BMI_OFFSET_GYR98_MASK     (BIT(6) - 1)
+#define BMI_OFFSET_ACC_MULTI_MG   (3900 * 1024)
+#define BMI_OFFSET_ACC_DIV_MG     1000000
+#define BMI_OFFSET_GYRO_MULTI_MDS (61 * 1024)
+#define BMI_OFFSET_GYRO_DIV_MDS   1000
+
+#define BMI_FIFO_LENGTH_0 BMI160_FIFO_LENGTH_0
+#define BMI_FIFO_LENGTH_MASK BMI160_FIFO_LENGTH_MASK
+#define BMI_FIFO_DATA BMI160_FIFO_DATA
+
+#define BMI_CMD_REG BMI160_CMD_REG
+#define BMI_CMD_FIFO_FLUSH 0xb0
+
+
+
+/*
+ * Struct for pairing an engineering value with the register value for a
+ * parameter.
+ */
+struct bmi_accel_param_pair {
+	int val; /* Value in engineering units. */
+	int reg_val; /* Corresponding register value. */
+};
+
+int bmi_get_xyz_reg(const struct motion_sensor_t *s);
+
+/**
+ * @param type   Accel/Gyro
+ * @param psize  Size of the table
+ *
+ * @return       Range table of the type.
+ */
+const struct bmi_accel_param_pair *bmi_get_range_table(
+		const struct motion_sensor_t *s, int *psize);
+
+/**
+ * @return reg value that matches the given engineering value passed in.
+ * The round_up flag is used to specify whether to round up or down.
+ * Note, this function always returns a valid reg value. If the request is
+ * outside the range of values, it returns the closest valid reg value.
+ */
+int bmi_get_reg_val(const int eng_val, const int round_up,
+		const struct bmi_accel_param_pair *pairs, const int size);
+
+/**
+ * @return engineering value that matches the given reg val
+ */
+int bmi_get_engineering_val(const int reg_val,
+		const struct bmi_accel_param_pair *pairs, const int size);
+
+#ifdef CONFIG_SPI_ACCEL_PORT
+int bmi_spi_raw_read(const int addr, const uint8_t reg,
+		     uint8_t *data, const int len);
+#endif
+
+/**
+ * Read 8bit register from accelerometer.
+ */
+int bmi_read8(const int port, const uint16_t i2c_spi_addr_flags,
+	      const int reg, int *data_ptr);
+
+/**
+ * Write 8bit register from accelerometer.
+ */
+int bmi_write8(const int port, const uint16_t i2c_spi_addr_flags,
+	       const int reg, int data);
+
+/**
+ * Read 16bit register from accelerometer.
+ */
+int bmi_read16(const int port, const uint16_t i2c_spi_addr_flags,
+	       const uint8_t reg, int *data_ptr);
+
+/**
+ * Write 16bit register from accelerometer.
+ */
+int bmi_write16(const int port, const uint16_t i2c_spi_addr_flags,
+		const int reg, int data);
+
+/**
+ * Read 32bit register from accelerometer.
+ */
+int bmi_read32(const int port, const uint16_t i2c_spi_addr_flags,
+	       const uint8_t reg, int *data_ptr);
+
+/**
+ * Read n bytes from accelerometer.
+ */
+int bmi_read_n(const int port, const uint16_t i2c_spi_addr_flags,
+	       const uint8_t reg, uint8_t *data_ptr, const int len);
+
+/**
+ * Write n bytes from accelerometer.
+ */
+int bmi_write_n(const int port, const uint16_t i2c_spi_addr_flags,
+		const uint8_t reg, const uint8_t *data_ptr, const int len);
+
+/*
+ * Enable/Disable specific bit set of a 8-bit reg.
+ */
+int bmi_enable_reg8(const struct motion_sensor_t *s,
+		    int reg, uint8_t bits, int enable);
+
+/*
+ * Set specific bit set to certain value of a 8-bit reg.
+ */
+int bmi_set_reg8(const struct motion_sensor_t *s, int reg,
+		 uint8_t bits, int mask);
+
+/*
+ * @s: base sensor.
+ * @v: output vector.
+ * @input: 6-bits array input.
+ */
+void bmi_normalize(const struct motion_sensor_t *s, intv3_t v, uint8_t *input);
+
+/*
+ * Decode the header from the fifo.
+ * Return 0 if we need further processing.
+ * Sensor mutex must be held during processing, to protect the fifos.
+ *
+ * @accel: base sensor
+ * @hdr: the header to decode
+ * @last_ts: the last timestamp of fifo interrupt.
+ * @bp: current pointer in the buffer, updated when processing the header.
+ * @ep: pointer to the end of the valid data in the buffer.
+ */
+int bmi_decode_header(struct motion_sensor_t *accel,
+		      enum fifo_header hdr, uint32_t last_ts,
+		      uint8_t **bp, uint8_t *ep);
+/**
+ * Retrieve hardware FIFO from sensor,
+ * - put data in Sensor Hub fifo.
+ * - update sensor raw_xyz vector with the last information.
+ * We put raw data in hub fifo and process data from there.
+ * @s: Pointer to sensor data.
+ * @last_ts: The last timestamp of fifo interrupt.
+ *
+ * Read only up to  bmi_buffer. If more reads are needed, we will be called
+ * again by the interrupt routine.
+ *
+ * NOTE: If a new driver supports this function, be sure to add a check
+ * for spoof_mode in order to load the sensor stack with the spoofed
+ * data.  See accelgyro_bmi260.c::load_fifo for an example.
+ */
+int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts);
+
+int bmi_set_range(const struct motion_sensor_t *s, int range, int rnd);
+
+int bmi_get_range(const struct motion_sensor_t *s);
+
+int bmi_get_data_rate(const struct motion_sensor_t *s);
+
+
+int bmi_get_offset(const struct motion_sensor_t *s,
+		   int16_t *offset, int16_t *temp);
+
+int bmi_get_resolution(const struct motion_sensor_t *s);
+
+int bmi_set_scale(const struct motion_sensor_t *s,
+		  const uint16_t *scale, int16_t temp);
+
+int bmi_get_scale(const struct motion_sensor_t *s,
+		  uint16_t *scale, int16_t *temp);
+
+/* Start/Stop the FIFO collecting events */
+int bmi_enable_fifo(const struct motion_sensor_t *s, int enable);
+
+/* Read the xyz data of accel/gyro */
+int bmi_read(const struct motion_sensor_t *s, intv3_t v);
+
+/* Read temperature of sensor s */
+int bmi_read_temp(const struct motion_sensor_t *s, int *temp_ptr);
+
+/* Read temperature of sensor idx */
+int bmi_get_sensor_temp(int idx, int *temp_ptr);
+
+/*
+ * Get the normalized rate according to input rate and input rnd
+ * @rate: input rate
+ * @rnd: round up
+ * @normalized_rate_ptr: normalized rate pointer for output
+ * @reg_val_ptr: pointer to the actual register value of normalized rate for
+ *               output.
+ */
+int bmi_get_normalized_rate(const struct motion_sensor_t *s, int rate, int rnd,
+			    int *normalized_rate_ptr, uint8_t *reg_val_ptr);
+
+/* Get the accelerometer offset */
+void bmi_accel_get_offset(const struct motion_sensor_t *accel, intv3_t v);
+
+/* Get the gyroscope offset */
+void bmi_gyro_get_offset(const struct motion_sensor_t *gyro, intv3_t v);
+
+/* Set the accelerometer offset */
+void bmi_set_accel_offset(const struct motion_sensor_t *accel, intv3_t v);
+
+/* Set the gyroscope offset */
+void bmi_set_gyro_offset(const struct motion_sensor_t *gyro, intv3_t v,
+			 int *val98_ptr);
+
+#endif /* __CROS_EC_ACCELGYRO_BMI_COMMON_H */
diff --git a/driver/build.mk b/driver/build.mk
index 683778502e..e121be3237 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -11,8 +11,8 @@ driver-$(CONFIG_ACCEL_BMA255)+=accel_bma2x2.o
 driver-$(CONFIG_ACCEL_KXCJ9)+=accel_kionix.o
 driver-$(CONFIG_ACCEL_KX022)+=accel_kionix.o
 driver-$(CONFIG_ACCELGYRO_LSM6DS0)+=accelgyro_lsm6ds0.o
-driver-$(CONFIG_ACCELGYRO_BMI160)+=accelgyro_bmi160.o
-driver-$(CONFIG_MAG_BMI160_BMM150)+=mag_bmm150.o
+driver-$(CONFIG_ACCELGYRO_BMI160)+=accelgyro_bmi160.o accelgyro_bmi_common.o
+driver-$(CONFIG_MAG_BMI_BMM150)+=mag_bmm150.o
 driver-$(CONFIG_ACCELGYRO_LSM6DSM)+=accelgyro_lsm6dsm.o stm_mems_common.o
 driver-$(CONFIG_ACCEL_LIS2D_COMMON)+=accel_lis2dh.o stm_mems_common.o
 driver-$(CONFIG_MAG_LIS2MDL)+=mag_lis2mdl.o
diff --git a/driver/mag_bmm150.c b/driver/mag_bmm150.c
index cb53641c00..0da75d497c 100644
--- a/driver/mag_bmm150.c
+++ b/driver/mag_bmm150.c
@@ -17,7 +17,7 @@
 #include "timer.h"
 #include "util.h"
 
-#ifdef CONFIG_MAG_BMI160_BMM150
+#ifdef CONFIG_MAG_BMI_BMM150
 #include "driver/accelgyro_bmi160.h"
 #define raw_mag_read8 bmi160_sec_raw_read8
 #define raw_mag_write8 bmi160_sec_raw_write8
diff --git a/include/config.h b/include/config.h
index 5b260ea827..c25e9ee578 100644
--- a/include/config.h
+++ b/include/config.h
@@ -3860,6 +3860,17 @@
 #include "config_chip.h"
 #include "board.h"
 
+/*
+ * Define BMI macro when BMI160 is defined
+ * TODO(chingkang): replace all BMI160 macro by BMI.
+ */
+#ifdef CONFIG_MAG_BMI160_BMM150
+#define CONFIG_MAG_BMI_BMM150
+#endif
+#ifdef CONFIG_MAG_BMI160_LIS2MDL
+#define CONFIG_MAG_BMI_LIS2MDL
+#endif
+
 /******************************************************************************/
 /*
  * Automatically define CONFIG_HOSTCMD_X86 if either child option is defined.