Re: [PATCH v2 1/2] iio: imu: add support to lsm6dsx driver

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 




> Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor

nitpicking below

> http://www.st.com/resource/en/datasheet/lsm6ds3.pdf
> http://www.st.com/resource/en/datasheet/lsm6dsm.pdf
> 
> - continuous mode support
> - i2c support
> - spi support
> - sw fifo mode support
> - supported devices: lsm6ds3, lsm6dsm
> 
> Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>
> ---
>  drivers/iio/imu/Kconfig                      |   1 +
>  drivers/iio/imu/Makefile                     |   2 +
>  drivers/iio/imu/st_lsm6dsx/Kconfig           |  23 +
>  drivers/iio/imu/st_lsm6dsx/Makefile          |   6 +
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h      | 107 ++++
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 696 +++++++++++++++++++++++++++
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c  | 111 +++++
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_ring.c | 401 +++++++++++++++
>  drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c  | 129 +++++
>  9 files changed, 1476 insertions(+)
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/Kconfig
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/Makefile
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_ring.c
>  create mode 100644 drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
> 
> diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig
> index 1f1ad41..156630a 100644
> --- a/drivers/iio/imu/Kconfig
> +++ b/drivers/iio/imu/Kconfig
> @@ -39,6 +39,7 @@ config KMX61
>  	  be called kmx61.
>  
>  source "drivers/iio/imu/inv_mpu6050/Kconfig"
> +source "drivers/iio/imu/st_lsm6dsx/Kconfig"
>  
>  endmenu
>  
> diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile
> index c71bcd3..8b563c3 100644
> --- a/drivers/iio/imu/Makefile
> +++ b/drivers/iio/imu/Makefile
> @@ -17,3 +17,5 @@ obj-y += bmi160/
>  obj-y += inv_mpu6050/
>  
>  obj-$(CONFIG_KMX61) += kmx61.o
> +
> +obj-y += st_lsm6dsx/
> diff --git a/drivers/iio/imu/st_lsm6dsx/Kconfig b/drivers/iio/imu/st_lsm6dsx/Kconfig
> new file mode 100644
> index 0000000..9a0781b
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/Kconfig
> @@ -0,0 +1,23 @@
> +
> +config IIO_ST_LSM6DSX
> +	tristate "ST_LSM6DSx driver for STM 6-axis imu Mems sensors"

IMU and MEMS are both abbreviations, consider consistent uppercasing

> +	depends on (I2C || SPI)
> +	select IIO_BUFFER
> +	select IIO_KFIFO_BUF
> +	select IIO_ST_LSM6DSX_I2C if (I2C)
> +	select IIO_ST_LSM6DSX_SPI if (SPI_MASTER)
> +	help
> +	  Say yes here to build support for STMicroelectronics LSM6DSx imu
> +	  sensor. Supported devices: lsm6ds3, lsm6dsm
> +
> +	  To compile this driver as a module, choose M here: the module
> +	  will be called st_lsm6dsx.
> +
> +config IIO_ST_LSM6DSX_I2C
> +	tristate
> +	depends on IIO_ST_LSM6DSX
> +
> +config IIO_ST_LSM6DSX_SPI
> +	tristate
> +	depends on IIO_ST_LSM6DSX
> +
> diff --git a/drivers/iio/imu/st_lsm6dsx/Makefile b/drivers/iio/imu/st_lsm6dsx/Makefile
> new file mode 100644
> index 0000000..812d655
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/Makefile
> @@ -0,0 +1,6 @@
> +st_lsm6dsx-y := st_lsm6dsx_core.o \
> +		st_lsm6dsx_ring.o
> +
> +obj-$(CONFIG_IIO_ST_LSM6DSX) += st_lsm6dsx.o
> +obj-$(CONFIG_IIO_ST_LSM6DSX_I2C) += st_lsm6dsx_i2c.o
> +obj-$(CONFIG_IIO_ST_LSM6DSX_SPI) += st_lsm6dsx_spi.o
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> new file mode 100644
> index 0000000..a43beab
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -0,0 +1,107 @@
> +/*
> + * STMicroelectronics st_lsm6dsx sensor driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>
> + * Denis Ciocca <denis.ciocca@xxxxxx>
> + *
> + * Licensed under the GPL-2.
> + */
> +
> +#ifndef ST_LSM6DSX_H
> +#define ST_LSM6DSX_H
> +
> +#include <linux/device.h>
> +
> +#define ST_LSM6DS3_DEV_NAME	"lsm6ds3"
> +#define ST_LSM6DSM_DEV_NAME	"lsm6dsm"
> +
> +#define ST_LSM6DSX_CHAN_SIZE		2
> +#define ST_LSM6DSX_SAMPLE_SIZE		6
> +#define ST_LSM6DSX_SAMPLE_DEPTH		(ST_LSM6DSX_SAMPLE_SIZE / \
> +					 ST_LSM6DSX_CHAN_SIZE)
> +
> +#if defined(CONFIG_SPI_MASTER)
> +#define ST_LSM6DSX_RX_MAX_LENGTH	256
> +#define ST_LSM6DSX_TX_MAX_LENGTH	8
> +
> +struct st_lsm6dsx_transfer_buffer {
> +	u8 rx_buf[ST_LSM6DSX_RX_MAX_LENGTH];
> +	u8 tx_buf[ST_LSM6DSX_TX_MAX_LENGTH] ____cacheline_aligned;
> +};
> +#endif /* CONFIG_SPI_MASTER */
> +
> +struct st_lsm6dsx_transfer_function {
> +	int (*read)(struct device *dev, u8 addr, int len, u8 *data);
> +	int (*write)(struct device *dev, u8 addr, int len, u8 *data);
> +};
> +
> +struct st_lsm6dsx_reg {
> +	u8 addr;
> +	u8 mask;
> +};
> +
> +struct st_lsm6dsx_settings {
> +	u8 wai;
> +	u16 max_fifo_size;
> +};
> +
> +enum st_lsm6dsx_sensor_id {
> +	ST_LSM6DSX_ID_ACC,
> +	ST_LSM6DSX_ID_GYRO,
> +	ST_LSM6DSX_ID_MAX,
> +};
> +
> +enum st_lsm6dsx_fifo_mode {
> +	ST_LSM6DSX_FIFO_BYPASS = 0x0,
> +	ST_LSM6DSX_FIFO_CONT = 0x6,
> +};
> +
> +struct st_lsm6dsx_sensor {
> +	enum st_lsm6dsx_sensor_id id;
> +	struct st_lsm6dsx_hw *hw;
> +
> +	u32 gain;
> +	u16 odr;
> +
> +	u16 watermark;
> +	u8 sip;
> +	u8 decimator;
> +	u8 decimator_mask;
> +
> +	s64 delta_ts;
> +	s64 ts;
> +};
> +
> +struct st_lsm6dsx_hw {
> +	const char *name;
> +	struct device *dev;
> +	int irq;
> +	struct mutex lock;
> +
> +	enum st_lsm6dsx_fifo_mode fifo_mode;
> +	u8 enable_mask;
> +	u8 sip;
> +
> +	struct iio_dev *iio_devs[ST_LSM6DSX_ID_MAX];
> +
> +	const struct st_lsm6dsx_settings *settings;
> +
> +	const struct st_lsm6dsx_transfer_function *tf;
> +#if defined(CONFIG_SPI_MASTER)
> +	struct st_lsm6dsx_transfer_buffer tb;
> +#endif /* CONFIG_SPI_MASTER */
> +};
> +
> +int st_lsm6dsx_probe(struct st_lsm6dsx_hw *hw);
> +int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
> +int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
> +int st_lsm6dsx_allocate_rings(struct st_lsm6dsx_hw *hw);
> +int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask,
> +			       u8 val);
> +int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor,
> +				u16 watermark);
> +
> +#endif /* ST_LSM6DSX_H */
> +
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> new file mode 100644
> index 0000000..ae4cf30
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> @@ -0,0 +1,696 @@
> +/*
> + * STMicroelectronics st_lsm6dsx sensor driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>
> + * Denis Ciocca <denis.ciocca@xxxxxx>
> + *
> + * Licensed under the GPL-2.
> + */
> +
> +#include <linux/kernel.h>
> +#include <linux/module.h>
> +#include <linux/delay.h>
> +#include <linux/iio/iio.h>
> +#include <linux/iio/sysfs.h>
> +#include <asm/unaligned.h>
> +
> +#include "st_lsm6dsx.h"
> +
> +#define ST_LSM6DSX_REG_ACC_DEC_MASK		0x07

