4 files changed, 169 insertions, 69 deletions

diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c
index 09b236d6ee8..1665c8e4b62 100644
--- a/drivers/iio/common/st_sensors/st_sensors_buffer.c
+++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c
@@ -24,11 +24,20 @@
 
 int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf)
 {
+	u8 *addr;
 	int i, n = 0, len;
-	u8 addr[ST_SENSORS_NUMBER_DATA_CHANNELS];
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
+	unsigned int num_data_channels = sdata->num_data_channels;
+	unsigned int byte_for_channel =
+			indio_dev->channels[0].scan_type.storagebits >> 3;
 
-	for (i = 0; i < ST_SENSORS_NUMBER_DATA_CHANNELS; i++) {
+	addr = kmalloc(num_data_channels, GFP_KERNEL);
+	if (!addr) {
+		len = -ENOMEM;
+		goto st_sensors_get_buffer_element_error;
+	}
+
+	for (i = 0; i < num_data_channels; i++) {
 		if (test_bit(i, indio_dev->active_scan_mask)) {
 			addr[n] = indio_dev->channels[i].address;
 			n++;
@@ -37,52 +46,58 @@ int st_sensors_get_buffer_element(struct iio_dev *indio_dev, u8 *buf)
 	switch (n) {
 	case 1:
 		len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
-			addr[0], ST_SENSORS_BYTE_FOR_CHANNEL, buf,
-			sdata->multiread_bit);
+			addr[0], byte_for_channel, buf, sdata->multiread_bit);
 		break;
 	case 2:
-		if ((addr[1] - addr[0]) == ST_SENSORS_BYTE_FOR_CHANNEL) {
+		if ((addr[1] - addr[0]) == byte_for_channel) {
 			len = sdata->tf->read_multiple_byte(&sdata->tb,
-					sdata->dev, addr[0],
-					ST_SENSORS_BYTE_FOR_CHANNEL*n,
-					buf, sdata->multiread_bit);
+				sdata->dev, addr[0], byte_for_channel * n,
+				buf, sdata->multiread_bit);
 		} else {
-			u8 rx_array[ST_SENSORS_BYTE_FOR_CHANNEL*
-				    ST_SENSORS_NUMBER_DATA_CHANNELS];
+			u8 *rx_array;
+			rx_array = kmalloc(byte_for_channel * num_data_channels,
+					   GFP_KERNEL);
+			if (!rx_array) {
+				len = -ENOMEM;
+				goto st_sensors_free_memory;
+			}
+
 			len = sdata->tf->read_multiple_byte(&sdata->tb,
 				sdata->dev, addr[0],
-				ST_SENSORS_BYTE_FOR_CHANNEL*
-				ST_SENSORS_NUMBER_DATA_CHANNELS,
+				byte_for_channel * num_data_channels,
 				rx_array, sdata->multiread_bit);
-			if (len < 0)
-				goto read_data_channels_error;
+			if (len < 0) {
+				kfree(rx_array);
+				goto st_sensors_free_memory;
+			}
 
-			for (i = 0; i < n * ST_SENSORS_NUMBER_DATA_CHANNELS;
-									i++) {
+			for (i = 0; i < n * num_data_channels; i++) {
 				if (i < n)
 					buf[i] = rx_array[i];
 				else
 					buf[i] = rx_array[n + i];
 			}
-			len = ST_SENSORS_BYTE_FOR_CHANNEL*n;
+			kfree(rx_array);
+			len = byte_for_channel * n;
 		}
 		break;
 	case 3:
 		len = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
-			addr[0], ST_SENSORS_BYTE_FOR_CHANNEL*
-			ST_SENSORS_NUMBER_DATA_CHANNELS,
+			addr[0], byte_for_channel * num_data_channels,
 			buf, sdata->multiread_bit);
 		break;
 	default:
 		len = -EINVAL;
-		goto read_data_channels_error;
+		goto st_sensors_free_memory;
 	}
-	if (len != ST_SENSORS_BYTE_FOR_CHANNEL*n) {
+	if (len != byte_for_channel * n) {
 		len = -EIO;
-		goto read_data_channels_error;
+		goto st_sensors_free_memory;
 	}
 
-read_data_channels_error:
+st_sensors_free_memory:
+	kfree(addr);
+st_sensors_get_buffer_element_error:
 	return len;
 }
 EXPORT_SYMBOL(st_sensors_get_buffer_element);
@@ -98,11 +113,8 @@ irqreturn_t st_sensors_trigger_handler(int irq, void *p)
 	if (len < 0)
 		goto st_sensors_get_buffer_element_error;
 
-	if (indio_dev->scan_timestamp)
-		*(s64 *)((u8 *)sdata->buffer_data +
-				ALIGN(len, sizeof(s64))) = pf->timestamp;
-
-	iio_push_to_buffers(indio_dev, sdata->buffer_data);
+	iio_push_to_buffers_with_timestamp(indio_dev, sdata->buffer_data,
+		pf->timestamp);
 
 st_sensors_get_buffer_element_error:
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 0198324a8b0..e8b932fed70 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -13,6 +13,7 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/iio/iio.h>
+#include <linux/regulator/consumer.h>
 #include <asm/unaligned.h>
 
 #include <linux/iio/common/st_sensors.h>
@@ -20,6 +21,11 @@
 
 #define ST_SENSORS_WAI_ADDRESS		0x0f
 
+static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
+{
+	return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
+}
+
 static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
 						u8 reg_addr, u8 mask, u8 data)
 {
@@ -62,7 +68,7 @@ st_sensors_match_odr_error:
 int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr)
 {
 	int err;
-	struct st_sensor_odr_avl odr_out;
+	struct st_sensor_odr_avl odr_out = {0, 0};
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
 	err = st_sensors_match_odr(sdata->sensor, odr, &odr_out);
@@ -112,9 +118,10 @@ st_sensors_match_odr_error:
 	return ret;
 }
 
-static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
+static int st_sensors_set_fullscale(struct iio_dev *indio_dev,
+								unsigned int fs)
 {
-	int err, i;
+	int err, i = 0;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
 	err = st_sensors_match_fs(sdata->sensor, fs, &i);
@@ -139,14 +146,13 @@ st_accel_set_fullscale_error:
 
 int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable)
 {
-	bool found;
 	u8 tmp_value;
 	int err = -EINVAL;
-	struct st_sensor_odr_avl odr_out;
+	bool found = false;
+	struct st_sensor_odr_avl odr_out = {0, 0};
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
 	if (enable) {
-		found = false;
 		tmp_value = sdata->sensor->pw.value_on;
 		if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) &&
 			(sdata->sensor->odr.mask == sdata->sensor->pw.mask)) {
@@ -193,35 +199,107 @@ int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable)
 }
 EXPORT_SYMBOL(st_sensors_set_axis_enable);
 
