diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670.h b/drivers/iio/imu/inv_icm42670/inv_icm42670.h index 3aee90fafed4..20211fb02a08 100644 --- a/drivers/iio/imu/inv_icm42670/inv_icm42670.h +++ b/drivers/iio/imu/inv_icm42670/inv_icm42670.h @@ -502,6 +502,7 @@ struct icm42670_data { s64 period_max; // Maximum interrupt period deviation in nanoseconds int period_divider; int interrupt_regval; + bool enable_fifo; u8 data_buff[ICM42670_DATA_BUFF_SIZE]; }; @@ -532,6 +533,7 @@ enum icm42670_sensor_type { // fifo #define ICM42670_OUTPUT_DATA_SIZE 24 // align 8, last 8 for timestamp #define ICM42670_OUTPUT_DATA_SIZE_PULS_ONE 25 +#define ICM42670_ACCEL_GYRO_SIZE 12 #define ICM42670_FIFO_DATUM 16 #define ICM42670_BYTES_PER_3AXIS_SENSOR 6 #define ICM42670_FIFO_COUNT_BYTE 2 @@ -545,6 +547,8 @@ extern const struct dev_pm_ops icm42670_pm_ops; irqreturn_t icm42670_read_fifo(int irq, void *p); int icm42670_reset_fifo(struct iio_dev *indio_dev); +irqreturn_t icm42670_read_data(int irq, void *p); +int icm42670_reset_data(struct iio_dev *indio_dev); int icm42670_core_probe(struct regmap *regmap, int irq, const char *name, int chip_type, icm42670_bus_setup bus_setup); void icm42670_core_remove(struct device *dev); diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c b/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c index 901ca0d66a6d..803e321bba9f 100644 --- a/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c +++ b/drivers/iio/imu/inv_icm42670/inv_icm42670_core.c @@ -846,26 +846,30 @@ static int icm42670_chip_init(struct icm42670_data *data, icm42670_bus_setup bus return ret; } - /* FIFO count is reported in records (1 record = 16 bytes for gyro+accel+tempsensor data) */ - ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0, - BIT_FIFO_COUNT_REC_FIFO_COUNT_REC, - BIT_FIFO_COUNT_REC_FIFO_COUNT_REC); - if (ret) - return ret; + if (true == data->enable_fifo) { + /* FIFO count is reported in records + * (1 record = 16 bytes for gyro+accel+tempsensor data) + */ + ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0, + BIT_FIFO_COUNT_REC_FIFO_COUNT_REC, + BIT_FIFO_COUNT_REC_FIFO_COUNT_REC); + if (ret) + return ret; - /* FIFO data in big-endian (default) */ - ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0, - BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN, - BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN); - if (ret) - return ret; + /* FIFO data in big-endian (default) */ + ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0, + BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN, + BIT_FIFO_COUNT_REC_FIFO_COUNT_ENDIAN); + if (ret) + return ret; - /* sensor data in big-endian (default) */ - ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0, - BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN, - BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN); - if (ret) - return ret; + /* sensor data in big-endian (default) */ + ret = regmap_update_bits(data->regmap, REG_INTF_CONFIG0, + BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN, + BIT_FIFO_COUNT_REC_SENSOR_DATA_ENDIAN); + if (ret) + return ret; + } ret = icm42670_set_mode(data, ICM42670_ACCEL, true); if (ret < 0) { @@ -951,6 +955,10 @@ int icm42670_core_probe(struct regmap *regmap, return -EINVAL; } + data->enable_fifo = of_property_read_bool(data->node, "enable-fifo"); + if (true != data->enable_fifo) + dev_info(dev, "Not using FIFO!\n"); + desc = irq_get_irq_data(irq); if (!desc) { dev_err(dev, "Could not find IRQ %d\n", irq); @@ -1007,8 +1015,12 @@ int icm42670_core_probe(struct regmap *regmap, indio_dev->info = &icm42670_info; indio_dev->modes = INDIO_BUFFER_TRIGGERED; - ret = devm_iio_triggered_buffer_setup(dev, indio_dev, iio_pollfunc_store_time, - icm42670_read_fifo, NULL); + if (true == data->enable_fifo) + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, + iio_pollfunc_store_time, icm42670_read_fifo, NULL); + else + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, + iio_pollfunc_store_time, icm42670_read_data, NULL); if (ret < 0) goto uninit; diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c b/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c index 1dbddb85f217..7787356bcf4a 100644 --- a/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c +++ b/drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c @@ -242,3 +242,110 @@ flush_fifo: return IRQ_HANDLED; } + +int icm42670_reset_data(struct iio_dev *indio_dev) +{ + int ret; + struct icm42670_data *st = iio_priv(indio_dev); + const struct device *dev = regmap_get_device(st->regmap); + + /* reset it timestamp validation */ + st->it_timestamp = 0; + + ret = regmap_write(st->regmap, REG_INT_SOURCE0, BIT_INT_MODE_OFF); + if (ret) { + dev_err(dev, "int_enable failed %d\n", + ret); + goto reset_fail; + } + + /* disable the sensor output to FIFO */ + ret = regmap_write(st->regmap, REG_FIFO_CONFIG1, BIT_FIFO_MODE_BYPASS); + if (ret) { + dev_err(dev, "Failed to write to REG_FIFO_CONFIG1 register!\n"); + goto reset_fail; + } + + ret = regmap_write(st->regmap, REG_INT_SOURCE0, + BIT_INT_DRDY_INT_EN); + if (ret) { + dev_err(dev, "Failed to write to REG_INT_SOURCE0 register!\n"); + goto reset_fail; + } + + msleep(50); + + st->interrupt_period = st->standard_period; + + return 0; + +reset_fail: + dev_err(regmap_get_device(st->regmap), "%s :reset fifo failed %d\n", __func__, ret); + + return ret; +} + +irqreturn_t icm42670_read_data(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct icm42670_data *st = iio_priv(indio_dev); + const struct device *dev = regmap_get_device(st->regmap); + int ret, int_drdy; + //int pts; + u8 data[ICM42670_OUTPUT_DATA_SIZE_PULS_ONE]; + s64 timestamp = 0; + u8 temp_data[2]; + u16 temp = 0; + + mutex_lock(&st->lock); + + ret = regmap_read(st->regmap, REG_INT_STATUS_DRDY, &int_drdy); + if (ret) { + dev_err(dev, "failed to ack interrupt\n"); + goto read_fail; + } + + if (!(int_drdy & (BIT_INT_STATUS_DRDY))) + goto read_fail; + + icm42670_update_period(st, st->data_timestamp, 1); + + ret = regmap_bulk_read(st->regmap, REG_ACCEL_DATA_X0_UI, + (u8 *)data, ICM42670_ACCEL_GYRO_SIZE); + if (ret) { + dev_err(dev, "regmap_bulk_read accel+gyro failed\n"); + goto read_fail; + } + + /* + * 8-bit quantity: Temperature in Degrees Centigrade = (TEMP_DATA / 2) + 25 + * where TEMP_DATA refers to the 8 MSBs of the 16-bit word coming from + * the temperature sensor. In this mode the 8 LSBs are set to ‘0’. + * 16-bit quantity: Temperature in Degrees Centigrade = (TEMP_DATA / 128) + 25 + */ + ret = regmap_bulk_read(st->regmap, REG_TEMP_DATA0_UI, (u8 *)&temp_data, 2); + if (ret) { + dev_err(dev, "regmap_bulk_read temperature failed\n"); + goto read_fail; + } + temp = (get_unaligned_be16(temp_data) >> 7) + 25; + data[ICM42670_ACCEL_GYRO_SIZE] = (temp - 25) << 1; + + timestamp = icm42670_get_timestamp(st); + iio_push_to_buffers_with_timestamp(indio_dev, &(data[0]), timestamp); + + mutex_unlock(&st->lock); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; + +read_fail: + /* Flush HW and SW FIFOs. */ + dev_info(dev, "flush info\n"); + icm42670_reset_fifo(indio_dev); + mutex_unlock(&st->lock); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} diff --git a/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c b/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c index 8b52eca60738..e1364454c891 100644 --- a/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c +++ b/drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c @@ -26,9 +26,15 @@ int icm42670_set_enable(struct iio_dev *indio_dev, bool enable) if (ret) goto error_accl_off; - ret = icm42670_reset_fifo(indio_dev); - if (ret) - goto error_gyro_off; + if (true == data->enable_fifo) { + ret = icm42670_reset_fifo(indio_dev); + if (ret) + goto error_gyro_off; + } else { + ret = icm42670_reset_data(indio_dev); + if (ret) + goto error_gyro_off; + } /* Theoretical interrupt period */ data->standard_period = data->period_divider > 0 ? @@ -43,20 +49,25 @@ int icm42670_set_enable(struct iio_dev *indio_dev, bool enable) data->period_max = div_s64((data->standard_period * (100 + ICM42670_TS_PERIOD_JITTER)), 100); } else { + ret = regmap_write(data->regmap, REG_INT_SOURCE0, BIT_INT_MODE_OFF); + if (ret) { + dev_err(dev, "int_enable failed %d\n", ret); + goto error_gyro_off; + } ret = regmap_write(data->regmap, REG_FIFO_CONFIG1, BIT_FIFO_MODE_BYPASS); if (ret) { dev_err(dev, "set REG_FIFO_CONFIG1 fail: %d\n", ret); goto error_gyro_off; } - ret = icm42670_set_mode(data, ICM42670_GYRO, false); - if (ret) - goto error_gyro_off; ret = icm42670_set_mode(data, ICM42670_ACCEL, false); if (ret) goto error_accl_off; + + ret = icm42670_set_mode(data, ICM42670_GYRO, false); + if (ret) + goto error_gyro_off; } - dev_info(dev, "set fifo mode end\n"); return ret; error_gyro_off: