diff options
Diffstat (limited to 'driver/accelgyro_icm426xx.c')
| -rw-r--r-- | driver/accelgyro_icm426xx.c | 953 |
1 files changed, 953 insertions, 0 deletions
diff --git a/driver/accelgyro_icm426xx.c b/driver/accelgyro_icm426xx.c new file mode 100644 index 0000000000..40adaaaace --- /dev/null +++ b/driver/accelgyro_icm426xx.c @@ -0,0 +1,953 @@ +/* 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 +}; |