driver: Add BMI260 basic driver support

Add BMI260 accel/gyro driver code. Be able to do initial configuration,
start up sensor, config interrupt and fifo, perform calibration, and
access the sensor data from AP.

BRANCH=None
BUG=b:146144827
TEST=make -j4 BOARD=volteer \
     && flash_ec --image build/volteer/ec.bin --board volteer
     Then, check sensor data is available on DUT by "ectool motionsense".
TEST=make buildall -j
TEST=tast run ${IP} hardware.SensorRing

Cq-Depend: chromium:2148802
Change-Id: I5124ee8359a74f98cfe8d26da640325f5e00cb15
Signed-off-by: Ching-Kang Yen <chingkang@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2086534
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

5 files changed, 962 insertions, 52 deletions

diff --git a/driver/accelgyro_bmi260.c b/driver/accelgyro_bmi260.c
new file mode 100644
index 0000000000..9650d48136
--- /dev/null
+++ b/driver/accelgyro_bmi260.c
@@ -0,0 +1,512 @@
+/* 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.
+ */
+
+/**
+ * BMI260 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 "driver/accelgyro_bmi260.h"
+#include "hwtimer.h"
+#include "i2c.h"
+#include "math_util.h"
+#include "motion_sense_fifo.h"
+#include "spi.h"
+#include "task.h"
+#include "third_party/bmi260/accelgyro_bmi260_config_tbin.h"
+#include "timer.h"
+#include "util.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)
+
+STATIC_IF(CONFIG_ACCEL_FIFO) volatile uint32_t last_interrupt_timestamp;
+
+/*
+ * The gyro start-up time is 45ms in normal mode
+ *                            2ms in fast start-up mode
+ */
+static int wakeup_time[] = {
+	[MOTIONSENSE_TYPE_ACCEL] = 2,
+	[MOTIONSENSE_TYPE_GYRO] = 45,
+	[MOTIONSENSE_TYPE_MAG] = 1
+};
+
+static int enable_sensor(const struct motion_sensor_t *s, int enable)
+{
+	int ret;
+
+	ret = bmi_enable_reg8(s, BMI260_PWR_CTRL,
+			      BMI260_PWR_EN(s->type),
+			      enable);
+	if (ret)
+		return ret;
+
+	if (s->type == MOTIONSENSE_TYPE_GYRO) {
+		/* switch to performance mode */
+		ret = bmi_enable_reg8(s, BMI_CONF_REG(s->type),
+				      BMI260_FILTER_PERF |
+				      BMI260_GYR_NOISE_PERF,
+				      enable);
+	} else {
+		ret = bmi_enable_reg8(s, BMI_CONF_REG(s->type),
+				      BMI260_FILTER_PERF,
+				      enable);
+	}
+	return ret;
+
+}
+
+static int set_data_rate(const struct motion_sensor_t *s,
+			 int rate,
+			 int rnd)
+{
+	int ret, normalized_rate;
+	uint8_t reg_val;
+	struct accelgyro_saved_data_t *data = BMI_GET_SAVED_DATA(s);
+
+	if (rate == 0) {
+		/* FIFO stop collecting events */
+		if (IS_ENABLED(CONFIG_ACCEL_FIFO))
+			bmi_enable_fifo(s, 0);
+		/* disable sensor */
+		ret = enable_sensor(s, 0);
+		msleep(3);
+		data->odr = 0;
+		return ret;
+	} else if (data->odr == 0) {
+		/* enable sensor */
+		ret = enable_sensor(s, 1);
+		if (ret)
+			return ret;
+		/* Wait for accel/gyro to wake up */
+		msleep(wakeup_time[s->type]);
+	}
+
+	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
+	 * to write accel parameters until we are done.
+	 */
+	mutex_lock(s->mutex);
+
+	ret = bmi_set_reg8(s, BMI_CONF_REG(s->type),
+			   reg_val, BMI_ODR_MASK);
+	if (ret != EC_SUCCESS)
+		goto accel_cleanup;
+
+	/* Wait for the change to become effective */
+	if (data->odr != 0)
+		msleep(1000000 / MIN(data->odr, normalized_rate));
+	/* Now that we have set the odr, update the driver's value. */
+	data->odr = normalized_rate;
+
+	/*
+	 * FIFO start collecting events.
+	 * They will be discarded if AP does not want them.
+	 */
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO))
+		bmi_enable_fifo(s, 1);
+accel_cleanup:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int set_offset(const struct motion_sensor_t *s,
+			const int16_t *offset,
+			int16_t    temp)
+{
+	int ret, val98, val_nv_conf;
+	intv3_t v = { offset[X], offset[Y], offset[Z] };
+
+	rotate_inv(v, *s->rot_standard_ref, v);
+
+	ret = bmi_read8(s->port, s->i2c_spi_addr_flags,
+			BMI260_OFFSET_EN_GYR98, &val98);
+	if (ret)
+		return ret;
+	ret = bmi_read8(s->port, s->i2c_spi_addr_flags,
+			BMI260_NV_CONF, &val_nv_conf);
+	if (ret)
+		return ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		bmi_set_accel_offset(s, v);
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_NV_CONF,
+				 val_nv_conf | BMI260_ACC_OFFSET_EN);
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		bmi_set_gyro_offset(s, v, &val98);
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_OFFSET_EN_GYR98,
+				 val98 | BMI260_OFFSET_GYRO_EN);
+		break;
+	default:
+		ret = EC_RES_INVALID_PARAM;
+	}
+	return ret;
+}
+
+static int wait_and_read_data(const struct motion_sensor_t *s,
+			      intv3_t v, int try_cnt, int msec)
+{
+	uint8_t data[6];
+	int ret, status = 0;
+
+	/* Check if data is ready */
+	while (try_cnt && !(status & BMI260_DRDY_ACC)) {
+		msleep(msec);
+		ret = bmi_read8(s->port, s->i2c_spi_addr_flags,
+				BMI260_STATUS, &status);
+		if (ret)
+			return ret;
+		try_cnt -= 1;
+	}
+	if (!(status & BMI260_DRDY_ACC))
+		return EC_ERROR_TIMEOUT;
+	/* Read 6 bytes starting at xyz_reg */
+	ret = bmi_read_n(s->port, s->i2c_spi_addr_flags,
+			 bmi_get_xyz_reg(s), data, 6);
+	bmi_normalize(s, v, data);
+	return ret;
+}
+
+static int calibrate_offset(const struct motion_sensor_t *s,
+			    int range, intv3_t target, int16_t *offset)
+{
+	int ret = EC_ERROR_UNKNOWN;
+	int i, n_sample = 32;
+	int data_diff[3] = {0};
+
+	/* Manually offset compensation */
+	for (i = 0; i < n_sample; ++i) {
+		intv3_t v;
+		/* Wait data for at most 3 * 10 msec */
+		ret = wait_and_read_data(s, v, 3, 10);
+		if (ret)
+			return ret;
+		data_diff[X] += v[X] - target[X];
+		data_diff[Y] += v[Y] - target[Y];
+		data_diff[Z] += v[Z] - target[Z];
+	}
+
+	/* The data LSB: 1000 * range / 32768 (mdps | mg)*/
+	for (i = X; i <= Z; ++i)
+		offset[i] -= ((int64_t)(data_diff[i] / n_sample) *
+			     1000 * range) >> 15;
+	return ret;
+}
+
+static int perform_calib(const struct motion_sensor_t *s, int enable)
+{
+	int ret, rate;
+	int16_t temp;
+	int16_t offset[3];
+	intv3_t target = {0, 0, 0};
+	/* Get sensor range for calibration*/
+	int range = bmi_get_range(s);
+
+	if (!enable)
+		return EC_SUCCESS;
+	rate = bmi_get_data_rate(s);
+	ret = set_data_rate(s, 100000, 0);
+	if (ret)
+		return ret;
+
+	ret = bmi_get_offset(s, offset, &temp);
+	if (ret)
+		goto end_perform_calib;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		target[Z] = BMI260_ACC_DATA_PLUS_1G(range);
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		break;
+	default:
+		/* Not supported on Magnetometer */
+		ret = EC_RES_INVALID_PARAM;
+		goto end_perform_calib;
+	}
+
+	/* Get the calibrated offset */
+	ret = calibrate_offset(s, range, target, offset);
+	if (ret)
+		goto end_perform_calib;
+
+	ret = set_offset(s, offset, temp);
+	if (ret)
+		goto end_perform_calib;
+
+end_perform_calib:
+	if (ret == EC_ERROR_TIMEOUT)
+		CPRINTS("%s timeout", __func__);
+	set_data_rate(s, rate, 0);
+	return ret;
+}
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+
+/**
+ * bmi260_interrupt - called when the sensor activates the interrupt line.
+ *
+ * This is a "top half" interrupt handler, it just asks motion sense ask
+ * to schedule the "bottom half", ->irq_handler().
+ */
+void bmi260_interrupt(enum gpio_signal signal)
+{
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO))
+		last_interrupt_timestamp = __hw_clock_source_read();
+
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCELGYRO_BMI260_INT_EVENT, 0);
+}
+
+static int config_interrupt(const struct motion_sensor_t *s)
+{
+	int ret;
+
+	if (s->type != MOTIONSENSE_TYPE_ACCEL)
+		return EC_SUCCESS;
+
+	mutex_lock(s->mutex);
+	bmi_write8(s->port, s->i2c_spi_addr_flags,
+		   BMI260_CMD_REG, BMI260_CMD_FIFO_FLUSH);
+
+	/* configure int1 as an interrupt */
+	ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+			 BMI260_INT1_IO_CTRL,
+			 BMI260_INT1_OUTPUT_EN);
+#ifdef CONFIG_ACCELGYRO_BMI260_INT2_OUTPUT
+	/* TODO(chingkang): Test it if we want int2 as an interrupt */
+	/* configure int2 as an interrupt */
+	ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+			 BMI260_INT2_IO_CTRL,
+			 BMI260_INT2_OUTPUT_EN);
+#else
+	/* configure int2 as an external input. */
+	ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+			 BMI260_INT2_IO_CTRL,
+			 BMI260_INT2_INPUT_EN);
+#endif
+
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
+		/* map fifo water mark to int 1 */
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_INT_MAP_DATA,
+				 BMI260_INT_MAP_DATA_REG(1, FWM) |
+				 BMI260_INT_MAP_DATA_REG(1, FFULL));
+
+		/*
+		 * Configure fifo watermark to int whenever there's any data in
+		 * there
+		 */
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_FIFO_WTM_0, 1);
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_FIFO_WTM_1, 0);
+#ifdef CONFIG_ACCELGYRO_BMI260_INT2_OUTPUT
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_FIFO_CONFIG_1,
+				 BMI260_FIFO_HEADER_EN);
+#else
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_FIFO_CONFIG_1,
+				 (BMI260_FIFO_TAG_INT_LEVEL <<
+				 BMI260_FIFO_TAG_INT2_EN_OFFSET) |
+				 BMI260_FIFO_HEADER_EN);
+#endif
+		/* disable FIFO sensortime frame */
+		ret = bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_FIFO_CONFIG_0, 0);
+	}
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+/**
+ * irq_handler - bottom half of the interrupt stack.
+ * Ran from the motion_sense task, finds the events that raised the interrupt.
+ *
+ * For now, we just print out. We should set a bitmask motion sense code will
+ * act upon.
+ */
+static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	/* use uint16_t interrupt can cause error. */
+	uint32_t interrupt = 0;
+	int8_t has_read_fifo = 0;
+	int rv;
+
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+			(!(*event & CONFIG_ACCELGYRO_BMI260_INT_EVENT)))
+		return EC_ERROR_NOT_HANDLED;
+
+	do {
+		rv = bmi_read16(s->port, s->i2c_spi_addr_flags,
+				BMI260_INT_STATUS_0, &interrupt);
+		/*
+		 * Bail out of this loop there was an error reading the register
+		 */
+		if (rv)
+			return rv;
+
+		if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
+			interrupt & (BMI260_FWM_INT | BMI260_FFULL_INT)) {
+			bmi_load_fifo(s, last_interrupt_timestamp);
+			has_read_fifo = 1;
+		}
+	} while (interrupt != 0);
+
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO) && has_read_fifo)
+		motion_sense_fifo_commit_data();
+
+	return EC_SUCCESS;
+}
+#endif  /* CONFIG_ACCEL_INTERRUPTS */
+
+static int init_config(const struct motion_sensor_t *s)
+{
+	int init_status, ret;
+	uint16_t i;
+	/*
+	 * Due to i2c transaction timeout limit,
+	 * burst_write_len should not be above 2048 to prevent timeout.
+	 */
+	const int burst_write_len = 2048;
+	/* We have to write the config even bytes of data every time */
+	BUILD_ASSERT((burst_write_len & 1) == 0);
+
+	/* disable advance power save but remain fifo self wakeup*/
+	bmi_write8(s->port, s->i2c_spi_addr_flags, BMI260_PWR_CONF, 2);
+	msleep(1);
+	/* prepare for config load */
+	bmi_write8(s->port, s->i2c_spi_addr_flags, BMI260_INIT_CTRL, 0);
+
+	/* load config file to INIT_DATA */
+	for (i = 0; i < g_bmi260_config_tbin_len; i += burst_write_len) {
+		uint8_t addr[2];
+		const int len = MIN(burst_write_len,
+				    g_bmi260_config_tbin_len - i);
+
+		addr[0] = (i / 2) & 0xF;
+		addr[1] = (i / 2) >> 4;
+		ret = bmi_write_n(s->port, s->i2c_spi_addr_flags,
+				  BMI260_INIT_ADDR_0, addr, 2);
+		if (ret)
+			break;
+		ret = bmi_write_n(s->port, s->i2c_spi_addr_flags,
+				  BMI260_INIT_DATA, &g_bmi260_config_tbin[i],
+				  len);
+		if (ret)
+			break;
+	}
+	/* finish config load */
+	bmi_write8(s->port, s->i2c_spi_addr_flags, BMI260_INIT_CTRL, 1);
+	/* return error if load config failed */
+	if (ret)
+		return ret;
+	/* wait INTERNAL_STATUS.message to be 0x1 which take at most 150ms */
+	for (i = 0; i < 15; ++i) {
+		msleep(10);
+		ret = bmi_read8(s->port, s->i2c_spi_addr_flags,
+			BMI260_INTERNAL_STATUS, &init_status);
+		if (ret)
+			break;
+		init_status &= BMI260_MESSAGE_MASK;
+		if (init_status == BMI260_INIT_OK)
+			break;
+	}
+	if (ret || init_status != BMI260_INIT_OK)
+		return EC_ERROR_INVALID_CONFIG;
+	return EC_SUCCESS;
+}
+
+static int init(const struct motion_sensor_t *s)
+{
+	int ret = 0, tmp, i;
+	struct accelgyro_saved_data_t *saved_data = BMI_GET_SAVED_DATA(s);
+
+	ret = bmi_read8(s->port, s->i2c_spi_addr_flags,
+			BMI260_CHIP_ID, &tmp);
+	if (ret)
+		return EC_ERROR_UNKNOWN;
+
+	if (tmp != BMI260_CHIP_ID_MAJOR)
+		return EC_ERROR_ACCESS_DENIED;
+
+	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
+		struct bmi_drv_data_t *data = BMI_GET_DATA(s);
+
+		/* Reset the chip to be in a good state */
+		bmi_write8(s->port, s->i2c_spi_addr_flags,
+				 BMI260_CMD_REG, BMI260_CMD_SOFT_RESET);
+		msleep(2);
+		if (init_config(s))
+			return EC_ERROR_INVALID_CONFIG;
+
+		data->flags &= ~(BMI_FLAG_SEC_I2C_ENABLED |
+				(BMI_FIFO_ALL_MASK <<
+				 BMI_FIFO_FLAG_OFFSET));
+	}
+
+	for (i = X; i <= Z; i++)
+		saved_data->scale[i] = MOTION_SENSE_DEFAULT_SCALE;
+	/*
+	 * The sensor is in Suspend mode at init,
+	 * so set data rate to 0.
+	 */
+	saved_data->odr = 0;
+	bmi_set_range(s, s->default_range, 0);
+
+	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
+#ifdef CONFIG_ACCEL_INTERRUPTS
+		ret = config_interrupt(s);
+#endif
+	}
+
+	return sensor_init_done(s);
+}
+
+const struct accelgyro_drv bmi260_drv = {
+	.init = init,
+	.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 = bmi_get_data_rate,
+	.set_offset = set_offset,
+	.get_scale = bmi_get_scale,
+	.set_scale = bmi_set_scale,
+	.get_offset = bmi_get_offset,
+	.perform_calib = perform_calib,
+	.read_temp = bmi_read_temp,
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.irq_handler = irq_handler,
+#endif
+};
+
+#ifdef CONFIG_CMD_I2C_STRESS_TEST_ACCEL
+struct i2c_stress_test_dev bmi260_i2c_stress_test_dev = {
+	.reg_info = {
+		.read_reg = BMI260_CHIP_ID,
+		.read_val = BMI260_CHIP_ID_MAJOR,
+		.write_reg = BMI260_PMU_TRIGGER,
+	},
+	.i2c_read = &bmi_read8,
+	.i2c_write = &bmi_write8,
+};
+#endif /* CONFIG_CMD_I2C_STRESS_TEST_ACCEL */
diff --git a/driver/accelgyro_bmi260.h b/driver/accelgyro_bmi260.h
new file mode 100644
index 0000000000..4358e657fe
--- /dev/null
+++ b/driver/accelgyro_bmi260.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.
+ */
+
+/* BMI260 accelerometer and gyro for Chrome EC */
+
+#ifndef __CROS_EC_ACCELGYRO_BMI260_H
+#define __CROS_EC_ACCELGYRO_BMI260_H
+
+#include "accelgyro.h"
+#include "common.h"
+#include "driver/mag_bmm150.h"
+
+/*
+ * The addr field of motion_sensor support both SPI and I2C:
+ * This is defined in include/i2c.h and is no longer an 8bit
+ * address. The 7/10 bit address starts at bit 0 and leaves
+ * room for a 10 bit address, although we don't currently
+ * have any 10 bit slaves.  I2C or SPI is indicated by a
+ * more significant bit
+ */
+
+/* I2C addresses */
+#define BMI260_ADDR0_FLAGS	0x68
+
+#define BMI260_CHIP_ID      0x00
+#define BMI260_CHIP_ID_MAJOR    0x27
+
+#define BMI260_ERR_REG          0x02
+
+#define BMI260_STATUS           0x03
+#define BMI260_AUX_BUSY             BIT(2)
+#define BMI260_CMD_RDY              BIT(4)
+#define BMI260_DRDY_AUX             BIT(5)
+#define BMI260_DRDY_GYR             BIT(6)
+#define BMI260_DRDY_ACC             BIT(7)
+#define BMI260_DRDY_OFF(_sensor)    (7 - (_sensor))
+#define BMI260_DRDY_MASK(_sensor)   (1 << BMI260_DRDY_OFF(_sensor))
+
+#define BMI260_AUX_X_L_G           0x04
+#define BMI260_AUX_X_H_G           0x05
+#define BMI260_AUX_Y_L_G           0x06
+#define BMI260_AUX_Y_H_G           0x07
+#define BMI260_AUX_Z_L_G           0x08
+#define BMI260_AUX_Z_H_G           0x09
+#define BMI260_AUX_R_L_G           0x0a
+#define BMI260_AUX_R_H_G           0x0b
+#define BMI260_ACC_X_L_G           0x0c
+#define BMI260_ACC_X_H_G           0x0d
+#define BMI260_ACC_Y_L_G           0x0e
+#define BMI260_ACC_Y_H_G           0x0f
+#define BMI260_ACC_Z_L_G           0x10
+#define BMI260_ACC_Z_H_G           0x11
+#define BMI260_GYR_X_L_G           0x12
+#define BMI260_GYR_X_H_G           0x13
+#define BMI260_GYR_Y_L_G           0x14
+#define BMI260_GYR_Y_H_G           0x15
+#define BMI260_GYR_Z_L_G           0x16
+#define BMI260_GYR_Z_H_G           0x17
+
+#define BMI260_SENSORTIME_0     0x18
+#define BMI260_SENSORTIME_1     0x19
+#define BMI260_SENSORTIME_2     0x1a
+
+#define BMI260_EVENT            0x1b
+
+/* 2 bytes interrupt reasons*/
+#define BMI260_INT_STATUS_0     0x1c
+#define BMI260_SIG_MOTION_OUT       BIT(0)
+#define BMI260_STEP_COUNTER_OUT     BIT(1)
+#define BMI260_HIGH_LOW_G_OUT       BIT(2)
+#define BMI260_TAP_OUT              BIT(3)
+#define BMI260_FLAT_OUT             BIT(4)
+#define BMI260_NO_MOTION_OUT        BIT(5)
+#define BMI260_ANY_MOTION_OUT       BIT(6)
+#define BMI260_ORIENTATION_OUT      BIT(7)
+
+#define BMI260_INT_STATUS_1     0x1d
+#define BMI260_FFULL_INT            BIT(0 + 8)
+#define BMI260_FWM_INT              BIT(1 + 8)
+#define BMI260_ERR_INT              BIT(2 + 8)
+#define BMI260_AUX_DRDY_INT         BIT(5 + 8)
+#define BMI260_GYR_DRDY_INT         BIT(6 + 8)
+#define BMI260_ACC_DRDY_INT         BIT(7 + 8)
+
+#define BMI260_INT_MASK             0xFFFF
+
+#define BMI260_SC_OUT_0         0x1e
+#define BMI260_SC_OUT_1         0x1f
+
+#define BMI260_ORIENT_ACT       0x20
+
+#define BMI260_INTERNAL_STATUS  0X21
+#define BMI260_MESSAGE_MASK         0xf
+#define BMI260_NOT_INIT             0x00
+#define BMI260_INIT_OK              0x01
+#define BMI260_INIT_ERR             0x02
+#define BMI260_DRV_ERR              0x03
+#define BMI260_SNS_STOP             0x04
+#define BMI260_NVM_ERROR            0x05
+#define BMI260_START_UP_ERROR       0x06
+#define BMI260_COMPAT_ERROR         0x07
+
+#define BMI260_TEMPERATURE_0    0x22
+#define BMI260_TEMPERATURE_1    0x23
+
+#define BMI260_FIFO_LENGTH_0    0x24
+#define BMI260_FIFO_LENGTH_1    0x25
+#define BMI260_FIFO_LENGTH_MASK     (BIT(14) - 1)
+#define BMI260_FIFO_DATA        0x26
+
+#define BMI260_FEAT_PAGE        0x2f
+/*
+ * The register of feature page should be read/write as 16-bit register
+ * Otherwise, there can be invalid data
+ */
+/* Features page 0 */
+#define BMI260_ORIENT_OUT       0x36
+#define BMI260_ORIENT_OUT_PORTRAIT_LANDSCAPE_MASK   3
+#define BMI260_ORIENT_PORTRAIT                      0x0
+#define BMI260_ORIENT_LANDSCAPE                     0x1
+#define BMI260_ORIENT_PORTRAIT_INVERT               0x2
+#define BMI260_ORIENT_LANDSCAPE_INVERT              0x3
+
+/* Features page 1 */
+#define BMI260_GEN_SET_1        0x34
+#define BMI260_GYR_SELF_OFF         BIT(9)
+
+#define BMI260_TAP_1            0x3e
+#define BMI260_TAP_1_EN                     BIT(0)
+#define BMI260_TAP_1_SENSITIVITY_OFFSET     1
+#define BMI260_TAP_1_SENSITIVITY_MASK       \
+	(0x7 << BMI260_TAP_1_SENSITIVITY_OFFSET)
+
+/* Features page 2 */
+#define BMI260_ORIENT_1         0x30
+#define BMI260_ORIENT_1_EN          BIT(0)
+#define BMI260_ORIENT_1_UD_EN       BIT(1)
+#define BMI260_ORIENT_1_MODE_OFFSET 2
+#define BMI260_ORIENT_1_MODE_MASK   (0x3 << BMI260_ORIENT_1_MODE_OFFSET)
+#define BMI260_ORIENT_1_BLOCK_OFFSET    4
+#define BMI260_ORIENT_1_BLOCK_MASK      (0x3 << BMI260_ORIENT_1_BLOCK_OFFSET)
+#define BMI260_ORIENT_1_THETA_OFFSET     6
+#define BMI260_ORIENT_1_THETA_MASK      \
+	((BIT(6) - 1) << BMI260_ORIENT_1_THETA_OFFSET)
+
+#define BMI260_ORIENT_2         0x32
+/* hysteresis(10...0) range is 0~1g, default is 128 (0.0625g) */
+#define BMI260_ORIENT_2_HYSTERESIS_MASK     (BIT(11) - 1)
+
+#define BMI260_ACC_CONF         0x40
+#define BMI260_ACC_BW_OFFSET        4
+#define BMI260_ACC_BW_MASK          (0x7 << BMI260_ACC_BW_OFFSET)
+#define BMI260_FILTER_PERF          BIT(7)
+#define BMI260_ULP                  0x0
+#define BMI260_HP                   0x1
+
+#define BMI260_ACC_RANGE        0x41
+#define BMI260_GSEL_2G              0x00
+#define BMI260_GSEL_4G              0x01
+#define BMI260_GSEL_8G              0x02
+#define BMI260_GSEL_16G             0x03
+
+/* The max positvie value of accel data is 0x7FFF, equal to range(g) */
+/* So, in order to get +1g, divide the 0x7FFF by range */
+#define BMI260_ACC_DATA_PLUS_1G(range) (0x7FFF / (range))
+#define BMI260_ACC_DATA_MINUS_1G(range) (-BMI260_ACC_DATA_PLUS_1G(range))
+
+#define BMI260_GYR_CONF         0x42
+#define BMI260_GYR_BW_OFFSET        4
+#define BMI260_GYR_BW_MASK          (0x3 << BMI260_GYR_BW_OFFSET)
+#define BMI260_GYR_NOISE_PERF       BIT(6)
+
+#define BMI260_GYR_RANGE        0x43
+#define BMI260_DPS_SEL_2000         0x00
+#define BMI260_DPS_SEL_1000         0x01
+#define BMI260_DPS_SEL_500          0x02
+#define BMI260_DPS_SEL_250          0x03
+#define BMI260_DPS_SEL_125          0x04
+
+#define BMI260_AUX_CONF         0x44
+
+#define BMI260_FIFO_DOWNS       0x45
+
+#define BMI260_FIFO_WTM_0       0x46
+#define BMI260_FIFO_WTM_1       0x47
+
+#define BMI260_FIFO_CONFIG_0    0x48
+#define BMI260_FIFO_STOP_ON_FULL    BIT(0)
+#define BMI260_FIFO_TIME_EN     BIT(1)
+
+#define BMI260_FIFO_CONFIG_1    0x49
+#define BMI260_FIFO_TAG_INT1_EN_OFFSET  0
+#define BMI260_FIFO_TAG_INT1_EN_MASK    (0x3 << BMI260_FIFO_TAG_INT1_EN_OFFSET)
+#define BMI260_FIFO_TAG_INT2_EN_OFFSET  2
+#define BMI260_FIFO_TAG_INT2_EN_MASK    (0x3 << BMI260_FIFO_TAG_INT2_EN_OFFSET)
+#define BMI260_FIFO_TAG_INT_EDGE        0x0
+#define BMI260_FIFO_TAG_INT_LEVEL       0x1
+#define BMI260_FIFO_TAG_ACC_SAT         0x2
+#define BMI260_FIFO_TAG_GYR_SAT         0x3
+#define BMI260_FIFO_HEADER_EN           BIT(4)
+#define BMI260_FIFO_AUX_EN              BIT(5)
+#define BMI260_FIFO_ACC_EN              BIT(6)
+#define BMI260_FIFO_GYR_EN              BIT(7)
+#define BMI260_FIFO_SENSOR_EN(_sensor) \
+	((_sensor) == MOTIONSENSE_TYPE_ACCEL ? BMI260_FIFO_ACC_EN : \
+	  ((_sensor) == MOTIONSENSE_TYPE_GYRO ? BMI260_FIFO_GYR_EN : \
+	   BMI260_FIFO_AUX_EN))
+
+#define BMI260_AUX_DEV_ID       0x4b
+#define BMI260_AUX_I2C_ADDRESS          BMI260_AUX_DEV_ID
+
+#define BMI260_AUX_IF_CONF      0x4c
+#define BMI260_AUX_I2C_CONTROL          BMI260_AUX_IF_CONF
+#define BMI260_AUX_READ_BURST_MASK      3
+#define BMI260_AUX_MAN_READ_BURST_OFF   2
+#define BMI260_AUX_MAN_READ_BURST_MASK  (0x3 << BMI280_AUX_MAN_READ_BURST_OFF)
+#define BMI260_AUX_READ_BURST_1         0
+#define BMI260_AUX_READ_BURST_2         1
+#define BMI260_AUX_READ_BURST_6         2
+#define BMI260_AUX_READ_BURST_8         3
+#define BMI260_AUX_FCU_WRITE_EN         BIT(6)
+#define BMI260_AUX_MANUAL_EN            BIT(7)
+
+#define BMI260_AUX_RD_ADDR      0x4d
+#define BMI260_AUX_I2C_READ_ADDR    BMI260_AUX_RD_ADDR
+#define BMI260_AUX_WR_ADDR      0x4e
+#define BMI260_AUX_I2C_WRITE_ADDR   BMI260_AUX_WR_ADDR
+#define BMI260_AUX_WR_DATA      0x4f
+#define BMI260_AUX_I2C_WRITE_DATA   BMI260_AUX_WR_DATA
+#define BMI260_AUX_I2C_READ_DATA    BMI260_AUX_X_L_G
+
+#define BMI260_ERR_REG_MSK      0x52
+#define BMI260_FATAL_ERR            BIT(0)
+#define BMI260_INTERNAL_ERR_OFF     1
+#define BMI260_INTERNAL_ERR_MASK    (0xf << BMI260_INTERNAL_ERR_OFF)
+#define BMI260_FIFO_ERR             BIT(6)
+#define BMI260_AUX_ERR              BIT(7)
+
+#define BMI260_INT1_IO_CTRL     0x53
+#define BMI260_INT1_LVL             BIT(1)
+#define BMI260_INT1_OD              BIT(2)
+#define BMI260_INT1_OUTPUT_EN       BIT(3)
+#define BMI260_INT1_INPUT_EN        BIT(4)
+
+#define BMI260_INT2_IO_CTRL     0x54
+#define BMI260_INT2_LVL             BIT(1)
+#define BMI260_INT2_OD              BIT(2)
+#define BMI260_INT2_OUTPUT_EN       BIT(3)
+#define BMI260_INT2_INPUT_EN        BIT(4)
+
+#define BMI260_INT_LATCH        0x55
+#define BMI260_INT_LATCH_EN         BIT(0)
+
+#define BMI260_INT1_MAP_FEAT    0x56
+#define BMI260_INT2_MAP_FEAT    0x57
+#define BMI260_MAP_SIG_MOTION_OUT   BIT(0)
+#define BMI260_MAP_STEP_COUNTER_OUT BIT(1)
+#define BMI260_MAP_HIGH_LOW_G_OUT   BIT(2)
+#define BMI260_MAP_TAP_OUT          BIT(3)
+#define BMI260_MAP_FLAT_OUT         BIT(4)
+#define BMI260_MAP_NO_MOTION_OUT    BIT(5)
+#define BMI260_MAP_ANY_MOTION_OUT   BIT(6)
+#define BMI260_MAP_ORIENTAION_OUT   BIT(7)
+
+#define BMI260_INT_MAP_DATA     0x58
+#define BMI260_MAP_FFULL_INT        BIT(0)
+#define BMI260_MAP_FWM_INT          BIT(1)
+#define BMI260_MAP_DRDY_INT         BIT(2)
+#define BMI260_MAP_ERR_INT          BIT(3)
+#define BMI260_INT_MAP_DATA_INT1_OFFSET     0
+#define BMI260_INT_MAP_DATA_INT2_OFFSET     4
+#define BMI260_INT_MAP_DATA_REG(_i, _bit) \
+	(CONCAT3(BMI260_MAP_, _bit, _INT) << \
+	CONCAT3(BMI260_INT_MAP_DATA_INT, _i, _OFFSET))
+
+#define BMI260_INIT_CTRL        0x59
+#define BMI260_INIT_ADDR_0      0x5b
+#define BMI260_INIT_ADDR_1      0x5c
+#define BMI260_INIT_DATA        0x5e
+#define BMI260_INTERNAL_ERROR   0x5f
+#define BMI260_INT_ERR_1            BIT(1)
+#define BMI260_INT_ERR_2            BIT(2)
+#define BMI260_FEAT_ENG_DISABLED    BIT(4)
+
+#define BMI260_AUX_IF_TRIM      0x68
+#define BMI260_GYR_CRT_CONF     0x69
+
+#define BMI260_NVM_CONF         0x6a
+#define BMI260_NVM_PROG_EN          BIT(1)
+
+#define BMI260_IF_CONF          0x6b
+#define BMI260_IF_SPI3              BIT(0)
+#define BMI260_IF_SPI3_OIS          BIT(1)
+#define BMI260_IF_OIS_EN            BIT(4)
+#define BMI260_IF_AUX_EN            BIT(5)
+
+#define BMI260_DRV              0x6c
+#define BMI260_ACC_SELF_TEST    0x6d
+
+#define BMI260_NV_CONF          0x70
+#define BMI260_ACC_OFFSET_EN        BIT(3)
+
+#define BMI260_OFFSET_ACC70     0x71
+#define BMI260_OFFSET_GYR70     0x74
+#define BMI260_OFFSET_EN_GYR98  0x77
+#define BMI260_OFFSET_GYRO_EN       BIT(6)
+#define BMI260_GYR_GAIN_EN          BIT(7)
+
+#define BMI260_PWR_CONF         0x7c
+#define BMI260_ADV_POWER_SAVE       BIT(0)
+#define BMI260_FIFO_SELF_WAKE_UP    BIT(1)
+#define BMI260_FUP_EN               BIT(2)
+
+#define BMI260_PWR_CTRL         0x7d
+#define BMI260_AUX_EN               BIT(0)
+#define BMI260_GYR_EN               BIT(1)
+#define BMI260_ACC_EN               BIT(2)
+#define BMI260_PWR_EN(_sensor_type) BIT(2 - _sensor_type)
+#define BMI260_TEMP_EN              BIT(3)
+
+#define BMI260_CMD_REG          0x7e
+#define BMI260_CMD_FIFO_FLUSH       0xb0
+#define BMI260_CMD_SOFT_RESET       0xb6
+
+#define BMI260_FF_FRAME_LEN_TS          4
+#define BMI260_FF_DATA_LEN_ACC          6
+#define BMI260_FF_DATA_LEN_GYR          6
+
+#define BMI260_FF_DATA_LEN_MAG          8
+
+extern const struct accelgyro_drv bmi260_drv;
+
+void bmi260_interrupt(enum gpio_signal signal);
+
+#ifdef CONFIG_CMD_I2C_STRESS_TEST_ACCEL
+extern struct i2c_stress_test_dev bmi260_i2c_stress_test_dev;
+#endif
+
+#endif /* __CROS_EC_ACCELGYRO_BMI260_H */
diff --git a/driver/accelgyro_bmi_common.c b/driver/accelgyro_bmi_common.c
index 87ad203170..f348edb1c5 100644
--- a/driver/accelgyro_bmi_common.c
+++ b/driver/accelgyro_bmi_common.c
@@ -21,35 +21,66 @@
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
 
