[mirror_ubuntu-hirsute-kernel.git] / drivers / staging / iio / accel / lis3l02dq_ring.c

#include <linux/interrupt.h>
#include <linux/gpio.h>
#include <linux/mutex.h>
#include <linux/kernel.h>
#include <linux/spi/spi.h>
#include <linux/slab.h>

#include "../iio.h"
#include "../ring_sw.h"
#include "../kfifo_buf.h"
#include "../trigger.h"
#include "lis3l02dq.h"

/**
 * combine_8_to_16() utility function to munge to u8s into u16
 **/
static inline u16 combine_8_to_16(u8 lower, u8 upper)
{
	u16 _lower = lower;
	u16 _upper = upper;
	return _lower | (_upper << 8);
}

/**
 * lis3l02dq_data_rdy_trig_poll() the event handler for the data rdy trig
 **/
irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
{
	struct iio_dev *indio_dev = private;
	struct lis3l02dq_state *st = iio_priv(indio_dev);

	if (st->trigger_on) {
		iio_trigger_poll(st->trig, iio_get_time_ns());
		return IRQ_HANDLED;
	} else
		return IRQ_WAKE_THREAD;
}

/**
 * lis3l02dq_read_accel_from_ring() individual acceleration read from ring
 **/
ssize_t lis3l02dq_read_accel_from_ring(struct iio_ring_buffer *ring,
				       int index,
				       int *val)
{
	int ret;
	s16 *data;

	if (!iio_scan_mask_query(ring, index))
		return -EINVAL;

	if (!ring->access->read_last)
		return -EBUSY;

	data = kmalloc(ring->access->get_bytes_per_datum(ring),
		       GFP_KERNEL);
	if (data == NULL)
		return -ENOMEM;

	ret = ring->access->read_last(ring, (u8 *)data);
	if (ret)
		goto error_free_data;
	*val = data[bitmap_weight(&ring->scan_mask, index)];
error_free_data:
	kfree(data);

	return ret;
}

static const u8 read_all_tx_array[] = {
	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_L_ADDR), 0,
	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_H_ADDR), 0,
	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_L_ADDR), 0,
	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_H_ADDR), 0,
	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_L_ADDR), 0,
	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_H_ADDR), 0,
};

/**
 * lis3l02dq_read_all() Reads all channels currently selected
 * @st:		device specific state
 * @rx_array:	(dma capable) receive array, must be at least
 *		4*number of channels
 **/
static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array)
{
	struct iio_ring_buffer *ring = indio_dev->ring;
	struct lis3l02dq_state *st = iio_priv(indio_dev);
	struct spi_transfer *xfers;
	struct spi_message msg;
	int ret, i, j = 0;

	xfers = kzalloc((ring->scan_count) * 2
			* sizeof(*xfers), GFP_KERNEL);
	if (!xfers)
		return -ENOMEM;

	mutex_lock(&st->buf_lock);

	for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++)
		if (ring->scan_mask & (1 << i)) {
			/* lower byte */
			xfers[j].tx_buf = st->tx + 2*j;
			st->tx[2*j] = read_all_tx_array[i*4];
			st->tx[2*j + 1] = 0;
			if (rx_array)
				xfers[j].rx_buf = rx_array + j*2;
			xfers[j].bits_per_word = 8;
			xfers[j].len = 2;
			xfers[j].cs_change = 1;
			j++;

			/* upper byte */
			xfers[j].tx_buf = st->tx + 2*j;
			st->tx[2*j] = read_all_tx_array[i*4 + 2];
			st->tx[2*j + 1] = 0;
			if (rx_array)
				xfers[j].rx_buf = rx_array + j*2;
			xfers[j].bits_per_word = 8;
			xfers[j].len = 2;
			xfers[j].cs_change = 1;
			j++;
		}

	/* After these are transmitted, the rx_buff should have
	 * values in alternate bytes
	 */
	spi_message_init(&msg);
	for (j = 0; j < ring->scan_count * 2; j++)
		spi_message_add_tail(&xfers[j], &msg);

	ret = spi_sync(st->us, &msg);
	mutex_unlock(&st->buf_lock);
	kfree(xfers);

	return ret;
}

