shinys000114/linux
1
0
Fork 0
mirror of https://github.com/hardkernel/linux.git synced 2026-06-05 10:31:46 +09:00
Code Issues Packages Projects Releases Wiki Activity

iio: imu: inv_icm42670: support pass-through mode

Browse Source 
Change-Id: I63a394cf03c5b6cd09308e05e0996d1bf1f885f5
Signed-off-by: LiuDiMing Lin <fenrir.lin@rock-chips.com>
This commit is contained in:
LiuDiMing Lin
2024-11-01 09:50:49 +08:00
committed by Tao Huang
parent e433f21b41
commit 780c320c07
4 changed files with 161 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
drivers/iio/imu/inv_icm42670/inv_icm42670.h
View File

@@ -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);

52
drivers/iio/imu/inv_icm42670/inv_icm42670_core.c
View File

@@ -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;

107
drivers/iio/imu/inv_icm42670/inv_icm42670_ring.c
View File

@@ -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;
}

25
drivers/iio/imu/inv_icm42670/inv_icm42670_trigger.c
View File

@@ -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: