Initial ST patch

Change-Id: I66a587f5fcdf026ed472cb867fe9903051c2f78b

12 files changed, 4873 insertions, 0 deletions

diff --git a/Documentation/devicetree/bindings/iio/st_lsm6ds3.txt b/Documentation/devicetree/bindings/iio/st_lsm6ds3.txt
new file mode 100644
index 00000000000..bef52f2e290
--- /dev/null
+++ b/Documentation/devicetree/bindings/iio/st_lsm6ds3.txt
@@ -0,0 +1,19 @@
+/* STMicroelectronics lsm6ds3 sensor */
+
+Required properties:
+	- compatible : should be "st,lsm6ds3"
+	- reg : the I2C address of the sensor
+
+Optional properties:
+	- interrupt-parent : should be the phandle for the interrupt controller
+	- interrupts : interrupt mapping for GPIO IRQ, it should by configured with
+		flags IRQ_TYPE_EDGE_RISING
+
+Example:
+
+lsm6ds3@6b {
+	compatible = "st,lsm6ds3";
+	reg = <0x6b>;
+	interrupt-parent = <&gpio>;
+	interrupts = <1 IRQ_TYPE_EDGE_RISING>;
+};
\ No newline at end of file
diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig
index 4f40a10cb74..124aa491101 100644
--- a/drivers/iio/imu/Kconfig
+++ b/drivers/iio/imu/Kconfig
@@ -38,3 +38,4 @@ config IIO_ADIS_LIB_BUFFER
 	  family.
 
 source "drivers/iio/imu/inv_mpu6050/Kconfig"
+source "drivers/iio/imu/st_lsm6ds3/Kconfig"
diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile
index f2f56ceaed2..ebbe6001a32 100644
--- a/drivers/iio/imu/Makefile
+++ b/drivers/iio/imu/Makefile
@@ -13,3 +13,4 @@ adis_lib-$(CONFIG_IIO_ADIS_LIB_BUFFER) += adis_buffer.o
 obj-$(CONFIG_IIO_ADIS_LIB) += adis_lib.o
 
 obj-y += inv_mpu6050/
