[PATCH v2 1/2] iio: accel: STK8BA50: suspend sensor on init errors

[PATCH v2 1/2] iio: accel: STK8BA50: suspend sensor on init errors

[Tiberiu Breana]

Put chip in 'suspend' mode in case something goes wrong
during probe.

Signed-off-by: Tiberiu Breana <tiberiu.a.breana@intel.com>
---
no changes since v1
---
 drivers/iio/accel/stk8ba50.c | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/drivers/iio/accel/stk8ba50.c b/drivers/iio/accel/stk8ba50.c
index 92229bd..9836880 100644
--- a/drivers/iio/accel/stk8ba50.c
+++ b/drivers/iio/accel/stk8ba50.c
@@ -265,7 +265,7 @@ static int stk8ba50_probe(struct i2c_client *client,
 			STK8BA50_REG_SWRST, STK8BA50_RESET_CMD);
 	if (ret < 0) {
 		dev_err(&client->dev, "failed to reset sensor\n");
-		return ret;
+		goto err_power_off;
 	}
 
 	/* The default range is +/-2g */
@@ -277,10 +277,14 @@ static int stk8ba50_probe(struct i2c_client *client,
 	ret = iio_device_register(indio_dev);
 	if (ret < 0) {
 		dev_err(&client->dev, "device_register failed\n");
-		stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
+		goto err_power_off;
 	}
 
 	return ret;
+
+err_power_off:
+	stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
+	return ret;
 }
 
 static int stk8ba50_remove(struct i2c_client *client)
-- 
1.9.1

--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in

[PATCH v2 2/2] iio: accel: Add trigger support for STK8BA50

[Tiberiu Breana]

Add data-ready interrupts and trigger support for STK8BA50.

Additional changes:
- read_accel now returns raw acceleration data instead of the
  sign_extend32 value
- read_raw will now enable/disable the sensor with each reading

Signed-off-by: Tiberiu Breana <tiberiu.a.breana@intel.com>
---
v2:
- addressed Peter's comments
- added chan_info's shift field
- added an optimization to bulk read all accel channel data
  with one i2c operation when requested
---
 drivers/iio/accel/stk8ba50.c | 307 +++++++++++++++++++++++++++++++++++++++----
 1 file changed, 282 insertions(+), 25 deletions(-)

diff --git a/drivers/iio/accel/stk8ba50.c b/drivers/iio/accel/stk8ba50.c
index 9836880..f18731c 100644
--- a/drivers/iio/accel/stk8ba50.c
+++ b/drivers/iio/accel/stk8ba50.c
@@ -11,11 +11,17 @@
  */
 
 #include <linux/acpi.h>
+#include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
+#include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/iio/buffer.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
 
 #define STK8BA50_REG_XOUT			0x02
 #define STK8BA50_REG_YOUT			0x04
@@ -24,6 +30,8 @@
 #define STK8BA50_REG_BWSEL			0x10
 #define STK8BA50_REG_POWMODE			0x11
 #define STK8BA50_REG_SWRST			0x14
+#define STK8BA50_REG_INTEN2			0x17
+#define STK8BA50_REG_INTMAP2			0x1A
 
 #define STK8BA50_MODE_NORMAL			0
 #define STK8BA50_MODE_SUSPEND			1
@@ -31,8 +39,14 @@
 #define STK8BA50_DATA_SHIFT			6
 #define STK8BA50_RESET_CMD			0xB6
 #define STK8BA50_SR_1792HZ_IDX			7
+#define STK8BA50_DREADY_INT_MASK		0x10
+#define STK8BA50_DREADY_INT_MAP			0x81
+#define STK8BA50_ALL_CHANNEL_MASK		7
+#define STK8BA50_ALL_CHANNEL_SIZE		6
 
 #define STK8BA50_DRIVER_NAME			"stk8ba50"
+#define STK8BA50_GPIO				"stk8ba50_gpio"
+#define STK8BA50_IRQ_NAME			"stk8ba50_event"
 
 #define STK8BA50_SCALE_AVAIL			"0.0384 0.0767 0.1534 0.3069"
 
@@ -73,9 +87,17 @@ struct stk8ba50_data {
 	struct mutex lock;
 	int range;
 	u8 sample_rate_idx;
+	struct iio_trigger *dready_trig;
+	bool dready_trigger_on;
+	/*
+	 * 3 x 16-bit channels (10-bit data, 6-bit padding) +
+	 * 1 x 16 padding +
+	 * 4 x 16 64-bit timestamp
+	 */
+	s16 buffer[8];
 };
 