static int lis3l02dq_get_ring_element(struct iio_dev *indio_dev,
				u8 *buf)
{
	int ret, i;
	u8 *rx_array ;
	s16 *data = (s16 *)buf;

	rx_array = kzalloc(4 * (indio_dev->ring->scan_count), GFP_KERNEL);
	if (rx_array == NULL)
		return -ENOMEM;
	ret = lis3l02dq_read_all(indio_dev, rx_array);
	if (ret < 0)
		return ret;
	for (i = 0; i < indio_dev->ring->scan_count; i++)
		data[i] = combine_8_to_16(rx_array[i*4+1],
					rx_array[i*4+3]);
	kfree(rx_array);

	return i*sizeof(data[0]);
}

static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
{
	struct iio_poll_func *pf = p;
	struct iio_dev *indio_dev = pf->private_data;
	struct iio_ring_buffer *ring = indio_dev->ring;
	int len = 0;
	size_t datasize = ring->access->get_bytes_per_datum(ring);
	char *data = kmalloc(datasize, GFP_KERNEL);

	if (data == NULL) {
		dev_err(indio_dev->dev.parent,
			"memory alloc failed in ring bh");
		return -ENOMEM;
	}

	if (ring->scan_count)
		len = lis3l02dq_get_ring_element(indio_dev, data);

	  /* Guaranteed to be aligned with 8 byte boundary */
	if (ring->scan_timestamp)
		*(s64 *)(((phys_addr_t)data + len
				+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
			= pf->timestamp;
	ring->access->store_to(ring, (u8 *)data, pf->timestamp);

	iio_trigger_notify_done(indio_dev->trig);
	kfree(data);
	return IRQ_HANDLED;
}

/* Caller responsible for locking as necessary. */
static int
__lis3l02dq_write_data_ready_config(struct device *dev, bool state)
{
	int ret;
	u8 valold;
	bool currentlyset;
	struct iio_dev *indio_dev = dev_get_drvdata(dev);
	struct lis3l02dq_state *st = iio_priv(indio_dev);

/* Get the current event mask register */
	ret = lis3l02dq_spi_read_reg_8(indio_dev,
				       LIS3L02DQ_REG_CTRL_2_ADDR,
				       &valold);
	if (ret)
		goto error_ret;
/* Find out if data ready is already on */
	currentlyset
		= valold & LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;

/* Disable requested */
	if (!state && currentlyset) {
		/* disable the data ready signal */
		valold &= ~LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;

		/* The double write is to overcome a hardware bug?*/
		ret = lis3l02dq_spi_write_reg_8(indio_dev,
						LIS3L02DQ_REG_CTRL_2_ADDR,
						valold);
		if (ret)
			goto error_ret;
		ret = lis3l02dq_spi_write_reg_8(indio_dev,
						LIS3L02DQ_REG_CTRL_2_ADDR,
						valold);
		if (ret)
			goto error_ret;
		st->trigger_on = false;
/* Enable requested */
	} else if (state && !currentlyset) {
		/* if not set, enable requested */
		/* first disable all events */
		ret = lis3l02dq_disable_all_events(indio_dev);
		if (ret < 0)
			goto error_ret;

		valold = ret |
			LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;

		st->trigger_on = true;
		ret = lis3l02dq_spi_write_reg_8(indio_dev,
						LIS3L02DQ_REG_CTRL_2_ADDR,
						valold);
		if (ret)
			goto error_ret;
	}

	return 0;
error_ret:
	return ret;
}

/**
 * lis3l02dq_data_rdy_trigger_set_state() set datardy interrupt state
 *
 * If disabling the interrupt also does a final read to ensure it is clear.
 * This is only important in some cases where the scan enable elements are
 * switched before the ring is reenabled.
 **/
static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
						bool state)
{
	struct iio_dev *indio_dev = trig->private_data;
	int ret = 0;
	u8 t;

	__lis3l02dq_write_data_ready_config(&indio_dev->dev, state);
	if (state == false) {
		/*
		 * A possible quirk with teh handler is currently worked around
		 *  by ensuring outstanding read events are cleared.
		 */
		ret = lis3l02dq_read_all(indio_dev, NULL);
	}
	lis3l02dq_spi_read_reg_8(indio_dev,
				 LIS3L02DQ_REG_WAKE_UP_SRC_ADDR,
				 &t);
	return ret;
}

/**
 * lis3l02dq_trig_try_reen() try renabling irq for data rdy trigger
 * @trig:	the datardy trigger
 */
static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
{
	struct iio_dev *indio_dev = trig->private_data;
	struct lis3l02dq_state *st = iio_priv(indio_dev);
	int i;

	/* If gpio still high (or high again) */
	/* In theory possible we will need to do this several times */
	for (i = 0; i < 5; i++)
		if (gpio_get_value(irq_to_gpio(st->us->irq)))
			lis3l02dq_read_all(indio_dev, NULL);
		else
			break;
	if (i == 5)
		printk(KERN_INFO
		       "Failed to clear the interrupt for lis3l02dq\n");

	/* irq reenabled so success! */
	return 0;
}

static const struct iio_trigger_ops lis3l02dq_trigger_ops = {
	.owner = THIS_MODULE,
	.set_trigger_state = &lis3l02dq_data_rdy_trigger_set_state,
	.try_reenable = &lis3l02dq_trig_try_reen,
};

int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
{
	int ret;
	struct lis3l02dq_state *st = iio_priv(indio_dev);

	st->trig = iio_allocate_trigger("lis3l02dq-dev%d", indio_dev->id);
	if (!st->trig) {
		ret = -ENOMEM;
		goto error_ret;
	}

	st->trig->dev.parent = &st->us->dev;
	st->trig->ops = &lis3l02dq_trigger_ops;
	st->trig->private_data = indio_dev;
	ret = iio_trigger_register(st->trig);
	if (ret)
		goto error_free_trig;

	return 0;

error_free_trig:
	iio_free_trigger(st->trig);
error_ret:
	return ret;
}

void lis3l02dq_remove_trigger(struct iio_dev *indio_dev)
{
	struct lis3l02dq_state *st = iio_priv(indio_dev);

	iio_trigger_unregister(st->trig);
	iio_free_trigger(st->trig);
}

void lis3l02dq_unconfigure_ring(struct iio_dev *indio_dev)
{
	iio_dealloc_pollfunc(indio_dev->pollfunc);
	lis3l02dq_free_buf(indio_dev->ring);
}

static int lis3l02dq_ring_postenable(struct iio_dev *indio_dev)
{
	/* Disable unwanted channels otherwise the interrupt will not clear */
	u8 t;
	int ret;
	bool oneenabled = false;

	ret = lis3l02dq_spi_read_reg_8(indio_dev,
				       LIS3L02DQ_REG_CTRL_1_ADDR,
				       &t);
	if (ret)
		goto error_ret;

	if (iio_scan_mask_query(indio_dev->ring, 0)) {
		t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
		oneenabled = true;
	} else
		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
	if (iio_scan_mask_query(indio_dev->ring, 1)) {
		t |= LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
		oneenabled = true;
	} else
		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
	if (iio_scan_mask_query(indio_dev->ring, 2)) {
		t |= LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
		oneenabled = true;
	} else
		t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;

	if (!oneenabled) /* what happens in this case is unknown */
		return -EINVAL;
	ret = lis3l02dq_spi_write_reg_8(indio_dev,
					LIS3L02DQ_REG_CTRL_1_ADDR,
					t);
	if (ret)
		goto error_ret;

	return iio_triggered_ring_postenable(indio_dev);
error_ret:
	return ret;
}

/* Turn all channels on again */
static int lis3l02dq_ring_predisable(struct iio_dev *indio_dev)
{
	u8 t;
	int ret;

	ret = iio_triggered_ring_predisable(indio_dev);
	if (ret)
		goto error_ret;

	ret = lis3l02dq_spi_read_reg_8(indio_dev,
				       LIS3L02DQ_REG_CTRL_1_ADDR,
				       &t);
	if (ret)
		goto error_ret;
	t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE |
		LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE |
		LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;

	ret = lis3l02dq_spi_write_reg_8(indio_dev,
					LIS3L02DQ_REG_CTRL_1_ADDR,
					t);

error_ret:
	return ret;
}

static const struct iio_ring_setup_ops lis3l02dq_ring_setup_ops = {
	.preenable = &iio_sw_ring_preenable,
	.postenable = &lis3l02dq_ring_postenable,
	.predisable = &lis3l02dq_ring_predisable,
};

int lis3l02dq_configure_ring(struct iio_dev *indio_dev)
{
	int ret;
	struct iio_ring_buffer *ring;

	ring = lis3l02dq_alloc_buf(indio_dev);
	if (!ring)
		return -ENOMEM;

	indio_dev->ring = ring;
	/* Effectively select the ring buffer implementation */
	indio_dev->ring->access = &lis3l02dq_access_funcs;
	ring->bpe = 2;

	ring->scan_timestamp = true;
	ring->setup_ops = &lis3l02dq_ring_setup_ops;
	ring->owner = THIS_MODULE;

	/* Set default scan mode */
	iio_scan_mask_set(ring, 0);
	iio_scan_mask_set(ring, 1);
	iio_scan_mask_set(ring, 2);

	/* Functions are NULL as we set handler below */
	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
						 &lis3l02dq_trigger_handler,
						 0,
						 indio_dev,
						 "lis3l02dq_consumer%d",
						 indio_dev->id);

	if (indio_dev->pollfunc == NULL) {
		ret = -ENOMEM;
		goto error_iio_sw_rb_free;
	}

	indio_dev->modes |= INDIO_RING_TRIGGERED;
	return 0;

error_iio_sw_rb_free:
	lis3l02dq_free_buf(indio_dev->ring);
	return ret;
}
Commit	Line	Data
14cd9a73	1	#include <linux/interrupt.h>
14cd9a73	2	#include <linux/gpio.h>
14cd9a73	3	#include <linux/mutex.h>
14cd9a73 JC	4	#include <linux/kernel.h>
14cd9a73 JC	5	#include <linux/spi/spi.h>
5a0e3ad6	6	#include <linux/slab.h>
14cd9a73 JC	7
14cd9a73 JC	8	#include "../iio.h"
14cd9a73	9	#include "../ring_sw.h"
b949793b	10	#include "../kfifo_buf.h"
14cd9a73 JC	11	#include "../trigger.h"
	12	#include "lis3l02dq.h"
	13
	14	/**
	15	* combine_8_to_16() utility function to munge to u8s into u16
	16	**/
	17	static inline u16 combine_8_to_16(u8 lower, u8 upper)
	18	{
	19	u16 _lower = lower;
	20	u16 _upper = upper;
	21	return _lower \| (_upper << 8);
	22	}
	23
