diff options
author | JuHyun Kim <jkim@invensense.com> | 2021-04-12 08:16:44 -0700 |
---|---|---|
committer | Commit Bot <commit-bot@chromium.org> | 2021-05-18 01:48:45 +0000 |
commit | 0602debe983036d8d7285c207b2b5cd0339eb2a8 (patch) | |
tree | 11916e244fc21cd540c521e6c2a6823f2b5d2844 /driver/accelgyro_icm42607.c | |
parent | 266c917b95afddb3b163a13ac5f48fba261c2abd (diff) | |
download | chrome-ec-0602debe983036d8d7285c207b2b5cd0339eb2a8.tar.gz |
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>
Diffstat (limited to 'driver/accelgyro_icm42607.c')
-rw-r--r-- | driver/accelgyro_icm42607.c | 1134 |
1 files changed, 1134 insertions, 0 deletions
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 +}; |