-int st_sensors_init_sensor(struct iio_dev *indio_dev)
+void st_sensors_power_enable(struct iio_dev *indio_dev)
 {
+	struct st_sensor_data *pdata = iio_priv(indio_dev);
 	int err;
+
+	/* Regulators not mandatory, but if requested we should enable them. */
+	pdata->vdd = devm_regulator_get_optional(indio_dev->dev.parent, "vdd");
+	if (!IS_ERR(pdata->vdd)) {
+		err = regulator_enable(pdata->vdd);
+		if (err != 0)
+			dev_warn(&indio_dev->dev,
+				 "Failed to enable specified Vdd supply\n");
+	}
+
+	pdata->vdd_io = devm_regulator_get_optional(indio_dev->dev.parent, "vddio");
+	if (!IS_ERR(pdata->vdd_io)) {
+		err = regulator_enable(pdata->vdd_io);
+		if (err != 0)
+			dev_warn(&indio_dev->dev,
+				 "Failed to enable specified Vdd_IO supply\n");
+	}
+}
+EXPORT_SYMBOL(st_sensors_power_enable);
+
+void st_sensors_power_disable(struct iio_dev *indio_dev)
+{
+	struct st_sensor_data *pdata = iio_priv(indio_dev);
+
+	if (!IS_ERR(pdata->vdd))
+		regulator_disable(pdata->vdd);
+
+	if (!IS_ERR(pdata->vdd_io))
+		regulator_disable(pdata->vdd_io);
+}
+EXPORT_SYMBOL(st_sensors_power_disable);
+
+static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
+				       struct st_sensors_platform_data *pdata)
+{
+	struct st_sensor_data *sdata = iio_priv(indio_dev);
+
+	switch (pdata->drdy_int_pin) {
+	case 1:
+		if (sdata->sensor->drdy_irq.mask_int1 == 0) {
+			dev_err(&indio_dev->dev,
+					"DRDY on INT1 not available.\n");
+			return -EINVAL;
+		}
+		sdata->drdy_int_pin = 1;
+		break;
+	case 2:
+		if (sdata->sensor->drdy_irq.mask_int2 == 0) {
+			dev_err(&indio_dev->dev,
+					"DRDY on INT2 not available.\n");
+			return -EINVAL;
+		}
+		sdata->drdy_int_pin = 2;
+		break;
+	default:
+		dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int st_sensors_init_sensor(struct iio_dev *indio_dev,
+					struct st_sensors_platform_data *pdata)
+{
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
+	int err = 0;
 
 	mutex_init(&sdata->tb.buf_lock);
 
+	if (pdata)
+		err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
+
 	err = st_sensors_set_enable(indio_dev, false);
 	if (err < 0)
-		goto init_error;
+		return err;
 
-	err = st_sensors_set_fullscale(indio_dev,
-						sdata->current_fullscale->num);
-	if (err < 0)
-		goto init_error;
+	if (sdata->current_fullscale) {
+		err = st_sensors_set_fullscale(indio_dev,
+					       sdata->current_fullscale->num);
+		if (err < 0)
+			return err;
+	} else
+		dev_info(&indio_dev->dev, "Full-scale not possible\n");
 
 	err = st_sensors_set_odr(indio_dev, sdata->odr);
 	if (err < 0)
-		goto init_error;
+		return err;
 
 	/* set BDU */
 	err = st_sensors_write_data_with_mask(indio_dev,
 			sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
 	if (err < 0)
-		goto init_error;
+		return err;
 
 	err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
 
-init_error:
 	return err;
 }
 EXPORT_SYMBOL(st_sensors_init_sensor);
@@ -229,8 +307,12 @@ EXPORT_SYMBOL(st_sensors_init_sensor);
 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
 {
 	int err;
+	u8 drdy_mask;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
 
+	if (!sdata->sensor->drdy_irq.addr)
+		return 0;
+
 	/* Enable/Disable the interrupt generator 1. */
 	if (sdata->sensor->drdy_irq.ig1.en_addr > 0) {
 		err = st_sensors_write_data_with_mask(indio_dev,
@@ -240,10 +322,14 @@ int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
 			goto st_accel_set_dataready_irq_error;
 	}
 
+	if (sdata->drdy_int_pin == 1)
+		drdy_mask = sdata->sensor->drdy_irq.mask_int1;
+	else
+		drdy_mask = sdata->sensor->drdy_irq.mask_int2;
+
 	/* Enable/Disable the interrupt generator for data ready. */
 	err = st_sensors_write_data_with_mask(indio_dev,
-			sdata->sensor->drdy_irq.addr,
-			sdata->sensor->drdy_irq.mask, (int)enable);
+			sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable);
 
 st_accel_set_dataready_irq_error:
 	return err;
@@ -274,21 +360,31 @@ st_sensors_match_scale_error:
 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
 
 static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
-							u8 ch_addr, int *data)
+				struct iio_chan_spec const *ch, int *data)
 {
 	int err;
-	u8 outdata[ST_SENSORS_BYTE_FOR_CHANNEL];
+	u8 *outdata;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
+	unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
+
+	outdata = kmalloc(byte_for_channel, GFP_KERNEL);
+	if (!outdata)
+		return -ENOMEM;
 
 	err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
-				ch_addr, ST_SENSORS_BYTE_FOR_CHANNEL,
+				ch->address, byte_for_channel,
 				outdata, sdata->multiread_bit);
 	if (err < 0)
-		goto read_error;
+		goto st_sensors_free_memory;
+
+	if (byte_for_channel == 2)
+		*data = (s16)get_unaligned_le16(outdata);
+	else if (byte_for_channel == 3)
+		*data = (s32)st_sensors_get_unaligned_le24(outdata);
 
-	*data = (s16)get_unaligned_le16(outdata);
+st_sensors_free_memory:
+	kfree(outdata);
 
-read_error:
 	return err;
 }
 
@@ -301,26 +397,25 @@ int st_sensors_read_info_raw(struct iio_dev *indio_dev,
 	mutex_lock(&indio_dev->mlock);
 	if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
 		err = -EBUSY;
-		goto read_error;
+		goto out;
 	} else {
 		err = st_sensors_set_enable(indio_dev, true);
 		if (err < 0)
-			goto read_error;
+			goto out;
 
 		msleep((sdata->sensor->bootime * 1000) / sdata->odr);
-		err = st_sensors_read_axis_data(indio_dev, ch->address, val);
+		err = st_sensors_read_axis_data(indio_dev, ch, val);
 		if (err < 0)
-			goto read_error;
+			goto out;
 
 		*val = *val >> ch->scan_type.shift;
+
+		err = st_sensors_set_enable(indio_dev, false);
 	}
+out:
 	mutex_unlock(&indio_dev->mlock);
 
 	return err;
-
-read_error:
-	mutex_unlock(&indio_dev->mlock);
-	return err;
 }
 EXPORT_SYMBOL(st_sensors_read_info_raw);
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_spi.c b/drivers/iio/common/st_sensors/st_sensors_spi.c
index f0aa2f10522..251baf6abc2 100644
--- a/drivers/iio/common/st_sensors/st_sensors_spi.c
+++ b/drivers/iio/common/st_sensors/st_sensors_spi.c
@@ -29,7 +29,6 @@ static unsigned int st_sensors_spi_get_irq(struct iio_dev *indio_dev)
 static int st_sensors_spi_read(struct st_sensor_transfer_buffer *tb,
 	struct device *dev, u8 reg_addr, int len, u8 *data, bool multiread_bit)
 {
-	struct spi_message msg;
 	int err;
 
 	struct spi_transfer xfers[] = {
@@ -51,10 +50,7 @@ static int st_sensors_spi_read(struct st_sensor_transfer_buffer *tb,
 	else
 		tb->tx_buf[0] = reg_addr | ST_SENSORS_SPI_READ;
 
-	spi_message_init(&msg);
-	spi_message_add_tail(&xfers[0], &msg);
-	spi_message_add_tail(&xfers[1], &msg);
-	err = spi_sync(to_spi_device(dev), &msg);
+	err = spi_sync_transfer(to_spi_device(dev), xfers, ARRAY_SIZE(xfers));
 	if (err)
 		goto acc_spi_read_error;
 
@@ -83,7 +79,6 @@ static int st_sensors_spi_read_multiple_byte(
 static int st_sensors_spi_write_byte(struct st_sensor_transfer_buffer *tb,
 				struct device *dev, u8 reg_addr, u8 data)
 {
-	struct spi_message msg;
 	int err;
 
 	struct spi_transfer xfers = {
@@ -96,9 +91,7 @@ static int st_sensors_spi_write_byte(struct st_sensor_transfer_buffer *tb,
 	tb->tx_buf[0] = reg_addr;
 	tb->tx_buf[1] = data;
 
-	spi_message_init(&msg);
-	spi_message_add_tail(&xfers, &msg);
-	err = spi_sync(to_spi_device(dev), &msg);
+	err = spi_sync_transfer(to_spi_device(dev), &xfers, 1);
 	mutex_unlock(&tb->buf_lock);
 
 	return err;
diff --git a/drivers/iio/common/st_sensors/st_sensors_trigger.c b/drivers/iio/common/st_sensors/st_sensors_trigger.c
index 139ed030abb..8fc3a97eb26 100644
--- a/drivers/iio/common/st_sensors/st_sensors_trigger.c
+++ b/drivers/iio/common/st_sensors/st_sensors_trigger.c
@@ -40,7 +40,7 @@ int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
 	if (err)
 		goto request_irq_error;
 
-	sdata->trig->private_data = indio_dev;
+	iio_trigger_set_drvdata(sdata->trig, indio_dev);
 	sdata->trig->ops = trigger_ops;
 	sdata->trig->dev.parent = sdata->dev;