+#if defined(CONFIG_ACCELGYRO_BMI260) && !defined(CONFIG_ACCELGYRO_BMI160)
+#define V(s_) 1
+#elif defined(CONFIG_ACCELGYRO_BMI160) && !defined(CONFIG_ACCELGYRO_BMI260)
+#define V(s_) 0
+#else
+#define V(s_) ((s_)->chip == MOTIONSENSE_CHIP_BMI260)
+#endif
+/* Index for which table to use. */
+#if !defined(CONFIG_ACCELGYRO_BMI160) || !defined(CONFIG_ACCELGYRO_BMI260)
+#define T(s_) 0
+#else
+#define T(s_) V(s_)
+#endif
+
 /* 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},
+const struct bmi_accel_param_pair g_ranges[][4] = {
+#ifdef CONFIG_ACCELGYRO_BMI160
+	{ {2,  BMI160_GSEL_2G},
+	  {4,  BMI160_GSEL_4G},
+	  {8,  BMI160_GSEL_8G},
+	  {16, BMI160_GSEL_16G} },
+#endif
+#ifdef CONFIG_ACCELGYRO_BMI260
+	{ {2,  BMI260_GSEL_2G},
+	  {4,  BMI260_GSEL_4G},
+	  {8,  BMI260_GSEL_8G},
+	  {16, BMI260_GSEL_16G} },
+#endif
 };
 
 /*
  * 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},
+const struct bmi_accel_param_pair dps_ranges[][5] = {
+#ifdef CONFIG_ACCELGYRO_BMI160
+	{ {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} },
+#endif
+#ifdef CONFIG_ACCELGYRO_BMI260
+	{ {125,  BMI260_DPS_SEL_125},
+	  {250,  BMI260_DPS_SEL_250},
+	  {500,  BMI260_DPS_SEL_500},
+	  {1000, BMI260_DPS_SEL_1000},
+	  {2000, BMI260_DPS_SEL_2000} },
+#endif
 };
 
 int bmi_get_xyz_reg(const struct motion_sensor_t *s)
 {
 	switch (s->type) {
 	case MOTIONSENSE_TYPE_ACCEL:
-		return BMI_ACC_DATA;
+		return BMI_ACC_DATA(V(s));
 	case MOTIONSENSE_TYPE_GYRO:
-		return BMI_GYR_DATA;
+		return BMI_GYR_DATA(V(s));
 	case MOTIONSENSE_TYPE_MAG:
-		return BMI_AUX_DATA;
+		return BMI_AUX_DATA(V(s));
 	default:
 		return -1;
 	}
@@ -60,12 +91,12 @@ const struct bmi_accel_param_pair *bmi_get_range_table(
 {
 	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
 		if (psize)
-			*psize = ARRAY_SIZE(g_ranges);
-		return g_ranges;
+			*psize = ARRAY_SIZE(g_ranges[T(s)]);
+		return g_ranges[T(s)];
 	}
 	if (psize)
-		*psize = ARRAY_SIZE(dps_ranges);
-	return dps_ranges;
+		*psize = ARRAY_SIZE(dps_ranges[T(s)]);
+	return dps_ranges[T(s)];
 }
 
 /**
@@ -188,7 +219,7 @@ int bmi_read16(const int port, const uint16_t i2c_spi_addr_flags,
 	if (SLAVE_IS_SPI(i2c_spi_addr_flags)) {
 #ifdef CONFIG_SPI_ACCEL_PORT
 		rv = bmi_spi_raw_read(SLAVE_GET_SPI_ADDR(i2c_spi_addr_flags),
-				  reg, (uint8_t *)data_ptr, 2);
+				      reg, (uint8_t *)data_ptr, 2);
 #endif
 	} else {
 #ifdef I2C_PORT_ACCEL
@@ -430,21 +461,27 @@ int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
 		 * When sensor is resumed, we won't read old data.
 		 */
 		bmi_write8(s->port, s->i2c_spi_addr_flags,
-			   BMI_CMD_REG, BMI_CMD_FIFO_FLUSH);
+			   BMI_CMD_REG(V(s)), BMI_CMD_FIFO_FLUSH);
 		return EC_SUCCESS;
 	}
 
 	bmi_read_n(s->port, s->i2c_spi_addr_flags,
-		   BMI_FIFO_LENGTH_0,
+		   BMI_FIFO_LENGTH_0(V(s)),
 		   (uint8_t *)&length, sizeof(length));
