Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit db6a19b8 authored by Tiberiu Breana's avatar Tiberiu Breana Committed by Jonathan Cameron
Browse files

iio: accel: Add trigger support for STK8BA50 



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

Change-Id: I9c2d7be4256b2dcc5546e4432308ea54f8004333
Signed-off-by: default avatarTiberiu Breana <tiberiu.a.breana@intel.com>
Signed-off-by: default avatarJonathan Cameron <jic23@kernel.org>
parent 9690d81a

drivers/iio/accel/stk8ba50.c

+271 −25
Original line number Diff line number Diff line
@@ -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"
@@ -68,14 +82,29 @@ static const struct { 
	{0x0C, 224}, {0x0D, 448}, {0x0E, 896}, {0x0F, 1792}

};



/* Used to map scan mask bits to their corresponding channel register. */

static const int stk8ba50_channel_table[] = {

	STK8BA50_REG_XOUT,

	STK8BA50_REG_YOUT,

	STK8BA50_REG_ZOUT

};



struct stk8ba50_data {

	struct i2c_client *client;

	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 +112,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 +154,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 +216,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 +315,100 @@ 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)

{

	int ret;

	u8 masked_reg;

	struct i2c_client *client = data->client;

	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;



	ret = i2c_smbus_read_byte_data(client, STK8BA50_REG_POWMODE);

	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,

						    (u8 *)data->buffer);

		if (ret < STK8BA50_ALL_CHANNEL_SIZE) {

			dev_err(&data->client->dev, "register read failed\n");

			goto err;

		}

	} else {

		for_each_set_bit(bit, indio_dev->active_scan_mask,

				 indio_dev->masklength) {

			ret = stk8ba50_read_accel(data,

						  stk8ba50_channel_table[bit]);

			if (ret < 0)

		goto exit_err;

				goto err;



	if (mode)

		masked_reg = ret | STK8BA50_MODE_POWERBIT;

	else

		masked_reg = ret & (~STK8BA50_MODE_POWERBIT);

			data->buffer[i++] = ret;

		}

	}

	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,

					   pf->timestamp);

err:

	mutex_unlock(&data->lock);

	iio_trigger_notify_done(indio_dev->trig);



	ret = i2c_smbus_write_byte_data(client, STK8BA50_REG_POWMODE,

					masked_reg);

	if (ret < 0)

		goto exit_err;

	return IRQ_HANDLED;

}



	return ret;

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;



	if (!client)

		return -EINVAL;



	dev = &client->dev;



	/* 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);

	}



	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 +451,78 @@ 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);

	if (ret < 0) {

		dev_err(&client->dev, "failed to set up interrupts\n");

		goto err_power_off;

	}

	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 +531,15 @@ static int stk8ba50_probe(struct i2c_client *client, 
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);

	iio_triggered_buffer_cleanup(indio_dev);



	return stk8ba50_set_power(iio_priv(indio_dev), STK8BA50_MODE_SUSPEND);

	if (data->dready_trig)

		iio_trigger_unregister(data->dready_trig);



	return stk8ba50_set_power(data, STK8BA50_MODE_SUSPEND);

}



#ifdef CONFIG_PM_SLEEP