diff options
Diffstat (limited to 'driver/accelgyro_icm426xx.c')
| -rw-r--r-- | driver/accelgyro_icm426xx.c | 981 |
1 files changed, 0 insertions, 981 deletions
diff --git a/driver/accelgyro_icm426xx.c b/driver/accelgyro_icm426xx.c deleted file mode 100644 index 5d09c8e4ef..0000000000 --- a/driver/accelgyro_icm426xx.c +++ /dev/null @@ -1,981 +0,0 @@ -/* 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 "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 icm426xx_normalize(const struct motion_sensor_t *s, intv3_t v, - const uint8_t *raw) -{ - struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s); - int i; - - /* sensor data is configured as little-endian */ - v[X] = (int16_t)UINT16_FROM_BYTE_ARRAY_LE(raw, 0); - v[Y] = (int16_t)UINT16_FROM_BYTE_ARRAY_LE(raw, 2); - v[Z] = (int16_t)UINT16_FROM_BYTE_ARRAY_LE(raw, 4); - - /* check if data is valid */ - if (v[X] == ICM426XX_INVALID_DATA && - v[Y] == ICM426XX_INVALID_DATA && - v[Z] == ICM426XX_INVALID_DATA) { - return EC_ERROR_INVAL; - } - - rotate(v, *s->rot_standard_ref, v); - - for (i = X; i <= Z; i++) - v[i] = SENSOR_APPLY_SCALE(v[i], data->scale[i]); - - return EC_SUCCESS; -} - -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_stage_data(&vect, s, 3, ts); - } -} - -static int __maybe_unused icm426xx_load_fifo(struct motion_sensor_t *s, - uint32_t ts) -{ - struct icm_drv_data_t *st = ICM_GET_DATA(s); - int count, i, size; - const uint8_t *accel, *gyro; - int ret; - - ret = icm_read16(s, ICM426XX_REG_FIFO_COUNT, &count); - if (ret != EC_SUCCESS) - return ret; - - if (count <= 0) - return EC_ERROR_INVAL; - - /* flush FIFO if buffer is not large enough */ - if (count > ICM_FIFO_BUFFER) { - CPRINTS("It should not happen, the EC is too slow for the ODR"); - ret = icm_write8(s, ICM426XX_REG_SIGNAL_PATH_RESET, - ICM426XX_FIFO_FLUSH); - if (ret != EC_SUCCESS) - return ret; - return EC_ERROR_OVERFLOW; - } - - ret = icm_read_n(s, ICM426XX_REG_FIFO_DATA, st->fifo_buffer, count); - if (ret != EC_SUCCESS) - return ret; - - for (i = 0; i < count; i += size) { - size = icm_fifo_decode_packet(&st->fifo_buffer[i], - &accel, &gyro); - /* exit if error or FIFO is empty */ - if (size <= 0) - return -size; - if (accel != NULL) { - 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) -{ - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - last_interrupt_timestamp = __hw_clock_source_read(); - - task_set_event(TASK_ID_MOTIONSENSE, - CONFIG_ACCELGYRO_ICM426XX_INT_EVENT); -} - -/** - * 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; - - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) { - if (status & ICM426XX_FIFO_INT_STATUS) { - ret = icm426xx_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 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; - if (s->flags & MOTIONSENSE_FLAG_INT_ACTIVE_HIGH) - val |= 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; - - if (IS_ENABLED(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; - } - - 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) { - /* disable data in FIFO */ - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - icm426xx_config_fifo(s, 0); - /* 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; - /* enable data in FIFO */ - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - icm426xx_config_fifo(s, 1); - } - - data->odr = normalized_rate; - return EC_SUCCESS; - -out_unlock: - mutex_unlock(s->mutex); - return ret; -} - -static int icm426xx_set_range(struct motion_sensor_t *s, int range, - int rnd) -{ - 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) - s->current_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_read_temp(const struct motion_sensor_t *s, int *temp_ptr) -{ - int val, ret; - - mutex_lock(s->mutex); - ret = icm_read16(s, ICM426XX_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 == ICM426XX_INVALID_DATA) - return EC_ERROR_NOT_POWERED; - - *temp_ptr = C_TO_K((val * 100) / 13248 + 25); - 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. - */ - -#ifdef CONFIG_ACCELGYRO_ICM_COMM_SPI - icm_field_update8( - s, ICM426XX_REG_INTF_CONFIG6, ICM426XX_INTF_CONFIG6_MASK, - ICM426XX_I3C_EN | ICM426XX_I3C_SDR_EN | ICM426XX_I3C_DDR_EN); - ret = icm_field_update8(s, ICM426XX_REG_INTF_CONFIG4, - ICM426XX_I3C_BUS_MODE, ICM426XX_I3C_BUS_MODE); -#else - 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; - -#ifdef CONFIG_ACCELGYRO_ICM_COMM_SPI - ret = icm_field_update8( - s, ICM426XX_REG_DRIVE_CONFIG, ICM426XX_DRIVE_CONFIG_MASK, - ICM426XX_I2C_SLEW_RATE(ICM426XX_SLEW_RATE_20NS_60NS) | - ICM426XX_SPI_SLEW_RATE(ICM426XX_SLEW_RATE_INF_2NS)); -#else - 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; -#ifdef CONFIG_ACCELGYRO_ICM_COMM_SPI - val = ICM426XX_UI_SIFS_CFG_I2C_DIS; -#else - 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(struct motion_sensor_t *s) -{ - struct icm_drv_data_t *st = ICM_GET_DATA(s); - struct accelgyro_saved_data_t *saved_data = ICM_GET_SAVED_DATA(s); - int mask, val; - int ret, i; - - mutex_lock(s->mutex); - - /* manually force register bank to 0 */ - st->bank = 0; - ret = icm_write8(s, ICM426XX_REG_BANK_SEL, ICM426XX_BANK_SEL(0)); - if (ret) - goto out_unlock; - - /* detect chip using whoami */ - ret = icm_read8(s, ICM426XX_REG_WHO_AM_I, &val); - if (ret) - goto out_unlock; - - if (val != ICM426XX_CHIP_ICM40608 && val != ICM426XX_CHIP_ICM42605) { - CPRINTS("Unknown WHO_AM_I: 0x%02x", val); - ret = EC_ERROR_ACCESS_DENIED; - goto out_unlock; - } - - /* first time init done only for 1st sensor (accel) */ - if (s->type == MOTIONSENSE_TYPE_ACCEL) { - /* reset the chip and verify it is ready */ - ret = icm_write8(s, ICM426XX_REG_DEVICE_CONFIG, - ICM426XX_SOFT_RESET_CONFIG); - if (ret) - goto out_unlock; - msleep(1); - - ret = icm_read8(s, ICM426XX_REG_INT_STATUS, &val); - if (ret) - goto out_unlock; - if (!(val & ICM426XX_RESET_DONE_INT)) { - ret = EC_ERROR_ACCESS_DENIED; - goto out_unlock; - } - - /* configure sensor */ - ret = icm426xx_init_config(s); - if (ret) - goto out_unlock; - - if (IS_ENABLED(CONFIG_ACCEL_INTERRUPTS)) - ret = icm426xx_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; - - /* 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); - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - st->accel = (struct motion_sensor_t *)s; - break; - case MOTIONSENSE_TYPE_GYRO: - mask = ICM426XX_GYRO_UI_FILT_MASK; - val = ICM426XX_GYRO_UI_FILT_BW(ICM426XX_FILTER_BW_ODR_DIV_2); - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) - st->gyro = (struct motion_sensor_t *)s; - 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, - .read_temp = icm426xx_read_temp, - .set_range = icm426xx_set_range, - .get_resolution = icm_get_resolution, - .set_data_rate = icm426xx_set_data_rate, - .get_data_rate = icm_get_data_rate, - .set_offset = icm426xx_set_offset, - .get_offset = icm426xx_get_offset, - .set_scale = icm_set_scale, - .get_scale = icm_get_scale, -#ifdef CONFIG_ACCEL_INTERRUPTS - .irq_handler = icm426xx_irq_handler, -#endif -}; |