From cef49c7810418f39af60358bf32dc0326c42f962 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Maneyrol Date: Fri, 17 Jul 2020 19:29:54 +0200 Subject: driver: add ICM-426xx driver support Add ICM-426xx accel/gyro driver code. BUG=chromium:1117541 BRANCH=None TEST=ectool motionsense fifo_read && tast run hardware.SensorRing Signed-off-by: Jean-Baptiste Maneyrol Change-Id: I83fe48abc6aa9cde86576a777ac4272d90fac597 Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2317888 Reviewed-by: Gwendal Grignou Commit-Queue: Gwendal Grignou Tested-by: Gwendal Grignou Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2584545 Tested-by: David Huang Reviewed-by: Zhuohao Lee Commit-Queue: Zhuohao Lee Auto-Submit: David Huang Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2600803 Reviewed-by: YH Lin Commit-Queue: YH Lin --- common/build.mk | 1 + driver/accelgyro_icm426xx.c | 916 ++++++++++++++++++++++++++++++++++++++++++ driver/accelgyro_icm426xx.h | 256 ++++++++++++ driver/accelgyro_icm_common.c | 296 ++++++++++++++ driver/accelgyro_icm_common.h | 101 +++++ driver/build.mk | 1 + include/accelgyro.h | 3 + include/common.h | 5 + include/config.h | 2 + 9 files changed, 1581 insertions(+) create mode 100644 driver/accelgyro_icm426xx.c create mode 100644 driver/accelgyro_icm426xx.h create mode 100644 driver/accelgyro_icm_common.c create mode 100644 driver/accelgyro_icm_common.h diff --git a/common/build.mk b/common/build.mk index 39e111956f..30afabd146 100644 --- a/common/build.mk +++ b/common/build.mk @@ -11,6 +11,7 @@ common-y+=version.o printf.o queue.o queue_policies.o common-$(CONFIG_ACCELGYRO_BMA255)+=math_util.o common-$(CONFIG_ACCELGYRO_BMI160)+=math_util.o +common-$(CONFIG_ACCELGYRO_ICM426XX)+=math_util.o common-$(CONFIG_ACCELGYRO_LSM6DS0)+=math_util.o common-$(CONFIG_ACCELGYRO_LSM6DSM)+=math_util.o common-$(CONFIG_ACCELGYRO_LSM6DSO)+=math_util.o diff --git a/driver/accelgyro_icm426xx.c b/driver/accelgyro_icm426xx.c new file mode 100644 index 0000000000..5598400fca --- /dev/null +++ b/driver/accelgyro_icm426xx.c @@ -0,0 +1,916 @@ +/* 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 + +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; +} + +/* 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) + icm426xx_push_fifo_data(st->accel, accel, ts); + if (gyro != NULL) + 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 + 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 + +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; + + 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 + + return ret; +} + +#endif /* CONFIG_ACCEL_INTERRUPTS */ + +static int icm426xx_enable_sensor(const struct motion_sensor_t *s, int enable) +{ + unsigned int sleep; + uint8_t mask, val; + int ret; + + switch (s->type) { + case MOTIONSENSE_TYPE_ACCEL: + mask = ICM426XX_ACCEL_MODE_MASK; + if (enable) { + sleep = 20; + val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_LOW_POWER); + } else { + sleep = 0; + val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_OFF); + } + break; + case MOTIONSENSE_TYPE_GYRO: + mask = ICM426XX_GYRO_MODE_MASK; + if (enable) { + sleep = 60; + val = ICM426XX_GYRO_MODE(ICM426XX_MODE_LOW_NOISE); + } else { + sleep = 150; + val = ICM426XX_GYRO_MODE(ICM426XX_MODE_OFF); + } + break; + default: + return -EC_ERROR_INVAL; + } + + mutex_lock(s->mutex); + + ret = icm_field_update8(s, ICM426XX_REG_PWR_MGMT0, mask, val); + /* when turning sensor on block any register write for 200 us */ + if (ret == EC_SUCCESS && enable) + usleep(200); + + mutex_unlock(s->mutex); + + if (ret != EC_SUCCESS) + return ret; + + if (sleep) + msleep(sleep); + + return EC_SUCCESS; +} + +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 */ +#ifdef CONFIG_ACCEL_FIFO + icm426xx_config_fifo(s, 0); +#endif + /* disable sensor */ + ret = icm426xx_enable_sensor(s, 0); + data->odr = 0; + return ret; + } else if (data->odr == 0) { + /* enable sensor */ + ret = icm426xx_enable_sensor(s, 1); + if (ret) + 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; + + data->odr = normalized_rate; + + mutex_unlock(s->mutex); + + /* enable data in FIFO */ +#ifdef CONFIG_ACCEL_FIFO + icm426xx_config_fifo(s, 1); +#endif + + 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) +{ + struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s); + intv3_t v = { offset[X], offset[Y], offset[Z] }; + int range, div1, div2; + int i; + + /* unscale values and rotate back to chip frame */ + for (i = X; i <= Z; ++i) + v[i] = SENSOR_APPLY_DIV_SCALE(v[i], data->scale[i]); + rotate_inv(v, *s->rot_standard_ref, v); + + /* convert raw data to hardware offset units */ + range = icm_get_range(s); + switch (s->type) { + case MOTIONSENSE_TYPE_ACCEL: + /* hardware offset is 1/2048g by LSB */ + div1 = range * 2048; + div2 = 32768; + break; + case MOTIONSENSE_TYPE_GYRO: + /* hardware offset is 1/32dps by LSB */ + div1 = range * 32; + div2 = 32768; + 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) +{ + struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s); + intv3_t v; + int range, div1, div2; + int i, ret; + + ret = icm426xx_get_hw_offset(s, v); + if (ret != EC_SUCCESS) + return ret; + + /* transform offset to raw data */ + range = icm_get_range(s); + switch (s->type) { + case MOTIONSENSE_TYPE_ACCEL: + /* hardware offset is 1/2048g by LSB */ + div1 = 32768; + div2 = 2048 * range; + break; + case MOTIONSENSE_TYPE_GYRO: + /* hardware offset is 1/32dps by LSB */ + div1 = 32768; + div2 = 32 * range; + 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); + for (i = X; i <= Z; i++) + v[i] = SENSOR_APPLY_SCALE(v[i], data->scale[i]); + + 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 */ + mutex_lock(s->mutex); + 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; + 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); + if (ret) + return ret; + + ret = 0; + 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)); + 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; + val |= ICM426XX_UI_SIFS_CFG_SPI_DIS; + + return icm_field_update8(s, ICM426XX_REG_INTF_CONFIG0, mask, val); +} + +static int icm426xx_init(const 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; + +#ifdef CONFIG_ACCEL_INTERRUPTS + ret = icm426xx_config_interrupt(s); + if (ret) + goto out_unlock; +#endif + } + + for (i = X; i <= Z; i++) + saved_data->scale[i] = MOTION_SENSE_DEFAULT_SCALE; + + /* 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 + 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 + 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, + .set_scale = icm_set_scale, + .get_scale = icm_get_scale, +#ifdef CONFIG_ACCEL_INTERRUPTS + .irq_handler = icm426xx_irq_handler, +#endif +}; diff --git a/driver/accelgyro_icm426xx.h b/driver/accelgyro_icm426xx.h new file mode 100644 index 0000000000..0b4e3aa14a --- /dev/null +++ b/driver/accelgyro_icm426xx.h @@ -0,0 +1,256 @@ +/* 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 for Chrome EC */ + +#ifndef __CROS_EC_ACCELGYRO_ICM426XX_H +#define __CROS_EC_ACCELGYRO_ICM426XX_H + +#include "accelgyro.h" +#include "common.h" + +/* + * 7-bit address is 110100Xb. Where 'X' is determined + * by the logic level on pin AP_AD0. + */ +#define ICM426XX_ADDR0_FLAGS 0x68 +#define ICM426XX_ADDR1_FLAGS 0x69 + +/* Min and Max sampling frequency in mHz */ +#define ICM426XX_ACCEL_MIN_FREQ 3125 +#define ICM426XX_ACCEL_MAX_FREQ MOTION_MAX_SENSOR_FREQUENCY(500000, 100000) +#define ICM426XX_GYRO_MIN_FREQ 12500 +#define ICM426XX_GYRO_MAX_FREQ MOTION_MAX_SENSOR_FREQUENCY(4000000, 100000) + +/* Min and Max Accel FS in G */ +#define ICM426XX_ACCEL_FS_MIN_VAL 2 +#define ICM426XX_ACCEL_FS_MAX_VAL 16 + +/* Min and Max Gyro FS in dps */ +#define ICM426XX_GYRO_FS_MIN_VAL 125 +#define ICM426XX_GYRO_FS_MAX_VAL 2000 + +/* Reg value from Accel FS in G */ +#define ICM426XX_ACCEL_FS_TO_REG(_fs) ((_fs) < 2 ? 3 : \ + (_fs) > 16 ? 0 : \ + 3 - __fls((_fs) / 2)) + +/* Accel FSR in G from Reg value */ +#define ICM426XX_ACCEL_REG_TO_FS(_reg) ((1 << (3 - (_reg))) * 2) + +/* Reg value from Gyro FS in dps */ +#define ICM426XX_GYRO_FS_TO_REG(_fs) ((_fs) < 125 ? 4 : \ + (_fs) > 2000 ? 0 : \ + 4 - __fls((_fs) / 125)) + +/* Gyro FSR in dps from Reg value */ +#define ICM426XX_GYRO_REG_TO_FS(_reg) ((1 << (4 - (_reg))) * 125) + +/* Reg value from ODR in mHz */ +#define ICM426XX_ODR_TO_REG(_odr) ((_odr) <= 200000 ? \ + 13 - __fls((_odr) / 3125) : \ + (_odr) < 500000 ? 7 : \ + (_odr) < 1000000 ? 15 : \ + 6 - __fls((_odr) / 1000000)) + +/* ODR in mHz from Reg value */ +#define ICM426XX_REG_TO_ODR(_reg) ((_reg) == 15 ? 500000 : \ + (_reg) >= 7 ? \ + (1 << (13 - (_reg))) * 3125 : \ + (1 << (6 - (_reg))) * 1000000) + +/* Reg value for the next higher ODR */ +#define ICM426XX_ODR_REG_UP(_reg) ((_reg) == 15 ? 6 : \ + (_reg) == 7 ? 15 : \ + (_reg) - 1) + +/* + * Register addresses are virtual address on 16 bits. + * MSB is coding register bank and LSB real register address. + * ex: bank 4, register 1F => 0x041F + */ +#define ICM426XX_REG_DEVICE_CONFIG 0x0011 +#define ICM426XX_SOFT_RESET_CONFIG BIT(0) + +enum icm426xx_slew_rate { + ICM426XX_SLEW_RATE_20NS_60NS, + ICM426XX_SLEW_RATE_12NS_36NS, + ICM426XX_SLEW_RATE_6NS_18NS, + ICM426XX_SLEW_RATE_4NS_12NS, + ICM426XX_SLEW_RATE_2NS_6NS, + ICM426XX_SLEW_RATE_INF_2NS, +}; +#define ICM426XX_REG_DRIVE_CONFIG 0x0013 +#define ICM426XX_DRIVE_CONFIG_MASK GENMASK(5, 0) +#define ICM426XX_I2C_SLEW_RATE(_s) (((_s) & 0x07) << 3) +#define ICM426XX_SPI_SLEW_RATE(_s) ((_s) & 0x07) + +/* default int configuration is pulsed mode, open drain, and active low */ +#define ICM426XX_REG_INT_CONFIG 0x0014 +#define ICM426XX_INT2_LATCHED BIT(5) +#define ICM426XX_INT2_PUSH_PULL BIT(4) +#define ICM426XX_INT2_ACTIVE_HIGH BIT(3) +#define ICM426XX_INT1_LATCHED BIT(2) +#define ICM426XX_INT1_PUSH_PULL BIT(1) +#define ICM426XX_INT1_ACTIVE_HIGH BIT(0) + +#define ICM426XX_REG_FIFO_CONFIG 0x0016 +#define ICM426XX_FIFO_MODE_BYPASS (0x00 << 6) +#define ICM426XX_FIFO_MODE_STREAM (0x01 << 6) +#define ICM426XX_FIFO_MODE_STOP_FULL (0x02 << 6) + +/* data are 16 bits */ +#define ICM426XX_REG_TEMP_DATA 0x001D +/* X + Y + Z: 3 * 16 bits */ +#define ICM426XX_REG_ACCEL_DATA_XYZ 0x001F +#define ICM426XX_REG_GYRO_DATA_XYZ 0x0025 + +#define ICM426XX_INVALID_DATA -32768 + +#define ICM426XX_REG_INT_STATUS 0x002D +#define ICM426XX_UI_FSYNC_INT BIT(6) +#define ICM426XX_PLL_RDY_INT BIT(5) +#define ICM426XX_RESET_DONE_INT BIT(4) +#define ICM426XX_DATA_RDY_INT BIT(3) +#define ICM426XX_FIFO_THS_INT BIT(2) +#define ICM426XX_FIFO_FULL_INT BIT(1) +#define ICM426XX_AGC_RDY_INT BIT(0) + +/* FIFO count is 16 bits */ +#define ICM426XX_REG_FIFO_COUNT 0x002E +#define ICM426XX_REG_FIFO_DATA 0x0030 + +#define ICM426XX_REG_SIGNAL_PATH_RESET 0x004B +#define ICM426XX_ABORT_AND_RESET BIT(3) +#define ICM426XX_TMST_STROBE BIT(2) +#define ICM426XX_FIFO_FLUSH BIT(1) + +#define ICM426XX_REG_INTF_CONFIG0 0x004C +#define ICM426XX_DATA_CONF_MASK GENMASK(7, 4) +#define ICM426XX_FIFO_HOLD_LAST_DATA BIT(7) +#define ICM426XX_FIFO_COUNT_REC BIT(6) +#define ICM426XX_FIFO_COUNT_BE BIT(5) +#define ICM426XX_SENSOR_DATA_BE BIT(4) +#define ICM426XX_UI_SIFS_CFG_MASK GENMASK(1, 0) +#define ICM426XX_UI_SIFS_CFG_SPI_DIS 0x02 +#define ICM426XX_UI_SIFS_CFG_I2C_DIS 0x03 + +enum icm426xx_sensor_mode { + ICM426XX_MODE_OFF, + ICM426XX_MODE_STANDBY, + ICM426XX_MODE_LOW_POWER, + ICM426XX_MODE_LOW_NOISE, +}; +#define ICM426XX_REG_PWR_MGMT0 0x004E +#define ICM426XX_TEMP_DIS BIT(5) +#define ICM426XX_IDLE BIT(4) +#define ICM426XX_GYRO_MODE_MASK GENMASK(3, 2) +#define ICM426XX_GYRO_MODE(_m) (((_m) & 0x03) << 2) +#define ICM426XX_ACCEL_MODE_MASK GENMASK(1, 0) +#define ICM426XX_ACCEL_MODE(_m) ((_m) & 0x03) + +#define ICM426XX_REG_GYRO_CONFIG0 0x004F +#define ICM426XX_REG_ACCEL_CONFIG0 0x0050 +#define ICM426XX_FS_MASK GENMASK(7, 5) +#define ICM426XX_FS_SEL(_fs) (((_fs) & 0x07) << 5) +#define ICM426XX_ODR_MASK GENMASK(3, 0) +#define ICM426XX_ODR(_odr) ((_odr) & 0x0F) + +enum icm426xx_filter_bw { + /* low noise mode */ + ICM426XX_FILTER_BW_ODR_DIV_2 = 0, + + /* low power mode */ + ICM426XX_FILTER_BW_AVG_1X = 1, + ICM426XX_FILTER_BW_AVG_16X = 6, +}; + +#define ICM426XX_REG_GYRO_ACCEL_CONFIG0 0x0052 +#define ICM426XX_ACCEL_UI_FILT_MASK GENMASK(7, 4) +#define ICM426XX_ACCEL_UI_FILT_BW(_f) (((_f) & 0x0F) << 4) +#define ICM426XX_GYRO_UI_FILT_MASK GENMASK(3, 0) +#define ICM426XX_GYRO_UI_FILT_BW(_f) ((_f) & 0x0F) + +#define ICM426XX_REG_FIFO_CONFIG1 0x005F +#define ICM426XX_FIFO_PARTIAL_READ BIT(6) +#define ICM426XX_FIFO_WM_GT_TH BIT(5) +#define ICM426XX_FIFO_EN_MASK GENMASK(3, 0) +#define ICM426XX_FIFO_TMST_FSYNC_EN BIT(3) +#define ICM426XX_FIFO_TEMP_EN BIT(2) +#define ICM426XX_FIFO_GYRO_EN BIT(1) +#define ICM426XX_FIFO_ACCEL_EN BIT(0) + +/* FIFO watermark value is 16 bits little endian */ +#define ICM426XX_REG_FIFO_WATERMARK 0x0060 + +#define ICM426XX_REG_INT_CONFIG1 0x0064 +#define ICM426XX_INT_PULSE_DURATION BIT(6) +#define ICM426XX_INT_TDEASSERT_DIS BIT(5) +#define ICM426XX_INT_ASYNC_RESET BIT(4) + +#define ICM426XX_REG_INT_SOURCE0 0x0065 +#define ICM426XX_UI_FSYNC_INT1_EN BIT(6) +#define ICM426XX_PLL_RDY_INT1_EN BIT(5) +#define ICM426XX_RESET_DONE_INT1_EN BIT(4) +#define ICM426XX_UI_DRDY_INT1_EN BIT(3) +#define ICM426XX_FIFO_THS_INT1_EN BIT(2) +#define ICM426XX_FIFO_FULL_INT1_EN BIT(1) +#define ICM426XX_UI_AGC_RDY_INT1_EN BIT(0) + +#define ICM426XX_REG_INT_SOURCE3 0x0068 +#define ICM426XX_UI_FSYNC_INT2_EN BIT(6) +#define ICM426XX_PLL_RDY_INT2_EN BIT(5) +#define ICM426XX_RESET_DONE_INT2_EN BIT(4) +#define ICM426XX_UI_DRDY_INT2_EN BIT(3) +#define ICM426XX_FIFO_THS_INT2_EN BIT(2) +#define ICM426XX_FIFO_FULL_INT2_EN BIT(1) +#define ICM426XX_UI_AGC_RDY_INT2_EN BIT(0) + +#define ICM426XX_REG_WHO_AM_I 0x0075 +#define ICM426XX_CHIP_ICM40608 0x39 +#define ICM426XX_CHIP_ICM42605 0x42 + +#define ICM426XX_REG_BANK_SEL 0x0076 +#define ICM426XX_BANK_SEL(_b) ((_b) & 0x07) + +#define ICM426XX_REG_INTF_CONFIG4 0x017A +#define ICM426XX_I3C_BUS_MODE BIT(6) +#define ICM426XX_SPI_AP_4WIRE BIT(1) + +#define ICM426XX_REG_INTF_CONFIG5 0x017B +#define ICM426XX_PIN9_FUNC_INT2 (0x00 << 1) +#define ICM426XX_PIN9_FUNC_FSYNC (0x01 << 1) + +#define ICM426XX_REG_INTF_CONFIG6 0x017C +#define ICM426XX_INTF_CONFIG6_MASK GENMASK(4, 0) +#define ICM426XX_I3C_EN BIT(4) +#define ICM426XX_I3C_IBI_BYTE_EN BIT(3) +#define ICM426XX_I3C_IBI_EN BIT(2) +#define ICM426XX_I3C_DDR_EN BIT(1) +#define ICM426XX_I3C_SDR_EN BIT(0) + +#define ICM426XX_REG_INT_SOURCE8 0x044F +#define ICM426XX_FSYNC_IBI_EN BIT(5) +#define ICM426XX_PLL_RDY_IBI_EN BIT(4) +#define ICM426XX_UI_DRDY_IBI_EN BIT(3) +#define ICM426XX_FIFO_THS_IBI_EN BIT(2) +#define ICM426XX_FIFO_FULL_IBI_EN BIT(1) +#define ICM426XX_AGC_RDY_IBI_EN BIT(0) + +#define ICM426XX_REG_OFFSET_USER0 0x0477 +#define ICM426XX_REG_OFFSET_USER1 0x0478 +#define ICM426XX_REG_OFFSET_USER2 0x0479 +#define ICM426XX_REG_OFFSET_USER3 0x047A +#define ICM426XX_REG_OFFSET_USER4 0x047B +#define ICM426XX_REG_OFFSET_USER5 0x047C +#define ICM426XX_REG_OFFSET_USER6 0x047D +#define ICM426XX_REG_OFFSET_USER7 0x047E +#define ICM426XX_REG_OFFSET_USER8 0x047F + +extern const struct accelgyro_drv icm426xx_drv; + +void icm426xx_interrupt(enum gpio_signal signal); + +#endif /* __CROS_EC_ACCELGYRO_ICM426XX_H */ diff --git a/driver/accelgyro_icm_common.c b/driver/accelgyro_icm_common.c new file mode 100644 index 0000000000..dc3abd71f2 --- /dev/null +++ b/driver/accelgyro_icm_common.c @@ -0,0 +1,296 @@ +/* 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 accelerometer and gyroscope module for Chrome EC + * 3D digital accelerometer & 3D digital gyroscope + */ + +#include "accelgyro.h" +#include "console.h" +#include "i2c.h" +#include "spi.h" +#include "driver/accelgyro_icm_common.h" +#include "driver/accelgyro_icm426xx.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_SPI_ACCEL_PORT +static int icm_spi_raw_read(const int addr, const uint8_t reg, + uint8_t *data, const int len) +{ + uint8_t cmd = 0x80 | reg; + + return spi_transaction(&spi_devices[addr], &cmd, 1, data, len); +} + +static int icm_spi_raw_write(const int addr, const uint8_t reg, + const uint8_t *data, const int len) +{ + uint8_t cmd[3]; + int i; + + if (len > 2) + return EC_ERROR_UNIMPLEMENTED; + + cmd[0] = reg; + for (i = 0; i < len; ++i) + cmd[i + 1] = data[i]; + + return spi_transaction(&spi_devices[addr], cmd, len + 1, NULL, 0); +} +#endif + +static int icm_bank_sel(const struct motion_sensor_t *s, const int reg) +{ + struct icm_drv_data_t *st = ICM_GET_DATA(s); + uint8_t bank = ICM426XX_REG_GET_BANK(reg); + int ret; + + if (bank == st->bank) + return EC_SUCCESS; + + ret = EC_ERROR_UNIMPLEMENTED; + ret = i2c_write8(s->port, s->i2c_spi_addr_flags, + ICM426XX_REG_BANK_SEL, bank); + + if (ret == EC_SUCCESS) + st->bank = bank; + + return ret; +} + +/** + * Read 8 bits register + */ +int icm_read8(const struct motion_sensor_t *s, const int reg, int *data_ptr) +{ + const uint8_t addr = ICM426XX_REG_GET_ADDR(reg); + int ret; + + ret = icm_bank_sel(s, reg); + if (ret != EC_SUCCESS) + return ret; + + ret = EC_ERROR_UNIMPLEMENTED; + ret = i2c_read8(s->port, s->i2c_spi_addr_flags, addr, data_ptr); + + return ret; +} + +/** + * Write 8 bits register + */ +int icm_write8(const struct motion_sensor_t *s, const int reg, int data) +{ + const uint8_t addr = ICM426XX_REG_GET_ADDR(reg); + int ret; + + ret = icm_bank_sel(s, reg); + if (ret != EC_SUCCESS) + return ret; + + ret = EC_ERROR_UNIMPLEMENTED; + ret = i2c_write8(s->port, s->i2c_spi_addr_flags, addr, data); + + return ret; +} + +/** + * Read 16 bits register + */ +int icm_read16(const struct motion_sensor_t *s, const int reg, int *data_ptr) +{ + const uint8_t addr = ICM426XX_REG_GET_ADDR(reg); + int ret; + + ret = icm_bank_sel(s, reg); + if (ret != EC_SUCCESS) + return ret; + + ret = EC_ERROR_UNIMPLEMENTED; + ret = i2c_read16(s->port, s->i2c_spi_addr_flags, + addr, data_ptr); + + return ret; +} + +/** + * Write 16 bits register + */ +int icm_write16(const struct motion_sensor_t *s, const int reg, int data) +{ + const uint8_t addr = ICM426XX_REG_GET_ADDR(reg); + int ret; + + ret = icm_bank_sel(s, reg); + if (ret != EC_SUCCESS) + return ret; + + ret = EC_ERROR_UNIMPLEMENTED; + ret = i2c_write16(s->port, s->i2c_spi_addr_flags, addr, data); + + return ret; +} + +/** + * Read n bytes + */ +int icm_read_n(const struct motion_sensor_t *s, const int reg, + uint8_t *data_ptr, const int len) +{ + const uint8_t addr = ICM426XX_REG_GET_ADDR(reg); + int ret; + + ret = icm_bank_sel(s, reg); + if (ret != EC_SUCCESS) + return ret; + + ret = EC_ERROR_UNIMPLEMENTED; + ret = i2c_read_block(s->port, s->i2c_spi_addr_flags, addr, + data_ptr, len); + + return ret; +} + +int icm_field_update8(const struct motion_sensor_t *s, const int reg, + const uint8_t field_mask, const uint8_t set_value) +{ + const uint8_t addr = ICM426XX_REG_GET_ADDR(reg); + int ret; + + ret = icm_bank_sel(s, reg); + if (ret != EC_SUCCESS) + return ret; + + ret = EC_ERROR_UNIMPLEMENTED; + ret = i2c_field_update8(s->port, s->i2c_spi_addr_flags, addr, + field_mask, set_value); + + return ret; +} + +int icm_get_resolution(const struct motion_sensor_t *s) +{ + return ICM_RESOLUTION; +} + +int icm_get_range(const struct motion_sensor_t *s) +{ + struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s); + + return data->range; +} + +int icm_get_data_rate(const struct motion_sensor_t *s) +{ + struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s); + + return data->odr; +} + +int icm_set_scale(const struct motion_sensor_t *s, const uint16_t *scale, + int16_t temp) +{ + struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s); + + data->scale[X] = scale[X]; + data->scale[Y] = scale[Y]; + data->scale[Z] = scale[Z]; + return EC_SUCCESS; +} + +int icm_get_scale(const struct motion_sensor_t *s, uint16_t *scale, + int16_t *temp) +{ + struct accelgyro_saved_data_t *data = ICM_GET_SAVED_DATA(s); + + scale[X] = data->scale[X]; + scale[Y] = data->scale[Y]; + scale[Z] = data->scale[Z]; + *temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP; + return EC_SUCCESS; +} + +/* FIFO header: 1 byte */ +#define ICM_FIFO_HEADER_MSG BIT(7) +#define ICM_FIFO_HEADER_ACCEL BIT(6) +#define ICM_FIFO_HEADER_GYRO BIT(5) +#define ICM_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2) +#define ICM_FIFO_HEADER_ODR_ACCEL BIT(1) +#define ICM_FIFO_HEADER_ODR_GYRO BIT(0) + +/* FIFO data packet */ +struct icm_fifo_sensor_data { + int16_t x; + int16_t y; + int16_t z; +} __packed; + +struct icm_fifo_1sensor_packet { + uint8_t header; + struct icm_fifo_sensor_data data; + int8_t temp; +} __packed; +#define ICM_FIFO_1SENSOR_PACKET_SIZE 8 + +struct icm_fifo_2sensors_packet { + uint8_t header; + struct icm_fifo_sensor_data accel; + struct icm_fifo_sensor_data gyro; + int8_t temp; + uint16_t timestamp; +} __packed; +#define ICM_FIFO_2SENSORS_PACKET_SIZE 16 + +ssize_t icm_fifo_decode_packet(const void *packet, const uint8_t **accel, + const uint8_t **gyro) +{ + const struct icm_fifo_1sensor_packet *pack1 = packet; + const struct icm_fifo_2sensors_packet *pack2 = packet; + uint8_t header = *((const uint8_t *)packet); + + /* FIFO empty */ + if (header & ICM_FIFO_HEADER_MSG) { + if (accel != NULL) + *accel = NULL; + if (gyro != NULL) + *gyro = NULL; + return 0; + } + + /* accel + gyro */ + if ((header & ICM_FIFO_HEADER_ACCEL) && + (header & ICM_FIFO_HEADER_GYRO)) { + if (accel != NULL) + *accel = (uint8_t *)&pack2->accel; + if (gyro != NULL) + *gyro = (uint8_t *)&pack2->gyro; + return ICM_FIFO_2SENSORS_PACKET_SIZE; + } + + /* accel only */ + if (header & ICM_FIFO_HEADER_ACCEL) { + if (accel != NULL) + *accel = (uint8_t *)&pack1->data; + if (gyro != NULL) + *gyro = NULL; + return ICM_FIFO_1SENSOR_PACKET_SIZE; + } + + /* gyro only */ + if (header & ICM_FIFO_HEADER_GYRO) { + if (accel != NULL) + *accel = NULL; + if (gyro != NULL) + *gyro = (uint8_t *)&pack1->data; + return ICM_FIFO_1SENSOR_PACKET_SIZE; + } + + /* invalid packet if here */ + return -EC_ERROR_INVAL; +} diff --git a/driver/accelgyro_icm_common.h b/driver/accelgyro_icm_common.h new file mode 100644 index 0000000000..4d991ce341 --- /dev/null +++ b/driver/accelgyro_icm_common.h @@ -0,0 +1,101 @@ +/* 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 accelerometer and gyroscope common definitions for Chrome EC */ + +#ifndef __CROS_EC_ACCELGYRO_ICM_COMMON_H +#define __CROS_EC_ACCELGYRO_ICM_COMMON_H + +#include "accelgyro.h" + +#ifdef CONFIG_ACCEL_FIFO +/* reserve maximum 4 samples of 16 bytes */ +#define ICM_FIFO_BUFFER 64 +#else +#define ICM_FIFO_BUFFER 0 +#endif + +struct icm_drv_data_t { + struct accelgyro_saved_data_t saved_data[2]; + struct motion_sensor_t *accel; + struct motion_sensor_t *gyro; + uint8_t bank; + uint8_t fifo_en; + uint8_t fifo_buffer[ICM_FIFO_BUFFER] __aligned(sizeof(long)); +}; + +#define ICM_GET_DATA(_s) \ + ((struct icm_drv_data_t *)(_s)->drv_data) +#define ICM_GET_SAVED_DATA(_s) \ + (&ICM_GET_DATA(_s)->saved_data[(_s)->type]) + +/* + * Virtual register address is 16 bits: + * - 8 bits MSB coding bank number + * - 8 bits LSB coding physical address + */ +#define ICM426XX_REG_GET_BANK(_r) (((_r) & 0xFF00) >> 8) +#define ICM426XX_REG_GET_ADDR(_r) ((_r) & 0x00FF) + +/* Sensor resolution in number of bits */ +#define ICM_RESOLUTION 16 + +/** + * sign_extend - sign extend a standard int value using the given sign-bit + * @value: value to sign extend + * @index: 0 based bit index to sign bit + */ +static inline int sign_extend(int value, int index) +{ + int shift = (sizeof(int) * 8) - index - 1; + + return (int)(value << shift) >> shift; +} + +/** + * Read 8 bits register + */ +int icm_read8(const struct motion_sensor_t *s, const int reg, int *data_ptr); + +/** + * Write 8 bits register + */ +int icm_write8(const struct motion_sensor_t *s, const int reg, int data); + +/** + * Read 16 bits register + */ +int icm_read16(const struct motion_sensor_t *s, const int reg, int *data_ptr); + +/** + * Write 16 bits register + */ +int icm_write16(const struct motion_sensor_t *s, const int reg, int data); + +/** + * Read n bytes + */ +int icm_read_n(const struct motion_sensor_t *s, const int reg, + uint8_t *data_ptr, const int len); + +int icm_field_update8(const struct motion_sensor_t *s, const int reg, + const uint8_t field_mask, const uint8_t set_value); + +int icm_get_resolution(const struct motion_sensor_t *s); + +int icm_get_range(const struct motion_sensor_t *s); + +int icm_get_data_rate(const struct motion_sensor_t *s); + +int icm_set_scale(const struct motion_sensor_t *s, const uint16_t *scale, + int16_t temp); + +int icm_get_scale(const struct motion_sensor_t *s, uint16_t *scale, + int16_t *temp); + +ssize_t icm_fifo_decode_packet(const void *packet, const uint8_t **accel, + const uint8_t **gyro); + +#endif /* __CROS_EC_ACCELGYRO_ICM_COMMON_H */ diff --git a/driver/build.mk b/driver/build.mk index b33da44eb5..e1e39c7a5b 100644 --- a/driver/build.mk +++ b/driver/build.mk @@ -20,6 +20,7 @@ driver-$(CONFIG_MAG_LIS2MDL)+=mag_lis2mdl.o driver-$(CONFIG_SENSORHUB_LSM6DSM)+=sensorhub_lsm6dsm.o driver-$(CONFIG_SYNC)+=sync.o driver-$(CONFIG_ACCEL_LIS2DW_COMMON)+=accel_lis2dw12.o stm_mems_common.o +driver-$(CONFIG_ACCELGYRO_ICM426XX)+=accelgyro_icm426xx.o accelgyro_icm_common.o # BC1.2 Charger Detection Devices driver-$(CONFIG_BC12_DETECT_MAX14637)+=bc12/max14637.o diff --git a/include/accelgyro.h b/include/accelgyro.h index 2d415621d1..71127e46bd 100644 --- a/include/accelgyro.h +++ b/include/accelgyro.h @@ -152,6 +152,9 @@ struct accelgyro_saved_data_t { uint16_t scale[3]; }; +#define SENSOR_APPLY_DIV_SCALE(_input, _scale) \ + (((_input) * (uint64_t)MOTION_SENSE_DEFAULT_SCALE) / (_scale)) + #define SENSOR_APPLY_SCALE(_input, _scale) \ (((_input) * (_scale)) / MOTION_SENSE_DEFAULT_SCALE) diff --git a/include/common.h b/include/common.h index 13101fd1f4..db08216e05 100644 --- a/include/common.h +++ b/include/common.h @@ -124,6 +124,11 @@ #endif #endif +/* Macros for combining bytes into uint16s. */ +#define UINT16_FROM_BYTES(lsb, msb) ((lsb) | (msb) << 8) +#define UINT16_FROM_BYTE_ARRAY_LE(data, lsb_index) \ + UINT16_FROM_BYTES((data)[(lsb_index)], (data)[(lsb_index + 1)]) + /* There isn't really a better place for this */ #define C_TO_K(temp_c) ((temp_c) + 273) #define K_TO_C(temp_c) ((temp_c) - 273) diff --git a/include/config.h b/include/config.h index 3d7f3dad4e..e6a72b0d7f 100644 --- a/include/config.h +++ b/include/config.h @@ -99,6 +99,7 @@ #undef CONFIG_ACCEL_LIS2DW_AS_BASE #undef CONFIG_ACCELGYRO_BMI160 +#undef CONFIG_ACCELGYRO_ICM426XX #undef CONFIG_ACCELGYRO_LSM6DS0 /* Use CONFIG_ACCELGYRO_LSM6DSM for LSM6DSL, LSM6DSM, and/or LSM6DS3 */ #undef CONFIG_ACCELGYRO_LSM6DSM @@ -270,6 +271,7 @@ * Must be within TASK_EVENT_MOTION_INTERRUPT_MASK. */ #undef CONFIG_ACCELGYRO_BMI160_INT_EVENT +#undef CONFIG_ACCELGYRO_ICM426XX_INT_EVENT #undef CONFIG_ACCEL_LSM6DSM_INT_EVENT #undef CONFIG_ACCEL_LSM6DSO_INT_EVENT #undef CONFIG_ACCEL_LIS2DW12_INT_EVENT -- cgit v1.2.1