* [PATCH v4 0/2] add support to STM LSM6DS3-LSM6DSM 6-axis Mems sensor
@ 2017-01-10 21:55 ` Lorenzo Bianconi
0 siblings, 0 replies; 11+ messages in thread
From: Lorenzo Bianconi @ 2017-01-10 21:55 UTC (permalink / raw)
To: jic23-DgEjT+Ai2ygdnm+yROfE0A
Cc: linux-iio-u79uwXL29TY76Z2rM5mHXA,
devicetree-u79uwXL29TY76Z2rM5mHXA, lorenzo.bianconi-qxv4g6HH51o
Changes since v3:
- use I2C protocol instead of SMBus commands
- allow reading of the scale/sampling frequency whilst buffered operation is
on going
- move st_lsm6dsx_hw allocation/initialization in st_lsm6dsx_probe() in order
to avoid code duplication
- introduce enum st_lsm6dsx_hw_id to be used as I2C/SPI driver_data instead of
the device name (string)
- improve code documentation
Changes since v2:
- improve code documentation
- improve code readability
- use spi_write() instead of spi_sync_transfer() in st_lsm6dsx_spi_write()
- use SMBus commands instead of I2C protocol
- use fifo_lock mutex to prevent concurrent access to hw FIFO instead of
disabling/enabling irq line in st_lsm6dsx_flush_fifo()
- rename ring occurrences in buffer ones
Changes since v1:
- add sw fifo support
- drop trigger dependency
- use iio_claim_direct_mode() routine instead of grabbing the mutex directly
- use more unique prefix for all defines
- use info_mask_shared_by_all element for sampling_frequency
- use devm_iio_* routines
- use of_match_ptr instead of access directly to of_match_table
- fix device tree binding
- rename st_lsm6dsx_dev in st_lsm6dsx_hw
- cosmetics
Lorenzo Bianconi (2):
iio: imu: add support to lsm6dsx driver
Documentation: dt: iio: add st_lsm6dsx sensor device binding
.../devicetree/bindings/iio/imu/st_lsm6dsx.txt | 24 +
drivers/iio/imu/Kconfig | 1 +
drivers/iio/imu/Makefile | 2 +
drivers/iio/imu/st_lsm6dsx/Kconfig | 23 +
drivers/iio/imu/st_lsm6dsx/Makefile | 5 +
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 142 +++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 455 ++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 673 +++++++++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 101 ++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 118 ++++
10 files changed, 1544 insertions(+)
create mode 100644 Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
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_buffer.c
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_spi.c
--
2.9.3
--
To unsubscribe from this list: send the line "unsubscribe devicetree" in
the body of a message to majordomo-u79uwXL29TY76Z2rM5mHXA@public.gmane.org
More majordomo info at http://vger.kernel.org/majordomo-info.html
^ permalink raw reply [flat|nested] 11+ messages in thread
* [PATCH v4 0/2] add support to STM LSM6DS3-LSM6DSM 6-axis Mems sensor
@ 2017-01-10 21:55 ` Lorenzo Bianconi
0 siblings, 0 replies; 11+ messages in thread
From: Lorenzo Bianconi @ 2017-01-10 21:55 UTC (permalink / raw)
To: jic23; +Cc: linux-iio, devicetree, lorenzo.bianconi
Changes since v3:
- use I2C protocol instead of SMBus commands
- allow reading of the scale/sampling frequency whilst buffered operation is
on going
- move st_lsm6dsx_hw allocation/initialization in st_lsm6dsx_probe() in order
to avoid code duplication
- introduce enum st_lsm6dsx_hw_id to be used as I2C/SPI driver_data instead of
the device name (string)
- improve code documentation
Changes since v2:
- improve code documentation
- improve code readability
- use spi_write() instead of spi_sync_transfer() in st_lsm6dsx_spi_write()
- use SMBus commands instead of I2C protocol
- use fifo_lock mutex to prevent concurrent access to hw FIFO instead of
disabling/enabling irq line in st_lsm6dsx_flush_fifo()
- rename ring occurrences in buffer ones
Changes since v1:
- add sw fifo support
- drop trigger dependency
- use iio_claim_direct_mode() routine instead of grabbing the mutex directly
- use more unique prefix for all defines
- use info_mask_shared_by_all element for sampling_frequency
- use devm_iio_* routines
- use of_match_ptr instead of access directly to of_match_table
- fix device tree binding
- rename st_lsm6dsx_dev in st_lsm6dsx_hw
- cosmetics
Lorenzo Bianconi (2):
iio: imu: add support to lsm6dsx driver
Documentation: dt: iio: add st_lsm6dsx sensor device binding
.../devicetree/bindings/iio/imu/st_lsm6dsx.txt | 24 +
drivers/iio/imu/Kconfig | 1 +
drivers/iio/imu/Makefile | 2 +
drivers/iio/imu/st_lsm6dsx/Kconfig | 23 +
drivers/iio/imu/st_lsm6dsx/Makefile | 5 +
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 142 +++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 455 ++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 673 +++++++++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 101 ++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 118 ++++
10 files changed, 1544 insertions(+)
create mode 100644 Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
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_buffer.c
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_spi.c
--
2.9.3
^ permalink raw reply [flat|nested] 11+ messages in thread
* [PATCH v4 1/2] iio: imu: add support to lsm6dsx driver
2017-01-10 21:55 ` Lorenzo Bianconi
@ 2017-01-10 21:55 ` Lorenzo Bianconi
-1 siblings, 0 replies; 11+ messages in thread
From: Lorenzo Bianconi @ 2017-01-10 21:55 UTC (permalink / raw)
To: jic23-DgEjT+Ai2ygdnm+yROfE0A
Cc: linux-iio-u79uwXL29TY76Z2rM5mHXA,
devicetree-u79uwXL29TY76Z2rM5mHXA, lorenzo.bianconi-qxv4g6HH51o
Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor
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-qxv4g6HH51o@public.gmane.org>
---
drivers/iio/imu/Kconfig | 1 +
drivers/iio/imu/Makefile | 2 +
drivers/iio/imu/st_lsm6dsx/Kconfig | 23 +
drivers/iio/imu/st_lsm6dsx/Makefile | 5 +
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 142 ++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 455 +++++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 673 +++++++++++++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 101 ++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 118 +++++
9 files changed, 1520 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_buffer.c
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_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..2ebcb74
--- /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"
+ 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..35919fe
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/Makefile
@@ -0,0 +1,5 @@
+st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.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..16189ff
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -0,0 +1,142 @@
+/*
+ * STMicroelectronics st_lsm6dsx sensor driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
+ * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
+ *
+ * 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"
+
+enum st_lsm6dsx_hw_id {
+ ST_LSM6DS3_ID,
+ ST_LSM6DSM_ID,
+};
+
+#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_hw_id id;
+};
+
+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 - ST IMU sensor instance
+ * @id: Sensor identifier.
+ * @hw: Pointer to instance of struct st_lsm6dsx_hw.
+ * @gain: Configured sensor sensitivity.
+ * @odr: Output data rate of the sensor [Hz].
+ * @watermark: Sensor watermark level.
+ * @sip: Number of samples in a given pattern.
+ * @decimator: FIFO decimation factor.
+ * @decimator_mask: Sensor mask for decimation register.
+ * @delta_ts: Delta time between two consecutive interrupts.
+ * @ts: Latest timestamp from the interrupt handler.
+ */
+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 - ST IMU MEMS hw instance
+ * @dev: Pointer to instance of struct device (I2C or SPI).
+ * @irq: Device interrupt line (I2C or SPI).
+ * @lock: Mutex to protect read and write operations.
+ * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
+ * @fifo_mode: FIFO operating mode supported by the device.
+ * @enable_mask: Enabled sensor bitmask.
+ * @sip: Total number of samples (acc/gyro) in a given pattern.
+ * @iio_devs: Pointers to acc/gyro iio_dev instances.
+ * @settings: Pointer to the specific sensor settings in use.
+ * @tf: Transfer function structure used by I/O operations.
+ * @tb: Transfer buffers used by SPI I/O operations.
+ */
+struct st_lsm6dsx_hw {
+ struct device *dev;
+ int irq;
+
+ struct mutex lock;
+ struct mutex fifo_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 device *dev, int irq, int hw_id,
+ const struct st_lsm6dsx_transfer_function *tf_ops);
+int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
+int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
+int st_lsm6dsx_fifo_setup(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_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
new file mode 100644
index 0000000..a16d7c9
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -0,0 +1,455 @@
+/*
+ * STMicroelectronics st_lsm6dsx FIFO buffer library driver
+ *
+ * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data
+ * from gyroscope and accelerometer. Samples are queued without any tag
+ * according to a specific pattern based on 'FIFO data sets' (6 bytes each):
+ * - 1st data set is reserved for gyroscope data
+ * - 2nd data set is reserved for accelerometer data
+ * The FIFO pattern changes depending on the ODRs and decimation factors
+ * assigned to the FIFO data sets. The first sequence of data stored in FIFO
+ * buffer contains the data of all the enabled FIFO data sets
+ * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
+ * value of the decimation factor and ODR set for each FIFO data set.
+ * FIFO supported modes:
+ * - BYPASS: FIFO disabled
+ * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
+ * restarts from the beginning and the oldest sample is overwritten
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
+ * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
+ *
+ * Licensed under the GPL-2.
+ */
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.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 GENMASK(11, 0)
+#define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR 0x08
+#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
+#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
+#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
+#define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR 0x3a
+#define ST_LSM6DSX_FIFO_DIFF_MASK GENMASK(11, 0)
+#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
+#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
+
+#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
+
+struct st_lsm6dsx_decimator_entry {
+ u8 decimator;
+ u8 val;
+};
+
+static const
+struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_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)
+{
+ const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
+ int i;
+
+ for (i = 0; i < max_size; i++)
+ if (st_lsm6dsx_decimator_table[i].decimator == val)
+ break;
+
+ return i == max_size ? 0 : st_lsm6dsx_decimator_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 = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
+ __ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
+ 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;
+ __le16 wdata;
+ 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);
+
+ wdata = cpu_to_le16(fifo_watermark);
+ err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
+ sizeof(wdata), (u8 *)&wdata);
+out:
+ mutex_unlock(&hw->lock);
+
+ return err < 0 ? err : 0;
+}
+
+/**
+ * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
+ * @hw: Pointer to instance of struct st_lsm6dsx_hw.
+ *
+ * Read samples from the hw FIFO and push them to IIO buffers.
+ *
+ * Return: Number of bytes read from the FIFO
+ */
+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, samples, offset;
+ struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
+ s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
+ u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
+ u8 buff[pattern_len];
+ __le16 fifo_status;
+
+ err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
+ sizeof(fifo_status), (u8 *)&fifo_status);
+ if (err < 0)
+ return err;
+
+ if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
+ return 0;
+
+ fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
+ ST_LSM6DSX_CHAN_SIZE;
+ samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
+ fifo_len = (fifo_len / pattern_len) * pattern_len;
+
+ /*
+ * compute delta timestamp between two consecutive samples
+ * in order to estimate queueing time of data generated
+ * by the sensor
+ */
+ 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(buff), buff);
+ if (err < 0)
+ return err;
+
+ /*
+ * Data are written to the FIFO with a specific pattern
+ * depending on the configured ODRs. The first sequence of data
+ * stored in FIFO contains the data of all enabled sensors
+ * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
+ * depending on the value of the decimation factor set for each
+ * sensor.
+ *
+ * Supposing the FIFO is storing data from gyroscope and
+ * accelerometer at different ODRs:
+ * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
+ * Since the gyroscope ODR is twice the accelerometer one, the
+ * following pattern is repeated every 9 samples:
+ * - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
+ */
+ 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_buff, &buff[offset],
+ ST_LSM6DSX_SAMPLE_SIZE);
+ iio_push_to_buffers_with_timestamp(
+ hw->iio_devs[ST_LSM6DSX_ID_GYRO],
+ iio_buff, gyro_ts);
+ offset += ST_LSM6DSX_SAMPLE_SIZE;
+ gyro_ts += gyro_delta_ts;
+ }
+
+ if (acc_sip-- > 0) {
+ memcpy(iio_buff, &buff[offset],
+ ST_LSM6DSX_SAMPLE_SIZE);
+ iio_push_to_buffers_with_timestamp(
+ hw->iio_devs[ST_LSM6DSX_ID_ACC],
+ iio_buff, 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;
+
+ mutex_lock(&hw->fifo_lock);
+
+ st_lsm6dsx_read_fifo(hw);
+ err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
+
+ mutex_unlock(&hw->fifo_lock);
+
+ 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;
+ }
+
+ if (enable) {
+ err = st_lsm6dsx_sensor_enable(sensor);
+ if (err < 0)
+ return err;
+ } else {
+ err = 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;
+
+ /*
+ * store enable buffer timestamp as reference to compute
+ * first delta timestamp
+ */
+ sensor->ts = iio_get_time_ns(iio_dev);
+ }
+
+ return 0;
+}
+
+static irqreturn_t st_lsm6dsx_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_handler_thread(int irq, void *private)
+{
+ struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
+ int count;
+
+ mutex_lock(&hw->fifo_lock);
+ count = st_lsm6dsx_read_fifo(hw);
+ mutex_unlock(&hw->fifo_lock);
+
+ 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_fifo_setup(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\n", irq_type);
+ return -EINVAL;
+ }
+
+ err = devm_request_threaded_irq(hw->dev, hw->irq,
+ st_lsm6dsx_handler_irq,
+ st_lsm6dsx_handler_thread,
+ irq_type | IRQF_ONESHOT,
+ "lsm6dsx", 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_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
new file mode 100644
index 0000000..01e002c
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -0,0 +1,673 @@
+/*
+ * STMicroelectronics st_lsm6dsx sensor driver
+ *
+ * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
+ * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
+ * interface standard output.
+ * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
+ * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
+ * +-125/+-245/+-500/+-1000/+-2000 dps
+ * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
+ * allowing dynamic batching of sensor data.
+ *
+ * Supported sensors:
+ * - LSM6DS3:
+ * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
+ * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
+ * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
+ * - FIFO size: 8KB
+ *
+ * - LSM6DSM:
+ * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
+ * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
+ * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
+ * - FIFO size: 4KB
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
+ * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
+ *
+ * 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 "st_lsm6dsx.h"
+
+#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
+#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
+#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
+#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
+#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
+#define ST_LSM6DSX_REG_RESET_ADDR 0x12
+#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
+#define ST_LSM6DSX_REG_BDU_ADDR 0x12
+#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
+#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
+#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
+#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16
+#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
+#define ST_LSM6DSX_REG_LIR_ADDR 0x58
+#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
+
+#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10
+#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4)
+#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10
+#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2)
+#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 GENMASK(7, 4)
+#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11
+#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2)
+#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,
+ .id = ST_LSM6DS3_ID,
+ },
+ {
+ .wai = ST_LSM6DSM_WHOAMI,
+ .max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
+ .id = ST_LSM6DSM_ID,
+ },
+};
+
+#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
+{ \
+ .type = chan_type, \
+ .address = addr, \
+ .modified = 1, \
+ .channel2 = mod, \
+ .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 = scan_idx, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 16, \
+ .storagebits = 16, \
+ .endianness = IIO_LE, \
+ }, \
+}
+
+static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
+ ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
+ IIO_MOD_X, 0),
+ ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
+ IIO_MOD_Y, 1),
+ ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
+ IIO_MOD_Z, 2),
+ IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
+ ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
+ IIO_MOD_X, 0),
+ ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
+ IIO_MOD_Y, 1),
+ ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
+ IIO_MOD_Z, 2),
+ 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 id)
+{
+ int err, i;
+ u8 data;
+
+ for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
+ if (id == st_lsm6dsx_sensor_settings[i].id)
+ break;
+ }
+
+ if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
+ dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
+ return -ENODEV;
+ }
+
+ 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;
+ }
+
+ if (data != st_lsm6dsx_sensor_settings[i].wai) {
+ dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
+ return -ENODEV;
+ }
+
+ hw->settings = &st_lsm6dsx_sensor_settings[i];
+
+ return 0;
+}
+
+static int st_lsm6dsx_set_full_scale(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;
+ u8 val;
+
+ 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;
+ __le16 data;
+
+ 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),
+ (u8 *)&data);
+ if (err < 0)
+ return err;
+
+ st_lsm6dsx_sensor_disable(sensor);
+
+ *val = (s16)data;
+
+ 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;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_RAW:
+ ret = iio_device_claim_direct_mode(iio_dev);
+ if (ret)
+ break;
+
+ ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
+ iio_device_release_direct_mode(iio_dev);
+ 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;
+ }
+
+ 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_full_scale(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_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_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_avail);
+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 Block Data Update */
+ 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 device *dev, int irq, int hw_id,
+ const struct st_lsm6dsx_transfer_function *tf_ops)
+{
+ struct st_lsm6dsx_hw *hw;
+ int i, err;
+
+ hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
+ if (!hw)
+ return -ENOMEM;
+
+ dev_set_drvdata(dev, (void *)hw);
+
+ mutex_init(&hw->lock);
+ mutex_init(&hw->fifo_lock);
+
+ hw->dev = dev;
+ hw->irq = irq;
+ hw->tf = tf_ops;
+
+ err = st_lsm6dsx_check_whoami(hw, hw_id);
+ 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_fifo_setup(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-qxv4g6HH51o@public.gmane.org>");
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>");
+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..ea30411
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
@@ -0,0 +1,101 @@
+/*
+ * STMicroelectronics st_lsm6dsx i2c driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
+ * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
+ *
+ * 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];
+
+ 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)
+{
+ return st_lsm6dsx_probe(&client->dev, client->irq,
+ (int)id->driver_data,
+ &st_lsm6dsx_transfer_fn);
+}
+
+static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
+ {
+ .compatible = "st,lsm6ds3",
+ .data = (void *)ST_LSM6DS3_ID,
+ },
+ {
+ .compatible = "st,lsm6dsm",
+ .data = (void *)ST_LSM6DSM_ID,
+ },
+ {},
+};
+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_LSM6DS3_ID },
+ { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
+ {},
+};
+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-qxv4g6HH51o@public.gmane.org>");
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>");
+MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver");
+MODULE_LICENSE("GPL v2");
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..fbe7247
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
@@ -0,0 +1,118 @@
+/*
+ * STMicroelectronics st_lsm6dsx spi driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
+ * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
+ *
+ * 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 BIT(7)
+
+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 st_lsm6dsx_hw *hw;
+ struct spi_device *spi;
+
+ if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
+ return -ENOMEM;
+
+ spi = to_spi_device(dev);
+ hw = spi_get_drvdata(spi);
+
+ hw->tb.tx_buf[0] = addr;
+ memcpy(&hw->tb.tx_buf[1], data, len);
+
+ return spi_write(spi, hw->tb.tx_buf, len + 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)
+{
+ const struct spi_device_id *id = spi_get_device_id(spi);
+
+ return st_lsm6dsx_probe(&spi->dev, spi->irq,
+ (int)id->driver_data,
+ &st_lsm6dsx_transfer_fn);
+}
+
+static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
+ {
+ .compatible = "st,lsm6ds3",
+ .data = (void *)ST_LSM6DS3_ID,
+ },
+ {
+ .compatible = "st,lsm6dsm",
+ .data = (void *)ST_LSM6DSM_ID,
+ },
+ {},
+};
+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_LSM6DS3_ID },
+ { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
+ {},
+};
+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-qxv4g6HH51o@public.gmane.org>");
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>");
+MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver");
+MODULE_LICENSE("GPL v2");
--
2.9.3
^ permalink raw reply related [flat|nested] 11+ messages in thread
* [PATCH v4 1/2] iio: imu: add support to lsm6dsx driver
@ 2017-01-10 21:55 ` Lorenzo Bianconi
0 siblings, 0 replies; 11+ messages in thread
From: Lorenzo Bianconi @ 2017-01-10 21:55 UTC (permalink / raw)
To: jic23; +Cc: linux-iio, devicetree, lorenzo.bianconi
Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor
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@st.com>
---
drivers/iio/imu/Kconfig | 1 +
drivers/iio/imu/Makefile | 2 +
drivers/iio/imu/st_lsm6dsx/Kconfig | 23 +
drivers/iio/imu/st_lsm6dsx/Makefile | 5 +
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 142 ++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 455 +++++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 673 +++++++++++++++++++++++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 101 ++++
drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 118 +++++
9 files changed, 1520 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_buffer.c
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_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..2ebcb74
--- /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"
+ 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..35919fe
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/Makefile
@@ -0,0 +1,5 @@
+st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.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..16189ff
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -0,0 +1,142 @@
+/*
+ * STMicroelectronics st_lsm6dsx sensor driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * 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"
+
+enum st_lsm6dsx_hw_id {
+ ST_LSM6DS3_ID,
+ ST_LSM6DSM_ID,
+};
+
+#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_hw_id id;
+};
+
+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 - ST IMU sensor instance
+ * @id: Sensor identifier.
+ * @hw: Pointer to instance of struct st_lsm6dsx_hw.
+ * @gain: Configured sensor sensitivity.
+ * @odr: Output data rate of the sensor [Hz].
+ * @watermark: Sensor watermark level.
+ * @sip: Number of samples in a given pattern.
+ * @decimator: FIFO decimation factor.
+ * @decimator_mask: Sensor mask for decimation register.
+ * @delta_ts: Delta time between two consecutive interrupts.
+ * @ts: Latest timestamp from the interrupt handler.
+ */
+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 - ST IMU MEMS hw instance
+ * @dev: Pointer to instance of struct device (I2C or SPI).
+ * @irq: Device interrupt line (I2C or SPI).
+ * @lock: Mutex to protect read and write operations.
+ * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
+ * @fifo_mode: FIFO operating mode supported by the device.
+ * @enable_mask: Enabled sensor bitmask.
+ * @sip: Total number of samples (acc/gyro) in a given pattern.
+ * @iio_devs: Pointers to acc/gyro iio_dev instances.
+ * @settings: Pointer to the specific sensor settings in use.
+ * @tf: Transfer function structure used by I/O operations.
+ * @tb: Transfer buffers used by SPI I/O operations.
+ */
+struct st_lsm6dsx_hw {
+ struct device *dev;
+ int irq;
+
+ struct mutex lock;
+ struct mutex fifo_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 device *dev, int irq, int hw_id,
+ const struct st_lsm6dsx_transfer_function *tf_ops);
+int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
+int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
+int st_lsm6dsx_fifo_setup(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_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
new file mode 100644
index 0000000..a16d7c9
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
@@ -0,0 +1,455 @@
+/*
+ * STMicroelectronics st_lsm6dsx FIFO buffer library driver
+ *
+ * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data
+ * from gyroscope and accelerometer. Samples are queued without any tag
+ * according to a specific pattern based on 'FIFO data sets' (6 bytes each):
+ * - 1st data set is reserved for gyroscope data
+ * - 2nd data set is reserved for accelerometer data
+ * The FIFO pattern changes depending on the ODRs and decimation factors
+ * assigned to the FIFO data sets. The first sequence of data stored in FIFO
+ * buffer contains the data of all the enabled FIFO data sets
+ * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
+ * value of the decimation factor and ODR set for each FIFO data set.
+ * FIFO supported modes:
+ * - BYPASS: FIFO disabled
+ * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
+ * restarts from the beginning and the oldest sample is overwritten
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.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 GENMASK(11, 0)
+#define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR 0x08
+#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
+#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
+#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
+#define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR 0x3a
+#define ST_LSM6DSX_FIFO_DIFF_MASK GENMASK(11, 0)
+#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
+#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
+
+#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
+
+struct st_lsm6dsx_decimator_entry {
+ u8 decimator;
+ u8 val;
+};
+
+static const
+struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_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)
+{
+ const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
+ int i;
+
+ for (i = 0; i < max_size; i++)
+ if (st_lsm6dsx_decimator_table[i].decimator == val)
+ break;
+
+ return i == max_size ? 0 : st_lsm6dsx_decimator_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 = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
+ __ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
+ 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;
+ __le16 wdata;
+ 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);
+
+ wdata = cpu_to_le16(fifo_watermark);
+ err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
+ sizeof(wdata), (u8 *)&wdata);
+out:
+ mutex_unlock(&hw->lock);
+
+ return err < 0 ? err : 0;
+}
+
+/**
+ * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
+ * @hw: Pointer to instance of struct st_lsm6dsx_hw.
+ *
+ * Read samples from the hw FIFO and push them to IIO buffers.
+ *
+ * Return: Number of bytes read from the FIFO
+ */
+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, samples, offset;
+ struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
+ s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
+ u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
+ u8 buff[pattern_len];
+ __le16 fifo_status;
+
+ err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
+ sizeof(fifo_status), (u8 *)&fifo_status);
+ if (err < 0)
+ return err;
+
+ if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
+ return 0;
+
+ fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
+ ST_LSM6DSX_CHAN_SIZE;
+ samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
+ fifo_len = (fifo_len / pattern_len) * pattern_len;
+
+ /*
+ * compute delta timestamp between two consecutive samples
+ * in order to estimate queueing time of data generated
+ * by the sensor
+ */
+ 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(buff), buff);
+ if (err < 0)
+ return err;
+
+ /*
+ * Data are written to the FIFO with a specific pattern
+ * depending on the configured ODRs. The first sequence of data
+ * stored in FIFO contains the data of all enabled sensors
+ * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
+ * depending on the value of the decimation factor set for each
+ * sensor.
+ *
+ * Supposing the FIFO is storing data from gyroscope and
+ * accelerometer at different ODRs:
+ * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
+ * Since the gyroscope ODR is twice the accelerometer one, the
+ * following pattern is repeated every 9 samples:
+ * - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
+ */
+ 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_buff, &buff[offset],
+ ST_LSM6DSX_SAMPLE_SIZE);
+ iio_push_to_buffers_with_timestamp(
+ hw->iio_devs[ST_LSM6DSX_ID_GYRO],
+ iio_buff, gyro_ts);
+ offset += ST_LSM6DSX_SAMPLE_SIZE;
+ gyro_ts += gyro_delta_ts;
+ }
+
+ if (acc_sip-- > 0) {
+ memcpy(iio_buff, &buff[offset],
+ ST_LSM6DSX_SAMPLE_SIZE);
+ iio_push_to_buffers_with_timestamp(
+ hw->iio_devs[ST_LSM6DSX_ID_ACC],
+ iio_buff, 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;
+
+ mutex_lock(&hw->fifo_lock);
+
+ st_lsm6dsx_read_fifo(hw);
+ err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
+
+ mutex_unlock(&hw->fifo_lock);
+
+ 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;
+ }
+
+ if (enable) {
+ err = st_lsm6dsx_sensor_enable(sensor);
+ if (err < 0)
+ return err;
+ } else {
+ err = 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;
+
+ /*
+ * store enable buffer timestamp as reference to compute
+ * first delta timestamp
+ */
+ sensor->ts = iio_get_time_ns(iio_dev);
+ }
+
+ return 0;
+}
+
+static irqreturn_t st_lsm6dsx_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_handler_thread(int irq, void *private)
+{
+ struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
+ int count;
+
+ mutex_lock(&hw->fifo_lock);
+ count = st_lsm6dsx_read_fifo(hw);
+ mutex_unlock(&hw->fifo_lock);
+
+ 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_fifo_setup(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\n", irq_type);
+ return -EINVAL;
+ }
+
+ err = devm_request_threaded_irq(hw->dev, hw->irq,
+ st_lsm6dsx_handler_irq,
+ st_lsm6dsx_handler_thread,
+ irq_type | IRQF_ONESHOT,
+ "lsm6dsx", 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_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
new file mode 100644
index 0000000..01e002c
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -0,0 +1,673 @@
+/*
+ * STMicroelectronics st_lsm6dsx sensor driver
+ *
+ * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
+ * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
+ * interface standard output.
+ * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
+ * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
+ * +-125/+-245/+-500/+-1000/+-2000 dps
+ * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
+ * allowing dynamic batching of sensor data.
+ *
+ * Supported sensors:
+ * - LSM6DS3:
+ * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
+ * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
+ * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
+ * - FIFO size: 8KB
+ *
+ * - LSM6DSM:
+ * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
+ * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
+ * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
+ * - FIFO size: 4KB
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * 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 "st_lsm6dsx.h"
+
+#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
+#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
+#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
+#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
+#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
+#define ST_LSM6DSX_REG_RESET_ADDR 0x12
+#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
+#define ST_LSM6DSX_REG_BDU_ADDR 0x12
+#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
+#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
+#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
+#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16
+#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
+#define ST_LSM6DSX_REG_LIR_ADDR 0x58
+#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
+
+#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10
+#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4)
+#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10
+#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2)
+#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 GENMASK(7, 4)
+#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11
+#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2)
+#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,
+ .id = ST_LSM6DS3_ID,
+ },
+ {
+ .wai = ST_LSM6DSM_WHOAMI,
+ .max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
+ .id = ST_LSM6DSM_ID,
+ },
+};
+
+#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
+{ \
+ .type = chan_type, \
+ .address = addr, \
+ .modified = 1, \
+ .channel2 = mod, \
+ .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 = scan_idx, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 16, \
+ .storagebits = 16, \
+ .endianness = IIO_LE, \
+ }, \
+}
+
+static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
+ ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
+ IIO_MOD_X, 0),
+ ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
+ IIO_MOD_Y, 1),
+ ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
+ IIO_MOD_Z, 2),
+ IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
+ ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
+ IIO_MOD_X, 0),
+ ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
+ IIO_MOD_Y, 1),
+ ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
+ IIO_MOD_Z, 2),
+ 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 id)
+{
+ int err, i;
+ u8 data;
+
+ for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
+ if (id == st_lsm6dsx_sensor_settings[i].id)
+ break;
+ }
+
+ if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
+ dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
+ return -ENODEV;
+ }
+
+ 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;
+ }
+
+ if (data != st_lsm6dsx_sensor_settings[i].wai) {
+ dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
+ return -ENODEV;
+ }
+
+ hw->settings = &st_lsm6dsx_sensor_settings[i];
+
+ return 0;
+}
+
+static int st_lsm6dsx_set_full_scale(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;
+ u8 val;
+
+ 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;
+ __le16 data;
+
+ 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),
+ (u8 *)&data);
+ if (err < 0)
+ return err;
+
+ st_lsm6dsx_sensor_disable(sensor);
+
+ *val = (s16)data;
+
+ 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;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_RAW:
+ ret = iio_device_claim_direct_mode(iio_dev);
+ if (ret)
+ break;
+
+ ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
+ iio_device_release_direct_mode(iio_dev);
+ 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;
+ }
+
+ 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_full_scale(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_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_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_avail);
+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 Block Data Update */
+ 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 device *dev, int irq, int hw_id,
+ const struct st_lsm6dsx_transfer_function *tf_ops)
+{
+ struct st_lsm6dsx_hw *hw;
+ int i, err;
+
+ hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
+ if (!hw)
+ return -ENOMEM;
+
+ dev_set_drvdata(dev, (void *)hw);
+
+ mutex_init(&hw->lock);
+ mutex_init(&hw->fifo_lock);
+
+ hw->dev = dev;
+ hw->irq = irq;
+ hw->tf = tf_ops;
+
+ err = st_lsm6dsx_check_whoami(hw, hw_id);
+ 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_fifo_setup(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@st.com>");
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+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..ea30411
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
@@ -0,0 +1,101 @@
+/*
+ * STMicroelectronics st_lsm6dsx i2c driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * 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];
+
+ 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)
+{
+ return st_lsm6dsx_probe(&client->dev, client->irq,
+ (int)id->driver_data,
+ &st_lsm6dsx_transfer_fn);
+}
+
+static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
+ {
+ .compatible = "st,lsm6ds3",
+ .data = (void *)ST_LSM6DS3_ID,
+ },
+ {
+ .compatible = "st,lsm6dsm",
+ .data = (void *)ST_LSM6DSM_ID,
+ },
+ {},
+};
+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_LSM6DS3_ID },
+ { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
+ {},
+};
+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@st.com>");
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver");
+MODULE_LICENSE("GPL v2");
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..fbe7247
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
@@ -0,0 +1,118 @@
+/*
+ * STMicroelectronics st_lsm6dsx spi driver
+ *
+ * Copyright 2016 STMicroelectronics Inc.
+ *
+ * Lorenzo Bianconi <lorenzo.bianconi@st.com>
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * 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 BIT(7)
+
+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 st_lsm6dsx_hw *hw;
+ struct spi_device *spi;
+
+ if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
+ return -ENOMEM;
+
+ spi = to_spi_device(dev);
+ hw = spi_get_drvdata(spi);
+
+ hw->tb.tx_buf[0] = addr;
+ memcpy(&hw->tb.tx_buf[1], data, len);
+
+ return spi_write(spi, hw->tb.tx_buf, len + 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)
+{
+ const struct spi_device_id *id = spi_get_device_id(spi);
+
+ return st_lsm6dsx_probe(&spi->dev, spi->irq,
+ (int)id->driver_data,
+ &st_lsm6dsx_transfer_fn);
+}
+
+static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
+ {
+ .compatible = "st,lsm6ds3",
+ .data = (void *)ST_LSM6DS3_ID,
+ },
+ {
+ .compatible = "st,lsm6dsm",
+ .data = (void *)ST_LSM6DSM_ID,
+ },
+ {},
+};
+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_LSM6DS3_ID },
+ { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
+ {},
+};
+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@st.com>");
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver");
+MODULE_LICENSE("GPL v2");
--
2.9.3
^ permalink raw reply related [flat|nested] 11+ messages in thread
* [PATCH v4 2/2] Documentation: dt: iio: add st_lsm6dsx sensor device binding
2017-01-10 21:55 ` Lorenzo Bianconi
@ 2017-01-10 21:55 ` Lorenzo Bianconi
-1 siblings, 0 replies; 11+ messages in thread
From: Lorenzo Bianconi @ 2017-01-10 21:55 UTC (permalink / raw)
To: jic23-DgEjT+Ai2ygdnm+yROfE0A
Cc: linux-iio-u79uwXL29TY76Z2rM5mHXA,
devicetree-u79uwXL29TY76Z2rM5mHXA, lorenzo.bianconi-qxv4g6HH51o
Acked-by: Rob Herring <robh-DgEjT+Ai2ygdnm+yROfE0A@public.gmane.org>
Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
---
.../devicetree/bindings/iio/imu/st_lsm6dsx.txt | 24 ++++++++++++++++++++++
1 file changed, 24 insertions(+)
create mode 100644 Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
diff --git a/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
new file mode 100644
index 0000000..ed3cdac
--- /dev/null
+++ b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
@@ -0,0 +1,24 @@
+* ST_LSM6DSx driver for STM 6-axis (acc + gyro) imu Mems sensors
+
+Required properties:
+- compatible: must be one of:
+ "st,lsm6ds3"
+ "st,lsm6dsm"
+- reg: i2c address of the sensor / spi cs line
+
+Optional properties:
+- interrupt-parent: should be the phandle for the interrupt controller
+- interrupts: interrupt mapping for IRQ. It should be configured with
+ flags IRQ_TYPE_LEVEL_HIGH or IRQ_TYPE_EDGE_RISING.
+
+ Refer to interrupt-controller/interrupts.txt for generic interrupt
+ client node bindings.
+
+Example:
+
+lsm6dsm@6b {
+ compatible = "st,lsm6dsm";
+ reg = <0x6b>;
+ interrupt-parent = <&gpio0>;
+ interrupts = <0 IRQ_TYPE_EDGE_RISING>;
+};
--
2.9.3
^ permalink raw reply related [flat|nested] 11+ messages in thread
* [PATCH v4 2/2] Documentation: dt: iio: add st_lsm6dsx sensor device binding
@ 2017-01-10 21:55 ` Lorenzo Bianconi
0 siblings, 0 replies; 11+ messages in thread
From: Lorenzo Bianconi @ 2017-01-10 21:55 UTC (permalink / raw)
To: jic23; +Cc: linux-iio, devicetree, lorenzo.bianconi
Acked-by: Rob Herring <robh@kernel.org>
Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@st.com>
---
.../devicetree/bindings/iio/imu/st_lsm6dsx.txt | 24 ++++++++++++++++++++++
1 file changed, 24 insertions(+)
create mode 100644 Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
diff --git a/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
new file mode 100644
index 0000000..ed3cdac
--- /dev/null
+++ b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
@@ -0,0 +1,24 @@
+* ST_LSM6DSx driver for STM 6-axis (acc + gyro) imu Mems sensors
+
+Required properties:
+- compatible: must be one of:
+ "st,lsm6ds3"
+ "st,lsm6dsm"
+- reg: i2c address of the sensor / spi cs line
+
+Optional properties:
+- interrupt-parent: should be the phandle for the interrupt controller
+- interrupts: interrupt mapping for IRQ. It should be configured with
+ flags IRQ_TYPE_LEVEL_HIGH or IRQ_TYPE_EDGE_RISING.
+
+ Refer to interrupt-controller/interrupts.txt for generic interrupt
+ client node bindings.
+
+Example:
+
+lsm6dsm@6b {
+ compatible = "st,lsm6dsm";
+ reg = <0x6b>;
+ interrupt-parent = <&gpio0>;
+ interrupts = <0 IRQ_TYPE_EDGE_RISING>;
+};
--
2.9.3
^ permalink raw reply related [flat|nested] 11+ messages in thread
* Re: [PATCH v4 1/2] iio: imu: add support to lsm6dsx driver
2017-01-10 21:55 ` Lorenzo Bianconi
@ 2017-01-14 12:41 ` Jonathan Cameron
-1 siblings, 0 replies; 11+ messages in thread
From: Jonathan Cameron @ 2017-01-14 12:41 UTC (permalink / raw)
To: Lorenzo Bianconi
Cc: linux-iio-u79uwXL29TY76Z2rM5mHXA,
devicetree-u79uwXL29TY76Z2rM5mHXA, lorenzo.bianconi-qxv4g6HH51o
On 10/01/17 21:55, Lorenzo Bianconi wrote:
> Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor
>
> 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-qxv4g6HH51o@public.gmane.org>
A nice driver, for a complex device. Good work!
Applied to the togreg branch of iio.git - pushed out as testing for the
autobuilders to play with it.
Please watch out for double newlines at the end of files though.
I've fixed up when applying.
Thanks,
Jonathan
> ---
> drivers/iio/imu/Kconfig | 1 +
> drivers/iio/imu/Makefile | 2 +
> drivers/iio/imu/st_lsm6dsx/Kconfig | 23 +
> drivers/iio/imu/st_lsm6dsx/Makefile | 5 +
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 142 ++++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 455 +++++++++++++++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 673 +++++++++++++++++++++++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 101 ++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 118 +++++
> 9 files changed, 1520 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_buffer.c
> 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_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..2ebcb74
> --- /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"
> + 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..35919fe
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/Makefile
> @@ -0,0 +1,5 @@
> +st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.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..16189ff
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -0,0 +1,142 @@
> +/*
> + * STMicroelectronics st_lsm6dsx sensor driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
> + * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
> + *
> + * 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"
> +
> +enum st_lsm6dsx_hw_id {
> + ST_LSM6DS3_ID,
> + ST_LSM6DSM_ID,
> +};
> +
> +#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_hw_id id;
> +};
> +
> +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 - ST IMU sensor instance
> + * @id: Sensor identifier.
> + * @hw: Pointer to instance of struct st_lsm6dsx_hw.
> + * @gain: Configured sensor sensitivity.
> + * @odr: Output data rate of the sensor [Hz].
> + * @watermark: Sensor watermark level.
> + * @sip: Number of samples in a given pattern.
> + * @decimator: FIFO decimation factor.
> + * @decimator_mask: Sensor mask for decimation register.
> + * @delta_ts: Delta time between two consecutive interrupts.
> + * @ts: Latest timestamp from the interrupt handler.
> + */
> +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 - ST IMU MEMS hw instance
> + * @dev: Pointer to instance of struct device (I2C or SPI).
> + * @irq: Device interrupt line (I2C or SPI).
> + * @lock: Mutex to protect read and write operations.
> + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
> + * @fifo_mode: FIFO operating mode supported by the device.
> + * @enable_mask: Enabled sensor bitmask.
> + * @sip: Total number of samples (acc/gyro) in a given pattern.
> + * @iio_devs: Pointers to acc/gyro iio_dev instances.
> + * @settings: Pointer to the specific sensor settings in use.
> + * @tf: Transfer function structure used by I/O operations.
> + * @tb: Transfer buffers used by SPI I/O operations.
> + */
> +struct st_lsm6dsx_hw {
> + struct device *dev;
> + int irq;
> +
> + struct mutex lock;
> + struct mutex fifo_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 device *dev, int irq, int hw_id,
> + const struct st_lsm6dsx_transfer_function *tf_ops);
> +int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
> +int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
> +int st_lsm6dsx_fifo_setup(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_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> new file mode 100644
> index 0000000..a16d7c9
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> @@ -0,0 +1,455 @@
> +/*
> + * STMicroelectronics st_lsm6dsx FIFO buffer library driver
> + *
> + * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data
> + * from gyroscope and accelerometer. Samples are queued without any tag
> + * according to a specific pattern based on 'FIFO data sets' (6 bytes each):
> + * - 1st data set is reserved for gyroscope data
> + * - 2nd data set is reserved for accelerometer data
> + * The FIFO pattern changes depending on the ODRs and decimation factors
> + * assigned to the FIFO data sets. The first sequence of data stored in FIFO
> + * buffer contains the data of all the enabled FIFO data sets
> + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
> + * value of the decimation factor and ODR set for each FIFO data set.
> + * FIFO supported modes:
> + * - BYPASS: FIFO disabled
> + * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
> + * restarts from the beginning and the oldest sample is overwritten
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
> + * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
> + *
> + * Licensed under the GPL-2.
> + */
> +#include <linux/module.h>
> +#include <linux/interrupt.h>
> +#include <linux/irq.h>
> +#include <linux/iio/kfifo_buf.h>
> +#include <linux/iio/iio.h>
> +#include <linux/iio/buffer.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 GENMASK(11, 0)
> +#define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR 0x08
> +#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
> +#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
> +#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
> +#define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR 0x3a
> +#define ST_LSM6DSX_FIFO_DIFF_MASK GENMASK(11, 0)
> +#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
> +#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
> +
> +#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
> +
> +struct st_lsm6dsx_decimator_entry {
> + u8 decimator;
> + u8 val;
> +};
> +
> +static const
> +struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_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)
> +{
> + const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
> + int i;
> +
> + for (i = 0; i < max_size; i++)
> + if (st_lsm6dsx_decimator_table[i].decimator == val)
> + break;
> +
> + return i == max_size ? 0 : st_lsm6dsx_decimator_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 = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
> + __ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
> + 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;
> + __le16 wdata;
> + 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);
> +
> + wdata = cpu_to_le16(fifo_watermark);
> + err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
> + sizeof(wdata), (u8 *)&wdata);
> +out:
> + mutex_unlock(&hw->lock);
> +
> + return err < 0 ? err : 0;
> +}
> +
> +/**
> + * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
> + * @hw: Pointer to instance of struct st_lsm6dsx_hw.
> + *
> + * Read samples from the hw FIFO and push them to IIO buffers.
> + *
> + * Return: Number of bytes read from the FIFO
> + */
> +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, samples, offset;
> + struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
> + s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
> + u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
> + u8 buff[pattern_len];
> + __le16 fifo_status;
> +
> + err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
> + sizeof(fifo_status), (u8 *)&fifo_status);
> + if (err < 0)
> + return err;
> +
> + if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
> + return 0;
> +
> + fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
> + ST_LSM6DSX_CHAN_SIZE;
> + samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
> + fifo_len = (fifo_len / pattern_len) * pattern_len;
> +
> + /*
> + * compute delta timestamp between two consecutive samples
> + * in order to estimate queueing time of data generated
> + * by the sensor
> + */
> + 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(buff), buff);
> + if (err < 0)
> + return err;
> +
> + /*
> + * Data are written to the FIFO with a specific pattern
> + * depending on the configured ODRs. The first sequence of data
> + * stored in FIFO contains the data of all enabled sensors
> + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
> + * depending on the value of the decimation factor set for each
> + * sensor.
> + *
> + * Supposing the FIFO is storing data from gyroscope and
> + * accelerometer at different ODRs:
> + * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
> + * Since the gyroscope ODR is twice the accelerometer one, the
> + * following pattern is repeated every 9 samples:
> + * - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
> + */
> + 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_buff, &buff[offset],
> + ST_LSM6DSX_SAMPLE_SIZE);
> + iio_push_to_buffers_with_timestamp(
> + hw->iio_devs[ST_LSM6DSX_ID_GYRO],
> + iio_buff, gyro_ts);
> + offset += ST_LSM6DSX_SAMPLE_SIZE;
> + gyro_ts += gyro_delta_ts;
> + }
> +
> + if (acc_sip-- > 0) {
> + memcpy(iio_buff, &buff[offset],
> + ST_LSM6DSX_SAMPLE_SIZE);
> + iio_push_to_buffers_with_timestamp(
> + hw->iio_devs[ST_LSM6DSX_ID_ACC],
> + iio_buff, 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;
> +
> + mutex_lock(&hw->fifo_lock);
> +
> + st_lsm6dsx_read_fifo(hw);
> + err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
> +
> + mutex_unlock(&hw->fifo_lock);
> +
> + 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;
> + }
> +
> + if (enable) {
> + err = st_lsm6dsx_sensor_enable(sensor);
> + if (err < 0)
> + return err;
> + } else {
> + err = 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;
> +
> + /*
> + * store enable buffer timestamp as reference to compute
> + * first delta timestamp
> + */
> + sensor->ts = iio_get_time_ns(iio_dev);
> + }
> +
> + return 0;
> +}
> +
> +static irqreturn_t st_lsm6dsx_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_handler_thread(int irq, void *private)
> +{
> + struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
> + int count;
> +
> + mutex_lock(&hw->fifo_lock);
> + count = st_lsm6dsx_read_fifo(hw);
> + mutex_unlock(&hw->fifo_lock);
> +
> + 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_fifo_setup(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\n", irq_type);
> + return -EINVAL;
> + }
> +
> + err = devm_request_threaded_irq(hw->dev, hw->irq,
> + st_lsm6dsx_handler_irq,
> + st_lsm6dsx_handler_thread,
> + irq_type | IRQF_ONESHOT,
> + "lsm6dsx", 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_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> new file mode 100644
> index 0000000..01e002c
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> @@ -0,0 +1,673 @@
> +/*
> + * STMicroelectronics st_lsm6dsx sensor driver
> + *
> + * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
> + * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
> + * interface standard output.
> + * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
> + * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
> + * +-125/+-245/+-500/+-1000/+-2000 dps
> + * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
> + * allowing dynamic batching of sensor data.
> + *
> + * Supported sensors:
> + * - LSM6DS3:
> + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
> + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
> + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
> + * - FIFO size: 8KB
> + *
> + * - LSM6DSM:
> + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
> + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
> + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
> + * - FIFO size: 4KB
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
> + * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
> + *
> + * 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 "st_lsm6dsx.h"
> +
> +#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
> +#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
> +#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
> +#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
> +#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
> +#define ST_LSM6DSX_REG_RESET_ADDR 0x12
> +#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
> +#define ST_LSM6DSX_REG_BDU_ADDR 0x12
> +#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
> +#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
> +#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
> +#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16
> +#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
> +#define ST_LSM6DSX_REG_LIR_ADDR 0x58
> +#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
> +
> +#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10
> +#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4)
> +#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10
> +#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2)
> +#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 GENMASK(7, 4)
> +#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11
> +#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2)
> +#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,
> + .id = ST_LSM6DS3_ID,
> + },
> + {
> + .wai = ST_LSM6DSM_WHOAMI,
> + .max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
> + .id = ST_LSM6DSM_ID,
> + },
> +};
> +
> +#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
> +{ \
> + .type = chan_type, \
> + .address = addr, \
> + .modified = 1, \
> + .channel2 = mod, \
> + .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 = scan_idx, \
> + .scan_type = { \
> + .sign = 's', \
> + .realbits = 16, \
> + .storagebits = 16, \
> + .endianness = IIO_LE, \
> + }, \
> +}
> +
> +static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
> + IIO_MOD_X, 0),
> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
> + IIO_MOD_Y, 1),
> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
> + IIO_MOD_Z, 2),
> + IIO_CHAN_SOFT_TIMESTAMP(3),
> +};
> +
> +static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
> + IIO_MOD_X, 0),
> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
> + IIO_MOD_Y, 1),
> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
> + IIO_MOD_Z, 2),
> + 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 id)
> +{
> + int err, i;
> + u8 data;
> +
> + for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
> + if (id == st_lsm6dsx_sensor_settings[i].id)
> + break;
> + }
> +
> + if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
> + dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
> + return -ENODEV;
> + }
> +
> + 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;
> + }
> +
> + if (data != st_lsm6dsx_sensor_settings[i].wai) {
> + dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
> + return -ENODEV;
> + }
> +
> + hw->settings = &st_lsm6dsx_sensor_settings[i];
> +
> + return 0;
> +}
> +
> +static int st_lsm6dsx_set_full_scale(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;
> + u8 val;
> +
> + 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;
> + __le16 data;
> +
> + 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),
> + (u8 *)&data);
> + if (err < 0)
> + return err;
> +
> + st_lsm6dsx_sensor_disable(sensor);
> +
> + *val = (s16)data;
> +
> + 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;
> +
> + switch (mask) {
> + case IIO_CHAN_INFO_RAW:
> + ret = iio_device_claim_direct_mode(iio_dev);
> + if (ret)
> + break;
> +
> + ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
> + iio_device_release_direct_mode(iio_dev);
> + 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;
> + }
> +
> + 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_full_scale(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_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_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_avail);
> +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 Block Data Update */
> + 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 device *dev, int irq, int hw_id,
> + const struct st_lsm6dsx_transfer_function *tf_ops)
> +{
> + struct st_lsm6dsx_hw *hw;
> + int i, err;
> +
> + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
> + if (!hw)
> + return -ENOMEM;
> +
> + dev_set_drvdata(dev, (void *)hw);
> +
> + mutex_init(&hw->lock);
> + mutex_init(&hw->fifo_lock);
> +
> + hw->dev = dev;
> + hw->irq = irq;
> + hw->tf = tf_ops;
> +
> + err = st_lsm6dsx_check_whoami(hw, hw_id);
> + 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_fifo_setup(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-qxv4g6HH51o@public.gmane.org>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>");
> +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..ea30411
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
> @@ -0,0 +1,101 @@
> +/*
> + * STMicroelectronics st_lsm6dsx i2c driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
> + * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
> + *
> + * 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];
> +
> + 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)
> +{
> + return st_lsm6dsx_probe(&client->dev, client->irq,
> + (int)id->driver_data,
> + &st_lsm6dsx_transfer_fn);
> +}
> +
> +static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
> + {
> + .compatible = "st,lsm6ds3",
> + .data = (void *)ST_LSM6DS3_ID,
> + },
> + {
> + .compatible = "st,lsm6dsm",
> + .data = (void *)ST_LSM6DSM_ID,
> + },
> + {},
> +};
> +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_LSM6DS3_ID },
> + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
> + {},
> +};
> +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-qxv4g6HH51o@public.gmane.org>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>");
> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver");
> +MODULE_LICENSE("GPL v2");
> 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..fbe7247
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
> @@ -0,0 +1,118 @@
> +/*
> + * STMicroelectronics st_lsm6dsx spi driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
> + * Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>
> + *
> + * 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 BIT(7)
> +
> +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 st_lsm6dsx_hw *hw;
> + struct spi_device *spi;
> +
> + if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
> + return -ENOMEM;
> +
> + spi = to_spi_device(dev);
> + hw = spi_get_drvdata(spi);
> +
> + hw->tb.tx_buf[0] = addr;
> + memcpy(&hw->tb.tx_buf[1], data, len);
> +
> + return spi_write(spi, hw->tb.tx_buf, len + 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)
> +{
> + const struct spi_device_id *id = spi_get_device_id(spi);
> +
> + return st_lsm6dsx_probe(&spi->dev, spi->irq,
> + (int)id->driver_data,
> + &st_lsm6dsx_transfer_fn);
> +}
> +
> +static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
> + {
> + .compatible = "st,lsm6ds3",
> + .data = (void *)ST_LSM6DS3_ID,
> + },
> + {
> + .compatible = "st,lsm6dsm",
> + .data = (void *)ST_LSM6DSM_ID,
> + },
> + {},
> +};
> +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_LSM6DS3_ID },
> + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
> + {},
> +};
> +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-qxv4g6HH51o@public.gmane.org>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca-qxv4g6HH51o@public.gmane.org>");
> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver");
> +MODULE_LICENSE("GPL v2");
>
^ permalink raw reply [flat|nested] 11+ messages in thread
* Re: [PATCH v4 1/2] iio: imu: add support to lsm6dsx driver
@ 2017-01-14 12:41 ` Jonathan Cameron
0 siblings, 0 replies; 11+ messages in thread
From: Jonathan Cameron @ 2017-01-14 12:41 UTC (permalink / raw)
To: Lorenzo Bianconi; +Cc: linux-iio, devicetree, lorenzo.bianconi
On 10/01/17 21:55, Lorenzo Bianconi wrote:
> Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor
>
> 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@st.com>
A nice driver, for a complex device. Good work!
Applied to the togreg branch of iio.git - pushed out as testing for the
autobuilders to play with it.
Please watch out for double newlines at the end of files though.
I've fixed up when applying.
Thanks,
Jonathan
> ---
> drivers/iio/imu/Kconfig | 1 +
> drivers/iio/imu/Makefile | 2 +
> drivers/iio/imu/st_lsm6dsx/Kconfig | 23 +
> drivers/iio/imu/st_lsm6dsx/Makefile | 5 +
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 142 ++++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 455 +++++++++++++++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 673 +++++++++++++++++++++++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 101 ++++
> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 118 +++++
> 9 files changed, 1520 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_buffer.c
> 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_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..2ebcb74
> --- /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"
> + 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..35919fe
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/Makefile
> @@ -0,0 +1,5 @@
> +st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.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..16189ff
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
> @@ -0,0 +1,142 @@
> +/*
> + * STMicroelectronics st_lsm6dsx sensor driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
> + * Denis Ciocca <denis.ciocca@st.com>
> + *
> + * 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"
> +
> +enum st_lsm6dsx_hw_id {
> + ST_LSM6DS3_ID,
> + ST_LSM6DSM_ID,
> +};
> +
> +#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_hw_id id;
> +};
> +
> +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 - ST IMU sensor instance
> + * @id: Sensor identifier.
> + * @hw: Pointer to instance of struct st_lsm6dsx_hw.
> + * @gain: Configured sensor sensitivity.
> + * @odr: Output data rate of the sensor [Hz].
> + * @watermark: Sensor watermark level.
> + * @sip: Number of samples in a given pattern.
> + * @decimator: FIFO decimation factor.
> + * @decimator_mask: Sensor mask for decimation register.
> + * @delta_ts: Delta time between two consecutive interrupts.
> + * @ts: Latest timestamp from the interrupt handler.
> + */
> +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 - ST IMU MEMS hw instance
> + * @dev: Pointer to instance of struct device (I2C or SPI).
> + * @irq: Device interrupt line (I2C or SPI).
> + * @lock: Mutex to protect read and write operations.
> + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
> + * @fifo_mode: FIFO operating mode supported by the device.
> + * @enable_mask: Enabled sensor bitmask.
> + * @sip: Total number of samples (acc/gyro) in a given pattern.
> + * @iio_devs: Pointers to acc/gyro iio_dev instances.
> + * @settings: Pointer to the specific sensor settings in use.
> + * @tf: Transfer function structure used by I/O operations.
> + * @tb: Transfer buffers used by SPI I/O operations.
> + */
> +struct st_lsm6dsx_hw {
> + struct device *dev;
> + int irq;
> +
> + struct mutex lock;
> + struct mutex fifo_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 device *dev, int irq, int hw_id,
> + const struct st_lsm6dsx_transfer_function *tf_ops);
> +int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
> +int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
> +int st_lsm6dsx_fifo_setup(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_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> new file mode 100644
> index 0000000..a16d7c9
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
> @@ -0,0 +1,455 @@
> +/*
> + * STMicroelectronics st_lsm6dsx FIFO buffer library driver
> + *
> + * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data
> + * from gyroscope and accelerometer. Samples are queued without any tag
> + * according to a specific pattern based on 'FIFO data sets' (6 bytes each):
> + * - 1st data set is reserved for gyroscope data
> + * - 2nd data set is reserved for accelerometer data
> + * The FIFO pattern changes depending on the ODRs and decimation factors
> + * assigned to the FIFO data sets. The first sequence of data stored in FIFO
> + * buffer contains the data of all the enabled FIFO data sets
> + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
> + * value of the decimation factor and ODR set for each FIFO data set.
> + * FIFO supported modes:
> + * - BYPASS: FIFO disabled
> + * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
> + * restarts from the beginning and the oldest sample is overwritten
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
> + * Denis Ciocca <denis.ciocca@st.com>
> + *
> + * Licensed under the GPL-2.
> + */
> +#include <linux/module.h>
> +#include <linux/interrupt.h>
> +#include <linux/irq.h>
> +#include <linux/iio/kfifo_buf.h>
> +#include <linux/iio/iio.h>
> +#include <linux/iio/buffer.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 GENMASK(11, 0)
> +#define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR 0x08
> +#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
> +#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
> +#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
> +#define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR 0x3a
> +#define ST_LSM6DSX_FIFO_DIFF_MASK GENMASK(11, 0)
> +#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
> +#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
> +
> +#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
> +
> +struct st_lsm6dsx_decimator_entry {
> + u8 decimator;
> + u8 val;
> +};
> +
> +static const
> +struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_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)
> +{
> + const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
> + int i;
> +
> + for (i = 0; i < max_size; i++)
> + if (st_lsm6dsx_decimator_table[i].decimator == val)
> + break;
> +
> + return i == max_size ? 0 : st_lsm6dsx_decimator_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 = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
> + __ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
> + 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;
> + __le16 wdata;
> + 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);
> +
> + wdata = cpu_to_le16(fifo_watermark);
> + err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
> + sizeof(wdata), (u8 *)&wdata);
> +out:
> + mutex_unlock(&hw->lock);
> +
> + return err < 0 ? err : 0;
> +}
> +
> +/**
> + * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
> + * @hw: Pointer to instance of struct st_lsm6dsx_hw.
> + *
> + * Read samples from the hw FIFO and push them to IIO buffers.
> + *
> + * Return: Number of bytes read from the FIFO
> + */
> +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, samples, offset;
> + struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
> + s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
> + u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
> + u8 buff[pattern_len];
> + __le16 fifo_status;
> +
> + err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
> + sizeof(fifo_status), (u8 *)&fifo_status);
> + if (err < 0)
> + return err;
> +
> + if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
> + return 0;
> +
> + fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
> + ST_LSM6DSX_CHAN_SIZE;
> + samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
> + fifo_len = (fifo_len / pattern_len) * pattern_len;
> +
> + /*
> + * compute delta timestamp between two consecutive samples
> + * in order to estimate queueing time of data generated
> + * by the sensor
> + */
> + 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(buff), buff);
> + if (err < 0)
> + return err;
> +
> + /*
> + * Data are written to the FIFO with a specific pattern
> + * depending on the configured ODRs. The first sequence of data
> + * stored in FIFO contains the data of all enabled sensors
> + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
> + * depending on the value of the decimation factor set for each
> + * sensor.
> + *
> + * Supposing the FIFO is storing data from gyroscope and
> + * accelerometer at different ODRs:
> + * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
> + * Since the gyroscope ODR is twice the accelerometer one, the
> + * following pattern is repeated every 9 samples:
> + * - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
> + */
> + 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_buff, &buff[offset],
> + ST_LSM6DSX_SAMPLE_SIZE);
> + iio_push_to_buffers_with_timestamp(
> + hw->iio_devs[ST_LSM6DSX_ID_GYRO],
> + iio_buff, gyro_ts);
> + offset += ST_LSM6DSX_SAMPLE_SIZE;
> + gyro_ts += gyro_delta_ts;
> + }
> +
> + if (acc_sip-- > 0) {
> + memcpy(iio_buff, &buff[offset],
> + ST_LSM6DSX_SAMPLE_SIZE);
> + iio_push_to_buffers_with_timestamp(
> + hw->iio_devs[ST_LSM6DSX_ID_ACC],
> + iio_buff, 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;
> +
> + mutex_lock(&hw->fifo_lock);
> +
> + st_lsm6dsx_read_fifo(hw);
> + err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
> +
> + mutex_unlock(&hw->fifo_lock);
> +
> + 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;
> + }
> +
> + if (enable) {
> + err = st_lsm6dsx_sensor_enable(sensor);
> + if (err < 0)
> + return err;
> + } else {
> + err = 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;
> +
> + /*
> + * store enable buffer timestamp as reference to compute
> + * first delta timestamp
> + */
> + sensor->ts = iio_get_time_ns(iio_dev);
> + }
> +
> + return 0;
> +}
> +
> +static irqreturn_t st_lsm6dsx_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_handler_thread(int irq, void *private)
> +{
> + struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
> + int count;
> +
> + mutex_lock(&hw->fifo_lock);
> + count = st_lsm6dsx_read_fifo(hw);
> + mutex_unlock(&hw->fifo_lock);
> +
> + 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_fifo_setup(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\n", irq_type);
> + return -EINVAL;
> + }
> +
> + err = devm_request_threaded_irq(hw->dev, hw->irq,
> + st_lsm6dsx_handler_irq,
> + st_lsm6dsx_handler_thread,
> + irq_type | IRQF_ONESHOT,
> + "lsm6dsx", 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_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> new file mode 100644
> index 0000000..01e002c
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
> @@ -0,0 +1,673 @@
> +/*
> + * STMicroelectronics st_lsm6dsx sensor driver
> + *
> + * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
> + * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
> + * interface standard output.
> + * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
> + * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
> + * +-125/+-245/+-500/+-1000/+-2000 dps
> + * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
> + * allowing dynamic batching of sensor data.
> + *
> + * Supported sensors:
> + * - LSM6DS3:
> + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
> + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
> + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
> + * - FIFO size: 8KB
> + *
> + * - LSM6DSM:
> + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
> + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
> + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
> + * - FIFO size: 4KB
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
> + * Denis Ciocca <denis.ciocca@st.com>
> + *
> + * 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 "st_lsm6dsx.h"
> +
> +#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
> +#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
> +#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
> +#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
> +#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
> +#define ST_LSM6DSX_REG_RESET_ADDR 0x12
> +#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
> +#define ST_LSM6DSX_REG_BDU_ADDR 0x12
> +#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
> +#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
> +#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
> +#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16
> +#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
> +#define ST_LSM6DSX_REG_LIR_ADDR 0x58
> +#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
> +
> +#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10
> +#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4)
> +#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10
> +#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2)
> +#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 GENMASK(7, 4)
> +#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11
> +#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2)
> +#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,
> + .id = ST_LSM6DS3_ID,
> + },
> + {
> + .wai = ST_LSM6DSM_WHOAMI,
> + .max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
> + .id = ST_LSM6DSM_ID,
> + },
> +};
> +
> +#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
> +{ \
> + .type = chan_type, \
> + .address = addr, \
> + .modified = 1, \
> + .channel2 = mod, \
> + .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 = scan_idx, \
> + .scan_type = { \
> + .sign = 's', \
> + .realbits = 16, \
> + .storagebits = 16, \
> + .endianness = IIO_LE, \
> + }, \
> +}
> +
> +static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
> + IIO_MOD_X, 0),
> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
> + IIO_MOD_Y, 1),
> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
> + IIO_MOD_Z, 2),
> + IIO_CHAN_SOFT_TIMESTAMP(3),
> +};
> +
> +static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
> + IIO_MOD_X, 0),
> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
> + IIO_MOD_Y, 1),
> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
> + IIO_MOD_Z, 2),
> + 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 id)
> +{
> + int err, i;
> + u8 data;
> +
> + for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
> + if (id == st_lsm6dsx_sensor_settings[i].id)
> + break;
> + }
> +
> + if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
> + dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
> + return -ENODEV;
> + }
> +
> + 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;
> + }
> +
> + if (data != st_lsm6dsx_sensor_settings[i].wai) {
> + dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
> + return -ENODEV;
> + }
> +
> + hw->settings = &st_lsm6dsx_sensor_settings[i];
> +
> + return 0;
> +}
> +
> +static int st_lsm6dsx_set_full_scale(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;
> + u8 val;
> +
> + 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;
> + __le16 data;
> +
> + 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),
> + (u8 *)&data);
> + if (err < 0)
> + return err;
> +
> + st_lsm6dsx_sensor_disable(sensor);
> +
> + *val = (s16)data;
> +
> + 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;
> +
> + switch (mask) {
> + case IIO_CHAN_INFO_RAW:
> + ret = iio_device_claim_direct_mode(iio_dev);
> + if (ret)
> + break;
> +
> + ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
> + iio_device_release_direct_mode(iio_dev);
> + 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;
> + }
> +
> + 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_full_scale(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_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_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_avail);
> +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 Block Data Update */
> + 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 device *dev, int irq, int hw_id,
> + const struct st_lsm6dsx_transfer_function *tf_ops)
> +{
> + struct st_lsm6dsx_hw *hw;
> + int i, err;
> +
> + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
> + if (!hw)
> + return -ENOMEM;
> +
> + dev_set_drvdata(dev, (void *)hw);
> +
> + mutex_init(&hw->lock);
> + mutex_init(&hw->fifo_lock);
> +
> + hw->dev = dev;
> + hw->irq = irq;
> + hw->tf = tf_ops;
> +
> + err = st_lsm6dsx_check_whoami(hw, hw_id);
> + 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_fifo_setup(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@st.com>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
> +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..ea30411
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
> @@ -0,0 +1,101 @@
> +/*
> + * STMicroelectronics st_lsm6dsx i2c driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
> + * Denis Ciocca <denis.ciocca@st.com>
> + *
> + * 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];
> +
> + 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)
> +{
> + return st_lsm6dsx_probe(&client->dev, client->irq,
> + (int)id->driver_data,
> + &st_lsm6dsx_transfer_fn);
> +}
> +
> +static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
> + {
> + .compatible = "st,lsm6ds3",
> + .data = (void *)ST_LSM6DS3_ID,
> + },
> + {
> + .compatible = "st,lsm6dsm",
> + .data = (void *)ST_LSM6DSM_ID,
> + },
> + {},
> +};
> +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_LSM6DS3_ID },
> + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
> + {},
> +};
> +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@st.com>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver");
> +MODULE_LICENSE("GPL v2");
> 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..fbe7247
> --- /dev/null
> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
> @@ -0,0 +1,118 @@
> +/*
> + * STMicroelectronics st_lsm6dsx spi driver
> + *
> + * Copyright 2016 STMicroelectronics Inc.
> + *
> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
> + * Denis Ciocca <denis.ciocca@st.com>
> + *
> + * 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 BIT(7)
> +
> +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 st_lsm6dsx_hw *hw;
> + struct spi_device *spi;
> +
> + if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
> + return -ENOMEM;
> +
> + spi = to_spi_device(dev);
> + hw = spi_get_drvdata(spi);
> +
> + hw->tb.tx_buf[0] = addr;
> + memcpy(&hw->tb.tx_buf[1], data, len);
> +
> + return spi_write(spi, hw->tb.tx_buf, len + 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)
> +{
> + const struct spi_device_id *id = spi_get_device_id(spi);
> +
> + return st_lsm6dsx_probe(&spi->dev, spi->irq,
> + (int)id->driver_data,
> + &st_lsm6dsx_transfer_fn);
> +}
> +
> +static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
> + {
> + .compatible = "st,lsm6ds3",
> + .data = (void *)ST_LSM6DS3_ID,
> + },
> + {
> + .compatible = "st,lsm6dsm",
> + .data = (void *)ST_LSM6DSM_ID,
> + },
> + {},
> +};
> +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_LSM6DS3_ID },
> + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
> + {},
> +};
> +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@st.com>");
> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver");
> +MODULE_LICENSE("GPL v2");
>
^ permalink raw reply [flat|nested] 11+ messages in thread
* Re: [PATCH v4 2/2] Documentation: dt: iio: add st_lsm6dsx sensor device binding
2017-01-10 21:55 ` Lorenzo Bianconi
@ 2017-01-14 12:42 ` Jonathan Cameron
-1 siblings, 0 replies; 11+ messages in thread
From: Jonathan Cameron @ 2017-01-14 12:42 UTC (permalink / raw)
To: Lorenzo Bianconi
Cc: linux-iio-u79uwXL29TY76Z2rM5mHXA,
devicetree-u79uwXL29TY76Z2rM5mHXA, lorenzo.bianconi-qxv4g6HH51o
On 10/01/17 21:55, Lorenzo Bianconi wrote:
> Acked-by: Rob Herring <robh-DgEjT+Ai2ygdnm+yROfE0A@public.gmane.org>
> Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
Applied.
Thanks,
Jonathan
> ---
> .../devicetree/bindings/iio/imu/st_lsm6dsx.txt | 24 ++++++++++++++++++++++
> 1 file changed, 24 insertions(+)
> create mode 100644 Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
>
> diff --git a/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
> new file mode 100644
> index 0000000..ed3cdac
> --- /dev/null
> +++ b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
> @@ -0,0 +1,24 @@
> +* ST_LSM6DSx driver for STM 6-axis (acc + gyro) imu Mems sensors
> +
> +Required properties:
> +- compatible: must be one of:
> + "st,lsm6ds3"
> + "st,lsm6dsm"
> +- reg: i2c address of the sensor / spi cs line
> +
> +Optional properties:
> +- interrupt-parent: should be the phandle for the interrupt controller
> +- interrupts: interrupt mapping for IRQ. It should be configured with
> + flags IRQ_TYPE_LEVEL_HIGH or IRQ_TYPE_EDGE_RISING.
> +
> + Refer to interrupt-controller/interrupts.txt for generic interrupt
> + client node bindings.
> +
> +Example:
> +
> +lsm6dsm@6b {
> + compatible = "st,lsm6dsm";
> + reg = <0x6b>;
> + interrupt-parent = <&gpio0>;
> + interrupts = <0 IRQ_TYPE_EDGE_RISING>;
> +};
>
^ permalink raw reply [flat|nested] 11+ messages in thread
* Re: [PATCH v4 2/2] Documentation: dt: iio: add st_lsm6dsx sensor device binding
@ 2017-01-14 12:42 ` Jonathan Cameron
0 siblings, 0 replies; 11+ messages in thread
From: Jonathan Cameron @ 2017-01-14 12:42 UTC (permalink / raw)
To: Lorenzo Bianconi; +Cc: linux-iio, devicetree, lorenzo.bianconi
On 10/01/17 21:55, Lorenzo Bianconi wrote:
> Acked-by: Rob Herring <robh@kernel.org>
> Signed-off-by: Lorenzo Bianconi <lorenzo.bianconi@st.com>
Applied.
Thanks,
Jonathan
> ---
> .../devicetree/bindings/iio/imu/st_lsm6dsx.txt | 24 ++++++++++++++++++++++
> 1 file changed, 24 insertions(+)
> create mode 100644 Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
>
> diff --git a/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
> new file mode 100644
> index 0000000..ed3cdac
> --- /dev/null
> +++ b/Documentation/devicetree/bindings/iio/imu/st_lsm6dsx.txt
> @@ -0,0 +1,24 @@
> +* ST_LSM6DSx driver for STM 6-axis (acc + gyro) imu Mems sensors
> +
> +Required properties:
> +- compatible: must be one of:
> + "st,lsm6ds3"
> + "st,lsm6dsm"
> +- reg: i2c address of the sensor / spi cs line
> +
> +Optional properties:
> +- interrupt-parent: should be the phandle for the interrupt controller
> +- interrupts: interrupt mapping for IRQ. It should be configured with
> + flags IRQ_TYPE_LEVEL_HIGH or IRQ_TYPE_EDGE_RISING.
> +
> + Refer to interrupt-controller/interrupts.txt for generic interrupt
> + client node bindings.
> +
> +Example:
> +
> +lsm6dsm@6b {
> + compatible = "st,lsm6dsm";
> + reg = <0x6b>;
> + interrupt-parent = <&gpio0>;
> + interrupts = <0 IRQ_TYPE_EDGE_RISING>;
> +};
>
^ permalink raw reply [flat|nested] 11+ messages in thread
* Re: [PATCH v4 1/2] iio: imu: add support to lsm6dsx driver
2017-01-14 12:41 ` Jonathan Cameron
(?)
@ 2017-01-14 13:18 ` Lorenzo Bianconi
-1 siblings, 0 replies; 11+ messages in thread
From: Lorenzo Bianconi @ 2017-01-14 13:18 UTC (permalink / raw)
To: Jonathan Cameron; +Cc: linux-iio, Lorenzo BIANCONI
> On 10/01/17 21:55, Lorenzo Bianconi wrote:
>> Add support to STM LSM6DS3-LSM6DSM 6-axis (acc + gyro) Mems sensor
>>
>> 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@st.com>
> A nice driver, for a complex device. Good work!
>
> Applied to the togreg branch of iio.git - pushed out as testing for the
> autobuilders to play with it.
>
> Please watch out for double newlines at the end of files though.
> I've fixed up when applying.
Ack
>
> Thanks,
>
> Jonathan
Thanks,
Lorenzo
>> ---
>> drivers/iio/imu/Kconfig | 1 +
>> drivers/iio/imu/Makefile | 2 +
>> drivers/iio/imu/st_lsm6dsx/Kconfig | 23 +
>> drivers/iio/imu/st_lsm6dsx/Makefile | 5 +
>> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h | 142 ++++++
>> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c | 455 +++++++++++++++++
>> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c | 673 +++++++++++++++++++++++++
>> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c | 101 ++++
>> drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c | 118 +++++
>> 9 files changed, 1520 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_buffer.c
>> 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_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..2ebcb74
>> --- /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"
>> + 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..35919fe
>> --- /dev/null
>> +++ b/drivers/iio/imu/st_lsm6dsx/Makefile
>> @@ -0,0 +1,5 @@
>> +st_lsm6dsx-y := st_lsm6dsx_core.o st_lsm6dsx_buffer.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..16189ff
>> --- /dev/null
>> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
>> @@ -0,0 +1,142 @@
>> +/*
>> + * STMicroelectronics st_lsm6dsx sensor driver
>> + *
>> + * Copyright 2016 STMicroelectronics Inc.
>> + *
>> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
>> + * Denis Ciocca <denis.ciocca@st.com>
>> + *
>> + * 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"
>> +
>> +enum st_lsm6dsx_hw_id {
>> + ST_LSM6DS3_ID,
>> + ST_LSM6DSM_ID,
>> +};
>> +
>> +#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_hw_id id;
>> +};
>> +
>> +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 - ST IMU sensor instance
>> + * @id: Sensor identifier.
>> + * @hw: Pointer to instance of struct st_lsm6dsx_hw.
>> + * @gain: Configured sensor sensitivity.
>> + * @odr: Output data rate of the sensor [Hz].
>> + * @watermark: Sensor watermark level.
>> + * @sip: Number of samples in a given pattern.
>> + * @decimator: FIFO decimation factor.
>> + * @decimator_mask: Sensor mask for decimation register.
>> + * @delta_ts: Delta time between two consecutive interrupts.
>> + * @ts: Latest timestamp from the interrupt handler.
>> + */
>> +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 - ST IMU MEMS hw instance
>> + * @dev: Pointer to instance of struct device (I2C or SPI).
>> + * @irq: Device interrupt line (I2C or SPI).
>> + * @lock: Mutex to protect read and write operations.
>> + * @fifo_lock: Mutex to prevent concurrent access to the hw FIFO.
>> + * @fifo_mode: FIFO operating mode supported by the device.
>> + * @enable_mask: Enabled sensor bitmask.
>> + * @sip: Total number of samples (acc/gyro) in a given pattern.
>> + * @iio_devs: Pointers to acc/gyro iio_dev instances.
>> + * @settings: Pointer to the specific sensor settings in use.
>> + * @tf: Transfer function structure used by I/O operations.
>> + * @tb: Transfer buffers used by SPI I/O operations.
>> + */
>> +struct st_lsm6dsx_hw {
>> + struct device *dev;
>> + int irq;
>> +
>> + struct mutex lock;
>> + struct mutex fifo_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 device *dev, int irq, int hw_id,
>> + const struct st_lsm6dsx_transfer_function *tf_ops);
>> +int st_lsm6dsx_sensor_enable(struct st_lsm6dsx_sensor *sensor);
>> +int st_lsm6dsx_sensor_disable(struct st_lsm6dsx_sensor *sensor);
>> +int st_lsm6dsx_fifo_setup(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_buffer.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
>> new file mode 100644
>> index 0000000..a16d7c9
>> --- /dev/null
>> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_buffer.c
>> @@ -0,0 +1,455 @@
>> +/*
>> + * STMicroelectronics st_lsm6dsx FIFO buffer library driver
>> + *
>> + * LSM6DS3/LSM6DSM: The FIFO buffer can be configured to store data
>> + * from gyroscope and accelerometer. Samples are queued without any tag
>> + * according to a specific pattern based on 'FIFO data sets' (6 bytes each):
>> + * - 1st data set is reserved for gyroscope data
>> + * - 2nd data set is reserved for accelerometer data
>> + * The FIFO pattern changes depending on the ODRs and decimation factors
>> + * assigned to the FIFO data sets. The first sequence of data stored in FIFO
>> + * buffer contains the data of all the enabled FIFO data sets
>> + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated depending on the
>> + * value of the decimation factor and ODR set for each FIFO data set.
>> + * FIFO supported modes:
>> + * - BYPASS: FIFO disabled
>> + * - CONTINUOUS: FIFO enabled. When the buffer is full, the FIFO index
>> + * restarts from the beginning and the oldest sample is overwritten
>> + *
>> + * Copyright 2016 STMicroelectronics Inc.
>> + *
>> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
>> + * Denis Ciocca <denis.ciocca@st.com>
>> + *
>> + * Licensed under the GPL-2.
>> + */
>> +#include <linux/module.h>
>> +#include <linux/interrupt.h>
>> +#include <linux/irq.h>
>> +#include <linux/iio/kfifo_buf.h>
>> +#include <linux/iio/iio.h>
>> +#include <linux/iio/buffer.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 GENMASK(11, 0)
>> +#define ST_LSM6DSX_REG_FIFO_DEC_GXL_ADDR 0x08
>> +#define ST_LSM6DSX_REG_FIFO_MODE_ADDR 0x0a
>> +#define ST_LSM6DSX_FIFO_MODE_MASK GENMASK(2, 0)
>> +#define ST_LSM6DSX_FIFO_ODR_MASK GENMASK(6, 3)
>> +#define ST_LSM6DSX_REG_FIFO_DIFFL_ADDR 0x3a
>> +#define ST_LSM6DSX_FIFO_DIFF_MASK GENMASK(11, 0)
>> +#define ST_LSM6DSX_FIFO_EMPTY_MASK BIT(12)
>> +#define ST_LSM6DSX_REG_FIFO_OUTL_ADDR 0x3e
>> +
>> +#define ST_LSM6DSX_MAX_FIFO_ODR_VAL 0x08
>> +
>> +struct st_lsm6dsx_decimator_entry {
>> + u8 decimator;
>> + u8 val;
>> +};
>> +
>> +static const
>> +struct st_lsm6dsx_decimator_entry st_lsm6dsx_decimator_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)
>> +{
>> + const int max_size = ARRAY_SIZE(st_lsm6dsx_decimator_table);
>> + int i;
>> +
>> + for (i = 0; i < max_size; i++)
>> + if (st_lsm6dsx_decimator_table[i].decimator == val)
>> + break;
>> +
>> + return i == max_size ? 0 : st_lsm6dsx_decimator_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 = (ST_LSM6DSX_MAX_FIFO_ODR_VAL <<
>> + __ffs(ST_LSM6DSX_FIFO_ODR_MASK)) | fifo_mode;
>> + 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;
>> + __le16 wdata;
>> + 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);
>> +
>> + wdata = cpu_to_le16(fifo_watermark);
>> + err = hw->tf->write(hw->dev, ST_LSM6DSX_REG_FIFO_THL_ADDR,
>> + sizeof(wdata), (u8 *)&wdata);
>> +out:
>> + mutex_unlock(&hw->lock);
>> +
>> + return err < 0 ? err : 0;
>> +}
>> +
>> +/**
>> + * st_lsm6dsx_read_fifo() - LSM6DS3-LSM6DSM read FIFO routine
>> + * @hw: Pointer to instance of struct st_lsm6dsx_hw.
>> + *
>> + * Read samples from the hw FIFO and push them to IIO buffers.
>> + *
>> + * Return: Number of bytes read from the FIFO
>> + */
>> +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, samples, offset;
>> + struct st_lsm6dsx_sensor *acc_sensor, *gyro_sensor;
>> + s64 acc_ts, acc_delta_ts, gyro_ts, gyro_delta_ts;
>> + u8 iio_buff[ALIGN(ST_LSM6DSX_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
>> + u8 buff[pattern_len];
>> + __le16 fifo_status;
>> +
>> + err = hw->tf->read(hw->dev, ST_LSM6DSX_REG_FIFO_DIFFL_ADDR,
>> + sizeof(fifo_status), (u8 *)&fifo_status);
>> + if (err < 0)
>> + return err;
>> +
>> + if (fifo_status & cpu_to_le16(ST_LSM6DSX_FIFO_EMPTY_MASK))
>> + return 0;
>> +
>> + fifo_len = (le16_to_cpu(fifo_status) & ST_LSM6DSX_FIFO_DIFF_MASK) *
>> + ST_LSM6DSX_CHAN_SIZE;
>> + samples = fifo_len / ST_LSM6DSX_SAMPLE_SIZE;
>> + fifo_len = (fifo_len / pattern_len) * pattern_len;
>> +
>> + /*
>> + * compute delta timestamp between two consecutive samples
>> + * in order to estimate queueing time of data generated
>> + * by the sensor
>> + */
>> + 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(buff), buff);
>> + if (err < 0)
>> + return err;
>> +
>> + /*
>> + * Data are written to the FIFO with a specific pattern
>> + * depending on the configured ODRs. The first sequence of data
>> + * stored in FIFO contains the data of all enabled sensors
>> + * (e.g. Gx, Gy, Gz, Ax, Ay, Az), then data are repeated
>> + * depending on the value of the decimation factor set for each
>> + * sensor.
>> + *
>> + * Supposing the FIFO is storing data from gyroscope and
>> + * accelerometer at different ODRs:
>> + * - gyroscope ODR = 208Hz, accelerometer ODR = 104Hz
>> + * Since the gyroscope ODR is twice the accelerometer one, the
>> + * following pattern is repeated every 9 samples:
>> + * - Gx, Gy, Gz, Ax, Ay, Az, Gx, Gy, Gz
>> + */
>> + 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_buff, &buff[offset],
>> + ST_LSM6DSX_SAMPLE_SIZE);
>> + iio_push_to_buffers_with_timestamp(
>> + hw->iio_devs[ST_LSM6DSX_ID_GYRO],
>> + iio_buff, gyro_ts);
>> + offset += ST_LSM6DSX_SAMPLE_SIZE;
>> + gyro_ts += gyro_delta_ts;
>> + }
>> +
>> + if (acc_sip-- > 0) {
>> + memcpy(iio_buff, &buff[offset],
>> + ST_LSM6DSX_SAMPLE_SIZE);
>> + iio_push_to_buffers_with_timestamp(
>> + hw->iio_devs[ST_LSM6DSX_ID_ACC],
>> + iio_buff, 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;
>> +
>> + mutex_lock(&hw->fifo_lock);
>> +
>> + st_lsm6dsx_read_fifo(hw);
>> + err = st_lsm6dsx_set_fifo_mode(hw, ST_LSM6DSX_FIFO_BYPASS);
>> +
>> + mutex_unlock(&hw->fifo_lock);
>> +
>> + 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;
>> + }
>> +
>> + if (enable) {
>> + err = st_lsm6dsx_sensor_enable(sensor);
>> + if (err < 0)
>> + return err;
>> + } else {
>> + err = 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;
>> +
>> + /*
>> + * store enable buffer timestamp as reference to compute
>> + * first delta timestamp
>> + */
>> + sensor->ts = iio_get_time_ns(iio_dev);
>> + }
>> +
>> + return 0;
>> +}
>> +
>> +static irqreturn_t st_lsm6dsx_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_handler_thread(int irq, void *private)
>> +{
>> + struct st_lsm6dsx_hw *hw = (struct st_lsm6dsx_hw *)private;
>> + int count;
>> +
>> + mutex_lock(&hw->fifo_lock);
>> + count = st_lsm6dsx_read_fifo(hw);
>> + mutex_unlock(&hw->fifo_lock);
>> +
>> + 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_fifo_setup(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\n", irq_type);
>> + return -EINVAL;
>> + }
>> +
>> + err = devm_request_threaded_irq(hw->dev, hw->irq,
>> + st_lsm6dsx_handler_irq,
>> + st_lsm6dsx_handler_thread,
>> + irq_type | IRQF_ONESHOT,
>> + "lsm6dsx", 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_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
>> new file mode 100644
>> index 0000000..01e002c
>> --- /dev/null
>> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
>> @@ -0,0 +1,673 @@
>> +/*
>> + * STMicroelectronics st_lsm6dsx sensor driver
>> + *
>> + * The ST LSM6DSx IMU MEMS series consists of 3D digital accelerometer
>> + * and 3D digital gyroscope system-in-package with a digital I2C/SPI serial
>> + * interface standard output.
>> + * LSM6DSx IMU MEMS series has a dynamic user-selectable full-scale
>> + * acceleration range of +-2/+-4/+-8/+-16 g and an angular rate range of
>> + * +-125/+-245/+-500/+-1000/+-2000 dps
>> + * LSM6DSx series has an integrated First-In-First-Out (FIFO) buffer
>> + * allowing dynamic batching of sensor data.
>> + *
>> + * Supported sensors:
>> + * - LSM6DS3:
>> + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
>> + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
>> + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
>> + * - FIFO size: 8KB
>> + *
>> + * - LSM6DSM:
>> + * - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
>> + * - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
>> + * - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
>> + * - FIFO size: 4KB
>> + *
>> + * Copyright 2016 STMicroelectronics Inc.
>> + *
>> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
>> + * Denis Ciocca <denis.ciocca@st.com>
>> + *
>> + * 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 "st_lsm6dsx.h"
>> +
>> +#define ST_LSM6DSX_REG_ACC_DEC_MASK GENMASK(2, 0)
>> +#define ST_LSM6DSX_REG_GYRO_DEC_MASK GENMASK(5, 3)
>> +#define ST_LSM6DSX_REG_INT1_ADDR 0x0d
>> +#define ST_LSM6DSX_REG_FIFO_FTH_IRQ_MASK BIT(3)
>> +#define ST_LSM6DSX_REG_WHOAMI_ADDR 0x0f
>> +#define ST_LSM6DSX_REG_RESET_ADDR 0x12
>> +#define ST_LSM6DSX_REG_RESET_MASK BIT(0)
>> +#define ST_LSM6DSX_REG_BDU_ADDR 0x12
>> +#define ST_LSM6DSX_REG_BDU_MASK BIT(6)
>> +#define ST_LSM6DSX_REG_INT2_ON_INT1_ADDR 0x13
>> +#define ST_LSM6DSX_REG_INT2_ON_INT1_MASK BIT(5)
>> +#define ST_LSM6DSX_REG_ROUNDING_ADDR 0x16
>> +#define ST_LSM6DSX_REG_ROUNDING_MASK BIT(2)
>> +#define ST_LSM6DSX_REG_LIR_ADDR 0x58
>> +#define ST_LSM6DSX_REG_LIR_MASK BIT(0)
>> +
>> +#define ST_LSM6DSX_REG_ACC_ODR_ADDR 0x10
>> +#define ST_LSM6DSX_REG_ACC_ODR_MASK GENMASK(7, 4)
>> +#define ST_LSM6DSX_REG_ACC_FS_ADDR 0x10
>> +#define ST_LSM6DSX_REG_ACC_FS_MASK GENMASK(3, 2)
>> +#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 GENMASK(7, 4)
>> +#define ST_LSM6DSX_REG_GYRO_FS_ADDR 0x11
>> +#define ST_LSM6DSX_REG_GYRO_FS_MASK GENMASK(3, 2)
>> +#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,
>> + .id = ST_LSM6DS3_ID,
>> + },
>> + {
>> + .wai = ST_LSM6DSM_WHOAMI,
>> + .max_fifo_size = ST_LSM6DSM_MAX_FIFO_SIZE,
>> + .id = ST_LSM6DSM_ID,
>> + },
>> +};
>> +
>> +#define ST_LSM6DSX_CHANNEL(chan_type, addr, mod, scan_idx) \
>> +{ \
>> + .type = chan_type, \
>> + .address = addr, \
>> + .modified = 1, \
>> + .channel2 = mod, \
>> + .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 = scan_idx, \
>> + .scan_type = { \
>> + .sign = 's', \
>> + .realbits = 16, \
>> + .storagebits = 16, \
>> + .endianness = IIO_LE, \
>> + }, \
>> +}
>> +
>> +static const struct iio_chan_spec st_lsm6dsx_acc_channels[] = {
>> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_X_L_ADDR,
>> + IIO_MOD_X, 0),
>> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Y_L_ADDR,
>> + IIO_MOD_Y, 1),
>> + ST_LSM6DSX_CHANNEL(IIO_ACCEL, ST_LSM6DSX_REG_ACC_OUT_Z_L_ADDR,
>> + IIO_MOD_Z, 2),
>> + IIO_CHAN_SOFT_TIMESTAMP(3),
>> +};
>> +
>> +static const struct iio_chan_spec st_lsm6dsx_gyro_channels[] = {
>> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_X_L_ADDR,
>> + IIO_MOD_X, 0),
>> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Y_L_ADDR,
>> + IIO_MOD_Y, 1),
>> + ST_LSM6DSX_CHANNEL(IIO_ANGL_VEL, ST_LSM6DSX_REG_GYRO_OUT_Z_L_ADDR,
>> + IIO_MOD_Z, 2),
>> + 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 id)
>> +{
>> + int err, i;
>> + u8 data;
>> +
>> + for (i = 0; i < ARRAY_SIZE(st_lsm6dsx_sensor_settings); i++) {
>> + if (id == st_lsm6dsx_sensor_settings[i].id)
>> + break;
>> + }
>> +
>> + if (i == ARRAY_SIZE(st_lsm6dsx_sensor_settings)) {
>> + dev_err(hw->dev, "unsupported hw id [%02x]\n", id);
>> + return -ENODEV;
>> + }
>> +
>> + 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;
>> + }
>> +
>> + if (data != st_lsm6dsx_sensor_settings[i].wai) {
>> + dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
>> + return -ENODEV;
>> + }
>> +
>> + hw->settings = &st_lsm6dsx_sensor_settings[i];
>> +
>> + return 0;
>> +}
>> +
>> +static int st_lsm6dsx_set_full_scale(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;
>> + u8 val;
>> +
>> + 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;
>> + __le16 data;
>> +
>> + 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),
>> + (u8 *)&data);
>> + if (err < 0)
>> + return err;
>> +
>> + st_lsm6dsx_sensor_disable(sensor);
>> +
>> + *val = (s16)data;
>> +
>> + 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;
>> +
>> + switch (mask) {
>> + case IIO_CHAN_INFO_RAW:
>> + ret = iio_device_claim_direct_mode(iio_dev);
>> + if (ret)
>> + break;
>> +
>> + ret = st_lsm6dsx_read_oneshot(sensor, ch->address, val);
>> + iio_device_release_direct_mode(iio_dev);
>> + 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;
>> + }
>> +
>> + 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_full_scale(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_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_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_avail);
>> +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 Block Data Update */
>> + 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 device *dev, int irq, int hw_id,
>> + const struct st_lsm6dsx_transfer_function *tf_ops)
>> +{
>> + struct st_lsm6dsx_hw *hw;
>> + int i, err;
>> +
>> + hw = devm_kzalloc(dev, sizeof(*hw), GFP_KERNEL);
>> + if (!hw)
>> + return -ENOMEM;
>> +
>> + dev_set_drvdata(dev, (void *)hw);
>> +
>> + mutex_init(&hw->lock);
>> + mutex_init(&hw->fifo_lock);
>> +
>> + hw->dev = dev;
>> + hw->irq = irq;
>> + hw->tf = tf_ops;
>> +
>> + err = st_lsm6dsx_check_whoami(hw, hw_id);
>> + 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_fifo_setup(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@st.com>");
>> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
>> +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..ea30411
>> --- /dev/null
>> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_i2c.c
>> @@ -0,0 +1,101 @@
>> +/*
>> + * STMicroelectronics st_lsm6dsx i2c driver
>> + *
>> + * Copyright 2016 STMicroelectronics Inc.
>> + *
>> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
>> + * Denis Ciocca <denis.ciocca@st.com>
>> + *
>> + * 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];
>> +
>> + 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)
>> +{
>> + return st_lsm6dsx_probe(&client->dev, client->irq,
>> + (int)id->driver_data,
>> + &st_lsm6dsx_transfer_fn);
>> +}
>> +
>> +static const struct of_device_id st_lsm6dsx_i2c_of_match[] = {
>> + {
>> + .compatible = "st,lsm6ds3",
>> + .data = (void *)ST_LSM6DS3_ID,
>> + },
>> + {
>> + .compatible = "st,lsm6dsm",
>> + .data = (void *)ST_LSM6DSM_ID,
>> + },
>> + {},
>> +};
>> +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_LSM6DS3_ID },
>> + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
>> + {},
>> +};
>> +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@st.com>");
>> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
>> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx i2c driver");
>> +MODULE_LICENSE("GPL v2");
>> 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..fbe7247
>> --- /dev/null
>> +++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_spi.c
>> @@ -0,0 +1,118 @@
>> +/*
>> + * STMicroelectronics st_lsm6dsx spi driver
>> + *
>> + * Copyright 2016 STMicroelectronics Inc.
>> + *
>> + * Lorenzo Bianconi <lorenzo.bianconi@st.com>
>> + * Denis Ciocca <denis.ciocca@st.com>
>> + *
>> + * 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 BIT(7)
>> +
>> +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 st_lsm6dsx_hw *hw;
>> + struct spi_device *spi;
>> +
>> + if (len >= ST_LSM6DSX_TX_MAX_LENGTH)
>> + return -ENOMEM;
>> +
>> + spi = to_spi_device(dev);
>> + hw = spi_get_drvdata(spi);
>> +
>> + hw->tb.tx_buf[0] = addr;
>> + memcpy(&hw->tb.tx_buf[1], data, len);
>> +
>> + return spi_write(spi, hw->tb.tx_buf, len + 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)
>> +{
>> + const struct spi_device_id *id = spi_get_device_id(spi);
>> +
>> + return st_lsm6dsx_probe(&spi->dev, spi->irq,
>> + (int)id->driver_data,
>> + &st_lsm6dsx_transfer_fn);
>> +}
>> +
>> +static const struct of_device_id st_lsm6dsx_spi_of_match[] = {
>> + {
>> + .compatible = "st,lsm6ds3",
>> + .data = (void *)ST_LSM6DS3_ID,
>> + },
>> + {
>> + .compatible = "st,lsm6dsm",
>> + .data = (void *)ST_LSM6DSM_ID,
>> + },
>> + {},
>> +};
>> +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_LSM6DS3_ID },
>> + { ST_LSM6DSM_DEV_NAME, ST_LSM6DSM_ID },
>> + {},
>> +};
>> +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@st.com>");
>> +MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
>> +MODULE_DESCRIPTION("STMicroelectronics st_lsm6dsx spi driver");
>> +MODULE_LICENSE("GPL v2");
>>
>
--
UNIX is Sexy: who | grep -i blonde | talk; cd ~; wine; talk; touch;
unzip; touch; strip; gasp; finger; gasp; mount; fsck; more; yes; gasp;
umount; make clean; sleep
^ permalink raw reply [flat|nested] 11+ messages in thread
end of thread, other threads:[~2017-01-14 13:18 UTC | newest]
Thread overview: 11+ messages (download: mbox.gz / follow: Atom feed)
-- links below jump to the message on this page --
2017-01-10 21:55 [PATCH v4 0/2] add support to STM LSM6DS3-LSM6DSM 6-axis Mems sensor Lorenzo Bianconi
2017-01-10 21:55 ` Lorenzo Bianconi
[not found] ` <20170110215519.960-1-lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
2017-01-10 21:55 ` [PATCH v4 1/2] iio: imu: add support to lsm6dsx driver Lorenzo Bianconi
2017-01-10 21:55 ` Lorenzo Bianconi
[not found] ` <20170110215519.960-2-lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
2017-01-14 12:41 ` Jonathan Cameron
2017-01-14 12:41 ` Jonathan Cameron
2017-01-14 13:18 ` Lorenzo Bianconi
2017-01-10 21:55 ` [PATCH v4 2/2] Documentation: dt: iio: add st_lsm6dsx sensor device binding Lorenzo Bianconi
2017-01-10 21:55 ` Lorenzo Bianconi
[not found] ` <20170110215519.960-3-lorenzo.bianconi-qxv4g6HH51o@public.gmane.org>
2017-01-14 12:42 ` Jonathan Cameron
2017-01-14 12:42 ` Jonathan Cameron
This is an external index of several public inboxes,
see mirroring instructions on how to clone and mirror
all data and code used by this external index.