masks could be represented using GENMASK() or BIT()

> +#define ST_LSM6DSX_REG_GYRO_DEC_MASK		0x38
> +#define ST_LSM6DSX_REG_INT1_ADDR		0x0d
> +#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK	0x08
> +#define ST_LSM6DSX_REG_WHOAMI_ADDR		0x0f
> +#define ST_LSM6DSX_REG_RESET_ADDR		0x12
> +#define ST_LSM6DSX_REG_RESET_MASK		0x01
> +#define ST_LSM6DSX_REG_BDU_ADDR			0x12
> +#define ST_LSM6DSX_REG_BDU_MASK			0x40
> +#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR	0x13
> +#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK	0x20
> +#define ST_LSM6DSX_REG_ROUNDING_ADDR		0x16
> +#define ST_LSM6DSX_REG_ROUNDING_MASK		0x04
> +#define ST_LSM6DSX_REG_LIR_ADDR			0x58
> +#define ST_LSM6DSX_REG_LIR_MASK			0x01
> +
> +#define ST_LSM6DSX_REG_ACC_ODR_ADDR		0x10
> +#define ST_LSM6DSX_REG_ACC_ODR_MASK		0xf0
> +#define ST_LSM6DSX_REG_ACC_FS_ADDR		0x10
> +#define ST_LSM6DSX_REG_ACC_FS_MASK		0x0c
> +#define ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR		0x28
> +#define ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR		0x2a
> +#define ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR		0x2c
> +
> +#define ST_LSM6DSX_REG_GYRO_ODR_ADDR		0x11
> +#define ST_LSM6DSX_REG_GYRO_ODR_MASK		0xf0
> +#define ST_LSM6DSX_REG_GYRO_FS_ADDR		0x11
> +#define ST_LSM6DSX_REG_GYRO_FS_MASK		0x0c
> +#define ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR	0x22
> +#define ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR	0x24
> +#define ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR	0x26
> +
> +#define ST_LSM6DS3_WHOAMI			0x69
> +#define ST_LSM6DSM_WHOAMI			0x6a
> +
> +#define ST_LSM6DS3_MAX_FIFO_SIZE		8192
> +#define ST_LSM6DSM_MAX_FIFO_SIZE		4096
> +
> +#define ST_LSM6DSX_ACC_FS_2G_GAIN		IIO_G_TO_M_S_2(61)
> +#define ST_LSM6DSX_ACC_FS_4G_GAIN		IIO_G_TO_M_S_2(122)
> +#define ST_LSM6DSX_ACC_FS_8G_GAIN		IIO_G_TO_M_S_2(244)
> +#define ST_LSM6DSX_ACC_FS_16G_GAIN		IIO_G_TO_M_S_2(488)
> +
> +#define ST_LSM6DSX_GYRO_FS_245_GAIN		IIO_DEGREE_TO_RAD(4375)
> +#define ST_LSM6DSX_GYRO_FS_500_GAIN		IIO_DEGREE_TO_RAD(8750)
> +#define ST_LSM6DSX_GYRO_FS_1000_GAIN		IIO_DEGREE_TO_RAD(17500)
> +#define ST_LSM6DSX_GYRO_FS_2000_GAIN		IIO_DEGREE_TO_RAD(70000)
> +
> +struct st_lsm6dsx_odr {
> +	u16 hz;
> +	u8 val;
> +};
> +
> +#define ST_LSM6DSX_ODR_LIST_SIZE	6
> +struct st_lsm6dsx_odr_table_entry {
> +	struct st_lsm6dsx_reg reg;
> +	struct st_lsm6dsx_odr odr_avl[ST_LSM6DSX_ODR_LIST_SIZE];
> +};
> +
> +static const struct st_lsm6dsx_odr_table_entry st_lsm6dsx_odr_table[] = {
> +	[ST_LSM6DSX_ID_ACC] = {
> +		.reg = {
> +			.addr = ST_LSM6DSX_REG_ACC_ODR_ADDR,
> +			.mask = ST_LSM6DSX_REG_ACC_ODR_MASK,
> +		},
> +		.odr_avl[0] = {  13, 0x01 },
> +		.odr_avl[1] = {  26, 0x02 },
> +		.odr_avl[2] = {  52, 0x03 },
> +		.odr_avl[3] = { 104, 0x04 },
> +		.odr_avl[4] = { 208, 0x05 },
> +		.odr_avl[5] = { 416, 0x06 },
> +	},
> +	[ST_LSM6DSX_ID_GYRO] = {
> +		.reg = {
> +			.addr = ST_LSM6DSX_REG_GYRO_ODR_ADDR,
> +			.mask = ST_LSM6DSX_REG_GYRO_ODR_MASK,
> +		},
> +		.odr_avl[0] = {  13, 0x01 },
> +		.odr_avl[1] = {  26, 0x02 },
> +		.odr_avl[2] = {  52, 0x03 },
> +		.odr_avl[3] = { 104, 0x04 },
> +		.odr_avl[4] = { 208, 0x05 },
> +		.odr_avl[5] = { 416, 0x06 },
> +	}
> +};
> +
> +struct st_lsm6dsx_fs {
> +	u32 gain;
> +	u8 val;
> +};
> +
> +#define ST_LSM6DSX_FS_LIST_SIZE		4
> +struct st_lsm6dsx_fs_table_entry {
> +	struct st_lsm6dsx_reg reg;
> +	struct st_lsm6dsx_fs fs_avl[ST_LSM6DSX_FS_LIST_SIZE];
> +};
> +
> +static const struct st_lsm6dsx_fs_table_entry st_lsm6dsx_fs_table[] = {
> +	[ST_LSM6DSX_ID_ACC] = {
> +		.reg = {
> +			.addr = ST_LSM6DSX_REG_ACC_FS_ADDR,
> +			.mask = ST_LSM6DSX_REG_ACC_FS_MASK,
> +		},
> +		.fs_avl[0] = {  ST_LSM6DSX_ACC_FS_2G_GAIN, 0x0 },
> +		.fs_avl[1] = {  ST_LSM6DSX_ACC_FS_4G_GAIN, 0x2 },
> +		.fs_avl[2] = {  ST_LSM6DSX_ACC_FS_8G_GAIN, 0x3 },
> +		.fs_avl[3] = { ST_LSM6DSX_ACC_FS_16G_GAIN, 0x1 },
> +	},
> +	[ST_LSM6DSX_ID_GYRO] = {
> +		.reg = {
> +			.addr = ST_LSM6DSX_REG_GYRO_FS_ADDR,
> +			.mask = ST_LSM6DSX_REG_GYRO_FS_MASK,
> +		},
> +		.fs_avl[0] = {  ST_LSM6DSX_GYRO_FS_245_GAIN, 0x0 },
> +		.fs_avl[1] = {  ST_LSM6DSX_GYRO_FS_500_GAIN, 0x1 },
> +		.fs_avl[2] = { ST_LSM6DSX_GYRO_FS_1000_GAIN, 0x2 },
> +		.fs_avl[3] = { ST_LSM6DSX_GYRO_FS_2000_GAIN, 0x3 },
> +	}
> +};
> +
> +static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
> +	{
> +		.wai = ST_LSM6DS3_WHOAMI,
> +		.max_fifo_size = ST_LSM6DS3_MAX_FIFO_SIZE,
> +	},
> +	{
> +		.wai = ST_LSM6DSM_WHOAMI,
> +		.max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
> +	},
> +};
> +
> +static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {

can't be have #defines for the channels as most drivers do? this is highly 
repetitive

> +	{
> +		.type = IIO_ACCEL,
> +		.address = ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
> +		.modified = 1,
> +		.channel2 = IIO_MOD_X,
> +		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
> +				      BIT(IIO_CHAN_INFO_SCALE),
> +		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
> +		.scan_index = 0,
> +		.scan_type = {
> +			.sign = 's',
> +			.realbits = 16,
> +			.storagebits = 16,
> +			.endianness = IIO_LE,
> +		},
> +	},
> +	{
> +		.type = IIO_ACCEL,
> +		.address = ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
> +		.modified = 1,
> +		.channel2 = IIO_MOD_Y,
> +		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
> +				      BIT(IIO_CHAN_INFO_SCALE),
> +		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
> +		.scan_index = 1,
> +		.scan_type = {
> +			.sign = 's',
> +			.realbits = 16,
> +			.storagebits = 16,
> +			.endianness = IIO_LE,
> +		},
> +	},
> +	{
> +		.type = IIO_ACCEL,
> +		.address = ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
> +		.modified = 1,
> +		.channel2 = IIO_MOD_Z,
> +		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
> +				      BIT(IIO_CHAN_INFO_SCALE),
> +		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
> +		.scan_index = 2,
> +		.scan_type = {
> +			.sign = 's',
> +			.realbits = 16,
> +			.storagebits = 16,
> +			.endianness = IIO_LE,
> +		},
> +	},
> +	IIO_CHAN_SOFT_TIMESTAMP(3),
> +};
> +
> +static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
> +	{
> +		.type = IIO_ANGL_VEL,
> +		.address = ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
> +		.modified = 1,
> +		.channel2 = IIO_MOD_X,
> +		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
> +				      BIT(IIO_CHAN_INFO_SCALE),
> +		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
> +		.scan_index = 0,
> +		.scan_type = {
> +			.sign = 's',
> +			.realbits = 16,
> +			.storagebits = 16,
> +			.endianness = IIO_LE,
> +		},
> +	},
> +	{
> +		.type = IIO_ANGL_VEL,
> +		.address = ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
> +		.modified = 1,
> +		.channel2 = IIO_MOD_Y,
> +		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
> +				      BIT(IIO_CHAN_INFO_SCALE),
> +		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
> +		.scan_index = 1,
> +		.scan_type = {
> +			.sign = 's',
> +			.realbits = 16,
> +			.storagebits = 16,
> +			.endianness = IIO_LE,
> +		},
> +	},
> +	{
> +		.type = IIO_ANGL_VEL,
> +		.address = ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
> +		.modified = 1,
> +		.channel2 = IIO_MOD_Z,
> +		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
> +				      BIT(IIO_CHAN_INFO_SCALE),
> +		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ),
> +		.scan_index = 2,
> +		.scan_type = {
> +			.sign = 's',
> +			.realbits = 16,
> +			.storagebits = 16,
> +			.endianness = IIO_LE,
> +		},
> +	},
> +	IIO_CHAN_SOFT_TIMESTAMP(3),
> +};
> +
> +int st_lsm6dsx_write_with_mask(struct st_lsm6dsx_hw *hw, u8 addr, u8 mask,
> +			       u8 val)
> +{
> +	u8 data;
> +	int err;
> +
> +	mutex_lock(&hw->lock);
> +
> +	err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
> +	if (err < 0) {
> +		dev_err(hw->dev, "failed to read %02x register\n", addr);
> +		goto out;
> +	}
> +
> +	data = (data & ~mask) | ((val << __ffs(mask)) & mask);
> +
> +	err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
> +	if (err < 0)
> +		dev_err(hw->dev, "failed to write %02x register\n", addr);
> +
> +out:
> +	mutex_unlock(&hw->lock);
> +
> +	return err;
> +}
> +
> +static int st_lsm6dsx_check_whoami(struct st_lsm6dsx_hw *hw)
> +{
> +	int err, i;

it is not clear when you use 'ret' or 'err'

> +	u8 data;
> +
> +	err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_WHOAMI_ADDR, sizeof(data),
> +			   &data);
> +	if (err < 0) {
> +		dev_err(hw->dev, "failed to read whoami register\n");
> +		return err;
> +	}
> +
> +	for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
> +		if (data == st_lsm6dsx_sensor_settings[i].wai) {
> +			hw->settings = &st_lsm6dsx_sensor_settings[i];
> +			break;
> +		}
> +	}
> +
> +	if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
> +		dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
> +		return -ENODEV;
> +	}
> +
> +	return 0;
> +}
> +
> +static int st_lsm6dsx_set_fs(struct st_lsm6dsx_sensor *sensor, u32 gain)
> +{
> +	enum st_lsm6dsx_sensor_id id = sensor->id;
> +	int i, err;
> +	u8 val;
> +
> +	for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
> +		if (st_lsm6dsx_fs_table[id].fs_avl[i].gain == gain)
> +			break;
> +
> +	if (i == ST_LSM6DSX_FS_LIST_SIZE)
> +		return -EINVAL;
> +
> +	val = st_lsm6dsx_fs_table[id].fs_avl[i].val;
> +	err = st_lsm6dsx_write_with_mask(sensor->hw,
> +					 st_lsm6dsx_fs_table[id].reg.addr,
> +					 st_lsm6dsx_fs_table[id].reg.mask,
> +					 val);
> +	if (err < 0)
> +		return err;
> +
> +	sensor->gain = gain;
> +
> +	return 0;
> +}
> +
> +static int st_lsm6dsx_set_odr(struct st_lsm6dsx_sensor *sensor, u16 odr)
> +{
> +	enum st_lsm6dsx_sensor_id id = sensor->id;
> +	int i, err, val;

val could be u8

> +
> +	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
> +		if (st_lsm6dsx_odr_table[id].odr_avl[i].hz == odr)
> +			break;
> +
> +	if (i == ST_LSM6DSX_ODR_LIST_SIZE)
> +		return -EINVAL;
> +
> +	val = st_lsm6dsx_odr_table[id].odr_avl[i].val;
> +	err = st_lsm6dsx_write_with_mask(sensor->hw,
> +					 st_lsm6dsx_odr_table[id].reg.addr,
> +					 st_lsm6dsx_odr_table[id].reg.mask,
> +					 val);
> +	if (err < 0)
> +		return err;
> +
> +	sensor->odr = odr;
> +
> +	return 0;
> +}
> +
> +int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor)
> +{
> +	int err;
> +
> +	err = st_lsm6dsx_set_odr(sensor, sensor->odr);
> +	if (err < 0)
> +		return err;
> +
> +	sensor->hw->enable_mask |= BIT(sensor->id);
> +
> +	return 0;
> +}
> +
> +int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor)
> +{
> +	enum st_lsm6dsx_sensor_id id = sensor->id;
> +	int err;
> +
> +	err = st_lsm6dsx_write_with_mask(sensor->hw,
> +					 st_lsm6dsx_odr_table[id].reg.addr,
> +					 st_lsm6dsx_odr_table[id].reg.mask, 0);
> +	if (err < 0)
> +		return err;
> +
> +	sensor->hw->enable_mask &= ~BIT(id);
> +
> +	return 0;
> +}
> +
> +static int st_lsm6dsx_read_oneshot(struct st_lsm6dsx_sensor *sensor,
> +				   u8 addr, int *val)
> +{
> +	int err, delay;
> +	u8 data[2];

data should be __le16 not an array[2] u8

> +	err = st_lsm6dsx_sensor_enable(sensor);
> +	if (err < 0)
> +		return err;
> +
> +	delay = 1000000 / sensor->odr;
> +	usleep_range(delay, 2 * delay);
> +
> +	err = sensor->hw->tf->read(sensor->hw->dev, addr, sizeof(data), data);
> +	if (err < 0)
> +		return err;
> +
> +	st_lsm6dsx_sensor_disable(sensor);
> +
> +	*val = (s16)get_unaligned_le16(data);

if the variable's type is le16, I think we don't need the get_unaligned()
anymore

> +
> +	return IIO_VAL_INT;
> +}
> +
> +static int st_lsm6dsx_read_raw(struct iio_dev *iio_dev,
> +			       struct iio_chan_spec const *ch,
> +			       int *val, int *val2, long mask)
> +{
> +	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
> +	int ret;
> +
> +	ret = iio_device_claim_direct_mode(iio_dev);
> +	if (ret)
> +		return ret;
> +
> +	switch (mask) {
> +	case IIO_CHAN_INFO_RAW:
> +		ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
> +		break;
> +	case IIO_CHAN_INFO_SAMP_FREQ:
> +		*val = sensor->odr;
> +		ret = IIO_VAL_INT;
> +		break;
> +	case IIO_CHAN_INFO_SCALE:
> +		*val = 0;
> +		*val2 = sensor->gain;
> +		ret = IIO_VAL_INT_PLUS_MICRO;
> +		break;
> +	default:
> +		ret = -EINVAL;
> +		break;
> +	}
> +
> +	iio_device_release_direct_mode(iio_dev);
> +
> +	return ret;
> +}
> +
> +static int st_lsm6dsx_write_raw(struct iio_dev *iio_dev,
> +				struct iio_chan_spec const *chan,
> +				int val, int val2, long mask)
> +{
> +	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
> +	int err;
> +
> +	err = iio_device_claim_direct_mode(iio_dev);
> +	if (err)
> +		return err;
> +
> +	switch (mask) {
> +	case IIO_CHAN_INFO_SCALE:
> +		err = st_lsm6dsx_set_fs(sensor, val2);
> +		break;
> +	case IIO_CHAN_INFO_SAMP_FREQ:
> +		err = st_lsm6dsx_set_odr(sensor, val);
> +		break;
> +	default:
> +		err = -EINVAL;
> +		break;
> +	}
> +
> +	iio_device_release_direct_mode(iio_dev);
> +
> +	return err;
> +}
> +
> +static int st_lsm6dsx_set_watermark(struct iio_dev *iio_dev, unsigned int val)
> +{
> +	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
> +	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	int err, max_fifo_len;
> +
> +	max_fifo_len = hw->settings->max_fifo_size / ST_LSM6DSX_SAMPLE_SIZE;
> +	if (val < 1 || val > max_fifo_len)
> +		return -EINVAL;
> +
> +	err = st_lsm6dsx_update_watermark(sensor, val);
> +	if (err < 0)
> +		return err;
> +
> +	sensor->watermark = val;
> +
> +	return 0;
> +}
> +
> +static ssize_t
> +st_lsm6dsx_sysfs_sampling_frequency_avl(struct device *dev,
> +					struct device_attribute *attr,
> +					char *buf)
> +{
> +	struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
> +	enum st_lsm6dsx_sensor_id id = sensor->id;
> +	int i, len = 0;
> +
> +	for (i = 0; i < ST_LSM6DSX_ODR_LIST_SIZE; i++)
> +		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
> +				 st_lsm6dsx_odr_table[id].odr_avl[i].hz);
> +	buf[len - 1] = '\n';
> +
> +	return len;
> +}
> +
> +static ssize_t st_lsm6dsx_sysfs_scale_avail(struct device *dev,
> +					    struct device_attribute *attr,
> +					    char *buf)
> +{
> +	struct st_lsm6dsx_sensor *sensor = iio_priv(dev_get_drvdata(dev));
> +	enum st_lsm6dsx_sensor_id id = sensor->id;
> +	int i, len = 0;
> +
> +	for (i = 0; i < ST_LSM6DSX_FS_LIST_SIZE; i++)
> +		len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
> +				 st_lsm6dsx_fs_table[id].fs_avl[i].gain);
> +	buf[len - 1] = '\n';
> +
> +	return len;
> +}
> +
> +static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_lsm6dsx_sysfs_sampling_frequency_avl);
> +static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
> +		       st_lsm6dsx_sysfs_scale_avail, NULL, 0);
> +static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
> +		       st_lsm6dsx_sysfs_scale_avail, NULL, 0);
> +
> +static struct attribute *st_lsm6dsx_acc_attributes[] = {
> +	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
> +	&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
> +	NULL,
> +};
> +
> +static const struct attribute_group st_lsm6dsx_acc_attribute_group = {
> +	.attrs = st_lsm6dsx_acc_attributes,
> +};
> +
> +static const struct iio_info st_lsm6dsx_acc_info = {
> +	.driver_module = THIS_MODULE,
> +	.attrs = &st_lsm6dsx_acc_attribute_group,
> +	.read_raw = st_lsm6dsx_read_raw,
> +	.write_raw = st_lsm6dsx_write_raw,
> +	.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
> +};
> +
> +static struct attribute *st_lsm6dsx_gyro_attributes[] = {
> +	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
> +	&iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
> +	NULL,
> +};
> +
> +static const struct attribute_group st_lsm6dsx_gyro_attribute_group = {
> +	.attrs = st_lsm6dsx_gyro_attributes,
> +};
> +
> +static const struct iio_info st_lsm6dsx_gyro_info = {
> +	.driver_module = THIS_MODULE,
> +	.attrs = &st_lsm6dsx_gyro_attribute_group,
> +	.read_raw = st_lsm6dsx_read_raw,
> +	.write_raw = st_lsm6dsx_write_raw,
> +	.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
> +};
> +
> +static const unsigned long st_lsm6dsx_available_scan_masks[] = {0x7, 0x0};
> +
> +static int st_lsm6dsx_init_device(struct st_lsm6dsx_hw *hw)
> +{
> +	int err;
> +	u8 data;
> +
> +	data = ST_LSM6DSX_REG_RESET_MASK;
> +	err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_RESET_ADDR, sizeof(data),
> +			    &data);
> +	if (err < 0)
> +		return err;
> +
> +	msleep(200);
> +
> +	/* latch interrupts */
> +	err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_LIR_ADDR,
> +					 ST_LSM6DSX_REG_LIR_MASK, 1);
> +	if (err < 0)
> +		return err;
> +
> +	/* enable BDU */