-	length &= BMI_FIFO_LENGTH_MASK;
+	length &= BMI_FIFO_LENGTH_MASK(V(s));
 
 	/*
 	 * 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");
+		/*
+		 * Disable this message on BMI260, due to this seems to always
+		 * happen after we complete to read the data.
+		 * TODO(chingkang): check why this happen on BMI260.
+		 */
+		if (V(s) == 0)
+			CPRINTS("unexpected empty FIFO");
 		return EC_SUCCESS;
 	}
 
@@ -456,7 +493,7 @@ int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
 	length = MIN(length, sizeof(bmi_buffer));
 
 	bmi_read_n(s->port, s->i2c_spi_addr_flags,
-		   BMI_FIFO_DATA, bmi_buffer, length);
+		   BMI_FIFO_DATA(V(s)), bmi_buffer, length);
 	beginning = *(uint32_t *)bmi_buffer;
 	ep = bmi_buffer + length;
 	/*
@@ -502,7 +539,7 @@ int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
 				CPRINTS("Unknown header: 0x%02x @ %zd",
 						hdr, bp - bmi_buffer);
 				bmi_write8(s->port, s->i2c_spi_addr_flags,
-						BMI_CMD_REG,
+						BMI_CMD_REG(V(s)),
 						BMI_CMD_FIFO_FLUSH);
 				return EC_ERROR_NOT_HANDLED;
 			}
@@ -518,7 +555,10 @@ int bmi_load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
 			CPRINTS("@ %zd - %d, config change: 0x%02x",
 					bp - bmi_buffer, length, *bp);
 			bp++;
-			state = FIFO_HEADER;
+			if (V(s))
+				state = FIFO_DATA_TIME;
+			else
+				state = FIFO_HEADER;
 			break;
 		case FIFO_DATA_TIME:
 			if (bp + 3 > ep) {
@@ -655,8 +695,8 @@ int bmi_enable_fifo(const struct motion_sensor_t *s, int enable)
 	int ret;
 
 	/* FIFO start/stop collecting events */