-#define STK8BA50_ACCEL_CHANNEL(reg, axis) {				\
+#define STK8BA50_ACCEL_CHANNEL(index, reg, axis) {			\
 	.type = IIO_ACCEL,						\
 	.address = reg,							\
 	.modified = 1,							\
@@ -83,12 +105,21 @@ struct stk8ba50_data {
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
 	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),		\
 				    BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
+	.scan_index = index,						\
+	.scan_type = {							\
+		.sign = 's',						\
+		.realbits = 10,						\
+		.storagebits = 16,					\
+		.shift = STK8BA50_DATA_SHIFT,				\
+		.endianness = IIO_CPU,					\
+	},								\
 }
 
 static const struct iio_chan_spec stk8ba50_channels[] = {
-	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_XOUT, X),
-	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_YOUT, Y),
-	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_ZOUT, Z),
+	STK8BA50_ACCEL_CHANNEL(0, STK8BA50_REG_XOUT, X),
+	STK8BA50_ACCEL_CHANNEL(1, STK8BA50_REG_YOUT, Y),
+	STK8BA50_ACCEL_CHANNEL(2, STK8BA50_REG_ZOUT, Z),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
 };
 
 static IIO_CONST_ATTR(in_accel_scale_available, STK8BA50_SCALE_AVAIL);
@@ -116,7 +147,61 @@ static int stk8ba50_read_accel(struct stk8ba50_data *data, u8 reg)
 		return ret;
 	}
 