d731aea0 JC	24	/**
	25	* lis3l02dq_data_rdy_trig_poll() the event handler for the data rdy trig
	26	**/
	27	irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
	28	{
	29	struct iio_dev *indio_dev = private;
7b2fdd19	30	struct lis3l02dq_state *st = iio_priv(indio_dev);
d731aea0 JC	31
	32	if (st->trigger_on) {
	33	iio_trigger_poll(st->trig, iio_get_time_ns());
	34	return IRQ_HANDLED;
	35	} else
	36	return IRQ_WAKE_THREAD;
	37	}
	38
14cd9a73 JC	39	/**
	40	* lis3l02dq_read_accel_from_ring() individual acceleration read from ring
	41	**/
f3736416 JC	42	ssize_t lis3l02dq_read_accel_from_ring(struct iio_ring_buffer *ring,
	43	int index,
	44	int *val)
14cd9a73	45	{
f3736416	46	int ret;
14cd9a73	47	s16 *data;
8d213f24	48
f3736416 JC	49	if (!iio_scan_mask_query(ring, index))
f3736416 JC	50	return -EINVAL;
14cd9a73	51
8d213f24 JC	52	if (!ring->access->read_last)
	53	return -EBUSY;
	54
5565a450	55	data = kmalloc(ring->access->get_bytes_per_datum(ring),
f3736416 JC	56	GFP_KERNEL);
	57	if (data == NULL)
	58	return -ENOMEM;
	59
5565a450	60	ret = ring->access->read_last(ring, (u8 *)data);
f3736416	61	if (ret)
14cd9a73	62	goto error_free_data;
8d213f24	63	*val = data[bitmap_weight(&ring->scan_mask, index)];
14cd9a73 JC	64	error_free_data:
14cd9a73 JC	65	kfree(data);
8d213f24	66
f3736416	67	return ret;
14cd9a73 JC	68	}
14cd9a73 JC	69
26de7208	70	static const u8 read_all_tx_array[] = {
14cd9a73 JC	71	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_L_ADDR), 0,
	72	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_H_ADDR), 0,
	73	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_L_ADDR), 0,
	74	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_H_ADDR), 0,
	75	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_L_ADDR), 0,
	76	LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_H_ADDR), 0,
	77	};
	78
	79	/**
	80	* lis3l02dq_read_all() Reads all channels currently selected
	81	* @st: device specific state
25985edc	82	* @rx_array: (dma capable) receive array, must be at least
14cd9a73 JC	83	* 4*number of channels
14cd9a73 JC	84	**/
7b2fdd19	85	static int lis3l02dq_read_all(struct iio_dev indio_dev, u8 rx_array)
14cd9a73	86	{
7b2fdd19 JC	87	struct iio_ring_buffer *ring = indio_dev->ring;
7b2fdd19 JC	88	struct lis3l02dq_state *st = iio_priv(indio_dev);
14cd9a73 JC	89	struct spi_transfer *xfers;
	90	struct spi_message msg;
	91	int ret, i, j = 0;
	92
bf32963c	93	xfers = kzalloc((ring->scan_count) * 2
14cd9a73 JC	94	* sizeof(*xfers), GFP_KERNEL);
	95	if (!xfers)
	96	return -ENOMEM;
	97
	98	mutex_lock(&st->buf_lock);
	99
f3736416	100	for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++)
bf32963c	101	if (ring->scan_mask & (1 << i)) {
14cd9a73 JC	102	/* lower byte */
	103	xfers[j].tx_buf = st->tx + 2*j;
	104	st->tx[2j] = read_all_tx_array[i4];
	105	st->tx[2*j + 1] = 0;
	106	if (rx_array)
	107	xfers[j].rx_buf = rx_array + j*2;
	108	xfers[j].bits_per_word = 8;
	109	xfers[j].len = 2;
	110	xfers[j].cs_change = 1;
	111	j++;
	112
	113	/* upper byte */
	114	xfers[j].tx_buf = st->tx + 2*j;
	115	st->tx[2j] = read_all_tx_array[i4 + 2];
	116	st->tx[2*j + 1] = 0;
	117	if (rx_array)
	118	xfers[j].rx_buf = rx_array + j*2;
	119	xfers[j].bits_per_word = 8;
	120	xfers[j].len = 2;
	121	xfers[j].cs_change = 1;
	122	j++;
	123	}
