driver: add ICM-426xx driver support

Add ICM-426xx accel/gyro driver code.

BUG=chromium:1117541
BRANCH=None
TEST=ectool motionsense fifo_read && tast run hardware.SensorRing

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Change-Id: I83fe48abc6aa9cde86576a777ac4272d90fac597
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2317888
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Commit-Queue: Gwendal Grignou <gwendal@chromium.org>
Tested-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2584545
Tested-by: David Huang <david.huang@quanta.corp-partner.google.com>
Reviewed-by: Zhuohao Lee <zhuohao@chromium.org>
Commit-Queue: Zhuohao Lee <zhuohao@chromium.org>
Auto-Submit: David Huang <david.huang@quanta.corp-partner.google.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2600803
Reviewed-by: YH Lin <yueherngl@chromium.org>
Commit-Queue: YH Lin <yueherngl@chromium.org>

9 files changed, 1581 insertions, 0 deletions

diff --git a/common/build.mk b/common/build.mk
index 39e111956f..30afabd146 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -11,6 +11,7 @@ common-y+=version.o printf.o queue.o queue_policies.o
 
 common-$(CONFIG_ACCELGYRO_BMA255)+=math_util.o
 common-$(CONFIG_ACCELGYRO_BMI160)+=math_util.o
+common-$(CONFIG_ACCELGYRO_ICM426XX)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DS0)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DSM)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DSO)+=math_util.o
diff --git a/driver/accelgyro_icm426xx.c b/driver/accelgyro_icm426xx.c
new file mode 100644
index 0000000000..5598400fca
--- /dev/null
+++ b/driver/accelgyro_icm426xx.c
@@ -0,0 +1,916 @@
+/* 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.
+ */
+
+/**
+ * ICM-426xx accelerometer and gyroscope module for Chrome EC
+ * 3D digital accelerometer & 3D digital gyroscope
+ */
+
+#include "accelgyro.h"
+#include "console.h"
+#include "driver/accelgyro_icm_common.h"
+#include "driver/accelgyro_icm426xx.h"
+#include "hwtimer.h"
+#include "i2c.h"
+#include "math_util.h"
+#include "motion_sense.h"
+#include "spi.h"
+#include "task.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)
+
+#ifdef CONFIG_ACCEL_FIFO
+volatile uint32_t last_interrupt_timestamp;
+#endif
+
+static int icm426xx_normalize(const struct motion_sensor_t *s, intv3_t v,
+			      const uint8_t *raw)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+	int i;
+
+	/* sensor data is configured as little-endian */
+	v[X] = (int16_t)UINT16_FROM_BYTE_ARRAY_LE(raw, 0);
+	v[Y] = (int16_t)UINT16_FROM_BYTE_ARRAY_LE(raw, 2);
+	v[Z] = (int16_t)UINT16_FROM_BYTE_ARRAY_LE(raw, 4);
+
+	/* check if data is valid */
+	if (v[X] == ICM426XX_INVALID_DATA &&
+	    v[Y] == ICM426XX_INVALID_DATA &&
+	    v[Z] == ICM426XX_INVALID_DATA) {
+		return EC_ERROR_INVAL;
+	}
+
+	rotate(v, *s->rot_standard_ref, v);
+
+	for (i = X; i <= Z; i++)
+		v[i] = SENSOR_APPLY_SCALE(v[i], data->scale[i]);
+
+	return EC_SUCCESS;
+}
+
+/* use FIFO threshold interrupt on INT1 */
+#define ICM426XX_FIFO_INT_EN		ICM426XX_FIFO_THS_INT1_EN
+#define ICM426XX_FIFO_INT_STATUS	ICM426XX_FIFO_THS_INT
+
+static int __maybe_unused icm426xx_enable_fifo(const struct motion_sensor_t *s,
+					       int enable)
+{
+	int val, ret;
+
+	if (enable) {
+		/* enable FIFO interrupts */
+		ret = icm_field_update8(s, ICM426XX_REG_INT_SOURCE0,
+					ICM426XX_FIFO_INT_EN,
+					ICM426XX_FIFO_INT_EN);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/* flush FIFO data */
+		ret = icm_write8(s, ICM426XX_REG_SIGNAL_PATH_RESET,
+				 ICM426XX_FIFO_FLUSH);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/* set FIFO in streaming mode */
+		ret = icm_write8(s, ICM426XX_REG_FIFO_CONFIG,
+				 ICM426XX_FIFO_MODE_STREAM);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/* workaround: first read of FIFO count is always 0 */
+		ret = icm_read16(s, ICM426XX_REG_FIFO_COUNT, &val);
+		if (ret != EC_SUCCESS)
+			return ret;
+	} else {
+		/* set FIFO in bypass mode */
+		ret = icm_write8(s, ICM426XX_REG_FIFO_CONFIG,
+				 ICM426XX_FIFO_MODE_BYPASS);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/* flush FIFO data */
+		ret = icm_write8(s, ICM426XX_REG_SIGNAL_PATH_RESET,
+				 ICM426XX_FIFO_FLUSH);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/* disable FIFO interrupts */
+		ret = icm_field_update8(s, ICM426XX_REG_INT_SOURCE0,
+					ICM426XX_FIFO_INT_EN, 0);
+		if (ret != EC_SUCCESS)
+			return ret;
+	}
+
+	return EC_SUCCESS;
+}
+
+static int __maybe_unused icm426xx_config_fifo(const struct motion_sensor_t *s,
+					       int enable)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	int mask, val;
+	uint8_t old_fifo_en;
+	int ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		mask = ICM426XX_FIFO_ACCEL_EN;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		mask = ICM426XX_FIFO_GYRO_EN;
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+	/* temperature data has to be always present in the FIFO */
+	mask |= ICM426XX_FIFO_TEMP_EN;
+
+	val = enable ? mask : 0;
+
+	mutex_lock(s->mutex);
+
+	ret = icm_field_update8(s, ICM426XX_REG_FIFO_CONFIG1, mask, val);
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	old_fifo_en = st->fifo_en;
+	if (enable)
+		st->fifo_en |= BIT(s->type);
+	else
+		st->fifo_en &= ~BIT(s->type);
+
+	if (!old_fifo_en && st->fifo_en) {
+		/* 1st sensor enabled => turn FIFO on */
+		ret = icm426xx_enable_fifo(s, 1);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+	} else if (old_fifo_en && !st->fifo_en) {
+		/* last sensor disabled => turn FIFO off */
+		ret = icm426xx_enable_fifo(s, 0);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+	}
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static void __maybe_unused icm426xx_push_fifo_data(struct motion_sensor_t *s,
+						const uint8_t *raw, uint32_t ts)
+{
+	intv3_t v;
+	struct ec_response_motion_sensor_data vect;
+	int ret;
+
+	if (s == NULL)
+		return;
+
+	ret = icm426xx_normalize(s, v, raw);
+	if (ret == EC_SUCCESS) {
+		vect.data[X] = v[X];
+		vect.data[Y] = v[Y];
+		vect.data[Z] = v[Z];
+		vect.flags = 0;
+		vect.sensor_num = s - motion_sensors;
+		motion_sense_fifo_stage_data(&vect, s, 3, ts);
+	}
+}
+
+static int __maybe_unused icm426xx_load_fifo(struct motion_sensor_t *s,
+					     uint32_t ts)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	int count, i, size;
+	const uint8_t *accel, *gyro;
+	int ret;
+
+	ret = icm_read16(s, ICM426XX_REG_FIFO_COUNT, &count);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	if (count <= 0)
+		return EC_ERROR_INVAL;
+
+	/* flush FIFO if buffer is not large enough */
+	if (count > ICM_FIFO_BUFFER) {
+		CPRINTS("It should not happen, the EC is too slow for the ODR");
+		ret = icm_write8(s, ICM426XX_REG_SIGNAL_PATH_RESET,
+				 ICM426XX_FIFO_FLUSH);
+		if (ret != EC_SUCCESS)
+			return ret;
+		return EC_ERROR_OVERFLOW;
+	}
+
+	ret = icm_read_n(s, ICM426XX_REG_FIFO_DATA, st->fifo_buffer, count);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	for (i = 0; i < count; i += size) {
+		size = icm_fifo_decode_packet(&st->fifo_buffer[i],
+				&accel, &gyro);
+		/* exit if error or FIFO is empty */
+		if (size <= 0)
+			return -size;
+		if (accel != NULL)
+			icm426xx_push_fifo_data(st->accel, accel, ts);
+		if (gyro != NULL)
+			icm426xx_push_fifo_data(st->gyro, gyro, ts);
+	}
+
+	return EC_SUCCESS;
+}
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+
+/**
+ * icm426xx_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", ->icm426xx_irq_handler().
+ */
+void icm426xx_interrupt(enum gpio_signal signal)
+{
+#ifdef CONFIG_ACCEL_FIFO
+	last_interrupt_timestamp = __hw_clock_source_read();
+#endif
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCELGYRO_ICM426XX_INT_EVENT, 0);
+}
+
+/**
+ * icm426xx_irq_handler - bottom half of the interrupt stack.
+ * Ran from the motion_sense task, finds the events that raised the interrupt.
+ */
+static int icm426xx_irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	int status;
+	int ret;
+
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+	    (!(*event & CONFIG_ACCELGYRO_ICM426XX_INT_EVENT)))
+		return EC_ERROR_NOT_HANDLED;
+
+	mutex_lock(s->mutex);
+
+	/* read and clear interrupt status */
+	ret = icm_read8(s, ICM426XX_REG_INT_STATUS, &status);
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+#ifdef CONFIG_ACCEL_FIFO
+	if (status & ICM426XX_FIFO_INT_STATUS) {
+		ret = icm426xx_load_fifo(s, last_interrupt_timestamp);
+	}
+#endif
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int icm426xx_config_interrupt(const struct motion_sensor_t *s)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	int val, ret;
+
+	/* configure INT1 pin */
+	val = ICM426XX_INT1_PUSH_PULL;
+
+	ret = icm_write8(s, ICM426XX_REG_INT_CONFIG, val);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* deassert async reset for proper INT pin operation */
+	ret = icm_field_update8(s, ICM426XX_REG_INT_CONFIG1,
+				ICM426XX_INT_ASYNC_RESET, 0);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+#ifdef CONFIG_ACCEL_FIFO
+	/*
+	 * configure FIFO:
+	 * - enable FIFO partial read
+	 * - enable continuous watermark interrupt
+	 * - disable all FIFO en bits
+	 */
+	val = ICM426XX_FIFO_PARTIAL_READ | ICM426XX_FIFO_WM_GT_TH;
+	ret = icm_field_update8(s, ICM426XX_REG_FIFO_CONFIG1,
+				GENMASK(6, 5) | ICM426XX_FIFO_EN_MASK,
+				val);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* clear internal FIFO enable bits tracking */
+	st->fifo_en = 0;
+
+	/* set FIFO watermark to 1 data packet (8 bytes) */
+	ret = icm_write16(s, ICM426XX_REG_FIFO_WATERMARK, 8);
+	if (ret != EC_SUCCESS)
+		return ret;
+#endif
+
+	return ret;
+}
+
+#endif	/* CONFIG_ACCEL_INTERRUPTS */
+
+static int icm426xx_enable_sensor(const struct motion_sensor_t *s, int enable)
+{
+	unsigned int sleep;
+	uint8_t mask, val;
+	int ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		mask = ICM426XX_ACCEL_MODE_MASK;
+		if (enable) {
+			sleep = 20;
+			val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_LOW_POWER);
+		} else {
+			sleep = 0;
+			val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_OFF);
+		}
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		mask = ICM426XX_GYRO_MODE_MASK;
+		if (enable) {
+			sleep = 60;
+			val = ICM426XX_GYRO_MODE(ICM426XX_MODE_LOW_NOISE);
+		} else {
+			sleep = 150;
+			val = ICM426XX_GYRO_MODE(ICM426XX_MODE_OFF);
+		}
+		break;
+	default:
+		return -EC_ERROR_INVAL;
+	}
+
+	mutex_lock(s->mutex);
+
+	ret = icm_field_update8(s, ICM426XX_REG_PWR_MGMT0, mask, val);
+	/* when turning sensor on block any register write for 200 us */
+	if (ret == EC_SUCCESS && enable)
+		usleep(200);
+
+	mutex_unlock(s->mutex);
+
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	if (sleep)
+		msleep(sleep);
+
+	return EC_SUCCESS;
+}
+
+static int icm426xx_set_data_rate(const struct motion_sensor_t *s, int rate,
+				  int rnd)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+	int reg, ret, reg_val;
+	int normalized_rate;
+	int max_rate, min_rate;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		reg = ICM426XX_REG_ACCEL_CONFIG0;
+		min_rate = ICM426XX_ACCEL_MIN_FREQ;
+		max_rate = ICM426XX_ACCEL_MAX_FREQ;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		reg = ICM426XX_REG_GYRO_CONFIG0;
+		min_rate = ICM426XX_GYRO_MIN_FREQ;
+		max_rate = ICM426XX_GYRO_MAX_FREQ;
+		break;
+	default:
+		return EC_RES_INVALID_PARAM;
+	}
+
+	/* normalize the rate */
+	reg_val = ICM426XX_ODR_TO_REG(rate);
+	normalized_rate = ICM426XX_REG_TO_ODR(reg_val);
+
+	/* round up the rate */
+	if (rnd && (normalized_rate < rate)) {
+		reg_val = ICM426XX_ODR_REG_UP(reg_val);
+		normalized_rate = ICM426XX_REG_TO_ODR(reg_val);
+	}
+
+	if (rate > 0) {
+		if ((normalized_rate < min_rate) ||
+			(normalized_rate > max_rate))
+			return EC_RES_INVALID_PARAM;
+	}
+
+	if (rate == 0) {
+		/* disable data in FIFO */
+#ifdef CONFIG_ACCEL_FIFO
+		icm426xx_config_fifo(s, 0);
+#endif
+		/* disable sensor */
+		ret = icm426xx_enable_sensor(s, 0);
+		data->odr = 0;
+		return ret;
+	} else if (data->odr == 0) {
+		/* enable sensor */
+		ret = icm426xx_enable_sensor(s, 1);
+		if (ret)
+			return ret;
+	}
+
+	mutex_lock(s->mutex);
+
+	ret = icm_field_update8(s, reg, ICM426XX_ODR_MASK,
+				ICM426XX_ODR(reg_val));
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	data->odr = normalized_rate;
+
+	mutex_unlock(s->mutex);
+
+	/* enable data in FIFO */
+#ifdef CONFIG_ACCEL_FIFO
+	icm426xx_config_fifo(s, 1);
+#endif
+
+	return EC_SUCCESS;
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int icm426xx_set_range(const struct motion_sensor_t *s, int range,
+			      int rnd)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+	int reg, ret, reg_val;
+	int newrange;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		reg = ICM426XX_REG_ACCEL_CONFIG0;
+
+		reg_val = ICM426XX_ACCEL_FS_TO_REG(range);
+		newrange = ICM426XX_ACCEL_REG_TO_FS(reg_val);
+
+		if (rnd && (newrange < range) && (reg_val > 0)) {
+			reg_val--;
+			newrange = ICM426XX_ACCEL_REG_TO_FS(reg_val);
+		}
+
+		if (newrange > ICM426XX_ACCEL_FS_MAX_VAL) {
+			newrange = ICM426XX_ACCEL_FS_MAX_VAL;
+			reg_val = ICM426XX_ACCEL_FS_TO_REG(range);
+		}
+
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		reg = ICM426XX_REG_GYRO_CONFIG0;
+
+		reg_val = ICM426XX_GYRO_FS_TO_REG(range);
+		newrange = ICM426XX_GYRO_REG_TO_FS(reg_val);
+
+		if (rnd && (newrange < range) && (reg_val > 0)) {
+			reg_val--;
+			newrange = ICM426XX_GYRO_REG_TO_FS(reg_val);
+		}
+
+		if (newrange > ICM426XX_GYRO_FS_MAX_VAL) {
+			newrange = ICM426XX_GYRO_FS_MAX_VAL;
+			reg_val = ICM426XX_GYRO_FS_TO_REG(newrange);
+		}
+
+		break;
+	default:
+		return EC_RES_INVALID_PARAM;
+	}
+
+	mutex_lock(s->mutex);
+
+	ret = icm_field_update8(s, reg, ICM426XX_FS_MASK,
+				ICM426XX_FS_SEL(reg_val));
+	if (ret == EC_SUCCESS)
+		data->range = newrange;
+
+	mutex_unlock(s->mutex);
+
+	return ret;
+}
+
+static int icm426xx_get_hw_offset(const struct motion_sensor_t *s,
+				  intv3_t offset)
+{
+	uint8_t raw[5];
+	int i, ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		mutex_lock(s->mutex);
+		ret = icm_read_n(s, ICM426XX_REG_OFFSET_USER4,
+				raw, sizeof(raw));
+		mutex_unlock(s->mutex);
+		if (ret != EC_SUCCESS)
+			return ret;
+		/*
+		 * raw[0]: Accel X[11:8] | gyro Z[11:8]
+		 * raw[1]: Accel X[0:7]
+		 * raw[2]: Accel Y[7:0]
+		 * raw[3]: Accel Z[11:8] | Accel Y[11:8]
+		 * raw[4]: Accel Z[7:0]
+		 */
+		offset[X] = (((int)raw[0] << 4) & ~GENMASK(7, 0)) | raw[1];
+		offset[Y] = (((int)raw[3] << 8) & ~GENMASK(7, 0)) | raw[2];
+		offset[Z] = (((int)raw[3] << 4) & ~GENMASK(7, 0)) | raw[4];
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		mutex_lock(s->mutex);
+		ret = icm_read_n(s, ICM426XX_REG_OFFSET_USER0,
+				raw, sizeof(raw));
+		mutex_unlock(s->mutex);
+		if (ret != EC_SUCCESS)
+			return ret;
+		/*
+		 * raw[0]: Gyro X[7:0]
+		 * raw[1]: Gyro Y[11:8] | Gyro X[11:8]
+		 * raw[2]: Gyro Y[7:0]
+		 * raw[3]: Gyro Z[7:0]
+		 * raw[4]: Accel X[11:8] | gyro Z[11:8]
+		 */
+		offset[X] = (((int)raw[1] << 8) & ~GENMASK(7, 0)) | raw[0];
+		offset[Y] = (((int)raw[1] << 4) & ~GENMASK(7, 0)) | raw[2];
+		offset[Z] = (((int)raw[4] << 8) & ~GENMASK(7, 0)) | raw[3];
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+
+	/* Extend sign-bit of 12 bits signed values */
+	for (i = X; i <= Z; ++i)
+		offset[i] = sign_extend(offset[i], 11);
+
+	return EC_SUCCESS;
+}
+
+static int icm426xx_set_hw_offset(const struct motion_sensor_t *s,
+				  intv3_t offset)
+{
+	int i, val, ret;
+
+	/* value is 12 bits signed maximum */
+	for (i = X; i <= Z; ++i) {
+		if (offset[i] > 2047)
+			offset[i] = 2047;
+		else if (offset[i] < -2048)
+			offset[i] = -2048;
+	}
+
+	mutex_lock(s->mutex);
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/* Accel X[11:8] | gyro Z[11:8] */
+		val = (offset[X] >> 4) & GENMASK(7, 4);
+		ret = icm_field_update8(s, ICM426XX_REG_OFFSET_USER4,
+					GENMASK(7, 4), val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Accel X[7:0] */
+		val = offset[X] & GENMASK(7, 0);
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER5, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Accel Y[7:0] */
+		val = offset[Y] & GENMASK(7, 0);
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER6, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Accel Z[11:8] | Accel Y[11:8] */
+		val = ((offset[Z] >> 4) & GENMASK(7, 4)) |
+		      ((offset[Y] >> 8) & GENMASK(3, 0));
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER7, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Accel Z[7:0] */
+		val = offset[Z] & GENMASK(7, 0);
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER8, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+		break;
+
+	case MOTIONSENSE_TYPE_GYRO:
+		/* Gyro X[7:0] */
+		val = offset[X] & GENMASK(7, 0);
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER0, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Gyro Y[11:8] | Gyro X[11:8] */
+		val = ((offset[Y] >> 4) & GENMASK(7, 4)) |
+		      ((offset[X] >> 8) & GENMASK(3, 0));
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER1, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Gyro Y[7:0] */
+		val = offset[Y] & GENMASK(7, 0);
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER2, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Gyro Z[7:0] */
+		val = offset[Z] & GENMASK(7, 0);
+		ret = icm_write8(s, ICM426XX_REG_OFFSET_USER3, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Accel X[11:8] | gyro Z[11:8] */
+		val = (offset[Z] >> 8) & GENMASK(3, 0);
+		ret = icm_field_update8(s, ICM426XX_REG_OFFSET_USER4,
+					GENMASK(3, 0), val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+		break;
+
+	default:
+		ret = EC_ERROR_INVAL;
+		break;
+	}
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int icm426xx_set_offset(const struct motion_sensor_t *s,
+			       const int16_t *offset, int16_t temp)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+	intv3_t v = { offset[X], offset[Y], offset[Z] };
+	int range, div1, div2;
+	int i;
+
+	/* unscale values and rotate back to chip frame */
+	for (i = X; i <= Z; ++i)
+		v[i] = SENSOR_APPLY_DIV_SCALE(v[i], data->scale[i]);
+	rotate_inv(v, *s->rot_standard_ref, v);
+
+	/* convert raw data to hardware offset units */
+	range = icm_get_range(s);
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/* hardware offset is 1/2048g by LSB */
+		div1 = range * 2048;
+		div2 = 32768;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		/* hardware offset is 1/32dps by LSB */
+		div1 = range * 32;
+		div2 = 32768;
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+	for (i = X; i <= Z; ++i)
+		v[i] = round_divide(v[i] * div1, div2);
+
+	return icm426xx_set_hw_offset(s, v);
+}
+
+static int icm426xx_get_offset(const struct motion_sensor_t *s, int16_t *offset,
+			       int16_t *temp)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+	intv3_t v;
+	int range, div1, div2;
+	int i, ret;
+
+	ret = icm426xx_get_hw_offset(s, v);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* transform offset to raw data */
+	range = icm_get_range(s);
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/* hardware offset is 1/2048g by LSB */
+		div1 = 32768;
+		div2 = 2048 * range;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		/* hardware offset is 1/32dps by LSB */
+		div1 = 32768;
+		div2 = 32 * range;
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+	for (i = X; i <= Z; ++i)
+		v[i] = round_divide(v[i] * div1, div2);
+
+	rotate(v, *s->rot_standard_ref, v);
+	for (i = X; i <= Z; i++)
+		v[i] = SENSOR_APPLY_SCALE(v[i], data->scale[i]);
+
+	offset[X] = v[X];
+	offset[Y] = v[Y];
+	offset[Z] = v[Z];
+	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
+	return EC_SUCCESS;
+}
+
+static int icm426xx_read(const struct motion_sensor_t *s, intv3_t v)
+{
+	uint8_t raw[6];
+	int reg, ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		reg = ICM426XX_REG_ACCEL_DATA_XYZ;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		reg = ICM426XX_REG_GYRO_DATA_XYZ;
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+
+	/* read data registers */
+	mutex_lock(s->mutex);
+	ret = icm_read_n(s, reg, raw, sizeof(raw));
+	mutex_unlock(s->mutex);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = icm426xx_normalize(s, v, raw);
+	/* if data is invalid return the previous read data */
+	if (ret != EC_SUCCESS) {
+		if (v != s->raw_xyz)
+			memcpy(v, s->raw_xyz, sizeof(s->raw_xyz));
+	}
+
+	return EC_SUCCESS;
+}
+
+static int icm426xx_init_config(const struct motion_sensor_t *s)
+{
+	uint8_t mask, val;
+	int ret;
+
+	/*
+	 * serial bus setup (i2c or spi)
+	 *
+	 * Do not check result for INTF_CONFIG6, since it can induce
+	 * interferences on the bus.
+	 */
+
+	ret = 0;
+	icm_field_update8(s, ICM426XX_REG_INTF_CONFIG6,
+		ICM426XX_INTF_CONFIG6_MASK,
+		ICM426XX_I3C_EN);
+	ret = icm_field_update8(s, ICM426XX_REG_INTF_CONFIG4,
+		ICM426XX_I3C_BUS_MODE,
+		0);
+	if (ret)
+		return ret;
+
+	ret = 0;
+	ret = icm_field_update8(s, ICM426XX_REG_DRIVE_CONFIG,
+		ICM426XX_DRIVE_CONFIG_MASK,
+		ICM426XX_I2C_SLEW_RATE(ICM426XX_SLEW_RATE_12NS_36NS) |
+		ICM426XX_SPI_SLEW_RATE(ICM426XX_SLEW_RATE_12NS_36NS));
+	if (ret)
+		return ret;
+
+	/*
+	 * Use invalid value in registers and FIFO.
+	 * Data registers in little-endian format.
+	 * Disable unused serial interface.
+	 */
+	mask = ICM426XX_DATA_CONF_MASK | ICM426XX_UI_SIFS_CFG_MASK;
+	val = 0;
+	val |= ICM426XX_UI_SIFS_CFG_SPI_DIS;
+
+	return icm_field_update8(s, ICM426XX_REG_INTF_CONFIG0, mask, val);
+}
+
+static int icm426xx_init(const struct motion_sensor_t *s)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	struct accelgyro_saved_data_t *saved_data = ICM_GET_SAVED_DATA(s);
+	int mask, val;
+	int ret, i;
+
+	mutex_lock(s->mutex);
+
+	/* manually force register bank to 0 */
+	st->bank = 0;
+	ret = icm_write8(s, ICM426XX_REG_BANK_SEL, ICM426XX_BANK_SEL(0));
+	if (ret)
+		goto out_unlock;
+
+	/* detect chip using whoami */
+	ret = icm_read8(s, ICM426XX_REG_WHO_AM_I, &val);
+	if (ret)
+		goto out_unlock;
+
+	if (val != ICM426XX_CHIP_ICM40608 && val != ICM426XX_CHIP_ICM42605) {
+		CPRINTS("Unknown WHO_AM_I: 0x%02x", val);
+		ret = EC_ERROR_ACCESS_DENIED;
+		goto out_unlock;
+	}
+
+	/* first time init done only for 1st sensor (accel) */
+	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
+		/* reset the chip and verify it is ready */
+		ret = icm_write8(s, ICM426XX_REG_DEVICE_CONFIG,
+				 ICM426XX_SOFT_RESET_CONFIG);
+		if (ret)
+			goto out_unlock;
+		msleep(1);
+
+		ret = icm_read8(s, ICM426XX_REG_INT_STATUS, &val);
+		if (ret)
+			goto out_unlock;
+		if (!(val & ICM426XX_RESET_DONE_INT)) {
+			ret = EC_ERROR_ACCESS_DENIED;
+			goto out_unlock;
+		}
+
+		/* configure sensor */
+		ret = icm426xx_init_config(s);
+		if (ret)
+			goto out_unlock;
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+		ret = icm426xx_config_interrupt(s);
+		if (ret)
+			goto out_unlock;
+#endif
+	}
+
+	for (i = X; i <= Z; i++)
+		saved_data->scale[i] = MOTION_SENSE_DEFAULT_SCALE;
+
+	/* set sensor filter */
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		mask = ICM426XX_ACCEL_UI_FILT_MASK;
+		val = ICM426XX_ACCEL_UI_FILT_BW(ICM426XX_FILTER_BW_AVG_16X);
+#ifdef CONFIG_ACCEL_FIFO
+		st->accel = (struct motion_sensor_t *)s;
+#endif
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		mask = ICM426XX_GYRO_UI_FILT_MASK;
+		val = ICM426XX_GYRO_UI_FILT_BW(ICM426XX_FILTER_BW_ODR_DIV_2);
+#ifdef CONFIG_ACCEL_FIFO
+		st->gyro = (struct motion_sensor_t *)s;
+#endif
+		break;
+	default:
+		ret = EC_ERROR_INVAL;
+		goto out_unlock;
+	}
+	ret = icm_field_update8(s, ICM426XX_REG_GYRO_ACCEL_CONFIG0, mask, val);
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	mutex_unlock(s->mutex);
+
+	return sensor_init_done(s);
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+const struct accelgyro_drv icm426xx_drv = {
+	.init = icm426xx_init,
+	.read = icm426xx_read,
+	.set_range = icm426xx_set_range,
+	.get_range = icm_get_range,
+	.get_resolution = icm_get_resolution,
+	.set_data_rate = icm426xx_set_data_rate,
+	.get_data_rate = icm_get_data_rate,
+	.set_offset = icm426xx_set_offset,
+	.get_offset = icm426xx_get_offset,
+	.set_scale = icm_set_scale,
+	.get_scale = icm_get_scale,
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.irq_handler = icm426xx_irq_handler,
+#endif
+};
diff --git a/driver/accelgyro_icm426xx.h b/driver/accelgyro_icm426xx.h
new file mode 100644
index 0000000000..0b4e3aa14a
--- /dev/null
+++ b/driver/accelgyro_icm426xx.h
@@ -0,0 +1,256 @@
+/* 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.
+ */
+
+/* ICM-426xx accelerometer and gyroscope for Chrome EC */
+
+#ifndef __CROS_EC_ACCELGYRO_ICM426XX_H
+#define __CROS_EC_ACCELGYRO_ICM426XX_H
+
+#include "accelgyro.h"
+#include "common.h"
+
+/*
+ * 7-bit address is 110100Xb. Where 'X' is determined
+ * by the logic level on pin AP_AD0.
+ */
+#define ICM426XX_ADDR0_FLAGS		0x68
+#define ICM426XX_ADDR1_FLAGS		0x69
+
+/* Min and Max sampling frequency in mHz */
+#define ICM426XX_ACCEL_MIN_FREQ	3125
+#define ICM426XX_ACCEL_MAX_FREQ	MOTION_MAX_SENSOR_FREQUENCY(500000, 100000)
+#define ICM426XX_GYRO_MIN_FREQ	12500
+#define ICM426XX_GYRO_MAX_FREQ	MOTION_MAX_SENSOR_FREQUENCY(4000000, 100000)
+
+/* Min and Max Accel FS in G */
+#define ICM426XX_ACCEL_FS_MIN_VAL	2
+#define ICM426XX_ACCEL_FS_MAX_VAL	16
+
+/* Min and Max Gyro FS in dps */
+#define ICM426XX_GYRO_FS_MIN_VAL	125
+#define ICM426XX_GYRO_FS_MAX_VAL	2000
+
+/* Reg value from Accel FS in G */
+#define ICM426XX_ACCEL_FS_TO_REG(_fs)	((_fs) < 2 ? 3 : \
+					(_fs) > 16 ? 0 : \
+					3 - __fls((_fs) / 2))
+
+/* Accel FSR in G from Reg value */
+#define ICM426XX_ACCEL_REG_TO_FS(_reg)	((1 << (3 - (_reg))) * 2)
+
+/* Reg value from Gyro FS in dps */
+#define ICM426XX_GYRO_FS_TO_REG(_fs)	((_fs) < 125 ? 4 : \
+					(_fs) > 2000 ? 0 : \
+					4 - __fls((_fs) / 125))
+
+/* Gyro FSR in dps from Reg value */
+#define ICM426XX_GYRO_REG_TO_FS(_reg)	((1 << (4 - (_reg))) * 125)
+
+/* Reg value from ODR in mHz */
+#define ICM426XX_ODR_TO_REG(_odr)	((_odr) <= 200000 ? \
+					13 - __fls((_odr) / 3125) : \
+					(_odr) < 500000 ? 7 : \
+					(_odr) < 1000000 ? 15 : \
+					6 - __fls((_odr) / 1000000))
+
+/* ODR in mHz from Reg value */
+#define ICM426XX_REG_TO_ODR(_reg)	((_reg) == 15 ? 500000 : \
+					(_reg) >= 7 ? \
+					(1 << (13 - (_reg))) * 3125 : \
+					(1 << (6 - (_reg))) * 1000000)
+
+/* Reg value for the next higher ODR */
+#define ICM426XX_ODR_REG_UP(_reg)	((_reg) == 15 ? 6 : \
+					(_reg) == 7 ? 15 : \
+					(_reg) - 1)
+
+/*
+ * Register addresses are virtual address on 16 bits.
+ * MSB is coding register bank and LSB real register address.
+ * ex: bank 4, register 1F => 0x041F
+ */
+#define ICM426XX_REG_DEVICE_CONFIG	0x0011
+#define ICM426XX_SOFT_RESET_CONFIG	BIT(0)
+
+enum icm426xx_slew_rate {
+	ICM426XX_SLEW_RATE_20NS_60NS,
+	ICM426XX_SLEW_RATE_12NS_36NS,
+	ICM426XX_SLEW_RATE_6NS_18NS,
+	ICM426XX_SLEW_RATE_4NS_12NS,
+	ICM426XX_SLEW_RATE_2NS_6NS,
+	ICM426XX_SLEW_RATE_INF_2NS,
+};
+#define ICM426XX_REG_DRIVE_CONFIG	0x0013
+#define ICM426XX_DRIVE_CONFIG_MASK	GENMASK(5, 0)
+#define ICM426XX_I2C_SLEW_RATE(_s)	(((_s) & 0x07) << 3)
+#define ICM426XX_SPI_SLEW_RATE(_s)	((_s) & 0x07)
+
+/* default int configuration is pulsed mode, open drain, and active low */
+#define ICM426XX_REG_INT_CONFIG		0x0014
+#define ICM426XX_INT2_LATCHED		BIT(5)
+#define ICM426XX_INT2_PUSH_PULL		BIT(4)
+#define ICM426XX_INT2_ACTIVE_HIGH	BIT(3)
+#define ICM426XX_INT1_LATCHED		BIT(2)
+#define ICM426XX_INT1_PUSH_PULL		BIT(1)
+#define ICM426XX_INT1_ACTIVE_HIGH	BIT(0)
+
+#define ICM426XX_REG_FIFO_CONFIG	0x0016
+#define ICM426XX_FIFO_MODE_BYPASS	(0x00 << 6)
+#define ICM426XX_FIFO_MODE_STREAM	(0x01 << 6)
+#define ICM426XX_FIFO_MODE_STOP_FULL	(0x02 << 6)
+
+/* data are 16 bits */
+#define ICM426XX_REG_TEMP_DATA		0x001D
+/* X + Y + Z: 3 * 16 bits */
+#define ICM426XX_REG_ACCEL_DATA_XYZ	0x001F
+#define ICM426XX_REG_GYRO_DATA_XYZ	0x0025
+
+#define ICM426XX_INVALID_DATA		-32768
+
+#define ICM426XX_REG_INT_STATUS		0x002D
+#define ICM426XX_UI_FSYNC_INT		BIT(6)
+#define ICM426XX_PLL_RDY_INT		BIT(5)
+#define ICM426XX_RESET_DONE_INT		BIT(4)
+#define ICM426XX_DATA_RDY_INT		BIT(3)
+#define ICM426XX_FIFO_THS_INT		BIT(2)
+#define ICM426XX_FIFO_FULL_INT		BIT(1)
+#define ICM426XX_AGC_RDY_INT		BIT(0)
+
+/* FIFO count is 16 bits */
+#define ICM426XX_REG_FIFO_COUNT		0x002E
+#define ICM426XX_REG_FIFO_DATA		0x0030
+
+#define ICM426XX_REG_SIGNAL_PATH_RESET	0x004B
+#define ICM426XX_ABORT_AND_RESET	BIT(3)
+#define ICM426XX_TMST_STROBE		BIT(2)
+#define ICM426XX_FIFO_FLUSH		BIT(1)
+
+#define ICM426XX_REG_INTF_CONFIG0	0x004C
+#define ICM426XX_DATA_CONF_MASK		GENMASK(7, 4)
+#define ICM426XX_FIFO_HOLD_LAST_DATA	BIT(7)
+#define ICM426XX_FIFO_COUNT_REC		BIT(6)
+#define ICM426XX_FIFO_COUNT_BE		BIT(5)
+#define ICM426XX_SENSOR_DATA_BE		BIT(4)
+#define ICM426XX_UI_SIFS_CFG_MASK	GENMASK(1, 0)
+#define ICM426XX_UI_SIFS_CFG_SPI_DIS	0x02
+#define ICM426XX_UI_SIFS_CFG_I2C_DIS	0x03
+
+enum icm426xx_sensor_mode {
+	ICM426XX_MODE_OFF,
+	ICM426XX_MODE_STANDBY,
+	ICM426XX_MODE_LOW_POWER,
+	ICM426XX_MODE_LOW_NOISE,
+};
+#define ICM426XX_REG_PWR_MGMT0		0x004E
+#define ICM426XX_TEMP_DIS		BIT(5)
+#define ICM426XX_IDLE			BIT(4)
+#define ICM426XX_GYRO_MODE_MASK		GENMASK(3, 2)
+#define ICM426XX_GYRO_MODE(_m)		(((_m) & 0x03) << 2)
+#define ICM426XX_ACCEL_MODE_MASK	GENMASK(1, 0)
+#define ICM426XX_ACCEL_MODE(_m)		((_m) & 0x03)
+
+#define ICM426XX_REG_GYRO_CONFIG0	0x004F
+#define ICM426XX_REG_ACCEL_CONFIG0	0x0050
+#define ICM426XX_FS_MASK		GENMASK(7, 5)
+#define ICM426XX_FS_SEL(_fs)		(((_fs) & 0x07) << 5)
+#define ICM426XX_ODR_MASK		GENMASK(3, 0)
+#define ICM426XX_ODR(_odr)		((_odr) & 0x0F)
+
+enum icm426xx_filter_bw {
+	/* low noise mode */
+	ICM426XX_FILTER_BW_ODR_DIV_2 = 0,
+
+	/* low power mode */
+	ICM426XX_FILTER_BW_AVG_1X = 1,
+	ICM426XX_FILTER_BW_AVG_16X = 6,
+};
+
+#define ICM426XX_REG_GYRO_ACCEL_CONFIG0	0x0052
+#define ICM426XX_ACCEL_UI_FILT_MASK	GENMASK(7, 4)
+#define ICM426XX_ACCEL_UI_FILT_BW(_f)	(((_f) & 0x0F) << 4)
+#define ICM426XX_GYRO_UI_FILT_MASK	GENMASK(3, 0)
+#define ICM426XX_GYRO_UI_FILT_BW(_f)	((_f) & 0x0F)
+
+#define ICM426XX_REG_FIFO_CONFIG1	0x005F
+#define ICM426XX_FIFO_PARTIAL_READ	BIT(6)
+#define ICM426XX_FIFO_WM_GT_TH		BIT(5)
+#define ICM426XX_FIFO_EN_MASK		GENMASK(3, 0)
+#define ICM426XX_FIFO_TMST_FSYNC_EN	BIT(3)
+#define ICM426XX_FIFO_TEMP_EN		BIT(2)
+#define ICM426XX_FIFO_GYRO_EN		BIT(1)
+#define ICM426XX_FIFO_ACCEL_EN		BIT(0)
+
+/* FIFO watermark value is 16 bits little endian */
+#define ICM426XX_REG_FIFO_WATERMARK	0x0060
+
+#define ICM426XX_REG_INT_CONFIG1	0x0064
+#define ICM426XX_INT_PULSE_DURATION	BIT(6)
+#define ICM426XX_INT_TDEASSERT_DIS	BIT(5)
+#define ICM426XX_INT_ASYNC_RESET	BIT(4)
+
+#define ICM426XX_REG_INT_SOURCE0	0x0065
+#define ICM426XX_UI_FSYNC_INT1_EN	BIT(6)
+#define ICM426XX_PLL_RDY_INT1_EN	BIT(5)
+#define ICM426XX_RESET_DONE_INT1_EN	BIT(4)
+#define ICM426XX_UI_DRDY_INT1_EN	BIT(3)
+#define ICM426XX_FIFO_THS_INT1_EN	BIT(2)
+#define ICM426XX_FIFO_FULL_INT1_EN	BIT(1)
+#define ICM426XX_UI_AGC_RDY_INT1_EN	BIT(0)
+
+#define ICM426XX_REG_INT_SOURCE3	0x0068
+#define ICM426XX_UI_FSYNC_INT2_EN	BIT(6)
+#define ICM426XX_PLL_RDY_INT2_EN	BIT(5)
+#define ICM426XX_RESET_DONE_INT2_EN	BIT(4)
+#define ICM426XX_UI_DRDY_INT2_EN	BIT(3)
+#define ICM426XX_FIFO_THS_INT2_EN	BIT(2)
+#define ICM426XX_FIFO_FULL_INT2_EN	BIT(1)
+#define ICM426XX_UI_AGC_RDY_INT2_EN	BIT(0)
+
+#define ICM426XX_REG_WHO_AM_I		0x0075
+#define ICM426XX_CHIP_ICM40608		0x39
+#define ICM426XX_CHIP_ICM42605		0x42
+
+#define ICM426XX_REG_BANK_SEL		0x0076
+#define ICM426XX_BANK_SEL(_b)		((_b) & 0x07)
+
+#define ICM426XX_REG_INTF_CONFIG4	0x017A
+#define ICM426XX_I3C_BUS_MODE		BIT(6)
+#define ICM426XX_SPI_AP_4WIRE		BIT(1)
+
+#define ICM426XX_REG_INTF_CONFIG5	0x017B
+#define ICM426XX_PIN9_FUNC_INT2		(0x00 << 1)
+#define ICM426XX_PIN9_FUNC_FSYNC	(0x01 << 1)
+
+#define ICM426XX_REG_INTF_CONFIG6	0x017C
+#define ICM426XX_INTF_CONFIG6_MASK	GENMASK(4, 0)
+#define ICM426XX_I3C_EN			BIT(4)
+#define ICM426XX_I3C_IBI_BYTE_EN	BIT(3)
+#define ICM426XX_I3C_IBI_EN		BIT(2)
+#define ICM426XX_I3C_DDR_EN		BIT(1)
+#define ICM426XX_I3C_SDR_EN		BIT(0)
+
+#define ICM426XX_REG_INT_SOURCE8	0x044F
+#define ICM426XX_FSYNC_IBI_EN		BIT(5)
+#define ICM426XX_PLL_RDY_IBI_EN		BIT(4)
+#define ICM426XX_UI_DRDY_IBI_EN		BIT(3)
+#define ICM426XX_FIFO_THS_IBI_EN	BIT(2)
+#define ICM426XX_FIFO_FULL_IBI_EN	BIT(1)
+#define ICM426XX_AGC_RDY_IBI_EN		BIT(0)
+
+#define ICM426XX_REG_OFFSET_USER0	0x0477
+#define ICM426XX_REG_OFFSET_USER1	0x0478
+#define ICM426XX_REG_OFFSET_USER2	0x0479
+#define ICM426XX_REG_OFFSET_USER3	0x047A
+#define ICM426XX_REG_OFFSET_USER4	0x047B
+#define ICM426XX_REG_OFFSET_USER5	0x047C
+#define ICM426XX_REG_OFFSET_USER6	0x047D
+#define ICM426XX_REG_OFFSET_USER7	0x047E
+#define ICM426XX_REG_OFFSET_USER8	0x047F
+
+extern const struct accelgyro_drv icm426xx_drv;
+
+void icm426xx_interrupt(enum gpio_signal signal);
+
+#endif /* __CROS_EC_ACCELGYRO_ICM426XX_H */
diff --git a/driver/accelgyro_icm_common.c b/driver/accelgyro_icm_common.c
new file mode 100644
index 0000000000..dc3abd71f2
--- /dev/null
+++ b/driver/accelgyro_icm_common.c
@@ -0,0 +1,296 @@
+/* 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.
+ */
+
+/**
+ * ICM accelerometer and gyroscope module for Chrome EC
+ * 3D digital accelerometer & 3D digital gyroscope
+ */
+
+#include "accelgyro.h"
+#include "console.h"
+#include "i2c.h"
+#include "spi.h"
+#include "driver/accelgyro_icm_common.h"
+#include "driver/accelgyro_icm426xx.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)
+
+#ifdef CONFIG_SPI_ACCEL_PORT
+static int icm_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);
+}
+
+static int icm_spi_raw_write(const int addr, const uint8_t reg,
+			     const uint8_t *data, const int len)
+{
+	uint8_t cmd[3];
+	int i;
+
+	if (len > 2)
+		return EC_ERROR_UNIMPLEMENTED;
+
+	cmd[0] = reg;
+	for (i = 0; i < len; ++i)
+		cmd[i + 1] = data[i];
+
+	return spi_transaction(&spi_devices[addr], cmd, len + 1, NULL, 0);
+}
+#endif
+
+static int icm_bank_sel(const struct motion_sensor_t *s, const int reg)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	uint8_t bank = ICM426XX_REG_GET_BANK(reg);
+	int ret;
+
+	if (bank == st->bank)
+		return EC_SUCCESS;
+
+	ret = EC_ERROR_UNIMPLEMENTED;
+	ret = i2c_write8(s->port, s->i2c_spi_addr_flags,
+			 ICM426XX_REG_BANK_SEL, bank);
+
+	if (ret == EC_SUCCESS)
+		st->bank = bank;
+
+	return ret;
+}
+
+/**
+ * Read 8 bits register
+ */
+int icm_read8(const struct motion_sensor_t *s, const int reg, int *data_ptr)
+{
+	const uint8_t addr = ICM426XX_REG_GET_ADDR(reg);
+	int ret;
+
+	ret = icm_bank_sel(s, reg);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = EC_ERROR_UNIMPLEMENTED;
+	ret = i2c_read8(s->port, s->i2c_spi_addr_flags, addr, data_ptr);
+
+	return ret;
+}
+
+/**
+ * Write 8 bits register
+ */
+int icm_write8(const struct motion_sensor_t *s, const int reg, int data)
+{
+	const uint8_t addr = ICM426XX_REG_GET_ADDR(reg);
+	int ret;
+
+	ret = icm_bank_sel(s, reg);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = EC_ERROR_UNIMPLEMENTED;
+	ret = i2c_write8(s->port, s->i2c_spi_addr_flags, addr, data);
+
+	return ret;
+}
+
+/**
+ * Read 16 bits register
+ */
+int icm_read16(const struct motion_sensor_t *s, const int reg, int *data_ptr)
+{
+	const uint8_t addr = ICM426XX_REG_GET_ADDR(reg);
+	int ret;
+
+	ret = icm_bank_sel(s, reg);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = EC_ERROR_UNIMPLEMENTED;
+	ret = i2c_read16(s->port, s->i2c_spi_addr_flags,
+				addr, data_ptr);
+
+	return ret;
+}
+
+/**
+ * Write 16 bits register
+ */
+int icm_write16(const struct motion_sensor_t *s, const int reg, int data)
+{
+	const uint8_t addr = ICM426XX_REG_GET_ADDR(reg);
+	int ret;
+
+	ret = icm_bank_sel(s, reg);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = EC_ERROR_UNIMPLEMENTED;
+	ret = i2c_write16(s->port, s->i2c_spi_addr_flags, addr, data);
+
+	return ret;
+}
+
+/**
+ * Read n bytes
+ */
+int icm_read_n(const struct motion_sensor_t *s, const int reg,
+	       uint8_t *data_ptr, const int len)
+{
+	const uint8_t addr = ICM426XX_REG_GET_ADDR(reg);
+	int ret;
+
+	ret = icm_bank_sel(s, reg);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = EC_ERROR_UNIMPLEMENTED;
+	ret = i2c_read_block(s->port, s->i2c_spi_addr_flags, addr,
+			     data_ptr, len);
+
+	return ret;
+}
+
+int icm_field_update8(const struct motion_sensor_t *s, const int reg,
+		      const uint8_t field_mask, const uint8_t set_value)
+{
+	const uint8_t addr = ICM426XX_REG_GET_ADDR(reg);
+	int ret;
+
+	ret = icm_bank_sel(s, reg);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = EC_ERROR_UNIMPLEMENTED;
+	ret = i2c_field_update8(s->port, s->i2c_spi_addr_flags, addr,
+				field_mask, set_value);
+
+	return ret;
+}
+
+int icm_get_resolution(const struct motion_sensor_t *s)
+{
+	return ICM_RESOLUTION;
+}
+
+int icm_get_range(const struct motion_sensor_t *s)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+
+	return data->range;
+}
+
+int icm_get_data_rate(const struct motion_sensor_t *s)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+
+	return data->odr;
+}
+
+int icm_set_scale(const struct motion_sensor_t *s, const uint16_t *scale,
+		  int16_t temp)
+{
+	struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s);
+
+	data->scale[X] = scale[X];
+	data->scale[Y] = scale[Y];
+	data->scale[Z] = scale[Z];
+	return EC_SUCCESS;
+}
+
+int icm_get_scale(const struct motion_sensor_t *s, uint16_t *scale,
+		  int16_t *temp)
+{
+	struct accelgyro_saved_data_t *data = ICM_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;
+}
+
+/* FIFO header: 1 byte */
+#define ICM_FIFO_HEADER_MSG		BIT(7)
+#define ICM_FIFO_HEADER_ACCEL		BIT(6)
+#define ICM_FIFO_HEADER_GYRO		BIT(5)
+#define ICM_FIFO_HEADER_TMST_FSYNC	GENMASK(3, 2)
+#define ICM_FIFO_HEADER_ODR_ACCEL	BIT(1)
+#define ICM_FIFO_HEADER_ODR_GYRO	BIT(0)
+
+/* FIFO data packet */
+struct icm_fifo_sensor_data {
+	int16_t x;
+	int16_t y;
+	int16_t z;
+} __packed;
+
+struct icm_fifo_1sensor_packet {
+	uint8_t header;
+	struct icm_fifo_sensor_data data;
+	int8_t temp;
+} __packed;
+#define ICM_FIFO_1SENSOR_PACKET_SIZE	8
+
+struct icm_fifo_2sensors_packet {
+	uint8_t header;
+	struct icm_fifo_sensor_data accel;
+	struct icm_fifo_sensor_data gyro;
+	int8_t temp;
+	uint16_t timestamp;
+} __packed;
+#define ICM_FIFO_2SENSORS_PACKET_SIZE	16
+
+ssize_t icm_fifo_decode_packet(const void *packet, const uint8_t **accel,
+		const uint8_t **gyro)
+{
+	const struct icm_fifo_1sensor_packet *pack1 = packet;
+	const struct icm_fifo_2sensors_packet *pack2 = packet;
+	uint8_t header = *((const uint8_t *)packet);
+
+	/* FIFO empty */
+	if (header & ICM_FIFO_HEADER_MSG) {
+		if (accel != NULL)
+			*accel = NULL;
+		if (gyro != NULL)
+			*gyro = NULL;
+		return 0;
+	}
+
+	/* accel + gyro */
+	if ((header & ICM_FIFO_HEADER_ACCEL) &&
+	    (header & ICM_FIFO_HEADER_GYRO)) {
+		if (accel != NULL)
+			*accel = (uint8_t *)&pack2->accel;
+		if (gyro != NULL)
+			*gyro = (uint8_t *)&pack2->gyro;
+		return ICM_FIFO_2SENSORS_PACKET_SIZE;
+	}
+
+	/* accel only */
+	if (header & ICM_FIFO_HEADER_ACCEL) {
+		if (accel != NULL)
+			*accel = (uint8_t *)&pack1->data;
+		if (gyro != NULL)
+			*gyro = NULL;
+		return ICM_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* gyro only */
+	if (header & ICM_FIFO_HEADER_GYRO) {
+		if (accel != NULL)
+			*accel = NULL;
+		if (gyro != NULL)
+			*gyro = (uint8_t *)&pack1->data;
+		return ICM_FIFO_1SENSOR_PACKET_SIZE;
+	}
+
+	/* invalid packet if here */
+	return -EC_ERROR_INVAL;
+}
diff --git a/driver/accelgyro_icm_common.h b/driver/accelgyro_icm_common.h
new file mode 100644
index 0000000000..4d991ce341
--- /dev/null
+++ b/driver/accelgyro_icm_common.h
@@ -0,0 +1,101 @@
+/* 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.
+ */
+
+/* ICM accelerometer and gyroscope common definitions for Chrome EC */
+
+#ifndef __CROS_EC_ACCELGYRO_ICM_COMMON_H
+#define __CROS_EC_ACCELGYRO_ICM_COMMON_H
+
+#include "accelgyro.h"
+
+#ifdef CONFIG_ACCEL_FIFO
+/* reserve maximum 4 samples of 16 bytes */
+#define ICM_FIFO_BUFFER	64
+#else
+#define ICM_FIFO_BUFFER	0
+#endif
+
+struct icm_drv_data_t {
+	struct accelgyro_saved_data_t saved_data[2];
+	struct motion_sensor_t *accel;
+	struct motion_sensor_t *gyro;
+	uint8_t bank;
+	uint8_t fifo_en;
+	uint8_t fifo_buffer[ICM_FIFO_BUFFER] __aligned(sizeof(long));
+};
+
+#define ICM_GET_DATA(_s) \
+	((struct icm_drv_data_t *)(_s)->drv_data)
+#define ICM_GET_SAVED_DATA(_s) \
+	(&ICM_GET_DATA(_s)->saved_data[(_s)->type])
+
+/*
+ * Virtual register address is 16 bits:
+ * - 8 bits MSB coding bank number
+ * - 8 bits LSB coding physical address
+ */
+#define ICM426XX_REG_GET_BANK(_r)	(((_r) & 0xFF00) >> 8)
+#define ICM426XX_REG_GET_ADDR(_r)	((_r) & 0x00FF)
+
+/* Sensor resolution in number of bits */
+#define ICM_RESOLUTION		16
+
+/**
+ * sign_extend - sign extend a standard int value using the given sign-bit
+ * @value: value to sign extend
+ * @index: 0 based bit index to sign bit
+ */
+static inline int sign_extend(int value, int index)
+{
+	int shift = (sizeof(int) * 8) - index - 1;
+
+	return (int)(value << shift) >> shift;
+}
+
+/**
+ * Read 8 bits register
+ */
+int icm_read8(const struct motion_sensor_t *s, const int reg, int *data_ptr);
+
+/**
+ * Write 8 bits register
+ */
+int icm_write8(const struct motion_sensor_t *s, const int reg, int data);
+
+/**
+ * Read 16 bits register
+ */
+int icm_read16(const struct motion_sensor_t *s, const int reg, int *data_ptr);
+
+/**
+ * Write 16 bits register
+ */
+int icm_write16(const struct motion_sensor_t *s, const int reg, int data);
+
+/**
+ * Read n bytes
+ */
+int icm_read_n(const struct motion_sensor_t *s, const int reg,
+	       uint8_t *data_ptr, const int len);
+
+int icm_field_update8(const struct motion_sensor_t *s, const int reg,
+		      const uint8_t field_mask, const uint8_t set_value);
+
+int icm_get_resolution(const struct motion_sensor_t *s);
+
+int icm_get_range(const struct motion_sensor_t *s);
+
+int icm_get_data_rate(const struct motion_sensor_t *s);
+
+int icm_set_scale(const struct motion_sensor_t *s, const uint16_t *scale,
+		  int16_t temp);
+
+int icm_get_scale(const struct motion_sensor_t *s, uint16_t *scale,
+		  int16_t *temp);
+
+ssize_t icm_fifo_decode_packet(const void *packet, const uint8_t **accel,
+		const uint8_t **gyro);
+
+#endif	/* __CROS_EC_ACCELGYRO_ICM_COMMON_H */
diff --git a/driver/build.mk b/driver/build.mk
index b33da44eb5..e1e39c7a5b 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -20,6 +20,7 @@ driver-$(CONFIG_MAG_LIS2MDL)+=mag_lis2mdl.o
 driver-$(CONFIG_SENSORHUB_LSM6DSM)+=sensorhub_lsm6dsm.o
 driver-$(CONFIG_SYNC)+=sync.o
 driver-$(CONFIG_ACCEL_LIS2DW_COMMON)+=accel_lis2dw12.o stm_mems_common.o
+driver-$(CONFIG_ACCELGYRO_ICM426XX)+=accelgyro_icm426xx.o accelgyro_icm_common.o
 
 # BC1.2 Charger Detection Devices
 driver-$(CONFIG_BC12_DETECT_MAX14637)+=bc12/max14637.o
diff --git a/include/accelgyro.h b/include/accelgyro.h
index 2d415621d1..71127e46bd 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -152,6 +152,9 @@ struct accelgyro_saved_data_t {
 	uint16_t scale[3];
 };
 
+#define SENSOR_APPLY_DIV_SCALE(_input, _scale) \
+	(((_input) * (uint64_t)MOTION_SENSE_DEFAULT_SCALE) / (_scale))
+
 #define SENSOR_APPLY_SCALE(_input, _scale) \
 	(((_input) * (_scale)) / MOTION_SENSE_DEFAULT_SCALE)
 
diff --git a/include/common.h b/include/common.h
index 13101fd1f4..db08216e05 100644
--- a/include/common.h
+++ b/include/common.h
@@ -124,6 +124,11 @@
 #endif
 #endif
 
+/* Macros for combining bytes into uint16s. */
+#define UINT16_FROM_BYTES(lsb, msb) ((lsb) | (msb) << 8)
+#define UINT16_FROM_BYTE_ARRAY_LE(data, lsb_index) \
+	UINT16_FROM_BYTES((data)[(lsb_index)], (data)[(lsb_index + 1)])
+
 /* There isn't really a better place for this */
 #define C_TO_K(temp_c) ((temp_c) + 273)
 #define K_TO_C(temp_c) ((temp_c) - 273)
diff --git a/include/config.h b/include/config.h
index 3d7f3dad4e..e6a72b0d7f 100644
--- a/include/config.h
+++ b/include/config.h
@@ -99,6 +99,7 @@
 #undef CONFIG_ACCEL_LIS2DW_AS_BASE
 
 #undef CONFIG_ACCELGYRO_BMI160
+#undef CONFIG_ACCELGYRO_ICM426XX
 #undef CONFIG_ACCELGYRO_LSM6DS0
 /* Use CONFIG_ACCELGYRO_LSM6DSM for LSM6DSL, LSM6DSM, and/or LSM6DS3 */
 #undef CONFIG_ACCELGYRO_LSM6DSM
@@ -270,6 +271,7 @@
  * Must be within TASK_EVENT_MOTION_INTERRUPT_MASK.
  */
 #undef CONFIG_ACCELGYRO_BMI160_INT_EVENT
+#undef CONFIG_ACCELGYRO_ICM426XX_INT_EVENT
 #undef CONFIG_ACCEL_LSM6DSM_INT_EVENT
 #undef CONFIG_ACCEL_LSM6DSO_INT_EVENT
 #undef CONFIG_ACCEL_LIS2DW12_INT_EVENT