diff options
Diffstat (limited to 'drivers/iio/imu')
25 files changed, 5617 insertions, 33 deletions
diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig index 15612f0f189b..7e0181c27bb6 100644 --- a/drivers/iio/imu/Kconfig +++ b/drivers/iio/imu/Kconfig @@ -109,6 +109,7 @@ config KMX61 be called kmx61. source "drivers/iio/imu/inv_icm42600/Kconfig" +source "drivers/iio/imu/inv_icm45600/Kconfig" source "drivers/iio/imu/inv_mpu6050/Kconfig" config SMI240 @@ -124,6 +125,7 @@ config SMI240 This driver can also be built as a module. If so, the module will be called smi240. +source "drivers/iio/imu/smi330/Kconfig" source "drivers/iio/imu/st_lsm6dsx/Kconfig" source "drivers/iio/imu/st_lsm9ds0/Kconfig" diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile index e901aea498d3..13fb7846e9c9 100644 --- a/drivers/iio/imu/Makefile +++ b/drivers/iio/imu/Makefile @@ -25,11 +25,13 @@ obj-$(CONFIG_FXOS8700_I2C) += fxos8700_i2c.o obj-$(CONFIG_FXOS8700_SPI) += fxos8700_spi.o obj-y += inv_icm42600/ +obj-y += inv_icm45600/ obj-y += inv_mpu6050/ obj-$(CONFIG_KMX61) += kmx61.o obj-$(CONFIG_SMI240) += smi240.o +obj-y += smi330/ obj-y += st_lsm6dsx/ obj-y += st_lsm9ds0/ diff --git a/drivers/iio/imu/bmi270/bmi270_core.c b/drivers/iio/imu/bmi270/bmi270_core.c index 519f1c9d466d..2ad230788532 100644 --- a/drivers/iio/imu/bmi270/bmi270_core.c +++ b/drivers/iio/imu/bmi270/bmi270_core.c @@ -31,6 +31,8 @@ #define BMI270_INT_STATUS_0_REG 0x1c #define BMI270_INT_STATUS_0_STEP_CNT_MSK BIT(1) +#define BMI270_INT_STATUS_0_NOMOTION_MSK BIT(5) +#define BMI270_INT_STATUS_0_MOTION_MSK BIT(6) #define BMI270_INT_STATUS_1_REG 0x1d #define BMI270_INT_STATUS_1_ACC_GYR_DRDY_MSK GENMASK(7, 6) @@ -81,6 +83,8 @@ #define BMI270_INT1_MAP_FEAT_REG 0x56 #define BMI270_INT2_MAP_FEAT_REG 0x57 #define BMI270_INT_MAP_FEAT_STEP_CNT_WTRMRK_MSK BIT(1) +#define BMI270_INT_MAP_FEAT_NOMOTION_MSK BIT(5) +#define BMI270_INT_MAP_FEAT_ANYMOTION_MSK BIT(6) #define BMI270_INT_MAP_DATA_REG 0x58 #define BMI270_INT_MAP_DATA_DRDY_INT1_MSK BIT(2) @@ -106,6 +110,25 @@ #define BMI270_STEP_SC26_RST_CNT_MSK BIT(10) #define BMI270_STEP_SC26_EN_CNT_MSK BIT(12) +#define BMI270_FEAT_MOTION_DURATION_MSK GENMASK(12, 0) +#define BMI270_FEAT_MOTION_X_EN_MSK BIT(13) +#define BMI270_FEAT_MOTION_Y_EN_MSK BIT(14) +#define BMI270_FEAT_MOTION_Z_EN_MSK BIT(15) +#define BMI270_FEAT_MOTION_XYZ_EN_MSK GENMASK(15, 13) +#define BMI270_FEAT_MOTION_THRESHOLD_MSK GENMASK(10, 0) +#define BMI270_FEAT_MOTION_OUT_CONF_MSK GENMASK(14, 11) +#define BMI270_FEAT_MOTION_ENABLE_MSK BIT(15) + +#define BMI270_MOTION_XYZ_MSK GENMASK(2, 0) + +/* See pages 92 and 93 of the datasheet */ +#define BMI270_MOTION_THRES_FULL_SCALE GENMASK(10, 0) +#define BMI270_MOTION_DURAT_SCALE 50 +#define BMI270_MOTION_DURAT_MAX 162 + +/* 9.81 * 1000000 m/s^2 */ +#define BMI270_G_MICRO_M_S_2 9810000 + /* See datasheet section 4.6.14, Temperature Sensor */ #define BMI270_TEMP_OFFSET 11776 #define BMI270_TEMP_SCALE 1953125 @@ -114,6 +137,11 @@ #define BMI270_STEP_COUNTER_FACTOR 20 #define BMI270_STEP_COUNTER_MAX 20460 +#define BMI270_INT_MICRO_TO_RAW(val, val2, scale) \ + ((val) * (scale) + ((val2) * (scale)) / MEGA) +#define BMI270_RAW_TO_MICRO(raw, scale) \ + ((((raw) % (scale)) * MEGA) / scale) + #define BMI260_INIT_DATA_FILE "bmi260-init-data.fw" #define BMI270_INIT_DATA_FILE "bmi270-init-data.fw" @@ -309,6 +337,13 @@ static const struct bmi270_odr_item bmi270_odr_table[] = { }; enum bmi270_feature_reg_id { + /* Page 1 registers */ + BMI270_ANYMO1_REG, + BMI270_ANYMO2_REG, + /* Page 2 registers */ + BMI270_NOMO1_REG, + BMI270_NOMO2_REG, + /* Page 6 registers */ BMI270_SC_26_REG, }; @@ -318,6 +353,22 @@ struct bmi270_feature_reg { }; static const struct bmi270_feature_reg bmi270_feature_regs[] = { + [BMI270_ANYMO1_REG] = { + .page = 1, + .addr = 0x3c, + }, + [BMI270_ANYMO2_REG] = { + .page = 1, + .addr = 0x3e, + }, + [BMI270_NOMO1_REG] = { + .page = 2, + .addr = 0x30, + }, + [BMI270_NOMO2_REG] = { + .page = 2, + .addr = 0x32, + }, [BMI270_SC_26_REG] = { .page = 6, .addr = 0x32, @@ -439,6 +490,121 @@ static int bmi270_step_wtrmrk_en(struct bmi270_data *data, bool state) state)); } +static int bmi270_motion_reg(enum iio_event_type type, enum iio_event_info info) +{ + switch (info) { + case IIO_EV_INFO_PERIOD: + switch (type) { + case IIO_EV_TYPE_MAG_ADAPTIVE: + return BMI270_ANYMO1_REG; + case IIO_EV_TYPE_ROC: + return BMI270_NOMO1_REG; + default: + return -EINVAL; + } + case IIO_EV_INFO_VALUE: + switch (type) { + case IIO_EV_TYPE_MAG_ADAPTIVE: + return BMI270_ANYMO2_REG; + case IIO_EV_TYPE_ROC: + return BMI270_NOMO2_REG; + default: + return -EINVAL; + } + default: + return -EINVAL; + } +} + +static int bmi270_anymotion_event_en(struct bmi270_data *data, + struct iio_chan_spec const *chan, + bool state) +{ + u16 axis_msk, axis_field_val, regval; + int ret, irq_reg; + bool axis_en; + + irq_reg = bmi270_int_map_reg(data->irq_pin); + if (irq_reg < 0) + return irq_reg; + + guard(mutex)(&data->mutex); + + ret = bmi270_read_feature_reg(data, BMI270_ANYMO1_REG, ®val); + if (ret) + return ret; + + switch (chan->channel2) { + case IIO_MOD_X: + axis_msk = BMI270_FEAT_MOTION_X_EN_MSK; + axis_field_val = FIELD_PREP(BMI270_FEAT_MOTION_X_EN_MSK, state); + axis_en = FIELD_GET(BMI270_FEAT_MOTION_Y_EN_MSK, regval) | + FIELD_GET(BMI270_FEAT_MOTION_Z_EN_MSK, regval); + break; + case IIO_MOD_Y: + axis_msk = BMI270_FEAT_MOTION_Y_EN_MSK; + axis_field_val = FIELD_PREP(BMI270_FEAT_MOTION_Y_EN_MSK, state); + axis_en = FIELD_GET(BMI270_FEAT_MOTION_X_EN_MSK, regval) | + FIELD_GET(BMI270_FEAT_MOTION_Z_EN_MSK, regval); + break; + case IIO_MOD_Z: + axis_msk = BMI270_FEAT_MOTION_Z_EN_MSK; + axis_field_val = FIELD_PREP(BMI270_FEAT_MOTION_Z_EN_MSK, state); + axis_en = FIELD_GET(BMI270_FEAT_MOTION_X_EN_MSK, regval) | + FIELD_GET(BMI270_FEAT_MOTION_Y_EN_MSK, regval); + break; + default: + return -EINVAL; + } + + ret = bmi270_update_feature_reg(data, BMI270_ANYMO1_REG, axis_msk, + axis_field_val); + if (ret) + return ret; + + ret = bmi270_update_feature_reg(data, BMI270_ANYMO2_REG, + BMI270_FEAT_MOTION_ENABLE_MSK, + FIELD_PREP(BMI270_FEAT_MOTION_ENABLE_MSK, + state || axis_en)); + if (ret) + return ret; + + return regmap_update_bits(data->regmap, irq_reg, + BMI270_INT_MAP_FEAT_ANYMOTION_MSK, + FIELD_PREP(BMI270_INT_MAP_FEAT_ANYMOTION_MSK, + state || axis_en)); +} + +static int bmi270_nomotion_event_en(struct bmi270_data *data, bool state) +{ + int ret, irq_reg; + + irq_reg = bmi270_int_map_reg(data->irq_pin); + if (irq_reg < 0) + return irq_reg; + + guard(mutex)(&data->mutex); + + ret = bmi270_update_feature_reg(data, BMI270_NOMO1_REG, + BMI270_FEAT_MOTION_XYZ_EN_MSK, + FIELD_PREP(BMI270_FEAT_MOTION_XYZ_EN_MSK, + state ? BMI270_MOTION_XYZ_MSK : 0)); + if (ret) + return ret; + + ret = bmi270_update_feature_reg(data, BMI270_NOMO2_REG, + BMI270_FEAT_MOTION_ENABLE_MSK, + FIELD_PREP(BMI270_FEAT_MOTION_ENABLE_MSK, + state)); + if (ret) + return ret; + + return regmap_update_bits(data->regmap, irq_reg, + BMI270_INT_MAP_FEAT_NOMOTION_MSK, + FIELD_PREP(BMI270_INT_MAP_FEAT_NOMOTION_MSK, + state)); +} + static int bmi270_set_scale(struct bmi270_data *data, int chan_type, int uscale) { int i; @@ -479,8 +645,6 @@ static int bmi270_get_scale(struct bmi270_data *data, int chan_type, int *scale, unsigned int val; struct bmi270_scale_item bmi270_scale_item; - guard(mutex)(&data->mutex); - switch (chan_type) { case IIO_ACCEL: ret = regmap_read(data->regmap, BMI270_ACC_CONF_RANGE_REG, &val); @@ -614,6 +778,20 @@ static irqreturn_t bmi270_irq_thread_handler(int irq, void *private) if (FIELD_GET(BMI270_INT_STATUS_1_ACC_GYR_DRDY_MSK, status1)) iio_trigger_poll_nested(data->trig); + if (FIELD_GET(BMI270_INT_STATUS_0_MOTION_MSK, status0)) + iio_push_event(indio_dev, IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, + IIO_MOD_X_OR_Y_OR_Z, + IIO_EV_TYPE_MAG_ADAPTIVE, + IIO_EV_DIR_RISING), + timestamp); + + if (FIELD_GET(BMI270_INT_STATUS_0_NOMOTION_MSK, status0)) + iio_push_event(indio_dev, IIO_MOD_EVENT_CODE(IIO_ACCEL, 0, + IIO_MOD_X_AND_Y_AND_Z, + IIO_EV_TYPE_ROC, + IIO_EV_DIR_RISING), + timestamp); + if (FIELD_GET(BMI270_INT_STATUS_0_STEP_CNT_MSK, status0)) iio_push_event(indio_dev, IIO_UNMOD_EVENT_CODE(IIO_STEPS, 0, IIO_EV_TYPE_CHANGE, @@ -827,6 +1005,39 @@ static int bmi270_read_avail(struct iio_dev *indio_dev, } } +static ssize_t in_accel_value_available_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct bmi270_data *data = iio_priv(indio_dev); + int ret, scale, uscale; + unsigned int step, max; + + ret = bmi270_get_scale(data, IIO_ACCEL, &scale, &uscale); + if (ret) + return ret; + + max = BMI270_G_MICRO_M_S_2 / uscale; + step = max / BMI270_MOTION_THRES_FULL_SCALE; + + return sysfs_emit(buf, "[0 %u %u]\n", step, max); +} + +static IIO_DEVICE_ATTR_RO(in_accel_value_available, 0); + +static IIO_CONST_ATTR(in_accel_period_available, "[0.0 0.02 162.0]"); + +static struct attribute *bmi270_event_attributes[] = { + &iio_dev_attr_in_accel_value_available.dev_attr.attr, + &iio_const_attr_in_accel_period_available.dev_attr.attr, + NULL +}; + +static const struct attribute_group bmi270_event_attribute_group = { + .attrs = bmi270_event_attributes, +}; + static int bmi270_write_event_config(struct iio_dev *indio_dev, const struct iio_chan_spec *chan, enum iio_event_type type, @@ -835,6 +1046,10 @@ static int bmi270_write_event_config(struct iio_dev *indio_dev, struct bmi270_data *data = iio_priv(indio_dev); switch (type) { + case IIO_EV_TYPE_MAG_ADAPTIVE: + return bmi270_anymotion_event_en(data, chan, state); + case IIO_EV_TYPE_ROC: + return bmi270_nomotion_event_en(data, state); case IIO_EV_TYPE_CHANGE: return bmi270_step_wtrmrk_en(data, state); default: @@ -848,21 +1063,55 @@ static int bmi270_read_event_config(struct iio_dev *indio_dev, enum iio_event_direction dir) { struct bmi270_data *data = iio_priv(indio_dev); + bool feat_en, axis_en; int ret, reg, regval; + u16 motion_reg; guard(mutex)(&data->mutex); + reg = bmi270_int_map_reg(data->irq_pin); + if (reg < 0) + return reg; + + ret = regmap_read(data->regmap, reg, ®val); + if (ret) + return ret; + switch (chan->type) { case IIO_STEPS: - reg = bmi270_int_map_reg(data->irq_pin); - if (reg) - return reg; - - ret = regmap_read(data->regmap, reg, ®val); - if (ret) - return ret; - return FIELD_GET(BMI270_INT_MAP_FEAT_STEP_CNT_WTRMRK_MSK, - regval) ? 1 : 0; + return !!FIELD_GET(BMI270_INT_MAP_FEAT_STEP_CNT_WTRMRK_MSK, regval); + case IIO_ACCEL: + switch (type) { + case IIO_EV_TYPE_ROC: + return !!FIELD_GET(BMI270_INT_MAP_FEAT_NOMOTION_MSK, regval); + case IIO_EV_TYPE_MAG_ADAPTIVE: + ret = bmi270_read_feature_reg(data, BMI270_ANYMO1_REG, + &motion_reg); + if (ret) + return ret; + + feat_en = FIELD_GET(BMI270_INT_MAP_FEAT_ANYMOTION_MSK, + regval); + switch (chan->channel2) { + case IIO_MOD_X: + axis_en = FIELD_GET(BMI270_FEAT_MOTION_X_EN_MSK, + motion_reg); + break; + case IIO_MOD_Y: + axis_en = FIELD_GET(BMI270_FEAT_MOTION_Y_EN_MSK, + motion_reg); + break; + case IIO_MOD_Z: + axis_en = FIELD_GET(BMI270_FEAT_MOTION_Z_EN_MSK, + motion_reg); + break; + default: + return -EINVAL; + } + return axis_en && feat_en; + default: + return -EINVAL; + } default: return -EINVAL; } @@ -876,20 +1125,50 @@ static int bmi270_write_event_value(struct iio_dev *indio_dev, int val, int val2) { struct bmi270_data *data = iio_priv(indio_dev); - unsigned int raw; + unsigned int raw, mask, regval; + int ret, reg, scale, uscale; + u64 tmp; guard(mutex)(&data->mutex); - switch (type) { - case IIO_EV_TYPE_CHANGE: + if (type == IIO_EV_TYPE_CHANGE) { if (!in_range(val, 0, BMI270_STEP_COUNTER_MAX + 1)) return -EINVAL; raw = val / BMI270_STEP_COUNTER_FACTOR; - return bmi270_update_feature_reg(data, BMI270_SC_26_REG, - BMI270_STEP_SC26_WTRMRK_MSK, - FIELD_PREP(BMI270_STEP_SC26_WTRMRK_MSK, - raw)); + mask = BMI270_STEP_SC26_WTRMRK_MSK; + regval = FIELD_PREP(BMI270_STEP_SC26_WTRMRK_MSK, raw); + return bmi270_update_feature_reg(data, BMI270_SC_26_REG, mask, + regval); + } + + reg = bmi270_motion_reg(type, info); + if (reg < 0) + return reg; + + switch (info) { + case IIO_EV_INFO_VALUE: + ret = bmi270_get_scale(data, IIO_ACCEL, &scale, &uscale); + if (ret) + return ret; + + if (!in_range(val, 0, (BMI270_G_MICRO_M_S_2 / uscale) + 1)) + return -EINVAL; + + tmp = (u64)val * BMI270_MOTION_THRES_FULL_SCALE * uscale; + raw = DIV_ROUND_CLOSEST_ULL(tmp, BMI270_G_MICRO_M_S_2); + mask = BMI270_FEAT_MOTION_THRESHOLD_MSK; + regval = FIELD_PREP(BMI270_FEAT_MOTION_THRESHOLD_MSK, raw); + return bmi270_update_feature_reg(data, reg, mask, regval); + case IIO_EV_INFO_PERIOD: + if (!in_range(val, 0, BMI270_MOTION_DURAT_MAX + 1)) + return -EINVAL; + + raw = BMI270_INT_MICRO_TO_RAW(val, val2, + BMI270_MOTION_DURAT_SCALE); + mask = BMI270_FEAT_MOTION_DURATION_MSK; + regval = FIELD_PREP(BMI270_FEAT_MOTION_DURATION_MSK, raw); + return bmi270_update_feature_reg(data, reg, mask, regval); default: return -EINVAL; } @@ -903,14 +1182,14 @@ static int bmi270_read_event_value(struct iio_dev *indio_dev, int *val, int *val2) { struct bmi270_data *data = iio_priv(indio_dev); + int ret, reg, scale, uscale; unsigned int raw; u16 regval; - int ret; + u64 tmp; guard(mutex)(&data->mutex); - switch (type) { - case IIO_EV_TYPE_CHANGE: + if (type == IIO_EV_TYPE_CHANGE) { ret = bmi270_read_feature_reg(data, BMI270_SC_26_REG, ®val); if (ret) return ret; @@ -918,6 +1197,36 @@ static int bmi270_read_event_value(struct iio_dev *indio_dev, raw = FIELD_GET(BMI270_STEP_SC26_WTRMRK_MSK, regval); *val = raw * BMI270_STEP_COUNTER_FACTOR; return IIO_VAL_INT; + } + + reg = bmi270_motion_reg(type, info); + if (reg < 0) + return reg; + + switch (info) { + case IIO_EV_INFO_VALUE: + ret = bmi270_read_feature_reg(data, reg, ®val); + if (ret) + return ret; + + ret = bmi270_get_scale(data, IIO_ACCEL, &scale, &uscale); + if (ret) + return ret; + + raw = FIELD_GET(BMI270_FEAT_MOTION_THRESHOLD_MSK, regval); + tmp = (u64)raw * BMI270_G_MICRO_M_S_2; + *val = DIV_ROUND_CLOSEST_ULL(tmp, + BMI270_MOTION_THRES_FULL_SCALE * uscale); + return IIO_VAL_INT; + case IIO_EV_INFO_PERIOD: + ret = bmi270_read_feature_reg(data, reg, ®val); + if (ret) + return ret; + + raw = FIELD_GET(BMI270_FEAT_MOTION_DURATION_MSK, regval); + *val = raw / BMI270_MOTION_DURAT_SCALE; + *val2 = BMI270_RAW_TO_MICRO(raw, BMI270_MOTION_DURAT_SCALE); + return IIO_VAL_INT_PLUS_MICRO; default: return -EINVAL; } @@ -929,6 +1238,20 @@ static const struct iio_event_spec bmi270_step_wtrmrk_event = { .mask_shared_by_type = BIT(IIO_EV_INFO_ENABLE) | BIT(IIO_EV_INFO_VALUE), }; +static const struct iio_event_spec bmi270_anymotion_event = { + .type = IIO_EV_TYPE_MAG_ADAPTIVE, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), + .mask_shared_by_type = BIT(IIO_EV_INFO_VALUE) | BIT(IIO_EV_INFO_PERIOD), +}; + +static const struct iio_event_spec bmi270_nomotion_event = { + .type = IIO_EV_TYPE_ROC, + .dir = IIO_EV_DIR_RISING, + .mask_separate = BIT(IIO_EV_INFO_ENABLE), + .mask_shared_by_type = BIT(IIO_EV_INFO_VALUE) | BIT(IIO_EV_INFO_PERIOD), +}; + static const struct iio_info bmi270_info = { .read_raw = bmi270_read_raw, .write_raw = bmi270_write_raw, @@ -937,6 +1260,7 @@ static const struct iio_info bmi270_info = { .read_event_config = bmi270_read_event_config, .write_event_value = bmi270_write_event_value, .read_event_value = bmi270_read_event_value, + .event_attrs = &bmi270_event_attribute_group, }; #define BMI270_ACCEL_CHANNEL(_axis) { \ @@ -956,6 +1280,8 @@ static const struct iio_info bmi270_info = { .storagebits = 16, \ .endianness = IIO_LE, \ }, \ + .event_spec = &bmi270_anymotion_event, \ + .num_event_specs = 1, \ } #define BMI270_ANG_VEL_CHANNEL(_axis) { \ @@ -1000,6 +1326,14 @@ static const struct iio_chan_spec bmi270_channels[] = { .num_event_specs = 1, }, IIO_CHAN_SOFT_TIMESTAMP(BMI270_SCAN_TIMESTAMP), + { + .type = IIO_ACCEL, + .modified = 1, + .channel2 = IIO_MOD_X_AND_Y_AND_Z, + .scan_index = -1, /* Fake channel */ + .event_spec = &bmi270_nomotion_event, + .num_event_specs = 1, + }, }; static int bmi270_int_pin_config(struct bmi270_data *data, @@ -1107,6 +1441,13 @@ static int bmi270_trigger_probe(struct bmi270_data *data, return dev_err_probe(data->dev, ret, "Trigger registration failed\n"); + /* Disable axes for motion events */ + ret = bmi270_update_feature_reg(data, BMI270_ANYMO1_REG, + BMI270_FEAT_MOTION_XYZ_EN_MSK, + FIELD_PREP(BMI270_FEAT_MOTION_XYZ_EN_MSK, 0)); + if (ret) + return ret; + data->irq_pin = irq_pin; return 0; diff --git a/drivers/iio/imu/bmi270/bmi270_spi.c b/drivers/iio/imu/bmi270/bmi270_spi.c index 19dd7734f9d0..80c9fa1d685a 100644 --- a/drivers/iio/imu/bmi270/bmi270_spi.c +++ b/drivers/iio/imu/bmi270/bmi270_spi.c @@ -60,7 +60,7 @@ static int bmi270_spi_probe(struct spi_device *spi) &bmi270_spi_regmap_config); if (IS_ERR(regmap)) return dev_err_probe(dev, PTR_ERR(regmap), - "Failed to init i2c regmap"); + "Failed to init spi regmap\n"); return bmi270_core_probe(dev, regmap, chip_info); } diff --git a/drivers/iio/imu/inv_icm45600/Kconfig b/drivers/iio/imu/inv_icm45600/Kconfig new file mode 100644 index 000000000000..dc133402f6d7 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/Kconfig @@ -0,0 +1,70 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +config INV_ICM45600 + tristate + select IIO_BUFFER + select IIO_KFIFO_BUF + select IIO_INV_SENSORS_TIMESTAMP + +config INV_ICM45600_I2C + tristate "InvenSense ICM-456xx I2C driver" + depends on I2C + select INV_ICM45600 + select REGMAP_I2C + help + This driver supports the InvenSense ICM-456xx motion tracking + devices over I2C. + Supported devices: + - ICM-45605 + - ICM-45606 + - ICM-45608 + - ICM-45634 + - ICM-45686 + - ICM-45687 + - ICM-45688-P + - ICM-45689 + + This driver can be built as a module. The module will be called + inv-icm45600-i2c. + +config INV_ICM45600_SPI + tristate "InvenSense ICM-456xx SPI driver" + depends on SPI_MASTER + select INV_ICM45600 + select REGMAP_SPI + help + This driver supports the InvenSense ICM-456xx motion tracking + devices over SPI. + Supported devices: + - ICM-45605 + - ICM-45606 + - ICM-45608 + - ICM-45634 + - ICM-45686 + - ICM-45687 + - ICM-45688-P + - ICM-45689 + + This driver can be built as a module. The module will be called + inv-icm45600-spi. + +config INV_ICM45600_I3C + tristate "InvenSense ICM-456xx I3C driver" + depends on I3C + select INV_ICM45600 + select REGMAP_I3C + help + This driver supports the InvenSense ICM-456xx motion tracking + devices over I3C. + Supported devices: + - ICM-45605 + - ICM-45606 + - ICM-45608 + - ICM-45634 + - ICM-45686 + - ICM-45687 + - ICM-45688-P + - ICM-45689 + + This driver can be built as a module. The module will be called + inv-icm45600-i3c. diff --git a/drivers/iio/imu/inv_icm45600/Makefile b/drivers/iio/imu/inv_icm45600/Makefile new file mode 100644 index 000000000000..c98b8365b467 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/Makefile @@ -0,0 +1,16 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +obj-$(CONFIG_INV_ICM45600) += inv-icm45600.o +inv-icm45600-y += inv_icm45600_core.o +inv-icm45600-y += inv_icm45600_buffer.o +inv-icm45600-y += inv_icm45600_gyro.o +inv-icm45600-y += inv_icm45600_accel.o + +obj-$(CONFIG_INV_ICM45600_I2C) += inv-icm45600-i2c.o +inv-icm45600-i2c-y += inv_icm45600_i2c.o + +obj-$(CONFIG_INV_ICM45600_SPI) += inv-icm45600-spi.o +inv-icm45600-spi-y += inv_icm45600_spi.o + +obj-$(CONFIG_INV_ICM45600_I3C) += inv-icm45600-i3c.o +inv-icm45600-i3c-y += inv_icm45600_i3c.o diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600.h b/drivers/iio/imu/inv_icm45600/inv_icm45600.h new file mode 100644 index 000000000000..c5b5446f6c3b --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600.h @@ -0,0 +1,385 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* Copyright (C) 2025 Invensense, Inc. */ + +#ifndef INV_ICM45600_H_ +#define INV_ICM45600_H_ + +#include <linux/bits.h> +#include <linux/limits.h> +#include <linux/mutex.h> +#include <linux/regmap.h> +#include <linux/regulator/consumer.h> +#include <linux/sizes.h> +#include <linux/types.h> + +#include <linux/iio/common/inv_sensors_timestamp.h> +#include <linux/iio/iio.h> + +#include "inv_icm45600_buffer.h" + +#define INV_ICM45600_REG_BANK_MASK GENMASK(15, 8) +#define INV_ICM45600_REG_ADDR_MASK GENMASK(7, 0) + +enum inv_icm45600_sensor_mode { + INV_ICM45600_SENSOR_MODE_OFF, + INV_ICM45600_SENSOR_MODE_STANDBY, + INV_ICM45600_SENSOR_MODE_LOW_POWER, + INV_ICM45600_SENSOR_MODE_LOW_NOISE, + INV_ICM45600_SENSOR_MODE_MAX +}; + +/* gyroscope fullscale values */ +enum inv_icm45600_gyro_fs { + INV_ICM45600_GYRO_FS_2000DPS, + INV_ICM45600_GYRO_FS_1000DPS, + INV_ICM45600_GYRO_FS_500DPS, + INV_ICM45600_GYRO_FS_250DPS, + INV_ICM45600_GYRO_FS_125DPS, + INV_ICM45600_GYRO_FS_62_5DPS, + INV_ICM45600_GYRO_FS_31_25DPS, + INV_ICM45600_GYRO_FS_15_625DPS, + INV_ICM45600_GYRO_FS_MAX +}; + +enum inv_icm45686_gyro_fs { + INV_ICM45686_GYRO_FS_4000DPS, + INV_ICM45686_GYRO_FS_2000DPS, + INV_ICM45686_GYRO_FS_1000DPS, + INV_ICM45686_GYRO_FS_500DPS, + INV_ICM45686_GYRO_FS_250DPS, + INV_ICM45686_GYRO_FS_125DPS, + INV_ICM45686_GYRO_FS_62_5DPS, + INV_ICM45686_GYRO_FS_31_25DPS, + INV_ICM45686_GYRO_FS_15_625DPS, + INV_ICM45686_GYRO_FS_MAX +}; + +/* accelerometer fullscale values */ +enum inv_icm45600_accel_fs { + INV_ICM45600_ACCEL_FS_16G, + INV_ICM45600_ACCEL_FS_8G, + INV_ICM45600_ACCEL_FS_4G, + INV_ICM45600_ACCEL_FS_2G, + INV_ICM45600_ACCEL_FS_MAX +}; + +enum inv_icm45686_accel_fs { + INV_ICM45686_ACCEL_FS_32G, + INV_ICM45686_ACCEL_FS_16G, + INV_ICM45686_ACCEL_FS_8G, + INV_ICM45686_ACCEL_FS_4G, + INV_ICM45686_ACCEL_FS_2G, + INV_ICM45686_ACCEL_FS_MAX +}; + +/* ODR suffixed by LN or LP are Low-Noise or Low-Power mode only */ +enum inv_icm45600_odr { + INV_ICM45600_ODR_6400HZ_LN = 0x03, + INV_ICM45600_ODR_3200HZ_LN, + INV_ICM45600_ODR_1600HZ_LN, + INV_ICM45600_ODR_800HZ_LN, + INV_ICM45600_ODR_400HZ, + INV_ICM45600_ODR_200HZ, + INV_ICM45600_ODR_100HZ, + INV_ICM45600_ODR_50HZ, + INV_ICM45600_ODR_25HZ, + INV_ICM45600_ODR_12_5HZ, + INV_ICM45600_ODR_6_25HZ_LP, + INV_ICM45600_ODR_3_125HZ_LP, + INV_ICM45600_ODR_1_5625HZ_LP, + INV_ICM45600_ODR_MAX +}; + +struct inv_icm45600_sensor_conf { + u8 mode; + u8 fs; + u8 odr; + u8 filter; +}; + +#define INV_ICM45600_SENSOR_CONF_KEEP_VALUES { U8_MAX, U8_MAX, U8_MAX, U8_MAX } + +struct inv_icm45600_conf { + struct inv_icm45600_sensor_conf gyro; + struct inv_icm45600_sensor_conf accel; +}; + +struct inv_icm45600_suspended { + enum inv_icm45600_sensor_mode gyro; + enum inv_icm45600_sensor_mode accel; +}; + +struct inv_icm45600_chip_info { + u8 whoami; + const char *name; + const struct inv_icm45600_conf *conf; + const int *accel_scales; + const int accel_scales_len; + const int *gyro_scales; + const int gyro_scales_len; +}; + +extern const struct inv_icm45600_chip_info inv_icm45605_chip_info; +extern const struct inv_icm45600_chip_info inv_icm45606_chip_info; +extern const struct inv_icm45600_chip_info inv_icm45608_chip_info; +extern const struct inv_icm45600_chip_info inv_icm45634_chip_info; +extern const struct inv_icm45600_chip_info inv_icm45686_chip_info; +extern const struct inv_icm45600_chip_info inv_icm45687_chip_info; +extern const struct inv_icm45600_chip_info inv_icm45688p_chip_info; +extern const struct inv_icm45600_chip_info inv_icm45689_chip_info; + +extern const int inv_icm45600_accel_scale[][2]; +extern const int inv_icm45686_accel_scale[][2]; +extern const int inv_icm45600_gyro_scale[][2]; +extern const int inv_icm45686_gyro_scale[][2]; + +/** + * struct inv_icm45600_state - driver state variables + * @lock: lock for serializing multiple registers access. + * @map: regmap pointer. + * @vddio_supply: I/O voltage regulator for the chip. + * @orientation: sensor chip orientation relative to main hardware. + * @conf: chip sensors configurations. + * @suspended: suspended sensors configuration. + * @indio_gyro: gyroscope IIO device. + * @indio_accel: accelerometer IIO device. + * @chip_info: chip driver data. + * @timestamp: interrupt timestamps. + * @fifo: FIFO management structure. + * @buffer: data transfer buffer aligned for DMA. + */ +struct inv_icm45600_state { + struct mutex lock; + struct regmap *map; + struct regulator *vddio_supply; + struct iio_mount_matrix orientation; + struct inv_icm45600_conf conf; + struct inv_icm45600_suspended suspended; + struct iio_dev *indio_gyro; + struct iio_dev *indio_accel; + const struct inv_icm45600_chip_info *chip_info; + struct { + s64 gyro; + s64 accel; + } timestamp; + struct inv_icm45600_fifo fifo; + union { + u8 buff[2]; + __le16 u16; + u8 ireg[3]; + } buffer __aligned(IIO_DMA_MINALIGN); +}; + +/** + * struct inv_icm45600_sensor_state - sensor state variables + * @scales: table of scales. + * @scales_len: length (nb of items) of the scales table. + * @power_mode: sensor requested power mode (for common frequencies) + * @ts: timestamp module states. + */ +struct inv_icm45600_sensor_state { + const int *scales; + size_t scales_len; + enum inv_icm45600_sensor_mode power_mode; + struct inv_sensors_timestamp ts; +}; + +/* Virtual register addresses: @bank on MSB (16 bits), @address on LSB */ + +/* Indirect register access */ +#define INV_ICM45600_REG_IREG_ADDR 0x7C +#define INV_ICM45600_REG_IREG_DATA 0x7E + +/* Direct acces registers */ +#define INV_ICM45600_REG_MISC2 0x007F +#define INV_ICM45600_MISC2_SOFT_RESET BIT(1) + +#define INV_ICM45600_REG_DRIVE_CONFIG0 0x0032 +#define INV_ICM45600_DRIVE_CONFIG0_SPI_MASK GENMASK(3, 1) +#define INV_ICM45600_SPI_SLEW_RATE_0_5NS 6 +#define INV_ICM45600_SPI_SLEW_RATE_4NS 5 +#define INV_ICM45600_SPI_SLEW_RATE_5NS 4 +#define INV_ICM45600_SPI_SLEW_RATE_7NS 3 +#define INV_ICM45600_SPI_SLEW_RATE_10NS 2 +#define INV_ICM45600_SPI_SLEW_RATE_14NS 1 +#define INV_ICM45600_SPI_SLEW_RATE_38NS 0 + +#define INV_ICM45600_REG_INT1_CONFIG2 0x0018 +#define INV_ICM45600_INT1_CONFIG2_PUSH_PULL BIT(2) +#define INV_ICM45600_INT1_CONFIG2_LATCHED BIT(1) +#define INV_ICM45600_INT1_CONFIG2_ACTIVE_HIGH BIT(0) +#define INV_ICM45600_INT1_CONFIG2_ACTIVE_LOW 0x00 + +#define INV_ICM45600_REG_FIFO_CONFIG0 0x001D +#define INV_ICM45600_FIFO_CONFIG0_MODE_MASK GENMASK(7, 6) +#define INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS 0 +#define INV_ICM45600_FIFO_CONFIG0_MODE_STREAM 1 +#define INV_ICM45600_FIFO_CONFIG0_MODE_STOP_ON_FULL 2 +#define INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MASK GENMASK(5, 0) +#define INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MAX 0x1F + +#define INV_ICM45600_REG_FIFO_CONFIG2 0x0020 +#define INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH BIT(7) +#define INV_ICM45600_REG_FIFO_CONFIG2_WM_GT_TH BIT(3) + +#define INV_ICM45600_REG_FIFO_CONFIG3 0x0021 +#define INV_ICM45600_FIFO_CONFIG3_ES1_EN BIT(5) +#define INV_ICM45600_FIFO_CONFIG3_ES0_EN BIT(4) +#define INV_ICM45600_FIFO_CONFIG3_HIRES_EN BIT(3) +#define INV_ICM45600_FIFO_CONFIG3_GYRO_EN BIT(2) +#define INV_ICM45600_FIFO_CONFIG3_ACCEL_EN BIT(1) +#define INV_ICM45600_FIFO_CONFIG3_IF_EN BIT(0) + +#define INV_ICM45600_REG_FIFO_CONFIG4 0x0022 +#define INV_ICM45600_FIFO_CONFIG4_COMP_EN BIT(2) +#define INV_ICM45600_FIFO_CONFIG4_TMST_FSYNC_EN BIT(1) +#define INV_ICM45600_FIFO_CONFIG4_ES0_9B BIT(0) + +/* all sensor data are 16 bits (2 registers wide) in big-endian */ +#define INV_ICM45600_REG_TEMP_DATA 0x000C +#define INV_ICM45600_REG_ACCEL_DATA_X 0x0000 +#define INV_ICM45600_REG_ACCEL_DATA_Y 0x0002 +#define INV_ICM45600_REG_ACCEL_DATA_Z 0x0004 +#define INV_ICM45600_REG_GYRO_DATA_X 0x0006 +#define INV_ICM45600_REG_GYRO_DATA_Y 0x0008 +#define INV_ICM45600_REG_GYRO_DATA_Z 0x000A + +#define INV_ICM45600_REG_INT_STATUS 0x0019 +#define INV_ICM45600_INT_STATUS_RESET_DONE BIT(7) +#define INV_ICM45600_INT_STATUS_AUX1_AGC_RDY BIT(6) +#define INV_ICM45600_INT_STATUS_AP_AGC_RDY BIT(5) +#define INV_ICM45600_INT_STATUS_AP_FSYNC BIT(4) +#define INV_ICM45600_INT_STATUS_AUX1_DRDY BIT(3) +#define INV_ICM45600_INT_STATUS_DATA_RDY BIT(2) +#define INV_ICM45600_INT_STATUS_FIFO_THS BIT(1) +#define INV_ICM45600_INT_STATUS_FIFO_FULL BIT(0) + +/* + * FIFO access registers + * FIFO count is 16 bits (2 registers) + * FIFO data is a continuous read register to read FIFO content + */ +#define INV_ICM45600_REG_FIFO_COUNT 0x0012 +#define INV_ICM45600_REG_FIFO_DATA 0x0014 + +#define INV_ICM45600_REG_PWR_MGMT0 0x0010 +#define INV_ICM45600_PWR_MGMT0_GYRO_MODE_MASK GENMASK(3, 2) +#define INV_ICM45600_PWR_MGMT0_ACCEL_MODE_MASK GENMASK(1, 0) + +#define INV_ICM45600_REG_ACCEL_CONFIG0 0x001B +#define INV_ICM45600_ACCEL_CONFIG0_FS_MASK GENMASK(6, 4) +#define INV_ICM45600_ACCEL_CONFIG0_ODR_MASK GENMASK(3, 0) +#define INV_ICM45600_REG_GYRO_CONFIG0 0x001C +#define INV_ICM45600_GYRO_CONFIG0_FS_MASK GENMASK(7, 4) +#define INV_ICM45600_GYRO_CONFIG0_ODR_MASK GENMASK(3, 0) + +#define INV_ICM45600_REG_SMC_CONTROL_0 0xA258 +#define INV_ICM45600_SMC_CONTROL_0_ACCEL_LP_CLK_SEL BIT(4) +#define INV_ICM45600_SMC_CONTROL_0_TMST_EN BIT(0) + +/* FIFO watermark is 16 bits (2 registers wide) in little-endian */ +#define INV_ICM45600_REG_FIFO_WATERMARK 0x001E + +/* FIFO is configured for 8kb */ +#define INV_ICM45600_FIFO_SIZE_MAX SZ_8K + +#define INV_ICM45600_REG_INT1_CONFIG0 0x0016 +#define INV_ICM45600_INT1_CONFIG0_RESET_DONE_EN BIT(7) +#define INV_ICM45600_INT1_CONFIG0_AUX1_AGC_RDY_EN BIT(6) +#define INV_ICM45600_INT1_CONFIG0_AP_AGC_RDY_EN BIT(5) +#define INV_ICM45600_INT1_CONFIG0_AP_FSYNC_EN BIT(4) +#define INV_ICM45600_INT1_CONFIG0_AUX1_DRDY_EN BIT(3) +#define INV_ICM45600_INT1_CONFIG0_DRDY_EN BIT(2) +#define INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN BIT(1) +#define INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN BIT(0) + +#define INV_ICM45600_REG_WHOAMI 0x0072 +#define INV_ICM45600_WHOAMI_ICM45605 0xE5 +#define INV_ICM45600_WHOAMI_ICM45686 0xE9 +#define INV_ICM45600_WHOAMI_ICM45688P 0xE7 +#define INV_ICM45600_WHOAMI_ICM45608 0x81 +#define INV_ICM45600_WHOAMI_ICM45634 0x82 +#define INV_ICM45600_WHOAMI_ICM45689 0x83 +#define INV_ICM45600_WHOAMI_ICM45606 0x84 +#define INV_ICM45600_WHOAMI_ICM45687 0x85 + +/* Gyro USER offset */ +#define INV_ICM45600_IPREG_SYS1_REG_42 0xA42A +#define INV_ICM45600_IPREG_SYS1_REG_56 0xA438 +#define INV_ICM45600_IPREG_SYS1_REG_70 0xA446 +#define INV_ICM45600_GYRO_OFFUSER_MASK GENMASK(13, 0) +/* Gyro Averaging filter */ +#define INV_ICM45600_IPREG_SYS1_REG_170 0xA4AA +#define INV_ICM45600_IPREG_SYS1_170_GYRO_LP_AVG_MASK GENMASK(4, 1) +#define INV_ICM45600_GYRO_LP_AVG_SEL_8X 5 +#define INV_ICM45600_GYRO_LP_AVG_SEL_2X 1 +/* Accel USER offset */ +#define INV_ICM45600_IPREG_SYS2_REG_24 0xA518 +#define INV_ICM45600_IPREG_SYS2_REG_32 0xA520 +#define INV_ICM45600_IPREG_SYS2_REG_40 0xA528 +#define INV_ICM45600_ACCEL_OFFUSER_MASK GENMASK(13, 0) +/* Accel averaging filter */ +#define INV_ICM45600_IPREG_SYS2_REG_129 0xA581 +#define INV_ICM45600_ACCEL_LP_AVG_SEL_1X 0x0000 +#define INV_ICM45600_ACCEL_LP_AVG_SEL_4X 0x0002 + +/* Sleep times required by the driver */ +#define INV_ICM45600_ACCEL_STARTUP_TIME_MS 60 +#define INV_ICM45600_GYRO_STARTUP_TIME_MS 60 +#define INV_ICM45600_GYRO_STOP_TIME_MS 150 +#define INV_ICM45600_IREG_DELAY_US 4 + +typedef int (*inv_icm45600_bus_setup)(struct inv_icm45600_state *); + +extern const struct dev_pm_ops inv_icm45600_pm_ops; + +const struct iio_mount_matrix * +inv_icm45600_get_mount_matrix(const struct iio_dev *indio_dev, + const struct iio_chan_spec *chan); + +#define INV_ICM45600_TEMP_CHAN(_index) \ + { \ + .type = IIO_TEMP, \ + .info_mask_separate = \ + BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_OFFSET) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ + } + +int inv_icm45600_temp_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask); + +u32 inv_icm45600_odr_to_period(enum inv_icm45600_odr odr); + +int inv_icm45600_set_accel_conf(struct inv_icm45600_state *st, + struct inv_icm45600_sensor_conf *conf, + unsigned int *sleep_ms); + +int inv_icm45600_set_gyro_conf(struct inv_icm45600_state *st, + struct inv_icm45600_sensor_conf *conf, + unsigned int *sleep_ms); + +int inv_icm45600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval); + +int inv_icm45600_core_probe(struct regmap *regmap, + const struct inv_icm45600_chip_info *chip_info, + bool reset, inv_icm45600_bus_setup bus_setup); + +struct iio_dev *inv_icm45600_gyro_init(struct inv_icm45600_state *st); + +int inv_icm45600_gyro_parse_fifo(struct iio_dev *indio_dev); + +struct iio_dev *inv_icm45600_accel_init(struct inv_icm45600_state *st); + +int inv_icm45600_accel_parse_fifo(struct iio_dev *indio_dev); + +#endif diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_accel.c b/drivers/iio/imu/inv_icm45600/inv_icm45600_accel.c new file mode 100644 index 000000000000..efa22e02657f --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_accel.c @@ -0,0 +1,782 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2025 Invensense, Inc. + */ + +#include <linux/delay.h> +#include <linux/device.h> +#include <linux/err.h> +#include <linux/math64.h> +#include <linux/mutex.h> +#include <linux/pm_runtime.h> +#include <linux/regmap.h> +#include <linux/types.h> + +#include <linux/iio/buffer.h> +#include <linux/iio/common/inv_sensors_timestamp.h> +#include <linux/iio/iio.h> +#include <linux/iio/kfifo_buf.h> + +#include "inv_icm45600_buffer.h" +#include "inv_icm45600.h" + +enum inv_icm45600_accel_scan { + INV_ICM45600_ACCEL_SCAN_X, + INV_ICM45600_ACCEL_SCAN_Y, + INV_ICM45600_ACCEL_SCAN_Z, + INV_ICM45600_ACCEL_SCAN_TEMP, + INV_ICM45600_ACCEL_SCAN_TIMESTAMP, +}; + +static const struct iio_chan_spec_ext_info inv_icm45600_accel_ext_infos[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm45600_get_mount_matrix), + { } +}; + +#define INV_ICM45600_ACCEL_CHAN(_modifier, _index, _ext_info) \ + { \ + .type = IIO_ACCEL, \ + .modified = 1, \ + .channel2 = _modifier, \ + .info_mask_separate = \ + BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_type_available = \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_all = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .info_mask_shared_by_all_available = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ + .ext_info = _ext_info, \ + } + +static const struct iio_chan_spec inv_icm45600_accel_channels[] = { + INV_ICM45600_ACCEL_CHAN(IIO_MOD_X, INV_ICM45600_ACCEL_SCAN_X, + inv_icm45600_accel_ext_infos), + INV_ICM45600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM45600_ACCEL_SCAN_Y, + inv_icm45600_accel_ext_infos), + INV_ICM45600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM45600_ACCEL_SCAN_Z, + inv_icm45600_accel_ext_infos), + INV_ICM45600_TEMP_CHAN(INV_ICM45600_ACCEL_SCAN_TEMP), + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM45600_ACCEL_SCAN_TIMESTAMP), +}; + +/* + * IIO buffer data: size must be a power of 2 and timestamp aligned + * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp + */ +struct inv_icm45600_accel_buffer { + struct inv_icm45600_fifo_sensor_data accel; + s16 temp; + aligned_s64 timestamp; +}; + +static const unsigned long inv_icm45600_accel_scan_masks[] = { + /* 3-axis accel + temperature */ + BIT(INV_ICM45600_ACCEL_SCAN_X) | + BIT(INV_ICM45600_ACCEL_SCAN_Y) | + BIT(INV_ICM45600_ACCEL_SCAN_Z) | + BIT(INV_ICM45600_ACCEL_SCAN_TEMP), + 0 +}; + +/* enable accelerometer sensor and FIFO write */ +static int inv_icm45600_accel_update_scan_mode(struct iio_dev *indio_dev, + const unsigned long *scan_mask) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev); + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + unsigned int fifo_en = 0; + unsigned int sleep = 0; + int ret; + + scoped_guard(mutex, &st->lock) { + if (*scan_mask & BIT(INV_ICM45600_ACCEL_SCAN_TEMP)) + fifo_en |= INV_ICM45600_SENSOR_TEMP; + + if (*scan_mask & (BIT(INV_ICM45600_ACCEL_SCAN_X) | + BIT(INV_ICM45600_ACCEL_SCAN_Y) | + BIT(INV_ICM45600_ACCEL_SCAN_Z))) { + /* enable accel sensor */ + conf.mode = accel_st->power_mode; + ret = inv_icm45600_set_accel_conf(st, &conf, &sleep); + if (ret) + return ret; + fifo_en |= INV_ICM45600_SENSOR_ACCEL; + } + + /* Update data FIFO write. */ + ret = inv_icm45600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); + } + + /* Sleep required time. */ + if (sleep) + msleep(sleep); + + return ret; +} + +static int _inv_icm45600_accel_read_sensor(struct inv_icm45600_state *st, + struct inv_icm45600_sensor_state *accel_st, + unsigned int reg, int *val) +{ + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + /* enable accel sensor */ + conf.mode = accel_st->power_mode; + ret = inv_icm45600_set_accel_conf(st, &conf, NULL); + if (ret) + return ret; + + /* read accel register data */ + ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16)); + if (ret) + return ret; + + *val = sign_extend32(le16_to_cpup(&st->buffer.u16), 15); + if (*val == INV_ICM45600_DATA_INVALID) + return -ENODATA; + + return 0; +} + +static int inv_icm45600_accel_read_sensor(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int reg; + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM45600_REG_ACCEL_DATA_X; + break; + case IIO_MOD_Y: + reg = INV_ICM45600_REG_ACCEL_DATA_Y; + break; + case IIO_MOD_Z: + reg = INV_ICM45600_REG_ACCEL_DATA_Z; + break; + default: + return -EINVAL; + } + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = _inv_icm45600_accel_read_sensor(st, accel_st, reg, val); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* IIO format int + nano */ +const int inv_icm45600_accel_scale[][2] = { + /* +/- 16G => 0.004788403 m/s-2 */ + [INV_ICM45600_ACCEL_FS_16G] = { 0, 4788403 }, + /* +/- 8G => 0.002394202 m/s-2 */ + [INV_ICM45600_ACCEL_FS_8G] = { 0, 2394202 }, + /* +/- 4G => 0.001197101 m/s-2 */ + [INV_ICM45600_ACCEL_FS_4G] = { 0, 1197101 }, + /* +/- 2G => 0.000598550 m/s-2 */ + [INV_ICM45600_ACCEL_FS_2G] = { 0, 598550 }, +}; + +const int inv_icm45686_accel_scale[][2] = { + /* +/- 32G => 0.009576806 m/s-2 */ + [INV_ICM45686_ACCEL_FS_32G] = { 0, 9576806 }, + /* +/- 16G => 0.004788403 m/s-2 */ + [INV_ICM45686_ACCEL_FS_16G] = { 0, 4788403 }, + /* +/- 8G => 0.002394202 m/s-2 */ + [INV_ICM45686_ACCEL_FS_8G] = { 0, 2394202 }, + /* +/- 4G => 0.001197101 m/s-2 */ + [INV_ICM45686_ACCEL_FS_4G] = { 0, 1197101 }, + /* +/- 2G => 0.000598550 m/s-2 */ + [INV_ICM45686_ACCEL_FS_2G] = { 0, 598550 }, +}; + +static int inv_icm45600_accel_read_scale(struct iio_dev *indio_dev, + int *val, int *val2) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev); + unsigned int idx; + + idx = st->conf.accel.fs; + + /* Full scale register starts at 1 for not High FSR parts */ + if (accel_st->scales == (const int *)&inv_icm45600_accel_scale) + idx--; + + *val = accel_st->scales[2 * idx]; + *val2 = accel_st->scales[2 * idx + 1]; + return IIO_VAL_INT_PLUS_NANO; +} + +static int inv_icm45600_accel_write_scale(struct iio_dev *indio_dev, + int val, int val2) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + for (idx = 0; idx < accel_st->scales_len; idx += 2) { + if (val == accel_st->scales[idx] && + val2 == accel_st->scales[idx + 1]) + break; + } + if (idx == accel_st->scales_len) + return -EINVAL; + + conf.fs = idx / 2; + + /* Full scale register starts at 1 for not High FSR parts */ + if (accel_st->scales == (const int *)&inv_icm45600_accel_scale) + conf.fs++; + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = inv_icm45600_set_accel_conf(st, &conf, NULL); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* IIO format int + micro */ +static const int inv_icm45600_accel_odr[] = { + 1, 562500, /* 1.5625Hz */ + 3, 125000, /* 3.125Hz */ + 6, 250000, /* 6.25Hz */ + 12, 500000, /* 12.5Hz */ + 25, 0, /* 25Hz */ + 50, 0, /* 50Hz */ + 100, 0, /* 100Hz */ + 200, 0, /* 200Hz */ + 400, 0, /* 400Hz */ + 800, 0, /* 800Hz */ + 1600, 0, /* 1.6kHz */ + 3200, 0, /* 3.2kHz */ + 6400, 0, /* 6.4kHz */ +}; + +static const int inv_icm45600_accel_odr_conv[] = { + INV_ICM45600_ODR_1_5625HZ_LP, + INV_ICM45600_ODR_3_125HZ_LP, + INV_ICM45600_ODR_6_25HZ_LP, + INV_ICM45600_ODR_12_5HZ, + INV_ICM45600_ODR_25HZ, + INV_ICM45600_ODR_50HZ, + INV_ICM45600_ODR_100HZ, + INV_ICM45600_ODR_200HZ, + INV_ICM45600_ODR_400HZ, + INV_ICM45600_ODR_800HZ_LN, + INV_ICM45600_ODR_1600HZ_LN, + INV_ICM45600_ODR_3200HZ_LN, + INV_ICM45600_ODR_6400HZ_LN, +}; + +static int inv_icm45600_accel_read_odr(struct inv_icm45600_state *st, + int *val, int *val2) +{ + unsigned int odr; + unsigned int i; + + odr = st->conf.accel.odr; + + for (i = 0; i < ARRAY_SIZE(inv_icm45600_accel_odr_conv); ++i) { + if (inv_icm45600_accel_odr_conv[i] == odr) + break; + } + if (i >= ARRAY_SIZE(inv_icm45600_accel_odr_conv)) + return -EINVAL; + + *val = inv_icm45600_accel_odr[2 * i]; + *val2 = inv_icm45600_accel_odr[2 * i + 1]; + + return IIO_VAL_INT_PLUS_MICRO; +} + +static int _inv_icm45600_accel_write_odr(struct iio_dev *indio_dev, int odr) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev); + struct inv_sensors_timestamp *ts = &accel_st->ts; + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + conf.odr = odr; + ret = inv_sensors_timestamp_update_odr(ts, inv_icm45600_odr_to_period(conf.odr), + iio_buffer_enabled(indio_dev)); + if (ret) + return ret; + + if (st->conf.accel.mode != INV_ICM45600_SENSOR_MODE_OFF) + conf.mode = accel_st->power_mode; + + ret = inv_icm45600_set_accel_conf(st, &conf, NULL); + if (ret) + return ret; + + inv_icm45600_buffer_update_fifo_period(st); + inv_icm45600_buffer_update_watermark(st); + + return 0; +} + +static int inv_icm45600_accel_write_odr(struct iio_dev *indio_dev, + int val, int val2) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + int odr; + int ret; + + for (idx = 0; idx < ARRAY_SIZE(inv_icm45600_accel_odr); idx += 2) { + if (val == inv_icm45600_accel_odr[idx] && + val2 == inv_icm45600_accel_odr[idx + 1]) + break; + } + if (idx >= ARRAY_SIZE(inv_icm45600_accel_odr)) + return -EINVAL; + + odr = inv_icm45600_accel_odr_conv[idx / 2]; + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = _inv_icm45600_accel_write_odr(indio_dev, odr); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* + * Calibration bias values, IIO range format int + micro. + * Value is limited to +/-1g coded on 14 bits signed. Step is 0.125mg. + */ +static int inv_icm45600_accel_calibbias[] = { + -9, 806650, /* min: -9.806650 m/s² */ + 0, 1197, /* step: 0.001197 m/s² */ + 9, 805453, /* max: 9.805453 m/s² */ +}; + +static int inv_icm45600_accel_read_offset(struct inv_icm45600_state *st, + struct iio_chan_spec const *chan, + int *val, int *val2) +{ + struct device *dev = regmap_get_device(st->map); + s64 val64; + s32 bias; + unsigned int reg; + s16 offset; + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM45600_IPREG_SYS2_REG_24; + break; + case IIO_MOD_Y: + reg = INV_ICM45600_IPREG_SYS2_REG_32; + break; + case IIO_MOD_Z: + reg = INV_ICM45600_IPREG_SYS2_REG_40; + break; + default: + return -EINVAL; + } + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16)); + + pm_runtime_put_autosuspend(dev); + if (ret) + return ret; + + offset = le16_to_cpup(&st->buffer.u16) & INV_ICM45600_ACCEL_OFFUSER_MASK; + /* 14 bits signed value */ + offset = sign_extend32(offset, 13); + + /* + * convert raw offset to g then to m/s² + * 14 bits signed raw step 1/8192g + * g to m/s²: 9.806650 + * result in micro (* 1000000) + * (offset * 9806650) / 8192 + */ + val64 = (s64)offset * 9806650LL; + /* for rounding, add + or - divisor (8192) divided by 2 */ + if (val64 >= 0) + val64 += 8192LL / 2LL; + else + val64 -= 8192LL / 2LL; + bias = div_s64(val64, 8192L); + *val = bias / 1000000L; + *val2 = bias % 1000000L; + + return IIO_VAL_INT_PLUS_MICRO; +} + +static int inv_icm45600_accel_write_offset(struct inv_icm45600_state *st, + struct iio_chan_spec const *chan, + int val, int val2) +{ + struct device *dev = regmap_get_device(st->map); + s64 val64; + s32 min, max; + unsigned int reg; + s16 offset; + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM45600_IPREG_SYS2_REG_24; + break; + case IIO_MOD_Y: + reg = INV_ICM45600_IPREG_SYS2_REG_32; + break; + case IIO_MOD_Z: + reg = INV_ICM45600_IPREG_SYS2_REG_40; + break; + default: + return -EINVAL; + } + + /* inv_icm45600_accel_calibbias: min - step - max in micro */ + min = inv_icm45600_accel_calibbias[0] * 1000000L - + inv_icm45600_accel_calibbias[1]; + max = inv_icm45600_accel_calibbias[4] * 1000000L + + inv_icm45600_accel_calibbias[5]; + val64 = (s64)val * 1000000LL; + if (val >= 0) + val64 += (s64)val2; + else + val64 -= (s64)val2; + if (val64 < min || val64 > max) + return -EINVAL; + + /* + * convert m/s² to g then to raw value + * m/s² to g: 1 / 9.806650 + * g to raw 14 bits signed, step 1/8192g: * 8192 + * val in micro (1000000) + * val * 8192 / (9.806650 * 1000000) + */ + val64 = val64 * 8192LL; + /* for rounding, add + or - divisor (9806650) divided by 2 */ + if (val64 >= 0) + val64 += 9806650 / 2; + else + val64 -= 9806650 / 2; + offset = div_s64(val64, 9806650); + + /* clamp value limited to 14 bits signed */ + offset = clamp(offset, -8192, 8191); + + st->buffer.u16 = cpu_to_le16(offset & INV_ICM45600_ACCEL_OFFUSER_MASK); + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = regmap_bulk_write(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16)); + + pm_runtime_put_autosuspend(dev); + return ret; +} + +static int inv_icm45600_accel_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + switch (chan->type) { + case IIO_ACCEL: + break; + case IIO_TEMP: + return inv_icm45600_temp_read_raw(indio_dev, chan, val, val2, mask); + default: + return -EINVAL; + } + + switch (mask) { + case IIO_CHAN_INFO_RAW: + if (!iio_device_claim_direct(indio_dev)) + return -EBUSY; + ret = inv_icm45600_accel_read_sensor(indio_dev, chan, val); + iio_device_release_direct(indio_dev); + if (ret) + return ret; + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + return inv_icm45600_accel_read_scale(indio_dev, val, val2); + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm45600_accel_read_odr(st, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + return inv_icm45600_accel_read_offset(st, chan, val, val2); + default: + return -EINVAL; + } +} + +static int inv_icm45600_accel_read_avail(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + const int **vals, + int *type, int *length, long mask) +{ + struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev); + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + *vals = accel_st->scales; + *type = IIO_VAL_INT_PLUS_NANO; + *length = accel_st->scales_len; + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_SAMP_FREQ: + *vals = inv_icm45600_accel_odr; + *type = IIO_VAL_INT_PLUS_MICRO; + *length = ARRAY_SIZE(inv_icm45600_accel_odr); + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_CALIBBIAS: + *vals = inv_icm45600_accel_calibbias; + *type = IIO_VAL_INT_PLUS_MICRO; + return IIO_AVAIL_RANGE; + default: + return -EINVAL; + } +} + +static int inv_icm45600_accel_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + if (!iio_device_claim_direct(indio_dev)) + return -EBUSY; + ret = inv_icm45600_accel_write_scale(indio_dev, val, val2); + iio_device_release_direct(indio_dev); + return ret; + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm45600_accel_write_odr(indio_dev, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + if (!iio_device_claim_direct(indio_dev)) + return -EBUSY; + ret = inv_icm45600_accel_write_offset(st, chan, val, val2); + iio_device_release_direct(indio_dev); + return ret; + default: + return -EINVAL; + } +} + +static int inv_icm45600_accel_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (chan->type != IIO_ACCEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + return IIO_VAL_INT_PLUS_NANO; + case IIO_CHAN_INFO_SAMP_FREQ: + return IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_CALIBBIAS: + return IIO_VAL_INT_PLUS_MICRO; + default: + return -EINVAL; + } +} + +static int inv_icm45600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev, + unsigned int val) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + + guard(mutex)(&st->lock); + + st->fifo.watermark.accel = val; + return inv_icm45600_buffer_update_watermark(st); +} + +static int inv_icm45600_accel_hwfifo_flush(struct iio_dev *indio_dev, + unsigned int count) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (count == 0) + return 0; + + guard(mutex)(&st->lock); + + ret = inv_icm45600_buffer_hwfifo_flush(st, count); + if (ret) + return ret; + + return st->fifo.nb.accel; +} + +static const struct iio_info inv_icm45600_accel_info = { + .read_raw = inv_icm45600_accel_read_raw, + .read_avail = inv_icm45600_accel_read_avail, + .write_raw = inv_icm45600_accel_write_raw, + .write_raw_get_fmt = inv_icm45600_accel_write_raw_get_fmt, + .debugfs_reg_access = inv_icm45600_debugfs_reg, + .update_scan_mode = inv_icm45600_accel_update_scan_mode, + .hwfifo_set_watermark = inv_icm45600_accel_hwfifo_set_watermark, + .hwfifo_flush_to_buffer = inv_icm45600_accel_hwfifo_flush, +}; + +struct iio_dev *inv_icm45600_accel_init(struct inv_icm45600_state *st) +{ + struct device *dev = regmap_get_device(st->map); + struct inv_icm45600_sensor_state *accel_st; + struct inv_sensors_timestamp_chip ts_chip; + struct iio_dev *indio_dev; + const char *name; + int ret; + + name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->chip_info->name); + if (!name) + return ERR_PTR(-ENOMEM); + + indio_dev = devm_iio_device_alloc(dev, sizeof(*accel_st)); + if (!indio_dev) + return ERR_PTR(-ENOMEM); + accel_st = iio_priv(indio_dev); + + accel_st->scales = st->chip_info->accel_scales; + accel_st->scales_len = st->chip_info->accel_scales_len * 2; + + /* low-power (LP) mode by default at init, no ULP mode */ + accel_st->power_mode = INV_ICM45600_SENSOR_MODE_LOW_POWER; + ret = regmap_set_bits(st->map, INV_ICM45600_REG_SMC_CONTROL_0, + INV_ICM45600_SMC_CONTROL_0_ACCEL_LP_CLK_SEL); + if (ret) + return ERR_PTR(ret); + + /* + * clock period is 32kHz (31250ns) + * jitter is +/- 2% (20 per mille) + */ + ts_chip.clock_period = 31250; + ts_chip.jitter = 20; + ts_chip.init_period = inv_icm45600_odr_to_period(st->conf.accel.odr); + inv_sensors_timestamp_init(&accel_st->ts, &ts_chip); + + iio_device_set_drvdata(indio_dev, st); + indio_dev->name = name; + indio_dev->info = &inv_icm45600_accel_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->channels = inv_icm45600_accel_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_icm45600_accel_channels); + indio_dev->available_scan_masks = inv_icm45600_accel_scan_masks; + + ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, + &inv_icm45600_buffer_ops); + if (ret) + return ERR_PTR(ret); + + ret = devm_iio_device_register(dev, indio_dev); + if (ret) + return ERR_PTR(ret); + + return indio_dev; +} + +int inv_icm45600_accel_parse_fifo(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *accel_st = iio_priv(indio_dev); + struct inv_sensors_timestamp *ts = &accel_st->ts; + ssize_t i, size; + unsigned int no; + + /* parse all fifo packets */ + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { + struct inv_icm45600_accel_buffer buffer = { }; + const struct inv_icm45600_fifo_sensor_data *accel, *gyro; + const __le16 *timestamp; + const s8 *temp; + unsigned int odr; + s64 ts_val; + + size = inv_icm45600_fifo_decode_packet(&st->fifo.data[i], + &accel, &gyro, &temp, ×tamp, &odr); + /* quit if error or FIFO is empty */ + if (size <= 0) + return size; + + /* skip packet if no accel data or data is invalid */ + if (accel == NULL || !inv_icm45600_fifo_is_data_valid(accel)) + continue; + + /* update odr */ + if (odr & INV_ICM45600_SENSOR_ACCEL) + inv_sensors_timestamp_apply_odr(ts, st->fifo.period, + st->fifo.nb.total, no); + + memcpy(&buffer.accel, accel, sizeof(buffer.accel)); + /* convert 8 bits FIFO temperature in high resolution format */ + buffer.temp = temp ? (*temp * 64) : 0; + ts_val = inv_sensors_timestamp_pop(ts); + iio_push_to_buffers_with_ts(indio_dev, &buffer, sizeof(buffer), ts_val); + } + + return 0; +} diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.c b/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.c new file mode 100644 index 000000000000..2b9ea317385c --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.c @@ -0,0 +1,558 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* Copyright (C) 2025 Invensense, Inc. */ + +#include <linux/bitfield.h> +#include <linux/delay.h> +#include <linux/device.h> +#include <linux/err.h> +#include <linux/minmax.h> +#include <linux/mutex.h> +#include <linux/pm_runtime.h> +#include <linux/regmap.h> +#include <linux/time.h> +#include <linux/types.h> + +#include <asm/byteorder.h> + +#include <linux/iio/buffer.h> +#include <linux/iio/common/inv_sensors_timestamp.h> +#include <linux/iio/iio.h> + +#include "inv_icm45600_buffer.h" +#include "inv_icm45600.h" + +/* FIFO header: 1 byte */ +#define INV_ICM45600_FIFO_EXT_HEADER BIT(7) +#define INV_ICM45600_FIFO_HEADER_ACCEL BIT(6) +#define INV_ICM45600_FIFO_HEADER_GYRO BIT(5) +#define INV_ICM45600_FIFO_HEADER_HIGH_RES BIT(4) +#define INV_ICM45600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2) +#define INV_ICM45600_FIFO_HEADER_ODR_ACCEL BIT(1) +#define INV_ICM45600_FIFO_HEADER_ODR_GYRO BIT(0) + +struct inv_icm45600_fifo_1sensor_packet { + u8 header; + struct inv_icm45600_fifo_sensor_data data; + s8 temp; +} __packed; + +struct inv_icm45600_fifo_2sensors_packet { + u8 header; + struct inv_icm45600_fifo_sensor_data accel; + struct inv_icm45600_fifo_sensor_data gyro; + s8 temp; + __le16 timestamp; +} __packed; + +ssize_t inv_icm45600_fifo_decode_packet(const void *packet, + const struct inv_icm45600_fifo_sensor_data **accel, + const struct inv_icm45600_fifo_sensor_data **gyro, + const s8 **temp, + const __le16 **timestamp, unsigned int *odr) +{ + const struct inv_icm45600_fifo_1sensor_packet *pack1 = packet; + const struct inv_icm45600_fifo_2sensors_packet *pack2 = packet; + u8 header = *((const u8 *)packet); + + /* FIFO extended header */ + if (header & INV_ICM45600_FIFO_EXT_HEADER) { + /* Not yet supported */ + return 0; + } + + /* handle odr flags. */ + *odr = 0; + if (header & INV_ICM45600_FIFO_HEADER_ODR_GYRO) + *odr |= INV_ICM45600_SENSOR_GYRO; + if (header & INV_ICM45600_FIFO_HEADER_ODR_ACCEL) + *odr |= INV_ICM45600_SENSOR_ACCEL; + + /* Accel + Gyro data are present. */ + if ((header & INV_ICM45600_FIFO_HEADER_ACCEL) && + (header & INV_ICM45600_FIFO_HEADER_GYRO)) { + *accel = &pack2->accel; + *gyro = &pack2->gyro; + *temp = &pack2->temp; + *timestamp = &pack2->timestamp; + return sizeof(*pack2); + } + + /* Accel data only. */ + if (header & INV_ICM45600_FIFO_HEADER_ACCEL) { + *accel = &pack1->data; + *gyro = NULL; + *temp = &pack1->temp; + *timestamp = NULL; + return sizeof(*pack1); + } + + /* Gyro data only. */ + if (header & INV_ICM45600_FIFO_HEADER_GYRO) { + *accel = NULL; + *gyro = &pack1->data; + *temp = &pack1->temp; + *timestamp = NULL; + return sizeof(*pack1); + } + + /* Invalid packet if here. */ + return -EINVAL; +} + +void inv_icm45600_buffer_update_fifo_period(struct inv_icm45600_state *st) +{ + u32 period_gyro, period_accel; + + if (st->fifo.en & INV_ICM45600_SENSOR_GYRO) + period_gyro = inv_icm45600_odr_to_period(st->conf.gyro.odr); + else + period_gyro = U32_MAX; + + if (st->fifo.en & INV_ICM45600_SENSOR_ACCEL) + period_accel = inv_icm45600_odr_to_period(st->conf.accel.odr); + else + period_accel = U32_MAX; + + st->fifo.period = min(period_gyro, period_accel); +} + +int inv_icm45600_buffer_set_fifo_en(struct inv_icm45600_state *st, + unsigned int fifo_en) +{ + unsigned int mask; + int ret; + + mask = INV_ICM45600_FIFO_CONFIG3_GYRO_EN | + INV_ICM45600_FIFO_CONFIG3_ACCEL_EN; + + ret = regmap_assign_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, mask, + (fifo_en & INV_ICM45600_SENSOR_GYRO) || + (fifo_en & INV_ICM45600_SENSOR_ACCEL)); + if (ret) + return ret; + + st->fifo.en = fifo_en; + inv_icm45600_buffer_update_fifo_period(st); + + return 0; +} + +static unsigned int inv_icm45600_wm_truncate(unsigned int watermark, size_t packet_size, + unsigned int fifo_period) +{ + size_t watermark_max, grace_samples; + + /* Keep 20ms for processing FIFO.*/ + grace_samples = (20U * NSEC_PER_MSEC) / fifo_period; + if (grace_samples < 1) + grace_samples = 1; + + watermark_max = INV_ICM45600_FIFO_SIZE_MAX / packet_size; + watermark_max -= grace_samples; + + return min(watermark, watermark_max); +} + +/** + * inv_icm45600_buffer_update_watermark - update watermark FIFO threshold + * @st: driver internal state + * + * FIFO watermark threshold is computed based on the required watermark values + * set for gyro and accel sensors. Since watermark is all about acceptable data + * latency, use the smallest setting between the 2. It means choosing the + * smallest latency but this is not as simple as choosing the smallest watermark + * value. Latency depends on watermark and ODR. It requires several steps: + * 1) compute gyro and accel latencies and choose the smallest value. + * 2) adapt the chosen latency so that it is a multiple of both gyro and accel + * ones. Otherwise it is possible that you don't meet a requirement. (for + * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the + * value of 4 will not meet accel latency requirement because 6 is not a + * multiple of 4. You need to use the value 2.) + * 3) Since all periods are multiple of each others, watermark is computed by + * dividing this computed latency by the smallest period, which corresponds + * to the FIFO frequency. + * + * Returns: 0 on success, a negative error code otherwise. + */ +int inv_icm45600_buffer_update_watermark(struct inv_icm45600_state *st) +{ + const size_t packet_size = sizeof(struct inv_icm45600_fifo_2sensors_packet); + unsigned int wm_gyro, wm_accel, watermark; + u32 period_gyro, period_accel, period; + u32 latency_gyro, latency_accel, latency; + + /* Compute sensors latency, depending on sensor watermark and odr. */ + wm_gyro = inv_icm45600_wm_truncate(st->fifo.watermark.gyro, packet_size, + st->fifo.period); + wm_accel = inv_icm45600_wm_truncate(st->fifo.watermark.accel, packet_size, + st->fifo.period); + /* Use us for odr to avoid overflow using 32 bits values. */ + period_gyro = inv_icm45600_odr_to_period(st->conf.gyro.odr) / NSEC_PER_USEC; + period_accel = inv_icm45600_odr_to_period(st->conf.accel.odr) / NSEC_PER_USEC; + latency_gyro = period_gyro * wm_gyro; + latency_accel = period_accel * wm_accel; + + /* 0 value for watermark means that the sensor is turned off. */ + if (wm_gyro == 0 && wm_accel == 0) + return 0; + + if (latency_gyro == 0) { + watermark = wm_accel; + st->fifo.watermark.eff_accel = wm_accel; + } else if (latency_accel == 0) { + watermark = wm_gyro; + st->fifo.watermark.eff_gyro = wm_gyro; + } else { + /* Compute the smallest latency that is a multiple of both. */ + if (latency_gyro <= latency_accel) + latency = latency_gyro - (latency_accel % latency_gyro); + else + latency = latency_accel - (latency_gyro % latency_accel); + /* Use the shortest period. */ + period = min(period_gyro, period_accel); + /* All this works because periods are multiple of each others. */ + watermark = max(latency / period, 1); + /* Update effective watermark. */ + st->fifo.watermark.eff_gyro = max(latency / period_gyro, 1); + st->fifo.watermark.eff_accel = max(latency / period_accel, 1); + } + + st->buffer.u16 = cpu_to_le16(watermark); + return regmap_bulk_write(st->map, INV_ICM45600_REG_FIFO_WATERMARK, + &st->buffer.u16, sizeof(st->buffer.u16)); +} + +static int inv_icm45600_buffer_preenable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct device *dev = regmap_get_device(st->map); + struct inv_icm45600_sensor_state *sensor_st = iio_priv(indio_dev); + struct inv_sensors_timestamp *ts = &sensor_st->ts; + int ret; + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + guard(mutex)(&st->lock); + inv_sensors_timestamp_reset(ts); + + return 0; +} + +/* + * Update_scan_mode callback is turning sensors on and setting data FIFO enable + * bits. + */ +static int inv_icm45600_buffer_postenable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + unsigned int val; + int ret; + + guard(mutex)(&st->lock); + + /* Exit if FIFO is already on. */ + if (st->fifo.on) { + st->fifo.on++; + return 0; + } + + ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2, + INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH); + if (ret) + return ret; + + ret = regmap_set_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0, + INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN | + INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN); + if (ret) + return ret; + + val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_STREAM); + ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + + /* Enable writing sensor data to FIFO. */ + ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + if (ret) + return ret; + + st->fifo.on++; + return 0; +} + +static int inv_icm45600_buffer_predisable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + unsigned int val; + int ret; + + guard(mutex)(&st->lock); + + /* Exit if there are several sensors using the FIFO. */ + if (st->fifo.on > 1) { + st->fifo.on--; + return 0; + } + + /* Disable writing sensor data to FIFO. */ + ret = regmap_clear_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + if (ret) + return ret; + + val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS); + ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + + ret = regmap_clear_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0, + INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN | + INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN); + if (ret) + return ret; + + ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2, + INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH); + if (ret) + return ret; + + st->fifo.on--; + return 0; +} + +static int _inv_icm45600_buffer_postdisable(struct inv_icm45600_state *st, + unsigned int sensor, unsigned int *watermark, + unsigned int *sleep) +{ + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + ret = inv_icm45600_buffer_set_fifo_en(st, st->fifo.en & ~sensor); + if (ret) + return ret; + + *watermark = 0; + ret = inv_icm45600_buffer_update_watermark(st); + if (ret) + return ret; + + conf.mode = INV_ICM45600_SENSOR_MODE_OFF; + if (sensor == INV_ICM45600_SENSOR_GYRO) + return inv_icm45600_set_gyro_conf(st, &conf, sleep); + else + return inv_icm45600_set_accel_conf(st, &conf, sleep); +} + +static int inv_icm45600_buffer_postdisable(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int sensor; + unsigned int *watermark; + unsigned int sleep; + int ret; + + if (indio_dev == st->indio_gyro) { + sensor = INV_ICM45600_SENSOR_GYRO; + watermark = &st->fifo.watermark.gyro; + } else if (indio_dev == st->indio_accel) { + sensor = INV_ICM45600_SENSOR_ACCEL; + watermark = &st->fifo.watermark.accel; + } else { + return -EINVAL; + } + + sleep = 0; + scoped_guard(mutex, &st->lock) + ret = _inv_icm45600_buffer_postdisable(st, sensor, watermark, &sleep); + + /* Sleep required time. */ + if (sleep) + msleep(sleep); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +const struct iio_buffer_setup_ops inv_icm45600_buffer_ops = { + .preenable = inv_icm45600_buffer_preenable, + .postenable = inv_icm45600_buffer_postenable, + .predisable = inv_icm45600_buffer_predisable, + .postdisable = inv_icm45600_buffer_postdisable, +}; + +int inv_icm45600_buffer_fifo_read(struct inv_icm45600_state *st, + unsigned int max) +{ + const ssize_t packet_size = sizeof(struct inv_icm45600_fifo_2sensors_packet); + __le16 *raw_fifo_count; + size_t fifo_nb, i; + ssize_t size; + const struct inv_icm45600_fifo_sensor_data *accel, *gyro; + const __le16 *timestamp; + const s8 *temp; + unsigned int odr; + int ret; + + /* Reset all samples counters. */ + st->fifo.count = 0; + st->fifo.nb.gyro = 0; + st->fifo.nb.accel = 0; + st->fifo.nb.total = 0; + + raw_fifo_count = &st->buffer.u16; + ret = regmap_bulk_read(st->map, INV_ICM45600_REG_FIFO_COUNT, + raw_fifo_count, sizeof(*raw_fifo_count)); + if (ret) + return ret; + + /* Check and limit number of samples if requested. */ + fifo_nb = le16_to_cpup(raw_fifo_count); + if (fifo_nb == 0) + return 0; + if (max > 0 && fifo_nb > max) + fifo_nb = max; + + /* Try to read all FIFO data in internal buffer. */ + st->fifo.count = fifo_nb * packet_size; + ret = regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA, + st->fifo.data, st->fifo.count); + if (ret == -ENOTSUPP || ret == -EFBIG) { + /* Read full fifo is not supported, read samples one by one. */ + ret = 0; + for (i = 0; i < st->fifo.count && ret == 0; i += packet_size) + ret = regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA, + &st->fifo.data[i], packet_size); + } + if (ret) + return ret; + + for (i = 0; i < st->fifo.count; i += size) { + size = inv_icm45600_fifo_decode_packet(&st->fifo.data[i], &accel, &gyro, + &temp, ×tamp, &odr); + if (size <= 0) + /* No more sample in buffer */ + break; + if (gyro && inv_icm45600_fifo_is_data_valid(gyro)) + st->fifo.nb.gyro++; + if (accel && inv_icm45600_fifo_is_data_valid(accel)) + st->fifo.nb.accel++; + st->fifo.nb.total++; + } + + return 0; +} + +int inv_icm45600_buffer_fifo_parse(struct inv_icm45600_state *st) +{ + struct inv_icm45600_sensor_state *gyro_st = iio_priv(st->indio_gyro); + struct inv_icm45600_sensor_state *accel_st = iio_priv(st->indio_accel); + struct inv_sensors_timestamp *ts; + int ret; + + if (st->fifo.nb.total == 0) + return 0; + + /* Handle gyroscope timestamp and FIFO data parsing. */ + if (st->fifo.nb.gyro > 0) { + ts = &gyro_st->ts; + inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_gyro, + st->timestamp.gyro); + ret = inv_icm45600_gyro_parse_fifo(st->indio_gyro); + if (ret) + return ret; + } + + /* Handle accelerometer timestamp and FIFO data parsing. */ + if (st->fifo.nb.accel > 0) { + ts = &accel_st->ts; + inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_accel, + st->timestamp.accel); + ret = inv_icm45600_accel_parse_fifo(st->indio_accel); + if (ret) + return ret; + } + + return 0; +} + +int inv_icm45600_buffer_hwfifo_flush(struct inv_icm45600_state *st, + unsigned int count) +{ + struct inv_icm45600_sensor_state *gyro_st = iio_priv(st->indio_gyro); + struct inv_icm45600_sensor_state *accel_st = iio_priv(st->indio_accel); + struct inv_sensors_timestamp *ts; + s64 gyro_ts, accel_ts; + int ret; + + gyro_ts = iio_get_time_ns(st->indio_gyro); + accel_ts = iio_get_time_ns(st->indio_accel); + + ret = inv_icm45600_buffer_fifo_read(st, count); + if (ret) + return ret; + + if (st->fifo.nb.total == 0) + return 0; + + if (st->fifo.nb.gyro > 0) { + ts = &gyro_st->ts; + inv_sensors_timestamp_interrupt(ts, st->fifo.nb.gyro, gyro_ts); + ret = inv_icm45600_gyro_parse_fifo(st->indio_gyro); + if (ret) + return ret; + } + + if (st->fifo.nb.accel > 0) { + ts = &accel_st->ts; + inv_sensors_timestamp_interrupt(ts, st->fifo.nb.accel, accel_ts); + ret = inv_icm45600_accel_parse_fifo(st->indio_accel); + if (ret) + return ret; + } + + return 0; +} + +int inv_icm45600_buffer_init(struct inv_icm45600_state *st) +{ + int ret; + unsigned int val; + + st->fifo.watermark.eff_gyro = 1; + st->fifo.watermark.eff_accel = 1; + + /* Disable all FIFO EN bits. */ + ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG3, 0); + if (ret) + return ret; + + /* Disable FIFO and set depth. */ + val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS) | + FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MASK, + INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MAX); + + ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG0, val); + if (ret) + return ret; + + /* Enable only timestamp in fifo, disable compression. */ + ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG4, + INV_ICM45600_FIFO_CONFIG4_TMST_FSYNC_EN); + if (ret) + return ret; + + /* Enable FIFO continuous watermark interrupt. */ + return regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2, + INV_ICM45600_REG_FIFO_CONFIG2_WM_GT_TH); +} diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.h b/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.h new file mode 100644 index 000000000000..e047871cdbe2 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.h @@ -0,0 +1,101 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* Copyright (C) 2025 Invensense, Inc. */ + +#ifndef INV_ICM45600_BUFFER_H_ +#define INV_ICM45600_BUFFER_H_ + +#include <linux/bits.h> +#include <linux/limits.h> +#include <linux/types.h> + +#include <asm/byteorder.h> + +#include <linux/iio/iio.h> + +struct inv_icm45600_state; + +#define INV_ICM45600_SENSOR_GYRO BIT(0) +#define INV_ICM45600_SENSOR_ACCEL BIT(1) +#define INV_ICM45600_SENSOR_TEMP BIT(2) + +/** + * struct inv_icm45600_fifo - FIFO state variables + * @on: reference counter for FIFO on. + * @en: bits field of INV_ICM45600_SENSOR_* for FIFO EN bits. + * @period: FIFO internal period. + * @watermark: watermark configuration values for accel and gyro. + * @watermark.gyro: requested watermark for gyro. + * @watermark.accel: requested watermark for accel. + * @watermark.eff_gyro: effective watermark for gyro. + * @watermark.eff_accel: effective watermark for accel. + * @count: number of bytes in the FIFO data buffer. + * @nb: gyro, accel and total samples in the FIFO data buffer. + * @data: FIFO data buffer aligned for DMA (8kB) + */ +struct inv_icm45600_fifo { + unsigned int on; + unsigned int en; + u32 period; + struct { + unsigned int gyro; + unsigned int accel; + unsigned int eff_gyro; + unsigned int eff_accel; + } watermark; + size_t count; + struct { + size_t gyro; + size_t accel; + size_t total; + } nb; + u8 *data; +}; + +/* FIFO data packet */ +struct inv_icm45600_fifo_sensor_data { + __le16 x; + __le16 y; + __le16 z; +} __packed; +#define INV_ICM45600_DATA_INVALID S16_MIN + +static inline bool +inv_icm45600_fifo_is_data_valid(const struct inv_icm45600_fifo_sensor_data *s) +{ + s16 x, y, z; + + x = le16_to_cpu(s->x); + y = le16_to_cpu(s->y); + z = le16_to_cpu(s->z); + + return (x != INV_ICM45600_DATA_INVALID || + y != INV_ICM45600_DATA_INVALID || + z != INV_ICM45600_DATA_INVALID); +} + +ssize_t inv_icm45600_fifo_decode_packet(const void *packet, + const struct inv_icm45600_fifo_sensor_data **accel, + const struct inv_icm45600_fifo_sensor_data **gyro, + const s8 **temp, + const __le16 **timestamp, unsigned int *odr); + +extern const struct iio_buffer_setup_ops inv_icm45600_buffer_ops; + +int inv_icm45600_buffer_init(struct inv_icm45600_state *st); + +void inv_icm45600_buffer_update_fifo_period(struct inv_icm45600_state *st); + +int inv_icm45600_buffer_set_fifo_en(struct inv_icm45600_state *st, + unsigned int fifo_en); + +int inv_icm45600_buffer_update_watermark(struct inv_icm45600_state *st); + +int inv_icm45600_buffer_fifo_read(struct inv_icm45600_state *st, + unsigned int max); + +int inv_icm45600_buffer_fifo_parse(struct inv_icm45600_state *st); + +int inv_icm45600_buffer_hwfifo_flush(struct inv_icm45600_state *st, + unsigned int count); + +#endif diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_core.c b/drivers/iio/imu/inv_icm45600/inv_icm45600_core.c new file mode 100644 index 000000000000..ab1cb7b9dba4 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_core.c @@ -0,0 +1,988 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* Copyright (C) 2025 Invensense, Inc. */ + +#include <linux/bitfield.h> +#include <linux/delay.h> +#include <linux/device.h> +#include <linux/err.h> +#include <linux/interrupt.h> +#include <linux/irq.h> +#include <linux/limits.h> +#include <linux/minmax.h> +#include <linux/module.h> +#include <linux/mutex.h> +#include <linux/pm_runtime.h> +#include <linux/property.h> +#include <linux/regmap.h> +#include <linux/regulator/consumer.h> +#include <linux/time.h> +#include <linux/types.h> + +#include <asm/byteorder.h> + +#include <linux/iio/iio.h> + +#include "inv_icm45600_buffer.h" +#include "inv_icm45600.h" + +static int inv_icm45600_ireg_read(struct regmap *map, unsigned int reg, + u8 *data, size_t count) +{ + const struct device *dev = regmap_get_device(map); + struct inv_icm45600_state *st = dev_get_drvdata(dev); + unsigned int d; + size_t i; + int ret; + + st->buffer.ireg[0] = FIELD_GET(INV_ICM45600_REG_BANK_MASK, reg); + st->buffer.ireg[1] = FIELD_GET(INV_ICM45600_REG_ADDR_MASK, reg); + + /* Burst write address. */ + ret = regmap_bulk_write(map, INV_ICM45600_REG_IREG_ADDR, st->buffer.ireg, 2); + /* + * Wait while the device is busy processing the address. + * Datasheet: 13.3 MINIMUM WAIT TIME-GAP + */ + fsleep(INV_ICM45600_IREG_DELAY_US); + if (ret) + return ret; + + /* Read the data. */ + for (i = 0; i < count; i++) { + ret = regmap_read(map, INV_ICM45600_REG_IREG_DATA, &d); + /* + * Wait while the device is busy processing the address. + * Datasheet: 13.3 MINIMUM WAIT TIME-GAP + */ + fsleep(INV_ICM45600_IREG_DELAY_US); + if (ret) + return ret; + data[i] = d; + } + + return 0; +} + +static int inv_icm45600_ireg_write(struct regmap *map, unsigned int reg, + const u8 *data, size_t count) +{ + const struct device *dev = regmap_get_device(map); + struct inv_icm45600_state *st = dev_get_drvdata(dev); + size_t i; + int ret; + + st->buffer.ireg[0] = FIELD_GET(INV_ICM45600_REG_BANK_MASK, reg); + st->buffer.ireg[1] = FIELD_GET(INV_ICM45600_REG_ADDR_MASK, reg); + st->buffer.ireg[2] = data[0]; + + /* Burst write address and first byte. */ + ret = regmap_bulk_write(map, INV_ICM45600_REG_IREG_ADDR, st->buffer.ireg, 3); + /* + * Wait while the device is busy processing the address. + * Datasheet: 13.3 MINIMUM WAIT TIME-GAP + */ + fsleep(INV_ICM45600_IREG_DELAY_US); + if (ret) + return ret; + + /* Write the remaining bytes. */ + for (i = 1; i < count; i++) { + ret = regmap_write(map, INV_ICM45600_REG_IREG_DATA, data[i]); + /* + * Wait while the device is busy processing the address. + * Datasheet: 13.3 MINIMUM WAIT TIME-GAP + */ + fsleep(INV_ICM45600_IREG_DELAY_US); + if (ret) + return ret; + } + + return 0; +} + +static int inv_icm45600_read(void *context, const void *reg_buf, size_t reg_size, + void *val_buf, size_t val_size) +{ + unsigned int reg = be16_to_cpup(reg_buf); + struct regmap *map = context; + + if (FIELD_GET(INV_ICM45600_REG_BANK_MASK, reg)) + return inv_icm45600_ireg_read(map, reg, val_buf, val_size); + + return regmap_bulk_read(map, FIELD_GET(INV_ICM45600_REG_ADDR_MASK, reg), + val_buf, val_size); +} + +static int inv_icm45600_write(void *context, const void *data, size_t count) +{ + const u8 *d = data; + unsigned int reg = be16_to_cpup(data); + struct regmap *map = context; + + if (FIELD_GET(INV_ICM45600_REG_BANK_MASK, reg)) + return inv_icm45600_ireg_write(map, reg, d + 2, count - 2); + + return regmap_bulk_write(map, FIELD_GET(INV_ICM45600_REG_ADDR_MASK, reg), + d + 2, count - 2); +} + +static const struct regmap_bus inv_icm45600_regmap_bus = { + .read = inv_icm45600_read, + .write = inv_icm45600_write, +}; + +static const struct regmap_config inv_icm45600_regmap_config = { + .reg_bits = 16, + .val_bits = 8, +}; + +/* These are the chip initial default configurations (default FS value is based on icm45686) */ +static const struct inv_icm45600_conf inv_icm45600_default_conf = { + .gyro = { + .mode = INV_ICM45600_SENSOR_MODE_OFF, + .fs = INV_ICM45686_GYRO_FS_2000DPS, + .odr = INV_ICM45600_ODR_800HZ_LN, + .filter = INV_ICM45600_GYRO_LP_AVG_SEL_8X, + }, + .accel = { + .mode = INV_ICM45600_SENSOR_MODE_OFF, + .fs = INV_ICM45686_ACCEL_FS_16G, + .odr = INV_ICM45600_ODR_800HZ_LN, + .filter = INV_ICM45600_ACCEL_LP_AVG_SEL_4X, + }, +}; + +static const struct inv_icm45600_conf inv_icm45686_default_conf = { + .gyro = { + .mode = INV_ICM45600_SENSOR_MODE_OFF, + .fs = INV_ICM45686_GYRO_FS_4000DPS, + .odr = INV_ICM45600_ODR_800HZ_LN, + .filter = INV_ICM45600_GYRO_LP_AVG_SEL_8X, + }, + .accel = { + .mode = INV_ICM45600_SENSOR_MODE_OFF, + .fs = INV_ICM45686_ACCEL_FS_32G, + .odr = INV_ICM45600_ODR_800HZ_LN, + .filter = INV_ICM45600_ACCEL_LP_AVG_SEL_4X, + }, +}; + +const struct inv_icm45600_chip_info inv_icm45605_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45605, + .name = "icm45605", + .conf = &inv_icm45600_default_conf, + .accel_scales = (const int *)inv_icm45600_accel_scale, + .accel_scales_len = INV_ICM45600_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45600_gyro_scale, + .gyro_scales_len = INV_ICM45600_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45605_chip_info, "IIO_ICM45600"); + +const struct inv_icm45600_chip_info inv_icm45606_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45606, + .name = "icm45606", + .conf = &inv_icm45600_default_conf, + .accel_scales = (const int *)inv_icm45600_accel_scale, + .accel_scales_len = INV_ICM45600_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45600_gyro_scale, + .gyro_scales_len = INV_ICM45600_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45606_chip_info, "IIO_ICM45600"); + +const struct inv_icm45600_chip_info inv_icm45608_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45608, + .name = "icm45608", + .conf = &inv_icm45600_default_conf, + .accel_scales = (const int *)inv_icm45600_accel_scale, + .accel_scales_len = INV_ICM45600_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45600_gyro_scale, + .gyro_scales_len = INV_ICM45600_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45608_chip_info, "IIO_ICM45600"); + +const struct inv_icm45600_chip_info inv_icm45634_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45634, + .name = "icm45634", + .conf = &inv_icm45600_default_conf, + .accel_scales = (const int *)inv_icm45600_accel_scale, + .accel_scales_len = INV_ICM45600_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45600_gyro_scale, + .gyro_scales_len = INV_ICM45600_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45634_chip_info, "IIO_ICM45600"); + +const struct inv_icm45600_chip_info inv_icm45686_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45686, + .name = "icm45686", + .conf = &inv_icm45686_default_conf, + .accel_scales = (const int *)inv_icm45686_accel_scale, + .accel_scales_len = INV_ICM45686_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45686_gyro_scale, + .gyro_scales_len = INV_ICM45686_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45686_chip_info, "IIO_ICM45600"); + +const struct inv_icm45600_chip_info inv_icm45687_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45687, + .name = "icm45687", + .conf = &inv_icm45686_default_conf, + .accel_scales = (const int *)inv_icm45686_accel_scale, + .accel_scales_len = INV_ICM45686_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45686_gyro_scale, + .gyro_scales_len = INV_ICM45686_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45687_chip_info, "IIO_ICM45600"); + +const struct inv_icm45600_chip_info inv_icm45688p_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45688P, + .name = "icm45688p", + .conf = &inv_icm45686_default_conf, + .accel_scales = (const int *)inv_icm45686_accel_scale, + .accel_scales_len = INV_ICM45686_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45686_gyro_scale, + .gyro_scales_len = INV_ICM45686_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45688p_chip_info, "IIO_ICM45600"); + +const struct inv_icm45600_chip_info inv_icm45689_chip_info = { + .whoami = INV_ICM45600_WHOAMI_ICM45689, + .name = "icm45689", + .conf = &inv_icm45686_default_conf, + .accel_scales = (const int *)inv_icm45686_accel_scale, + .accel_scales_len = INV_ICM45686_ACCEL_FS_MAX, + .gyro_scales = (const int *)inv_icm45686_gyro_scale, + .gyro_scales_len = INV_ICM45686_GYRO_FS_MAX, +}; +EXPORT_SYMBOL_NS_GPL(inv_icm45689_chip_info, "IIO_ICM45600"); + +const struct iio_mount_matrix * +inv_icm45600_get_mount_matrix(const struct iio_dev *indio_dev, + const struct iio_chan_spec *chan) +{ + const struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + + return &st->orientation; +} + +u32 inv_icm45600_odr_to_period(enum inv_icm45600_odr odr) +{ + static const u32 odr_periods[INV_ICM45600_ODR_MAX] = { + /* 3 first values are reserved, left to 0 */ + [INV_ICM45600_ODR_6400HZ_LN] = 156250, + [INV_ICM45600_ODR_3200HZ_LN] = 312500, + [INV_ICM45600_ODR_1600HZ_LN] = 625000, + [INV_ICM45600_ODR_800HZ_LN] = 1250000, + [INV_ICM45600_ODR_400HZ] = 2500000, + [INV_ICM45600_ODR_200HZ] = 5000000, + [INV_ICM45600_ODR_100HZ] = 10000000, + [INV_ICM45600_ODR_50HZ] = 20000000, + [INV_ICM45600_ODR_25HZ] = 40000000, + [INV_ICM45600_ODR_12_5HZ] = 80000000, + [INV_ICM45600_ODR_6_25HZ_LP] = 160000000, + [INV_ICM45600_ODR_3_125HZ_LP] = 320000000, + [INV_ICM45600_ODR_1_5625HZ_LP] = 640000000, + }; + + return odr_periods[odr]; +} + +static int inv_icm45600_set_pwr_mgmt0(struct inv_icm45600_state *st, + enum inv_icm45600_sensor_mode gyro, + enum inv_icm45600_sensor_mode accel, + unsigned int *sleep_ms) +{ + enum inv_icm45600_sensor_mode oldgyro = st->conf.gyro.mode; + enum inv_icm45600_sensor_mode oldaccel = st->conf.accel.mode; + unsigned int sleepval; + unsigned int val; + int ret; + + /* if nothing changed, exit */ + if (gyro == oldgyro && accel == oldaccel) + return 0; + + val = FIELD_PREP(INV_ICM45600_PWR_MGMT0_GYRO_MODE_MASK, gyro) | + FIELD_PREP(INV_ICM45600_PWR_MGMT0_ACCEL_MODE_MASK, accel); + ret = regmap_write(st->map, INV_ICM45600_REG_PWR_MGMT0, val); + if (ret) + return ret; + + st->conf.gyro.mode = gyro; + st->conf.accel.mode = accel; + + /* Compute the required wait time for sensors to stabilize. */ + sleepval = 0; + if (accel != oldaccel && oldaccel == INV_ICM45600_SENSOR_MODE_OFF) + sleepval = max(sleepval, INV_ICM45600_ACCEL_STARTUP_TIME_MS); + + if (gyro != oldgyro) { + if (oldgyro == INV_ICM45600_SENSOR_MODE_OFF) + sleepval = max(sleepval, INV_ICM45600_GYRO_STARTUP_TIME_MS); + else if (gyro == INV_ICM45600_SENSOR_MODE_OFF) + sleepval = max(sleepval, INV_ICM45600_GYRO_STOP_TIME_MS); + } + + /* Deferred sleep value if sleep pointer is provided or direct sleep */ + if (sleep_ms) + *sleep_ms = sleepval; + else if (sleepval) + msleep(sleepval); + + return 0; +} + +static void inv_icm45600_set_default_conf(struct inv_icm45600_sensor_conf *conf, + struct inv_icm45600_sensor_conf *oldconf) +{ + /* Sanitize missing values with current values. */ + if (conf->mode == U8_MAX) + conf->mode = oldconf->mode; + if (conf->fs == U8_MAX) + conf->fs = oldconf->fs; + if (conf->odr == U8_MAX) + conf->odr = oldconf->odr; + if (conf->filter == U8_MAX) + conf->filter = oldconf->filter; +} + +int inv_icm45600_set_accel_conf(struct inv_icm45600_state *st, + struct inv_icm45600_sensor_conf *conf, + unsigned int *sleep_ms) +{ + struct inv_icm45600_sensor_conf *oldconf = &st->conf.accel; + unsigned int val; + int ret; + + inv_icm45600_set_default_conf(conf, oldconf); + + /* Force the power mode against the ODR when sensor is on. */ + if (conf->mode > INV_ICM45600_SENSOR_MODE_STANDBY) { + if (conf->odr <= INV_ICM45600_ODR_800HZ_LN) { + conf->mode = INV_ICM45600_SENSOR_MODE_LOW_NOISE; + } else { + conf->mode = INV_ICM45600_SENSOR_MODE_LOW_POWER; + /* sanitize averaging value depending on ODR for low-power mode */ + /* maximum 1x @400Hz */ + if (conf->odr == INV_ICM45600_ODR_400HZ) + conf->filter = INV_ICM45600_ACCEL_LP_AVG_SEL_1X; + else + conf->filter = INV_ICM45600_ACCEL_LP_AVG_SEL_4X; + } + } + + /* Set accel fullscale & odr. */ + if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) { + val = FIELD_PREP(INV_ICM45600_ACCEL_CONFIG0_FS_MASK, conf->fs) | + FIELD_PREP(INV_ICM45600_ACCEL_CONFIG0_ODR_MASK, conf->odr); + ret = regmap_write(st->map, INV_ICM45600_REG_ACCEL_CONFIG0, val); + if (ret) + return ret; + oldconf->fs = conf->fs; + oldconf->odr = conf->odr; + } + + /* Set accel low-power average filter. */ + if (conf->filter != oldconf->filter) { + ret = regmap_write(st->map, INV_ICM45600_IPREG_SYS2_REG_129, + conf->filter); + if (ret) + return ret; + oldconf->filter = conf->filter; + } + + /* Update the sensor accel mode. */ + return inv_icm45600_set_pwr_mgmt0(st, st->conf.gyro.mode, conf->mode, + sleep_ms); +} + +int inv_icm45600_set_gyro_conf(struct inv_icm45600_state *st, + struct inv_icm45600_sensor_conf *conf, + unsigned int *sleep_ms) +{ + struct inv_icm45600_sensor_conf *oldconf = &st->conf.gyro; + unsigned int val; + int ret; + + inv_icm45600_set_default_conf(conf, oldconf); + + /* Force the power mode against ODR when sensor is on. */ + if (conf->mode > INV_ICM45600_SENSOR_MODE_STANDBY) { + if (conf->odr >= INV_ICM45600_ODR_6_25HZ_LP) { + conf->mode = INV_ICM45600_SENSOR_MODE_LOW_POWER; + conf->filter = INV_ICM45600_GYRO_LP_AVG_SEL_8X; + } else { + conf->mode = INV_ICM45600_SENSOR_MODE_LOW_NOISE; + } + } + + /* Set gyro fullscale & odr. */ + if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) { + val = FIELD_PREP(INV_ICM45600_GYRO_CONFIG0_FS_MASK, conf->fs) | + FIELD_PREP(INV_ICM45600_GYRO_CONFIG0_ODR_MASK, conf->odr); + ret = regmap_write(st->map, INV_ICM45600_REG_GYRO_CONFIG0, val); + if (ret) + return ret; + oldconf->fs = conf->fs; + oldconf->odr = conf->odr; + } + + /* Set gyro low-power average filter. */ + if (conf->filter != oldconf->filter) { + val = FIELD_PREP(INV_ICM45600_IPREG_SYS1_170_GYRO_LP_AVG_MASK, conf->filter); + ret = regmap_update_bits(st->map, INV_ICM45600_IPREG_SYS1_REG_170, + INV_ICM45600_IPREG_SYS1_170_GYRO_LP_AVG_MASK, val); + if (ret) + return ret; + oldconf->filter = conf->filter; + } + + /* Update the sensor gyro mode. */ + return inv_icm45600_set_pwr_mgmt0(st, conf->mode, st->conf.accel.mode, + sleep_ms); +} + +int inv_icm45600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg, + unsigned int writeval, unsigned int *readval) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + + guard(mutex)(&st->lock); + + if (readval) + return regmap_read(st->map, reg, readval); + else + return regmap_write(st->map, reg, writeval); +} + +static int inv_icm45600_set_conf(struct inv_icm45600_state *st, + const struct inv_icm45600_conf *conf) +{ + unsigned int val; + int ret; + + val = FIELD_PREP(INV_ICM45600_PWR_MGMT0_GYRO_MODE_MASK, conf->gyro.mode) | + FIELD_PREP(INV_ICM45600_PWR_MGMT0_ACCEL_MODE_MASK, conf->accel.mode); + ret = regmap_write(st->map, INV_ICM45600_REG_PWR_MGMT0, val); + if (ret) + return ret; + + val = FIELD_PREP(INV_ICM45600_GYRO_CONFIG0_FS_MASK, conf->gyro.fs) | + FIELD_PREP(INV_ICM45600_GYRO_CONFIG0_ODR_MASK, conf->gyro.odr); + ret = regmap_write(st->map, INV_ICM45600_REG_GYRO_CONFIG0, val); + if (ret) + return ret; + + val = FIELD_PREP(INV_ICM45600_ACCEL_CONFIG0_FS_MASK, conf->accel.fs) | + FIELD_PREP(INV_ICM45600_ACCEL_CONFIG0_ODR_MASK, conf->accel.odr); + ret = regmap_write(st->map, INV_ICM45600_REG_ACCEL_CONFIG0, val); + if (ret) + return ret; + + /* Save configuration. */ + st->conf = *conf; + + return 0; +} + +/** + * inv_icm45600_setup() - check and setup chip + * @st: driver internal state + * @chip_info: detected chip description + * @reset: define whether a reset is required or not + * @bus_setup: callback for setting up bus specific registers + * + * Returns: 0 on success, a negative error code otherwise. + */ +static int inv_icm45600_setup(struct inv_icm45600_state *st, + const struct inv_icm45600_chip_info *chip_info, + bool reset, inv_icm45600_bus_setup bus_setup) +{ + const struct device *dev = regmap_get_device(st->map); + unsigned int val; + int ret; + + /* Set chip bus configuration if specified. */ + if (bus_setup) { + ret = bus_setup(st); + if (ret) + return ret; + } + + /* Check chip self-identification value. */ + ret = regmap_read(st->map, INV_ICM45600_REG_WHOAMI, &val); + if (ret) + return ret; + if (val != chip_info->whoami) { + /* + * SPI interface has no ack mechanism. + * 0xFF or 0x00 whoami means no response from the device. + */ + if (val == U8_MAX || val == 0) + return dev_err_probe(dev, -ENODEV, + "Invalid whoami %#02x expected %#02x (%s)\n", + val, chip_info->whoami, chip_info->name); + + dev_warn(dev, "Unexpected whoami %#02x expected %#02x (%s)\n", + val, chip_info->whoami, chip_info->name); + } + + st->chip_info = chip_info; + + if (reset) { + /* Reset previous state. */ + ret = regmap_write(st->map, INV_ICM45600_REG_MISC2, + INV_ICM45600_MISC2_SOFT_RESET); + if (ret) + return ret; + /* + * IMU reset time. + * Datasheet: 16.84 REG_MISC2 + */ + fsleep(USEC_PER_MSEC); + + if (bus_setup) { + ret = bus_setup(st); + if (ret) + return ret; + } + + ret = regmap_read(st->map, INV_ICM45600_REG_INT_STATUS, &val); + if (ret) + return ret; + if (!(val & INV_ICM45600_INT_STATUS_RESET_DONE)) { + dev_err(dev, "reset error, reset done bit not set\n"); + return -ENODEV; + } + } + + return inv_icm45600_set_conf(st, chip_info->conf); +} + +static irqreturn_t inv_icm45600_irq_timestamp(int irq, void *_data) +{ + struct inv_icm45600_state *st = _data; + + st->timestamp.gyro = iio_get_time_ns(st->indio_gyro); + st->timestamp.accel = iio_get_time_ns(st->indio_accel); + + return IRQ_WAKE_THREAD; +} + +static irqreturn_t inv_icm45600_irq_handler(int irq, void *_data) +{ + struct inv_icm45600_state *st = _data; + struct device *dev = regmap_get_device(st->map); + unsigned int mask, status; + int ret; + + guard(mutex)(&st->lock); + + ret = regmap_read(st->map, INV_ICM45600_REG_INT_STATUS, &status); + if (ret) + return IRQ_HANDLED; + + /* Read the FIFO data. */ + mask = INV_ICM45600_INT_STATUS_FIFO_THS | INV_ICM45600_INT_STATUS_FIFO_FULL; + if (status & mask) { + ret = inv_icm45600_buffer_fifo_read(st, 0); + if (ret) { + dev_err(dev, "FIFO read error %d\n", ret); + return IRQ_HANDLED; + } + ret = inv_icm45600_buffer_fifo_parse(st); + if (ret) + dev_err(dev, "FIFO parsing error %d\n", ret); + } + + /* FIFO full warning. */ + if (status & INV_ICM45600_INT_STATUS_FIFO_FULL) + dev_warn(dev, "FIFO full possible data lost!\n"); + + return IRQ_HANDLED; +} + +/** + * inv_icm45600_irq_init() - initialize int pin and interrupt handler + * @st: driver internal state + * @irq: irq number + * @irq_type: irq trigger type + * @open_drain: true if irq is open drain, false for push-pull + * + * Returns: 0 on success, a negative error code otherwise. + */ +static int inv_icm45600_irq_init(struct inv_icm45600_state *st, int irq, + int irq_type, bool open_drain) +{ + struct device *dev = regmap_get_device(st->map); + unsigned int val; + int ret; + + /* Configure INT1 interrupt: default is active low on edge. */ + switch (irq_type) { + case IRQF_TRIGGER_RISING: + case IRQF_TRIGGER_HIGH: + val = INV_ICM45600_INT1_CONFIG2_ACTIVE_HIGH; + break; + default: + val = INV_ICM45600_INT1_CONFIG2_ACTIVE_LOW; + break; + } + + switch (irq_type) { + case IRQF_TRIGGER_LOW: + case IRQF_TRIGGER_HIGH: + val |= INV_ICM45600_INT1_CONFIG2_LATCHED; + break; + default: + break; + } + + if (!open_drain) + val |= INV_ICM45600_INT1_CONFIG2_PUSH_PULL; + + ret = regmap_write(st->map, INV_ICM45600_REG_INT1_CONFIG2, val); + if (ret) + return ret; + + return devm_request_threaded_irq(dev, irq, inv_icm45600_irq_timestamp, + inv_icm45600_irq_handler, irq_type | IRQF_ONESHOT, + "inv_icm45600", st); +} + +static int inv_icm45600_timestamp_setup(struct inv_icm45600_state *st) +{ + /* Enable timestamps. */ + return regmap_set_bits(st->map, INV_ICM45600_REG_SMC_CONTROL_0, + INV_ICM45600_SMC_CONTROL_0_TMST_EN); +} + +static int inv_icm45600_enable_regulator_vddio(struct inv_icm45600_state *st) +{ + int ret; + + ret = regulator_enable(st->vddio_supply); + if (ret) + return ret; + + /* + * Wait a little for supply ramp. + * Duration is empirically defined. + */ + fsleep(3 * USEC_PER_MSEC); + + return 0; +} + +static void inv_icm45600_disable_vddio_reg(void *_data) +{ + struct inv_icm45600_state *st = _data; + struct device *dev = regmap_get_device(st->map); + + if (pm_runtime_status_suspended(dev)) + return; + + regulator_disable(st->vddio_supply); +} + +int inv_icm45600_core_probe(struct regmap *regmap, const struct inv_icm45600_chip_info *chip_info, + bool reset, inv_icm45600_bus_setup bus_setup) +{ + struct device *dev = regmap_get_device(regmap); + struct inv_icm45600_state *st; + struct regmap *regmap_custom; + struct fwnode_handle *fwnode; + int irq, irq_type; + bool open_drain; + int ret; + + /* Get INT1 only supported interrupt. */ + fwnode = dev_fwnode(dev); + irq = fwnode_irq_get_byname(fwnode, "int1"); + if (irq < 0) + return dev_err_probe(dev, irq, "Missing int1 interrupt\n"); + + irq_type = irq_get_trigger_type(irq); + + open_drain = device_property_read_bool(dev, "drive-open-drain"); + + regmap_custom = devm_regmap_init(dev, &inv_icm45600_regmap_bus, regmap, + &inv_icm45600_regmap_config); + if (IS_ERR(regmap_custom)) + return dev_err_probe(dev, PTR_ERR(regmap_custom), "Failed to register regmap\n"); + + st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL); + if (!st) + return -ENOMEM; + + dev_set_drvdata(dev, st); + + st->fifo.data = devm_kzalloc(dev, 8192, GFP_KERNEL); + if (!st->fifo.data) + return -ENOMEM; + + ret = devm_mutex_init(dev, &st->lock); + if (ret) + return ret; + + st->map = regmap_custom; + + ret = iio_read_mount_matrix(dev, &st->orientation); + if (ret) + return dev_err_probe(dev, ret, "Failed to retrieve mounting matrix\n"); + + st->vddio_supply = devm_regulator_get(dev, "vddio"); + if (IS_ERR(st->vddio_supply)) + return PTR_ERR(st->vddio_supply); + + ret = devm_regulator_get_enable(dev, "vdd"); + if (ret) + return dev_err_probe(dev, ret, "Failed to get vdd regulator\n"); + + /* + * Supply ramp time + Start-up time. + * Datasheet: 3.3.2 A.C. Electrical Characteristics + */ + fsleep(5 * USEC_PER_MSEC); + + ret = inv_icm45600_enable_regulator_vddio(st); + if (ret) + return ret; + + ret = devm_add_action_or_reset(dev, inv_icm45600_disable_vddio_reg, st); + if (ret) + return ret; + + ret = inv_icm45600_setup(st, chip_info, reset, bus_setup); + if (ret) + return ret; + + ret = inv_icm45600_timestamp_setup(st); + if (ret) + return ret; + + ret = inv_icm45600_buffer_init(st); + if (ret) + return ret; + + st->indio_gyro = inv_icm45600_gyro_init(st); + if (IS_ERR(st->indio_gyro)) + return PTR_ERR(st->indio_gyro); + + st->indio_accel = inv_icm45600_accel_init(st); + if (IS_ERR(st->indio_accel)) + return PTR_ERR(st->indio_accel); + + ret = inv_icm45600_irq_init(st, irq, irq_type, open_drain); + if (ret) + return ret; + + ret = devm_pm_runtime_set_active_enabled(dev); + if (ret) + return ret; + + pm_runtime_get_noresume(dev); + pm_runtime_set_autosuspend_delay(dev, 2 * USEC_PER_MSEC); + pm_runtime_use_autosuspend(dev); + pm_runtime_put(dev); + + return 0; +} +EXPORT_SYMBOL_NS_GPL(inv_icm45600_core_probe, "IIO_ICM45600"); + +/* + * Suspend saves sensors state and turns everything off. + */ +static int inv_icm45600_suspend(struct device *dev) +{ + struct inv_icm45600_state *st = dev_get_drvdata(dev); + int ret; + + scoped_guard(mutex, &st->lock) { + /* Disable FIFO data streaming. */ + if (st->fifo.on) { + unsigned int val; + + /* Clear FIFO_CONFIG3_IF_EN before changing the FIFO configuration */ + ret = regmap_clear_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + if (ret) + return ret; + val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS); + ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + } + + /* Save sensors states */ + st->suspended.gyro = st->conf.gyro.mode; + st->suspended.accel = st->conf.accel.mode; + } + + return pm_runtime_force_suspend(dev); +} + +/* + * System resume gets the system back on and restores the sensors state. + * Manually put runtime power management in system active state. + */ +static int inv_icm45600_resume(struct device *dev) +{ + struct inv_icm45600_state *st = dev_get_drvdata(dev); + int ret; + + ret = pm_runtime_force_resume(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) { + /* Restore sensors state. */ + ret = inv_icm45600_set_pwr_mgmt0(st, st->suspended.gyro, + st->suspended.accel, NULL); + if (ret) + return ret; + + /* Restore FIFO data streaming. */ + if (st->fifo.on) { + struct inv_icm45600_sensor_state *gyro_st = iio_priv(st->indio_gyro); + struct inv_icm45600_sensor_state *accel_st = iio_priv(st->indio_accel); + unsigned int val; + + inv_sensors_timestamp_reset(&gyro_st->ts); + inv_sensors_timestamp_reset(&accel_st->ts); + val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK, + INV_ICM45600_FIFO_CONFIG0_MODE_STREAM); + ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0, + INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val); + if (ret) + return ret; + /* FIFO_CONFIG3_IF_EN must only be set at end of FIFO the configuration */ + ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, + INV_ICM45600_FIFO_CONFIG3_IF_EN); + if (ret) + return ret; + } + } + + return ret; +} + +/* Runtime suspend will turn off sensors that are enabled by iio devices. */ +static int inv_icm45600_runtime_suspend(struct device *dev) +{ + struct inv_icm45600_state *st = dev_get_drvdata(dev); + int ret; + + guard(mutex)(&st->lock); + + /* disable all sensors */ + ret = inv_icm45600_set_pwr_mgmt0(st, INV_ICM45600_SENSOR_MODE_OFF, + INV_ICM45600_SENSOR_MODE_OFF, NULL); + if (ret) + return ret; + + regulator_disable(st->vddio_supply); + + return 0; +} + +/* Sensors are enabled by iio devices, no need to turn them back on here. */ +static int inv_icm45600_runtime_resume(struct device *dev) +{ + struct inv_icm45600_state *st = dev_get_drvdata(dev); + + guard(mutex)(&st->lock); + + return inv_icm45600_enable_regulator_vddio(st); +} + +static int _inv_icm45600_temp_read(struct inv_icm45600_state *st, s16 *temp) +{ + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + /* Make sure a sensor is on. */ + if (st->conf.gyro.mode == INV_ICM45600_SENSOR_MODE_OFF && + st->conf.accel.mode == INV_ICM45600_SENSOR_MODE_OFF) { + conf.mode = INV_ICM45600_SENSOR_MODE_LOW_POWER; + ret = inv_icm45600_set_accel_conf(st, &conf, NULL); + if (ret) + return ret; + } + + ret = regmap_bulk_read(st->map, INV_ICM45600_REG_TEMP_DATA, + &st->buffer.u16, sizeof(st->buffer.u16)); + if (ret) + return ret; + + *temp = (s16)le16_to_cpup(&st->buffer.u16); + if (*temp == INV_ICM45600_DATA_INVALID) + return -EINVAL; + + return 0; +} + +static int inv_icm45600_temp_read(struct inv_icm45600_state *st, s16 *temp) +{ + struct device *dev = regmap_get_device(st->map); + int ret; + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = _inv_icm45600_temp_read(st, temp); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +int inv_icm45600_temp_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + s16 temp; + int ret; + + if (chan->type != IIO_TEMP) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + ret = inv_icm45600_temp_read(st, &temp); + if (ret) + return ret; + *val = temp; + return IIO_VAL_INT; + /* + * T°C = (temp / 128) + 25 + * Tm°C = 1000 * ((temp * 100 / 12800) + 25) + * scale: 100000 / 13248 = 7.8125 + * offset: 25000 + */ + case IIO_CHAN_INFO_SCALE: + *val = 7; + *val2 = 812500; + return IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_OFFSET: + *val = 25000; + return IIO_VAL_INT; + default: + return -EINVAL; + } +} + +EXPORT_NS_GPL_DEV_PM_OPS(inv_icm45600_pm_ops, IIO_ICM45600) = { + SYSTEM_SLEEP_PM_OPS(inv_icm45600_suspend, inv_icm45600_resume) + RUNTIME_PM_OPS(inv_icm45600_runtime_suspend, + inv_icm45600_runtime_resume, NULL) +}; + +MODULE_AUTHOR("InvenSense, Inc."); +MODULE_DESCRIPTION("InvenSense ICM-456xx device driver"); +MODULE_LICENSE("GPL"); +MODULE_IMPORT_NS("IIO_INV_SENSORS_TIMESTAMP"); diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_gyro.c b/drivers/iio/imu/inv_icm45600/inv_icm45600_gyro.c new file mode 100644 index 000000000000..1e85fd0e4ea9 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_gyro.c @@ -0,0 +1,791 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Copyright (C) 2025 Invensense, Inc. + */ + +#include <linux/delay.h> +#include <linux/device.h> +#include <linux/err.h> +#include <linux/math64.h> +#include <linux/mutex.h> +#include <linux/pm_runtime.h> +#include <linux/regmap.h> +#include <linux/types.h> + +#include <linux/iio/buffer.h> +#include <linux/iio/common/inv_sensors_timestamp.h> +#include <linux/iio/iio.h> +#include <linux/iio/kfifo_buf.h> + +#include "inv_icm45600_buffer.h" +#include "inv_icm45600.h" + +enum inv_icm45600_gyro_scan { + INV_ICM45600_GYRO_SCAN_X, + INV_ICM45600_GYRO_SCAN_Y, + INV_ICM45600_GYRO_SCAN_Z, + INV_ICM45600_GYRO_SCAN_TEMP, + INV_ICM45600_GYRO_SCAN_TIMESTAMP, +}; + +static const struct iio_chan_spec_ext_info inv_icm45600_gyro_ext_infos[] = { + IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm45600_get_mount_matrix), + { } +}; + +#define INV_ICM45600_GYRO_CHAN(_modifier, _index, _ext_info) \ + { \ + .type = IIO_ANGL_VEL, \ + .modified = 1, \ + .channel2 = _modifier, \ + .info_mask_separate = \ + BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_type = \ + BIT(IIO_CHAN_INFO_SCALE), \ + .info_mask_shared_by_type_available = \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_CALIBBIAS), \ + .info_mask_shared_by_all = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .info_mask_shared_by_all_available = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ + .ext_info = _ext_info, \ + } + +static const struct iio_chan_spec inv_icm45600_gyro_channels[] = { + INV_ICM45600_GYRO_CHAN(IIO_MOD_X, INV_ICM45600_GYRO_SCAN_X, + inv_icm45600_gyro_ext_infos), + INV_ICM45600_GYRO_CHAN(IIO_MOD_Y, INV_ICM45600_GYRO_SCAN_Y, + inv_icm45600_gyro_ext_infos), + INV_ICM45600_GYRO_CHAN(IIO_MOD_Z, INV_ICM45600_GYRO_SCAN_Z, + inv_icm45600_gyro_ext_infos), + INV_ICM45600_TEMP_CHAN(INV_ICM45600_GYRO_SCAN_TEMP), + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM45600_GYRO_SCAN_TIMESTAMP), +}; + +/* + * IIO buffer data: size must be a power of 2 and timestamp aligned + * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp + */ +struct inv_icm45600_gyro_buffer { + struct inv_icm45600_fifo_sensor_data gyro; + s16 temp; + aligned_s64 timestamp; +}; + +static const unsigned long inv_icm45600_gyro_scan_masks[] = { + /* 3-axis gyro + temperature */ + BIT(INV_ICM45600_GYRO_SCAN_X) | + BIT(INV_ICM45600_GYRO_SCAN_Y) | + BIT(INV_ICM45600_GYRO_SCAN_Z) | + BIT(INV_ICM45600_GYRO_SCAN_TEMP), + 0 +}; + +/* enable gyroscope sensor and FIFO write */ +static int inv_icm45600_gyro_update_scan_mode(struct iio_dev *indio_dev, + const unsigned long *scan_mask) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev); + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + unsigned int fifo_en = 0; + unsigned int sleep = 0; + int ret; + + scoped_guard(mutex, &st->lock) { + if (*scan_mask & BIT(INV_ICM45600_GYRO_SCAN_TEMP)) + fifo_en |= INV_ICM45600_SENSOR_TEMP; + + if (*scan_mask & (BIT(INV_ICM45600_GYRO_SCAN_X) | + BIT(INV_ICM45600_GYRO_SCAN_Y) | + BIT(INV_ICM45600_GYRO_SCAN_Z))) { + /* enable gyro sensor */ + conf.mode = gyro_st->power_mode; + ret = inv_icm45600_set_gyro_conf(st, &conf, &sleep); + if (ret) + return ret; + fifo_en |= INV_ICM45600_SENSOR_GYRO; + } + ret = inv_icm45600_buffer_set_fifo_en(st, fifo_en | st->fifo.en); + } + if (sleep) + msleep(sleep); + + return ret; +} + +static int _inv_icm45600_gyro_read_sensor(struct inv_icm45600_state *st, + struct inv_icm45600_sensor_state *gyro_st, + unsigned int reg, int *val) +{ + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + /* enable gyro sensor */ + conf.mode = gyro_st->power_mode; + ret = inv_icm45600_set_gyro_conf(st, &conf, NULL); + if (ret) + return ret; + + /* read gyro register data */ + ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16)); + if (ret) + return ret; + + *val = sign_extend32(le16_to_cpup(&st->buffer.u16), 15); + if (*val == INV_ICM45600_DATA_INVALID) + return -ENODATA; + + return 0; +} + +static int inv_icm45600_gyro_read_sensor(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int reg; + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM45600_REG_GYRO_DATA_X; + break; + case IIO_MOD_Y: + reg = INV_ICM45600_REG_GYRO_DATA_Y; + break; + case IIO_MOD_Z: + reg = INV_ICM45600_REG_GYRO_DATA_Z; + break; + default: + return -EINVAL; + } + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = _inv_icm45600_gyro_read_sensor(st, gyro_st, reg, val); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* IIO format int + nano */ +const int inv_icm45600_gyro_scale[][2] = { + /* +/- 2000dps => 0.001065264 rad/s */ + [INV_ICM45600_GYRO_FS_2000DPS] = { 0, 1065264 }, + /* +/- 1000dps => 0.000532632 rad/s */ + [INV_ICM45600_GYRO_FS_1000DPS] = { 0, 532632 }, + /* +/- 500dps => 0.000266316 rad/s */ + [INV_ICM45600_GYRO_FS_500DPS] = { 0, 266316 }, + /* +/- 250dps => 0.000133158 rad/s */ + [INV_ICM45600_GYRO_FS_250DPS] = { 0, 133158 }, + /* +/- 125dps => 0.000066579 rad/s */ + [INV_ICM45600_GYRO_FS_125DPS] = { 0, 66579 }, + /* +/- 62.5dps => 0.000033290 rad/s */ + [INV_ICM45600_GYRO_FS_62_5DPS] = { 0, 33290 }, + /* +/- 31.25dps => 0.000016645 rad/s */ + [INV_ICM45600_GYRO_FS_31_25DPS] = { 0, 16645 }, + /* +/- 15.625dps => 0.000008322 rad/s */ + [INV_ICM45600_GYRO_FS_15_625DPS] = { 0, 8322 }, +}; + +/* IIO format int + nano */ +const int inv_icm45686_gyro_scale[][2] = { + /* +/- 4000dps => 0.002130529 rad/s */ + [INV_ICM45686_GYRO_FS_4000DPS] = { 0, 2130529 }, + /* +/- 2000dps => 0.001065264 rad/s */ + [INV_ICM45686_GYRO_FS_2000DPS] = { 0, 1065264 }, + /* +/- 1000dps => 0.000532632 rad/s */ + [INV_ICM45686_GYRO_FS_1000DPS] = { 0, 532632 }, + /* +/- 500dps => 0.000266316 rad/s */ + [INV_ICM45686_GYRO_FS_500DPS] = { 0, 266316 }, + /* +/- 250dps => 0.000133158 rad/s */ + [INV_ICM45686_GYRO_FS_250DPS] = { 0, 133158 }, + /* +/- 125dps => 0.000066579 rad/s */ + [INV_ICM45686_GYRO_FS_125DPS] = { 0, 66579 }, + /* +/- 62.5dps => 0.000033290 rad/s */ + [INV_ICM45686_GYRO_FS_62_5DPS] = { 0, 33290 }, + /* +/- 31.25dps => 0.000016645 rad/s */ + [INV_ICM45686_GYRO_FS_31_25DPS] = { 0, 16645 }, + /* +/- 15.625dps => 0.000008322 rad/s */ + [INV_ICM45686_GYRO_FS_15_625DPS] = { 0, 8322 }, +}; + +static int inv_icm45600_gyro_read_scale(struct iio_dev *indio_dev, + int *val, int *val2) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev); + unsigned int idx; + + idx = st->conf.gyro.fs; + + /* Full scale register starts at 1 for not High FSR parts */ + if (gyro_st->scales == (const int *)&inv_icm45600_gyro_scale) + idx--; + + *val = gyro_st->scales[2 * idx]; + *val2 = gyro_st->scales[2 * idx + 1]; + return IIO_VAL_INT_PLUS_NANO; +} + +static int inv_icm45600_gyro_write_scale(struct iio_dev *indio_dev, + int val, int val2) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + for (idx = 0; idx < gyro_st->scales_len; idx += 2) { + if (val == gyro_st->scales[idx] && + val2 == gyro_st->scales[idx + 1]) + break; + } + if (idx == gyro_st->scales_len) + return -EINVAL; + + conf.fs = idx / 2; + + /* Full scale register starts at 1 for not High FSR parts */ + if (gyro_st->scales == (const int *)&inv_icm45600_gyro_scale) + conf.fs++; + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = inv_icm45600_set_gyro_conf(st, &conf, NULL); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* IIO format int + micro */ +static const int inv_icm45600_gyro_odr[] = { + 1, 562500, /* 1.5625Hz */ + 3, 125000, /* 3.125Hz */ + 6, 250000, /* 6.25Hz */ + 12, 500000, /* 12.5Hz */ + 25, 0, /* 25Hz */ + 50, 0, /* 50Hz */ + 100, 0, /* 100Hz */ + 200, 0, /* 200Hz */ + 400, 0, /* 400Hz */ + 800, 0, /* 800Hz */ + 1600, 0, /* 1.6kHz */ + 3200, 0, /* 3.2kHz */ + 6400, 0, /* 6.4kHz */ +}; + +static const int inv_icm45600_gyro_odr_conv[] = { + INV_ICM45600_ODR_1_5625HZ_LP, + INV_ICM45600_ODR_3_125HZ_LP, + INV_ICM45600_ODR_6_25HZ_LP, + INV_ICM45600_ODR_12_5HZ, + INV_ICM45600_ODR_25HZ, + INV_ICM45600_ODR_50HZ, + INV_ICM45600_ODR_100HZ, + INV_ICM45600_ODR_200HZ, + INV_ICM45600_ODR_400HZ, + INV_ICM45600_ODR_800HZ_LN, + INV_ICM45600_ODR_1600HZ_LN, + INV_ICM45600_ODR_3200HZ_LN, + INV_ICM45600_ODR_6400HZ_LN, +}; + +static int inv_icm45600_gyro_read_odr(struct inv_icm45600_state *st, + int *val, int *val2) +{ + unsigned int odr; + unsigned int i; + + odr = st->conf.gyro.odr; + + for (i = 0; i < ARRAY_SIZE(inv_icm45600_gyro_odr_conv); ++i) { + if (inv_icm45600_gyro_odr_conv[i] == odr) + break; + } + if (i >= ARRAY_SIZE(inv_icm45600_gyro_odr_conv)) + return -EINVAL; + + *val = inv_icm45600_gyro_odr[2 * i]; + *val2 = inv_icm45600_gyro_odr[2 * i + 1]; + + return IIO_VAL_INT_PLUS_MICRO; +} + +static int _inv_icm45600_gyro_write_odr(struct iio_dev *indio_dev, int odr) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev); + struct inv_sensors_timestamp *ts = &gyro_st->ts; + struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES; + int ret; + + conf.odr = odr; + ret = inv_sensors_timestamp_update_odr(ts, inv_icm45600_odr_to_period(conf.odr), + iio_buffer_enabled(indio_dev)); + if (ret) + return ret; + + if (st->conf.gyro.mode != INV_ICM45600_SENSOR_MODE_OFF) + conf.mode = gyro_st->power_mode; + + ret = inv_icm45600_set_gyro_conf(st, &conf, NULL); + if (ret) + return ret; + + inv_icm45600_buffer_update_fifo_period(st); + inv_icm45600_buffer_update_watermark(st); + + return 0; +} + +static int inv_icm45600_gyro_write_odr(struct iio_dev *indio_dev, + int val, int val2) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct device *dev = regmap_get_device(st->map); + unsigned int idx; + int odr; + int ret; + + for (idx = 0; idx < ARRAY_SIZE(inv_icm45600_gyro_odr); idx += 2) { + if (val == inv_icm45600_gyro_odr[idx] && + val2 == inv_icm45600_gyro_odr[idx + 1]) + break; + } + if (idx >= ARRAY_SIZE(inv_icm45600_gyro_odr)) + return -EINVAL; + + odr = inv_icm45600_gyro_odr_conv[idx / 2]; + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = _inv_icm45600_gyro_write_odr(indio_dev, odr); + + pm_runtime_put_autosuspend(dev); + + return ret; +} + +/* + * Calibration bias values, IIO range format int + nano. + * Value is limited to +/-62.5dps coded on 14 bits signed. Step is 7.5mdps. + */ +static int inv_icm45600_gyro_calibbias[] = { + -1, 90830336, /* min: -1.090830336 rad/s */ + 0, 133158, /* step: 0.000133158 rad/s */ + 1, 90697178, /* max: 1.090697178 rad/s */ +}; + +static int inv_icm45600_gyro_read_offset(struct inv_icm45600_state *st, + struct iio_chan_spec const *chan, + int *val, int *val2) +{ + struct device *dev = regmap_get_device(st->map); + s64 val64; + s32 bias; + unsigned int reg; + s16 offset; + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM45600_IPREG_SYS1_REG_42; + break; + case IIO_MOD_Y: + reg = INV_ICM45600_IPREG_SYS1_REG_56; + break; + case IIO_MOD_Z: + reg = INV_ICM45600_IPREG_SYS1_REG_70; + break; + default: + return -EINVAL; + } + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = regmap_bulk_read(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16)); + + pm_runtime_put_autosuspend(dev); + if (ret) + return ret; + + offset = le16_to_cpup(&st->buffer.u16) & INV_ICM45600_GYRO_OFFUSER_MASK; + /* 14 bits signed value */ + offset = sign_extend32(offset, 13); + + /* + * convert raw offset to dps then to rad/s + * 14 bits signed raw max 62.5 to dps: 625 / 81920 + * dps to rad: Pi / 180 + * result in nano (1000000000) + * (offset * 625 * Pi * 1000000000) / (81920 * 180) + */ + val64 = (s64)offset * 625LL * 3141592653LL; + /* for rounding, add + or - divisor (81920 * 180) divided by 2 */ + if (val64 >= 0) + val64 += 81920 * 180 / 2; + else + val64 -= 81920 * 180 / 2; + bias = div_s64(val64, 81920 * 180); + *val = bias / 1000000000L; + *val2 = bias % 1000000000L; + + return IIO_VAL_INT_PLUS_NANO; +} + +static int inv_icm45600_gyro_write_offset(struct inv_icm45600_state *st, + struct iio_chan_spec const *chan, + int val, int val2) +{ + struct device *dev = regmap_get_device(st->map); + s64 val64, min, max; + unsigned int reg; + s16 offset; + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (chan->channel2) { + case IIO_MOD_X: + reg = INV_ICM45600_IPREG_SYS1_REG_42; + break; + case IIO_MOD_Y: + reg = INV_ICM45600_IPREG_SYS1_REG_56; + break; + case IIO_MOD_Z: + reg = INV_ICM45600_IPREG_SYS1_REG_70; + break; + default: + return -EINVAL; + } + + /* inv_icm45600_gyro_calibbias: min - step - max in nano */ + min = (s64)inv_icm45600_gyro_calibbias[0] * 1000000000LL - + (s64)inv_icm45600_gyro_calibbias[1]; + max = (s64)inv_icm45600_gyro_calibbias[4] * 1000000000LL + + (s64)inv_icm45600_gyro_calibbias[5]; + val64 = (s64)val * 1000000000LL; + if (val >= 0) + val64 += (s64)val2; + else + val64 -= (s64)val2; + if (val64 < min || val64 > max) + return -EINVAL; + + /* + * convert rad/s to dps then to raw value + * rad to dps: 180 / Pi + * dps to raw 14 bits signed, max 62.5: 8192 / 62.5 + * val in nano (1000000000) + * val * 180 * 8192 / (Pi * 1000000000 * 62.5) + */ + val64 = val64 * 180LL * 8192; + /* for rounding, add + or - divisor (314159265 * 625) divided by 2 */ + if (val64 >= 0) + val64 += 314159265LL * 625LL / 2LL; + else + val64 -= 314159265LL * 625LL / 2LL; + offset = div64_s64(val64, 314159265LL * 625LL); + + /* clamp value limited to 14 bits signed */ + offset = clamp(offset, -8192, 8191); + + st->buffer.u16 = cpu_to_le16(offset & INV_ICM45600_GYRO_OFFUSER_MASK); + + ret = pm_runtime_resume_and_get(dev); + if (ret) + return ret; + + scoped_guard(mutex, &st->lock) + ret = regmap_bulk_write(st->map, reg, &st->buffer.u16, sizeof(st->buffer.u16)); + + pm_runtime_put_autosuspend(dev); + return ret; +} + +static int inv_icm45600_gyro_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + switch (chan->type) { + case IIO_ANGL_VEL: + break; + case IIO_TEMP: + return inv_icm45600_temp_read_raw(indio_dev, chan, val, val2, mask); + default: + return -EINVAL; + } + + switch (mask) { + case IIO_CHAN_INFO_RAW: + if (!iio_device_claim_direct(indio_dev)) + return -EBUSY; + ret = inv_icm45600_gyro_read_sensor(indio_dev, chan, val); + iio_device_release_direct(indio_dev); + if (ret) + return ret; + return IIO_VAL_INT; + case IIO_CHAN_INFO_SCALE: + return inv_icm45600_gyro_read_scale(indio_dev, val, val2); + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm45600_gyro_read_odr(st, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + return inv_icm45600_gyro_read_offset(st, chan, val, val2); + default: + return -EINVAL; + } +} + +static int inv_icm45600_gyro_read_avail(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + const int **vals, + int *type, int *length, long mask) +{ + struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev); + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + *vals = gyro_st->scales; + *type = IIO_VAL_INT_PLUS_NANO; + *length = gyro_st->scales_len; + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_SAMP_FREQ: + *vals = inv_icm45600_gyro_odr; + *type = IIO_VAL_INT_PLUS_MICRO; + *length = ARRAY_SIZE(inv_icm45600_gyro_odr); + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_CALIBBIAS: + *vals = inv_icm45600_gyro_calibbias; + *type = IIO_VAL_INT_PLUS_NANO; + return IIO_AVAIL_RANGE; + default: + return -EINVAL; + } +} + +static int inv_icm45600_gyro_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + if (!iio_device_claim_direct(indio_dev)) + return -EBUSY; + ret = inv_icm45600_gyro_write_scale(indio_dev, val, val2); + iio_device_release_direct(indio_dev); + return ret; + case IIO_CHAN_INFO_SAMP_FREQ: + return inv_icm45600_gyro_write_odr(indio_dev, val, val2); + case IIO_CHAN_INFO_CALIBBIAS: + if (!iio_device_claim_direct(indio_dev)) + return -EBUSY; + ret = inv_icm45600_gyro_write_offset(st, chan, val, val2); + iio_device_release_direct(indio_dev); + return ret; + default: + return -EINVAL; + } +} + +static int inv_icm45600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + long mask) +{ + if (chan->type != IIO_ANGL_VEL) + return -EINVAL; + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + return IIO_VAL_INT_PLUS_NANO; + case IIO_CHAN_INFO_SAMP_FREQ: + return IIO_VAL_INT_PLUS_MICRO; + case IIO_CHAN_INFO_CALIBBIAS: + return IIO_VAL_INT_PLUS_NANO; + default: + return -EINVAL; + } +} + +static int inv_icm45600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev, + unsigned int val) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + + guard(mutex)(&st->lock); + + st->fifo.watermark.gyro = val; + return inv_icm45600_buffer_update_watermark(st); +} + +static int inv_icm45600_gyro_hwfifo_flush(struct iio_dev *indio_dev, + unsigned int count) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + int ret; + + if (count == 0) + return 0; + + guard(mutex)(&st->lock); + + ret = inv_icm45600_buffer_hwfifo_flush(st, count); + if (ret) + return ret; + + return st->fifo.nb.gyro; +} + +static const struct iio_info inv_icm45600_gyro_info = { + .read_raw = inv_icm45600_gyro_read_raw, + .read_avail = inv_icm45600_gyro_read_avail, + .write_raw = inv_icm45600_gyro_write_raw, + .write_raw_get_fmt = inv_icm45600_gyro_write_raw_get_fmt, + .debugfs_reg_access = inv_icm45600_debugfs_reg, + .update_scan_mode = inv_icm45600_gyro_update_scan_mode, + .hwfifo_set_watermark = inv_icm45600_gyro_hwfifo_set_watermark, + .hwfifo_flush_to_buffer = inv_icm45600_gyro_hwfifo_flush, +}; + +struct iio_dev *inv_icm45600_gyro_init(struct inv_icm45600_state *st) +{ + struct device *dev = regmap_get_device(st->map); + struct inv_icm45600_sensor_state *gyro_st; + struct inv_sensors_timestamp_chip ts_chip; + struct iio_dev *indio_dev; + const char *name; + int ret; + + name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->chip_info->name); + if (!name) + return ERR_PTR(-ENOMEM); + + indio_dev = devm_iio_device_alloc(dev, sizeof(*gyro_st)); + if (!indio_dev) + return ERR_PTR(-ENOMEM); + gyro_st = iio_priv(indio_dev); + + gyro_st->scales = st->chip_info->gyro_scales; + gyro_st->scales_len = st->chip_info->gyro_scales_len * 2; + + /* low-noise by default at init */ + gyro_st->power_mode = INV_ICM45600_SENSOR_MODE_LOW_NOISE; + + /* + * clock period is 32kHz (31250ns) + * jitter is +/- 2% (20 per mille) + */ + ts_chip.clock_period = 31250; + ts_chip.jitter = 20; + ts_chip.init_period = inv_icm45600_odr_to_period(st->conf.gyro.odr); + inv_sensors_timestamp_init(&gyro_st->ts, &ts_chip); + + iio_device_set_drvdata(indio_dev, st); + indio_dev->name = name; + indio_dev->info = &inv_icm45600_gyro_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->channels = inv_icm45600_gyro_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_icm45600_gyro_channels); + indio_dev->available_scan_masks = inv_icm45600_gyro_scan_masks; + indio_dev->setup_ops = &inv_icm45600_buffer_ops; + + ret = devm_iio_kfifo_buffer_setup(dev, indio_dev, + &inv_icm45600_buffer_ops); + if (ret) + return ERR_PTR(ret); + + ret = devm_iio_device_register(dev, indio_dev); + if (ret) + return ERR_PTR(ret); + + return indio_dev; +} + +int inv_icm45600_gyro_parse_fifo(struct iio_dev *indio_dev) +{ + struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev); + struct inv_icm45600_sensor_state *gyro_st = iio_priv(indio_dev); + struct inv_sensors_timestamp *ts = &gyro_st->ts; + ssize_t i, size; + unsigned int no; + + /* parse all fifo packets */ + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) { + struct inv_icm45600_gyro_buffer buffer = { }; + const struct inv_icm45600_fifo_sensor_data *accel, *gyro; + const __le16 *timestamp; + const s8 *temp; + unsigned int odr; + s64 ts_val; + + size = inv_icm45600_fifo_decode_packet(&st->fifo.data[i], + &accel, &gyro, &temp, ×tamp, &odr); + /* quit if error or FIFO is empty */ + if (size <= 0) + return size; + + /* skip packet if no gyro data or data is invalid */ + if (gyro == NULL || !inv_icm45600_fifo_is_data_valid(gyro)) + continue; + + /* update odr */ + if (odr & INV_ICM45600_SENSOR_GYRO) + inv_sensors_timestamp_apply_odr(ts, st->fifo.period, + st->fifo.nb.total, no); + + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro)); + /* convert 8 bits FIFO temperature in high resolution format */ + buffer.temp = temp ? (*temp * 64) : 0; + ts_val = inv_sensors_timestamp_pop(ts); + iio_push_to_buffers_with_ts(indio_dev, &buffer, sizeof(buffer), ts_val); + } + + return 0; +} diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_i2c.c b/drivers/iio/imu/inv_icm45600/inv_icm45600_i2c.c new file mode 100644 index 000000000000..5ebc18121a11 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_i2c.c @@ -0,0 +1,98 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* Copyright (C) 2025 InvenSense, Inc. */ + +#include <linux/device.h> +#include <linux/err.h> +#include <linux/i2c.h> +#include <linux/module.h> +#include <linux/mod_devicetable.h> +#include <linux/regmap.h> + +#include "inv_icm45600.h" + +static const struct regmap_config inv_icm45600_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int inv_icm45600_probe(struct i2c_client *client) +{ + const struct inv_icm45600_chip_info *chip_info; + struct regmap *regmap; + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) + return -ENODEV; + + chip_info = device_get_match_data(&client->dev); + if (!chip_info) + return -ENODEV; + + regmap = devm_regmap_init_i2c(client, &inv_icm45600_regmap_config); + if (IS_ERR(regmap)) + return PTR_ERR(regmap); + + return inv_icm45600_core_probe(regmap, chip_info, true, NULL); +} + +/* + * The device id table is used to identify which device is + * supported by this driver. + */ +static const struct i2c_device_id inv_icm45600_id[] = { + { "icm45605", (kernel_ulong_t)&inv_icm45605_chip_info }, + { "icm45606", (kernel_ulong_t)&inv_icm45606_chip_info }, + { "icm45608", (kernel_ulong_t)&inv_icm45608_chip_info }, + { "icm45634", (kernel_ulong_t)&inv_icm45634_chip_info }, + { "icm45686", (kernel_ulong_t)&inv_icm45686_chip_info }, + { "icm45687", (kernel_ulong_t)&inv_icm45687_chip_info }, + { "icm45688p", (kernel_ulong_t)&inv_icm45688p_chip_info }, + { "icm45689", (kernel_ulong_t)&inv_icm45689_chip_info }, + { } +}; +MODULE_DEVICE_TABLE(i2c, inv_icm45600_id); + +static const struct of_device_id inv_icm45600_of_matches[] = { + { + .compatible = "invensense,icm45605", + .data = &inv_icm45605_chip_info, + }, { + .compatible = "invensense,icm45606", + .data = &inv_icm45606_chip_info, + }, { + .compatible = "invensense,icm45608", + .data = &inv_icm45608_chip_info, + }, { + .compatible = "invensense,icm45634", + .data = &inv_icm45634_chip_info, + }, { + .compatible = "invensense,icm45686", + .data = &inv_icm45686_chip_info, + }, { + .compatible = "invensense,icm45687", + .data = &inv_icm45687_chip_info, + }, { + .compatible = "invensense,icm45688p", + .data = &inv_icm45688p_chip_info, + }, { + .compatible = "invensense,icm45689", + .data = &inv_icm45689_chip_info, + }, + { } +}; +MODULE_DEVICE_TABLE(of, inv_icm45600_of_matches); + +static struct i2c_driver inv_icm45600_driver = { + .driver = { + .name = "inv-icm45600-i2c", + .of_match_table = inv_icm45600_of_matches, + .pm = pm_ptr(&inv_icm45600_pm_ops), + }, + .id_table = inv_icm45600_id, + .probe = inv_icm45600_probe, +}; +module_i2c_driver(inv_icm45600_driver); + +MODULE_AUTHOR("InvenSense, Inc."); +MODULE_DESCRIPTION("InvenSense ICM-456xx I2C driver"); +MODULE_LICENSE("GPL"); +MODULE_IMPORT_NS("IIO_ICM45600"); diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_i3c.c b/drivers/iio/imu/inv_icm45600/inv_icm45600_i3c.c new file mode 100644 index 000000000000..9247eae9b3e2 --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_i3c.c @@ -0,0 +1,79 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* Copyright (C) 2025 InvenSense, Inc. */ + +#include <linux/err.h> +#include <linux/mod_devicetable.h> +#include <linux/module.h> +#include <linux/regmap.h> + +#include <linux/i3c/device.h> +#include <linux/i3c/master.h> + +#include "inv_icm45600.h" + +static const struct regmap_config inv_icm45600_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static const struct i3c_device_id inv_icm45600_i3c_ids[] = { + I3C_DEVICE_EXTRA_INFO(0x0235, 0x0000, 0x0011, (void *)NULL), + I3C_DEVICE_EXTRA_INFO(0x0235, 0x0000, 0x0084, (void *)NULL), + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(i3c, inv_icm45600_i3c_ids); + +static const struct inv_icm45600_chip_info *i3c_chip_info[] = { + &inv_icm45605_chip_info, + &inv_icm45606_chip_info, + &inv_icm45608_chip_info, + &inv_icm45634_chip_info, + &inv_icm45686_chip_info, + &inv_icm45687_chip_info, + &inv_icm45688p_chip_info, + &inv_icm45689_chip_info, +}; + +static int inv_icm45600_i3c_probe(struct i3c_device *i3cdev) +{ + int ret; + unsigned int whoami; + struct regmap *regmap; + const int nb_chip = ARRAY_SIZE(i3c_chip_info); + int chip; + + regmap = devm_regmap_init_i3c(i3cdev, &inv_icm45600_regmap_config); + if (IS_ERR(regmap)) + return dev_err_probe(&i3cdev->dev, PTR_ERR(regmap), + "Failed to register i3c regmap %ld\n", PTR_ERR(regmap)); + + ret = regmap_read(regmap, INV_ICM45600_REG_WHOAMI, &whoami); + if (ret) + return dev_err_probe(&i3cdev->dev, ret, "Failed to read part id %d\n", whoami); + + for (chip = 0; chip < nb_chip; chip++) { + if (whoami == i3c_chip_info[chip]->whoami) + break; + } + + if (chip == nb_chip) + return dev_err_probe(&i3cdev->dev, -ENODEV, + "Failed to match part id %d\n", whoami); + + return inv_icm45600_core_probe(regmap, i3c_chip_info[chip], false, NULL); +} + +static struct i3c_driver inv_icm45600_driver = { + .driver = { + .name = "inv_icm45600_i3c", + .pm = pm_sleep_ptr(&inv_icm45600_pm_ops), + }, + .probe = inv_icm45600_i3c_probe, + .id_table = inv_icm45600_i3c_ids, +}; +module_i3c_driver(inv_icm45600_driver); + +MODULE_AUTHOR("Remi Buisson <remi.buisson@tdk.com>"); +MODULE_DESCRIPTION("InvenSense ICM-456xx i3c driver"); +MODULE_LICENSE("GPL"); +MODULE_IMPORT_NS("IIO_ICM45600"); diff --git a/drivers/iio/imu/inv_icm45600/inv_icm45600_spi.c b/drivers/iio/imu/inv_icm45600/inv_icm45600_spi.c new file mode 100644 index 000000000000..6288113a6d7c --- /dev/null +++ b/drivers/iio/imu/inv_icm45600/inv_icm45600_spi.c @@ -0,0 +1,108 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* Copyright (C) 2025 InvenSense, Inc. */ + +#include <linux/bitfield.h> +#include <linux/device.h> +#include <linux/err.h> +#include <linux/module.h> +#include <linux/mod_devicetable.h> +#include <linux/regmap.h> + +#include <linux/spi/spi.h> + +#include "inv_icm45600.h" + +static const struct regmap_config inv_icm45600_regmap_config = { + .reg_bits = 8, + .val_bits = 8, +}; + +static int inv_icm45600_spi_bus_setup(struct inv_icm45600_state *st) +{ + /* Set slew rates for SPI. */ + return regmap_update_bits(st->map, INV_ICM45600_REG_DRIVE_CONFIG0, + INV_ICM45600_DRIVE_CONFIG0_SPI_MASK, + FIELD_PREP(INV_ICM45600_DRIVE_CONFIG0_SPI_MASK, + INV_ICM45600_SPI_SLEW_RATE_5NS)); +} + +static int inv_icm45600_probe(struct spi_device *spi) +{ + const struct inv_icm45600_chip_info *chip_info; + struct regmap *regmap; + + chip_info = spi_get_device_match_data(spi); + if (!chip_info) + return -ENODEV; + + /* Use SPI specific regmap. */ + regmap = devm_regmap_init_spi(spi, &inv_icm45600_regmap_config); + if (IS_ERR(regmap)) + return PTR_ERR(regmap); + + return inv_icm45600_core_probe(regmap, chip_info, true, + inv_icm45600_spi_bus_setup); +} + +/* + * The device id table is used to identify which device is + * supported by this driver. + */ +static const struct spi_device_id inv_icm45600_id[] = { + { "icm45605", (kernel_ulong_t)&inv_icm45605_chip_info }, + { "icm45606", (kernel_ulong_t)&inv_icm45606_chip_info }, + { "icm45608", (kernel_ulong_t)&inv_icm45608_chip_info }, + { "icm45634", (kernel_ulong_t)&inv_icm45634_chip_info }, + { "icm45686", (kernel_ulong_t)&inv_icm45686_chip_info }, + { "icm45687", (kernel_ulong_t)&inv_icm45687_chip_info }, + { "icm45688p", (kernel_ulong_t)&inv_icm45688p_chip_info }, + { "icm45689", (kernel_ulong_t)&inv_icm45689_chip_info }, + { } +}; +MODULE_DEVICE_TABLE(spi, inv_icm45600_id); + +static const struct of_device_id inv_icm45600_of_matches[] = { + { + .compatible = "invensense,icm45605", + .data = &inv_icm45605_chip_info, + }, { + .compatible = "invensense,icm45606", + .data = &inv_icm45606_chip_info, + }, { + .compatible = "invensense,icm45608", + .data = &inv_icm45608_chip_info, + }, { + .compatible = "invensense,icm45634", + .data = &inv_icm45634_chip_info, + }, { + .compatible = "invensense,icm45686", + .data = &inv_icm45686_chip_info, + }, { + .compatible = "invensense,icm45687", + .data = &inv_icm45687_chip_info, + }, { + .compatible = "invensense,icm45688p", + .data = &inv_icm45688p_chip_info, + }, { + .compatible = "invensense,icm45689", + .data = &inv_icm45689_chip_info, + }, + { } +}; +MODULE_DEVICE_TABLE(of, inv_icm45600_of_matches); + +static struct spi_driver inv_icm45600_driver = { + .driver = { + .name = "inv-icm45600-spi", + .of_match_table = inv_icm45600_of_matches, + .pm = pm_ptr(&inv_icm45600_pm_ops), + }, + .id_table = inv_icm45600_id, + .probe = inv_icm45600_probe, +}; +module_spi_driver(inv_icm45600_driver); + +MODULE_AUTHOR("InvenSense, Inc."); +MODULE_DESCRIPTION("InvenSense ICM-456xx SPI driver"); +MODULE_LICENSE("GPL"); +MODULE_IMPORT_NS("IIO_ICM45600"); diff --git a/drivers/iio/imu/smi330/Kconfig b/drivers/iio/imu/smi330/Kconfig new file mode 100644 index 000000000000..856a315e15aa --- /dev/null +++ b/drivers/iio/imu/smi330/Kconfig @@ -0,0 +1,33 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# SMI330 IMU driver +# + +config SMI330 + tristate + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + +config SMI330_I2C + tristate "Bosch SMI330 I2C driver" + depends on I2C + select SMI330 + select REGMAP_I2C + help + Enable support for the Bosch SMI330 6-Axis IMU connected to I2C + interface. + + This driver can also be built as a module. If so, the module will be + called smi330_i2c. + +config SMI330_SPI + tristate "Bosch SMI330 SPI driver" + depends on SPI + select SMI330 + select REGMAP_SPI + help + Enable support for the Bosch SMI330 6-Axis IMU connected to SPI + interface. + + This driver can also be built as a module. If so, the module will be + called smi330_spi. diff --git a/drivers/iio/imu/smi330/Makefile b/drivers/iio/imu/smi330/Makefile new file mode 100644 index 000000000000..c663dcb5a9f2 --- /dev/null +++ b/drivers/iio/imu/smi330/Makefile @@ -0,0 +1,7 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for Bosch SMI330 IMU +# +obj-$(CONFIG_SMI330) += smi330_core.o +obj-$(CONFIG_SMI330_I2C) += smi330_i2c.o +obj-$(CONFIG_SMI330_SPI) += smi330_spi.o diff --git a/drivers/iio/imu/smi330/smi330.h b/drivers/iio/imu/smi330/smi330.h new file mode 100644 index 000000000000..a5c765645aaa --- /dev/null +++ b/drivers/iio/imu/smi330/smi330.h @@ -0,0 +1,25 @@ +/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */ +/* + * Copyright (c) 2025 Robert Bosch GmbH. + */ +#ifndef _SMI330_H +#define _SMI330_H + +#include <linux/iio/iio.h> + +enum { + SMI330_SCAN_ACCEL_X, + SMI330_SCAN_ACCEL_Y, + SMI330_SCAN_ACCEL_Z, + SMI330_SCAN_GYRO_X, + SMI330_SCAN_GYRO_Y, + SMI330_SCAN_GYRO_Z, + SMI330_SCAN_TIMESTAMP, + SMI330_SCAN_LEN = SMI330_SCAN_TIMESTAMP, +}; + +extern const struct regmap_config smi330_regmap_config; + +int smi330_core_probe(struct device *dev, struct regmap *regmap); + +#endif /* _SMI330_H */ diff --git a/drivers/iio/imu/smi330/smi330_core.c b/drivers/iio/imu/smi330/smi330_core.c new file mode 100644 index 000000000000..7564f12543e0 --- /dev/null +++ b/drivers/iio/imu/smi330/smi330_core.c @@ -0,0 +1,918 @@ +// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 +/* + * Copyright (c) 2025 Robert Bosch GmbH. + */ +#include <linux/bitfield.h> +#include <linux/delay.h> +#include <linux/interrupt.h> +#include <linux/irq.h> +#include <linux/module.h> +#include <linux/property.h> +#include <linux/regmap.h> +#include <linux/string.h> +#include <linux/units.h> + +#include <linux/iio/buffer.h> +#include <linux/iio/iio.h> +#include <linux/iio/trigger.h> +#include <linux/iio/trigger_consumer.h> +#include <linux/iio/triggered_buffer.h> + +#include "smi330.h" + +/* Register map */ +#define SMI330_CHIP_ID_REG 0x00 +#define SMI330_ERR_REG 0x01 +#define SMI330_STATUS_REG 0x02 +#define SMI330_ACCEL_X_REG 0x03 +#define SMI330_GYRO_X_REG 0x06 +#define SMI330_TEMP_REG 0x09 +#define SMI330_INT1_STATUS_REG 0x0D +#define SMI330_ACCEL_CFG_REG 0x20 +#define SMI330_GYRO_CFG_REG 0x21 +#define SMI330_IO_INT_CTRL_REG 0x38 +#define SMI330_INT_CONF_REG 0x39 +#define SMI330_INT_MAP1_REG 0x3A +#define SMI330_INT_MAP2_REG 0x3B +#define SMI330_CMD_REG 0x7E + +/* Register mask */ +#define SMI330_CHIP_ID_MASK GENMASK(7, 0) +#define SMI330_ERR_FATAL_MASK BIT(0) +#define SMI330_ERR_ACC_CONF_MASK BIT(5) +#define SMI330_ERR_GYR_CONF_MASK BIT(6) +#define SMI330_STATUS_POR_MASK BIT(0) +#define SMI330_INT_STATUS_ACC_GYR_DRDY_MASK GENMASK(13, 12) +#define SMI330_CFG_ODR_MASK GENMASK(3, 0) +#define SMI330_CFG_RANGE_MASK GENMASK(6, 4) +#define SMI330_CFG_BW_MASK BIT(7) +#define SMI330_CFG_AVG_NUM_MASK GENMASK(10, 8) +#define SMI330_CFG_MODE_MASK GENMASK(14, 12) +#define SMI330_IO_INT_CTRL_INT1_MASK GENMASK(2, 0) +#define SMI330_IO_INT_CTRL_INT2_MASK GENMASK(10, 8) +#define SMI330_INT_CONF_LATCH_MASK BIT(0) +#define SMI330_INT_MAP2_ACC_DRDY_MASK GENMASK(11, 10) +#define SMI330_INT_MAP2_GYR_DRDY_MASK GENMASK(9, 8) + +/* Register values */ +#define SMI330_IO_INT_CTRL_LVL BIT(0) +#define SMI330_IO_INT_CTRL_OD BIT(1) +#define SMI330_IO_INT_CTRL_EN BIT(2) +#define SMI330_CMD_SOFT_RESET 0xDEAF + +/* T°C = (temp / 512) + 23 */ +#define SMI330_TEMP_OFFSET 11776 /* 23 * 512 */ +#define SMI330_TEMP_SCALE 1953125 /* (1 / 512) * 1e9 */ + +#define SMI330_CHIP_ID 0x42 +#define SMI330_SOFT_RESET_DELAY 2000 + +/* Non-constant mask variant of FIELD_GET() and FIELD_PREP() */ +#define smi330_field_get(_mask, _reg) (((_reg) & (_mask)) >> (ffs(_mask) - 1)) +#define smi330_field_prep(_mask, _val) (((_val) << (ffs(_mask) - 1)) & (_mask)) + +#define SMI330_ACCEL_CHANNEL(_axis) { \ + .type = IIO_ACCEL, \ + .modified = 1, \ + .channel2 = IIO_MOD_##_axis, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ) | \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_type_available = \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_dir_available = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = SMI330_SCAN_ACCEL_##_axis, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +#define SMI330_GYRO_CHANNEL(_axis) { \ + .type = IIO_ANGL_VEL, \ + .modified = 1, \ + .channel2 = IIO_MOD_##_axis, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ + .info_mask_shared_by_type = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ) | \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_type_available = \ + BIT(IIO_CHAN_INFO_SCALE) | \ + BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO) | \ + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \ + .info_mask_shared_by_dir_available = \ + BIT(IIO_CHAN_INFO_SAMP_FREQ), \ + .scan_index = SMI330_SCAN_GYRO_##_axis, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +#define SMI330_TEMP_CHANNEL(_index) { \ + .type = IIO_TEMP, \ + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \ + BIT(IIO_CHAN_INFO_OFFSET) | \ + BIT(IIO_CHAN_INFO_SCALE), \ + .scan_index = _index, \ + .scan_type = { \ + .sign = 's', \ + .realbits = 16, \ + .storagebits = 16, \ + .endianness = IIO_LE, \ + }, \ +} + +enum smi330_accel_range { + SMI330_ACCEL_RANGE_2G = 0x00, + SMI330_ACCEL_RANGE_4G = 0x01, + SMI330_ACCEL_RANGE_8G = 0x02, + SMI330_ACCEL_RANGE_16G = 0x03 +}; + +enum smi330_gyro_range { + SMI330_GYRO_RANGE_125 = 0x0, + SMI330_GYRO_RANGE_250 = 0x01, + SMI330_GYRO_RANGE_500 = 0x02 +}; + +enum smi330_odr { + SMI330_ODR_12_5_HZ = 0x05, + SMI330_ODR_25_HZ = 0x06, + SMI330_ODR_50_HZ = 0x07, + SMI330_ODR_100_HZ = 0x08, + SMI330_ODR_200_HZ = 0x09, + SMI330_ODR_400_HZ = 0x0A, + SMI330_ODR_800_HZ = 0x0B, + SMI330_ODR_1600_HZ = 0x0C, + SMI330_ODR_3200_HZ = 0x0D, + SMI330_ODR_6400_HZ = 0x0E +}; + +enum smi330_avg_num { + SMI330_AVG_NUM_1 = 0x00, + SMI330_AVG_NUM_2 = 0x01, + SMI330_AVG_NUM_4 = 0x02, + SMI330_AVG_NUM_8 = 0x03, + SMI330_AVG_NUM_16 = 0x04, + SMI330_AVG_NUM_32 = 0x05, + SMI330_AVG_NUM_64 = 0x06 +}; + +enum smi330_mode { + SMI330_MODE_SUSPEND = 0x00, + SMI330_MODE_GYRO_DRIVE = 0x01, + SMI330_MODE_LOW_POWER = 0x03, + SMI330_MODE_NORMAL = 0x04, + SMI330_MODE_HIGH_PERF = 0x07 +}; + +enum smi330_bw { + SMI330_BW_2 = 0x00, /* ODR/2 */ + SMI330_BW_4 = 0x01 /* ODR/4 */ +}; + +enum smi330_operation_mode { + SMI330_POLLING, + SMI330_DATA_READY, +}; + +enum smi330_sensor { + SMI330_ACCEL, + SMI330_GYRO, +}; + +enum smi330_sensor_conf_select { + SMI330_ODR, + SMI330_RANGE, + SMI330_BW, + SMI330_AVG_NUM, +}; + +enum smi330_int_out { + SMI330_INT_DISABLED, + SMI330_INT_1, + SMI330_INT_2, +}; + +struct smi330_attributes { + int *reg_vals; + int *vals; + int len; + int type; + int mask; +}; + +struct smi330_cfg { + enum smi330_operation_mode op_mode; + enum smi330_int_out data_irq; +}; + +struct smi330_data { + struct regmap *regmap; + struct smi330_cfg cfg; + struct iio_trigger *trig; + IIO_DECLARE_BUFFER_WITH_TS(__le16, buf, SMI330_SCAN_LEN); +}; + +const struct regmap_config smi330_regmap_config = { + .reg_bits = 8, + .val_bits = 16, + .val_format_endian = REGMAP_ENDIAN_LITTLE, +}; +EXPORT_SYMBOL_NS_GPL(smi330_regmap_config, "IIO_SMI330"); + +static const struct iio_chan_spec smi330_channels[] = { + SMI330_ACCEL_CHANNEL(X), + SMI330_ACCEL_CHANNEL(Y), + SMI330_ACCEL_CHANNEL(Z), + SMI330_GYRO_CHANNEL(X), + SMI330_GYRO_CHANNEL(Y), + SMI330_GYRO_CHANNEL(Z), + SMI330_TEMP_CHANNEL(-1), /* No buffer support */ + IIO_CHAN_SOFT_TIMESTAMP(SMI330_SCAN_TIMESTAMP), +}; + +static const unsigned long smi330_avail_scan_masks[] = { + (BIT(SMI330_SCAN_ACCEL_X) | BIT(SMI330_SCAN_ACCEL_Y) | + BIT(SMI330_SCAN_ACCEL_Z) | BIT(SMI330_SCAN_GYRO_X) | + BIT(SMI330_SCAN_GYRO_Y) | BIT(SMI330_SCAN_GYRO_Z)), + 0 +}; + +static const struct smi330_attributes smi330_accel_scale_attr = { + .reg_vals = (int[]){ SMI330_ACCEL_RANGE_2G, SMI330_ACCEL_RANGE_4G, + SMI330_ACCEL_RANGE_8G, SMI330_ACCEL_RANGE_16G }, + .vals = (int[]){ 0, 61035, 0, 122070, 0, 244140, 0, 488281 }, + .len = 8, + .type = IIO_VAL_INT_PLUS_NANO, + .mask = SMI330_CFG_RANGE_MASK +}; + +static const struct smi330_attributes smi330_gyro_scale_attr = { + .reg_vals = (int[]){ SMI330_GYRO_RANGE_125, SMI330_GYRO_RANGE_250, + SMI330_GYRO_RANGE_500 }, + .vals = (int[]){ 0, 3814697, 0, 7629395, 0, 15258789 }, + .len = 6, + .type = IIO_VAL_INT_PLUS_NANO, + .mask = SMI330_CFG_RANGE_MASK +}; + +static const struct smi330_attributes smi330_average_attr = { + .reg_vals = (int[]){ SMI330_AVG_NUM_1, SMI330_AVG_NUM_2, + SMI330_AVG_NUM_4, SMI330_AVG_NUM_8, + SMI330_AVG_NUM_16, SMI330_AVG_NUM_32, + SMI330_AVG_NUM_64 }, + .vals = (int[]){ 1, 2, 4, 8, 16, 32, 64 }, + .len = 7, + .type = IIO_VAL_INT, + .mask = SMI330_CFG_AVG_NUM_MASK +}; + +static const struct smi330_attributes smi330_bandwidth_attr = { + .reg_vals = (int[]){ SMI330_BW_2, SMI330_BW_4 }, + .vals = (int[]){ 2, 4 }, + .len = 2, + .type = IIO_VAL_INT, + .mask = SMI330_CFG_BW_MASK +}; + +static const struct smi330_attributes smi330_odr_attr = { + .reg_vals = (int[]){ SMI330_ODR_12_5_HZ, SMI330_ODR_25_HZ, + SMI330_ODR_50_HZ, SMI330_ODR_100_HZ, + SMI330_ODR_200_HZ, SMI330_ODR_400_HZ, + SMI330_ODR_800_HZ, SMI330_ODR_1600_HZ, + SMI330_ODR_3200_HZ, SMI330_ODR_6400_HZ }, + .vals = (int[]){ 12, 25, 50, 100, 200, 400, 800, 1600, 3200, 6400 }, + .len = 10, + .type = IIO_VAL_INT, + .mask = SMI330_CFG_ODR_MASK +}; + +static int smi330_get_attributes(enum smi330_sensor_conf_select config, + enum smi330_sensor sensor, + const struct smi330_attributes **attr) +{ + switch (config) { + case SMI330_ODR: + *attr = &smi330_odr_attr; + return 0; + case SMI330_RANGE: + if (sensor == SMI330_ACCEL) + *attr = &smi330_accel_scale_attr; + else + *attr = &smi330_gyro_scale_attr; + return 0; + case SMI330_BW: + *attr = &smi330_bandwidth_attr; + return 0; + case SMI330_AVG_NUM: + *attr = &smi330_average_attr; + return 0; + default: + return -EINVAL; + } +} + +static int smi330_get_config_reg(enum smi330_sensor sensor, int *reg) +{ + switch (sensor) { + case SMI330_ACCEL: + *reg = SMI330_ACCEL_CFG_REG; + return 0; + case SMI330_GYRO: + *reg = SMI330_GYRO_CFG_REG; + return 0; + default: + return -EINVAL; + } +} + +static int smi330_get_sensor_config(struct smi330_data *data, + enum smi330_sensor sensor, + enum smi330_sensor_conf_select config, + int *value) + +{ + int ret, reg, reg_val, i; + const struct smi330_attributes *attr; + + ret = smi330_get_config_reg(sensor, ®); + if (ret) + return ret; + + ret = regmap_read(data->regmap, reg, ®_val); + if (ret) + return ret; + + ret = smi330_get_attributes(config, sensor, &attr); + if (ret) + return ret; + + reg_val = smi330_field_get(attr->mask, reg_val); + + if (attr->type == IIO_VAL_INT) { + for (i = 0; i < attr->len; i++) { + if (attr->reg_vals[i] == reg_val) { + *value = attr->vals[i]; + return 0; + } + } + } else { + for (i = 0; i < attr->len / 2; i++) { + if (attr->reg_vals[i] == reg_val) { + *value = attr->vals[2 * i + 1]; + return 0; + } + } + } + + return -EINVAL; +} + +static int smi330_set_sensor_config(struct smi330_data *data, + enum smi330_sensor sensor, + enum smi330_sensor_conf_select config, + int value) +{ + int ret, i, reg, reg_val, error; + const struct smi330_attributes *attr; + + ret = smi330_get_attributes(config, sensor, &attr); + if (ret) + return ret; + + for (i = 0; i < attr->len; i++) { + if (attr->vals[i] == value) { + if (attr->type == IIO_VAL_INT) + reg_val = attr->reg_vals[i]; + else + reg_val = attr->reg_vals[i / 2]; + break; + } + } + if (i == attr->len) + return -EINVAL; + + ret = smi330_get_config_reg(sensor, ®); + if (ret) + return ret; + + reg_val = smi330_field_prep(attr->mask, reg_val); + ret = regmap_update_bits(data->regmap, reg, attr->mask, reg_val); + if (ret) + return ret; + + ret = regmap_read(data->regmap, SMI330_ERR_REG, &error); + if (ret) + return ret; + + if (FIELD_GET(SMI330_ERR_ACC_CONF_MASK, error) || + FIELD_GET(SMI330_ERR_GYR_CONF_MASK, error)) + return -EIO; + + return 0; +} + +static int smi330_get_data(struct smi330_data *data, int chan_type, int axis, + int *val) +{ + u8 reg; + int ret, sample; + + switch (chan_type) { + case IIO_ACCEL: + reg = SMI330_ACCEL_X_REG + (axis - IIO_MOD_X); + break; + case IIO_ANGL_VEL: + reg = SMI330_GYRO_X_REG + (axis - IIO_MOD_X); + break; + case IIO_TEMP: + reg = SMI330_TEMP_REG; + break; + default: + return -EINVAL; + } + + ret = regmap_read(data->regmap, reg, &sample); + if (ret) + return ret; + + *val = sign_extend32(sample, 15); + + return 0; +} + +static int smi330_read_avail(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, const int **vals, + int *type, int *length, long mask) +{ + switch (mask) { + case IIO_CHAN_INFO_SCALE: + if (chan->type == IIO_ACCEL) { + *vals = smi330_accel_scale_attr.vals; + *length = smi330_accel_scale_attr.len; + *type = smi330_accel_scale_attr.type; + } else { + *vals = smi330_gyro_scale_attr.vals; + *length = smi330_gyro_scale_attr.len; + *type = smi330_gyro_scale_attr.type; + } + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_OVERSAMPLING_RATIO: + *vals = smi330_average_attr.vals; + *length = smi330_average_attr.len; + *type = smi330_average_attr.type; + *type = IIO_VAL_INT; + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + *vals = smi330_bandwidth_attr.vals; + *length = smi330_bandwidth_attr.len; + *type = smi330_bandwidth_attr.type; + return IIO_AVAIL_LIST; + case IIO_CHAN_INFO_SAMP_FREQ: + *vals = smi330_odr_attr.vals; + *length = smi330_odr_attr.len; + *type = smi330_odr_attr.type; + return IIO_AVAIL_LIST; + default: + return -EINVAL; + } +} + +static int smi330_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int *val, + int *val2, long mask) +{ + int ret; + struct smi330_data *data = iio_priv(indio_dev); + enum smi330_sensor sensor; + + /* valid for all channel types */ + switch (mask) { + case IIO_CHAN_INFO_RAW: + if (!iio_device_claim_direct(indio_dev)) + return -EBUSY; + ret = smi330_get_data(data, chan->type, chan->channel2, val); + iio_device_release_direct(indio_dev); + return ret ? ret : IIO_VAL_INT; + default: + break; + } + + switch (chan->type) { + case IIO_ACCEL: + sensor = SMI330_ACCEL; + break; + case IIO_ANGL_VEL: + sensor = SMI330_GYRO; + break; + case IIO_TEMP: + switch (mask) { + case IIO_CHAN_INFO_SCALE: + *val = SMI330_TEMP_SCALE / GIGA; + *val2 = SMI330_TEMP_SCALE % GIGA; + return IIO_VAL_INT_PLUS_NANO; + case IIO_CHAN_INFO_OFFSET: + *val = SMI330_TEMP_OFFSET; + return IIO_VAL_INT; + default: + return -EINVAL; + } + default: + return -EINVAL; + } + + /* valid for acc and gyro channels */ + switch (mask) { + case IIO_CHAN_INFO_OVERSAMPLING_RATIO: + ret = smi330_get_sensor_config(data, sensor, SMI330_AVG_NUM, + val); + return ret ? ret : IIO_VAL_INT; + + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + ret = smi330_get_sensor_config(data, sensor, SMI330_BW, val); + return ret ? ret : IIO_VAL_INT; + + case IIO_CHAN_INFO_SAMP_FREQ: + ret = smi330_get_sensor_config(data, sensor, SMI330_ODR, val); + return ret ? ret : IIO_VAL_INT; + + case IIO_CHAN_INFO_SCALE: + *val = 0; + ret = smi330_get_sensor_config(data, sensor, SMI330_RANGE, + val2); + return ret ? ret : IIO_VAL_INT_PLUS_NANO; + + default: + return -EINVAL; + } +} + +static int smi330_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, int val, int val2, + long mask) +{ + struct smi330_data *data = iio_priv(indio_dev); + enum smi330_sensor sensor; + + switch (chan->type) { + case IIO_ACCEL: + sensor = SMI330_ACCEL; + break; + case IIO_ANGL_VEL: + sensor = SMI330_GYRO; + break; + default: + return -EINVAL; + } + + switch (mask) { + case IIO_CHAN_INFO_SCALE: + return smi330_set_sensor_config(data, sensor, SMI330_RANGE, + val2); + case IIO_CHAN_INFO_OVERSAMPLING_RATIO: + return smi330_set_sensor_config(data, sensor, SMI330_AVG_NUM, + val); + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: + return smi330_set_sensor_config(data, sensor, SMI330_BW, val); + case IIO_CHAN_INFO_SAMP_FREQ: + return smi330_set_sensor_config(data, sensor, SMI330_ODR, val); + default: + return -EINVAL; + } +} + +static int smi330_write_raw_get_fmt(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, long info) +{ + switch (info) { + case IIO_CHAN_INFO_SCALE: + return IIO_VAL_INT_PLUS_NANO; + default: + return IIO_VAL_INT_PLUS_MICRO; + } +} + +static int smi330_soft_reset(struct smi330_data *data) +{ + int ret, dummy_byte; + + ret = regmap_write(data->regmap, SMI330_CMD_REG, SMI330_CMD_SOFT_RESET); + if (ret) + return ret; + fsleep(SMI330_SOFT_RESET_DELAY); + + /* Performing a dummy read after a soft-reset */ + regmap_read(data->regmap, SMI330_CHIP_ID_REG, &dummy_byte); + + return 0; +} + +static irqreturn_t smi330_trigger_handler(int irq, void *p) +{ + int ret; + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct smi330_data *data = iio_priv(indio_dev); + + ret = regmap_bulk_read(data->regmap, SMI330_ACCEL_X_REG, data->buf, + SMI330_SCAN_LEN); + if (ret) + goto out; + + iio_push_to_buffers_with_timestamp(indio_dev, data->buf, pf->timestamp); + +out: + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static irqreturn_t smi330_irq_thread_handler(int irq, void *indio_dev_) +{ + int ret, int_stat; + s16 int_status[2] = { 0 }; + struct iio_dev *indio_dev = indio_dev_; + struct smi330_data *data = iio_priv(indio_dev); + + ret = regmap_bulk_read(data->regmap, SMI330_INT1_STATUS_REG, int_status, 2); + if (ret) + return IRQ_NONE; + + int_stat = int_status[0] | int_status[1]; + + if (FIELD_GET(SMI330_INT_STATUS_ACC_GYR_DRDY_MASK, int_stat)) { + indio_dev->pollfunc->timestamp = iio_get_time_ns(indio_dev); + iio_trigger_poll_nested(data->trig); + } + + return IRQ_HANDLED; +} + +static int smi330_set_int_pin_config(struct smi330_data *data, + enum smi330_int_out irq_num, + bool active_high, bool open_drain, + bool latch) +{ + int ret, val; + + val = active_high ? SMI330_IO_INT_CTRL_LVL : 0; + val |= open_drain ? SMI330_IO_INT_CTRL_OD : 0; + val |= SMI330_IO_INT_CTRL_EN; + + switch (irq_num) { + case SMI330_INT_1: + val = FIELD_PREP(SMI330_IO_INT_CTRL_INT1_MASK, val); + ret = regmap_update_bits(data->regmap, SMI330_IO_INT_CTRL_REG, + SMI330_IO_INT_CTRL_INT1_MASK, val); + if (ret) + return ret; + break; + case SMI330_INT_2: + val = FIELD_PREP(SMI330_IO_INT_CTRL_INT2_MASK, val); + ret = regmap_update_bits(data->regmap, SMI330_IO_INT_CTRL_REG, + SMI330_IO_INT_CTRL_INT2_MASK, val); + if (ret) + return ret; + break; + default: + return -EINVAL; + } + + return regmap_update_bits(data->regmap, SMI330_INT_CONF_REG, + SMI330_INT_CONF_LATCH_MASK, + FIELD_PREP(SMI330_INT_CONF_LATCH_MASK, + latch)); +} + +static int smi330_setup_irq(struct device *dev, struct iio_dev *indio_dev, + int irq, enum smi330_int_out irq_num) +{ + int ret, irq_type; + bool open_drain, active_high, latch; + struct smi330_data *data = iio_priv(indio_dev); + struct irq_data *desc; + + desc = irq_get_irq_data(irq); + if (!desc) + return -EINVAL; + + irq_type = irqd_get_trigger_type(desc); + switch (irq_type) { + case IRQF_TRIGGER_RISING: + latch = false; + active_high = true; + break; + case IRQF_TRIGGER_HIGH: + latch = true; + active_high = true; + break; + case IRQF_TRIGGER_FALLING: + latch = false; + active_high = false; + break; + case IRQF_TRIGGER_LOW: + latch = true; + active_high = false; + break; + default: + return -EINVAL; + } + + open_drain = device_property_read_bool(dev, "drive-open-drain"); + + ret = smi330_set_int_pin_config(data, irq_num, active_high, open_drain, + latch); + if (ret) + return ret; + + return devm_request_threaded_irq(dev, irq, NULL, + smi330_irq_thread_handler, + irq_type | IRQF_ONESHOT, + indio_dev->name, indio_dev); +} + +static int smi330_register_irq(struct device *dev, struct iio_dev *indio_dev) +{ + int ret, irq; + struct smi330_data *data = iio_priv(indio_dev); + struct fwnode_handle *fwnode; + + fwnode = dev_fwnode(dev); + if (!fwnode) + return -ENODEV; + + data->cfg.data_irq = SMI330_INT_DISABLED; + + irq = fwnode_irq_get_byname(fwnode, "INT1"); + if (irq > 0) { + ret = smi330_setup_irq(dev, indio_dev, irq, SMI330_INT_1); + if (ret) + return ret; + data->cfg.data_irq = SMI330_INT_1; + } else { + irq = fwnode_irq_get_byname(fwnode, "INT2"); + if (irq > 0) { + ret = smi330_setup_irq(dev, indio_dev, irq, + SMI330_INT_2); + if (ret) + return ret; + data->cfg.data_irq = SMI330_INT_2; + } + } + + return 0; +} + +static int smi330_set_drdy_trigger_state(struct iio_trigger *trig, bool enable) +{ + int val; + struct smi330_data *data = iio_trigger_get_drvdata(trig); + + if (enable) + data->cfg.op_mode = SMI330_DATA_READY; + else + data->cfg.op_mode = SMI330_POLLING; + + val = FIELD_PREP(SMI330_INT_MAP2_ACC_DRDY_MASK, + enable ? data->cfg.data_irq : 0); + val |= FIELD_PREP(SMI330_INT_MAP2_GYR_DRDY_MASK, + enable ? data->cfg.data_irq : 0); + return regmap_update_bits(data->regmap, SMI330_INT_MAP2_REG, + SMI330_INT_MAP2_ACC_DRDY_MASK | + SMI330_INT_MAP2_GYR_DRDY_MASK, + val); +} + +static const struct iio_trigger_ops smi330_trigger_ops = { + .set_trigger_state = &smi330_set_drdy_trigger_state, +}; + +static struct iio_info smi330_info = { + .read_avail = smi330_read_avail, + .read_raw = smi330_read_raw, + .write_raw = smi330_write_raw, + .write_raw_get_fmt = smi330_write_raw_get_fmt, +}; + +static int smi330_dev_init(struct smi330_data *data) +{ + int ret, chip_id, val, mode; + struct device *dev = regmap_get_device(data->regmap); + + ret = regmap_read(data->regmap, SMI330_CHIP_ID_REG, &chip_id); + if (ret) + return ret; + + chip_id = FIELD_GET(SMI330_CHIP_ID_MASK, chip_id); + if (chip_id != SMI330_CHIP_ID) + dev_info(dev, "Unknown chip id: 0x%04x\n", chip_id); + + ret = regmap_read(data->regmap, SMI330_ERR_REG, &val); + if (ret) + return ret; + if (FIELD_GET(SMI330_ERR_FATAL_MASK, val)) + return -ENODEV; + + ret = regmap_read(data->regmap, SMI330_STATUS_REG, &val); + if (ret) + return ret; + if (FIELD_GET(SMI330_STATUS_POR_MASK, val) == 0) + return -ENODEV; + + mode = FIELD_PREP(SMI330_CFG_MODE_MASK, SMI330_MODE_NORMAL); + + ret = regmap_update_bits(data->regmap, SMI330_ACCEL_CFG_REG, + SMI330_CFG_MODE_MASK, mode); + if (ret) + return ret; + + return regmap_update_bits(data->regmap, SMI330_GYRO_CFG_REG, + SMI330_CFG_MODE_MASK, mode); +} + +int smi330_core_probe(struct device *dev, struct regmap *regmap) +{ + int ret; + struct iio_dev *indio_dev; + struct smi330_data *data; + + indio_dev = devm_iio_device_alloc(dev, sizeof(*data)); + if (!indio_dev) + return -ENOMEM; + + data = iio_priv(indio_dev); + data->regmap = regmap; + + ret = smi330_soft_reset(data); + if (ret) + return dev_err_probe(dev, ret, "Soft reset failed\n"); + + indio_dev->channels = smi330_channels; + indio_dev->num_channels = ARRAY_SIZE(smi330_channels); + indio_dev->available_scan_masks = smi330_avail_scan_masks; + indio_dev->name = "smi330"; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->info = &smi330_info; + + data->cfg.op_mode = SMI330_POLLING; + + ret = smi330_dev_init(data); + if (ret) + return dev_err_probe(dev, ret, "Init failed\n"); + + ret = smi330_register_irq(dev, indio_dev); + if (ret) + return dev_err_probe(dev, ret, "Register IRQ failed\n"); + + if (data->cfg.data_irq != SMI330_INT_DISABLED) { + data->trig = devm_iio_trigger_alloc(dev, "%s-drdy-trigger", + indio_dev->name); + if (!data->trig) + return -ENOMEM; + + data->trig->ops = &smi330_trigger_ops; + iio_trigger_set_drvdata(data->trig, data); + + ret = devm_iio_trigger_register(dev, data->trig); + if (ret) + return dev_err_probe(dev, ret, + "IIO register trigger failed\n"); + + /* Set default operation mode to data ready. */ + indio_dev->trig = iio_trigger_get(data->trig); + } + + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, + iio_pollfunc_store_time, + smi330_trigger_handler, NULL); + if (ret) + return dev_err_probe(dev, ret, "IIO buffer setup failed\n"); + + ret = devm_iio_device_register(dev, indio_dev); + if (ret) + return dev_err_probe(dev, ret, "Register IIO device failed\n"); + + return 0; +} +EXPORT_SYMBOL_NS_GPL(smi330_core_probe, "IIO_SMI330"); + +MODULE_AUTHOR("Stefan Gutmann <stefan.gutmann@de.bosch.com>"); +MODULE_AUTHOR("Roman Huber <roman.huber@de.bosch.com>"); +MODULE_AUTHOR("Filip Andrei <Andrei.Filip@ro.bosch.com>"); +MODULE_AUTHOR("Drimbarean Avram Andrei <Avram-Andrei.Drimbarean@ro.bosch.com>"); +MODULE_DESCRIPTION("Bosch SMI330 IMU driver"); +MODULE_LICENSE("Dual BSD/GPL"); diff --git a/drivers/iio/imu/smi330/smi330_i2c.c b/drivers/iio/imu/smi330/smi330_i2c.c new file mode 100644 index 000000000000..e5f1825beb71 --- /dev/null +++ b/drivers/iio/imu/smi330/smi330_i2c.c @@ -0,0 +1,133 @@ +// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 +/* + * Copyright (c) 2025 Robert Bosch GmbH. + */ +#include <linux/i2c.h> +#include <linux/mod_devicetable.h> +#include <linux/module.h> +#include <linux/regmap.h> + +#include "smi330.h" + +#define SMI330_NUM_DUMMY_BYTES 2 +#define SMI330_I2C_MAX_RX_BUFFER_SIZE \ + (SMI330_NUM_DUMMY_BYTES + SMI330_SCAN_LEN * sizeof(s16)) + +struct smi330_i2c_priv { + struct i2c_client *i2c; + u8 rx_buffer[SMI330_I2C_MAX_RX_BUFFER_SIZE]; +}; + +static int smi330_regmap_i2c_read(void *context, const void *reg_buf, + size_t reg_size, void *val_buf, + size_t val_size) +{ + struct smi330_i2c_priv *priv = context; + int ret; + + if (SMI330_NUM_DUMMY_BYTES + val_size > SMI330_I2C_MAX_RX_BUFFER_SIZE) + return -EINVAL; + + /* + * SMI330 I2C read frame: + * <Slave address[6:0], RnW> <x, Register address[6:0]> + * <Slave address[6:0], RnW> <Dummy[7:0]> <Dummy[7:0]> <Data_0[7:0]> <Data_1[15:8]>... + * <Data_N[7:0]> <Data_N[15:8]> + * Remark: Slave address is not considered part of the frame in the following definitions + */ + struct i2c_msg msgs[] = { + { + .addr = priv->i2c->addr, + .flags = priv->i2c->flags, + .len = reg_size, + .buf = (u8 *)reg_buf, + }, + { + .addr = priv->i2c->addr, + .flags = priv->i2c->flags | I2C_M_RD, + .len = SMI330_NUM_DUMMY_BYTES + val_size, + .buf = priv->rx_buffer, + }, + }; + + ret = i2c_transfer(priv->i2c->adapter, msgs, ARRAY_SIZE(msgs)); + if (ret < 0) + return ret; + + memcpy(val_buf, priv->rx_buffer + SMI330_NUM_DUMMY_BYTES, val_size); + + return 0; +} + +static int smi330_regmap_i2c_write(void *context, const void *data, + size_t count) +{ + struct smi330_i2c_priv *priv = context; + u8 reg; + + /* + * SMI330 I2C write frame: + * <Slave address[6:0], RnW> <x, Register address[6:0]> <Data_0[7:0]> <Data_1[15:8]>... + * <Data_N[7:0]> <Data_N[15:8]> + * Remark: Slave address is not considered part of the frame in the following definitions + */ + reg = *(u8 *)data; + return i2c_smbus_write_i2c_block_data(priv->i2c, reg, + count - sizeof(u8), + data + sizeof(u8)); +} + +static const struct regmap_bus smi330_regmap_bus = { + .read = smi330_regmap_i2c_read, + .write = smi330_regmap_i2c_write, +}; + +static int smi330_i2c_probe(struct i2c_client *i2c) +{ + struct device *dev = &i2c->dev; + struct smi330_i2c_priv *priv; + struct regmap *regmap; + + priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL); + if (!priv) + return -ENOMEM; + + priv->i2c = i2c; + regmap = devm_regmap_init(dev, &smi330_regmap_bus, priv, + &smi330_regmap_config); + if (IS_ERR(regmap)) + return dev_err_probe(dev, PTR_ERR(regmap), + "Failed to initialize I2C Regmap\n"); + + return smi330_core_probe(dev, regmap); +} + +static const struct i2c_device_id smi330_i2c_device_id[] = { + { .name = "smi330" }, + { } +}; +MODULE_DEVICE_TABLE(i2c, smi330_i2c_device_id); + +static const struct of_device_id smi330_of_match[] = { + { .compatible = "bosch,smi330" }, + { } +}; +MODULE_DEVICE_TABLE(of, smi330_of_match); + +static struct i2c_driver smi330_i2c_driver = { + .probe = smi330_i2c_probe, + .id_table = smi330_i2c_device_id, + .driver = { + .of_match_table = smi330_of_match, + .name = "smi330_i2c", + }, +}; +module_i2c_driver(smi330_i2c_driver); + +MODULE_AUTHOR("Stefan Gutmann <stefan.gutmann@de.bosch.com>"); +MODULE_AUTHOR("Roman Huber <roman.huber@de.bosch.com>"); +MODULE_AUTHOR("Filip Andrei <Andrei.Filip@ro.bosch.com>"); +MODULE_AUTHOR("Drimbarean Avram Andrei <Avram-Andrei.Drimbarean@ro.bosch.com>"); +MODULE_DESCRIPTION("Bosch SMI330 I2C driver"); +MODULE_LICENSE("Dual BSD/GPL"); +MODULE_IMPORT_NS("IIO_SMI330"); diff --git a/drivers/iio/imu/smi330/smi330_spi.c b/drivers/iio/imu/smi330/smi330_spi.c new file mode 100644 index 000000000000..a6044e02b451 --- /dev/null +++ b/drivers/iio/imu/smi330/smi330_spi.c @@ -0,0 +1,85 @@ +// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 +/* + * Copyright (c) 2025 Robert Bosch GmbH. + */ +#include <linux/mod_devicetable.h> +#include <linux/module.h> +#include <linux/regmap.h> +#include <linux/spi/spi.h> + +#include "smi330.h" + +static int smi330_regmap_spi_read(void *context, const void *reg_buf, + size_t reg_size, void *val_buf, + size_t val_size) +{ + struct spi_device *spi = context; + + /* Insert pad byte for reading */ + u8 reg[] = { *(u8 *)reg_buf, 0 }; + + if (reg_size + 1 != ARRAY_SIZE(reg)) { + dev_err(&spi->dev, "Invalid register size %zu\n", reg_size); + return -EINVAL; + } + + return spi_write_then_read(spi, reg, ARRAY_SIZE(reg), val_buf, + val_size); +} + +static int smi330_regmap_spi_write(void *context, const void *data, + size_t count) +{ + struct spi_device *spi = context; + + return spi_write(spi, data, count); +} + +static const struct regmap_bus smi330_regmap_bus = { + .read = smi330_regmap_spi_read, + .write = smi330_regmap_spi_write, + .read_flag_mask = 0x80, +}; + +static int smi330_spi_probe(struct spi_device *spi) +{ + struct regmap *regmap; + + regmap = devm_regmap_init(&spi->dev, &smi330_regmap_bus, &spi->dev, + &smi330_regmap_config); + if (IS_ERR(regmap)) + return dev_err_probe(&spi->dev, PTR_ERR(regmap), + "Failed to initialize SPI Regmap\n"); + + return smi330_core_probe(&spi->dev, regmap); +} + +static const struct spi_device_id smi330_spi_device_id[] = { + { .name = "smi330" }, + { } +}; +MODULE_DEVICE_TABLE(spi, smi330_spi_device_id); + +static const struct of_device_id smi330_of_match[] = { + { .compatible = "bosch,smi330" }, + { } +}; +MODULE_DEVICE_TABLE(of, smi330_of_match); + +static struct spi_driver smi330_spi_driver = { + .probe = smi330_spi_probe, + .id_table = smi330_spi_device_id, + .driver = { + .of_match_table = smi330_of_match, + .name = "smi330_spi", + }, +}; +module_spi_driver(smi330_spi_driver); + +MODULE_AUTHOR("Stefan Gutmann <stefan.gutmann@de.bosch.com>"); +MODULE_AUTHOR("Roman Huber <roman.huber@de.bosch.com>"); +MODULE_AUTHOR("Filip Andrei <Andrei.Filip@ro.bosch.com>"); +MODULE_AUTHOR("Drimbarean Avram Andrei <Avram-Andrei.Drimbarean@ro.bosch.com>"); +MODULE_DESCRIPTION("Bosch SMI330 SPI driver"); +MODULE_LICENSE("Dual BSD/GPL"); +MODULE_IMPORT_NS("IIO_SMI330"); diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h index 381b016fa524..6405a5367d76 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h @@ -383,7 +383,8 @@ enum st_lsm6dsx_fifo_mode { * @id: Sensor identifier. * @hw: Pointer to instance of struct st_lsm6dsx_hw. * @gain: Configured sensor sensitivity. - * @odr: Output data rate of the sensor [Hz]. + * @odr: Output data rate of the sensor [mHz]. + * hwfifo_odr_mHz: Batch data rate for hardware FIFO [mHz] * @samples_to_discard: Number of samples to discard for filters settling time. * @watermark: Sensor watermark level. * @decimator: Sensor decimation factor. @@ -398,6 +399,7 @@ struct st_lsm6dsx_sensor { u32 gain; u32 odr; + u32 hwfifo_odr_mHz; u16 samples_to_discard; u16 watermark; diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c index 8a9d2593576a..55d877745575 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c @@ -56,6 +56,7 @@ #include <linux/iio/kfifo_buf.h> #include <linux/iio/iio.h> #include <linux/iio/buffer.h> +#include <linux/iio/sysfs.h> #include <linux/regmap.h> #include <linux/bitfield.h> @@ -105,7 +106,7 @@ static int st_lsm6dsx_get_decimator_val(struct st_lsm6dsx_sensor *sensor, u32 max_odr) { const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table); - u32 decimator = max_odr / sensor->odr; + u32 decimator = max_odr / sensor->hwfifo_odr_mHz; int i; if (decimator > 1) @@ -136,14 +137,14 @@ static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw, if (!(hw->enable_mask & BIT(sensor->id))) continue; - *max_odr = max_t(u32, *max_odr, sensor->odr); - *min_odr = min_t(u32, *min_odr, sensor->odr); + *max_odr = max(*max_odr, sensor->hwfifo_odr_mHz); + *min_odr = min(*min_odr, sensor->hwfifo_odr_mHz); } } static u8 st_lsm6dsx_get_sip(struct st_lsm6dsx_sensor *sensor, u32 min_odr) { - u8 sip = sensor->odr / min_odr; + u8 sip = sensor->hwfifo_odr_mHz / min_odr; return sip > 1 ? round_down(sip, 2) : sip; } @@ -231,7 +232,7 @@ static int st_lsm6dsx_set_fifo_odr(struct st_lsm6dsx_sensor *sensor, if (enable) { int err; - err = st_lsm6dsx_check_odr(sensor, sensor->odr, + err = st_lsm6dsx_check_odr(sensor, sensor->hwfifo_odr_mHz, &data); if (err < 0) return err; @@ -713,7 +714,7 @@ st_lsm6dsx_update_samples_to_discard(struct st_lsm6dsx_sensor *sensor) data = &hw->settings->samples_to_discard[sensor->id]; for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++) { - if (data->val[i].milli_hz == sensor->odr) { + if (data->val[i].milli_hz == sensor->hwfifo_odr_mHz) { sensor->samples_to_discard = data->val[i].samples; return; } @@ -799,6 +800,59 @@ static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = { .postdisable = st_lsm6dsx_buffer_postdisable, }; +static ssize_t st_lsm6dsx_hwfifo_odr_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct st_lsm6dsx_sensor *sensor = iio_priv(dev_to_iio_dev(dev)); + + return sysfs_emit(buf, "%d.%03d\n", sensor->hwfifo_odr_mHz / 1000, + sensor->hwfifo_odr_mHz % 1000); +} + +static ssize_t st_lsm6dsx_hwfifo_odr_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) +{ + struct iio_dev *iio_dev = dev_to_iio_dev(dev); + struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev); + int integer, milli; + int ret; + u32 hwfifo_odr; + u8 data; + + if (!iio_device_claim_direct(iio_dev)) + return -EBUSY; + + ret = iio_str_to_fixpoint(buf, 100, &integer, &milli); + if (ret) + goto out; + + hwfifo_odr = integer * 1000 + milli; + ret = st_lsm6dsx_check_odr(sensor, hwfifo_odr, &data); + if (ret < 0) + goto out; + + hwfifo_odr = ret; + + /* the batch data rate must not exceed the sensor output data rate */ + if (hwfifo_odr <= sensor->odr) + sensor->hwfifo_odr_mHz = hwfifo_odr; + else + ret = -EINVAL; + +out: + iio_device_release_direct(iio_dev); + + return ret < 0 ? ret : len; +} + +static IIO_DEV_ATTR_SAMP_FREQ(0664, st_lsm6dsx_hwfifo_odr_show, st_lsm6dsx_hwfifo_odr_store); + +static const struct iio_dev_attr *st_lsm6dsx_buffer_attrs[] = { + &iio_dev_attr_sampling_frequency, + NULL +}; + int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) { int i, ret; @@ -807,8 +861,9 @@ int st_lsm6dsx_fifo_setup(struct st_lsm6dsx_hw *hw) if (!hw->iio_devs[i]) continue; - ret = devm_iio_kfifo_buffer_setup(hw->dev, hw->iio_devs[i], - &st_lsm6dsx_buffer_ops); + ret = devm_iio_kfifo_buffer_setup_ext(hw->dev, hw->iio_devs[i], + &st_lsm6dsx_buffer_ops, + st_lsm6dsx_buffer_attrs); if (ret) return ret; } diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c index a2daf0c14d96..49ac17806e72 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c @@ -1851,10 +1851,12 @@ static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev, val = val * 1000 + val2 / 1000; val = st_lsm6dsx_check_odr(sensor, val, &data); - if (val < 0) + if (val < 0) { err = val; - else + } else { sensor->odr = val; + sensor->hwfifo_odr_mHz = val; + } break; } default: @@ -2381,6 +2383,7 @@ static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw, sensor->id = id; sensor->hw = hw; sensor->odr = hw->settings->odr_table[id].odr_avl[0].milli_hz; + sensor->hwfifo_odr_mHz = sensor->odr; sensor->gain = hw->settings->fs_table[id].fs_avl[0].gain; sensor->watermark = 1; diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c index 3c5e65dc0f97..d6a1eeb151ca 100644 --- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c @@ -640,6 +640,7 @@ __st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev, sensor->ext_info.slv_odr = val; sensor->odr = odr; + sensor->hwfifo_odr_mHz = odr; return 0; } case IIO_CHAN_INFO_SCALE: @@ -746,6 +747,7 @@ st_lsm6dsx_shub_alloc_iiodev(struct st_lsm6dsx_hw *hw, sensor->id = id; sensor->hw = hw; sensor->odr = hw->settings->odr_table[ref_id].odr_avl[0].milli_hz; + sensor->hwfifo_odr_mHz = sensor->odr; sensor->ext_info.slv_odr = info->odr_table.odr_avl[0].milli_hz; sensor->gain = info->fs_table.fs_avl[0].gain; sensor->ext_info.settings = info; |