f3736416	124
14cd9a73 JC	125	/* After these are transmitted, the rx_buff should have
	126	* values in alternate bytes
	127	*/
	128	spi_message_init(&msg);
bf32963c	129	for (j = 0; j < ring->scan_count * 2; j++)
14cd9a73 JC	130	spi_message_add_tail(&xfers[j], &msg);
	131
	132	ret = spi_sync(st->us, &msg);
	133	mutex_unlock(&st->buf_lock);
	134	kfree(xfers);
	135
	136	return ret;
	137	}
	138
7b2fdd19	139	static int lis3l02dq_get_ring_element(struct iio_dev *indio_dev,
73bce12e JC	140	u8 *buf)
	141	{
	142	int ret, i;
	143	u8 *rx_array ;
	144	s16 data = (s16 )buf;
14cd9a73	145
7b2fdd19	146	rx_array = kzalloc(4 * (indio_dev->ring->scan_count), GFP_KERNEL);
73bce12e JC	147	if (rx_array == NULL)
73bce12e JC	148	return -ENOMEM;
7b2fdd19	149	ret = lis3l02dq_read_all(indio_dev, rx_array);
73bce12e JC	150	if (ret < 0)
73bce12e JC	151	return ret;
7b2fdd19	152	for (i = 0; i < indio_dev->ring->scan_count; i++)
73bce12e JC	153	data[i] = combine_8_to_16(rx_array[i*4+1],
73bce12e JC	154	rx_array[i*4+3]);
14cd9a73	155	kfree(rx_array);
14cd9a73	156
73bce12e	157	return i*sizeof(data[0]);
14cd9a73	158	}
14cd9a73	159
7b2fdd19 JC	160	static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
	161	{
	162	struct iio_poll_func *pf = p;
	163	struct iio_dev *indio_dev = pf->private_data;
	164	struct iio_ring_buffer *ring = indio_dev->ring;
	165	int len = 0;
	166	size_t datasize = ring->access->get_bytes_per_datum(ring);
	167	char *data = kmalloc(datasize, GFP_KERNEL);
	168
	169	if (data == NULL) {
	170	dev_err(indio_dev->dev.parent,
	171	"memory alloc failed in ring bh");
	172	return -ENOMEM;
	173	}
	174
	175	if (ring->scan_count)
	176	len = lis3l02dq_get_ring_element(indio_dev, data);
	177
	178	/* Guaranteed to be aligned with 8 byte boundary */
	179	if (ring->scan_timestamp)
	180	(s64 )(((phys_addr_t)data + len
	181	+ sizeof(s64) - 1) & ~(sizeof(s64) - 1))
	182	= pf->timestamp;
	183	ring->access->store_to(ring, (u8 *)data, pf->timestamp);
	184
	185	iio_trigger_notify_done(indio_dev->trig);
	186	kfree(data);
	187	return IRQ_HANDLED;
	188	}
	189
