microsoft
/
WSL2-Linux-Kernel
зеркало из https://github.com/microsoft/WSL2-Linux-Kernel.git
1
0
Форкнуть 0
Код Задачи Пакеты Проекты Релизы Вики Активность

iio: imu: inv_mpu6050: add fifo support for magnetometer data 

Put read magnetometer data by mpu inside the fifo.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
This commit is contained in:
Jean-Baptiste Maneyrol 2019-09-16 09:42:09 +00:00 коммит произвёл Jonathan Cameron
Родитель b1392de061
Коммит e764fb4e20
4 изменённых файлов: 88 добавлений и 12 удалений
Показать статистику Скачать Patch файл Скачать Diff файл Показать все файлы Свернуть все файлы

1
drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
Просмотреть файл

@ -104,6 +104,7 @@ static const struct inv_mpu6050_chip_config chip_config_6050 = {
.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE),
.gyro_fifo_enable = false,
.accl_fifo_enable = false,
.magn_fifo_enable = false,
.accl_fs = INV_MPU6050_FS_02G,
.user_ctrl = 0,
};

2
drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
Просмотреть файл

@ -86,6 +86,7 @@ enum inv_devices {
* @accl_fs: accel full scale range.
* @accl_fifo_enable: enable accel data output
* @gyro_fifo_enable: enable gyro data output
* @magn_fifo_enable: enable magn data output
* @divider: chip sample rate divider (sample rate divider - 1)
*/
struct inv_mpu6050_chip_config {
@ -94,6 +95,7 @@ struct inv_mpu6050_chip_config {
unsigned int accl_fs:2;
unsigned int accl_fifo_enable:1;
unsigned int gyro_fifo_enable:1;
unsigned int magn_fifo_enable:1;
u8 divider;
u8 user_ctrl;
};

11
drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
Просмотреть файл

@ -124,7 +124,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
/* enable interrupt */
if (st->chip_config.accl_fifo_enable ||
st->chip_config.gyro_fifo_enable) {
st->chip_config.gyro_fifo_enable ||
st->chip_config.magn_fifo_enable) {
result = regmap_write(st->map, st->reg->int_enable,
INV_MPU6050_BIT_DATA_RDY_EN);
if (result)
@ -141,6 +142,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
d |= INV_MPU6050_BITS_GYRO_OUT;
if (st->chip_config.accl_fifo_enable)
d |= INV_MPU6050_BIT_ACCEL_OUT;
if (st->chip_config.magn_fifo_enable)
d |= INV_MPU6050_BIT_SLAVE_0;
result = regmap_write(st->map, st->reg->fifo_en, d);
if (result)
goto reset_fifo_fail;
@ -190,7 +193,8 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
}
if (!(st->chip_config.accl_fifo_enable |
st->chip_config.gyro_fifo_enable))
st->chip_config.gyro_fifo_enable |
st->chip_config.magn_fifo_enable))
goto end_session;
bytes_per_datum = 0;
if (st->chip_config.accl_fifo_enable)
@ -202,6 +206,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
if (st->chip_type == INV_ICM20602)
bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;
if (st->chip_config.magn_fifo_enable)
bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
/*
* read fifo_count register to know how many bytes are inside the FIFO
* right now

86
drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
Просмотреть файл

@ -5,7 +5,7 @@
#include "inv_mpu_iio.h"
static void inv_scan_query(struct iio_dev *indio_dev)
static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
@ -26,6 +26,60 @@ static void inv_scan_query(struct iio_dev *indio_dev)
indio_dev->active_scan_mask);
}
static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
inv_scan_query_mpu6050(indio_dev);
/* no magnetometer if i2c auxiliary bus is used */
if (st->magn_disabled)
return;
st->chip_config.magn_fifo_enable =
test_bit(INV_MPU9X50_SCAN_MAGN_X,
indio_dev->active_scan_mask) ||
test_bit(INV_MPU9X50_SCAN_MAGN_Y,
indio_dev->active_scan_mask) ||
test_bit(INV_MPU9X50_SCAN_MAGN_Z,
indio_dev->active_scan_mask);
}
static void inv_scan_query(struct iio_dev *indio_dev)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
switch (st->chip_type) {
case INV_MPU9250:
case INV_MPU9255:
return inv_scan_query_mpu9x50(indio_dev);
default:
return inv_scan_query_mpu6050(indio_dev);
}
}
static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
{
unsigned int gyro_skip = 0;
unsigned int magn_skip = 0;
unsigned int skip_samples;
/* gyro first sample is out of specs, skip it */
if (st->chip_config.gyro_fifo_enable)
gyro_skip = 1;
/* mag first sample is always not ready, skip it */
if (st->chip_config.magn_fifo_enable)
magn_skip = 1;
/* compute first samples to skip */
skip_samples = gyro_skip;
if (magn_skip > skip_samples)
skip_samples = magn_skip;
return skip_samples;
}
/**
* inv_mpu6050_set_enable() - enable chip functions.
* @indio_dev: Device driver instance.
@ -34,6 +88,7 @@ static void inv_scan_query(struct iio_dev *indio_dev)
static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
uint8_t d;
int result;
if (enable) {
@ -41,14 +96,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
if (result)
return result;
inv_scan_query(indio_dev);
st->skip_samples = 0;
if (st->chip_config.gyro_fifo_enable) {
result = inv_mpu6050_switch_engine(st, true,
INV_MPU6050_BIT_PWR_GYRO_STBY);
if (result)
goto error_power_off;
/* gyro first sample is out of specs, skip it */
st->skip_samples = 1;
}
if (st->chip_config.accl_fifo_enable) {
result = inv_mpu6050_switch_engine(st, true,
@ -56,22 +108,32 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
if (result)
goto error_gyro_off;
}
if (st->chip_config.magn_fifo_enable) {
d = st->chip_config.user_ctrl |
INV_MPU6050_BIT_I2C_MST_EN;
result = regmap_write(st->map, st->reg->user_ctrl, d);
if (result)
goto error_accl_off;
st->chip_config.user_ctrl = d;
}
st->skip_samples = inv_compute_skip_samples(st);
result = inv_reset_fifo(indio_dev);
if (result)
goto error_accl_off;
goto error_magn_off;
} else {
result = regmap_write(st->map, st->reg->fifo_en, 0);
if (result)
goto error_accl_off;
goto error_magn_off;
result = regmap_write(st->map, st->reg->int_enable, 0);
if (result)
goto error_accl_off;
goto error_magn_off;
result = regmap_write(st->map, st->reg->user_ctrl,
st->chip_config.user_ctrl);
d = st->chip_config.user_ctrl & ~INV_MPU6050_BIT_I2C_MST_EN;
result = regmap_write(st->map, st->reg->user_ctrl, d);
if (result)
goto error_accl_off;
goto error_magn_off;
st->chip_config.user_ctrl = d;
result = inv_mpu6050_switch_engine(st, false,
INV_MPU6050_BIT_PWR_ACCL_STBY);
@ -90,6 +152,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
return 0;
error_magn_off:
/* always restore user_ctrl to disable fifo properly */
st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;
regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
error_accl_off:
if (st->chip_config.accl_fifo_enable)
inv_mpu6050_switch_engine(st, false,