-	ret = bmi_enable_reg8(s, BMI_FIFO_CONFIG_1,
-			      BMI_FIFO_SENSOR_EN(s->type), enable);
+	ret = bmi_enable_reg8(s, BMI_FIFO_CONFIG_1(V(s)),
+			      BMI_FIFO_SENSOR_EN(V(s), s->type), enable);
 	if (ret)
 		return ret;
 
@@ -674,7 +714,7 @@ int bmi_read(const struct motion_sensor_t *s, intv3_t v)
 	int ret, status = 0;
 
 	ret = bmi_read8(s->port, s->i2c_spi_addr_flags,
-			BMI_STATUS, &status);
+			BMI_STATUS(V(s)), &status);
 	if (ret != EC_SUCCESS)
 		return ret;
 
@@ -713,7 +753,7 @@ int bmi_get_sensor_temp(int idx, int *temp_ptr)
 	int ret;
 
 	ret = bmi_read_n(s->port, s->i2c_spi_addr_flags,
-			 BMI_TEMPERATURE_0,
+			 BMI_TEMPERATURE_0(V(s)),
 			 (uint8_t *)&temp, sizeof(temp));
 
 	if (ret || temp == BMI_INVALID_TEMP)
@@ -765,7 +805,7 @@ void bmi_accel_get_offset(const struct motion_sensor_t *accel, intv3_t v)
 
 	for (i = X; i <= Z; i++) {
 		bmi_read8(accel->port, accel->i2c_spi_addr_flags,
-			  BMI_OFFSET_ACC70 + i, &val);
+			  BMI_OFFSET_ACC70(V(accel)) + i, &val);
 		if (val > 0x7f)
 			val = -256 + val;
 		v[i] = round_divide(
@@ -781,10 +821,10 @@ void bmi_gyro_get_offset(const struct motion_sensor_t *gyro, intv3_t v)
 
 	/* Read the MSB first */
 	bmi_read8(gyro->port, gyro->i2c_spi_addr_flags,
-		  BMI_OFFSET_EN_GYR98, &val98);
+		  BMI_OFFSET_EN_GYR98(V(gyro)), &val98);
 	for (i = X; i <= Z; i++) {
 		bmi_read8(gyro->port, gyro->i2c_spi_addr_flags,
-			  BMI_OFFSET_GYR70 + i, &val);
+			  BMI_OFFSET_GYR70(V(gyro)) + i, &val);
 		val |= ((val98 >> (2 * i)) & 0x3) << 8;
 		if (val > 0x1ff)
 			val = -1024 + val;
@@ -809,7 +849,7 @@ void bmi_set_accel_offset(const struct motion_sensor_t *accel, intv3_t v)
 		if (val < 0)
 			val = 256 + val;
 		bmi_write8(accel->port, accel->i2c_spi_addr_flags,
-			   BMI_OFFSET_ACC70 + i, val);
+			   BMI_OFFSET_ACC70(V(accel)) + i, val);
 	}
 }
 
@@ -829,7 +869,7 @@ void bmi_set_gyro_offset(const struct motion_sensor_t *gyro, intv3_t v,
 		if (val < 0)
 			val = 1024 + val;
 		bmi_write8(gyro->port, gyro->i2c_spi_addr_flags,
-			   BMI_OFFSET_GYR70 + i, val & 0xFF);
+			   BMI_OFFSET_GYR70(V(gyro)) + 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
index cf7f0d06cb..583d43b04d 100644
--- a/driver/accelgyro_bmi_common.h
+++ b/driver/accelgyro_bmi_common.h
@@ -9,6 +9,7 @@
 
 #include "accelgyro.h"
 #include "driver/accelgyro_bmi160.h"
+#include "driver/accelgyro_bmi260.h"
 #include "mag_bmm150.h"
 
 #define BMI_CONF_REG(_sensor)      (0x40 + 2 * (_sensor))
@@ -99,39 +100,54 @@ struct bmi_drv_data_t {
 	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_ACC_DATA(v) (BMI160_ACC_X_L_G + \
+	(v) * (BMI260_ACC_X_L_G - BMI160_ACC_X_L_G))
+#define BMI_GYR_DATA(v) (BMI160_GYR_X_L_G + \
+	(v) * (BMI260_GYR_X_L_G - BMI160_GYR_X_L_G))
+#define BMI_AUX_DATA(v) (BMI160_MAG_X_L_G + \
+	(v) * (BMI260_AUX_X_L_G - BMI160_MAG_X_L_G))
+
+#define BMI_FIFO_CONFIG_0(v) (BMI160_FIFO_CONFIG_0 + \
+	(v) * (BMI260_FIFO_CONFIG_0 - BMI160_FIFO_CONFIG_0))
+#define BMI_FIFO_CONFIG_1(v) (BMI160_FIFO_CONFIG_1 + \
+	(v) * (BMI260_FIFO_CONFIG_1 - BMI160_FIFO_CONFIG_1))
+#define BMI_FIFO_SENSOR_EN(v, _sensor) (BMI160_FIFO_SENSOR_EN(_sensor) + \
+	(v) * (BMI260_FIFO_SENSOR_EN(_sensor) - BMI160_FIFO_SENSOR_EN(_sensor)))
+
+#define BMI_TEMPERATURE_0(v) (BMI160_TEMPERATURE_0 + \
+	(v) * (BMI260_TEMPERATURE_0 - BMI160_TEMPERATURE_0))
 #define BMI_INVALID_TEMP     0x8000
 