14cd9a73	190	/* Caller responsible for locking as necessary. */
26de7208	191	static int
1e3345bc	192	__lis3l02dq_write_data_ready_config(struct device *dev, bool state)
14cd9a73 JC	193	{
	194	int ret;
	195	u8 valold;
	196	bool currentlyset;
	197	struct iio_dev *indio_dev = dev_get_drvdata(dev);
7b2fdd19	198	struct lis3l02dq_state *st = iio_priv(indio_dev);
14cd9a73 JC	199
14cd9a73 JC	200	/* Get the current event mask register */
1b076b52	201	ret = lis3l02dq_spi_read_reg_8(indio_dev,
14cd9a73 JC	202	LIS3L02DQ_REG_CTRL_2_ADDR,
	203	&valold);
	204	if (ret)
	205	goto error_ret;
	206	/* Find out if data ready is already on */
	207	currentlyset
	208	= valold & LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
	209
	210	/* Disable requested */
	211	if (!state && currentlyset) {
1e3345bc	212	/* disable the data ready signal */
14cd9a73	213	valold &= ~LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
1e3345bc	214
14cd9a73	215	/* The double write is to overcome a hardware bug?*/
1b076b52	216	ret = lis3l02dq_spi_write_reg_8(indio_dev,
14cd9a73	217	LIS3L02DQ_REG_CTRL_2_ADDR,
7df86302	218	valold);
14cd9a73 JC	219	if (ret)
14cd9a73 JC	220	goto error_ret;
1b076b52	221	ret = lis3l02dq_spi_write_reg_8(indio_dev,
14cd9a73	222	LIS3L02DQ_REG_CTRL_2_ADDR,
7df86302	223	valold);
14cd9a73 JC	224	if (ret)
14cd9a73 JC	225	goto error_ret;
d731aea0	226	st->trigger_on = false;
14cd9a73 JC	227	/* Enable requested */
	228	} else if (state && !currentlyset) {
	229	/* if not set, enable requested */
1e3345bc JC	230	/* first disable all events */
	231	ret = lis3l02dq_disable_all_events(indio_dev);
	232	if (ret < 0)
	233	goto error_ret;
	234
	235	valold = ret \|
	236	LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
1e3345bc	237
d731aea0	238	st->trigger_on = true;
1b076b52	239	ret = lis3l02dq_spi_write_reg_8(indio_dev,
14cd9a73	240	LIS3L02DQ_REG_CTRL_2_ADDR,
7df86302	241	valold);
d731aea0	242	if (ret)
14cd9a73 JC	243	goto error_ret;
	244	}
	245
	246	return 0;
	247	error_ret:
	248	return ret;
	249	}
	250
	251	/**
	252	* lis3l02dq_data_rdy_trigger_set_state() set datardy interrupt state
	253	*
	254	* If disabling the interrupt also does a final read to ensure it is clear.
	255	* This is only important in some cases where the scan enable elements are
	256	* switched before the ring is reenabled.
	257	**/
	258	static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
	259	bool state)
	260	{
7b2fdd19	261	struct iio_dev *indio_dev = trig->private_data;
14cd9a73 JC	262	int ret = 0;
14cd9a73 JC	263	u8 t;
1e3345bc	264
7b2fdd19	265	__lis3l02dq_write_data_ready_config(&indio_dev->dev, state);
14cd9a73	266	if (state == false) {
d1dbf011 JC	267	/*
	268	* A possible quirk with teh handler is currently worked around
	269	* by ensuring outstanding read events are cleared.
	270	*/
7b2fdd19	271	ret = lis3l02dq_read_all(indio_dev, NULL);
14cd9a73	272	}
7b2fdd19	273	lis3l02dq_spi_read_reg_8(indio_dev,
14cd9a73 JC	274	LIS3L02DQ_REG_WAKE_UP_SRC_ADDR,
	275	&t);
	276	return ret;
	277	}
9dbfb6f1	278
14cd9a73 JC	279	/**
	280	* lis3l02dq_trig_try_reen() try renabling irq for data rdy trigger
	281	* @trig: the datardy trigger
d1dbf011	282	*/
14cd9a73 JC	283	static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
14cd9a73 JC	284	{
7b2fdd19 JC	285	struct iio_dev *indio_dev = trig->private_data;
7b2fdd19 JC	286	struct lis3l02dq_state *st = iio_priv(indio_dev);
d1dbf011 JC	287	int i;
d1dbf011 JC	288
14cd9a73	289	/* If gpio still high (or high again) */
d1dbf011 JC	290	/* In theory possible we will need to do this several times */
	291	for (i = 0; i < 5; i++)
	292	if (gpio_get_value(irq_to_gpio(st->us->irq)))
7b2fdd19	293	lis3l02dq_read_all(indio_dev, NULL);
d1dbf011 JC	294	else
	295	break;
	296	if (i == 5)
	297	printk(KERN_INFO
	298	"Failed to clear the interrupt for lis3l02dq\n");
	299
14cd9a73 JC	300	/* irq reenabled so success! */
	301	return 0;
	302	}
	303
d29f73db JC	304	static const struct iio_trigger_ops lis3l02dq_trigger_ops = {
	305	.owner = THIS_MODULE,
	306	.set_trigger_state = &lis3l02dq_data_rdy_trigger_set_state,
	307	.try_reenable = &lis3l02dq_trig_try_reen,
	308	};
	309
