driver: add ICM-42607 driver support

Add ICM-42607 accel/gyro driver code.

BUG=chromium:1198171
BRANCH=None
TEST=ectool motionsense && CROS-EC IIO drivers

Signed-off-by: JuHyun Kim <jkim@invensense.com>

Change-Id: If2cff2bd20ac69ca40bc56af50dcabbd4f5910d6
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2822268
Reviewed-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Commit-Queue: Gwendal Grignou <gwendal@chromium.org>
Tested-by: Gwendal Grignou <gwendal@chromium.org>

6 files changed, 1420 insertions, 0 deletions

diff --git a/common/build.mk b/common/build.mk
index 1e09abfce0..3623898c67 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -15,6 +15,7 @@ common-y+=version.o printf.o queue.o queue_policies.o irq_locking.o
 common-$(CONFIG_ACCELGYRO_BMI160)+=math_util.o
 common-$(CONFIG_ACCELGYRO_BMI260)+=math_util.o
 common-$(CONFIG_ACCELGYRO_ICM426XX)+=math_util.o
+common-$(CONFIG_ACCELGYRO_ICM42607)+=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_icm42607.c b/driver/accelgyro_icm42607.c
new file mode 100644
index 0000000000..671b488cc0
--- /dev/null
+++ b/driver/accelgyro_icm42607.c
@@ -0,0 +1,1134 @@
+/* Copyright 2021 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-42607 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_icm42607.h"
+#include "hwtimer.h"
+#include "i2c.h"
+#include "math_util.h"
+#include "motion_sense.h"
+#include "motion_sense_fifo.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)
+
+STATIC_IF(CONFIG_ACCEL_FIFO) volatile uint32_t last_interrupt_timestamp;
+
+static int icm_switch_on_mclk(const struct motion_sensor_t *s)
+{
+	int val;
+	int i, ret;
+
+	ret = icm_field_update8(s, ICM42607_REG_PWR_MGMT0, ICM42607_IDLE,
+				ICM42607_IDLE);
+	if (ret)
+		return ret;
+
+	/* Check if MCLK is ready */
+	for (i = 0; i < 10; ++i) {
+		ret = icm_read8(s, ICM42607_REG_MCLK_RDY, &val);
+		if (ret)
+			return ret;
+
+		if (val & ICM42607_MCLK_RDY)
+			return EC_SUCCESS;
+	}
+
+	return EC_ERROR_HW_INTERNAL;
+}
+
+static int icm_switch_off_mclk(const struct motion_sensor_t *s)
+{
+	return icm_field_update8(s, ICM42607_REG_PWR_MGMT0, ICM42607_IDLE, 0);
+}
+
+static int icm_read_mclk_reg(const struct motion_sensor_t *s, const int reg,
+			     int *buf)
+{
+	const int blk_sel = (reg & 0xFF00) >> 8;
+	const int val = reg & 0x00FF;
+	int ret;
+
+	/* optimize by changing BLK_SEL only if not 0 */
+	if (blk_sel) {
+		ret = icm_write8(s, ICM42607_REG_BLK_SEL_R, blk_sel);
+		if (ret)
+			return ret;
+	}
+
+	ret = icm_write8(s, ICM42607_REG_MADDR_R, val);
+	if (ret)
+		return ret;
+
+	udelay(10);
+
+	ret = icm_read8(s, ICM42607_REG_M_R, buf);
+	if (ret)
+		return ret;
+
+	udelay(10);
+
+	if (blk_sel) {
+		ret = icm_write8(s, ICM42607_REG_BLK_SEL_R, 0);
+		if (ret)
+			return ret;
+	}
+
+	return EC_SUCCESS;
+}
+
+static int icm_write_mclk_reg(const struct motion_sensor_t *s, const int reg,
+			      const int buf)
+{
+	const int blk_sel = (reg & 0xFF00) >> 8;
+	const int val = reg & 0x00FF;
+	int ret;
+
+	/* optimize by changing BLK_SEL only if not 0 */
+	if (blk_sel) {
+		ret = icm_write8(s, ICM42607_REG_BLK_SEL_W, blk_sel);
+		if (ret)
+			return ret;
+	}
+
+	ret = icm_write8(s, ICM42607_REG_MADDR_W, val);
+	if (ret)
+		return ret;
+
+	ret = icm_write8(s, ICM42607_REG_M_W, buf);
+	if (ret)
+		return ret;
+
+	udelay(10);
+
+	if (blk_sel) {
+		ret = icm_write8(s, ICM42607_REG_BLK_SEL_W, 0);
+		if (ret)
+			return ret;
+	}
+
+	return EC_SUCCESS;
+}
+
+static int icm_field_update_mclk_reg(const struct motion_sensor_t *s,
+				     const int reg, const uint8_t field_mask,
+				     const uint8_t set_value)
+{
+	int val, ret;
+
+	ret = icm_read_mclk_reg(s, reg, &val);
+	if (ret)
+		return ret;
+
+	val = (val & ~field_mask) | set_value;
+
+	return icm_write_mclk_reg(s, reg, val);
+}
+
+static int icm42607_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] == ICM42607_INVALID_DATA &&
+	    v[Y] == ICM42607_INVALID_DATA &&
+	    v[Z] == ICM42607_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;
+}
+
+static int icm42607_check_sensor_stabilized(const struct motion_sensor_t *s,
+					    uint32_t ts)
+{
+	int32_t rem;
+
+	rem = icm_get_sensor_stabilized(s, ts);
+	if (rem == 0)
+		return EC_SUCCESS;
+	if (rem > 0)
+		return EC_ERROR_BUSY;
+
+	/* rem < 0: reset check since ts has passed stabilize_ts */
+	icm_reset_stabilize_ts(s);
+	return EC_SUCCESS;
+}
+
+static int __maybe_unused icm42607_flush_fifo(const struct motion_sensor_t *s)
+{
+	int i, val, ret;
+
+	ret = icm_write8(s, ICM42607_REG_SIGNAL_PATH_RESET,
+			 ICM42607_FIFO_FLUSH);
+	if (ret)
+		return ret;
+
+	udelay(10);
+
+	for (i = 0; i < 10; ++i) {
+		ret = icm_read8(s, ICM42607_REG_SIGNAL_PATH_RESET, &val);
+		if (ret)
+			return ret;
+
+		if (!(val & ICM42607_FIFO_FLUSH))
+			return EC_SUCCESS;
+
+		udelay(1);
+	}
+
+	return EC_ERROR_HW_INTERNAL;
+}
+
+/* use FIFO threshold interrupt on INT1 */
+#define ICM42607_FIFO_INT_EN		ICM42607_FIFO_THS_INT1_EN
+#define ICM42607_FIFO_INT_STATUS	ICM42607_FIFO_THS_INT
+
+static int __maybe_unused icm42607_enable_fifo(const struct motion_sensor_t *s,
+					       int enable)
+{
+	int ret;
+
+	if (enable) {
+		/* enable FIFO interrupts */
+		ret = icm_field_update8(s, ICM42607_REG_INT_SOURCE0,
+					ICM42607_FIFO_INT_EN,
+					ICM42607_FIFO_INT_EN);
+		if (ret)
+			return ret;
+
+		/* enable FIFO in streaming mode */
+		ret = icm_write8(s, ICM42607_REG_FIFO_CONFIG1,
+				 ICM42607_FIFO_MODE_STREAM);
+		if (ret)
+			return ret;
+	} else {
+		/* disable FIFO interrupts */
+		ret = icm_field_update8(s, ICM42607_REG_INT_SOURCE0,
+					ICM42607_FIFO_INT_EN, 0);
+		if (ret)
+			return ret;
+
+		/* set FIFO in bypass mode */
+		ret = icm_write8(s, ICM42607_REG_FIFO_CONFIG1,
+				 ICM42607_FIFO_BYPASS);
+		if (ret)
+			return ret;
+
+		/* flush FIFO data */
+		ret = icm42607_flush_fifo(s);
+		if (ret)
+			return ret;
+	}
+
+	return EC_SUCCESS;
+}
+
+static int __maybe_unused icm42607_config_fifo(const struct motion_sensor_t *s,
+					       int enable)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	int val;
+	uint8_t old_fifo_en, fifo_en;
+	int ret;
+
+	mutex_lock(s->mutex);
+
+	/* compute new FIFO enable bits and update FIFO config */
+	fifo_en = st->fifo_en;
+	if (enable)
+		fifo_en |= BIT(s->type);
+	else
+		fifo_en &= ~BIT(s->type);
+
+	val = ICM42607_FIFO_WM_GT_TH;
+	if (fifo_en & BIT(MOTIONSENSE_TYPE_ACCEL))
+		val |= ICM42607_FIFO_ACCEL_EN;
+	if (fifo_en & BIT(MOTIONSENSE_TYPE_GYRO))
+		val |= ICM42607_FIFO_GYRO_EN;
+
+	ret = icm_switch_on_mclk(s);
+	if (ret)
+		goto out_unlock;
+
+	ret = icm_write_mclk_reg(s, ICM42607_MREG_FIFO_CONFIG5, val);
+	if (ret)
+		goto out_unlock;
+
+	ret = icm_switch_off_mclk(s);
+	if (ret)
+		goto out_unlock;
+
+	old_fifo_en = st->fifo_en;
+	st->fifo_en = fifo_en;
+
+	if (!old_fifo_en && st->fifo_en) {
+		/* 1st sensor enabled => turn FIFO on */
+		ret = icm42607_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 = icm42607_enable_fifo(s, 0);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+	}
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static void __maybe_unused icm42607_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 = icm42607_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 icm42607_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, ICM42607_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");
+		/* flush FIFO data */
+		ret = icm42607_flush_fifo(s);
+		if (ret)
+			return ret;
+
+		return EC_ERROR_OVERFLOW;
+	}
+
+	ret = icm_read_n(s, ICM42607_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) {
+			ret = icm42607_check_sensor_stabilized(s, ts);
+			if (ret == EC_SUCCESS)
+				icm42607_push_fifo_data(s, accel, ts);
+		}
+		if (gyro != NULL) {
+			ret = icm42607_check_sensor_stabilized(s + 1, ts);
+			if (ret == EC_SUCCESS)
+				icm42607_push_fifo_data(s + 1, gyro, ts);
+		}
+	}
+
+	return EC_SUCCESS;
+}
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+
+/**
+ * icm42607_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", ->icm42607_irq_handler().
+ */
+void icm42607_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_ICM42607_INT_EVENT, 0);
+}
+
+/**
+ * icm42607_irq_handler - bottom half of the interrupt stack.
+ * Ran from the motion_sense task, finds the events that raised the interrupt.
+ */
+static int icm42607_irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	int status;
+	int ret;
+
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+	    (!(*event & CONFIG_ACCELGYRO_ICM42607_INT_EVENT)))
+		return EC_ERROR_NOT_HANDLED;
+
+	mutex_lock(s->mutex);
+
+	/* read and clear interrupt status */
+	ret = icm_read8(s, ICM42607_REG_INT_STATUS, &status);
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
+		if (status & ICM42607_FIFO_INT_STATUS) {
+			ret = icm42607_load_fifo(s, last_interrupt_timestamp);
+			if (ret == EC_SUCCESS)
+				motion_sense_fifo_commit_data();
+		}
+	}
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int icm42607_config_interrupt(const struct motion_sensor_t *s)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	int val, mask, ret;
+
+	/* configure INT1 pin: push-pull active low */
+	ret = icm_write8(s, ICM42607_REG_INT_CONFIG, ICM42607_INT1_PUSH_PULL);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
+		/* configure FIFO in little endian */
+		mask = ICM42607_FIFO_COUNT_ENDIAN | ICM42607_SENSOR_DATA_ENDIAN;
+		ret = icm_field_update8(s, ICM42607_REG_INTF_CONFIG0, mask, 0);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		ret = icm_switch_on_mclk(s);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		/*
+		 * configure FIFO:
+		 * - enable continuous watermark interrupt
+		 * - disable all FIFO en bits
+		 */
+		val = ICM42607_FIFO_WM_GT_TH;
+		ret = icm_write_mclk_reg(s, ICM42607_MREG_FIFO_CONFIG5, val);
+		if (ret != EC_SUCCESS)
+			return ret;
+
+		ret = icm_switch_off_mclk(s);
+		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, ICM42607_REG_FIFO_WM, 8);
+		if (ret != EC_SUCCESS)
+			return ret;
+	}
+
+	return EC_SUCCESS;
+}
+
+#endif	/* CONFIG_ACCEL_INTERRUPTS */
+
+static int icm42607_enable_sensor(const struct motion_sensor_t *s, int enable)
+{
+	uint32_t delay, stop_delay;
+	int32_t rem;
+	uint8_t mask;
+	int val;
+	int ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		mask = ICM42607_ACCEL_MODE_MASK;
+		if (enable) {
+			delay = ICM42607_ACCEL_START_TIME;
+			stop_delay = ICM42607_ACCEL_STOP_TIME;
+			val = ICM42607_ACCEL_MODE(ICM42607_MODE_LOW_POWER);
+		} else {
+			delay = ICM42607_ACCEL_STOP_TIME;
+			val = ICM42607_ACCEL_MODE(ICM42607_MODE_OFF);
+		}
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		mask = ICM42607_GYRO_MODE_MASK;
+		if (enable) {
+			delay = ICM42607_GYRO_START_TIME;
+			stop_delay = ICM42607_GYRO_STOP_TIME;
+			val = ICM42607_GYRO_MODE(ICM42607_MODE_LOW_NOISE);
+		} else {
+			delay = ICM42607_GYRO_STOP_TIME;
+			val = ICM42607_GYRO_MODE(ICM42607_MODE_OFF);
+		}
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+
+	/* check stop delay and sleep if required */
+	if (enable) {
+		rem = icm_get_sensor_stabilized(s, __hw_clock_source_read());
+		/* rem > stop_delay means counter rollover */
+		if (rem > 0 && rem <= stop_delay)
+			usleep(rem);
+	}
+
+	mutex_lock(s->mutex);
+
+	ret = icm_field_update8(s, ICM42607_REG_PWR_MGMT0, mask, val);
+	if (ret == EC_SUCCESS) {
+		icm_set_stabilize_ts(s, delay);
+		/* when turning sensor on block any register write for 200 us */
+		if (enable)
+			usleep(200);
+	}
+
+	mutex_unlock(s->mutex);
+
+	return ret;
+}
+
+static int icm42607_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, reg2, ret, reg_val, reg2_val;
+	int normalized_rate;
+	int max_rate, min_rate;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		reg = ICM42607_REG_ACCEL_CONFIG0;
+		reg2 = ICM42607_REG_ACCEL_CONFIG1;
+		min_rate = ICM42607_ACCEL_MIN_FREQ;
+		max_rate = ICM42607_ACCEL_MAX_FREQ;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		reg = ICM42607_REG_GYRO_CONFIG0;
+		reg2 = ICM42607_REG_GYRO_CONFIG1;
+		min_rate = ICM42607_GYRO_MIN_FREQ;
+		max_rate = ICM42607_GYRO_MAX_FREQ;
+		break;
+	default:
+		return EC_RES_INVALID_PARAM;
+	}
+
+	if (rate == 0) {
+		/* disable data in FIFO */
+		if (IS_ENABLED(CONFIG_ACCEL_FIFO))
+			icm42607_config_fifo(s, 0);
+		/* disable sensor */
+		ret = icm42607_enable_sensor(s, 0);
+		data->odr = 0;
+		return ret;
+	}
+
+	/* normalize the rate */
+	reg_val = ICM42607_ODR_TO_REG(rate);
+	normalized_rate = ICM42607_REG_TO_ODR(reg_val);
+
+	/* round up the rate */
+	if (rnd && (normalized_rate < rate)) {
+		reg_val = ICM42607_ODR_REG_UP(reg_val);
+		normalized_rate = ICM42607_REG_TO_ODR(reg_val);
+	}
+
+	if (rate > 0) {
+		if ((normalized_rate < min_rate) ||
+				(normalized_rate > max_rate))
+			return EC_RES_INVALID_PARAM;
+	}
+
+	reg2_val = ICM42607_ODR_TO_FILT_BW(reg_val);
+
+	mutex_lock(s->mutex);
+
+	/* update filter bandwidth */
+	ret = icm_field_update8(s, reg2, ICM42607_UI_FILT_BW_MASK,
+				ICM42607_UI_FILT_BW_SET(reg2_val));
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	/* update ODR */
+	ret = icm_field_update8(s, reg, ICM42607_ODR_MASK,
+				ICM42607_ODR(reg_val));
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	mutex_unlock(s->mutex);
+
+	if (data->odr == 0) {
+		/* enable sensor */
+		ret = icm42607_enable_sensor(s, 1);
+		if (ret)
+			return ret;
+		/* enable data in FIFO */
+		if (IS_ENABLED(CONFIG_ACCEL_FIFO))
+			icm42607_config_fifo(s, 1);
+	}
+
+	data->odr = normalized_rate;
+
+	return EC_SUCCESS;
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int icm42607_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 = ICM42607_REG_ACCEL_CONFIG0;
+		reg_val = ICM42607_ACCEL_FS_TO_REG(range);
+		newrange = ICM42607_ACCEL_REG_TO_FS(reg_val);
+
+		if (rnd && (newrange < range) && (reg_val > 0)) {
+			reg_val--;
+			newrange = ICM42607_ACCEL_REG_TO_FS(reg_val);
+		}
+
+		if (newrange > ICM42607_ACCEL_FS_MAX_VAL) {
+			newrange = ICM42607_ACCEL_FS_MAX_VAL;
+			reg_val = ICM42607_ACCEL_FS_TO_REG(range);
+		}
+
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		reg = ICM42607_REG_GYRO_CONFIG0;
+		reg_val = ICM42607_GYRO_FS_TO_REG(range);
+		newrange = ICM42607_GYRO_REG_TO_FS(reg_val);
+
+		if (rnd && (newrange < range) && (reg_val > 0)) {
+			reg_val--;
+			newrange = ICM42607_GYRO_REG_TO_FS(reg_val);
+		}
+
+		if (newrange > ICM42607_GYRO_FS_MAX_VAL) {
+			newrange = ICM42607_GYRO_FS_MAX_VAL;
+			reg_val = ICM42607_GYRO_FS_TO_REG(newrange);
+		}
+
+		break;
+	default:
+		return EC_RES_INVALID_PARAM;
+	}
+
+	mutex_lock(s->mutex);
+
+	ret = icm_field_update8(s, reg, ICM42607_FS_MASK,
+				ICM42607_FS_SEL(reg_val));
+	if (ret == EC_SUCCESS)
+		data->range = newrange;
+
+	mutex_unlock(s->mutex);
+
+	return ret;
+}
+
+static int icm42607_get_hw_offset(const struct motion_sensor_t *s,
+				  intv3_t offset)
+{
+	uint8_t raw[5];
+	int i, reg, val, ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		reg = ICM42607_MREG_OFFSET_USER4;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		reg = ICM42607_MREG_OFFSET_USER0;
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+
+	mutex_lock(s->mutex);
+
+	ret = icm_switch_on_mclk(s);
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	for (i = 0; i < sizeof(raw); ++i) {
+		ret = icm_read_mclk_reg(s, reg + i, &val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+		raw[i] = val;
+	}
+
+	ret = icm_switch_off_mclk(s);
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	mutex_unlock(s->mutex);
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/*
+		 * raw[0]: Accel X[11:8] | gyro Z[11:8]
+		 * raw[1]: Accel X[7:0]
+		 * 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:
+		/*
+		 * 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;
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int icm42607_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);
+
+	ret = icm_switch_on_mclk(s);
+	if (ret != EC_SUCCESS)
+		goto out_unlock;
+
+	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_update_mclk_reg(s, ICM42607_MREG_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_write_mclk_reg(s, ICM42607_MREG_OFFSET_USER5, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Accel Y[7:0] */
+		val = offset[Y] & GENMASK(7, 0);
+		ret = icm_write_mclk_reg(s, ICM42607_MREG_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_write_mclk_reg(s, ICM42607_MREG_OFFSET_USER7, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Accel Z[7:0] */
+		val = offset[Z] & GENMASK(7, 0);
+		ret = icm_write_mclk_reg(s, ICM42607_MREG_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_write_mclk_reg(s, ICM42607_MREG_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_write_mclk_reg(s, ICM42607_MREG_OFFSET_USER1, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Gyro Y[7:0] */
+		val = offset[Y] & GENMASK(7, 0);
+		ret = icm_write_mclk_reg(s, ICM42607_MREG_OFFSET_USER2, val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+
+		/* Gyro Z[7:0] */
+		val = offset[Z] & GENMASK(7, 0);
+		ret = icm_write_mclk_reg(s, ICM42607_MREG_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_update_mclk_reg(s, ICM42607_MREG_OFFSET_USER4,
+						GENMASK(3, 0), val);
+		if (ret != EC_SUCCESS)
+			goto out_unlock;
+		break;
+
+	default:
+		ret = EC_ERROR_INVAL;
+		goto out_unlock;
+	}
+
+	ret = icm_switch_off_mclk(s);
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+static int icm42607_set_offset(const struct motion_sensor_t *s,
+			       const int16_t *offset, int16_t temp)
+{
+	intv3_t v = { offset[X], offset[Y], offset[Z] };
+	int div1, div2;
+	int i;
+
+	/* rotate back to chip frame */
+	rotate_inv(v, *s->rot_standard_ref, v);
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/* hardware offset is 1/2048g /LSB, EC offset 1/1024g /LSB. */
+		div1 = 2;
+		div2 = 1;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		/* hardware offset is 1/32dps /LSB, EC offset 1/1024dps /LSB. */
+		div1 = 1;
+		div2 = 32;
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+	for (i = X; i <= Z; ++i)
+		v[i] = round_divide(v[i] * div1, div2);
+
+	return icm42607_set_hw_offset(s, v);
+}
+
+static int icm42607_get_offset(const struct motion_sensor_t *s, int16_t *offset,
+			       int16_t *temp)
+{
+	intv3_t v;
+	int div1, div2;
+	int i, ret;
+
+	ret = icm42607_get_hw_offset(s, v);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
+		/* hardware offset is 1/2048g /LSB, EC offset 1/1024g /LSB. */
+		div1 = 1;
+		div2 = 2;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		/* hardware offset is 1/32dps /LSB, EC offset 1/1024dps /LSB. */
+		div1 = 32;
+		div2 = 1;
+		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);
+	offset[X] = v[X];
+	offset[Y] = v[Y];
+	offset[Z] = v[Z];
+	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
+
+	return EC_SUCCESS;
+}
+
+static int icm42607_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 = ICM42607_REG_ACCEL_DATA_XYZ;
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
+		reg = ICM42607_REG_GYRO_DATA_XYZ;
+		break;
+	default:
+		return EC_ERROR_INVAL;
+	}
+
+	/* read data registers if sensor is stabilized */
+	mutex_lock(s->mutex);
+
+	ret = icm42607_check_sensor_stabilized(s, __hw_clock_source_read());
+	if (ret == EC_SUCCESS)
+		ret = icm_read_n(s, reg, raw, sizeof(raw));
+
+	mutex_unlock(s->mutex);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	ret = icm42607_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 icm42607_read_temp(const struct motion_sensor_t *s, int *temp_ptr)
+{
+	int val, ret;
+
+	mutex_lock(s->mutex);
+	ret = icm_read16(s, ICM42607_REG_TEMP_DATA, &val);
+	mutex_unlock(s->mutex);
+
+	if (ret != EC_SUCCESS)
+		return ret;
+
+	/* ensure correct propagation of 16 bits sign bit */
+	val = sign_extend(val, 15);
+
+	if (val == ICM42607_INVALID_DATA)
+		return EC_ERROR_NOT_POWERED;
+
+	*temp_ptr = C_TO_K((val / 128) + 25);
+	return EC_SUCCESS;
+}
+
+static int icm42607_init_config(const struct motion_sensor_t *s)
+{
+	int mask, val, ret;
+
+	ret = icm_switch_on_mclk(s);
+	if (ret)
+		return ret;
+
+	/* Set otp_copy_mode register field */
+	ret = icm_field_update_mclk_reg(s, ICM42607_MREG_OTP_CONFIG,
+					ICM42607_OTP_COPY_MODE_MASK,
+					ICM42607_OTP_COPY_TRIM);
+	if (ret)
+		return ret;
+
+	/* Set otp_power_down register field to 0 */
+	ret = icm_field_update_mclk_reg(s, ICM42607_MREG_OTP_CTRL7,
+					ICM42607_OTP_PWR_DOWN, 0);
+	if (ret)
+		return ret;
+
+	/* Wait for 300us for the OTP to fully power up */
+	usleep(300);
+
+	/* Set otp_reload register field */
+	ret = icm_field_update_mclk_reg(s, ICM42607_MREG_OTP_CTRL7,
+					ICM42607_OTP_RELOAD,
+					ICM42607_OTP_RELOAD);
+	if (ret)
+		return ret;
+
+	/* Wait for 280 us for the OTP to load */
+	usleep(280);
+
+	ret = icm_switch_off_mclk(s);
+	if (ret)
+		return ret;
+
+	/* disable i3c support */
+	mask = ICM42607_I3C_SDR_EN | ICM42607_I3C_DDR_EN;
+	ret = icm_field_update8(s, ICM42607_REG_INTF_CONFIG1, mask, 0);
+	if (ret)
+		return ret;
+
+	/* set averaging filter for accel, 8x is max for 400Hz */
+	if (ICM42607_ACCEL_MAX_FREQ == 400000)
+		val = ICM42607_UI_AVG_SET(ICM42607_UI_AVG_8X);
+	else
+		val = ICM42607_UI_AVG_SET(ICM42607_UI_AVG_32X);
+	ret = icm_field_update8(s, ICM42607_REG_ACCEL_CONFIG1,
+				ICM42607_UI_AVG_MASK, val);
+	if (ret)
+		return ret;
+
+	/* disable all interrupts */
+	ret = icm_write8(s, ICM42607_REG_INT_SOURCE0, 0);
+	if (ret)
+		return ret;
+
+	/* disable FIFO */
+	return icm_write8(s, ICM42607_REG_FIFO_CONFIG1, ICM42607_FIFO_BYPASS);
+}
+
+static int icm42607_init(const struct motion_sensor_t *s)
+{
+	struct accelgyro_saved_data_t *saved_data = ICM_GET_SAVED_DATA(s);
+	int val;
+	int ret, i;
+
+	mutex_lock(s->mutex);
+
+	/* detect chip using whoami */
+	ret = icm_read8(s, ICM42607_REG_WHO_AM_I, &val);
+	if (ret)
+		goto out_unlock;
+
+	if (val != ICM42607_CHIP_ICM42607P) {
+		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) {
+		/* clear status register */
+		ret = icm_read8(s, ICM42607_REG_INT_STATUS, &val);
+		if (ret)
+			goto out_unlock;
+
+		/* Reset to make sure previous state are not there */
+		ret = icm_write8(s, ICM42607_REG_SIGNAL_PATH_RESET,
+				 ICM42607_SOFT_RESET_DEV_CONFIG);
+		if (ret)
+			goto out_unlock;
+
+		/* Check reset is done, 1ms max */
+		for (i = 0; i < 5; ++i) {
+			usleep(200);
+			ret = icm_read8(s, ICM42607_REG_INT_STATUS, &val);
+			if (ret)
+				goto out_unlock;
+			if (val == ICM42607_RESET_DONE_INT)
+				break;
+		}
+		if (val != ICM42607_RESET_DONE_INT) {
+			ret = EC_ERROR_HW_INTERNAL;
+			goto out_unlock;
+		}
+
+		/* configure sensor */
+		ret = icm42607_init_config(s);
+		if (ret)
+			goto out_unlock;
+
+		if (IS_ENABLED(CONFIG_ACCEL_INTERRUPTS)) {
+			ret = icm42607_config_interrupt(s);
+			if (ret)
+				goto out_unlock;
+		}
+	}
+
+	for (i = X; i <= Z; i++)
+		saved_data->scale[i] = MOTION_SENSE_DEFAULT_SCALE;
+
+	saved_data->odr = 0;
+
+	mutex_unlock(s->mutex);
+
+	return sensor_init_done(s);
+
+out_unlock:
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+const struct accelgyro_drv icm42607_drv = {
+	.init = icm42607_init,
+	.read = icm42607_read,
+	.read_temp = icm42607_read_temp,
+	.set_range = icm42607_set_range,
+	.get_range = icm_get_range,
+	.get_resolution = icm_get_resolution,
+	.set_data_rate = icm42607_set_data_rate,
+	.get_data_rate = icm_get_data_rate,
+	.set_offset = icm42607_set_offset,
+	.get_offset = icm42607_get_offset,
+	.set_scale = icm_set_scale,
+	.get_scale = icm_get_scale,
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.irq_handler = icm42607_irq_handler,
+#endif
+};
diff --git a/driver/accelgyro_icm42607.h b/driver/accelgyro_icm42607.h
new file mode 100644
index 0000000000..41747fff7a
--- /dev/null
+++ b/driver/accelgyro_icm42607.h
@@ -0,0 +1,280 @@
+/* Copyright 2021 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-42607 accelerometer and gyroscope for Chrome EC */
+
+#ifndef __CROS_EC_ACCELGYRO_ICM42607_H
+#define __CROS_EC_ACCELGYRO_ICM42607_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 ICM42607_ADDR0_FLAGS		0x68
+#define ICM42607_ADDR1_FLAGS		0x69
+
+/* Min and Max sampling frequency in mHz */
+#define ICM42607_ACCEL_MIN_FREQ		1562
+#define ICM42607_ACCEL_MAX_FREQ		\
+		MOTION_MAX_SENSOR_FREQUENCY(400000, 100000)
+#define ICM42607_GYRO_MIN_FREQ		12500
+#define ICM42607_GYRO_MAX_FREQ		\
+		MOTION_MAX_SENSOR_FREQUENCY(1600000, 100000)
+
+/* Min and Max Accel FS in g */
+#define ICM42607_ACCEL_FS_MIN_VAL	2
+#define ICM42607_ACCEL_FS_MAX_VAL	16
+
+/* Min and Max Gyro FS in dps */
+#define ICM42607_GYRO_FS_MIN_VAL	250
+#define ICM42607_GYRO_FS_MAX_VAL	2000
+
+/* accel stabilization time in us */
+#define ICM42607_ACCEL_START_TIME	20000
+#define ICM42607_ACCEL_STOP_TIME	0
+
+/* gyro stabilization time in us */
+#define ICM42607_GYRO_START_TIME	40000
+#define ICM42607_GYRO_STOP_TIME		20000
+
+/* Reg value from Accel FS in G */
+#define ICM42607_ACCEL_FS_TO_REG(_fs)	((_fs) <= 2 ? 3 : \
+					 (_fs) >= 16 ? 0 : \
+					 3 - __fls((_fs) / 2))
+
+/* Accel FSR in G from Reg value */
+#define ICM42607_ACCEL_REG_TO_FS(_reg)	((1 << (3 - (_reg))) * 2)
+
+/* Reg value from Gyro FS in dps */
+#define ICM42607_GYRO_FS_TO_REG(_fs)	((_fs) <= 250 ? 3 : \
+					 (_fs) >= 2000 ? 0 : \
+					 3 - __fls((_fs) / 250))
+
+/* Gyro FSR in dps from Reg value */
+#define ICM42607_GYRO_REG_TO_FS(_reg)	((1 << (3 - (_reg))) * 250)
+
+/* Reg value from ODR in mHz */
+#define ICM42607_ODR_TO_REG(_odr)	((_odr) == 0 ? 0 : \
+					 (__fls(1600000 / (_odr)) + 5))
+
+/* ODR in mHz from Reg value */
+#define ICM42607_REG_TO_ODR(_reg)	((_reg) <= 5 ? 1600000 : \
+					 (1600000 / (1 << ((_reg) - 5))))
+
+/* Reg value for the next higher ODR */
+#define ICM42607_ODR_REG_UP(_reg)	((_reg) - 1)
+
+/*
+ * Filter bandwidth values from ODR reg
+ *   >= 400Hz (7) -> 180Hz (1)
+ *   200Hz (8) -> 73Hz (3)
+ *   100Hz (9) -> 53Hz (4)
+ *   50Hz (10) -> 25Hz (6)
+ *   <= 25Hz (11) -> 16Hz (7)
+ */
+#define ICM42607_ODR_TO_FILT_BW(_odr)	((_odr) <= 7 ? 1 : \
+					 (_odr) <= 9 ? (_odr) - 5 : \
+					 (_odr) == 10 ? 6 : \
+					 7)
+
+/*
+ * Register addresses are virtual address on 16 bits.
+ * MSB is coding block selection value for MREG registers
+ * and LSB real register address.
+ * ex: MREG2 (block 0x28) register 03 => 0x2803
+ */
+#define ICM42607_REG_MCLK_RDY		0x0000
+#define ICM42607_MCLK_RDY		BIT(3)
+
+#define ICM426XX_REG_DEVICE_CONFIG	0x0001
+#define ICM426XX_SPI_MODE_1_2		BIT(0)
+#define ICM426XX_SPI_AP_4WIRE		BIT(2)
+
+#define ICM42607_REG_SIGNAL_PATH_RESET	0x0002
+#define ICM42607_SOFT_RESET_DEV_CONFIG	BIT(4)
+#define ICM42607_FIFO_FLUSH		BIT(2)
+
+#define ICM42607_REG_DRIVE_CONFIG1	0x0003
+
+#define ICM42607_REG_DRIVE_CONFIG2	0x0004
+
+#define ICM42607_REG_DRIVE_CONFIG3	0x0005
+
+/* default int configuration is pulsed mode, open drain, and active low */
+#define ICM42607_REG_INT_CONFIG		0x0006
+#define ICM42607_INT2_MASK		GENMASK(5, 3)
+#define ICM42607_INT2_LATCHED		BIT(5)
+#define ICM42607_INT2_PUSH_PULL		BIT(4)
+#define ICM42607_INT2_ACTIVE_HIGH	BIT(3)
+#define ICM42607_INT1_MASK		GENMASK(2, 0)
+#define ICM42607_INT1_LATCHED		BIT(2)
+#define ICM42607_INT1_PUSH_PULL		BIT(1)
+#define ICM42607_INT1_ACTIVE_HIGH	BIT(0)
+
+/* data are 16 bits */
+#define ICM42607_REG_TEMP_DATA		0x0009
+
+/* X + Y + Z: 3 * 16 bits */
+#define ICM42607_REG_ACCEL_DATA_XYZ	0x000B
+#define ICM42607_REG_GYRO_DATA_XYZ	0x0011
+
+#define ICM42607_INVALID_DATA		-32768
+
+/* data are 16 bits */
+#define ICM42607_REG_TMST_FSYNCH	0x0017
+
+#define ICM42607_REG_PWR_MGMT0		0x001F
+#define ICM42607_ACCEL_LP_CLK_SEL	BIT(7)
+#define ICM42607_IDLE			BIT(4)
+#define ICM42607_GYRO_MODE_MASK		GENMASK(3, 2)
+#define ICM42607_GYRO_MODE(_m)		(((_m) & 0x03) << 2)
+#define ICM42607_ACCEL_MODE_MASK	GENMASK(1, 0)
+#define ICM42607_ACCEL_MODE(_m)		((_m) & 0x03)
+
+enum icm42607_sensor_mode {
+	ICM42607_MODE_OFF,
+	ICM42607_MODE_STANDBY,
+	ICM42607_MODE_LOW_POWER,
+	ICM42607_MODE_LOW_NOISE,
+};
+
+#define ICM42607_REG_GYRO_CONFIG0	0x0020
+#define ICM42607_REG_ACCEL_CONFIG0	0x0021
+#define ICM42607_FS_MASK		GENMASK(6, 5)
+#define ICM42607_FS_SEL(_fs)		(((_fs) & 0x03) << 5)
+#define ICM42607_ODR_MASK		GENMASK(3, 0)
+#define ICM42607_ODR(_odr)		((_odr) & 0x0F)
+
+#define ICM42607_REG_TEMP_CONFIG0	0x0022
+
+enum icm42607_ui_avg {
+	ICM42607_UI_AVG_2X,
+	ICM42607_UI_AVG_4X,
+	ICM42607_UI_AVG_8X,
+	ICM42607_UI_AVG_16X,
+	ICM42607_UI_AVG_32X,
+	ICM42607_UI_AVG_64X,
+};
+
+enum icm42607_ui_filt_bw {
+	ICM42607_UI_FILT_BW_DISABLED,
+	ICM42607_UI_FILT_BW_180HZ,
+	ICM42607_UI_FILT_BW_121HZ,
+	ICM42607_UI_FILT_BW_73HZ,
+	ICM42607_UI_FILT_BW_53HZ,
+	ICM42607_UI_FILT_BW_34HZ,
+	ICM42607_UI_FILT_BW_25HZ,
+	ICM42607_UI_FILT_BW_16HZ,
+};
+
+#define ICM42607_REG_GYRO_CONFIG1	0x0023
+#define ICM42607_REG_ACCEL_CONFIG1	0x0024
+#define ICM42607_UI_AVG_MASK		GENMASK(6, 4)
+#define ICM42607_UI_AVG_SET(_avg)	(((_avg) & 0x07) << 4)
+#define ICM42607_UI_FILT_BW_MASK	GENMASK(2, 0)
+#define ICM42607_UI_FILT_BW_SET(_filt)	((_filt) & 0x07)
+
+#define ICM42607_REG_FIFO_CONFIG1	0x0028
+#define ICM42607_FIFO_STOP_ON_FULL_MODE	BIT(1)
+#define ICM42607_FIFO_BYPASS		BIT(0)
+#define ICM42607_FIFO_MODE_STREAM	0x00
+
+/* FIFO watermark value is 16 bits little endian */
+#define ICM42607_REG_FIFO_WM		0x0029
+
+#define ICM42607_REG_INT_SOURCE0	0x002B
+#define ICM42607_ST_INT1_EN		BIT(7)
+#define ICM42607_FSYNC_INT1_EN		BIT(6)
+#define ICM42607_PLL_RDY_INT1_EN	BIT(5)
+#define ICM42607_RESET_DONE_INT1_EN	BIT(4)
+#define ICM42607_DRDY_INT1_EN		BIT(3)
+#define ICM42607_FIFO_THS_INT1_EN	BIT(2)
+#define ICM42607_FIFO_FULL_INT1_EN	BIT(1)
+#define ICM42607_UI_AGC_RDY_INT1_EN	BIT(0)
+
+#define ICM42607_REG_INTF_CONFIG0	0x0035
+#define ICM42607_FIFO_COUNT_FORMAT	BIT(6)
+#define ICM42607_FIFO_COUNT_ENDIAN	BIT(5)
+#define ICM42607_SENSOR_DATA_ENDIAN	BIT(4)
+
+#define ICM42607_REG_INTF_CONFIG1	0x0036
+#define ICM42607_I3C_SDR_EN		BIT(3)
+#define ICM42607_I3C_DDR_EN		BIT(2)
+#define ICM42607_CLKSEL_MASK		GENMASK(1, 0)
+#define ICM42607_CLKSEL_PLL_ENABLE	0x01
+
+#define ICM42607_REG_INT_STATUS_DRDY	0x0039
+#define ICM42607_DATA_RDY_INT		BIT(0)
+
+#define ICM42607_REG_INT_STATUS		0x003A
+#define ICM42607_ST_INT			BIT(7)
+#define ICM42607_FSYNC_INT		BIT(6)
+#define ICM42607_PLL_RDY_INT		BIT(5)
+#define ICM42607_RESET_DONE_INT		BIT(4)
+#define ICM42607_FIFO_THS_INT		BIT(2)
+#define ICM42607_FIFO_FULL_INT		BIT(1)
+#define ICM42607_AGC_RDY_INT		BIT(0)
+
+/* FIFO count is 16 bits */
+#define ICM42607_REG_FIFO_COUNT		0x003D
+
+#define ICM42607_REG_FIFO_DATA		0x003F
+
+#define ICM42607_REG_APEX_CONFIG0	0x0025
+#define ICM42607_DMP_SRAM_RESET_APEX	BIT(0)
+
+#define ICM42607_REG_APEX_CONFIG1	0x0026
+#define ICM42607_DMP_ODR_50HZ		BIT(1)
+
+#define ICM42607_REG_WHO_AM_I		0x0075
+#define ICM42607_CHIP_ICM42607P		0x60
+
+/* MREG read access registers */
+#define ICM42607_REG_BLK_SEL_W		0x0079
+#define ICM42607_REG_MADDR_W		0x007A
+#define ICM42607_REG_M_W		0x007B
+
+/* MREG write access registers */
+#define ICM42607_REG_BLK_SEL_R		0x007C
+#define ICM42607_REG_MADDR_R		0x007D
+#define ICM42607_REG_M_R		0x007E
+
+/* USER BANK MREG1 */
+#define ICM42607_MREG_FIFO_CONFIG5	0x0001
+#define ICM42607_FIFO_WM_GT_TH		BIT(5)
+#define ICM42607_FIFO_RESUME_PARTIAL_RD	BIT(4)
+#define ICM42607_FIFO_HIRES_EN		BIT(3)
+#define ICM42607_FIFO_TMST_FSYNC_EN	BIT(2)
+#define ICM42607_FIFO_GYRO_EN		BIT(1)
+#define ICM42607_FIFO_ACCEL_EN		BIT(0)
+
+#define ICM42607_MREG_OTP_CONFIG	0x002B
+#define ICM42607_OTP_COPY_MODE_MASK	GENMASK(3, 2)
+#define ICM42607_OTP_COPY_TRIM		(0x01 << 2)
+#define ICM42607_OTP_COPY_ST_DATA	(0x03 << 2)
+
+#define ICM42607_MREG_OFFSET_USER0	0x004E
+#define ICM42607_MREG_OFFSET_USER1	0x004F
+#define ICM42607_MREG_OFFSET_USER2	0x0050
+#define ICM42607_MREG_OFFSET_USER3	0x0051
+#define ICM42607_MREG_OFFSET_USER4	0x0052
+#define ICM42607_MREG_OFFSET_USER5	0x0053
+#define ICM42607_MREG_OFFSET_USER6	0x0054
+#define ICM42607_MREG_OFFSET_USER7	0x0055
+#define ICM42607_MREG_OFFSET_USER8	0x0056
+
+/* USER BANK MREG2 */
+#define ICM42607_MREG_OTP_CTRL7		0x2806
+#define ICM42607_OTP_RELOAD		BIT(3)
+#define ICM42607_OTP_PWR_DOWN		BIT(1)
+
+extern const struct accelgyro_drv icm42607_drv;
+
+void icm42607_interrupt(enum gpio_signal signal);
+
+#endif /* __CROS_EC_ACCELGYRO_ICM42607_H */
diff --git a/driver/build.mk b/driver/build.mk
index 53f6c0cdb4..72bbd46a89 100644
--- a/driver/build.mk
+++ b/driver/build.mk
@@ -26,6 +26,7 @@ driver-$(CONFIG_SYNC)+=sync.o
 driver-$(CONFIG_ACCEL_LIS2DW_COMMON)+=accel_lis2dw12.o stm_mems_common.o
 driver-$(CONFIG_ACCEL_LIS2DS)+=accel_lis2ds.o stm_mems_common.o
 driver-$(CONFIG_ACCELGYRO_ICM426XX)+=accelgyro_icm426xx.o accelgyro_icm_common.o
+driver-$(CONFIG_ACCELGYRO_ICM42607)+=accelgyro_icm42607.o accelgyro_icm_common.o
 
 # BC1.2 Charger Detection Devices
 driver-$(CONFIG_BC12_DETECT_MAX14637)+=bc12/max14637.o
diff --git a/include/ec_commands.h b/include/ec_commands.h
index f299a525f9..7dc21165c3 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -2672,6 +2672,7 @@ enum motionsensor_chip {
 	MOTIONSENSE_CHIP_LIS2DS = 23,
 	MOTIONSENSE_CHIP_BMI260 = 24,
 	MOTIONSENSE_CHIP_ICM426XX = 25,
+	MOTIONSENSE_CHIP_ICM42607 = 26,
 	MOTIONSENSE_CHIP_MAX,
 };
 
diff --git a/util/ectool.c b/util/ectool.c
index f4722c06df..8d91d2cb74 100644
--- a/util/ectool.c
+++ b/util/ectool.c
@@ -5291,6 +5291,9 @@ static int cmd_motionsense(int argc, char **argv)
 		case MOTIONSENSE_CHIP_ICM426XX:
 			printf("icm426xx\n");
 			break;
+		case MOTIONSENSE_CHIP_ICM42607:
+			printf("icm42607\n");
+			break;
 		default:
 			printf("unknown\n");
 		}