2 files changed, 103 insertions, 30 deletions

diff --git a/drivers/iio/common/st_sensors/st_sensors_buffer.c b/drivers/iio/common/st_sensors/st_sensors_buffer.c
index 71a2c5f63b9..1665c8e4b62 100644
--- a/drivers/iio/common/st_sensors/st_sensors_buffer.c
+++ b/drivers/iio/common/st_sensors/st_sensors_buffer.c
@@ -113,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 865b1781df6..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>
@@ -22,7 +23,7 @@
 
 static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
 {
-	return ((s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8);
+	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,
@@ -118,7 +119,7 @@ st_sensors_match_odr_error:
 }
 
 static int st_sensors_set_fullscale(struct iio_dev *indio_dev,
-							unsigned int fs)
+								unsigned int fs)
 {
 	int err, i = 0;
 	struct st_sensor_data *sdata = iio_priv(indio_dev);
@@ -198,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);
@@ -234,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,
@@ -245,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;
@@ -287,10 +368,8 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
 	unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
 
 	outdata = kmalloc(byte_for_channel, GFP_KERNEL);
-	if (!outdata) {
-		err = -EINVAL;
-		goto st_sensors_read_axis_data_error;
-	}
+	if (!outdata)
+		return -ENOMEM;
 
 	err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
 				ch->address, byte_for_channel,
@@ -305,7 +384,7 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
 
 st_sensors_free_memory:
 	kfree(outdata);
-st_sensors_read_axis_data_error:
+
 	return err;
 }
 
@@ -318,28 +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, 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);