-#define BMI_STATUS BMI160_STATUS
+#define BMI_STATUS(v) (BMI160_STATUS + \
+	(v) * (BMI260_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
+#define BMI_OFFSET_ACC70(v) (BMI160_OFFSET_ACC70 + \
+	(v) * (BMI260_OFFSET_ACC70 - BMI160_OFFSET_ACC70))
+#define BMI_OFFSET_GYR70(v) (BMI160_OFFSET_GYR70 + \
+	(v) * (BMI260_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_EN_GYR98(v) (BMI160_OFFSET_EN_GYR98 + \
+	(v) * (BMI260_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_FIFO_LENGTH_0(v) (BMI160_FIFO_LENGTH_0 + \
+	(v) * (BMI260_FIFO_LENGTH_0 - BMI160_FIFO_LENGTH_0))
+#define BMI_FIFO_LENGTH_MASK(v) (BMI160_FIFO_LENGTH_MASK + \
+	(v) * (BMI260_FIFO_LENGTH_MASK - BMI160_FIFO_LENGTH_MASK))
+#define BMI_FIFO_DATA(v) (BMI160_FIFO_DATA + \
+	(v) * (BMI260_FIFO_DATA - BMI160_FIFO_DATA))
 
-#define BMI_CMD_REG BMI160_CMD_REG
+#define BMI_CMD_REG(v) (BMI160_CMD_REG + \
+	(v) * (BMI260_CMD_REG - BMI160_CMD_REG))
 #define BMI_CMD_FIFO_FLUSH 0xb0
 
 
diff --git a/driver/build.mk b/driver/build.mk
index a4d80d429b..4b02c90a0e 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -15,6 +15,7 @@ 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 accelgyro_bmi_common.o
+driver-$(CONFIG_ACCELGYRO_BMI260)+=accelgyro_bmi260.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_ACCELGYRO_LSM6DSO)+=accelgyro_lsm6dso.o stm_mems_common.o