/* 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 /* CONFIG_ACCEL_FIFO */

static int icm426xx_normalize(const struct motion_sensor_t *s, intv3_t v,
			      const uint8_t *raw)
{
	/* 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);

	return EC_SUCCESS;
}

static int icm426xx_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;
}

/* 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_add_data(&vect, s, 3, ts);
	}
}

/* division that round to the nearest integer */
static int round_divide(int64_t dividend, int divisor)
{
	return (dividend > 0) ^ (divisor > 0) ?
		(dividend - divisor / 2) / divisor :
		(dividend + divisor / 2) / divisor;
}

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) {
			ret = icm426xx_check_sensor_stabilized(st->accel, ts);
			if (ret == EC_SUCCESS)
				icm426xx_push_fifo_data(st->accel, accel, ts);
		}
		if (gyro != NULL) {
			ret = icm426xx_check_sensor_stabilized(st->gyro, ts);
			if (ret == EC_SUCCESS)
				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 /* CONFIG_ACCEL_FIFO */

	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 /* CONFIG_ACCEL_FIFO */

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 | ICM426XX_INT1_ACTIVE_HIGH;

	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 /* CONFIG_ACCEL_FIFO */

	return ret;
}

#endif	/* CONFIG_ACCEL_INTERRUPTS */

static int icm426xx_enable_sensor(const struct motion_sensor_t *s, int enable)
{
	uint32_t delay, stop_delay;
	int32_t rem;
	uint8_t mask, val;
	int ret;

	switch (s->type) {
	case MOTIONSENSE_TYPE_ACCEL:
		mask = ICM426XX_ACCEL_MODE_MASK;
		if (enable) {
			delay = ICM426XX_ACCEL_START_TIME;
			stop_delay = ICM426XX_ACCEL_STOP_TIME;
			val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_LOW_POWER);
		} else {
			delay = ICM426XX_ACCEL_STOP_TIME;
			val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_OFF);
		}
		break;
	case MOTIONSENSE_TYPE_GYRO:
		mask = ICM426XX_GYRO_MODE_MASK;
		if (enable) {
			delay = ICM426XX_GYRO_START_TIME;
			stop_delay = ICM426XX_GYRO_STOP_TIME;
			val = ICM426XX_GYRO_MODE(ICM426XX_MODE_LOW_NOISE);
		} else {
			delay = ICM426XX_GYRO_STOP_TIME;
			val = ICM426XX_GYRO_MODE(ICM426XX_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, ICM426XX_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 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) {
#ifdef CONFIG_ACCEL_FIFO
		/* disable data in FIFO */
		icm426xx_config_fifo(s, 0);
#endif /* CONFIG_ACCEL_FIFO */
		/* disable sensor */
		ret = icm426xx_enable_sensor(s, 0);
		data->odr = 0;
		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;

	mutex_unlock(s->mutex);

	if (data->odr == 0) {
		/* enable sensor */
		ret = icm426xx_enable_sensor(s, 1);
		if (ret)
			return ret;
#ifdef CONFIG_ACCEL_FIFO
		/* enable data in FIFO */
		icm426xx_config_fifo(s, 1);
#endif /* CONFIG_ACCEL_FIFO */
	}

	data->odr = normalized_rate;
	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)
{
	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);

	/* convert raw data to hardware offset units */
	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 icm426xx_set_hw_offset(s, v);
}

static int icm426xx_get_offset(const struct motion_sensor_t *s, int16_t *offset,
			       int16_t *temp)
{
	intv3_t v;
	int div1, div2;
	int i, ret;

	ret = icm426xx_get_hw_offset(s, v);
	if (ret != EC_SUCCESS)
		return ret;

	/* transform offset to raw data */
	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 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 if sensor is stabilized */
	mutex_lock(s->mutex);

	ret = icm426xx_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 = 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;
#ifdef I2C_PORT_ACCEL
	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);
#endif
	if (ret)
		return ret;

	ret = 0;
#ifdef I2C_PORT_ACCEL
	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));
#endif
	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;
#ifdef I2C_PORT_ACCEL
	val |= ICM426XX_UI_SIFS_CFG_SPI_DIS;
#endif

	ret = icm_field_update8(s, ICM426XX_REG_INTF_CONFIG0, mask, val);
	if (ret)
		return ret;

	/* set accel oscillator to RC clock to avoid bad transition with PLL */
	return icm_field_update8(s, ICM426XX_REG_INTF_CONFIG1,
				 ICM426XX_ACCEL_LP_CLK_SEL,
				 ICM426XX_ACCEL_LP_CLK_SEL);
}

static int icm426xx_init(const struct motion_sensor_t *s)
{
	struct accelgyro_saved_data_t *saved_data = ICM_GET_SAVED_DATA(s);
	struct icm_drv_data_t *st = ICM_GET_DATA(s);
	int mask, val;
	int ret;

	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
	}

	saved_data->odr = 0;

	/* 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 /* CONFIG_ACCEL_FIFO */
		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 /* CONFIG_ACCEL_FIFO */
		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,
#ifdef CONFIG_ACCEL_INTERRUPTS
	.irq_handler = icm426xx_irq_handler,
#endif
};