-	return sign_extend32(ret >> STK8BA50_DATA_SHIFT, 9);
+	return ret;
+}
+
+static int stk8ba50_data_rdy_trigger_set_state(struct iio_trigger *trig,
+					       bool state)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct stk8ba50_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (state)
+		ret = i2c_smbus_write_byte_data(data->client,
+			STK8BA50_REG_INTEN2, STK8BA50_DREADY_INT_MASK);
+	else
+		ret = i2c_smbus_write_byte_data(data->client,
+			STK8BA50_REG_INTEN2, 0x00);
+
+	if (ret < 0)
+		dev_err(&data->client->dev, "failed to set trigger state\n");
+	else
+		data->dready_trigger_on = state;
+
+	return ret;
+}
+
+static const struct iio_trigger_ops stk8ba50_trigger_ops = {
+	.set_trigger_state = stk8ba50_data_rdy_trigger_set_state,
+	.owner = THIS_MODULE,
+};
+
+static int stk8ba50_set_power(struct stk8ba50_data *data, bool mode)
+{
+	int ret;
+	u8 masked_reg;
+	struct i2c_client *client = data->client;
+
+	ret = i2c_smbus_read_byte_data(client, STK8BA50_REG_POWMODE);
+	if (ret < 0)
+		goto exit_err;
+
+	if (mode)
+		masked_reg = ret | STK8BA50_MODE_POWERBIT;
+	else
+		masked_reg = ret & (~STK8BA50_MODE_POWERBIT);
+
+	ret = i2c_smbus_write_byte_data(client, STK8BA50_REG_POWMODE,
+					masked_reg);
+	if (ret < 0)
+		goto exit_err;
+
+	return ret;
+
+exit_err:
+	dev_err(&client->dev, "failed to change sensor mode\n");
+	return ret;
 }
 
 static int stk8ba50_read_raw(struct iio_dev *indio_dev,
@@ -124,11 +209,26 @@ static int stk8ba50_read_raw(struct iio_dev *indio_dev,
 			     int *val, int *val2, long mask)
 {
 	struct stk8ba50_data *data = iio_priv(indio_dev);
+	int ret;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
+		if (iio_buffer_enabled(indio_dev))
+			return -EBUSY;
 		mutex_lock(&data->lock);
-		*val = stk8ba50_read_accel(data, chan->address);
+		ret = stk8ba50_set_power(data, STK8BA50_MODE_NORMAL);
+		if (ret < 0) {
+			mutex_unlock(&data->lock);
+			return -EINVAL;
+		}
+		ret = stk8ba50_read_accel(data, chan->address);
+		if (ret < 0) {
+			stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
+			mutex_unlock(&data->lock);
+			return -EINVAL;
+		}
+		*val = sign_extend32(ret >> STK8BA50_DATA_SHIFT, 9);
+		stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
 		mutex_unlock(&data->lock);
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_SCALE:
@@ -208,30 +308,121 @@ static const struct iio_info stk8ba50_info = {
 	.attrs			= &stk8ba50_attribute_group,
 };
 
-static int stk8ba50_set_power(struct stk8ba50_data *data, bool mode)
+static irqreturn_t stk8ba50_trigger_handler(int irq, void *p)
 {
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct stk8ba50_data *data = iio_priv(indio_dev);
+	int bit, ret, i = 0;
+	u8 buffer[STK8BA50_ALL_CHANNEL_SIZE];
+
+	mutex_lock(&data->lock);
+	/*
+	 * Do a bulk read if all channels are requested,
+	 * from 0x02 (XOUT1) to 0x07 (ZOUT2)
+	 */
+	if (*(indio_dev->active_scan_mask) == STK8BA50_ALL_CHANNEL_MASK) {
+		ret = i2c_smbus_read_i2c_block_data(data->client,
+						    STK8BA50_REG_XOUT,
+						    STK8BA50_ALL_CHANNEL_SIZE,
+						    buffer);
+		if (ret < STK8BA50_ALL_CHANNEL_SIZE) {
+			dev_err(&data->client->dev, "register read failed\n");
+			mutex_unlock(&data->lock);
+			goto err;
+		}
+		data->buffer[0] = buffer[1] << 8 | buffer[0];
+		data->buffer[1] = buffer[3] << 8 | buffer[2];
+		data->buffer[2] = buffer[5] << 8 | buffer[4];
+	} else {
+		for_each_set_bit(bit, indio_dev->active_scan_mask,
+				 indio_dev->masklength) {
+			switch (bit) {
+			case 0:
+				ret = stk8ba50_read_accel(data,
+							  STK8BA50_REG_XOUT);
+				break;
+			case 1:
+				ret = stk8ba50_read_accel(data,
+							  STK8BA50_REG_YOUT);
+				break;
+			case 2:
+				ret = stk8ba50_read_accel(data,
+							  STK8BA50_REG_ZOUT);
+				break;
+			default:
+				return -EINVAL;
+			}
+			if (ret < 0) {
+				mutex_unlock(&data->lock);
+				goto err;
+			}
+			data->buffer[i++] = ret;
+		}
+	}
+	mutex_unlock(&data->lock);
+
+	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
+					   pf->timestamp);
+err:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t stk8ba50_data_rdy_trig_poll(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct stk8ba50_data *data = iio_priv(indio_dev);
+
+	if (data->dready_trigger_on)
+		iio_trigger_poll(data->dready_trig);
+
+	return IRQ_HANDLED;
+}
+
+static int stk8ba50_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct stk8ba50_data *data = iio_priv(indio_dev);
+
+	return stk8ba50_set_power(data, STK8BA50_MODE_NORMAL);
+}
+
+static int stk8ba50_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct stk8ba50_data *data = iio_priv(indio_dev);
+
+	return stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
+}
+
+static const struct iio_buffer_setup_ops stk8ba50_buffer_setup_ops = {
+	.preenable   = stk8ba50_buffer_preenable,
+	.postenable  = iio_triggered_buffer_postenable,
+	.predisable  = iio_triggered_buffer_predisable,
+	.postdisable = stk8ba50_buffer_postdisable,
+};
+
+static int stk8ba50_gpio_probe(struct i2c_client *client)
+{
+	struct device *dev;
+	struct gpio_desc *gpio;
 	int ret;
-	u8 masked_reg;
-	struct i2c_client *client = data->client;
 
-	ret = i2c_smbus_read_byte_data(client, STK8BA50_REG_POWMODE);
-	if (ret < 0)
-		goto exit_err;
+	if (!client)
+		return -EINVAL;
 
-	if (mode)
-		masked_reg = ret | STK8BA50_MODE_POWERBIT;
-	else
-		masked_reg = ret & (~STK8BA50_MODE_POWERBIT);
+	dev = &client->dev;
 
-	ret = i2c_smbus_write_byte_data(client, STK8BA50_REG_POWMODE,
-					masked_reg);
-	if (ret < 0)
-		goto exit_err;
+	/* data ready gpio interrupt pin */
+	gpio = devm_gpiod_get_index(dev, STK8BA50_GPIO, 0, GPIOD_IN);
+	if (IS_ERR(gpio)) {
+		dev_err(dev, "acpi gpio get index failed\n");
+		return PTR_ERR(gpio);
+	}
 
-	return ret;
+	ret = gpiod_to_irq(gpio);
+	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
 
-exit_err:
-	dev_err(&client->dev, "failed to change sensor mode\n");
 	return ret;
 }
 
@@ -274,14 +465,74 @@ static int stk8ba50_probe(struct i2c_client *client,
 	/* The default sampling rate is 1792 Hz (maximum) */
 	data->sample_rate_idx = STK8BA50_SR_1792HZ_IDX;
 
+	/* Set up interrupts */
+	ret = i2c_smbus_write_byte_data(client,
+			STK8BA50_REG_INTEN2, STK8BA50_DREADY_INT_MASK);
+	ret = i2c_smbus_write_byte_data(client,
+			STK8BA50_REG_INTMAP2, STK8BA50_DREADY_INT_MAP);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to set up interrupts\n");
+		goto err_power_off;
+	}
+
+	if (client->irq < 0)
+		client->irq = stk8ba50_gpio_probe(client);
+
+	if (client->irq >= 0) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+						stk8ba50_data_rdy_trig_poll,
+						NULL,
+						IRQF_TRIGGER_RISING |
+						IRQF_ONESHOT,
+						STK8BA50_IRQ_NAME,
+						indio_dev);
+		if (ret < 0) {
+			dev_err(&client->dev, "request irq %d failed\n",
+				client->irq);
+			goto err_power_off;
+		}
+
+		data->dready_trig = devm_iio_trigger_alloc(&client->dev,
+							   "%s-dev%d",
+							   indio_dev->name,
+							   indio_dev->id);
+		if (!data->dready_trig) {
+			ret = -ENOMEM;
+			goto err_power_off;
+		}
+
+		data->dready_trig->dev.parent = &client->dev;
+		data->dready_trig->ops = &stk8ba50_trigger_ops;
+		iio_trigger_set_drvdata(data->dready_trig, indio_dev);
+		ret = iio_trigger_register(data->dready_trig);
+		if (ret) {
+			dev_err(&client->dev, "iio trigger register failed\n");
+			goto err_power_off;
+		}
+	}
+
+	ret = iio_triggered_buffer_setup(indio_dev,
+					 iio_pollfunc_store_time,
+					 stk8ba50_trigger_handler,
+					 &stk8ba50_buffer_setup_ops);
+	if (ret < 0) {
+		dev_err(&client->dev, "iio triggered buffer setup failed\n");
+		goto err_trigger_unregister;
+	}
+
 	ret = iio_device_register(indio_dev);
 	if (ret < 0) {
 		dev_err(&client->dev, "device_register failed\n");
-		goto err_power_off;
+		goto err_buffer_cleanup;
 	}
 
 	return ret;
 
+err_buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
+err_trigger_unregister:
+	if (data->dready_trig)
+		iio_trigger_unregister(data->dready_trig);
 err_power_off:
 	stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
 	return ret;
@@ -290,10 +541,16 @@ err_power_off:
 static int stk8ba50_remove(struct i2c_client *client)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct stk8ba50_data *data = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
 
-	return stk8ba50_set_power(iio_priv(indio_dev), STK8BA50_MODE_SUSPEND);
+	if (data->dready_trig) {
+		iio_triggered_buffer_cleanup(indio_dev);
+		iio_trigger_unregister(data->dready_trig);
+	}
+
+	return stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
 }
 
 #ifdef CONFIG_PM_SLEEP
-- 
1.9.1

--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in

Re: [PATCH v2 2/2] iio: accel: Add trigger support for STK8BA50

[Jonathan Cameron]

On 19/06/15 15:56, Tiberiu Breana wrote:
> Add data-ready interrupts and trigger support for STK8BA50.
> 
> Additional changes:
> - read_accel now returns raw acceleration data instead of the
>   sign_extend32 value
> - read_raw will now enable/disable the sensor with each reading
> 
> Signed-off-by: Tiberiu Breana <tiberiu.a.breana@intel.com>

Again, a few little bits and bobs left in here, but nothing terribly
significant.

J
> ---
> v2:
> - addressed Peter's comments
> - added chan_info's shift field
> - added an optimization to bulk read all accel channel data
>   with one i2c operation when requested
> ---
>  drivers/iio/accel/stk8ba50.c | 307 +++++++++++++++++++++++++++++++++++++++----
>  1 file changed, 282 insertions(+), 25 deletions(-)
> 
> diff --git a/drivers/iio/accel/stk8ba50.c b/drivers/iio/accel/stk8ba50.c
> index 9836880..f18731c 100644
> --- a/drivers/iio/accel/stk8ba50.c
> +++ b/drivers/iio/accel/stk8ba50.c
> @@ -11,11 +11,17 @@
>   */
>  
>  #include <linux/acpi.h>
> +#include <linux/gpio/consumer.h>
>  #include <linux/i2c.h>
> +#include <linux/interrupt.h>
>  #include <linux/kernel.h>
>  #include <linux/module.h>
> +#include <linux/iio/buffer.h>
>  #include <linux/iio/iio.h>
>  #include <linux/iio/sysfs.h>
> +#include <linux/iio/trigger.h>
> +#include <linux/iio/triggered_buffer.h>
> +#include <linux/iio/trigger_consumer.h>
>  
>  #define STK8BA50_REG_XOUT			0x02
>  #define STK8BA50_REG_YOUT			0x04
> @@ -24,6 +30,8 @@
>  #define STK8BA50_REG_BWSEL			0x10
>  #define STK8BA50_REG_POWMODE			0x11
>  #define STK8BA50_REG_SWRST			0x14
> +#define STK8BA50_REG_INTEN2			0x17
> +#define STK8BA50_REG_INTMAP2			0x1A
>  
>  #define STK8BA50_MODE_NORMAL			0
>  #define STK8BA50_MODE_SUSPEND			1
> @@ -31,8 +39,14 @@
>  #define STK8BA50_DATA_SHIFT			6
>  #define STK8BA50_RESET_CMD			0xB6
>  #define STK8BA50_SR_1792HZ_IDX			7
> +#define STK8BA50_DREADY_INT_MASK		0x10
> +#define STK8BA50_DREADY_INT_MAP			0x81
> +#define STK8BA50_ALL_CHANNEL_MASK		7
> +#define STK8BA50_ALL_CHANNEL_SIZE		6
>  
>  #define STK8BA50_DRIVER_NAME			"stk8ba50"
> +#define STK8BA50_GPIO				"stk8ba50_gpio"
> +#define STK8BA50_IRQ_NAME			"stk8ba50_event"
>  
>  #define STK8BA50_SCALE_AVAIL			"0.0384 0.0767 0.1534 0.3069"
>  
> @@ -73,9 +87,17 @@ struct stk8ba50_data {
>  	struct mutex lock;
>  	int range;
>  	u8 sample_rate_idx;
> +	struct iio_trigger *dready_trig;
> +	bool dready_trigger_on;
> +	/*
> +	 * 3 x 16-bit channels (10-bit data, 6-bit padding) +
> +	 * 1 x 16 padding +
> +	 * 4 x 16 64-bit timestamp
> +	 */
> +	s16 buffer[8];
>  };
>  
> -#define STK8BA50_ACCEL_CHANNEL(reg, axis) {				\
> +#define STK8BA50_ACCEL_CHANNEL(index, reg, axis) {			\
>  	.type = IIO_ACCEL,						\
>  	.address = reg,							\
>  	.modified = 1,							\
> @@ -83,12 +105,21 @@ struct stk8ba50_data {
>  	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
>  	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),		\
>  				    BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
> +	.scan_index = index,						\
> +	.scan_type = {							\
> +		.sign = 's',						\
> +		.realbits = 10,						\
> +		.storagebits = 16,					\
> +		.shift = STK8BA50_DATA_SHIFT,				\
> +		.endianness = IIO_CPU,					\
> +	},								\
>  }
>  
>  static const struct iio_chan_spec stk8ba50_channels[] = {
> -	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_XOUT, X),
> -	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_YOUT, Y),
> -	STK8BA50_ACCEL_CHANNEL(STK8BA50_REG_ZOUT, Z),
> +	STK8BA50_ACCEL_CHANNEL(0, STK8BA50_REG_XOUT, X),
> +	STK8BA50_ACCEL_CHANNEL(1, STK8BA50_REG_YOUT, Y),
> +	STK8BA50_ACCEL_CHANNEL(2, STK8BA50_REG_ZOUT, Z),
> +	IIO_CHAN_SOFT_TIMESTAMP(3),
>  };
>  
>  static IIO_CONST_ATTR(in_accel_scale_available, STK8BA50_SCALE_AVAIL);
> @@ -116,7 +147,61 @@ static int stk8ba50_read_accel(struct stk8ba50_data *data, u8 reg)
>  		return ret;
>  	}
>  
> -	return sign_extend32(ret >> STK8BA50_DATA_SHIFT, 9);
> +	return ret;
> +}
> +
> +static int stk8ba50_data_rdy_trigger_set_state(struct iio_trigger *trig,
> +					       bool state)
> +{
> +	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
> +	struct stk8ba50_data *data = iio_priv(indio_dev);
> +	int ret;
> +
> +	if (state)
> +		ret = i2c_smbus_write_byte_data(data->client,
> +			STK8BA50_REG_INTEN2, STK8BA50_DREADY_INT_MASK);
> +	else
> +		ret = i2c_smbus_write_byte_data(data->client,
> +			STK8BA50_REG_INTEN2, 0x00);
> +
> +	if (ret < 0)
> +		dev_err(&data->client->dev, "failed to set trigger state\n");
> +	else
> +		data->dready_trigger_on = state;
> +
> +	return ret;
> +}
> +
> +static const struct iio_trigger_ops stk8ba50_trigger_ops = {
> +	.set_trigger_state = stk8ba50_data_rdy_trigger_set_state,
> +	.owner = THIS_MODULE,
> +};
> +
> +static int stk8ba50_set_power(struct stk8ba50_data *data, bool mode)
> +{
> +	int ret;
> +	u8 masked_reg;
> +	struct i2c_client *client = data->client;
> +
> +	ret = i2c_smbus_read_byte_data(client, STK8BA50_REG_POWMODE);
> +	if (ret < 0)
> +		goto exit_err;
> +
> +	if (mode)
> +		masked_reg = ret | STK8BA50_MODE_POWERBIT;
> +	else
> +		masked_reg = ret & (~STK8BA50_MODE_POWERBIT);
> +
> +	ret = i2c_smbus_write_byte_data(client, STK8BA50_REG_POWMODE,
> +					masked_reg);
> +	if (ret < 0)
> +		goto exit_err;
> +
> +	return ret;
> +
> +exit_err:
> +	dev_err(&client->dev, "failed to change sensor mode\n");
> +	return ret;
>  }
>  
>  static int stk8ba50_read_raw(struct iio_dev *indio_dev,
> @@ -124,11 +209,26 @@ static int stk8ba50_read_raw(struct iio_dev *indio_dev,
>  			     int *val, int *val2, long mask)
>  {
>  	struct stk8ba50_data *data = iio_priv(indio_dev);
> +	int ret;
>  
>  	switch (mask) {
>  	case IIO_CHAN_INFO_RAW:
> +		if (iio_buffer_enabled(indio_dev))
> +			return -EBUSY;
>  		mutex_lock(&data->lock);
> -		*val = stk8ba50_read_accel(data, chan->address);
> +		ret = stk8ba50_set_power(data, STK8BA50_MODE_NORMAL);
> +		if (ret < 0) {
> +			mutex_unlock(&data->lock);
> +			return -EINVAL;
> +		}
> +		ret = stk8ba50_read_accel(data, chan->address);
> +		if (ret < 0) {
> +			stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
> +			mutex_unlock(&data->lock);
> +			return -EINVAL;
> +		}
> +		*val = sign_extend32(ret >> STK8BA50_DATA_SHIFT, 9);
> +		stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
>  		mutex_unlock(&data->lock);
>  		return IIO_VAL_INT;
>  	case IIO_CHAN_INFO_SCALE:
> @@ -208,30 +308,121 @@ static const struct iio_info stk8ba50_info = {
>  	.attrs			= &stk8ba50_attribute_group,
>  };
>  
> -static int stk8ba50_set_power(struct stk8ba50_data *data, bool mode)
> +static irqreturn_t stk8ba50_trigger_handler(int irq, void *p)
>  {
> +	struct iio_poll_func *pf = p;
> +	struct iio_dev *indio_dev = pf->indio_dev;
> +	struct stk8ba50_data *data = iio_priv(indio_dev);
> +	int bit, ret, i = 0;
> +	u8 buffer[STK8BA50_ALL_CHANNEL_SIZE];
> +
> +	mutex_lock(&data->lock);
> +	/*
> +	 * Do a bulk read if all channels are requested,
> +	 * from 0x02 (XOUT1) to 0x07 (ZOUT2)
> +	 */
> +	if (*(indio_dev->active_scan_mask) == STK8BA50_ALL_CHANNEL_MASK) {
> +		ret = i2c_smbus_read_i2c_block_data(data->client,
> +						    STK8BA50_REG_XOUT,
> +						    STK8BA50_ALL_CHANNEL_SIZE,
> +						    buffer);
> +		if (ret < STK8BA50_ALL_CHANNEL_SIZE) {
> +			dev_err(&data->client->dev, "register read failed\n");
> +			mutex_unlock(&data->lock);
> +			goto err;
> +		}
> +		data->buffer[0] = buffer[1] << 8 | buffer[0];
> +		data->buffer[1] = buffer[3] << 8 | buffer[2];
> +		data->buffer[2] = buffer[5] << 8 | buffer[4];
Hmm, do this with an endian conversion as it'll be a free copy for
one of the two cases.
Could read directly into data->buffer and if the endian conversion
is a noop hopefully the compiler would get rid of it entirely.

> +	} else {
> +		for_each_set_bit(bit, indio_dev->active_scan_mask,
> +				 indio_dev->masklength) {
> +			switch (bit) {
This is begging for a little static const look up table and replacing
the switch.
> +			case 0:
> +				ret = stk8ba50_read_accel(data,
> +							  STK8BA50_REG_XOUT);
> +				break;
> +			case 1:
> +				ret = stk8ba50_read_accel(data,
> +							  STK8BA50_REG_YOUT);
> +				break;
> +			case 2:
> +				ret = stk8ba50_read_accel(data,
> +							  STK8BA50_REG_ZOUT);
> +				break;
> +			default:
> +				return -EINVAL;
> +			}
> +			if (ret < 0) {
> +				mutex_unlock(&data->lock);
> +				goto err;
> +			}
> +			data->buffer[i++] = ret;
> +		}
> +	}
> +	mutex_unlock(&data->lock);
> +
> +	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
> +					   pf->timestamp);
> +err:
> +	iio_trigger_notify_done(indio_dev->trig);
> +
> +	return IRQ_HANDLED;
> +}
> +
> +static irqreturn_t stk8ba50_data_rdy_trig_poll(int irq, void *private)
> +{
> +	struct iio_dev *indio_dev = private;
> +	struct stk8ba50_data *data = iio_priv(indio_dev);
> +
> +	if (data->dready_trigger_on)
> +		iio_trigger_poll(data->dready_trig);
> +
> +	return IRQ_HANDLED;
> +}
> +
> +static int stk8ba50_buffer_preenable(struct iio_dev *indio_dev)
> +{
> +	struct stk8ba50_data *data = iio_priv(indio_dev);
> +
> +	return stk8ba50_set_power(data, STK8BA50_MODE_NORMAL);
> +}
> +
> +static int stk8ba50_buffer_postdisable(struct iio_dev *indio_dev)
> +{
> +	struct stk8ba50_data *data = iio_priv(indio_dev);
> +
> +	return stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
> +}
> +
> +static const struct iio_buffer_setup_ops stk8ba50_buffer_setup_ops = {
> +	.preenable   = stk8ba50_buffer_preenable,
> +	.postenable  = iio_triggered_buffer_postenable,
> +	.predisable  = iio_triggered_buffer_predisable,
> +	.postdisable = stk8ba50_buffer_postdisable,
> +};
> +
> +static int stk8ba50_gpio_probe(struct i2c_client *client)
> +{
> +	struct device *dev;
> +	struct gpio_desc *gpio;
>  	int ret;
> -	u8 masked_reg;
> -	struct i2c_client *client = data->client;
>  
> -	ret = i2c_smbus_read_byte_data(client, STK8BA50_REG_POWMODE);
> -	if (ret < 0)
> -		goto exit_err;
> +	if (!client)
> +		return -EINVAL;
>  
> -	if (mode)
> -		masked_reg = ret | STK8BA50_MODE_POWERBIT;
> -	else
> -		masked_reg = ret & (~STK8BA50_MODE_POWERBIT);
> +	dev = &client->dev;
>  
> -	ret = i2c_smbus_write_byte_data(client, STK8BA50_REG_POWMODE,
> -					masked_reg);
> -	if (ret < 0)
> -		goto exit_err;
> +	/* data ready gpio interrupt pin */
> +	gpio = devm_gpiod_get_index(dev, STK8BA50_GPIO, 0, GPIOD_IN);
> +	if (IS_ERR(gpio)) {
> +		dev_err(dev, "acpi gpio get index failed\n");
> +		return PTR_ERR(gpio);
> +	}
>  
> -	return ret;
> +	ret = gpiod_to_irq(gpio);
> +	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
>  
> -exit_err:
> -	dev_err(&client->dev, "failed to change sensor mode\n");
>  	return ret;
>  }
>  
> @@ -274,14 +465,74 @@ static int stk8ba50_probe(struct i2c_client *client,
>  	/* The default sampling rate is 1792 Hz (maximum) */
>  	data->sample_rate_idx = STK8BA50_SR_1792HZ_IDX;
>  
> +	/* Set up interrupts */
> +	ret = i2c_smbus_write_byte_data(client,
> +			STK8BA50_REG_INTEN2, STK8BA50_DREADY_INT_MASK);
  Error checking here?
> +	ret = i2c_smbus_write_byte_data(client,
> +			STK8BA50_REG_INTMAP2, STK8BA50_DREADY_INT_MAP);
> +	if (ret < 0) {
> +		dev_err(&client->dev, "failed to set up interrupts\n");
> +		goto err_power_off;
> +	}
> +
> +	if (client->irq < 0)
> +		client->irq = stk8ba50_gpio_probe(client);
> +
> +	if (client->irq >= 0) {
> +		ret = devm_request_threaded_irq(&client->dev, client->irq,
> +						stk8ba50_data_rdy_trig_poll,
> +						NULL,
> +						IRQF_TRIGGER_RISING |
> +						IRQF_ONESHOT,
> +						STK8BA50_IRQ_NAME,
> +						indio_dev);
> +		if (ret < 0) {
> +			dev_err(&client->dev, "request irq %d failed\n",
> +				client->irq);
> +			goto err_power_off;
> +		}
> +
> +		data->dready_trig = devm_iio_trigger_alloc(&client->dev,
> +							   "%s-dev%d",
> +							   indio_dev->name,
> +							   indio_dev->id);
> +		if (!data->dready_trig) {
> +			ret = -ENOMEM;
> +			goto err_power_off;
> +		}
> +
> +		data->dready_trig->dev.parent = &client->dev;
> +		data->dready_trig->ops = &stk8ba50_trigger_ops;
> +		iio_trigger_set_drvdata(data->dready_trig, indio_dev);
> +		ret = iio_trigger_register(data->dready_trig);
> +		if (ret) {
> +			dev_err(&client->dev, "iio trigger register failed\n");
> +			goto err_power_off;
> +		}
> +	}
> +
> +	ret = iio_triggered_buffer_setup(indio_dev,
> +					 iio_pollfunc_store_time,
> +					 stk8ba50_trigger_handler,
> +					 &stk8ba50_buffer_setup_ops);
> +	if (ret < 0) {
> +		dev_err(&client->dev, "iio triggered buffer setup failed\n");
> +		goto err_trigger_unregister;
> +	}
> +
>  	ret = iio_device_register(indio_dev);
>  	if (ret < 0) {
>  		dev_err(&client->dev, "device_register failed\n");
> -		goto err_power_off;
> +		goto err_buffer_cleanup;
>  	}
>  
>  	return ret;
>  
> +err_buffer_cleanup:
> +	iio_triggered_buffer_cleanup(indio_dev);
> +err_trigger_unregister:
> +	if (data->dready_trig)
> +		iio_trigger_unregister(data->dready_trig);
>  err_power_off:
>  	stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
>  	return ret;
> @@ -290,10 +541,16 @@ err_power_off:
>  static int stk8ba50_remove(struct i2c_client *client)
>  {
>  	struct iio_dev *indio_dev = i2c_get_clientdata(client);
> +	struct stk8ba50_data *data = iio_priv(indio_dev);
>  
>  	iio_device_unregister(indio_dev);
>  
> -	return stk8ba50_set_power(iio_priv(indio_dev), STK8BA50_MODE_SUSPEND);
> +	if (data->dready_trig) {
> +		iio_triggered_buffer_cleanup(indio_dev);
> +		iio_trigger_unregister(data->dready_trig);
> +	}
> +
> +	return stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
>  }
>  
>  #ifdef CONFIG_PM_SLEEP
> 

--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in

Re: [PATCH v2 1/2] iio: accel: STK8BA50: suspend sensor on init errors

[Jonathan Cameron]

On 19/06/15 15:56, Tiberiu Breana wrote:
> Put chip in 'suspend' mode in case something goes wrong
> during probe.
> 
> Signed-off-by: Tiberiu Breana <tiberiu.a.breana@intel.com>
Applied to the togreg branch of iio.git - initially pushed
out as testing for the autobuilders to play with it.

Thanks,

Jonathan
> ---
> no changes since v1
> ---
>  drivers/iio/accel/stk8ba50.c | 8 ++++++--
>  1 file changed, 6 insertions(+), 2 deletions(-)
> 
> diff --git a/drivers/iio/accel/stk8ba50.c b/drivers/iio/accel/stk8ba50.c
> index 92229bd..9836880 100644
> --- a/drivers/iio/accel/stk8ba50.c
> +++ b/drivers/iio/accel/stk8ba50.c
> @@ -265,7 +265,7 @@ static int stk8ba50_probe(struct i2c_client *client,
>  			STK8BA50_REG_SWRST, STK8BA50_RESET_CMD);
>  	if (ret < 0) {
>  		dev_err(&client->dev, "failed to reset sensor\n");
> -		return ret;
> +		goto err_power_off;
>  	}
>  
>  	/* The default range is +/-2g */
> @@ -277,10 +277,14 @@ static int stk8ba50_probe(struct i2c_client *client,
>  	ret = iio_device_register(indio_dev);
>  	if (ret < 0) {
>  		dev_err(&client->dev, "device_register failed\n");
> -		stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
> +		goto err_power_off;
>  	}
>  
>  	return ret;
> +
> +err_power_off:
> +	stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);
> +	return ret;
>  }
>  
>  static int stk8ba50_remove(struct i2c_client *client)
> 

--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in