+obj-y += st_lsm6ds3/
diff --git a/drivers/iio/imu/st_lsm6ds3/Kconfig b/drivers/iio/imu/st_lsm6ds3/Kconfig
new file mode 100644
index 00000000000..cb350b25d12
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/Kconfig
@@ -0,0 +1,75 @@
+#
+# st-lsm6ds3 drivers for STMicroelectronics combo sensor
+#
+
+config ST_LSM6DS3_IIO
+	tristate "STMicroelectronics LSM6DS3 sensor"
+	depends on (I2C || SPI) && SYSFS
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  This driver supports the STMicroelectronics LSM6DS3 sensor.
+	  It is a gyroscope/accelerometer combo device.
+	  This driver can be built as a module. The module will be called
+	  st-lsm6ds3.
+
+config ST_LSM6DS3_IIO_LIMIT_FIFO
+	int "Limit fifo read lenght (#n byte)"
+	depends on ST_LSM6DS3_IIO
+	range 0 8192
+	default 0
+	help
+	  Limit atomic fifo read to #n byte. In some platform i2c/spi read
+	  can be limited by software or hardware.
+	  
+	  Set 0 to disable the limit.
+
+config ST_LSM6DS3_IIO_SENSORS_WAKEUP
+	bool "All sensors can wake-up system during suspend"
+	depends on ST_LSM6DS3_IIO
+	default n
+	help
+	  If disabled only tilt and significant motion can wake-up system
+	  during suspend.
+	  
+	  If enabled all sensors can wake-up system during suspend.
+
+menuconfig ST_LSM6DS3_IIO_MASTER_SUPPORT
+	bool "I2C master controller"
+	depends on I2C && ST_LSM6DS3_IIO
+	default n
+	help
+	  Added support for I2C master controller. Supported sensors up
+	  to 4.
+
+if ST_LSM6DS3_IIO_MASTER_SUPPORT
+
+config ST_LSM6DS3_ENABLE_INTERNAL_PULLUP
+	bool "Enabled internals pull-up resistors"
+	default y
+
+choice
+	prompt "External sensor 0"
+	default ST_LSM6DS3_IIO_EXT0_LIS3MDL
+	help
+	  Choose the external sensor 0 connected to LSM6DS3.
+
+config ST_LSM6DS3_IIO_EXT0_LIS3MDL
+	bool "LIS3MDL"
+config ST_LSM6DS3_IIO_EXT0_AKM09912
+	bool "AKM09912"
+endchoice
+
+choice
+	prompt "External sensor 1"
+	default ST_LSM6DS3_IIO_EXT1_DISABLED
+	help
+	  Choose the external sensor 1 connected to LSM6DS3.
+
+config ST_LSM6DS3_IIO_EXT1_DISABLED
+	bool "Disabled"
+config ST_LSM6DS3_IIO_EXT1_LPS22HB
+	bool "LPS22HB"
+endchoice
+
+endif
diff --git a/drivers/iio/imu/st_lsm6ds3/Makefile b/drivers/iio/imu/st_lsm6ds3/Makefile
new file mode 100644
index 00000000000..36348a34e89
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for STMicroelectronics LSM6DS3 sensor.
+#
+
+obj-$(CONFIG_ST_LSM6DS3_IIO) += st_lsm6ds3.o
+st_lsm6ds3-objs := st_lsm6ds3_core.o
+
+obj-$(CONFIG_ST_LSM6DS3_IIO) += st_lsm6ds3_spi.o st_lsm6ds3_i2c.o
+st_lsm6ds3-$(CONFIG_IIO_BUFFER) += st_lsm6ds3_buffer.o
+st_lsm6ds3-$(CONFIG_IIO_TRIGGER) += st_lsm6ds3_trigger.o
+st_lsm6ds3-$(CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT) += st_lsm6ds3_i2c_master.o
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
new file mode 100644
index 00000000000..78fbf76f4c2
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
@@ -0,0 +1,280 @@
+/*
+ * STMicroelectronics lsm6ds3 driver
+ *
+ * Copyright 2014 STMicroelectronics Inc.
+ *
+ * Denis Ciocca <denis.ciocca@st.com>
+ * v. 2.1.2
+ * Licensed under the GPL-2.
+ */
+
+#ifndef ST_LSM6DS3_H
+#define ST_LSM6DS3_H
+
+#include <linux/types.h>
+#include <linux/iio/trigger.h>
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+#include <linux/i2c.h>
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+#define LSM6DS3_DEV_NAME			"lsm6ds3"
+
+#define ST_INDIO_DEV_ACCEL			0
+#define ST_INDIO_DEV_GYRO			1
+#define ST_INDIO_DEV_SIGN_MOTION		2
+#define ST_INDIO_DEV_STEP_COUNTER		3
+#define ST_INDIO_DEV_STEP_DETECTOR		4
+#define ST_INDIO_DEV_TILT			5
+#define ST_INDIO_DEV_NUM			6
+
+#define ST_INDIO_DEV_EXT0			ST_INDIO_DEV_NUM
+#define ST_INDIO_DEV_EXT1			(ST_INDIO_DEV_NUM + 1)
+
+#define ST_LSM6DS3_ACCEL_DEPENDENCY	((1 << ST_INDIO_DEV_ACCEL) | \
+					(1 << ST_INDIO_DEV_STEP_COUNTER) | \
+					(1 << ST_INDIO_DEV_TILT) | \
+					(1 << ST_INDIO_DEV_SIGN_MOTION) | \
+					(1 << ST_INDIO_DEV_STEP_DETECTOR) | \
+					(1 << ST_INDIO_DEV_EXT0) | \
+					(1 << ST_INDIO_DEV_EXT1))
+
+#define ST_LSM6DS3_PEDOMETER_DEPENDENCY ((1 << ST_INDIO_DEV_STEP_COUNTER) | \
+					(1 << ST_INDIO_DEV_STEP_DETECTOR) | \
+					(1 << ST_INDIO_DEV_SIGN_MOTION))
+
+#define ST_LSM6DS3_EXTRA_DEPENDENCY	((1 << ST_INDIO_DEV_STEP_COUNTER) | \
+					(1 << ST_INDIO_DEV_TILT) | \
+					(1 << ST_INDIO_DEV_SIGN_MOTION) | \
+					(1 << ST_INDIO_DEV_STEP_DETECTOR) | \
+					(1 << ST_INDIO_DEV_EXT0) | \
+					(1 << ST_INDIO_DEV_EXT1))
+
+#define ST_LSM6DS3_USE_BUFFER		((1 << ST_INDIO_DEV_ACCEL) | \
+					(1 << ST_INDIO_DEV_GYRO) | \
+					(1 << ST_INDIO_DEV_STEP_COUNTER))
+
+#define ST_LSM6DS3_EXT_SENSORS		((1 << ST_INDIO_DEV_EXT0) | \
+					(1 << ST_INDIO_DEV_EXT1))
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP
+#define ST_LSM6DS3_WAKE_UP_SENSORS	((1 << ST_INDIO_DEV_SIGN_MOTION) | \
+					(1 << ST_INDIO_DEV_TILT))
+#else /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */
+#define ST_LSM6DS3_WAKE_UP_SENSORS	((1 << ST_INDIO_DEV_SIGN_MOTION) | \
+					(1 << ST_INDIO_DEV_TILT) | \
+					(1 << ST_INDIO_DEV_ACCEL) | \
+					(1 << ST_INDIO_DEV_GYRO) | \
+					(1 << ST_INDIO_DEV_STEP_COUNTER) | \
+					(1 << ST_INDIO_DEV_STEP_DETECTOR) | \
+					(1 << ST_INDIO_DEV_EXT0) | \
+					(1 << ST_INDIO_DEV_EXT1))
+#endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */
+
+#define ST_LSM6DS3_TX_MAX_LENGTH		12
+#define ST_LSM6DS3_RX_MAX_LENGTH		8193
+
+#define ST_LSM6DS3_BYTE_FOR_CHANNEL		2
+#define ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE	6
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+#define ST_LSM6DS3_NUM_CLIENTS			2
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+#define ST_LSM6DS3_NUM_CLIENTS			0
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+#define ST_LSM6DS3_LSM_CHANNELS(device_type, modif, index, mod, \
+						endian, sbits, rbits, addr, s) \
+{ \
+	.type = device_type, \
+	.modified = modif, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_SCALE), \
+	.scan_index = index, \
+	.channel2 = mod, \
+	.address = addr, \
+	.scan_type = { \
+		.sign = s, \
+		.realbits = rbits, \
+		.shift = sbits - rbits, \
+		.storagebits = sbits, \
+		.endianness = endian, \
+	}, \
+}
+
+#define ST_LSM6DS3_FIFO_LENGHT() \
+	IIO_DEVICE_ATTR(hw_fifo_lenght, S_IRUGO, \
+				st_lsm6ds3_sysfs_get_hw_fifo_lenght, NULL, 0);
+
+#define ST_LSM6DS3_FIFO_FLUSH() \
+	IIO_DEVICE_ATTR(flush, S_IWUSR, NULL, st_lsm6ds3_sysfs_flush_fifo, 0);
+
+enum fifo_mode {
+	BYPASS = 0,
+	CONTINUOS,
+};
+
+struct st_lsm6ds3_transfer_buffer {
+	struct mutex buf_lock;
+	u8 rx_buf[ST_LSM6DS3_RX_MAX_LENGTH];
+	u8 tx_buf[ST_LSM6DS3_TX_MAX_LENGTH] ____cacheline_aligned;
+};
+
+struct lsm6ds3_data {
+	const char *name;
+
+	bool reset_steps;
+	bool sign_motion_event_ready;
+
+	u8 *fifo_data;
+	u8 sensors_enabled;
+	u8 gyro_selftest_status;
+	u8 accel_selftest_status;
+	u8 accel_samples_in_pattern;
+	u8 gyro_samples_in_pattern;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	u8 ext0_samples_in_pattern;
+	u8 ext1_samples_in_pattern;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+	u8 accel_samples_to_discard;
+	u8 gyro_samples_to_discard;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	u8 ext_samples_to_discard[ST_LSM6DS3_NUM_CLIENTS];
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	u16 fifo_threshold;
+
+	int irq;
+
+	s64 timestamp;
+	int64_t accel_deltatime;
+	int64_t accel_timestamp;
+	int64_t gyro_deltatime;
+	int64_t gyro_timestamp;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	int64_t ext0_deltatime;
+	int64_t ext0_timestamp;
+	int64_t ext1_deltatime;
+	int64_t ext1_timestamp;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	struct work_struct data_work;
+
+	struct device *dev;
+	struct iio_dev *indio_dev[ST_INDIO_DEV_NUM + ST_LSM6DS3_NUM_CLIENTS];
+	struct iio_trigger *trig[ST_INDIO_DEV_NUM + ST_LSM6DS3_NUM_CLIENTS];
+	struct mutex bank_registers_lock;
+	struct mutex fifo_lock;
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	struct i2c_client *master_client[ST_LSM6DS3_NUM_CLIENTS];
+	struct mutex passthrough_lock;
+	bool ext0_available;
+	bool ext1_available;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	const struct st_lsm6ds3_transfer_function *tf;
+	struct st_lsm6ds3_transfer_buffer tb;
+};
+
+struct st_lsm6ds3_transfer_function {
+	int (*write) (struct lsm6ds3_data *cdata,
+				u8 reg_addr, int len, u8 *data, bool b_lock);
+	int (*read) (struct lsm6ds3_data *cdata,
+				u8 reg_addr, int len, u8 *data, bool b_lock);
+};
+
+struct lsm6ds3_sensor_data {
+	struct lsm6ds3_data *cdata;
+
+	unsigned int c_odr;
+	unsigned int c_gain[3];
+
+	u8 num_data_channels;
+	u8 sindex;
+	u8 *buffer_data;
+};
+
+int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata,
+				u8 reg_addr, u8 mask, u8 data, bool b_lock);
+
+int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq);
+void st_lsm6ds3_common_remove(struct lsm6ds3_data *cdata, int irq);
+
+int st_lsm6ds3_set_enable(struct lsm6ds3_sensor_data *sdata, bool enable);
+int st_lsm6ds3_set_axis_enable(struct lsm6ds3_sensor_data *sdata, u8 value);
+int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state);
+int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm);
+int st_lsm6ds3_reconfigure_fifo(struct lsm6ds3_data *cdata,
+						bool disable_irq_and_flush);
+
+ssize_t st_lsm6ds3_sysfs_get_hw_fifo_lenght(struct device *dev,
+				struct device_attribute *attr, char *buf);
+ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size);
+
+#ifdef CONFIG_IIO_BUFFER
+int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata);
+void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata);
+int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state);
+void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len);
+int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata);
+#define ST_LSM6DS3_TRIGGER_SET_STATE (&st_lsm6ds3_trig_set_state)
+#else /* CONFIG_IIO_BUFFER */
+static inline int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata)
+{
+	return 0;
+}
+static inline void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata)
+{
+}
+#define ST_LSM6DS3_TRIGGER_SET_STATE NULL
+#endif /* CONFIG_IIO_BUFFER */
+
+#ifdef CONFIG_IIO_TRIGGER
+void st_lsm6ds3_flush_works(void);
+int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
+				const struct iio_trigger_ops *trigger_ops);
+
+void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata);
+
+#else /* CONFIG_IIO_TRIGGER */
+static inline int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
+			const struct iio_trigger_ops *trigger_ops, int irq)
+{
+	return 0;
+}
+static inline void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata,
+								int irq)
+{
+	return;
+}
+static inline void st_lsm6ds3_flush_works()
+{
+	return;
+}
+#endif /* CONFIG_IIO_TRIGGER */
+
+#ifdef CONFIG_PM
+int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata);
+int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata);
+#endif /* CONFIG_PM */
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata);
+int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata);
+int st_lsm6ds3_enable_passthrough(struct lsm6ds3_data *cdata, bool enable);
+int st_lsm6ds3_enable_accel_dependency(struct lsm6ds3_sensor_data *sdata,
+								bool enable);
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+static inline int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata)
+{
+	return 0;
+}
+static inline int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata)
+{
+	return 0;
+}
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+#endif /* ST_LSM6DS3_H */
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
new file mode 100644
index 00000000000..8a7cc5176c2
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
@@ -0,0 +1,448 @@
+/*
+ * STMicroelectronics lsm6ds3 buffer driver
+ *
+ * Copyright 2014 STMicroelectronics Inc.
+ *
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/stat.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#include "st_lsm6ds3.h"
+
+#define ST_LSM6DS3_ENABLE_AXIS			0x07
+#define ST_LSM6DS3_FIFO_DIFF_L			0x3a
+#define ST_LSM6DS3_FIFO_DIFF_MASK		0x0fff
+#define ST_LSM6DS3_FIFO_DATA_OUT_L		0x3e
+#define ST_LSM6DS3_FIFO_DATA_OVR_2REGS		0x4000
+
+static void st_lsm6ds3_push_data_with_timestamp(struct lsm6ds3_data *cdata,
+					u8 index, u8 *data, int64_t timestamp)
+{
+	int i, n = 0;
+	struct iio_chan_spec const *chs = cdata->indio_dev[index]->channels;
+	uint16_t bfch, bfchs_out = 0, bfchs_in = 0;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(cdata->indio_dev[index]);
+
+	for (i = 0; i < sdata->num_data_channels; i++) {
+		bfch = chs[i].scan_type.storagebits >> 3;
+
+		if (test_bit(i, cdata->indio_dev[index]->active_scan_mask)) {
+			memcpy(&sdata->buffer_data[bfchs_out],
+							&data[bfchs_in], bfch);
+			n++;
+			bfchs_out += bfch;
+		}
+
+		bfchs_in += bfch;
+	}
+
+	if (cdata->indio_dev[index]->scan_timestamp)
+		*(s64 *)((u8 *)sdata->buffer_data +
+			ALIGN(bfchs_out, sizeof(s64))) = timestamp;
+
+	iio_push_to_buffers(cdata->indio_dev[index], sdata->buffer_data);
+}
+
+static void st_lsm6ds3_parse_fifo_data(struct lsm6ds3_data *cdata, u16 read_len)
+{
+	u16 fifo_offset = 0;
+	u8 gyro_sip, accel_sip;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	u8 ext0_sip, ext1_sip;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	while (fifo_offset < read_len) {
+		gyro_sip = cdata->gyro_samples_in_pattern;
+		accel_sip = cdata->accel_samples_in_pattern;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+		ext0_sip = cdata->ext0_samples_in_pattern;
+		ext1_sip = cdata->ext1_samples_in_pattern;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+		do {
+			if (gyro_sip > 0) {
+				if (cdata->gyro_samples_to_discard > 0)
+					cdata->gyro_samples_to_discard--;
+				else {
+					st_lsm6ds3_push_data_with_timestamp(
+						cdata, ST_INDIO_DEV_GYRO,
+						&cdata->fifo_data[fifo_offset],
+						cdata->gyro_timestamp);
+				}
+
+				cdata->gyro_timestamp += cdata->gyro_deltatime;
+				fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+				gyro_sip--;
+			}
+
+			if (accel_sip > 0) {
+				if (cdata->accel_samples_to_discard > 0)
+					cdata->accel_samples_to_discard--;
+				else {
+					st_lsm6ds3_push_data_with_timestamp(
+						cdata, ST_INDIO_DEV_ACCEL,
+						&cdata->fifo_data[fifo_offset],
+						cdata->accel_timestamp);
+				}
+
+				cdata->accel_timestamp +=
+							cdata->accel_deltatime;
+				fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+				accel_sip--;
+			}
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+			if (ext0_sip > 0) {
+				if (cdata->ext_samples_to_discard[0] > 0)
+					cdata->ext_samples_to_discard[0]--;
+				else {
+					st_lsm6ds3_push_data_with_timestamp(
+						cdata, ST_INDIO_DEV_EXT0,
+						&cdata->fifo_data[fifo_offset],
+						cdata->ext0_timestamp);
+				}
+
+				cdata->ext0_timestamp += cdata->ext0_deltatime;
+				fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+				ext0_sip--;
+			}
+
+			if (ext1_sip > 0) {
+				if (cdata->ext_samples_to_discard[1] > 0)
+					cdata->ext_samples_to_discard[1]--;
+				else {
+					st_lsm6ds3_push_data_with_timestamp(
+						cdata, ST_INDIO_DEV_EXT1,
+						&cdata->fifo_data[fifo_offset],
+						cdata->ext1_timestamp);
+				}
+
+				cdata->ext1_timestamp += cdata->ext1_deltatime;
+				fifo_offset += ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+				ext1_sip--;
+			}
+
+		} while ((accel_sip > 0) || (gyro_sip > 0) ||
+					(ext0_sip > 0) || (ext1_sip > 0));
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+		} while ((accel_sip > 0) || (gyro_sip > 0));
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+	}
+
+	return;
+}
+
+void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len)
+{
+	int err;
+#if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO > 0)
+	u16 data_remaining, data_to_read;
+#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */
+	u16 read_len = cdata->fifo_threshold, byte_in_pattern;
+
+	if (!cdata->fifo_data)
+		return;
+
+	if (check_fifo_len) {
+		err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DIFF_L,
+						2, (u8 *)&read_len, true);
+		if (err < 0)
+			return;
+
+		if (read_len & ST_LSM6DS3_FIFO_DATA_OVR_2REGS) {
+			dev_err(cdata->dev,
+				"data fifo overrun, failed to read it.\n");
+			return;
+		}
+
+		read_len &= ST_LSM6DS3_FIFO_DIFF_MASK;
+		read_len *= ST_LSM6DS3_BYTE_FOR_CHANNEL;
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+		byte_in_pattern = (cdata->accel_samples_in_pattern +
+					cdata->gyro_samples_in_pattern +
+					cdata->ext0_samples_in_pattern +
+					cdata->ext1_samples_in_pattern) *
+					ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+		byte_in_pattern = (cdata->accel_samples_in_pattern +
+					cdata->gyro_samples_in_pattern) *
+					ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+		read_len = (read_len / byte_in_pattern) * byte_in_pattern;
+
+		if (read_len > cdata->fifo_threshold)
+			read_len = cdata->fifo_threshold;
+	}
+
+	if (read_len == 0)
+		return;
+
+#if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO == 0)
+	err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_OUT_L,
+					read_len, cdata->fifo_data, true);
+	if (err < 0)
+		return;
+#else /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */
+	data_remaining = read_len;
+
+	do {
+		if (data_remaining > CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO)
+			data_to_read = CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO;
+		else
+			data_to_read = data_remaining;
+
+		err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_OUT_L,
+			data_to_read, &cdata->fifo_data[read_len - data_remaining], true);
+		if (err < 0)
+			return;
+
+		data_remaining -= data_to_read;
+	} while (data_remaining > 0);
+#endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */
+
+	st_lsm6ds3_parse_fifo_data(cdata, read_len);
+}
+
+static irqreturn_t st_lsm6ds3_step_counter_trigger_handler(int irq, void *p)
+{
+	int err;
+	struct timespec ts;
+	int64_t timestamp = 0;
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	if (!sdata->cdata->reset_steps) {
+		err = sdata->cdata->tf->read(sdata->cdata,
+					(u8)indio_dev->channels[0].address,
+					ST_LSM6DS3_BYTE_FOR_CHANNEL,
+					sdata->buffer_data, true);
+		if (err < 0)
+			goto st_lsm6ds3_step_counter_done;
+
+		timestamp = sdata->cdata->timestamp;
+	} else {
+		memset(sdata->buffer_data, 0, ST_LSM6DS3_BYTE_FOR_CHANNEL);
+		get_monotonic_boottime(&ts);
+		timestamp = timespec_to_ns(&ts);
+		sdata->cdata->reset_steps = false;
+	}
+
+	if (indio_dev->scan_timestamp)
+		*(s64 *)((u8 *)sdata->buffer_data +
+				ALIGN(ST_LSM6DS3_BYTE_FOR_CHANNEL,
+						sizeof(s64))) = timestamp;
+
+	iio_push_to_buffers(indio_dev, sdata->buffer_data);
+
+st_lsm6ds3_step_counter_done:
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+static inline irqreturn_t st_lsm6ds3_handler_empty(int irq, void *p)
+{
+	return IRQ_HANDLED;
+}
+
+int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata;
+
+	sdata = iio_priv(iio_trigger_get_drvdata(trig));
+
+	err = st_lsm6ds3_set_drdy_irq(sdata, state);
+
+	return err < 0 ? err : 0;
+}
+
+static int st_lsm6ds3_buffer_preenable(struct iio_dev *indio_dev)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	err = st_lsm6ds3_set_enable(sdata, true);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true);
+	if (err < 0)
+		return err;
+
+	return iio_sw_buffer_preenable(indio_dev);
+}
+
+static int st_lsm6ds3_buffer_postenable(struct iio_dev *indio_dev)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	if ((1 << sdata->sindex) & ST_LSM6DS3_USE_BUFFER) {
+		sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
+		if (sdata->buffer_data == NULL)
+			return -ENOMEM;
+	}
+
+	if ((sdata->sindex == ST_INDIO_DEV_ACCEL) ||
+					(sdata->sindex == ST_INDIO_DEV_GYRO)) {
+		err = st_lsm6ds3_set_axis_enable(sdata,
+					(u8)indio_dev->active_scan_mask[0]);
+		if (err < 0)
+			goto free_buffer_data;
+	}
+
+	err = iio_triggered_buffer_postenable(indio_dev);
+	if (err < 0)
+		goto free_buffer_data;
+
+	if (sdata->sindex == ST_INDIO_DEV_STEP_COUNTER) {
+		iio_trigger_poll_chained(
+			sdata->cdata->trig[ST_INDIO_DEV_STEP_COUNTER], 0);
+	}
+
+	if (sdata->sindex == ST_INDIO_DEV_SIGN_MOTION)
+		sdata->cdata->sign_motion_event_ready = true;
+
+	return 0;
+
+free_buffer_data:
+	if ((1 << sdata->sindex) & ST_LSM6DS3_USE_BUFFER)
+		kfree(sdata->buffer_data);
+
+	return err;
+}
+
+static int st_lsm6ds3_buffer_predisable(struct iio_dev *indio_dev)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	err = iio_triggered_buffer_predisable(indio_dev);
+	if (err < 0)
+		return err;
+
+	if ((sdata->sindex == ST_INDIO_DEV_ACCEL) ||
+					(sdata->sindex == ST_INDIO_DEV_GYRO)) {
+		err = st_lsm6ds3_set_axis_enable(sdata, ST_LSM6DS3_ENABLE_AXIS);
+		if (err < 0)
+			return err;
+	}
+
+	if (sdata->sindex == ST_INDIO_DEV_SIGN_MOTION)
+		sdata->cdata->sign_motion_event_ready = false;
+
+	err = st_lsm6ds3_set_enable(sdata, false);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true);
+	if (err < 0)
+		return err;
+
+	if ((1 << sdata->sindex) & ST_LSM6DS3_USE_BUFFER)
+		kfree(sdata->buffer_data);
+
+	return 0;
+}
+
+static const struct iio_buffer_setup_ops st_lsm6ds3_buffer_setup_ops = {
+	.preenable = &st_lsm6ds3_buffer_preenable,
+	.postenable = &st_lsm6ds3_buffer_postenable,
+	.predisable = &st_lsm6ds3_buffer_predisable,
+};
+
+int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata)
+{
+	int err;
+
+	err = iio_triggered_buffer_setup(cdata->indio_dev[ST_INDIO_DEV_ACCEL],
+				&st_lsm6ds3_handler_empty, NULL,
+				&st_lsm6ds3_buffer_setup_ops);
+	if (err < 0)
+		return err;
+
+	err = iio_triggered_buffer_setup(cdata->indio_dev[ST_INDIO_DEV_GYRO],
+				&st_lsm6ds3_handler_empty, NULL,
+				&st_lsm6ds3_buffer_setup_ops);
+	if (err < 0)
+		goto buffer_cleanup_accel;
+
+	err = iio_triggered_buffer_setup(
+				cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION],
+				&st_lsm6ds3_handler_empty, NULL,
+				&st_lsm6ds3_buffer_setup_ops);
+	if (err < 0)
+		goto buffer_cleanup_gyro;
+
+	err = iio_triggered_buffer_setup(
+				cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER],
+				NULL,
+				&st_lsm6ds3_step_counter_trigger_handler,
+				&st_lsm6ds3_buffer_setup_ops);
+	if (err < 0)
+		goto buffer_cleanup_sign_motion;
+
+	err = iio_triggered_buffer_setup(
+				cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR],
+				&st_lsm6ds3_handler_empty, NULL,
+				&st_lsm6ds3_buffer_setup_ops);
+	if (err < 0)
+		goto buffer_cleanup_step_counter;
+
+	err = iio_triggered_buffer_setup(
+				cdata->indio_dev[ST_INDIO_DEV_TILT],
+				&st_lsm6ds3_handler_empty, NULL,
+				&st_lsm6ds3_buffer_setup_ops);
+	if (err < 0)
+		goto buffer_cleanup_step_detector;
+
+	return 0;
+
+buffer_cleanup_step_detector:
+	iio_triggered_buffer_cleanup(
+				cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]);
+buffer_cleanup_step_counter:
+	iio_triggered_buffer_cleanup(
+				cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]);
+buffer_cleanup_sign_motion:
+	iio_triggered_buffer_cleanup(
+				cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]);
+buffer_cleanup_gyro:
+	iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_GYRO]);
+buffer_cleanup_accel:
+	iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_ACCEL]);
+	return err;
+}
+
+void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata)
+{
+	iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_TILT]);
+	iio_triggered_buffer_cleanup(
+				cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]);
+	iio_triggered_buffer_cleanup(
+				cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]);
+	iio_triggered_buffer_cleanup(
+				cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]);
+	iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_ACCEL]);
+	iio_triggered_buffer_cleanup(cdata->indio_dev[ST_INDIO_DEV_GYRO]);
+}
+
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 buffer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
new file mode 100644
index 00000000000..72881512c1b
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -0,0 +1,2137 @@
+/*
+ * STMicroelectronics lsm6ds3 core driver
+ *
+ * Copyright 2014 STMicroelectronics Inc.
+ *
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/of.h>
+#include <linux/irq.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/events.h>
+#include <asm/unaligned.h>
+
+#include <linux/iio/common/st_sensors.h>
+#include "st_lsm6ds3.h"
+
+#define MS_TO_NS(msec)				((msec) * 1000 * 1000)
+
+#ifndef MAX
+#define MAX(a, b)				(((a) > (b)) ? (a) : (b))
+#endif
+
+#define MIN_BNZ(a, b)				(((a) < (b)) ? ((a == 0) ? \
+						(b) : (a)) : ((b == 0) ? \
+						(a) : (b)))
+
+/* COMMON VALUES FOR ACCEL-GYRO SENSORS */
+#define ST_LSM6DS3_WAI_ADDRESS			0x0f
+#define ST_LSM6DS3_WAI_EXP			0x69
+#define ST_LSM6DS3_AXIS_EN_MASK			0x38
+#define ST_LSM6DS3_INT1_ADDR			0x0d
+#define ST_LSM6DS3_INT2_ADDR			0x0e
+#define ST_LSM6DS3_MD1_ADDR			0x5e
+#define ST_LSM6DS3_ODR_LIST_NUM			5
+#define ST_LSM6DS3_ODR_POWER_OFF_VAL		0x00
+#define ST_LSM6DS3_ODR_26HZ_VAL			0x02
+#define ST_LSM6DS3_ODR_52HZ_VAL			0x03
+#define ST_LSM6DS3_ODR_104HZ_VAL		0x04
+#define ST_LSM6DS3_ODR_208HZ_VAL		0x05
+#define ST_LSM6DS3_ODR_416HZ_VAL		0x06
+#define ST_LSM6DS3_FS_LIST_NUM			4
+#define ST_LSM6DS3_BDU_ADDR			0x12
+#define ST_LSM6DS3_BDU_MASK			0x40
+#define ST_LSM6DS3_EN_BIT			0x01
+#define ST_LSM6DS3_DIS_BIT			0x00
+#define ST_LSM6DS3_FUNC_EN_ADDR			0x19
+#define ST_LSM6DS3_FUNC_EN_MASK			0x04
+#define ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR		0x01
+#define ST_LSM6DS3_FUNC_CFG_ACCESS_MASK		0x01
+#define ST_LSM6DS3_FUNC_CFG_ACCESS_MASK2	0x04
+#define ST_LSM6DS3_FUNC_CFG_REG2_MASK		0x80
+#define ST_LSM6DS3_FUNC_CFG_START1_ADDR		0x62
+#define ST_LSM6DS3_FUNC_CFG_START2_ADDR		0x63
+#define ST_LSM6DS3_PASS_THROUGH_MODE_ADDR	0x1a
+#define ST_LSM6DS3_PASS_THROUGH_MODE_MASK	0x04
+#define ST_LSM6DS3_INTER_PULLUP_ADDR		0x1a
+#define ST_LSM6DS3_INTER_PULLUP_MASK		0x08
+#define ST_LSM6DS3_SENSORHUB_ADDR		0x1a
+#define ST_LSM6DS3_SENSORHUB_MASK		0x01
+#define ST_LSM6DS3_STARTCONFIG_ADDR		0x1a
+#define ST_LSM6DS3_STARTCONFIG_MASK		0x10
+#define ST_LSM6DS3_SELFTEST_ADDR		0x14
+#define ST_LSM6DS3_SELFTEST_ACCEL_MASK		0x03
+#define ST_LSM6DS3_SELFTEST_GYRO_MASK		0x0c
+#define ST_LSM6DS3_SELF_TEST_DISABLED_VAL	0x00
+#define ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL	0x01
+#define ST_LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL	0x02
+#define ST_LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL	0x03
+#define ST_LSM6DS3_LIR_ADDR			0x58
+#define ST_LSM6DS3_LIR_MASK			0x01
+#define ST_LSM6DS3_TIMER_EN_ADDR		0x58
+#define ST_LSM6DS3_TIMER_EN_MASK		0x80
+#define ST_LSM6DS3_PEDOMETER_EN_ADDR		0x58
+#define ST_LSM6DS3_PEDOMETER_EN_MASK		0x40
+#define ST_LSM6DS3_INT2_ON_INT1_ADDR		0x13
+#define ST_LSM6DS3_INT2_ON_INT1_MASK		0x20
+#define ST_LSM6DS3_MIN_DURATION_MS		1638
+#define ST_LSM6DS3_ROUNDING_ADDR		0x16
+#define ST_LSM6DS3_ROUNDING_MASK		0x04
+#define ST_LSM6DS3_FIFO_MODE_ADDR		0x0a
+#define ST_LSM6DS3_FIFO_MODE_MASK		0x07
+#define ST_LSM6DS3_FIFO_MODE_BYPASS		0x00
+#define ST_LSM6DS3_FIFO_MODE_CONTINUOS		0x06
+#define ST_LSM6DS3_FIFO_THRESHOLD_IRQ_MASK	0x08
+#define ST_LSM6DS3_FIFO_ODR_ADDR		0x0a
+#define ST_LSM6DS3_FIFO_ODR_MASK		0x78
+#define ST_LSM6DS3_FIFO_ODR_MAX			0x08
+#define ST_LSM6DS3_FIFO_ODR_OFF			0x00
+#define ST_LSM6DS3_FIFO_DECIMATOR_ADDR		0x08
+#define ST_LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK	0x07
+#define ST_LSM6DS3_FIFO_GYRO_DECIMATOR_MASK	0x38
+#define ST_LSM6DS3_FIFO_DECIMATOR2_ADDR		0x09
+#define ST_LSM6DS3_FIFO_DS3_DECIMATOR_MASK	0x07
+#define ST_LSM6DS3_FIFO_DS4_DECIMATOR_MASK	0x38
+#define ST_LSM6DS3_FIFO_THR_L_ADDR		0x06
+#define ST_LSM6DS3_FIFO_THR_H_ADDR		0x07
+#define ST_LSM6DS3_FIFO_THR_H_MASK		0x0f
+#define ST_LSM6DS3_FIFO_THR_IRQ_MASK		0x08
+#define ST_LSM6DS3_RESET_ADDR			0x12
+#define ST_LSM6DS3_RESET_MASK			0x01
+#define ST_LSM6DS3_MAX_FIFO_SIZE		8192
+#define ST_LSM6DS3_MAX_FIFO_LENGHT		(ST_LSM6DS3_MAX_FIFO_SIZE / \
+					ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE)
+
+/* CUSTOM VALUES FOR ACCEL SENSOR */
+#define ST_LSM6DS3_ACCEL_ODR_ADDR		0x10
+#define ST_LSM6DS3_ACCEL_ODR_MASK		0xf0
+#define ST_LSM6DS3_ACCEL_FS_ADDR		0x10
+#define ST_LSM6DS3_ACCEL_FS_MASK		0x0c
+#define ST_LSM6DS3_ACCEL_FS_2G_VAL		0x00
+#define ST_LSM6DS3_ACCEL_FS_4G_VAL		0x02
+#define ST_LSM6DS3_ACCEL_FS_8G_VAL		0x03
+#define ST_LSM6DS3_ACCEL_FS_16G_VAL		0x01
+#define ST_LSM6DS3_ACCEL_FS_2G_GAIN		IIO_G_TO_M_S_2(61)
+#define ST_LSM6DS3_ACCEL_FS_4G_GAIN		IIO_G_TO_M_S_2(122)
+#define ST_LSM6DS3_ACCEL_FS_8G_GAIN		IIO_G_TO_M_S_2(244)
+#define ST_LSM6DS3_ACCEL_FS_16G_GAIN		IIO_G_TO_M_S_2(488)
+#define ST_LSM6DS3_ACCEL_OUT_X_L_ADDR		0x28
+#define ST_LSM6DS3_ACCEL_OUT_Y_L_ADDR		0x2a
+#define ST_LSM6DS3_ACCEL_OUT_Z_L_ADDR		0x2c
+#define ST_LSM6DS3_ACCEL_AXIS_EN_ADDR		0x18
+#define ST_LSM6DS3_ACCEL_STD			3
+
+/* CUSTOM VALUES FOR GYRO SENSOR */
+#define ST_LSM6DS3_GYRO_ODR_ADDR		0x11
+#define ST_LSM6DS3_GYRO_ODR_MASK		0xf0
+#define ST_LSM6DS3_GYRO_FS_ADDR			0x11
+#define ST_LSM6DS3_GYRO_FS_MASK			0x0c
+#define ST_LSM6DS3_GYRO_FS_245_VAL		0x00
+#define ST_LSM6DS3_GYRO_FS_500_VAL		0x01
+#define ST_LSM6DS3_GYRO_FS_1000_VAL		0x02
+#define ST_LSM6DS3_GYRO_FS_2000_VAL		0x03
+#define ST_LSM6DS3_GYRO_FS_245_GAIN		IIO_DEGREE_TO_RAD(4375)
+#define ST_LSM6DS3_GYRO_FS_500_GAIN		IIO_DEGREE_TO_RAD(8750)
+#define ST_LSM6DS3_GYRO_FS_1000_GAIN		IIO_DEGREE_TO_RAD(17500)
+#define ST_LSM6DS3_GYRO_FS_2000_GAIN		IIO_DEGREE_TO_RAD(70000)
+#define ST_LSM6DS3_GYRO_OUT_X_L_ADDR		0x22
+#define ST_LSM6DS3_GYRO_OUT_Y_L_ADDR		0x24
+#define ST_LSM6DS3_GYRO_OUT_Z_L_ADDR		0x26
+#define ST_LSM6DS3_GYRO_AXIS_EN_ADDR		0x19
+#define ST_LSM6DS3_GYRO_STD			6
+
+/* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */
+#define ST_LSM6DS3_SIGN_MOTION_EN_ADDR		0x19
+#define ST_LSM6DS3_SIGN_MOTION_EN_MASK		0x01
+
+/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */
+#define ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK	0x80
+
+/* CUSTOM VALUES FOR STEP COUNTER SENSOR */
+#define ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK	0x80
+#define ST_LSM6DS3_STEP_COUNTER_OUT_L_ADDR	0x4b
+#define ST_LSM6DS3_STEP_COUNTER_RES_ADDR	0x19
+#define ST_LSM6DS3_STEP_COUNTER_RES_MASK	0x06
+#define ST_LSM6DS3_STEP_COUNTER_RES_ALL_EN	0x03
+#define ST_LSM6DS3_STEP_COUNTER_RES_FUNC_EN	0x02
+#define ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR	0x15
+
+/* CUSTOM VALUES FOR TILT SENSOR */
+#define ST_LSM6DS3_TILT_EN_ADDR			0x58
+#define ST_LSM6DS3_TILT_EN_MASK			0x20
+#define ST_LSM6DS3_TILT_DRDY_IRQ_MASK		0x02
+
+#define ST_LSM6DS3_ACCEL_SUFFIX_NAME		"accel"
+#define ST_LSM6DS3_GYRO_SUFFIX_NAME		"gyro"
+#define ST_LSM6DS3_STEP_COUNTER_SUFFIX_NAME	"step_c"
+#define ST_LSM6DS3_STEP_DETECTOR_SUFFIX_NAME	"step_d"
+#define ST_LSM6DS3_SIGN_MOTION_SUFFIX_NAME	"sign_motion"
+#define ST_LSM6DS3_TILT_SUFFIX_NAME		"tilt"
+
+#define ST_LSM6DS3_DEV_ATTR_SAMP_FREQ() \
+		IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO, \
+			st_lsm6ds3_sysfs_get_sampling_frequency, \
+			st_lsm6ds3_sysfs_set_sampling_frequency)
+
+#define ST_LSM6DS3_DEV_ATTR_SAMP_FREQ_AVAIL() \
+		IIO_DEV_ATTR_SAMP_FREQ_AVAIL( \
+			st_lsm6ds3_sysfs_sampling_frequency_avail)
+
+#define ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(name) \
+		IIO_DEVICE_ATTR(name, S_IRUGO, \
+			st_lsm6ds3_sysfs_scale_avail, NULL , 0);
+
+static struct st_lsm6ds3_selftest_table {
+	char *string_mode;
+	u8 accel_value;
+	u8 gyro_value;
+	u8 accel_mask;
+	u8 gyro_mask;
+} st_lsm6ds3_selftest_table[] = {
+	[0] = {
+		.string_mode = "disabled",
+		.accel_value = ST_LSM6DS3_SELF_TEST_DISABLED_VAL,
+		.gyro_value = ST_LSM6DS3_SELF_TEST_DISABLED_VAL,
+	},
+	[1] = {
+		.string_mode = "positive-sign",
+		.accel_value = ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL,
+		.gyro_value = ST_LSM6DS3_SELF_TEST_POS_SIGN_VAL
+	},
+	[2] = {
+		.string_mode = "negative-sign",
+		.accel_value = ST_LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL,
+		.gyro_value = ST_LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL
+	},
+};
+
+struct st_lsm6ds3_odr_reg {
+	unsigned int hz;
+	u8 value;
+};
+
+static struct st_lsm6ds3_odr_table {
+	u8 addr[2];
+	u8 mask[2];
+	struct st_lsm6ds3_odr_reg odr_avl[ST_LSM6DS3_ODR_LIST_NUM];
+} st_lsm6ds3_odr_table = {
+	.addr[ST_INDIO_DEV_ACCEL] = ST_LSM6DS3_ACCEL_ODR_ADDR,
+	.mask[ST_INDIO_DEV_ACCEL] = ST_LSM6DS3_ACCEL_ODR_MASK,
+	.addr[ST_INDIO_DEV_GYRO] = ST_LSM6DS3_GYRO_ODR_ADDR,
+	.mask[ST_INDIO_DEV_GYRO] = ST_LSM6DS3_GYRO_ODR_MASK,
+	.odr_avl[0] = { .hz = 26, .value = ST_LSM6DS3_ODR_26HZ_VAL },
+	.odr_avl[1] = { .hz = 52, .value = ST_LSM6DS3_ODR_52HZ_VAL },
+	.odr_avl[2] = { .hz = 104, .value = ST_LSM6DS3_ODR_104HZ_VAL },
+	.odr_avl[3] = { .hz = 208, .value = ST_LSM6DS3_ODR_208HZ_VAL },
+	.odr_avl[4] = { .hz = 416, .value = ST_LSM6DS3_ODR_416HZ_VAL },
+};
+
+struct st_lsm6ds3_fs_reg {
+	unsigned int gain;
+	u8 value;
+};
+
+static struct st_lsm6ds3_fs_table {
+	u8 addr;
+	u8 mask;
+	struct st_lsm6ds3_fs_reg fs_avl[ST_LSM6DS3_FS_LIST_NUM];
+} st_lsm6ds3_fs_table[ST_INDIO_DEV_NUM] = {
+	[ST_INDIO_DEV_ACCEL] = {
+		.addr = ST_LSM6DS3_ACCEL_FS_ADDR,
+		.mask = ST_LSM6DS3_ACCEL_FS_MASK,
+		.fs_avl[0] = { .gain = ST_LSM6DS3_ACCEL_FS_2G_GAIN,
+					.value = ST_LSM6DS3_ACCEL_FS_2G_VAL },
+		.fs_avl[1] = { .gain = ST_LSM6DS3_ACCEL_FS_4G_GAIN,
+					.value = ST_LSM6DS3_ACCEL_FS_4G_VAL },
+		.fs_avl[2] = { .gain = ST_LSM6DS3_ACCEL_FS_8G_GAIN,
+					.value = ST_LSM6DS3_ACCEL_FS_8G_VAL },
+		.fs_avl[3] = { .gain = ST_LSM6DS3_ACCEL_FS_16G_GAIN,
+					.value = ST_LSM6DS3_ACCEL_FS_16G_VAL },
+	},
+	[ST_INDIO_DEV_GYRO] = {
+		.addr = ST_LSM6DS3_GYRO_FS_ADDR,
+		.mask = ST_LSM6DS3_GYRO_FS_MASK,
+		.fs_avl[0] = { .gain = ST_LSM6DS3_GYRO_FS_245_GAIN,
+					.value = ST_LSM6DS3_GYRO_FS_245_VAL },
+		.fs_avl[1] = { .gain = ST_LSM6DS3_GYRO_FS_500_GAIN,
+					.value = ST_LSM6DS3_GYRO_FS_500_VAL },
+		.fs_avl[2] = { .gain = ST_LSM6DS3_GYRO_FS_1000_GAIN,
+					.value = ST_LSM6DS3_GYRO_FS_1000_VAL },
+		.fs_avl[3] = { .gain = ST_LSM6DS3_GYRO_FS_2000_GAIN,
+					.value = ST_LSM6DS3_GYRO_FS_2000_VAL },
+	}
+};
+
+static const struct iio_chan_spec st_lsm6ds3_accel_ch[] = {
+	ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 0, IIO_MOD_X, IIO_LE,
+				16, 16, ST_LSM6DS3_ACCEL_OUT_X_L_ADDR, 's'),
+	ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 1, IIO_MOD_Y, IIO_LE,
+				16, 16, ST_LSM6DS3_ACCEL_OUT_Y_L_ADDR, 's'),
+	ST_LSM6DS3_LSM_CHANNELS(IIO_ACCEL, 1, 2, IIO_MOD_Z, IIO_LE,
+				16, 16, ST_LSM6DS3_ACCEL_OUT_Z_L_ADDR, 's'),
+	IIO_CHAN_SOFT_TIMESTAMP(3)
+};
+
+static const struct iio_chan_spec st_lsm6ds3_gyro_ch[] = {
+	ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 0, IIO_MOD_X, IIO_LE,
+				16, 16, ST_LSM6DS3_GYRO_OUT_X_L_ADDR, 's'),
+	ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 1, IIO_MOD_Y, IIO_LE,
+				16, 16, ST_LSM6DS3_GYRO_OUT_Y_L_ADDR, 's'),
+	ST_LSM6DS3_LSM_CHANNELS(IIO_ANGL_VEL, 1, 2, IIO_MOD_Z, IIO_LE,
+				16, 16, ST_LSM6DS3_GYRO_OUT_Z_L_ADDR, 's'),
+	IIO_CHAN_SOFT_TIMESTAMP(3)
+};
+
+static const struct iio_chan_spec st_lsm6ds3_sign_motion_ch[] = {
+	{
+		.type = IIO_SIGN_MOTION,
+		.channel = 0,
+		.modified = 0,
+		.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING),
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1)
+};
+
+static const struct iio_chan_spec st_lsm6ds3_step_c_ch[] = {
+	{
+		.type = IIO_STEP_COUNTER,
+		.modified = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.address = ST_LSM6DS3_STEP_COUNTER_OUT_L_ADDR,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 16,
+			.storagebits = 16,
+			.endianness = IIO_LE,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1)
+};
+
+static const struct iio_chan_spec st_lsm6ds3_step_d_ch[] = {
+	{
+		.type = IIO_STEP_DETECTOR,
+		.channel = 0,
+		.modified = 0,
+		.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING),
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1)
+};
+
+static const struct iio_chan_spec st_lsm6ds3_tilt_ch[] = {
+	{
+		.type = IIO_TILT,
+		.channel = 0,
+		.modified = 0,
+		.event_mask = IIO_EV_BIT(IIO_EV_TYPE_THRESH, IIO_EV_DIR_RISING),
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1)
+};
+
+int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata,
+				u8 reg_addr, u8 mask, u8 data, bool b_lock)
+{
+	int err;
+	u8 new_data = 0x00, old_data = 0x00;
+
+	err = cdata->tf->read(cdata, reg_addr, 1, &old_data, b_lock);
+	if (err < 0)
+		return err;
+
+	new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask));
+
+	if (new_data == old_data)
+		return 1;
+
+	return cdata->tf->write(cdata, reg_addr, 1, &new_data, b_lock);
+}
+EXPORT_SYMBOL(st_lsm6ds3_write_data_with_mask);
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+static int st_lsm6ds3_enable_sensor_hub(struct lsm6ds3_data *cdata, bool enable)
+{
+	int err;
+
+	if (enable) {
+		if (cdata->sensors_enabled & ST_LSM6DS3_EXT_SENSORS) {
+			err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_STARTCONFIG_ADDR,
+						ST_LSM6DS3_STARTCONFIG_MASK,
+						ST_LSM6DS3_DIS_BIT, true);
+			if (err < 0)
+				return err;
+		}
+
+		if (cdata->sensors_enabled & ST_LSM6DS3_EXTRA_DEPENDENCY) {
+			err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_FUNC_EN_ADDR,
+						ST_LSM6DS3_FUNC_EN_MASK,
+						ST_LSM6DS3_EN_BIT, true);
+			if (err < 0)
+				return err;
+		}
+
+		if (cdata->sensors_enabled & ST_LSM6DS3_EXT_SENSORS) {
+			err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_SENSORHUB_ADDR,
+						ST_LSM6DS3_SENSORHUB_MASK,
+						ST_LSM6DS3_EN_BIT, true);
+			if (err < 0)
+				return err;
+		}
+	} else {
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_STARTCONFIG_ADDR,
+						ST_LSM6DS3_STARTCONFIG_MASK,
+						ST_LSM6DS3_EN_BIT, true);
+		if (err < 0)
+			return err;
+
+		usleep_range(1500, 4000);
+
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_SENSORHUB_ADDR,
+						ST_LSM6DS3_SENSORHUB_MASK,
+						ST_LSM6DS3_DIS_BIT, true);
+		if (err < 0)
+			return err;
+
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_FUNC_EN_ADDR,
+						ST_LSM6DS3_FUNC_EN_MASK,
+						ST_LSM6DS3_DIS_BIT, true);
+		if (err < 0)
+			return err;
+	}
+
+	return 0;
+}
+
+int st_lsm6ds3_enable_passthrough(struct lsm6ds3_data *cdata, bool enable)
+{
+	int err;
+	u8 reg_value;
+
+	if (enable)
+		reg_value = ST_LSM6DS3_EN_BIT;
+	else
+		reg_value = ST_LSM6DS3_DIS_BIT;
+
+	if (enable) {
+		err = st_lsm6ds3_enable_sensor_hub(cdata, false);
+		if (err < 0)
+			return err;
+
+#ifdef CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_INTER_PULLUP_ADDR,
+						ST_LSM6DS3_INTER_PULLUP_MASK,
+						ST_LSM6DS3_DIS_BIT, true);
+		if (err < 0)
+			return err;
+#endif /* CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP */
+	}
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_PASS_THROUGH_MODE_ADDR,
+					ST_LSM6DS3_PASS_THROUGH_MODE_MASK,
+					reg_value, enable);
+	if (err < 0)
+		return err;
+
+	if (!enable) {
+#ifdef CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+						ST_LSM6DS3_INTER_PULLUP_ADDR,
+						ST_LSM6DS3_INTER_PULLUP_MASK,
+						ST_LSM6DS3_EN_BIT, true);
+		if (err < 0)
+			return err;
+#endif /* CONFIG_ST_LSM6DS3_ENABLE_INTERNAL_PULLUP */
+
+		err = st_lsm6ds3_enable_sensor_hub(cdata, true);
+		if (err < 0)
+			return err;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(st_lsm6ds3_enable_passthrough);
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+static int st_lsm6ds3_set_fifo_enable(struct lsm6ds3_data *cdata, bool status)
+{
+	int err;
+	u8 reg_value;
+	struct timespec ts;
+
+	if (status)
+		reg_value = ST_LSM6DS3_FIFO_ODR_MAX;
+	else
+		reg_value = ST_LSM6DS3_FIFO_ODR_OFF;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_FIFO_ODR_ADDR,
+					ST_LSM6DS3_FIFO_ODR_MASK,
+					reg_value, true);
+	if (err < 0)
+		return err;
+
+	get_monotonic_boottime(&ts);
+	cdata->gyro_timestamp = timespec_to_ns(&ts);
+	cdata->accel_timestamp = cdata->gyro_timestamp;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	cdata->ext0_timestamp = cdata->gyro_timestamp;
+	cdata->ext1_timestamp = cdata->gyro_timestamp;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	return 0;
+}
+
+int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm)
+{
+	int err;
+	u8 reg_value;
+	bool enable_fifo;
+
+	switch (fm) {
+	case BYPASS:
+		reg_value = ST_LSM6DS3_FIFO_MODE_BYPASS;
+		enable_fifo = false;
+		break;
+	case CONTINUOS:
+		reg_value = ST_LSM6DS3_FIFO_MODE_CONTINUOS;
+		enable_fifo = true;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = st_lsm6ds3_set_fifo_enable(cdata, enable_fifo);
+	if (err < 0)
+		return err;
+
+	return st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_FIFO_MODE_ADDR,
+				ST_LSM6DS3_FIFO_MODE_MASK, reg_value, true);
+}
+EXPORT_SYMBOL(st_lsm6ds3_set_fifo_mode);
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+static int st_lsm6ds3_force_accel_odr(struct lsm6ds3_sensor_data *sdata,
+							unsigned int odr)
+{
+	int i;
+
+	for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) {
+		if (st_lsm6ds3_odr_table.odr_avl[i].hz == odr)
+			break;
+	}
+	if (i == ST_LSM6DS3_ODR_LIST_NUM)
+		return -EINVAL;
+
+	return st_lsm6ds3_write_data_with_mask(sdata->cdata,
+			st_lsm6ds3_odr_table.addr[sdata->sindex],
+			st_lsm6ds3_odr_table.mask[sdata->sindex],
+			st_lsm6ds3_odr_table.odr_avl[i].value, true);
+}
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata)
+{
+	int err;
+	struct iio_dev *indio_dev;
+	u16 min_num_pattern, max_num_pattern;
+	unsigned int min_odr = 416, max_odr = 0;
+	u8 accel_decimator = 0, gyro_decimator = 0;
+	u16 num_pattern_accel = 0, num_pattern_gyro = 0;
+	struct lsm6ds3_sensor_data *sdata_accel, *sdata_gyro;
+	u16 fifo_len, fifo_threshold, fifo_len_accel = 0, fifo_len_gyro = 0;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	int i;
+	bool force_accel_odr = false;
+	u8 ext0_decimator = 0, ext1_decimator = 0;
+	u16 num_pattern_ext1 = 0, fifo_len_ext1 = 0;
+	u16 num_pattern_ext0 = 0, fifo_len_ext0 = 0;
+	struct lsm6ds3_sensor_data *sdata_ext0 = NULL, *sdata_ext1 = NULL;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	indio_dev = cdata->indio_dev[ST_INDIO_DEV_ACCEL];
+	sdata_accel = iio_priv(indio_dev);
+	if ((1 << sdata_accel->sindex) & cdata->sensors_enabled) {
+		if (min_odr > sdata_accel->c_odr)
+			min_odr = sdata_accel->c_odr;
+
+		if (max_odr < sdata_accel->c_odr)
+			max_odr = sdata_accel->c_odr;
+
+		fifo_len_accel = (indio_dev->buffer->length / 2);
+	}
+
+	indio_dev = cdata->indio_dev[ST_INDIO_DEV_GYRO];
+	sdata_gyro = iio_priv(indio_dev);
+	if ((1 << sdata_gyro->sindex) & cdata->sensors_enabled) {
+		if (min_odr > sdata_gyro->c_odr)
+			min_odr = sdata_gyro->c_odr;
+
+		if (max_odr < sdata_gyro->c_odr)
+			max_odr = sdata_gyro->c_odr;
+
+		fifo_len_gyro = (indio_dev->buffer->length / 2);
+	}
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	if (cdata->ext0_available) {
+		indio_dev = cdata->indio_dev[ST_INDIO_DEV_EXT0];
+		sdata_ext0 = iio_priv(indio_dev);
+		if ((1 << sdata_ext0->sindex) & cdata->sensors_enabled) {
+			if (min_odr > sdata_ext0->c_odr)
+				min_odr = sdata_ext0->c_odr;
+
+			if (max_odr < sdata_ext0->c_odr) {
+				force_accel_odr = true;
+				max_odr = sdata_ext0->c_odr;
+			}
+
+			fifo_len_ext0 = (indio_dev->buffer->length / 2);
+		}
+	}
+
+	if (cdata->ext1_available) {
+		indio_dev = cdata->indio_dev[ST_INDIO_DEV_EXT1];
+		sdata_ext1 = iio_priv(indio_dev);
+		if ((1 << sdata_ext1->sindex) & cdata->sensors_enabled) {
+			if (min_odr > sdata_ext1->c_odr)
+				min_odr = sdata_ext1->c_odr;
+
+			if (max_odr < sdata_ext1->c_odr) {
+				force_accel_odr = true;
+				max_odr = sdata_ext1->c_odr;
+			}
+
+			fifo_len_ext1 = (indio_dev->buffer->length / 2);
+		}
+	}
+
+	if (force_accel_odr) {
+		err = st_lsm6ds3_force_accel_odr(sdata_accel, max_odr);
+		if (err < 0)
+			return err;
+	} else {
+		for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) {
+			if (st_lsm6ds3_odr_table.odr_avl[i].hz ==
+							sdata_accel->c_odr)
+				break;
+		}
+		if (i == ST_LSM6DS3_ODR_LIST_NUM)
+			return -EINVAL;
+
+		if (cdata->sensors_enabled & (1 << sdata_accel->sindex)) {
+			cdata->accel_samples_to_discard = ST_LSM6DS3_ACCEL_STD;
+
+			err = st_lsm6ds3_write_data_with_mask(cdata,
+				st_lsm6ds3_odr_table.addr[sdata_accel->sindex],
+				st_lsm6ds3_odr_table.mask[sdata_accel->sindex],
+				st_lsm6ds3_odr_table.odr_avl[i].value, true);
+			if (err < 0)
+				return err;
+		}
+	}
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	if ((1 << sdata_accel->sindex) & cdata->sensors_enabled) {
+		cdata->accel_samples_in_pattern =
+						(sdata_accel->c_odr / min_odr);
+		num_pattern_accel = MAX(fifo_len_accel /
+					cdata->accel_samples_in_pattern, 1);
+		cdata->accel_deltatime = (1000000000ULL / sdata_accel->c_odr);
+		accel_decimator = max_odr / sdata_accel->c_odr;
+	} else
+		cdata->accel_samples_in_pattern = 0;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_FIFO_DECIMATOR_ADDR,
+					ST_LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK,
+					accel_decimator, true);
+	if (err < 0)
+		return err;
+
+	if ((1 << sdata_gyro->sindex) & cdata->sensors_enabled) {
+		cdata->gyro_samples_in_pattern = (sdata_gyro->c_odr / min_odr);
+		num_pattern_gyro = MAX(fifo_len_gyro /
+					cdata->gyro_samples_in_pattern, 1);
+		cdata->gyro_deltatime = (1000000000ULL / sdata_gyro->c_odr);
+		gyro_decimator = max_odr / sdata_gyro->c_odr;
+	} else
+		cdata->gyro_samples_in_pattern = 0;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_FIFO_DECIMATOR_ADDR,
+					ST_LSM6DS3_FIFO_GYRO_DECIMATOR_MASK,
+					gyro_decimator, true);
+	if (err < 0)
+		return err;
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	if (cdata->ext0_available) {
+		if ((1 << sdata_ext0->sindex) & cdata->sensors_enabled) {
+			cdata->ext0_samples_in_pattern =
+						(sdata_ext0->c_odr / min_odr);
+			num_pattern_ext0 = MAX(fifo_len_ext0 /
+					cdata->ext0_samples_in_pattern, 1);
+			cdata->ext0_deltatime =
+					(1000000000ULL / sdata_ext0->c_odr);
+			ext0_decimator = max_odr / sdata_ext0->c_odr;
+		} else
+			cdata->ext0_samples_in_pattern = 0;
+
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_FIFO_DECIMATOR2_ADDR,
+					ST_LSM6DS3_FIFO_DS3_DECIMATOR_MASK,
+					ext0_decimator, true);
+		if (err < 0)
+			return err;
+	}
+
+	if (cdata->ext1_available) {
+		if ((1 << sdata_ext1->sindex) & cdata->sensors_enabled) {
+			cdata->ext1_samples_in_pattern =
+						(sdata_ext1->c_odr / min_odr);
+			num_pattern_ext1 = MAX(fifo_len_ext1 /
+					cdata->ext1_samples_in_pattern, 1);
+			cdata->ext1_deltatime =
+					(1000000000ULL / sdata_ext1->c_odr);
+			ext1_decimator = max_odr / sdata_ext1->c_odr;
+		} else
+			cdata->ext1_samples_in_pattern = 0;
+
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_FIFO_DECIMATOR2_ADDR,
+					ST_LSM6DS3_FIFO_DS4_DECIMATOR_MASK,
+					ext1_decimator, true);
+		if (err < 0)
+			return err;
+	}
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	min_num_pattern = MIN_BNZ(MIN_BNZ(MIN_BNZ(num_pattern_gyro,
+		num_pattern_accel), num_pattern_ext0), num_pattern_ext1);
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+	min_num_pattern = MIN_BNZ(num_pattern_gyro, num_pattern_accel);
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	if ((cdata->accel_samples_in_pattern +
+				cdata->gyro_samples_in_pattern +
+					cdata->ext0_samples_in_pattern +
+					cdata->ext1_samples_in_pattern) > 0) {
+		max_num_pattern = ST_LSM6DS3_MAX_FIFO_SIZE /
+					((cdata->accel_samples_in_pattern +
+					cdata->gyro_samples_in_pattern +
+					cdata->ext0_samples_in_pattern +
+					cdata->ext1_samples_in_pattern) *
+					ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE);
+
+		if (min_num_pattern > max_num_pattern)
+			min_num_pattern = max_num_pattern;
+	}
+
+	fifo_len = (cdata->accel_samples_in_pattern +
+			cdata->gyro_samples_in_pattern +
+			cdata->ext0_samples_in_pattern +
+			cdata->ext1_samples_in_pattern) *
+			min_num_pattern * ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+	if ((cdata->accel_samples_in_pattern +
+					cdata->gyro_samples_in_pattern) > 0) {
+		max_num_pattern = ST_LSM6DS3_MAX_FIFO_SIZE /
+					((cdata->accel_samples_in_pattern +
+					cdata->gyro_samples_in_pattern) *
+					ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE);
+
+		if (min_num_pattern > max_num_pattern)
+			min_num_pattern = max_num_pattern;
+	}
+
+	fifo_len = (cdata->accel_samples_in_pattern +
+			cdata->gyro_samples_in_pattern) *
+			min_num_pattern * ST_LSM6DS3_FIFO_ELEMENT_LEN_BYTE;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	if (fifo_len > 0) {
+		fifo_threshold = fifo_len / 2;
+
+		err = cdata->tf->write(cdata, ST_LSM6DS3_FIFO_THR_L_ADDR,
+					1, (u8 *)&fifo_threshold, true);
+		if (err < 0)
+			return err;
+
+		err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_FIFO_THR_H_ADDR,
+					ST_LSM6DS3_FIFO_THR_H_MASK,
+					*(((u8 *)&fifo_threshold) + 1), true);
+		if (err < 0)
+			return err;
+
+		cdata->fifo_threshold = fifo_len;
+	}
+	kfree(cdata->fifo_data);
+	cdata->fifo_data = 0;
+
+	if (fifo_len > 0) {
+		cdata->fifo_data = kmalloc(cdata->fifo_threshold, GFP_KERNEL);
+		if (!cdata->fifo_data)
+			return -ENOMEM;
+	}
+
+	return fifo_len;
+}
+EXPORT_SYMBOL(st_lsm6ds3_set_fifo_decimators_and_threshold);
+
+int st_lsm6ds3_reconfigure_fifo(struct lsm6ds3_data *cdata,
+						bool disable_irq_and_flush)
+{
+	int err, fifo_len;
+
+	if (disable_irq_and_flush) {
+		disable_irq(cdata->irq);
+		st_lsm6ds3_flush_works();
+	}
+
+	mutex_lock(&cdata->fifo_lock);
+
+	st_lsm6ds3_read_fifo(cdata, true);
+
+	err = st_lsm6ds3_set_fifo_mode(cdata, BYPASS);
+	if (err < 0)
+		goto reconfigure_fifo_irq_restore;
+
+	fifo_len = st_lsm6ds3_set_fifo_decimators_and_threshold(cdata);
+	if (fifo_len < 0) {
+		err = fifo_len;
+		goto reconfigure_fifo_irq_restore;
+	}
+
+	if (fifo_len > 0) {
+		err = st_lsm6ds3_set_fifo_mode(cdata, CONTINUOS);
+		if (err < 0)
+			goto reconfigure_fifo_irq_restore;
+	}
+
+reconfigure_fifo_irq_restore:
+	mutex_unlock(&cdata->fifo_lock);
+
+	if (disable_irq_and_flush)
+		enable_irq(cdata->irq);
+
+	return err;
+}
+EXPORT_SYMBOL(st_lsm6ds3_reconfigure_fifo);
+
+int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state)
+{
+	u8 reg_addr, mask, value;
+
+	if (state)
+		value = ST_LSM6DS3_EN_BIT;
+	else
+		value = ST_LSM6DS3_DIS_BIT;
+
+	switch (sdata->sindex) {
+	case ST_INDIO_DEV_ACCEL:
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+		if ((sdata->cdata->sensors_enabled &
+				(1 << ST_INDIO_DEV_GYRO)) ||
+					(sdata->cdata->sensors_enabled &
+						ST_LSM6DS3_EXT_SENSORS))
+			return 0;
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+		if (sdata->cdata->sensors_enabled & (1 << ST_INDIO_DEV_GYRO))
+			return 0;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+		reg_addr = ST_LSM6DS3_INT1_ADDR;
+		mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK;
+		break;
+	case ST_INDIO_DEV_GYRO:
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+		if ((sdata->cdata->sensors_enabled &
+				(1 << ST_INDIO_DEV_ACCEL)) ||
+					(sdata->cdata->sensors_enabled &
+						ST_LSM6DS3_EXT_SENSORS))
+			return 0;
+#else /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+		if (sdata->cdata->sensors_enabled & (1 << ST_INDIO_DEV_ACCEL))
+			return 0;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+		reg_addr = ST_LSM6DS3_INT1_ADDR;
+		mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK;
+		break;
+	case ST_INDIO_DEV_SIGN_MOTION:
+		if (sdata->cdata->sensors_enabled &
+					(1 << ST_INDIO_DEV_STEP_DETECTOR))
+			return 0;
+
+		reg_addr = ST_LSM6DS3_INT1_ADDR;
+		mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK;
+		break;
+	case ST_INDIO_DEV_STEP_COUNTER:
+		reg_addr = ST_LSM6DS3_INT2_ADDR;
+		mask = ST_LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK;
+		break;
+	case ST_INDIO_DEV_STEP_DETECTOR:
+		if (sdata->cdata->sensors_enabled &
+					(1 << ST_INDIO_DEV_SIGN_MOTION))
+			return 0;
+
+		reg_addr = ST_LSM6DS3_INT1_ADDR;
+		mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK;
+		break;
+	case ST_INDIO_DEV_TILT:
+		reg_addr = ST_LSM6DS3_MD1_ADDR;
+		mask = ST_LSM6DS3_TILT_DRDY_IRQ_MASK;
+		break;
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	case ST_INDIO_DEV_EXT0:
+		if ((sdata->cdata->sensors_enabled &
+					(1 << ST_INDIO_DEV_ACCEL)) ||
+					(sdata->cdata->sensors_enabled &
+						(1 << ST_INDIO_DEV_GYRO)) ||
+					(sdata->cdata->sensors_enabled &
+						(1 << ST_INDIO_DEV_EXT1)))
+			return 0;
+
+		reg_addr = ST_LSM6DS3_INT1_ADDR;
+		mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK;
+		break;
+	case ST_INDIO_DEV_EXT1:
+		if ((sdata->cdata->sensors_enabled &
+					(1 << ST_INDIO_DEV_ACCEL)) ||
+					(sdata->cdata->sensors_enabled &
+						(1 << ST_INDIO_DEV_GYRO)) ||
+					(sdata->cdata->sensors_enabled &
+						(1 << ST_INDIO_DEV_EXT0)))
+			return 0;
+
+		reg_addr = ST_LSM6DS3_INT1_ADDR;
+		mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK;
+		break;
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+	default:
+		return -EINVAL;
+	}
+
+	return st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						reg_addr, mask, value, true);
+}
+EXPORT_SYMBOL(st_lsm6ds3_set_drdy_irq);
+
+int st_lsm6ds3_set_axis_enable(struct lsm6ds3_sensor_data *sdata, u8 value)
+{
+	u8 reg_addr;
+
+	switch (sdata->sindex) {
+	case ST_INDIO_DEV_ACCEL:
+		reg_addr = ST_LSM6DS3_ACCEL_AXIS_EN_ADDR;
+		break;
+	case ST_INDIO_DEV_GYRO:
+		reg_addr = ST_LSM6DS3_GYRO_AXIS_EN_ADDR;
+		break;
+	default:
+		return 0;
+	}
+
+	return st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				reg_addr, ST_LSM6DS3_AXIS_EN_MASK, value, true);
+}
+EXPORT_SYMBOL(st_lsm6ds3_set_axis_enable);
+
+int st_lsm6ds3_enable_accel_dependency(struct lsm6ds3_sensor_data *sdata,
+								bool enable)
+{
+	int err;
+
+	if (!((sdata->cdata->sensors_enabled &
+			ST_LSM6DS3_ACCEL_DEPENDENCY) & ~(1 << sdata->sindex))) {
+		if (enable) {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				st_lsm6ds3_odr_table.addr[ST_INDIO_DEV_ACCEL],
+				st_lsm6ds3_odr_table.mask[ST_INDIO_DEV_ACCEL],
+				st_lsm6ds3_odr_table.odr_avl[0].value, true);
+			if (err < 0)
+				return err;
+		} else {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				st_lsm6ds3_odr_table.addr[ST_INDIO_DEV_ACCEL],
+				st_lsm6ds3_odr_table.mask[ST_INDIO_DEV_ACCEL],
+				ST_LSM6DS3_ODR_POWER_OFF_VAL, true);
+			if (err < 0)
+				return err;
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(st_lsm6ds3_enable_accel_dependency);
+
+static int st_lsm6ds3_set_extra_dependency(struct lsm6ds3_sensor_data *sdata,
+								bool enable)
+{
+	int err;
+
+	if (!((sdata->cdata->sensors_enabled &
+			ST_LSM6DS3_EXTRA_DEPENDENCY) & ~(1 << sdata->sindex))) {
+		if (enable) {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_FUNC_EN_ADDR,
+						ST_LSM6DS3_FUNC_EN_MASK,
+						ST_LSM6DS3_EN_BIT, true);
+			if (err < 0)
+				return err;
+		} else {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_FUNC_EN_ADDR,
+						ST_LSM6DS3_FUNC_EN_MASK,
+						ST_LSM6DS3_DIS_BIT, true);
+			if (err < 0)
+				return err;
+		}
+	}
+
+	return st_lsm6ds3_enable_accel_dependency(sdata, enable);
+}
+
+static int st_lsm6ds3_enable_pedometer(struct lsm6ds3_sensor_data *sdata,
+								bool enable)
+{
+	u8 value = ST_LSM6DS3_DIS_BIT;
+
+	if ((sdata->cdata->sensors_enabled & ~(1 << sdata->sindex)) &
+						ST_LSM6DS3_PEDOMETER_DEPENDENCY)
+		return 0;
+
+	if (enable)
+		value = ST_LSM6DS3_EN_BIT;
+
+	return st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_PEDOMETER_EN_ADDR,
+						ST_LSM6DS3_PEDOMETER_EN_MASK,
+						value, true);
+
+}
+
+static int st_lsm6ds3_enable_sensors(struct lsm6ds3_sensor_data *sdata)
+{
+	int err, i;
+
+	switch (sdata->sindex) {
+	case ST_INDIO_DEV_ACCEL:
+	case ST_INDIO_DEV_GYRO:
+		for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) {
+			if (st_lsm6ds3_odr_table.odr_avl[i].hz == sdata->c_odr)
+				break;
+		}
+		if (i == ST_LSM6DS3_ODR_LIST_NUM)
+			return -EINVAL;
+
+		if (sdata->sindex == ST_INDIO_DEV_ACCEL) {
+			sdata->cdata->accel_samples_to_discard =
+							ST_LSM6DS3_ACCEL_STD;
+		}
+
+		sdata->cdata->gyro_samples_to_discard = ST_LSM6DS3_GYRO_STD;
+
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				st_lsm6ds3_odr_table.addr[sdata->sindex],
+				st_lsm6ds3_odr_table.mask[sdata->sindex],
+				st_lsm6ds3_odr_table.odr_avl[i].value, true);
+		if (err < 0)
+			return err;
+
+		sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[i].hz;
+
+		break;
+	case ST_INDIO_DEV_SIGN_MOTION:
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_SIGN_MOTION_EN_ADDR,
+					ST_LSM6DS3_SIGN_MOTION_EN_MASK,
+					ST_LSM6DS3_EN_BIT, true);
+		if (err < 0)
+			return err;
+
+		if ((sdata->cdata->sensors_enabled & ~(1 << sdata->sindex)) &
+					ST_LSM6DS3_PEDOMETER_DEPENDENCY) {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_PEDOMETER_EN_ADDR,
+						ST_LSM6DS3_PEDOMETER_EN_MASK,
+						ST_LSM6DS3_DIS_BIT, true);
+			if (err < 0)
+				return err;
+
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_PEDOMETER_EN_ADDR,
+						ST_LSM6DS3_PEDOMETER_EN_MASK,
+						ST_LSM6DS3_EN_BIT, true);
+			if (err < 0)
+				return err;
+		} else {
+			err = st_lsm6ds3_enable_pedometer(sdata, true);
+			if (err < 0)
+				return err;
+		}
+
+		break;
+	case ST_INDIO_DEV_STEP_COUNTER:
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_TIMER_EN_ADDR,
+					ST_LSM6DS3_TIMER_EN_MASK,
+					ST_LSM6DS3_EN_BIT, true);
+		if (err < 0)
+			return err;
+
+	case ST_INDIO_DEV_STEP_DETECTOR:
+		err = st_lsm6ds3_enable_pedometer(sdata, true);
+		if (err < 0)
+			return err;
+
+		break;
+	case ST_INDIO_DEV_TILT:
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_TILT_EN_ADDR,
+					ST_LSM6DS3_TILT_EN_MASK,
+					ST_LSM6DS3_EN_BIT, true);
+		if (err < 0)
+			return err;
+
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = st_lsm6ds3_set_extra_dependency(sdata, true);
+	if (err < 0)
+		return err;
+
+	sdata->cdata->sensors_enabled |= (1 << sdata->sindex);
+
+	return 0;
+}
+
+static int st_lsm6ds3_disable_sensors(struct lsm6ds3_sensor_data *sdata)
+{
+	int err;
+
+	switch (sdata->sindex) {
+	case ST_INDIO_DEV_ACCEL:
+		if (sdata->cdata->sensors_enabled &
+						ST_LSM6DS3_EXTRA_DEPENDENCY) {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				st_lsm6ds3_odr_table.addr[sdata->sindex],
+				st_lsm6ds3_odr_table.mask[sdata->sindex],
+				st_lsm6ds3_odr_table.odr_avl[0].value, true);
+		} else {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				st_lsm6ds3_odr_table.addr[sdata->sindex],
+				st_lsm6ds3_odr_table.mask[sdata->sindex],
+				ST_LSM6DS3_ODR_POWER_OFF_VAL, true);
+		}
+		if (err < 0)
+			return err;
+
+		break;
+	case ST_INDIO_DEV_GYRO:
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				st_lsm6ds3_odr_table.addr[sdata->sindex],
+				st_lsm6ds3_odr_table.mask[sdata->sindex],
+				ST_LSM6DS3_ODR_POWER_OFF_VAL, true);
+		if (err < 0)
+			return err;
+
+		break;
+	case ST_INDIO_DEV_SIGN_MOTION:
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_SIGN_MOTION_EN_ADDR,
+					ST_LSM6DS3_SIGN_MOTION_EN_MASK,
+					ST_LSM6DS3_DIS_BIT, true);
+		if (err < 0)
+			return err;
+
+		err = st_lsm6ds3_enable_pedometer(sdata, false);
+		if (err < 0)
+			return err;
+
+		break;
+	case ST_INDIO_DEV_STEP_COUNTER:
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_TIMER_EN_ADDR,
+					ST_LSM6DS3_TIMER_EN_MASK,
+					ST_LSM6DS3_DIS_BIT, true);
+		if (err < 0)
+			return err;
+
+	case ST_INDIO_DEV_STEP_DETECTOR:
+		err = st_lsm6ds3_enable_pedometer(sdata, false);
+		if (err < 0)
+			return err;
+
+		break;
+	case ST_INDIO_DEV_TILT:
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_TILT_EN_ADDR,
+					ST_LSM6DS3_TILT_EN_MASK,
+					ST_LSM6DS3_DIS_BIT, true);
+		if (err < 0)
+			return err;
+
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = st_lsm6ds3_set_extra_dependency(sdata, false);
+	if (err < 0)
+		return err;
+
+	sdata->cdata->sensors_enabled &= ~(1 << sdata->sindex);
+
+	return 0;
+}
+
+int st_lsm6ds3_set_enable(struct lsm6ds3_sensor_data *sdata, bool enable)
+{
+	if (enable)
+		return st_lsm6ds3_enable_sensors(sdata);
+	else
+		return st_lsm6ds3_disable_sensors(sdata);
+}
+EXPORT_SYMBOL(st_lsm6ds3_set_enable);
+
+static int st_lsm6ds3_set_odr(struct lsm6ds3_sensor_data *sdata,
+							unsigned int odr)
+{
+	int err, i;
+
+	for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) {
+		if (st_lsm6ds3_odr_table.odr_avl[i].hz == odr)
+			break;
+	}
+	if (i == ST_LSM6DS3_ODR_LIST_NUM)
+		return -EINVAL;
+
+	if (sdata->cdata->sensors_enabled & (1 << sdata->sindex)) {
+		disable_irq(sdata->cdata->irq);
+		st_lsm6ds3_flush_works();
+
+		if (sdata->sindex == ST_INDIO_DEV_ACCEL)
+			sdata->cdata->accel_samples_to_discard =
+							ST_LSM6DS3_ACCEL_STD;
+
+		sdata->cdata->gyro_samples_to_discard = ST_LSM6DS3_GYRO_STD;
+
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				st_lsm6ds3_odr_table.addr[sdata->sindex],
+				st_lsm6ds3_odr_table.mask[sdata->sindex],
+				st_lsm6ds3_odr_table.odr_avl[i].value, true);
+		if (err < 0) {
+			enable_irq(sdata->cdata->irq);
+			return err;
+		}
+
+		sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[i].hz;
+
+		st_lsm6ds3_reconfigure_fifo(sdata->cdata, false);
+		enable_irq(sdata->cdata->irq);
+	} else
+		sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[i].hz;
+
+	return 0;
+}
+
+static int st_lsm6ds3_set_fs(struct lsm6ds3_sensor_data *sdata,
+							unsigned int gain)
+{
+	int err, i;
+
+	for (i = 0; i < ST_LSM6DS3_FS_LIST_NUM; i++) {
+		if (st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].gain == gain)
+			break;
+	}
+	if (i == ST_LSM6DS3_FS_LIST_NUM)
+		return -EINVAL;
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+		st_lsm6ds3_fs_table[sdata->sindex].addr,
+		st_lsm6ds3_fs_table[sdata->sindex].mask,
+		st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].value, true);
+	if (err < 0)
+		return err;
+
+	sdata->c_gain[0] = gain;
+
+	return 0;
+}
+
+static int st_lsm6ds3_read_raw(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *ch, int *val,
+							int *val2, long mask)
+{
+	int err;
+	u8 outdata[ST_LSM6DS3_BYTE_FOR_CHANNEL];
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&indio_dev->mlock);
+
+		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
+			mutex_unlock(&indio_dev->mlock);
+			return -EBUSY;
+		}
+
+		err = st_lsm6ds3_set_enable(sdata, true);
+		if (err < 0) {
+			mutex_unlock(&indio_dev->mlock);
+			return -EBUSY;
+		}
+
+		if (sdata->sindex == ST_INDIO_DEV_ACCEL)
+			msleep(40);
+
+		if (sdata->sindex == ST_INDIO_DEV_GYRO)
+			msleep(120);
+
+		err = sdata->cdata->tf->read(sdata->cdata, ch->address,
+				ST_LSM6DS3_BYTE_FOR_CHANNEL, outdata, true);
+		if (err < 0) {
+			mutex_unlock(&indio_dev->mlock);
+			return err;
+		}
+
+		*val = (s16)get_unaligned_le16(outdata);
+		*val = *val >> ch->scan_type.shift;
+
+		err = st_lsm6ds3_set_enable(sdata, false);
+
+		mutex_unlock(&indio_dev->mlock);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = sdata->c_gain[0];
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int st_lsm6ds3_write_raw(struct iio_dev *indio_dev,
+		struct iio_chan_spec const *chan, int val, int val2, long mask)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		mutex_lock(&indio_dev->mlock);
+
+		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
+			mutex_unlock(&indio_dev->mlock);
+			return -EBUSY;
+		}
+
+		err = st_lsm6ds3_set_fs(sdata, val2);
+		mutex_unlock(&indio_dev->mlock);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return err;
+}
+
+static int st_lsm6ds3_reset_steps(struct lsm6ds3_data *cdata)
+{
+	int err;
+	u8 reg_value = 0x00;
+
+	err = cdata->tf->read(cdata,
+			ST_LSM6DS3_STEP_COUNTER_RES_ADDR, 1, &reg_value, true);
+	if (err < 0)
+		return err;
+
+	if (reg_value & ST_LSM6DS3_FUNC_EN_MASK)
+		reg_value = ST_LSM6DS3_STEP_COUNTER_RES_FUNC_EN;
+	else
+		reg_value = ST_LSM6DS3_DIS_BIT;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+				ST_LSM6DS3_STEP_COUNTER_RES_ADDR,
+				ST_LSM6DS3_STEP_COUNTER_RES_MASK,
+				ST_LSM6DS3_STEP_COUNTER_RES_ALL_EN, true);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+				ST_LSM6DS3_STEP_COUNTER_RES_ADDR,
+				ST_LSM6DS3_STEP_COUNTER_RES_MASK,
+				reg_value, true);
+	if (err < 0)
+		return err;
+
+	cdata->reset_steps = true;
+
+	return 0;
+}
+
+static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
+{
+	int err, i;
+	u8 default_reg_value = 0;
+	struct lsm6ds3_sensor_data *sdata;
+
+	mutex_init(&cdata->tb.buf_lock);
+
+	cdata->sensors_enabled = 0;
+	cdata->reset_steps = false;
+	cdata->sign_motion_event_ready = false;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_RESET_ADDR,
+				ST_LSM6DS3_RESET_MASK, ST_LSM6DS3_EN_BIT, true);
+	if (err < 0)
+		return err;
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
+		sdata = iio_priv(cdata->indio_dev[i]);
+
+		err = st_lsm6ds3_set_enable(sdata, false);
+		if (err < 0)
+			return err;
+
+		err = st_lsm6ds3_set_drdy_irq(sdata, false);
+		if (err < 0)
+			return err;
+
+		switch (sdata->sindex) {
+		case ST_INDIO_DEV_ACCEL:
+		case ST_INDIO_DEV_GYRO:
+			sdata->num_data_channels =
+					ARRAY_SIZE(st_lsm6ds3_accel_ch) - 1;
+
+			err = st_lsm6ds3_set_fs(sdata, sdata->c_gain[0]);
+			if (err < 0)
+				return err;
+
+			break;
+		case ST_INDIO_DEV_STEP_COUNTER:
+			sdata->num_data_channels =
+					ARRAY_SIZE(st_lsm6ds3_step_c_ch) - 1;
+			break;
+		default:
+			break;
+		}
+	}
+
+	cdata->gyro_selftest_status = 0;
+	cdata->accel_selftest_status = 0;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_LIR_ADDR,
+				ST_LSM6DS3_LIR_MASK, ST_LSM6DS3_EN_BIT, true);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_BDU_ADDR,
+				ST_LSM6DS3_BDU_MASK, ST_LSM6DS3_EN_BIT, true);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_set_fifo_enable(sdata->cdata, false);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_ROUNDING_ADDR,
+					ST_LSM6DS3_ROUNDING_MASK,
+					ST_LSM6DS3_EN_BIT, true);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_write_data_with_mask(cdata,
+					ST_LSM6DS3_INT2_ON_INT1_ADDR,
+					ST_LSM6DS3_INT2_ON_INT1_MASK,
+					ST_LSM6DS3_EN_BIT, true);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_reset_steps(sdata->cdata);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&cdata->bank_registers_lock);
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR,
+					ST_LSM6DS3_FUNC_CFG_REG2_MASK,
+					ST_LSM6DS3_EN_BIT, false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	err = sdata->cdata->tf->write(sdata->cdata,
+					ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR,
+					1, &default_reg_value, false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR,
+					ST_LSM6DS3_FUNC_CFG_REG2_MASK,
+					ST_LSM6DS3_DIS_BIT, false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	mutex_unlock(&cdata->bank_registers_lock);
+
+	sdata->c_odr = 0;
+
+	return 0;
+
+st_lsm6ds3_init_sensor_mutex_unlock:
+	mutex_unlock(&cdata->bank_registers_lock);
+	return err;
+}
+
+static int st_lsm6ds3_set_selftest(struct lsm6ds3_sensor_data *sdata, int index)
+{
+	int err;
+	u8 mode, mask;
+
+	switch (sdata->sindex) {
+	case ST_INDIO_DEV_ACCEL:
+		mask = ST_LSM6DS3_SELFTEST_ACCEL_MASK;
+		mode = st_lsm6ds3_selftest_table[index].accel_value;
+		break;
+	case ST_INDIO_DEV_GYRO:
+		mask = ST_LSM6DS3_SELFTEST_GYRO_MASK;
+		mode = st_lsm6ds3_selftest_table[index].gyro_value;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				ST_LSM6DS3_SELFTEST_ADDR, mask, mode, true);
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+static ssize_t st_lsm6ds3_sysfs_set_max_delivery_rate(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	u8 duration;
+	int err, err2;
+	unsigned int max_delivery_rate;
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	err = kstrtouint(buf, 10, &max_delivery_rate);
+	if (err < 0)
+		return -EINVAL;
+
+	if (max_delivery_rate == sdata->c_odr)
+		return size;
+
+	duration = max_delivery_rate / ST_LSM6DS3_MIN_DURATION_MS;
+
+	mutex_lock(&sdata->cdata->bank_registers_lock);
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR,
+					ST_LSM6DS3_FUNC_CFG_REG2_MASK,
+					ST_LSM6DS3_EN_BIT, false);
+	if (err < 0)
+		goto st_lsm6ds3_sysfs_set_max_delivery_rate_mutex_unlock;
+
+	err = sdata->cdata->tf->write(sdata->cdata,
+					ST_LSM6DS3_STEP_COUNTER_DURATION_ADDR,
+					1, &duration, false);
+	if (err < 0)
+		goto st_lsm6ds3_sysfs_set_max_delivery_rate_restore_bank;
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR,
+					ST_LSM6DS3_FUNC_CFG_REG2_MASK,
+					ST_LSM6DS3_DIS_BIT, false);
+	if (err < 0)
+		goto st_lsm6ds3_sysfs_set_max_delivery_rate_restore_bank;
+
+	mutex_unlock(&sdata->cdata->bank_registers_lock);
+
+	sdata->c_odr = max_delivery_rate;
+
+	return size;
+
+st_lsm6ds3_sysfs_set_max_delivery_rate_restore_bank:
+	do {
+		err2 = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR,
+					ST_LSM6DS3_FUNC_CFG_REG2_MASK,
+					ST_LSM6DS3_DIS_BIT, false);
+
+		msleep(500);
+	} while (err2 < 0);
+
+st_lsm6ds3_sysfs_set_max_delivery_rate_mutex_unlock:
+	mutex_unlock(&sdata->cdata->bank_registers_lock);
+	return err;
+}
+
+static ssize_t st_lsm6ds3_sysfs_get_max_delivery_rate(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
+
+	return sprintf(buf, "%d\n", sdata->c_odr);
+}
+
+static ssize_t st_lsm6ds3_sysfs_reset_counter(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
+
+	err = st_lsm6ds3_reset_steps(sdata->cdata);
+	if (err < 0)
+		return err;
+
+	return size;
+}
+
+static ssize_t st_lsm6ds3_sysfs_get_sampling_frequency(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
+
+	return sprintf(buf, "%d\n", sdata->c_odr);
+}
+
+static ssize_t st_lsm6ds3_sysfs_set_sampling_frequency(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	int err;
+	unsigned int odr;
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	err = kstrtoint(buf, 10, &odr);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&indio_dev->mlock);
+
+	if (sdata->c_odr != odr)
+		err = st_lsm6ds3_set_odr(sdata, odr);
+
+	mutex_unlock(&indio_dev->mlock);
+
+	return err < 0 ? err : size;
+}
+
+static ssize_t st_lsm6ds3_sysfs_sampling_frequency_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int i, len = 0;
+
+	for (i = 0; i < ST_LSM6DS3_ODR_LIST_NUM; i++) {
+		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
+					st_lsm6ds3_odr_table.odr_avl[i].hz);
+	}
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static ssize_t st_lsm6ds3_sysfs_scale_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	int i, len = 0;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
+
+	for (i = 0; i < ST_LSM6DS3_FS_LIST_NUM; i++) {
+		len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
+			st_lsm6ds3_fs_table[sdata->sindex].fs_avl[i].gain);
+	}
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static ssize_t st_lsm6ds3_sysfs_get_selftest_available(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%s, %s, %s\n",
+				st_lsm6ds3_selftest_table[0].string_mode,
+				st_lsm6ds3_selftest_table[1].string_mode,
+				st_lsm6ds3_selftest_table[2].string_mode);
+}
+
+static ssize_t st_lsm6ds3_sysfs_get_selftest_status(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	u8 index;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
+
+	switch (sdata->sindex) {
+	case ST_INDIO_DEV_ACCEL:
+		index = sdata->cdata->accel_selftest_status;
+		break;
+	case ST_INDIO_DEV_GYRO:
+		index = sdata->cdata->gyro_selftest_status;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return sprintf(buf, "%s\n",
+				st_lsm6ds3_selftest_table[index].string_mode);
+}
+
+static ssize_t st_lsm6ds3_sysfs_set_selftest_status(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	int err, i;
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	for (i = 0; i < ARRAY_SIZE(st_lsm6ds3_selftest_table); i++) {
+		if (strncmp(buf, st_lsm6ds3_selftest_table[i].string_mode,
+								size - 2) == 0)
+			break;
+	}
+	if (i == ARRAY_SIZE(st_lsm6ds3_selftest_table))
+		return -EINVAL;
+
+	err = st_lsm6ds3_set_selftest(sdata, i);
+	if (err < 0)
+		return err;
+
+	switch (sdata->sindex) {
+	case ST_INDIO_DEV_ACCEL:
+		sdata->cdata->accel_selftest_status = i;
+		break;
+	case ST_INDIO_DEV_GYRO:
+		sdata->cdata->gyro_selftest_status = i;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return size;
+}
+
+ssize_t st_lsm6ds3_sysfs_get_hw_fifo_lenght(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", ST_LSM6DS3_MAX_FIFO_LENGHT);
+}
+
+ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	disable_irq(sdata->cdata->irq);
+	st_lsm6ds3_flush_works();
+
+	mutex_lock(&sdata->cdata->fifo_lock);
+
+	st_lsm6ds3_read_fifo(sdata->cdata, true);
+
+	mutex_unlock(&sdata->cdata->fifo_lock);
+
+	enable_irq(sdata->cdata->irq);
+
+	return size;
+}
+
+static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ();
+static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ_AVAIL();
+static ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available);
+static ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(in_anglvel_scale_available);
+static ST_LSM6DS3_FIFO_LENGHT();
+static ST_LSM6DS3_FIFO_FLUSH();
+
+static IIO_DEVICE_ATTR(reset_counter, S_IWUSR,
+				NULL, st_lsm6ds3_sysfs_reset_counter, 0);
+
+static IIO_DEVICE_ATTR(max_delivery_rate, S_IWUSR | S_IRUGO,
+				st_lsm6ds3_sysfs_get_max_delivery_rate,
+				st_lsm6ds3_sysfs_set_max_delivery_rate, 0);
+
+static IIO_DEVICE_ATTR(self_test_available, S_IRUGO,
+				st_lsm6ds3_sysfs_get_selftest_available,
+				NULL, 0);
+
+static IIO_DEVICE_ATTR(self_test, S_IWUSR | S_IRUGO,
+				st_lsm6ds3_sysfs_get_selftest_status,
+				st_lsm6ds3_sysfs_set_selftest_status, 0);
+
+static struct attribute *st_lsm6ds3_accel_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	&iio_dev_attr_self_test_available.dev_attr.attr,
+	&iio_dev_attr_self_test.dev_attr.attr,
+	&iio_dev_attr_hw_fifo_lenght.dev_attr.attr,
+	&iio_dev_attr_flush.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_accel_attribute_group = {
+	.attrs = st_lsm6ds3_accel_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_accel_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_accel_attribute_group,
+	.read_raw = &st_lsm6ds3_read_raw,
+	.write_raw = &st_lsm6ds3_write_raw,
+};
+
+static struct attribute *st_lsm6ds3_gyro_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	&iio_dev_attr_self_test_available.dev_attr.attr,
+	&iio_dev_attr_self_test.dev_attr.attr,
+	&iio_dev_attr_hw_fifo_lenght.dev_attr.attr,
+	&iio_dev_attr_flush.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_gyro_attribute_group = {
+	.attrs = st_lsm6ds3_gyro_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_gyro_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_gyro_attribute_group,
+	.read_raw = &st_lsm6ds3_read_raw,
+	.write_raw = &st_lsm6ds3_write_raw,
+};
+
+static struct attribute *st_lsm6ds3_sign_motion_attributes[] = {
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_sign_motion_attribute_group = {
+	.attrs = st_lsm6ds3_sign_motion_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_sign_motion_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_sign_motion_attribute_group,
+};
+
+static struct attribute *st_lsm6ds3_step_c_attributes[] = {
+	&iio_dev_attr_reset_counter.dev_attr.attr,
+	&iio_dev_attr_max_delivery_rate.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_step_c_attribute_group = {
+	.attrs = st_lsm6ds3_step_c_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_step_c_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_step_c_attribute_group,
+	.read_raw = &st_lsm6ds3_read_raw,
+};
+
+static struct attribute *st_lsm6ds3_step_d_attributes[] = {
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_step_d_attribute_group = {
+	.attrs = st_lsm6ds3_step_d_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_step_d_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_step_d_attribute_group,
+};
+
+static struct attribute *st_lsm6ds3_tilt_attributes[] = {
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_tilt_attribute_group = {
+	.attrs = st_lsm6ds3_tilt_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_tilt_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_tilt_attribute_group,
+};
+
+#ifdef CONFIG_IIO_TRIGGER
+static const struct iio_trigger_ops st_lsm6ds3_trigger_ops = {
+	.owner = THIS_MODULE,
+	.set_trigger_state = ST_LSM6DS3_TRIGGER_SET_STATE,
+};
+#define ST_LSM6DS3_TRIGGER_OPS (&st_lsm6ds3_trigger_ops)
+#else
+#define ST_LSM6DS3_TRIGGER_OPS NULL
+#endif
+
+int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
+{
+	u8 wai = 0x00;
+	int i, n, err;
+	struct lsm6ds3_sensor_data *sdata;
+
+	mutex_init(&cdata->bank_registers_lock);
+	mutex_init(&cdata->fifo_lock);
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
+	mutex_init(&cdata->passthrough_lock);
+#endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
+
+	cdata->fifo_data = 0;
+
+	err = cdata->tf->read(cdata, ST_LSM6DS3_WAI_ADDRESS, 1, &wai, true);
+	if (err < 0) {
+		dev_err(cdata->dev, "failed to read Who-Am-I register.\n");
+		return err;
+	}
+	if (wai != ST_LSM6DS3_WAI_EXP) {
+		dev_err(cdata->dev, "Who-Am-I value not valid.\n");
+		return -ENODEV;
+	}
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
+		cdata->indio_dev[i] = iio_device_alloc(sizeof(*sdata));
+		if (cdata->indio_dev[i] == NULL) {
+			err = -ENOMEM;
+			goto iio_device_free;
+		}
+		sdata = iio_priv(cdata->indio_dev[i]);
+		sdata->cdata = cdata;
+		sdata->sindex = i;
+
+		if ((i == ST_INDIO_DEV_ACCEL) || (i == ST_INDIO_DEV_GYRO)) {
+			sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[0].hz;
+			sdata->c_gain[0] =
+					st_lsm6ds3_fs_table[i].fs_avl[0].gain;
+		}
+		cdata->indio_dev[i]->modes = INDIO_DIRECT_MODE;
+	}
+
+	if (irq > 0) {
+		cdata->irq = irq;
+		dev_info(cdata->dev, "driver use DRDY int pin 1\n");
+	}
+
+	cdata->indio_dev[ST_INDIO_DEV_ACCEL]->name =
+		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,
+					ST_LSM6DS3_ACCEL_SUFFIX_NAME);
+	cdata->indio_dev[ST_INDIO_DEV_ACCEL]->info = &st_lsm6ds3_accel_info;
+	cdata->indio_dev[ST_INDIO_DEV_ACCEL]->channels = st_lsm6ds3_accel_ch;
+	cdata->indio_dev[ST_INDIO_DEV_ACCEL]->num_channels =
+						ARRAY_SIZE(st_lsm6ds3_accel_ch);
+
+	cdata->indio_dev[ST_INDIO_DEV_GYRO]->name =
+		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,
+					ST_LSM6DS3_GYRO_SUFFIX_NAME);
+	cdata->indio_dev[ST_INDIO_DEV_GYRO]->info = &st_lsm6ds3_gyro_info;
+	cdata->indio_dev[ST_INDIO_DEV_GYRO]->channels = st_lsm6ds3_gyro_ch;
+	cdata->indio_dev[ST_INDIO_DEV_GYRO]->num_channels =
+						ARRAY_SIZE(st_lsm6ds3_gyro_ch);
+
+	cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->name =
+		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,
+					ST_LSM6DS3_SIGN_MOTION_SUFFIX_NAME);
+	cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->info =
+						&st_lsm6ds3_sign_motion_info;
+	cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->channels =
+						st_lsm6ds3_sign_motion_ch;
+	cdata->indio_dev[ST_INDIO_DEV_SIGN_MOTION]->num_channels =
+					ARRAY_SIZE(st_lsm6ds3_sign_motion_ch);
+
+	cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->name =
+		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,
+					ST_LSM6DS3_STEP_COUNTER_SUFFIX_NAME);
+	cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->info =
+						&st_lsm6ds3_step_c_info;
+	cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->channels =
+						st_lsm6ds3_step_c_ch;
+	cdata->indio_dev[ST_INDIO_DEV_STEP_COUNTER]->num_channels =
+					ARRAY_SIZE(st_lsm6ds3_step_c_ch);
+
+	cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->name =
+		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,
+					ST_LSM6DS3_STEP_DETECTOR_SUFFIX_NAME);
+	cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->info =
+						&st_lsm6ds3_step_d_info;
+	cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->channels =
+						st_lsm6ds3_step_d_ch;
+	cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR]->num_channels =
+					ARRAY_SIZE(st_lsm6ds3_step_d_ch);
+
+	cdata->indio_dev[ST_INDIO_DEV_TILT]->name =
+		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,
+					ST_LSM6DS3_TILT_SUFFIX_NAME);
+	cdata->indio_dev[ST_INDIO_DEV_TILT]->info = &st_lsm6ds3_tilt_info;
+	cdata->indio_dev[ST_INDIO_DEV_TILT]->channels = st_lsm6ds3_tilt_ch;
+	cdata->indio_dev[ST_INDIO_DEV_TILT]->num_channels =
+					ARRAY_SIZE(st_lsm6ds3_tilt_ch);
+
+	err = st_lsm6ds3_init_sensor(cdata);
+	if (err < 0)
+		goto iio_device_free;
+
+	err = st_lsm6ds3_allocate_rings(cdata);
+	if (err < 0)
+		goto iio_device_free;
+
+	if (irq > 0) {
+		err = st_lsm6ds3_allocate_triggers(cdata,
+							ST_LSM6DS3_TRIGGER_OPS);
+		if (err < 0)
+			goto deallocate_ring;
+	}
+
+	for (n = 0; n < ST_INDIO_DEV_NUM; n++) {
+		err = iio_device_register(cdata->indio_dev[n]);
+		if (err)
+			goto iio_device_unregister_and_trigger_deallocate;
+	}
+
+	err = st_lsm6ds3_i2c_master_probe(cdata);
+	if (err < 0)
+		goto iio_device_unregister_and_trigger_deallocate;
+
+	device_init_wakeup(cdata->dev, true);
+
+	return 0;
+
+iio_device_unregister_and_trigger_deallocate:
+	for (n--; n >= 0; n--)
+		iio_device_unregister(cdata->indio_dev[n]);
+
+	if (irq > 0)
+		st_lsm6ds3_deallocate_triggers(cdata);
+deallocate_ring:
+	st_lsm6ds3_deallocate_rings(cdata);
+iio_device_free:
+	for (i--; i >= 0; i--)
+		iio_device_free(cdata->indio_dev[i]);
+
+	return err;
+}
+EXPORT_SYMBOL(st_lsm6ds3_common_probe);
+
+void st_lsm6ds3_common_remove(struct lsm6ds3_data *cdata, int irq)
+{
+	int i;
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++)
+		iio_device_unregister(cdata->indio_dev[i]);
+
+	if (irq > 0)
+		st_lsm6ds3_deallocate_triggers(cdata);
+
+	st_lsm6ds3_deallocate_rings(cdata);
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++)
+		iio_device_free(cdata->indio_dev[i]);
+
+	st_lsm6ds3_i2c_master_exit(cdata);
+}
+EXPORT_SYMBOL(st_lsm6ds3_common_remove);
+
+#ifdef CONFIG_PM
+int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata)
+{
+#ifndef CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP
+	int err, i;
+	u8 tmp_sensors_enabled;
+	struct lsm6ds3_sensor_data *sdata;
+
+	tmp_sensors_enabled = cdata->sensors_enabled;
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
+		if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT))
+			continue;
+
+		sdata = iio_priv(cdata->indio_dev[i]);
+
+		err = st_lsm6ds3_set_enable(sdata, false);
+		if (err < 0)
+			return err;
+	}
+	cdata->sensors_enabled = tmp_sensors_enabled;
+#endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */
+
+	if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) {
+		if (device_may_wakeup(cdata->dev))
+			enable_irq_wake(cdata->irq);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(st_lsm6ds3_common_suspend);
+
+int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
+{
+#ifndef CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP
+	int err, i;
+	struct lsm6ds3_sensor_data *sdata;
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
+		if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT))
+			continue;
+
+		sdata = iio_priv(cdata->indio_dev[i]);
+
+		if ((1 << sdata->sindex) & cdata->sensors_enabled) {
+			err = st_lsm6ds3_set_enable(sdata, true);
+			if (err < 0)
+				return err;
+		}
+	}
+#endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */
+
+	if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) {
+		if (device_may_wakeup(cdata->dev))
+			disable_irq_wake(cdata->irq);
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(st_lsm6ds3_common_resume);
+#endif /* CONFIG_PM */
+
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 core driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
new file mode 100644
index 00000000000..436f09e0060
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
@@ -0,0 +1,159 @@
+/*
+ * STMicroelectronics lsm6ds3 i2c driver
+ *
+ * Copyright 2014 STMicroelectronics Inc.
+ *
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+
+#include "st_lsm6ds3.h"
+
+static int st_lsm6ds3_i2c_read(struct lsm6ds3_data *cdata,
+				u8 reg_addr, int len, u8 *data, bool b_lock)
+{
+	int err = 0;
+	struct i2c_msg msg[2];
+	struct i2c_client *client = to_i2c_client(cdata->dev);
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].len = 1;
+	msg[0].buf = &reg_addr;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].len = len;
+	msg[1].buf = data;
+
+	if (b_lock) {
+		mutex_lock(&cdata->bank_registers_lock);
+		err = i2c_transfer(client->adapter, msg, 2);
+		mutex_unlock(&cdata->bank_registers_lock);
+	} else
+		err = i2c_transfer(client->adapter, msg, 2);
+
+	return err;
+}
+
+static int st_lsm6ds3_i2c_write(struct lsm6ds3_data *cdata,
+				u8 reg_addr, int len, u8 *data, bool b_lock)
+{
+	int err = 0;
+	u8 send[len + 1];
+	struct i2c_msg msg;
+	struct i2c_client *client = to_i2c_client(cdata->dev);
+
+	send[0] = reg_addr;
+	memcpy(&send[1], data, len * sizeof(u8));
+	len++;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.len = len;
+	msg.buf = send;
+
+	if (b_lock) {
+		mutex_lock(&cdata->bank_registers_lock);
+		err = i2c_transfer(client->adapter, &msg, 1);
+		mutex_unlock(&cdata->bank_registers_lock);
+	} else
+		err = i2c_transfer(client->adapter, &msg, 1);
+
+	return err;
+}
+
+static const struct st_lsm6ds3_transfer_function st_lsm6ds3_tf_i2c = {
+	.write = st_lsm6ds3_i2c_write,
+	.read = st_lsm6ds3_i2c_read,
+};
+
+static int st_lsm6ds3_i2c_probe(struct i2c_client *client,
+						const struct i2c_device_id *id)
+{
+	int err;
+	struct lsm6ds3_data *cdata;
+
+	cdata = kmalloc(sizeof(*cdata), GFP_KERNEL);
+	if (!cdata)
+		return -ENOMEM;
+
+	cdata->dev = &client->dev;
+	cdata->name = client->name;
+	i2c_set_clientdata(client, cdata);
+
+	cdata->tf = &st_lsm6ds3_tf_i2c;
+
+	err = st_lsm6ds3_common_probe(cdata, client->irq);
+	if (err < 0)
+		goto free_data;
+
+	return 0;
+
+free_data:
+	kfree(cdata);
+	return err;
+}
+
+static int st_lsm6ds3_i2c_remove(struct i2c_client *client)
+{
+	struct lsm6ds3_data *cdata = i2c_get_clientdata(client);
+
+	st_lsm6ds3_common_remove(cdata, client->irq);
+	kfree(cdata);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int st_lsm6ds3_suspend(struct device *dev)
+{
+	struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
+
+	return st_lsm6ds3_common_suspend(cdata);
+}
+
+static int st_lsm6ds3_resume(struct device *dev)
+{
+	struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
+
+	return st_lsm6ds3_common_resume(cdata);
+}
+
+static const struct dev_pm_ops st_lsm6ds3_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume)
+};
+
+#define ST_LSM6DS3_PM_OPS		(&st_lsm6ds3_pm_ops)
+#else /* CONFIG_PM */
+#define ST_LSM6DS3_PM_OPS		NULL
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id st_lsm6ds3_id_table[] = {
+	{ LSM6DS3_DEV_NAME },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, st_lsm6ds3_id_table);
+
+static struct i2c_driver st_lsm6ds3_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "st-lsm6ds3-i2c",
+		.pm = ST_LSM6DS3_PM_OPS,
+	},
+	.probe = st_lsm6ds3_i2c_probe,
+	.remove = st_lsm6ds3_i2c_remove,
+	.id_table = st_lsm6ds3_id_table,
+};
+module_i2c_driver(st_lsm6ds3_driver);
+
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 i2c driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c
new file mode 100644
index 00000000000..49e0a08c8b5
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c_master.c
@@ -0,0 +1,1387 @@
+/*
+ * STMicroelectronics lsm6ds3 i2c master driver
+ *
+ * Copyright 2014 STMicroelectronics Inc.
+ *
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <asm/unaligned.h>
+
+#include "st_lsm6ds3.h"
+
+#define ST_LSM6DS3_EXT0_INDEX			0
+#define ST_LSM6DS3_EXT1_INDEX			1
+
+#define ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM	4
+#define ST_LSM6DS3_EN_BIT			0x01
+#define ST_LSM6DS3_DIS_BIT			0x00
+#define ST_LSM6DS3_HUB_REG1_ADDR		0x2e
+#define ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR		0x01
+#define ST_LSM6DS3_FUNC_CFG_REG2_MASK		0x80
+#define ST_LSM6DS3_SLV0_ADDR_ADDR		0x02
+#define ST_LSM6DS3_SLV0_SUBADDR_ADDR		0x03
+#define ST_LSM6DS3_SLV0_CONFIG_ADDR		0x04
+#define ST_LSM6DS3_SLV0_CONFIG_MASK		0x07
+#define ST_LSM6DS3_SLV1_ADDR_ADDR		0x05
+#define ST_LSM6DS3_SLV1_SUBADDR_ADDR		0x06
+#define ST_LSM6DS3_SLV1_CONFIG_ADDR		0x07
+#define ST_LSM6DS3_SLV1_CONFIG_MASK		0x07
+#define ST_LSM6DS3_SLV2_ADDR_ADDR		0x08
+#define ST_LSM6DS3_SLV2_SUBADDR_ADDR		0x09
+#define ST_LSM6DS3_SLV2_CONFIG_ADDR		0x0a
+#define ST_LSM6DS3_SLV2_CONFIG_MASK		0x07
+#define ST_LSM6DS3_SLV_AUX_ADDR			0x04
+#define ST_LSM6DS3_SLV_AUX_MASK			0x30
+#define ST_LSM6DS3_SLV_AUX_1			0x00
+#define ST_LSM6DS3_SLV_AUX_2			0x01
+#define ST_LSM6DS3_SLV_AUX_3			0x02
+
+/* External sensors configuration */
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL
+static int lis3mdl_initialization(struct lsm6ds3_sensor_data *sdata,
+								int ext_num);
+
+#define ST_LSM6DS3_EXT0_ADDR			0x1e
+#define ST_LSM6DS3_EXT0_WAI_ADDR		0x0f
+#define ST_LSM6DS3_EXT0_WAI_VALUE		0x3d
+#define ST_LSM6DS3_EXT0_RESET_ADDR		0x21
+#define ST_LSM6DS3_EXT0_RESET_MASK		0x04
+#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR		0x21
+#define ST_LSM6DS3_EXT0_FULLSCALE_MASK		0x60
+#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE		0x02
+#define ST_LSM6DS3_EXT0_ODR_ADDR		0x20
+#define ST_LSM6DS3_EXT0_ODR_MASK		0x1c
+#define ST_LSM6DS3_EXT0_ODR0_HZ			10
+#define ST_LSM6DS3_EXT0_ODR0_VALUE		0x04
+#define ST_LSM6DS3_EXT0_ODR1_HZ			20
+#define ST_LSM6DS3_EXT0_ODR1_VALUE		0x05
+#define ST_LSM6DS3_EXT0_ODR2_HZ			40
+#define ST_LSM6DS3_EXT0_ODR2_VALUE		0x06
+#define ST_LSM6DS3_EXT0_ODR3_HZ			80
+#define ST_LSM6DS3_EXT0_ODR3_VALUE		0x07
+#define ST_LSM6DS3_EXT0_PW_ADDR			0x22
+#define ST_LSM6DS3_EXT0_PW_MASK			0x03
+#define ST_LSM6DS3_EXT0_PW_OFF			0x02
+#define ST_LSM6DS3_EXT0_PW_ON			0x00
+#define ST_LSM6DS3_EXT0_GAIN_VALUE		438
+#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR		0x28
+#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR		0x2a
+#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR		0x2c
+#define ST_LSM6DS3_EXT0_READ_DATA_LEN		6
+#define ST_LSM6DS3_EXT0_BDU_ADDR		0x24
+#define ST_LSM6DS3_EXT0_BDU_MASK		0x40
+#define ST_LSM6DS3_EXT0_STD			3
+#define ST_LSM6DS3_EXT0_BOOT_FUNCTION		(&lis3mdl_initialization)
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912
+static int akm09912_initialization(struct lsm6ds3_sensor_data *sdata,
+								int ext_num);
+
+#define ST_LSM6DS3_EXT0_ADDR			0x0c
+#define ST_LSM6DS3_EXT0_WAI_ADDR		0x01
+#define ST_LSM6DS3_EXT0_WAI_VALUE		0x04
+#define ST_LSM6DS3_EXT0_RESET_ADDR		0x32
+#define ST_LSM6DS3_EXT0_RESET_MASK		0x01
+#define ST_LSM6DS3_EXT0_FULLSCALE_ADDR		0x00
+#define ST_LSM6DS3_EXT0_FULLSCALE_MASK		0x00
+#define ST_LSM6DS3_EXT0_FULLSCALE_VALUE		0x00
+#define ST_LSM6DS3_EXT0_ODR_ADDR		0x31
+#define ST_LSM6DS3_EXT0_ODR_MASK		0x1f
+#define ST_LSM6DS3_EXT0_ODR0_HZ			10
+#define ST_LSM6DS3_EXT0_ODR0_VALUE		0x02
+#define ST_LSM6DS3_EXT0_ODR1_HZ			20
+#define ST_LSM6DS3_EXT0_ODR1_VALUE		0x04
+#define ST_LSM6DS3_EXT0_ODR2_HZ			50
+#define ST_LSM6DS3_EXT0_ODR2_VALUE		0x06
+#define ST_LSM6DS3_EXT0_ODR3_HZ			100
+#define ST_LSM6DS3_EXT0_ODR3_VALUE		0x08
+#define ST_LSM6DS3_EXT0_PW_ADDR			ST_LSM6DS3_EXT0_ODR_ADDR
+#define ST_LSM6DS3_EXT0_PW_MASK			ST_LSM6DS3_EXT0_ODR_MASK
+#define ST_LSM6DS3_EXT0_PW_OFF			0x00
+#define ST_LSM6DS3_EXT0_PW_ON			ST_LSM6DS3_EXT0_ODR0_VALUE
+#define ST_LSM6DS3_EXT0_GAIN_VALUE		1500
+#define ST_LSM6DS3_EXT0_OUT_X_L_ADDR		0x11
+#define ST_LSM6DS3_EXT0_OUT_Y_L_ADDR		0x13
+#define ST_LSM6DS3_EXT0_OUT_Z_L_ADDR		0x15
+#define ST_LSM6DS3_EXT0_READ_DATA_LEN		6
+#define ST_LSM6DS3_EXT0_SENSITIVITY_ADDR	0x60
+#define ST_LSM6DS3_EXT0_SENSITIVITY_LEN		3
+#define ST_LSM6DS3_EXT0_STD			3
+#define ST_LSM6DS3_EXT0_BOOT_FUNCTION		(&akm09912_initialization)
+#define ST_LSM6DS3_EXT0_DATA_STATUS		0x18
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB
+static int lps22hb_initialization(struct lsm6ds3_sensor_data *sdata,
+								int ext_num);
+
+#define ST_LSM6DS3_EXT1_ADDR			0x5d
+#define ST_LSM6DS3_EXT1_WAI_ADDR		0x0f
+#define ST_LSM6DS3_EXT1_WAI_VALUE		0xb1
+#define ST_LSM6DS3_EXT1_RESET_ADDR		0x11
+#define ST_LSM6DS3_EXT1_RESET_MASK		0x80
+#define ST_LSM6DS3_EXT1_FULLSCALE_ADDR		0x00
+#define ST_LSM6DS3_EXT1_FULLSCALE_MASK		0x00
+#define ST_LSM6DS3_EXT1_FULLSCALE_VALUE		0x00
+#define ST_LSM6DS3_EXT1_ODR_ADDR		0x10
+#define ST_LSM6DS3_EXT1_ODR_MASK		0x70
+#define ST_LSM6DS3_EXT1_ODR0_HZ			1
+#define ST_LSM6DS3_EXT1_ODR0_VALUE		0x01
+#define ST_LSM6DS3_EXT1_ODR1_HZ			10
+#define ST_LSM6DS3_EXT1_ODR1_VALUE		0x02
+#define ST_LSM6DS3_EXT1_ODR2_HZ			25
+#define ST_LSM6DS3_EXT1_ODR2_VALUE		0x03
+#define ST_LSM6DS3_EXT1_ODR3_HZ			50
+#define ST_LSM6DS3_EXT1_ODR3_VALUE		0x04
+#define ST_LSM6DS3_EXT1_PW_ADDR			ST_LSM6DS3_EXT1_ODR_ADDR
+#define ST_LSM6DS3_EXT1_PW_MASK			ST_LSM6DS3_EXT1_ODR_MASK
+#define ST_LSM6DS3_EXT1_PW_OFF			0x00
+#define ST_LSM6DS3_EXT1_PW_ON			ST_LSM6DS3_EXT1_ODR0_VALUE
+#define ST_LSM6DS3_EXT1_GAIN_VALUE		244
+#define ST_LSM6DS3_EXT1_OUT_P_L_ADDR		0x28
+#define ST_LSM6DS3_EXT1_OUT_T_L_ADDR		0x2b
+#define ST_LSM6DS3_EXT1_READ_DATA_LEN		5
+#define ST_LSM6DS3_EXT1_BDU_ADDR		0x10
+#define ST_LSM6DS3_EXT1_BDU_MASK		0x02
+#define ST_LSM6DS3_EXT1_STD			1
+#define ST_LSM6DS3_EXT1_BOOT_FUNCTION		(&lps22hb_initialization)
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB */
+
+/* SENSORS SUFFIX NAMES */
+#define ST_LSM6DS3_EXT0_SUFFIX_NAME		"magn"
+#define ST_LSM6DS3_EXT1_SUFFIX_NAME		"press"
+
+struct st_lsm6ds3_i2c_master_odr_reg {
+	unsigned int hz;
+	u8 value;
+};
+
+struct st_lsm6ds3_i2c_master_odr_table {
+	u8 addr;
+	u8 mask;
+	struct st_lsm6ds3_i2c_master_odr_reg odr_avl[ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM];
+};
+
+static int st_lsm6ds3_i2c_master_read_raw(struct iio_dev *indio_dev,
+		struct iio_chan_spec const *ch, int *val, int *val2, long mask);
+
+static const struct iio_chan_spec st_lsm6ds3_ext0_ch[] = {
+	ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 0, IIO_MOD_X, IIO_LE,
+				16, 16, ST_LSM6DS3_EXT0_OUT_X_L_ADDR, 's'),
+	ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 1, IIO_MOD_Y, IIO_LE,
+				16, 16, ST_LSM6DS3_EXT0_OUT_Y_L_ADDR, 's'),
+	ST_LSM6DS3_LSM_CHANNELS(IIO_MAGN, 1, 2, IIO_MOD_Z, IIO_LE,
+				16, 16, ST_LSM6DS3_EXT0_OUT_Z_L_ADDR, 's'),
+	IIO_CHAN_SOFT_TIMESTAMP(3)
+};
+
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+static const struct iio_chan_spec st_lsm6ds3_ext1_ch[] = {
+	ST_LSM6DS3_LSM_CHANNELS(IIO_PRESSURE, 0, 0, IIO_NO_MOD, IIO_LE,
+				24, 24, ST_LSM6DS3_EXT1_OUT_P_L_ADDR, 'u'),
+	ST_LSM6DS3_LSM_CHANNELS(IIO_TEMP, 0, 1, IIO_NO_MOD, IIO_LE,
+				16, 16, ST_LSM6DS3_EXT1_OUT_T_L_ADDR, 's'),
+	IIO_CHAN_SOFT_TIMESTAMP(2)
+};
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+
+static int st_lsm6ds3_i2c_master_set_odr(struct lsm6ds3_sensor_data *sdata,
+							unsigned int odr);
+
+static ssize_t st_lsm6ds3_i2c_master_sysfs_sampling_frequency_avail(
+		struct device *dev, struct device_attribute *attr, char *buf)
+{
+	return scnprintf(buf, PAGE_SIZE,
+				"%d %d %d %d %d\n", 26, 52, 104, 208, 416);
+}
+
+static ssize_t st_lsm6ds3_i2c_master_sysfs_get_sampling_frequency(
+		struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
+
+	return sprintf(buf, "%d\n", sdata->c_odr);
+}
+
+static ssize_t st_lsm6ds3_i2c_master_sysfs_set_sampling_frequency(
+			struct device *dev, struct device_attribute *attr,
+						const char *buf, size_t size)
+{
+	int err, err2 = -EINVAL;
+	unsigned int odr;
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	err = kstrtoint(buf, 10, &odr);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&indio_dev->mlock);
+
+	switch (odr) {
+	case 26:
+	case 52:
+	case 104:
+	case 208:
+	case 416:
+		if (sdata->c_odr != odr) {
+
+			mutex_lock(&sdata->cdata->passthrough_lock);
+
+			disable_irq(sdata->cdata->irq);
+			st_lsm6ds3_flush_works();
+
+			err = st_lsm6ds3_enable_passthrough(sdata->cdata, true);
+			if (err < 0) {
+				enable_irq(sdata->cdata->irq);
+				mutex_unlock(&sdata->cdata->passthrough_lock);
+				mutex_unlock(&indio_dev->mlock);
+				return err;
+			}
+
+			err2 = st_lsm6ds3_i2c_master_set_odr(sdata, odr);
+
+			err = st_lsm6ds3_enable_passthrough(sdata->cdata,
+									false);
+			if (err < 0) {
+				enable_irq(sdata->cdata->irq);
+				mutex_unlock(&sdata->cdata->passthrough_lock);
+				mutex_unlock(&indio_dev->mlock);
+				return err;
+			}
+
+			enable_irq(sdata->cdata->irq);
+			mutex_unlock(&sdata->cdata->passthrough_lock);
+		}
+		break;
+	default:
+		err2 = -EINVAL;
+		break;
+	}
+
+	mutex_unlock(&indio_dev->mlock);
+
+	return err2 < 0 ? err2 : size;
+}
+
+static IIO_DEV_ATTR_SAMP_FREQ(S_IWUSR | S_IRUGO,
+			st_lsm6ds3_i2c_master_sysfs_get_sampling_frequency,
+			st_lsm6ds3_i2c_master_sysfs_set_sampling_frequency);
+
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(
+			st_lsm6ds3_i2c_master_sysfs_sampling_frequency_avail);
+static ST_LSM6DS3_FIFO_LENGHT();
+static ST_LSM6DS3_FIFO_FLUSH();
+
+static struct attribute *st_lsm6ds3_ext0_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	&iio_dev_attr_hw_fifo_lenght.dev_attr.attr,
+	&iio_dev_attr_flush.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_ext0_attribute_group = {
+	.attrs = st_lsm6ds3_ext0_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_ext0_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_ext0_attribute_group,
+	.read_raw = &st_lsm6ds3_i2c_master_read_raw,
+};
+
+static struct attribute *st_lsm6ds3_ext1_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	&iio_dev_attr_hw_fifo_lenght.dev_attr.attr,
+	&iio_dev_attr_flush.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group st_lsm6ds3_ext1_attribute_group = {
+	.attrs = st_lsm6ds3_ext1_attributes,
+};
+
+static const struct iio_info st_lsm6ds3_ext1_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &st_lsm6ds3_ext1_attribute_group,
+	.read_raw = &st_lsm6ds3_i2c_master_read_raw,
+};
+
+struct st_lsm6ds3_iio_info_data {
+	char suffix_name[20];
+	struct iio_info *info;
+	struct iio_chan_spec *channels;
+	int num_channels;
+};
+
+struct st_lsm6ds3_reg {
+	u8 addr;
+	u8 mask;
+	u8 def_value;
+};
+
+struct st_lsm6ds3_power_reg {
+	u8 addr;
+	u8 mask;
+	u8 off_value;
+	u8 on_value;
+	bool isodr;
+};
+
+struct st_lsm6ds3_custom_function {
+	int (*boot_initialization) (struct lsm6ds3_sensor_data *sdata,
+								int ext_num);
+};
+
+static struct st_lsm6ds3_exs_list {
+	struct st_lsm6ds3_reg wai;
+	struct st_lsm6ds3_reg reset;
+	struct st_lsm6ds3_reg fullscale;
+	struct st_lsm6ds3_i2c_master_odr_table odr;
+	struct st_lsm6ds3_power_reg power;
+	u8 fullscale_value;
+	u8 samples_to_discard;
+	u8 read_data_len;
+	u8 num_data_channels;
+	bool available;
+	unsigned int gain;
+	struct i2c_board_info board_info;
+	struct st_lsm6ds3_iio_info_data data;
+	struct st_lsm6ds3_custom_function cf;
+} st_lsm6ds3_exs_list[] = {
+	{
+		.wai = {
+			.addr = ST_LSM6DS3_EXT0_WAI_ADDR,
+			.def_value = ST_LSM6DS3_EXT0_WAI_VALUE,
+		},
+		.reset = {
+			.addr = ST_LSM6DS3_EXT0_RESET_ADDR,
+			.mask = ST_LSM6DS3_EXT0_RESET_MASK,
+		},
+		.fullscale = {
+			.addr = ST_LSM6DS3_EXT0_FULLSCALE_ADDR,
+			.mask = ST_LSM6DS3_EXT0_FULLSCALE_MASK,
+			.def_value = ST_LSM6DS3_EXT0_FULLSCALE_VALUE,
+		},
+		.odr = {
+			.addr = ST_LSM6DS3_EXT0_ODR_ADDR,
+			.mask = ST_LSM6DS3_EXT0_ODR_MASK,
+			.odr_avl = {
+				{
+				.hz = ST_LSM6DS3_EXT0_ODR0_HZ,
+				.value = ST_LSM6DS3_EXT0_ODR0_VALUE,
+				},
+				{
+				.hz = ST_LSM6DS3_EXT0_ODR1_HZ,
+				.value = ST_LSM6DS3_EXT0_ODR1_VALUE,
+				},
+				{
+				.hz = ST_LSM6DS3_EXT0_ODR2_HZ,
+				.value = ST_LSM6DS3_EXT0_ODR2_VALUE,
+				},
+				{
+				.hz = ST_LSM6DS3_EXT0_ODR3_HZ,
+				.value = ST_LSM6DS3_EXT0_ODR3_VALUE,
+				},
+			},
+		},
+		.power = {
+			.addr = ST_LSM6DS3_EXT0_PW_ADDR,
+			.mask = ST_LSM6DS3_EXT0_PW_MASK,
+			.off_value = ST_LSM6DS3_EXT0_PW_OFF,
+			.on_value = ST_LSM6DS3_EXT0_PW_ON,
+		},
+		.samples_to_discard = ST_LSM6DS3_EXT0_STD,
+		.read_data_len = ST_LSM6DS3_EXT0_READ_DATA_LEN,
+		.num_data_channels = ARRAY_SIZE(st_lsm6ds3_ext0_ch) - 1,
+		.available = false,
+		.gain = ST_LSM6DS3_EXT0_GAIN_VALUE,
+		.board_info = { .addr = ST_LSM6DS3_EXT0_ADDR, },
+		.data = {
+			.suffix_name = ST_LSM6DS3_EXT0_SUFFIX_NAME,
+			.info = (struct iio_info *)&st_lsm6ds3_ext0_info,
+			.channels = (struct iio_chan_spec *)&st_lsm6ds3_ext0_ch,
+			.num_channels = ARRAY_SIZE(st_lsm6ds3_ext0_ch),
+		},
+		.cf.boot_initialization = ST_LSM6DS3_EXT0_BOOT_FUNCTION,
+	},
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+	{
+		.wai = {
+			.addr = ST_LSM6DS3_EXT1_WAI_ADDR,
+			.def_value = ST_LSM6DS3_EXT1_WAI_VALUE,
+		},
+		.reset = {
+			.addr = ST_LSM6DS3_EXT1_RESET_ADDR,
+			.mask = ST_LSM6DS3_EXT1_RESET_MASK,
+		},
+		.fullscale = {
+			.addr = ST_LSM6DS3_EXT1_FULLSCALE_ADDR,
+			.mask = ST_LSM6DS3_EXT1_FULLSCALE_MASK,
+			.def_value = ST_LSM6DS3_EXT1_FULLSCALE_VALUE,
+		},
+		.odr = {
+			.addr = ST_LSM6DS3_EXT1_ODR_ADDR,
+			.mask = ST_LSM6DS3_EXT1_ODR_MASK,
+			.odr_avl = {
+				{
+				.hz = ST_LSM6DS3_EXT1_ODR0_HZ,
+				.value = ST_LSM6DS3_EXT1_ODR0_VALUE,
+				},
+				{
+				.hz = ST_LSM6DS3_EXT1_ODR1_HZ,
+				.value = ST_LSM6DS3_EXT1_ODR1_VALUE,
+				},
+				{
+				.hz = ST_LSM6DS3_EXT1_ODR2_HZ,
+				.value = ST_LSM6DS3_EXT1_ODR2_VALUE,
+				},
+				{
+				.hz = ST_LSM6DS3_EXT1_ODR3_HZ,
+				.value = ST_LSM6DS3_EXT1_ODR3_VALUE,
+				},
+			},
+		},
+		.power = {
+			.addr = ST_LSM6DS3_EXT1_PW_ADDR,
+			.mask = ST_LSM6DS3_EXT1_PW_MASK,
+			.off_value = ST_LSM6DS3_EXT1_PW_OFF,
+			.on_value = ST_LSM6DS3_EXT1_PW_ON,
+		},
+		.samples_to_discard = ST_LSM6DS3_EXT1_STD,
+		.read_data_len = ST_LSM6DS3_EXT1_READ_DATA_LEN,
+		.num_data_channels = ARRAY_SIZE(st_lsm6ds3_ext1_ch) - 1,
+		.available = false,
+		.gain = ST_LSM6DS3_EXT1_GAIN_VALUE,
+		.board_info = { .addr = ST_LSM6DS3_EXT1_ADDR, },
+		.data = {
+			.suffix_name = ST_LSM6DS3_EXT1_SUFFIX_NAME,
+			.info = (struct iio_info *)&st_lsm6ds3_ext1_info,
+			.channels = (struct iio_chan_spec *)&st_lsm6ds3_ext1_ch,
+			.num_channels = ARRAY_SIZE(st_lsm6ds3_ext1_ch),
+		},
+		.cf.boot_initialization = ST_LSM6DS3_EXT1_BOOT_FUNCTION,
+	},
+#else /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+	{
+	},
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+};
+
+static int st_lsm6ds3_i2c_master_read(struct i2c_client *client,
+						u8 reg_addr, int len, u8 *data)
+{
+	struct i2c_msg msg[2];
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].len = 1;
+	msg[0].buf = &reg_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_lsm6ds3_i2c_master_write(struct i2c_client *client,
+					u8 reg_addr, int len, u8 *data)
+{
+	u8 send[len + 1];
+	struct i2c_msg msg;
+
+	send[0] = reg_addr;
+	memcpy(&send[1], data, len * sizeof(u8));
+	len++;
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.len = len;
+	msg.buf = send;
+
+	return i2c_transfer(client->adapter, &msg, 1);
+}
+
+static int st_lsm6ds3_i2c_master_write_data_with_mask(struct i2c_client *client,
+						u8 reg_addr, u8 mask, u8 data)
+{
+	int err;
+	u8 new_data = 0x00, old_data = 0x00;
+
+	err = st_lsm6ds3_i2c_master_read(client, reg_addr, 1, &old_data);
+	if (err < 0)
+		return err;
+
+	new_data = ((old_data & (~mask)) | ((data << __ffs(mask)) & mask));
+
+	if (new_data == old_data)
+		return 1;
+
+	return st_lsm6ds3_i2c_master_write(client, reg_addr, 1, &new_data);
+}
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL
+static int lis3mdl_initialization(struct lsm6ds3_sensor_data *sdata,
+								int ext_num)
+{
+
+	return st_lsm6ds3_i2c_master_write_data_with_mask(
+				sdata->cdata->master_client[ext_num],
+				ST_LSM6DS3_EXT0_BDU_ADDR,
+				ST_LSM6DS3_EXT0_BDU_MASK, ST_LSM6DS3_EN_BIT);
+}
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_LIS3MDL */
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912
+static int akm09912_initialization(struct lsm6ds3_sensor_data *sdata,
+								int ext_num)
+{
+	int err;
+	u8 data[ST_LSM6DS3_EXT0_SENSITIVITY_LEN];
+
+	err = st_lsm6ds3_i2c_master_read(sdata->cdata->master_client[ext_num],
+				ST_LSM6DS3_EXT0_SENSITIVITY_ADDR,
+				ST_LSM6DS3_EXT0_SENSITIVITY_LEN,
+				data);
+	if (err < 0)
+		return err;
+
+	sdata->c_gain[0] *= ((((data[0] - 128) * 1000) >> 8) + 1000);
+	sdata->c_gain[1] *= ((((data[1] - 128) * 1000) >> 8) + 1000);
+	sdata->c_gain[2] *= ((((data[2] - 128) * 1000) >> 8) + 1000);
+
+	return 0;
+}
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB
+static int lps22hb_initialization(struct lsm6ds3_sensor_data *sdata,
+								int ext_num)
+{
+
+	return st_lsm6ds3_i2c_master_write_data_with_mask(
+				sdata->cdata->master_client[ext_num],
+				ST_LSM6DS3_EXT1_BDU_ADDR,
+				ST_LSM6DS3_EXT1_BDU_MASK, ST_LSM6DS3_EN_BIT);
+}
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_LPS22HB */
+
+static int st_lsm6ds3_i2c_master_set_odr(struct lsm6ds3_sensor_data *sdata,
+							unsigned int odr)
+{
+	int err, i, ext_num = sdata->sindex - ST_INDIO_DEV_EXT0;
+
+	for (i = 0; i < ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM; i++) {
+		if (st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].hz >= odr)
+			break;
+	}
+	if (i == ST_LSM6DS3_I2C_MASTER_ODR_LIST_NUM)
+		i--;
+
+	if (sdata->cdata->sensors_enabled & (1 << sdata->sindex)) {
+		err = st_lsm6ds3_i2c_master_write_data_with_mask(
+			sdata->cdata->master_client[ext_num],
+			st_lsm6ds3_exs_list[ext_num].odr.addr,
+			st_lsm6ds3_exs_list[ext_num].odr.mask,
+			st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].value);
+		if (err < 0)
+			return err;
+
+		sdata->cdata->ext_samples_to_discard[ext_num] =
+				st_lsm6ds3_exs_list[ext_num].samples_to_discard;
+
+		sdata->c_odr = odr;
+
+		if (st_lsm6ds3_exs_list[ext_num].power.isodr)
+			st_lsm6ds3_exs_list[ext_num].power.on_value =
+				st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].value;
+
+		st_lsm6ds3_reconfigure_fifo(sdata->cdata, false);
+	} else {
+		sdata->c_odr = odr;
+
+		if (st_lsm6ds3_exs_list[ext_num].power.isodr)
+			st_lsm6ds3_exs_list[ext_num].power.on_value =
+				st_lsm6ds3_exs_list[ext_num].odr.odr_avl[i].value;
+	}
+
+	return 0;
+}
+
+static int st_lsm6ds3_i2c_master_set_enable(struct lsm6ds3_sensor_data *sdata,
+								bool enable)
+{
+	u8 reg_value;
+	int err, ext_num = sdata->sindex - ST_INDIO_DEV_EXT0;
+
+	if (enable)
+		reg_value = st_lsm6ds3_exs_list[ext_num].power.on_value;
+	else
+		reg_value = st_lsm6ds3_exs_list[ext_num].power.off_value;
+
+	err = st_lsm6ds3_i2c_master_write_data_with_mask(
+				sdata->cdata->master_client[ext_num],
+				st_lsm6ds3_exs_list[ext_num].power.addr,
+				st_lsm6ds3_exs_list[ext_num].power.mask,
+				reg_value);
+	if (err < 0)
+		return err;
+
+	sdata->cdata->ext_samples_to_discard[ext_num] =
+				st_lsm6ds3_exs_list[ext_num].samples_to_discard;
+
+	if (enable)
+		sdata->cdata->sensors_enabled |= (1 << sdata->sindex);
+	else
+		sdata->cdata->sensors_enabled &= ~(1 << sdata->sindex);
+
+	return 0;
+}
+
+static int st_lsm6ds3_i2c_master_read_raw(struct iio_dev *indio_dev,
+		struct iio_chan_spec const *ch, int *val, int *val2, long mask)
+{
+	int err;
+	u8 outdata[(ch->scan_type.storagebits >> 3) + 1];
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&indio_dev->mlock);
+
+		if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
+			mutex_unlock(&indio_dev->mlock);
+			return -EBUSY;
+		}
+
+		mutex_lock(&sdata->cdata->passthrough_lock);
+
+		err = st_lsm6ds3_enable_accel_dependency(sdata, true);
+		if (err < 0)
+			return err;
+
+		disable_irq(sdata->cdata->irq);
+		st_lsm6ds3_flush_works();
+
+		err = st_lsm6ds3_enable_passthrough(sdata->cdata, true);
+		if (err < 0)
+			goto read_raw_reset_passthrough;
+
+		err = st_lsm6ds3_i2c_master_set_enable(sdata, true);
+		if (err < 0)
+			goto read_raw_reset_passthrough;
+
+		memset(outdata, 0, (ch->scan_type.storagebits >> 3) + 1);
+
+		msleep(200);
+
+		err = st_lsm6ds3_i2c_master_read(
+			sdata->cdata->master_client[sdata->sindex -
+				ST_INDIO_DEV_EXT0], ch->address,
+				ch->scan_type.storagebits >> 3, outdata);
+		if (err < 0)
+			goto read_raw_reset_passthrough;
+
+		err = st_lsm6ds3_i2c_master_set_enable(sdata, false);
+		if (err < 0)
+			goto read_raw_reset_passthrough;
+
+		err = st_lsm6ds3_enable_passthrough(sdata->cdata, false);
+		if (err < 0)
+			goto read_raw_reset_passthrough;
+
+		enable_irq(sdata->cdata->irq);
+		mutex_unlock(&sdata->cdata->passthrough_lock);
+
+		err = st_lsm6ds3_enable_accel_dependency(sdata, false);
+		if (err < 0)
+			return err;
+
+		if ((ch->scan_type.storagebits >> 3) > 2)
+			*val = (s32)get_unaligned_le32(outdata);
+		else
+			*val = (s16)get_unaligned_le16(outdata);
+
+		*val = *val >> ch->scan_type.shift;
+
+		mutex_unlock(&indio_dev->mlock);
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = sdata->c_gain[ch->scan_index];
+
+		if (ch->type == IIO_TEMP) {
+			*val = 1;
+			*val2 = 0;
+			return IIO_VAL_INT;
+		}
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912
+		if (sdata->sindex == ST_INDIO_DEV_EXT0)
+			return IIO_VAL_INT_PLUS_NANO;
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+
+read_raw_reset_passthrough:
+	st_lsm6ds3_enable_passthrough(sdata->cdata, false);
+	enable_irq(sdata->cdata->irq);
+	mutex_unlock(&sdata->cdata->passthrough_lock);
+	st_lsm6ds3_enable_accel_dependency(sdata, false);
+	return err;
+}
+
+static int st_lsm6ds3_i2c_master_buffer_preenable(struct iio_dev *indio_dev)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	mutex_lock(&sdata->cdata->passthrough_lock);
+
+	err = st_lsm6ds3_enable_accel_dependency(sdata, true);
+	if (err < 0)
+		return err;
+
+	disable_irq(sdata->cdata->irq);
+	st_lsm6ds3_flush_works();
+
+	err = st_lsm6ds3_enable_passthrough(sdata->cdata, true);
+	if (err < 0)
+		goto preenable_reset_passthrough;
+
+	err = st_lsm6ds3_i2c_master_set_enable(sdata, true);
+	if (err < 0)
+		goto preenable_reset_passthrough;
+
+	err = st_lsm6ds3_enable_passthrough(sdata->cdata, false);
+	if (err < 0)
+		goto preenable_reset_passthrough;
+
+	enable_irq(sdata->cdata->irq);
+	mutex_unlock(&sdata->cdata->passthrough_lock);
+
+	err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true);
+	if (err < 0)
+		return err;
+
+	return iio_sw_buffer_preenable(indio_dev);
+
+preenable_reset_passthrough:
+	st_lsm6ds3_enable_passthrough(sdata->cdata, false);
+	enable_irq(sdata->cdata->irq);
+	mutex_unlock(&sdata->cdata->passthrough_lock);
+
+	return err;
+}
+
+static int st_lsm6ds3_i2c_master_buffer_postenable(struct iio_dev *indio_dev)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	sdata->buffer_data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
+	if (sdata->buffer_data == NULL)
+		return -ENOMEM;
+
+	err = iio_triggered_buffer_postenable(indio_dev);
+	if (err < 0)
+		goto free_buffer_data;
+
+	return 0;
+
+free_buffer_data:
+	kfree(sdata->buffer_data);
+
+	return err;
+}
+
+static int st_lsm6ds3_i2c_master_buffer_predisable(struct iio_dev *indio_dev)
+{
+	int err;
+	struct lsm6ds3_sensor_data *sdata = iio_priv(indio_dev);
+
+	err = iio_triggered_buffer_predisable(indio_dev);
+	if (err < 0)
+		return err;
+
+	mutex_lock(&sdata->cdata->passthrough_lock);
+
+	disable_irq(sdata->cdata->irq);
+	st_lsm6ds3_flush_works();
+
+	err = st_lsm6ds3_enable_passthrough(sdata->cdata, true);
+	if (err < 0)
+		goto predisable_reset_passthrough;
+
+	err = st_lsm6ds3_i2c_master_set_enable(sdata, false);
+	if (err < 0)
+		goto predisable_reset_passthrough;
+
+	err = st_lsm6ds3_enable_passthrough(sdata->cdata, false);
+	if (err < 0)
+		goto predisable_reset_passthrough;
+
+	enable_irq(sdata->cdata->irq);
+	mutex_unlock(&sdata->cdata->passthrough_lock);
+
+	err = st_lsm6ds3_enable_accel_dependency(sdata, false);
+	if (err < 0)
+		return err;
+
+	err = st_lsm6ds3_reconfigure_fifo(sdata->cdata, true);
+	if (err < 0)
+		return err;
+
+	kfree(sdata->buffer_data);
+
+	return 0;
+
+predisable_reset_passthrough:
+	st_lsm6ds3_enable_passthrough(sdata->cdata, false);
+	enable_irq(sdata->cdata->irq);
+	mutex_unlock(&sdata->cdata->passthrough_lock);
+
+	return err;
+}
+
+static const struct iio_trigger_ops st_lsm6ds3_i2c_master_trigger_ops = {
+	.owner = THIS_MODULE,
+	.set_trigger_state = &st_lsm6ds3_trig_set_state,
+};
+
+int st_lsm6ds3_i2c_master_allocate_trigger(struct lsm6ds3_data *cdata,
+								int dev_index)
+{
+	int err;
+
+	cdata->trig[dev_index] = iio_trigger_alloc("%s-trigger",
+					cdata->indio_dev[dev_index]->name);
+	if (!cdata->trig[dev_index]) {
+		dev_err(cdata->dev, "failed to allocate iio trigger.\n");
+		return -ENOMEM;
+	}
+
+	iio_trigger_set_drvdata(cdata->trig[dev_index],
+						cdata->indio_dev[dev_index]);
+	cdata->trig[dev_index]->ops = &st_lsm6ds3_i2c_master_trigger_ops;
+	cdata->trig[dev_index]->dev.parent = cdata->dev;
+
+	err = iio_trigger_register(cdata->trig[dev_index]);
+	if (err < 0) {
+		dev_err(cdata->dev, "failed to register iio trigger.\n");
+		goto deallocate_trigger;
+	}
+
+	cdata->indio_dev[dev_index]->trig = cdata->trig[dev_index];
+
+	return 0;
+
+deallocate_trigger:
+	iio_trigger_free(cdata->trig[dev_index]);
+	return err;
+}
+
+static void st_lsm6ds3_i2c_master_deallocate_trigger(struct lsm6ds3_data *cdata,
+								int dev_index)
+{
+	iio_trigger_unregister(cdata->trig[dev_index]);
+}
+
+static const struct iio_buffer_setup_ops st_lsm6ds3_i2c_master_buffer_setup_ops = {
+	.preenable = &st_lsm6ds3_i2c_master_buffer_preenable,
+	.postenable = &st_lsm6ds3_i2c_master_buffer_postenable,
+	.predisable = &st_lsm6ds3_i2c_master_buffer_predisable,
+};
+
+static inline irqreturn_t st_lsm6ds3_i2c_master_handler_empty(int irq, void *p)
+{
+	return IRQ_HANDLED;
+}
+
+static int st_lsm6ds3_i2c_master_allocate_buffer(struct lsm6ds3_data *cdata,
+								int dev_index)
+{
+	return iio_triggered_buffer_setup(cdata->indio_dev[dev_index],
+				&st_lsm6ds3_i2c_master_handler_empty, NULL,
+				&st_lsm6ds3_i2c_master_buffer_setup_ops);
+}
+
+static void st_lsm6ds3_i2c_master_deallocate_buffer(struct lsm6ds3_data *cdata,
+								int dev_index)
+{
+	iio_triggered_buffer_cleanup(cdata->indio_dev[dev_index]);
+}
+
+static int st_lsm6ds3_i2c_master_send_sensor_hub_parameters(
+				struct lsm6ds3_sensor_data *sdata, int ext_num)
+{
+	int err;
+	u8 i2c_address_reg, data_start_address_reg, slave_num;
+	u8 i2c_address, data_start_address, config_reg_addr, config_reg_mask;
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912
+	u8 i2c_address_reg_akm, data_start_address_reg_akm, temp_reg;
+	u8 config_reg_addr_akm, config_reg_mask_akm;
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+
+	switch (ext_num) {
+	case ST_LSM6DS3_EXT0_INDEX:
+		i2c_address_reg = ST_LSM6DS3_SLV0_ADDR_ADDR;
+		data_start_address_reg = ST_LSM6DS3_SLV0_SUBADDR_ADDR;
+		config_reg_addr = ST_LSM6DS3_SLV0_CONFIG_ADDR;
+		config_reg_mask = ST_LSM6DS3_SLV0_CONFIG_MASK;
+		break;
+	case ST_LSM6DS3_EXT1_INDEX:
+		i2c_address_reg = ST_LSM6DS3_SLV1_ADDR_ADDR;
+		data_start_address_reg = ST_LSM6DS3_SLV1_SUBADDR_ADDR;
+		config_reg_addr = ST_LSM6DS3_SLV1_CONFIG_ADDR;
+		config_reg_mask = ST_LSM6DS3_SLV1_CONFIG_MASK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	i2c_address = (st_lsm6ds3_exs_list[ext_num].board_info.addr << 1) |
+							ST_LSM6DS3_EN_BIT;
+
+	data_start_address =
+			st_lsm6ds3_exs_list[ext_num].data.channels[0].address;
+
+	mutex_lock(&sdata->cdata->bank_registers_lock);
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR,
+					ST_LSM6DS3_FUNC_CFG_REG2_MASK,
+					ST_LSM6DS3_EN_BIT, false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	err = sdata->cdata->tf->write(sdata->cdata, i2c_address_reg,
+					1, &i2c_address, false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	err = sdata->cdata->tf->write(sdata->cdata,
+					data_start_address_reg,
+					1, &data_start_address, false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				config_reg_addr, config_reg_mask,
+				st_lsm6ds3_exs_list[ext_num].read_data_len,
+				false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	if (ext_num == ST_LSM6DS3_EXT0_INDEX) {
+		if (sdata->cdata->ext1_available) {
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912
+			slave_num = ST_LSM6DS3_SLV_AUX_3;
+			i2c_address_reg_akm = ST_LSM6DS3_SLV2_ADDR_ADDR;
+			data_start_address_reg_akm = ST_LSM6DS3_SLV2_SUBADDR_ADDR;
+			config_reg_addr_akm = ST_LSM6DS3_SLV2_CONFIG_ADDR;
+			config_reg_mask_akm = ST_LSM6DS3_SLV2_CONFIG_MASK;
+#else /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+			slave_num = ST_LSM6DS3_SLV_AUX_2;
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+		} else {
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912
+			slave_num = ST_LSM6DS3_SLV_AUX_2;
+			i2c_address_reg_akm = ST_LSM6DS3_SLV1_ADDR_ADDR;
+			data_start_address_reg_akm = ST_LSM6DS3_SLV1_SUBADDR_ADDR;
+			config_reg_addr_akm = ST_LSM6DS3_SLV1_CONFIG_ADDR;
+			config_reg_mask_akm = ST_LSM6DS3_SLV1_CONFIG_MASK;
+#else /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+			slave_num = ST_LSM6DS3_SLV_AUX_1;
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+		}
+
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_SLV_AUX_ADDR,
+						ST_LSM6DS3_SLV_AUX_MASK,
+						slave_num, false);
+		if (err < 0)
+			goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+#ifdef CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912
+		temp_reg = (ST_LSM6DS3_EXT0_ADDR << 1) | ST_LSM6DS3_EN_BIT;
+
+		err = sdata->cdata->tf->write(sdata->cdata, i2c_address_reg_akm,
+						1, &temp_reg, false);
+		if (err < 0)
+			goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+		temp_reg = ST_LSM6DS3_EXT0_DATA_STATUS;
+
+		err = sdata->cdata->tf->write(sdata->cdata,
+						data_start_address_reg_akm,
+						1, &temp_reg, false);
+		if (err < 0)
+			goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+				config_reg_addr_akm, config_reg_mask_akm,
+				1, false);
+		if (err < 0)
+			goto st_lsm6ds3_init_sensor_mutex_unlock;
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT0_AKM09912 */
+	}
+
+	err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+					ST_LSM6DS3_FUNC_CFG_ACCESS_ADDR,
+					ST_LSM6DS3_FUNC_CFG_REG2_MASK,
+					ST_LSM6DS3_DIS_BIT, false);
+	if (err < 0)
+		goto st_lsm6ds3_init_sensor_mutex_unlock;
+
+	mutex_unlock(&sdata->cdata->bank_registers_lock);
+
+	return 0;
+
+st_lsm6ds3_init_sensor_mutex_unlock:
+	mutex_unlock(&sdata->cdata->bank_registers_lock);
+	return err;
+}
+
+static int st_lsm6ds3_i2c_master_init_sensor(struct lsm6ds3_sensor_data *sdata,
+						int ext_num, int dev_index)
+{
+	int err;
+
+	mutex_lock(&sdata->cdata->passthrough_lock);
+
+	err = st_lsm6ds3_enable_passthrough(sdata->cdata, true);
+	if (err < 0)
+		goto unlock_passthrough;
+
+	sdata->c_gain[0] = st_lsm6ds3_exs_list[ext_num].gain;
+	sdata->c_gain[1] = st_lsm6ds3_exs_list[ext_num].gain;
+	sdata->c_gain[2] = st_lsm6ds3_exs_list[ext_num].gain;
+
+	if ((st_lsm6ds3_exs_list[ext_num].power.addr ==
+				st_lsm6ds3_exs_list[ext_num].odr.addr) &&
+			(st_lsm6ds3_exs_list[ext_num].power.mask ==
+					st_lsm6ds3_exs_list[ext_num].odr.mask))
+		st_lsm6ds3_exs_list[ext_num].power.isodr = true;
+	else
+		st_lsm6ds3_exs_list[ext_num].power.isodr = false;
+
+	err = st_lsm6ds3_i2c_master_write_data_with_mask(
+					sdata->cdata->master_client[ext_num],
+					st_lsm6ds3_exs_list[ext_num].reset.addr,
+					st_lsm6ds3_exs_list[ext_num].reset.mask,
+					ST_LSM6DS3_EN_BIT);
+	if (err < 0)
+		goto unlock_passthrough;
+
+	usleep_range(3000, 8000);
+
+	if (st_lsm6ds3_exs_list[ext_num].fullscale.addr > 0) {
+		err = st_lsm6ds3_i2c_master_write_data_with_mask(
+			sdata->cdata->master_client[ext_num],
+			st_lsm6ds3_exs_list[ext_num].fullscale.addr,
+			st_lsm6ds3_exs_list[ext_num].fullscale.mask,
+			st_lsm6ds3_exs_list[ext_num].fullscale.def_value);
+		if (err < 0)
+			goto unlock_passthrough;
+	}
+
+	if (st_lsm6ds3_exs_list[ext_num].cf.boot_initialization != NULL) {
+		err = st_lsm6ds3_exs_list[ext_num].cf.boot_initialization(sdata, ext_num);
+		if (err < 0)
+			goto unlock_passthrough;
+	}
+
+	err = st_lsm6ds3_i2c_master_set_enable(sdata, false);
+	if (err < 0)
+		goto unlock_passthrough;
+
+	err = st_lsm6ds3_i2c_master_set_odr(sdata, 26);
+	if (err < 0)
+		goto unlock_passthrough;
+
+	err = st_lsm6ds3_i2c_master_send_sensor_hub_parameters(sdata, ext_num);
+	if (err < 0)
+		goto unlock_passthrough;
+
+	err = st_lsm6ds3_enable_passthrough(sdata->cdata, false);
+	if (err < 0)
+		goto unlock_passthrough;
+
+	mutex_unlock(&sdata->cdata->passthrough_lock);
+
+	return 0;
+
+unlock_passthrough:
+	mutex_unlock(&sdata->cdata->passthrough_lock);
+	return err;
+}
+
+static int st_lsm6ds3_i2c_master_allocate_device(struct lsm6ds3_data *cdata,
+								int ext_num)
+{
+	int err, dev_index;
+	struct lsm6ds3_sensor_data *sdata_ext;
+
+	switch (ext_num) {
+	case ST_LSM6DS3_EXT0_INDEX:
+		dev_index = ST_INDIO_DEV_EXT0;
+		break;
+	case ST_LSM6DS3_EXT1_INDEX:
+		dev_index = ST_INDIO_DEV_EXT1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	cdata->indio_dev[dev_index] = iio_device_alloc(sizeof(*sdata_ext));
+	if (!cdata->indio_dev[dev_index])
+		return -ENOMEM;
+
+	sdata_ext = iio_priv(cdata->indio_dev[dev_index]);
+	sdata_ext->cdata = cdata;
+	sdata_ext->sindex = dev_index;
+
+	sdata_ext->num_data_channels =
+				st_lsm6ds3_exs_list[ext_num].num_data_channels;
+
+	cdata->indio_dev[dev_index]->name = kasprintf(GFP_KERNEL,
+				"%s_%s", cdata->name,
+				st_lsm6ds3_exs_list[ext_num].data.suffix_name);
+
+	cdata->indio_dev[dev_index]->info =
+				st_lsm6ds3_exs_list[ext_num].data.info;
+	cdata->indio_dev[dev_index]->channels =
+				st_lsm6ds3_exs_list[ext_num].data.channels;
+	cdata->indio_dev[dev_index]->num_channels =
+				st_lsm6ds3_exs_list[ext_num].data.num_channels;
+
+	cdata->indio_dev[dev_index]->modes = INDIO_DIRECT_MODE;
+
+	err = st_lsm6ds3_i2c_master_init_sensor(sdata_ext, ext_num, dev_index);
+	if (err < 0)
+		goto iio_device_free;
+
+	err = st_lsm6ds3_i2c_master_allocate_buffer(cdata, dev_index);
+	if (err < 0)
+		goto iio_device_free;
+
+	err = st_lsm6ds3_i2c_master_allocate_trigger(cdata, dev_index);
+	if (err < 0)
+		goto iio_deallocate_buffer;
+
+	err = iio_device_register(cdata->indio_dev[dev_index]);
+	if (err < 0)
+		goto iio_deallocate_trigger;
+
+	return 0;
+
+iio_deallocate_trigger:
+	st_lsm6ds3_i2c_master_deallocate_trigger(cdata, dev_index);
+iio_deallocate_buffer:
+	st_lsm6ds3_i2c_master_deallocate_buffer(cdata, dev_index);
+iio_device_free:
+	iio_device_free(cdata->indio_dev[dev_index]);
+
+	return err;
+}
+
+static void st_lsm6ds3_i2c_master_deallocate_device(struct lsm6ds3_data *cdata,
+								int ext_num)
+{
+	int dev_index;
+
+	switch (ext_num) {
+	case ST_LSM6DS3_EXT0_INDEX:
+		dev_index = ST_INDIO_DEV_EXT0;
+		break;
+	case ST_LSM6DS3_EXT1_INDEX:
+		dev_index = ST_INDIO_DEV_EXT1;
+		break;
+	default:
+		return;
+	}
+
+	iio_device_unregister(cdata->indio_dev[dev_index]);
+	st_lsm6ds3_i2c_master_deallocate_trigger(cdata, dev_index);
+	st_lsm6ds3_i2c_master_deallocate_buffer(cdata, dev_index);
+	iio_device_free(cdata->indio_dev[dev_index]);
+}
+
+int st_lsm6ds3_i2c_master_probe(struct lsm6ds3_data *cdata)
+{
+	u8 wai;
+	int err;
+	struct i2c_client *client = to_i2c_client(cdata->dev);
+
+	cdata->ext0_available = false;
+	cdata->ext1_available = false;
+
+	cdata->ext0_samples_in_pattern = 0;
+	cdata->ext1_samples_in_pattern = 0;
+
+	sprintf(st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].board_info.type,
+			"%s_ext%d", client->name, ST_LSM6DS3_EXT0_INDEX);
+
+	cdata->master_client[ST_LSM6DS3_EXT0_INDEX] =
+			i2c_new_device(client->adapter,
+			&st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].board_info);
+	if (!cdata->master_client[ST_LSM6DS3_EXT0_INDEX])
+		return -ENOMEM;
+
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+	sprintf(st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].board_info.type,
+			"%s_ext%d", client->name, ST_LSM6DS3_EXT1_INDEX);
+
+	cdata->master_client[ST_LSM6DS3_EXT1_INDEX] =
+			i2c_new_device(client->adapter,
+			&st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].board_info);
+	if (!cdata->master_client[ST_LSM6DS3_EXT1_INDEX]) {
+		err = -ENOMEM;
+		goto unregister_ext0_i2c_client;
+	}
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+
+	mutex_lock(&cdata->passthrough_lock);
+
+	err = st_lsm6ds3_enable_passthrough(cdata, true);
+	if (err < 0)
+		goto master_probe_passthrough_lock;
+
+	err = st_lsm6ds3_i2c_master_read(
+		cdata->master_client[ST_LSM6DS3_EXT0_INDEX],
+		st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].wai.addr, 1, &wai);
+	if (err < 0) {
+		dev_err(cdata->dev, "external sensor 0 not available\n");
+		i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]);
+	} else {
+		if (wai != st_lsm6ds3_exs_list[ST_LSM6DS3_EXT0_INDEX].wai.def_value) {
+			dev_err(cdata->dev, "wai value of external sensor 0 mismatch\n");
+			i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]);
+		} else
+			cdata->ext0_available = true;
+	}
+
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+	err = st_lsm6ds3_i2c_master_read(
+		cdata->master_client[ST_LSM6DS3_EXT1_INDEX],
+		st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].wai.addr, 1, &wai);
+	if (err < 0) {
+		dev_err(cdata->dev, "external sensor 1 not available\n");
+		i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]);
+	} else {
+		if (wai != st_lsm6ds3_exs_list[ST_LSM6DS3_EXT1_INDEX].wai.def_value) {
+			dev_err(cdata->dev, "wai value of external sensor 1 mismatch\n");
+			i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]);
+		} else
+			cdata->ext1_available = true;
+	}
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+
+	err = st_lsm6ds3_enable_passthrough(cdata, false);
+	if (err < 0) {
+		if (cdata->ext0_available)
+			i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]);
+
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+		if (cdata->ext1_available)
+			i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]);
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+
+		mutex_unlock(&cdata->passthrough_lock);
+
+		return err;
+	}
+
+	mutex_unlock(&cdata->passthrough_lock);
+
+	if (cdata->ext0_available) {
+		err = st_lsm6ds3_i2c_master_allocate_device(cdata, ST_LSM6DS3_EXT0_INDEX);
+		if (err < 0)
+			goto unregister_with_check_i2c_clients;
+
+	}
+
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+	if (cdata->ext1_available) {
+		err = st_lsm6ds3_i2c_master_allocate_device(cdata, ST_LSM6DS3_EXT1_INDEX);
+		if (err < 0)
+			goto deallocate_ext0_device;
+
+	}
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+
+	return 0;
+
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+deallocate_ext0_device:
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+	st_lsm6ds3_i2c_master_deallocate_device(cdata, ST_LSM6DS3_EXT0_INDEX);
+unregister_with_check_i2c_clients:
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+	if (cdata->ext1_available)
+		i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]);
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+	if (cdata->ext0_available)
+		i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]);
+
+	return err;
+
+master_probe_passthrough_lock:
+	mutex_unlock(&cdata->passthrough_lock);
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+	i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]);
+unregister_ext0_i2c_client:
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+	i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]);
+
+	return err;
+}
+EXPORT_SYMBOL(st_lsm6ds3_i2c_master_probe);
+
+int st_lsm6ds3_i2c_master_exit(struct lsm6ds3_data *cdata)
+{
+	if (cdata->ext0_available) {
+		st_lsm6ds3_i2c_master_deallocate_device(cdata, ST_LSM6DS3_EXT0_INDEX);
+		i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT0_INDEX]);
+	}
+#ifndef CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED
+	if (cdata->ext1_available) {
+		st_lsm6ds3_i2c_master_deallocate_device(cdata, ST_LSM6DS3_EXT1_INDEX);
+		i2c_unregister_device(cdata->master_client[ST_LSM6DS3_EXT1_INDEX]);
+	}
+#endif /* CONFIG_ST_LSM6DS3_IIO_EXT1_DISABLED */
+
+	return 0;
+}
+EXPORT_SYMBOL(st_lsm6ds3_i2c_master_exit);
+
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 i2c master driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_spi.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_spi.c
new file mode 100644
index 00000000000..7212ae225f8
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_spi.c
@@ -0,0 +1,180 @@
+/*
+ * STMicroelectronics lsm6ds3 spi driver
+ *
+ * Copyright 2014 STMicroelectronics Inc.
+ *
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/iio/iio.h>
+
+#include "st_lsm6ds3.h"
+
+#define ST_SENSORS_SPI_READ			0x80
+
+static int st_lsm6ds3_spi_read(struct lsm6ds3_data *cdata,
+				u8 reg_addr, int len, u8 *data, bool b_lock)
+{
+	int err;
+
+	struct spi_transfer xfers[] = {
+		{
+			.tx_buf = cdata->tb.tx_buf,
+			.bits_per_word = 8,
+			.len = 1,
+		},
+		{
+			.rx_buf = cdata->tb.rx_buf,
+			.bits_per_word = 8,
+			.len = len,
+		}
+	};
+
+	if (b_lock)
+		mutex_lock(&cdata->bank_registers_lock);
+
+	mutex_lock(&cdata->tb.buf_lock);
+	cdata->tb.tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ;
+
+	err = spi_sync_transfer(to_spi_device(cdata->dev),
+						xfers, ARRAY_SIZE(xfers));
+	if (err)
+		goto acc_spi_read_error;
+
+	memcpy(data, cdata->tb.rx_buf, len*sizeof(u8));
+	mutex_unlock(&cdata->tb.buf_lock);
+	if (b_lock)
+		mutex_unlock(&cdata->bank_registers_lock);
+
+	return len;
+
+acc_spi_read_error:
+	mutex_unlock(&cdata->tb.buf_lock);
+	if (b_lock)
+		mutex_unlock(&cdata->bank_registers_lock);
+
+	return err;
+}
+
+static int st_lsm6ds3_spi_write(struct lsm6ds3_data *cdata,
+				u8 reg_addr, int len, u8 *data, bool b_lock)
+{
+	int err;
+
+	struct spi_transfer xfers = {
+		.tx_buf = cdata->tb.tx_buf,
+		.bits_per_word = 8,
+		.len = len + 1,
+	};
+
+	if (len >= ST_LSM6DS3_RX_MAX_LENGTH)
+		return -ENOMEM;
+
+	if (b_lock)
+		mutex_lock(&cdata->bank_registers_lock);
+
+	mutex_lock(&cdata->tb.buf_lock);
+	cdata->tb.tx_buf[0] = reg_addr;
+
+	memcpy(&cdata->tb.tx_buf[1], data, len);
+
+	err = spi_sync_transfer(to_spi_device(cdata->dev), &xfers, 1);
+	mutex_unlock(&cdata->tb.buf_lock);
+	if (b_lock)
+		mutex_unlock(&cdata->bank_registers_lock);
+
+	return err;
+}
+
+static const struct st_lsm6ds3_transfer_function st_lsm6ds3_tf_spi = {
+	.write = st_lsm6ds3_spi_write,
+	.read = st_lsm6ds3_spi_read,
+};
+
+static int st_lsm6ds3_spi_probe(struct spi_device *spi)
+{
+	int err;
+	struct lsm6ds3_data *cdata;
+
+	cdata = kmalloc(sizeof(*cdata), GFP_KERNEL);
+	if (!cdata)
+		return -ENOMEM;
+
+	cdata->dev = &spi->dev;
+	cdata->name = spi->modalias;
+	spi_set_drvdata(spi, cdata);
+
+	cdata->tf = &st_lsm6ds3_tf_spi;
+
+	err = st_lsm6ds3_common_probe(cdata, spi->irq);
+	if (err < 0)
+		goto free_data;
+
+	return 0;
+
+free_data:
+	kfree(cdata);
+	return err;
+}
+
+static int st_lsm6ds3_spi_remove(struct spi_device *spi)
+{
+	struct lsm6ds3_data *cdata = spi_get_drvdata(spi);
+
+	st_lsm6ds3_common_remove(cdata, spi->irq);
+	kfree(cdata);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int st_lsm6ds3_suspend(struct device *dev)
+{
+	struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev));
+
+	return st_lsm6ds3_common_suspend(cdata);
+}
+
+static int st_lsm6ds3_resume(struct device *dev)
+{
+	struct lsm6ds3_data *cdata = spi_get_drvdata(to_spi_device(dev));
+
+	return st_lsm6ds3_common_resume(cdata);
+}
+
+static const struct dev_pm_ops st_lsm6ds3_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume)
+};
+
+#define ST_LSM6DS3_PM_OPS		(&st_lsm6ds3_pm_ops)
+#else /* CONFIG_PM */
+#define ST_LSM6DS3_PM_OPS		NULL
+#endif /* CONFIG_PM */
+
+static const struct spi_device_id st_lsm6ds3_id_table[] = {
+	{ LSM6DS3_DEV_NAME },
+	{ },
+};
+MODULE_DEVICE_TABLE(spi, st_lsm6ds3_id_table);
+
+static struct spi_driver st_lsm6ds3_driver = {
+	.driver = {
+		.owner = THIS_MODULE,
+		.name = "st-lsm6ds3-spi",
+		.pm = ST_LSM6DS3_PM_OPS,
+	},
+	.probe = st_lsm6ds3_spi_probe,
+	.remove = st_lsm6ds3_spi_remove,
+	.id_table = st_lsm6ds3_id_table,
+};
+module_spi_driver(st_lsm6ds3_driver);
+
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 spi driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
new file mode 100644
index 00000000000..fb739c883d4
--- /dev/null
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
@@ -0,0 +1,175 @@
+/*
+ * STMicroelectronics lsm6ds3 trigger driver
+ *
+ * Copyright 2014 STMicroelectronics Inc.
+ *
+ * Denis Ciocca <denis.ciocca@st.com>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
+#include <linux/interrupt.h>
+#include <linux/iio/events.h>
+
+#include "st_lsm6ds3.h"
+
+#define ST_LSM6DS3_SRC_FUNC_ADDR		0x53
+#define ST_LSM6DS3_FIFO_DATA_AVL_ADDR		0x3b
+
+#define ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL	0x10
+#define ST_LSM6DS3_SRC_TILT_DATA_AVL		0x20
+#define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL	0x80
+#define ST_LSM6DS3_FIFO_DATA_AVL		0x80
+#define ST_LSM6DS3_FIFO_DATA_OVR		0x40
+
+static struct workqueue_struct *st_lsm6ds3_wq;
+
+void st_lsm6ds3_flush_works()
+{
+	flush_workqueue(st_lsm6ds3_wq);
+}
+
+irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private)
+{
+	struct timespec ts;
+	struct lsm6ds3_data *cdata = private;
+
+	get_monotonic_boottime(&ts);
+	cdata->timestamp = timespec_to_ns(&ts);
+	queue_work(st_lsm6ds3_wq, &cdata->data_work);
+
+	disable_irq_nosync(irq);
+
+	return IRQ_HANDLED;
+}
+
+static void st_lsm6ds3_irq_management(struct work_struct *data_work)
+{
+	struct lsm6ds3_data *cdata;
+	u8 src_value = 0x00, src_fifo = 0x00;
+
+	cdata = container_of((struct work_struct *)data_work,
+						struct lsm6ds3_data, data_work);
+
+	cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR, 1, &src_value, true);
+	cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_AVL_ADDR, 1,
+							&src_fifo, true);
+
+	if (src_fifo & ST_LSM6DS3_FIFO_DATA_AVL) {
+		if (src_fifo & ST_LSM6DS3_FIFO_DATA_OVR) {
+			st_lsm6ds3_set_fifo_mode(cdata, BYPASS);
+			st_lsm6ds3_set_fifo_mode(cdata, CONTINUOS);
+			dev_err(cdata->dev,
+				"data fifo overrun, reduce fifo size.\n");
+		} else
+			st_lsm6ds3_read_fifo(cdata, false);
+	}
+
+	if (src_value & ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL) {
+		iio_push_event(cdata->indio_dev[ST_INDIO_DEV_STEP_DETECTOR],
+				IIO_UNMOD_EVENT_CODE(IIO_STEP_DETECTOR,
+				0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
+				cdata->timestamp);
+
+		if (cdata->sign_motion_event_ready) {
+			iio_push_event(cdata->indio_dev[
+				ST_INDIO_DEV_SIGN_MOTION],
+				IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION,
+				0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
+				cdata->timestamp);
+
+			cdata->sign_motion_event_ready = false;
+		}
+	}
+
+	if (src_value & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL) {
+		iio_trigger_poll_chained(
+				cdata->trig[ST_INDIO_DEV_STEP_COUNTER], 0);
+	}
+
+	if (src_value & ST_LSM6DS3_SRC_TILT_DATA_AVL) {
+		iio_push_event(cdata->indio_dev[ST_INDIO_DEV_TILT],
+				IIO_UNMOD_EVENT_CODE(IIO_TILT,
+				0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
+				cdata->timestamp);
+	}
+
+	enable_irq(cdata->irq);
+	return;
+}
+
+int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
+				const struct iio_trigger_ops *trigger_ops)
+{
+	int err, i, n;
+
+	if (!st_lsm6ds3_wq)
+		st_lsm6ds3_wq = create_workqueue(cdata->name);
+
+	if (!st_lsm6ds3_wq)
+		return -EINVAL;
+
+	INIT_WORK(&cdata->data_work, st_lsm6ds3_irq_management);
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
+		cdata->trig[i] = iio_trigger_alloc("%s-trigger",
+				cdata->indio_dev[i]->name);
+		if (!cdata->trig[i]) {
+			dev_err(cdata->dev,
+					"failed to allocate iio trigger.\n");
+			err = -ENOMEM;
+			goto deallocate_trigger;
+		}
+		iio_trigger_set_drvdata(cdata->trig[i], cdata->indio_dev[i]);
+		cdata->trig[i]->ops = trigger_ops;
+		cdata->trig[i]->dev.parent = cdata->dev;
+	}
+
+	err = request_threaded_irq(cdata->irq, st_lsm6ds3_save_timestamp, NULL,
+					IRQF_TRIGGER_HIGH, cdata->name, cdata);
+	if (err)
+		goto deallocate_trigger;
+
+	for (n = 0; n < ST_INDIO_DEV_NUM; n++) {
+		err = iio_trigger_register(cdata->trig[n]);
+		if (err < 0) {
+			dev_err(cdata->dev,
+					"failed to register iio trigger.\n");
+			goto free_irq;
+		}
+		cdata->indio_dev[n]->trig = cdata->trig[n];
+	}
+
+	return 0;
+
+free_irq:
+	free_irq(cdata->irq, cdata);
+	for (n--; n >= 0; n--)
+		iio_trigger_unregister(cdata->trig[n]);
+deallocate_trigger:
+	for (i--; i >= 0; i--)
+		iio_trigger_free(cdata->trig[i]);
+
+	return err;
+}
+EXPORT_SYMBOL(st_lsm6ds3_allocate_triggers);
+
+void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata)
+{
+	int i;
+
+	free_irq(cdata->irq, cdata);
+
+	for (i = 0; i < ST_INDIO_DEV_NUM; i++)
+		iio_trigger_unregister(cdata->trig[i]);
+}
+EXPORT_SYMBOL(st_lsm6ds3_deallocate_triggers);
+
+MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics lsm6ds3 trigger driver");
+MODULE_LICENSE("GPL v2");