whatever BDU is :)

> +	err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_BDU_ADDR,
> +					 ST_LSM6DSX_REG_BDU_MASK, 1);
> +	if (err < 0)
> +		return err;
> +
> +	err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_ROUNDING_ADDR,
> +					 ST_LSM6DSX_REG_ROUNDING_MASK, 1);
> +	if (err < 0)
> +		return err;
> +
> +	/* enable FIFO watermak interrupt */
> +	err = st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_INT1_ADDR,
> +					 ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK, 1);
> +	if (err < 0)
> +		return err;
> +
> +	/* redirect INT2 on INT1 */
> +	return st_lsm6dsx_write_with_mask(hw, ST_LSM6DSX_REG_INT2_ON_INT1_ADDR,
> +					  ST_LSM6DSX_REG_INT2_ON_INT1_MASK, 1);
> +}
> +
> +static struct iio_dev *st_lsm6dsx_alloc_iiodev(struct st_lsm6dsx_hw *hw,
> +					       enum st_lsm6dsx_sensor_id id)
> +{
> +	struct st_lsm6dsx_sensor *sensor;
> +	struct iio_dev *iio_dev;
> +
> +	iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
> +	if (!iio_dev)
> +		return NULL;
> +
> +	iio_dev->modes = INDIO_DIRECT_MODE;
> +	iio_dev->dev.parent = hw->dev;
> +	iio_dev->available_scan_masks = st_lsm6dsx_available_scan_masks;
> +
> +	sensor = iio_priv(iio_dev);
> +	sensor->id = id;
> +	sensor->hw = hw;
> +	sensor->odr = st_lsm6dsx_odr_table[id].odr_avl[0].hz;
> +	sensor->gain = st_lsm6dsx_fs_table[id].fs_avl[0].gain;
> +	sensor->watermark = 1;
> +
> +	switch (id) {
> +	case ST_LSM6DSX_ID_ACC:
> +		iio_dev->channels = st_lsm6dsx_acc_channels;
> +		iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_acc_channels);
> +		iio_dev->name = "lsm6dsx_accel";
> +		iio_dev->info = &st_lsm6dsx_acc_info;
> +
> +		sensor->decimator_mask = ST_LSM6DSX_REG_ACC_DEC_MASK;
> +		break;
> +	case ST_LSM6DSX_ID_GYRO:
> +		iio_dev->channels = st_lsm6dsx_gyro_channels;
> +		iio_dev->num_channels = ARRAY_SIZE(st_lsm6dsx_gyro_channels);
> +		iio_dev->name = "lsm6dsx_gyro";
> +		iio_dev->info = &st_lsm6dsx_gyro_info;
> +
> +		sensor->decimator_mask = ST_LSM6DSX_REG_GYRO_DEC_MASK;
> +		break;
> +	default:
> +		return NULL;
> +	}
> +
> +	return iio_dev;
> +}
> +
> +int st_lsm6dsx_probe(struct st_lsm6dsx_hw *hw)
> +{
> +	int i, err;
> +
> +	mutex_init(&hw->lock);
> +
> +	err = st_lsm6dsx_check_whoami(hw);
> +	if (err < 0)
> +		return err;
> +
> +	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
> +		hw->iio_devs[i] = st_lsm6dsx_alloc_iiodev(hw, i);
> +		if (!hw->iio_devs[i])
> +			return -ENOMEM;
> +	}
> +
> +	err = st_lsm6dsx_init_device(hw);
> +	if (err < 0)
> +		return err;
> +
> +	if (hw->irq > 0) {
> +		err = st_lsm6dsx_allocate_rings(hw);
> +		if (err < 0)
> +			return err;
> +	}
> +
> +	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
> +		err = devm_iio_device_register(hw->dev, hw->iio_devs[i]);
> +		if (err)
> +			return err;
> +	}
> +
> +	return 0;
> +}
> +EXPORT_SYMBOL(st_lsm6dsx_probe);
> +
> +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@xxxxxx>");
> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx driver");
> +MODULE_LICENSE("GPL v2");
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
> new file mode 100644
> index 0000000..c80e624
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
> @@ -0,0 +1,111 @@
> +/*
> + * STMicroelectronics st_lsm6dsx i2c driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>
> + * Denis Ciocca <denis.ciocca@xxxxxx>
> + *
> + * Licensed under the GPL-2.
> + */
> +
> +#include <linux/kernel.h>
> +#include <linux/module.h>
> +#include <linux/i2c.h>
> +#include <linux/slab.h>
> +#include <linux/of.h>
> +
> +#include "st_lsm6dsx.h"
> +
> +static int st_lsm6dsx_i2c_read(struct device *dev, u8 addr, int len, u8 *data)
> +{
> +	struct i2c_client *client = to_i2c_client(dev);
> +	struct i2c_msg msg[2];

is this basically i2c_smbus_read_i2c_block_data()?

> +
> +	msg[0].addr = client->addr;
> +	msg[0].flags = client->flags;
> +	msg[0].len = 1;
> +	msg[0].buf = &addr;
> +
> +	msg[1].addr = client->addr;
> +	msg[1].flags = client->flags | I2C_M_RD;
> +	msg[1].len = len;
> +	msg[1].buf = data;
> +
> +	return i2c_transfer(client->adapter, msg, 2);
> +}
> +
> +static int st_lsm6dsx_i2c_write(struct device *dev, u8 addr, int len, u8 *data)
> +{
> +	struct i2c_client *client = to_i2c_client(dev);
> +	struct i2c_msg msg;
> +	u8 send[len + 1];
> +
> +	send[0] = addr;
> +	memcpy(&send[1], data, len * sizeof(u8));
> +
> +	msg.addr = client->addr;
> +	msg.flags = client->flags;
> +	msg.len = len + 1;
> +	msg.buf = send;
> +
> +	return i2c_transfer(client->adapter, &msg, 1);
> +}
> +
> +static const struct st_lsm6dsx_transfer_function st_lsm6dsx_transfer_fn = {
> +	.read = st_lsm6dsx_i2c_read,
> +	.write = st_lsm6dsx_i2c_write,
> +};
> +
> +static int st_lsm6dsx_i2c_probe(struct i2c_client *client,
> +				const struct i2c_device_id *id)
> +{
> +	struct st_lsm6dsx_hw *hw;
> +
> +	hw = devm_kzalloc(&client->dev, sizeof(*hw), GFP_KERNEL);
> +	if (!hw)
> +		return -ENOMEM;
> +
> +	i2c_set_clientdata(client, hw);
> +	hw->name = client->name;
> +	hw->dev = &client->dev;
> +	hw->irq = client->irq;
> +	hw->tf = &st_lsm6dsx_transfer_fn;
> +
> +	return st_lsm6dsx_probe(hw);
> +}
> +
> +static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
> +	{
> +		.compatible = "st,lsm6ds3",
> +		.data = ST_LSM6DS3_DEV_NAME,
> +	},
> +	{
> +		.compatible = "st,lsm6dsm",
> +		.data = ST_LSM6DSM_DEV_NAME,
> +	},
> +	{},
> +};
> +MODULE_DEVICE_TABLE(of, st_lsm6dsx_i2c_of_match);
> +
> +static const struct i2c_device_id st_lsm6dsx_i2c_id_table[] = {
> +	{ ST_LSM6DS3_DEV_NAME },
> +	{ ST_LSM6DSM_DEV_NAME },
> +	{},
> +};
> +MODULE_DEVICE_TABLE(i2c, st_lsm6dsx_i2c_id_table);
> +
> +static struct i2c_driver st_lsm6dsx_driver = {
> +	.driver = {
> +		.name = "st_lsm6dsx_i2c",
> +		.of_match_table = of_match_ptr(st_lsm6dsx_i2c_of_match),
> +	},
> +	.probe = st_lsm6dsx_i2c_probe,
> +	.id_table = st_lsm6dsx_i2c_id_table,
> +};
> +module_i2c_driver(st_lsm6dsx_driver);
> +
> +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@xxxxxx>");
> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver");
> +MODULE_LICENSE("GPL v2");
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_ring.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_ring.c
> new file mode 100644
> index 0000000..9a8c503
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_ring.c
> @@ -0,0 +1,401 @@
> +/*
> + * STMicroelectronics st_lsm6dsx sensor driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>
> + * Denis Ciocca <denis.ciocca@xxxxxx>
> + *
> + * Licensed under the GPL-2.
> + */
> +#include <linux/module.h>
> +#include <linux/interrupt.h>
> +#include <linux/irq.h>
> +#include <linux/iio/kfifo_buf.h>
> +#include <asm/unaligned.h>
> +
> +#include "st_lsm6dsx.h"
> +
> +#define ST_LSM6DSX_REG_FIFO_THL_ADDR		0x06
> +#define ST_LSM6DSX_REG_FIFO_THH_ADDR		0x07
> +#define ST_LSM6DSX_FIFO_TH_MASK			0x0fff
> +#define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR	0x08
> +#define ST_LSM6DSX_REG_FIFO_MODE_ADDR		0x0a
> +#define ST_LSM6DSX_FIFO_MODE_MASK		0x07
> +#define ST_LSM6DSX_FIFO_ODR_MASK		0x78
> +#define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR		0x3a
> +#define ST_LSM6DSX_FIFO_DIFF_MASK		0x0f
> +#define ST_LSM6DSX_FIFO_EMPTY_MASK		0x10
> +#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR		0x3e
> +
> +struct st_lsm6dsx_dec_entry {
> +	u8 decimator;
> +	u8 val;
> +};
> +
> +static const struct st_lsm6dsx_dec_entry st_lsm6dsx_dec_table[] = {
> +	{  0, 0x0 },
> +	{  1, 0x1 },
> +	{  2, 0x2 },
> +	{  3, 0x3 },
> +	{  4, 0x4 },
> +	{  8, 0x5 },
> +	{ 16, 0x6 },
> +	{ 32, 0x7 },
> +};
> +
> +static int st_lsm6dsx_get_decimator_val(u8 val)
> +{
> +	int i, max_size = ARRAY_SIZE(st_lsm6dsx_dec_table);

max_size could be const

> +
> +	for (i = 0; i < max_size; i++)
> +		if (st_lsm6dsx_dec_table[i].decimator == val)
> +			break;
> +
> +	return i == max_size ? 0 : st_lsm6dsx_dec_table[i].val;
> +}
> +
> +static void st_lsm6dsx_get_max_min_odr(struct st_lsm6dsx_hw *hw,
> +				       u16 *max_odr, u16 *min_odr)
> +{
> +	struct st_lsm6dsx_sensor *sensor;
> +	int i;
> +
> +	*max_odr = 0, *min_odr = ~0;
> +	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
> +		sensor = iio_priv(hw->iio_devs[i]);
> +
> +		if (!(hw->enable_mask & BIT(sensor->id)))
> +			continue;
> +
> +		*max_odr = max_t(u16, *max_odr, sensor->odr);
> +		*min_odr = min_t(u16, *min_odr, sensor->odr);
> +	}
> +}
> +
> +static int st_lsm6dsx_update_decimators(struct st_lsm6dsx_hw *hw)
> +{
> +	struct st_lsm6dsx_sensor *sensor;
> +	u16 max_odr, min_odr, sip = 0;
> +	int err, i;
> +	u8 data;
> +
> +	st_lsm6dsx_get_max_min_odr(hw, &max_odr, &min_odr);
> +
> +	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
> +		sensor = iio_priv(hw->iio_devs[i]);
> +
> +		/* update fifo decimators and sample in pattern */
> +		if (hw->enable_mask & BIT(sensor->id)) {
> +			sensor->sip = sensor->odr / min_odr;
> +			sensor->decimator = max_odr / sensor->odr;
> +			data = st_lsm6dsx_get_decimator_val(sensor->decimator);
> +		} else {
> +			sensor->sip = 0;
> +			sensor->decimator = 0;
> +			data = 0;
> +		}
> +
> +		err = st_lsm6dsx_write_with_mask(hw,
> +					ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR,
> +					sensor->decimator_mask, data);
> +		if (err < 0)
> +			return err;
> +
> +		sip += sensor->sip;
> +	}
> +	hw->sip = sip;
> +
> +	return 0;
> +}
> +
> +static int st_lsm6dsx_set_fifo_mode(struct st_lsm6dsx_hw *hw,
> +				    enum st_lsm6dsx_fifo_mode fifo_mode)
> +{
> +	u8 data;
> +	int err;
> +
> +	switch (fifo_mode) {
> +	case ST_LSM6DSX_FIFO_BYPASS:
> +		data = fifo_mode;
> +		break;
> +	case ST_LSM6DSX_FIFO_CONT:
> +		data = fifo_mode | 0x40;

magic 0x40

> +		break;
> +	default:
> +		return -EINVAL;
> +	}
> +
> +	err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_MODE_ADDR,
> +			    sizeof(data), &data);
> +	if (err < 0)
> +		return err;
> +
> +	hw->fifo_mode = fifo_mode;
> +
> +	return 0;
> +}
> +
> +int st_lsm6dsx_update_watermark(struct st_lsm6dsx_sensor *sensor, u16 watermark)
> +{
> +	u16 fifo_watermark = ~0, cur_watermark, sip = 0;
> +	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	struct st_lsm6dsx_sensor *cur_sensor;
> +	int i, err;
> +	u8 data;
> +
> +	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
> +		cur_sensor = iio_priv(hw->iio_devs[i]);
> +
> +		if (!(hw->enable_mask & BIT(cur_sensor->id)))
> +			continue;
> +
> +		cur_watermark = (cur_sensor == sensor) ? watermark
> +						       : cur_sensor->watermark;
> +
> +		fifo_watermark = min_t(u16, fifo_watermark, cur_watermark);
> +		sip += cur_sensor->sip;
> +	}
> +
> +	if (!sip)
> +		return 0;
> +
> +	fifo_watermark = max_t(u16, fifo_watermark, sip);
> +	fifo_watermark = (fifo_watermark / sip) * sip;
> +	fifo_watermark = fifo_watermark * ST_LSM6DSX_SAMPLE_DEPTH;
> +
> +	mutex_lock(&hw->lock);
> +
> +	err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_THH_ADDR,
> +			   sizeof(data), &data);
> +	if (err < 0)
> +		goto out;
> +
> +	fifo_watermark = ((data & ~ST_LSM6DSX_FIFO_TH_MASK) << 8) |
> +			  (fifo_watermark & ST_LSM6DSX_FIFO_TH_MASK);
> +
> +	err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
> +			    sizeof(fifo_watermark), (u8 *)&fifo_watermark);
> +out:
> +	mutex_unlock(&hw->lock);
> +
> +	return err < 0 ? err : 0;
> +}
> +
> +static int st_lsm6dsx_read_fifo(struct st_lsm6dsx_hw *hw)
> +{
> +	u16 fifo_len, pattern_len = hw->sip * ST_LSM6DSX_SAMPLE_SIZE;
> +	int err, acc_sip, gyro_sip, read_len, offset, samples;
> +	struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
> +	s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
> +	u8 iio_buf[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
> +	u8 fifo_status[2], buf[pattern_len];

use __le16 fifo_status

> +
> +	err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
> +			   sizeof(fifo_status), fifo_status);
> +	if (err < 0)
> +		return err;
> +
> +	if (fifo_status[1] & ST_LSM6DSX_FIFO_EMPTY_MASK)
> +		return 0;
> +
> +	fifo_status[1] &= ST_LSM6DSX_FIFO_DIFF_MASK;
> +	fifo_len = (u16)get_unaligned_le16(fifo_status) * ST_LSM6DSX_CHAN_SIZE;
> +	samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
> +	fifo_len = (fifo_len / pattern_len) * pattern_len;
> +	/*
> +	 * leave one complete pattern in FIFO to guarantee
> +	 * proper alignment
> +	 */
> +	fifo_len -= pattern_len;
> +
> +	/* compute delta timestamp */
> +	acc_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_ACC]);
> +	acc_ts = acc_sensor->ts - acc_sensor->delta_ts;
> +	acc_delta_ts = div_s64(acc_sensor->delta_ts * acc_sensor->decimator,
> +			       samples);
> +
> +	gyro_sensor = iio_priv(hw->iio_devs[ST_LSM6DSX_ID_GYRO]);
> +	gyro_ts = gyro_sensor->ts - gyro_sensor->delta_ts;
> +	gyro_delta_ts = div_s64(gyro_sensor->delta_ts * gyro_sensor->decimator,
> +				samples);
> +
> +	for (read_len = 0; read_len < fifo_len; read_len += pattern_len) {
> +		err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_OUTL_ADDR,
> +				   sizeof(buf), buf);
> +		if (err < 0)
> +			return err;
> +
> +		gyro_sip = gyro_sensor->sip;
> +		acc_sip = acc_sensor->sip;
> +		offset = 0;
> +
> +		while (acc_sip > 0 || gyro_sip > 0) {
> +			if (gyro_sip-- > 0) {
> +				memcpy(iio_buf, &buf[offset],
> +				       ST_LSM6DSX_SAMPLE_SIZE);
> +				iio_push_to_buffers_with_timestamp(
> +					hw->iio_devs[ST_LSM6DSX_ID_GYRO],
> +					iio_buf, gyro_ts);
> +				offset += ST_LSM6DSX_SAMPLE_SIZE;
> +				gyro_ts += gyro_delta_ts;
> +			}
> +
> +			if (acc_sip-- > 0) {
> +				memcpy(iio_buf, &buf[offset],
> +				       ST_LSM6DSX_SAMPLE_SIZE);
> +				iio_push_to_buffers_with_timestamp(
> +					hw->iio_devs[ST_LSM6DSX_ID_ACC],
> +					iio_buf, acc_ts);
> +				offset += ST_LSM6DSX_SAMPLE_SIZE;
> +				acc_ts += acc_delta_ts;
> +			}
> +		}
> +	}
> +
> +	return read_len;
> +}
> +
> +static int st_lsm6dsx_flush_fifo(struct st_lsm6dsx_hw *hw)
> +{
> +	int err;
> +
> +	disable_irq(hw->irq);
> +
> +	st_lsm6dsx_read_fifo(hw);
> +	err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
> +
> +	enable_irq(hw->irq);
> +
> +	return err;
> +}
> +
> +static int st_lsm6dsx_update_fifo(struct iio_dev *iio_dev, bool enable)
> +{
> +	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
> +	struct st_lsm6dsx_hw *hw = sensor->hw;
> +	int err;
> +
> +	if (hw->fifo_mode != ST_LSM6DSX_FIFO_BYPASS) {
> +		err = st_lsm6dsx_flush_fifo(hw);
> +		if (err < 0)
> +			return err;
> +	}
> +
> +	err = enable ? st_lsm6dsx_sensor_enable(sensor)
> +		     : st_lsm6dsx_sensor_disable(sensor);
> +	if (err < 0)
> +		return err;
> +
> +	err = st_lsm6dsx_update_decimators(hw);
> +	if (err < 0)
> +		return err;
> +
> +	err = st_lsm6dsx_update_watermark(sensor, sensor->watermark);
> +	if (err < 0)
> +		return err;
> +
> +	if (hw->enable_mask) {
> +		err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_CONT);
> +		if (err < 0)
> +			return err;
> +
> +		sensor->ts = iio_get_time_ns(iio_dev);
> +	}
> +
> +	return 0;
> +}
> +
> +static irqreturn_t st_lsm6dsx_ring_handler_irq(int irq, void *private)
> +{
> +	struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
> +	struct st_lsm6dsx_sensor *sensor;
> +	int i;
> +
> +	if (!hw->sip)
> +		return IRQ_NONE;
> +
> +	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
> +		sensor = iio_priv(hw->iio_devs[i]);
> +
> +		if (sensor->sip > 0) {
> +			s64 timestamp;
> +
> +			timestamp = iio_get_time_ns(hw->iio_devs[i]);
> +			sensor->delta_ts = timestamp - sensor->ts;
> +			sensor->ts = timestamp;
> +		}
> +	}
> +
> +	return IRQ_WAKE_THREAD;
> +}
> +
> +static irqreturn_t st_lsm6dsx_ring_handler_thread(int irq, void *private)
> +{
> +	struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
> +	int count;
> +
> +	count = st_lsm6dsx_read_fifo(hw);
> +
> +	return !count ? IRQ_NONE : IRQ_HANDLED;
> +}
> +
> +static int st_lsm6dsx_buffer_preenable(struct iio_dev *iio_dev)
> +{
> +	return st_lsm6dsx_update_fifo(iio_dev, true);
> +}
> +
> +static int st_lsm6dsx_buffer_postdisable(struct iio_dev *iio_dev)
> +{
> +	return st_lsm6dsx_update_fifo(iio_dev, false);
> +}
> +
> +static const struct iio_buffer_setup_ops st_lsm6dsx_buffer_ops = {
> +	.preenable = st_lsm6dsx_buffer_preenable,
> +	.postdisable = st_lsm6dsx_buffer_postdisable,
> +};
> +
> +int st_lsm6dsx_allocate_rings(struct st_lsm6dsx_hw *hw)
> +{
> +	struct iio_buffer *buffer;
> +	unsigned long irq_type;
> +	int i, err;
> +
> +	irq_type = irqd_get_trigger_type(irq_get_irq_data(hw->irq));
> +
> +	switch (irq_type) {
> +	case IRQF_TRIGGER_HIGH:
> +	case IRQF_TRIGGER_RISING:
> +		break;
> +	default:
> +		dev_info(hw->dev,
> +			 "mode %lx unsupported, using IRQF_TRIGGER_HIGH\n",
> +			 irq_type);

I'd rather fail than autocorrect, matter of taste probably

> +		irq_type = IRQF_TRIGGER_HIGH;
> +		break;
> +	}
> +
> +	err = devm_request_threaded_irq(hw->dev, hw->irq,
> +					st_lsm6dsx_ring_handler_irq,
> +					st_lsm6dsx_ring_handler_thread,
> +					irq_type | IRQF_ONESHOT,
> +					hw->name, hw);
> +	if (err) {
> +		dev_err(hw->dev, "failed to request trigger irq %d\n",
> +			hw->irq);
> +		return err;
> +	}
> +
> +	for (i = 0; i < ST_LSM6DSX_ID_MAX; i++) {
> +		buffer = devm_iio_kfifo_allocate(hw->dev);
> +		if (!buffer)
> +			return -ENOMEM;
> +
> +		iio_device_attach_buffer(hw->iio_devs[i], buffer);
> +		hw->iio_devs[i]->modes |= INDIO_BUFFER_SOFTWARE;
> +		hw->iio_devs[i]->setup_ops = &st_lsm6dsx_buffer_ops;
> +	}
> +
> +	return 0;
> +}
> +
> diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
> new file mode 100644
> index 0000000..262eae6
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
> @@ -0,0 +1,129 @@
> +/*
> + * STMicroelectronics st_lsm6dsx spi driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>
> + * Denis Ciocca <denis.ciocca@xxxxxx>
> + *
> + * Licensed under the GPL-2.
> + */
> +
> +#include <linux/kernel.h>
> +#include <linux/module.h>
> +#include <linux/spi/spi.h>
> +#include <linux/slab.h>
> +#include <linux/of.h>
> +
> +#include "st_lsm6dsx.h"
> +
> +#define SENSORS_SPI_READ	0x80
> +
> +static int st_lsm6dsx_spi_read(struct device *dev, u8 addr, int len,
> +			       u8 *data)
> +{
> +	struct spi_device *spi = to_spi_device(dev);
> +	struct st_lsm6dsx_hw *hw = spi_get_drvdata(spi);
> +	int err;
> +
> +	struct spi_transfer xfers[] = {
> +		{
> +			.tx_buf = hw->tb.tx_buf,
> +			.bits_per_word = 8,
> +			.len = 1,
> +		},
> +		{
> +			.rx_buf = hw->tb.rx_buf,
> +			.bits_per_word = 8,
> +			.len = len,
> +		}
> +	};
> +
> +	hw->tb.tx_buf[0] = addr | SENSORS_SPI_READ;
> +
> +	err = spi_sync_transfer(spi, xfers,  ARRAY_SIZE(xfers));
> +	if (err < 0)
> +		return err;
> +
> +	memcpy(data, hw->tb.rx_buf, len * sizeof(u8));
> +
> +	return len;
> +}
> +
> +static int st_lsm6dsx_spi_write(struct device *dev, u8 addr, int len,
> +				u8 *data)
> +{
> +	struct spi_device *spi = to_spi_device(dev);
> +	struct st_lsm6dsx_hw *hw = spi_get_drvdata(spi);
> +
> +	struct spi_transfer xfers = {
> +		.tx_buf = hw->tb.tx_buf,
> +		.bits_per_word = 8,
> +		.len = len + 1,
> +	};
> +
> +	if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
> +		return -ENOMEM;
> +
> +	hw->tb.tx_buf[0] = addr;
> +	memcpy(&hw->tb.tx_buf[1], data, len);
> +
> +	return spi_sync_transfer(spi, &xfers, 1);
> +}
> +
> +static const struct st_lsm6dsx_transfer_function st_lsm6dsx_transfer_fn = {
> +	.read = st_lsm6dsx_spi_read,
> +	.write = st_lsm6dsx_spi_write,
> +};
> +
> +static int st_lsm6dsx_spi_probe(struct spi_device *spi)
> +{
> +	struct st_lsm6dsx_hw *hw;
> +
> +	hw = devm_kzalloc(&spi->dev, sizeof(*hw), GFP_KERNEL);
> +	if (!hw)
> +		return -ENOMEM;
> +
> +	spi_set_drvdata(spi, hw);
> +	hw->name = spi->modalias;
> +	hw->dev = &spi->dev;
> +	hw->irq = spi->irq;
> +	hw->tf = &st_lsm6dsx_transfer_fn;
> +
> +	return st_lsm6dsx_probe(hw);
> +}
> +
> +static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
> +	{
> +		.compatible = "st,lsm6ds3",
> +		.data = ST_LSM6DS3_DEV_NAME,
> +	},
> +	{
> +		.compatible = "st,lsm6dsm",
> +		.data = ST_LSM6DSM_DEV_NAME,
> +	},
> +	{},
> +};
> +MODULE_DEVICE_TABLE(of, st_lsm6dsx_spi_of_match);
> +
> +static const struct spi_device_id st_lsm6dsx_spi_id_table[] = {
> +	{ ST_LSM6DS3_DEV_NAME },
> +	{ ST_LSM6DSM_DEV_NAME },
> +	{},
> +};
> +MODULE_DEVICE_TABLE(spi, st_lsm6dsx_spi_id_table);
> +
> +static struct spi_driver st_lsm6dsx_driver = {
> +	.driver = {
> +		.name = "st_lsm6dsx_spi",
> +		.of_match_table = of_match_ptr(st_lsm6dsx_spi_of_match),
> +	},
> +	.probe = st_lsm6dsx_spi_probe,
> +	.id_table = st_lsm6dsx_spi_id_table,
> +};
> +module_spi_driver(st_lsm6dsx_driver);
> +
> +MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi@xxxxxx>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@xxxxxx>");
> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver");
> +MODULE_LICENSE("GPL v2");
> 

-- 

Peter Meerwald-Stadler
+43-664-2444418 (mobile)
--
To unsubscribe from this list: send the line "unsubscribe devicetree" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html



[Index of Archives]     [Device Tree Compilter]     [Device Tree Spec]     [Linux Driver Backports]     [Video for Linux]     [Linux USB Devel]     [Linux PCI Devel]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [XFree86]     [Yosemite Backpacking]
  Powered by Linux