14cd9a73 JC	310	int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
	311	{
	312	int ret;
7b2fdd19	313	struct lis3l02dq_state *st = iio_priv(indio_dev);
14cd9a73	314
59c85e82	315	st->trig = iio_allocate_trigger("lis3l02dq-dev%d", indio_dev->id);
d1dbf011 JC	316	if (!st->trig) {
d1dbf011 JC	317	ret = -ENOMEM;
59c85e82	318	goto error_ret;
14cd9a73	319	}
3c9bbf58	320
1b076b52	321	st->trig->dev.parent = &st->us->dev;
d29f73db	322	st->trig->ops = &lis3l02dq_trigger_ops;
7b2fdd19	323	st->trig->private_data = indio_dev;
1b076b52	324	ret = iio_trigger_register(st->trig);
14cd9a73	325	if (ret)
d1dbf011	326	goto error_free_trig;
14cd9a73 JC	327
	328	return 0;
	329
14cd9a73	330	error_free_trig:
1b076b52	331	iio_free_trigger(st->trig);
d1dbf011	332	error_ret:
14cd9a73 JC	333	return ret;
	334	}
	335
	336	void lis3l02dq_remove_trigger(struct iio_dev *indio_dev)
	337	{
7b2fdd19	338	struct lis3l02dq_state *st = iio_priv(indio_dev);
14cd9a73	339
1b076b52	340	iio_trigger_unregister(st->trig);
1b076b52	341	iio_free_trigger(st->trig);
14cd9a73 JC	342	}
	343
	344	void lis3l02dq_unconfigure_ring(struct iio_dev *indio_dev)
	345	{
0ed731d2	346	iio_dealloc_pollfunc(indio_dev->pollfunc);
b949793b	347	lis3l02dq_free_buf(indio_dev->ring);
14cd9a73 JC	348	}
14cd9a73 JC	349
f3736416 JC	350	static int lis3l02dq_ring_postenable(struct iio_dev *indio_dev)
	351	{
	352	/* Disable unwanted channels otherwise the interrupt will not clear */
	353	u8 t;
	354	int ret;
	355	bool oneenabled = false;
	356
1b076b52	357	ret = lis3l02dq_spi_read_reg_8(indio_dev,
f3736416 JC	358	LIS3L02DQ_REG_CTRL_1_ADDR,
	359	&t);
	360	if (ret)
	361	goto error_ret;
	362
	363	if (iio_scan_mask_query(indio_dev->ring, 0)) {
	364	t \|= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
	365	oneenabled = true;
	366	} else
	367	t &= ~LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
	368	if (iio_scan_mask_query(indio_dev->ring, 1)) {
	369	t \|= LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
	370	oneenabled = true;
	371	} else
	372	t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
	373	if (iio_scan_mask_query(indio_dev->ring, 2)) {
	374	t \|= LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
	375	oneenabled = true;
	376	} else
	377	t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
	378
	379	if (!oneenabled) /* what happens in this case is unknown */
	380	return -EINVAL;
1b076b52	381	ret = lis3l02dq_spi_write_reg_8(indio_dev,
f3736416	382	LIS3L02DQ_REG_CTRL_1_ADDR,
7df86302	383	t);
f3736416 JC	384	if (ret)
	385	goto error_ret;
	386
	387	return iio_triggered_ring_postenable(indio_dev);
	388	error_ret:
	389	return ret;
	390	}
	391
	392	/* Turn all channels on again */
	393	static int lis3l02dq_ring_predisable(struct iio_dev *indio_dev)
	394	{
	395	u8 t;
	396	int ret;
	397
	398	ret = iio_triggered_ring_predisable(indio_dev);
	399	if (ret)
	400	goto error_ret;
	401
1b076b52	402	ret = lis3l02dq_spi_read_reg_8(indio_dev,
f3736416 JC	403	LIS3L02DQ_REG_CTRL_1_ADDR,
	404	&t);
	405	if (ret)
	406	goto error_ret;
	407	t \|= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE \|
	408	LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE \|
	409	LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
	410
1b076b52	411	ret = lis3l02dq_spi_write_reg_8(indio_dev,
f3736416	412	LIS3L02DQ_REG_CTRL_1_ADDR,
7df86302	413	t);
f3736416 JC	414
	415	error_ret:
	416	return ret;
	417	}
	418
5565a450 JC	419	static const struct iio_ring_setup_ops lis3l02dq_ring_setup_ops = {
	420	.preenable = &iio_sw_ring_preenable,
	421	.postenable = &lis3l02dq_ring_postenable,
	422	.predisable = &lis3l02dq_ring_predisable,
	423	};
f3736416	424
14cd9a73 JC	425	int lis3l02dq_configure_ring(struct iio_dev *indio_dev)
14cd9a73 JC	426	{
73bce12e	427	int ret;
07e6229e	428	struct iio_ring_buffer *ring;
d1dbf011	429
b949793b	430	ring = lis3l02dq_alloc_buf(indio_dev);
07e6229e	431	if (!ring)
73bce12e JC	432	return -ENOMEM;
73bce12e JC	433
07e6229e	434	indio_dev->ring = ring;
14cd9a73	435	/* Effectively select the ring buffer implementation */
5565a450	436	indio_dev->ring->access = &lis3l02dq_access_funcs;
07e6229e	437	ring->bpe = 2;
f3736416	438
bf32963c	439	ring->scan_timestamp = true;
5565a450	440	ring->setup_ops = &lis3l02dq_ring_setup_ops;
07e6229e	441	ring->owner = THIS_MODULE;
14cd9a73	442
bf32963c	443	/* Set default scan mode */
f3736416 JC	444	iio_scan_mask_set(ring, 0);
	445	iio_scan_mask_set(ring, 1);
	446	iio_scan_mask_set(ring, 2);
bf32963c	447
d1dbf011	448	/* Functions are NULL as we set handler below */
0ed731d2 JC	449	indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
	450	&lis3l02dq_trigger_handler,
	451	0,
	452	indio_dev,
	453	"lis3l02dq_consumer%d",
	454	indio_dev->id);
d1dbf011 JC	455
	456	if (indio_dev->pollfunc == NULL) {
	457	ret = -ENOMEM;
859171ca	458	goto error_iio_sw_rb_free;
d1dbf011	459	}
d1dbf011	460
14cd9a73 JC	461	indio_dev->modes \|= INDIO_RING_TRIGGERED;
	462	return 0;
	463
	464	error_iio_sw_rb_free:
b949793b	465	lis3l02dq_free_buf(indio_dev->ring);
14cd9a73 JC	466	return ret;
14cd9a73 JC	467	}