[linux-block.git] / drivers / iio / gyro / itg3200_buffer.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * itg3200_buffer.c -- support InvenSense ITG3200
 *                     Digital 3-Axis Gyroscope driver
 *
 * Copyright (c) 2011 Christian Strobel <christian.strobel@iis.fraunhofer.de>
 * Copyright (c) 2011 Manuel Stahl <manuel.stahl@iis.fraunhofer.de>
 * Copyright (c) 2012 Thorsten Nowak <thorsten.nowak@iis.fraunhofer.de>
 */

#include <linux/slab.h>
#include <linux/i2c.h>
#include <linux/interrupt.h>

#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
#include <linux/iio/trigger.h>
#include <linux/iio/trigger_consumer.h>
#include <linux/iio/triggered_buffer.h>
#include <linux/iio/gyro/itg3200.h>


static int itg3200_read_all_channels(struct i2c_client *i2c, __be16 *buf)
{
	u8 tx = 0x80 | ITG3200_REG_TEMP_OUT_H;
	struct i2c_msg msg[2] = {
		{
			.addr = i2c->addr,
			.flags = i2c->flags,
			.len = 1,
			.buf = &tx,
		},
		{
			.addr = i2c->addr,
			.flags = i2c->flags | I2C_M_RD,
			.len = ITG3200_SCAN_ELEMENTS * sizeof(s16),
			.buf = (char *)&buf,
		},
	};

	return i2c_transfer(i2c->adapter, msg, 2);
}

static irqreturn_t itg3200_trigger_handler(int irq, void *p)
{
	struct iio_poll_func *pf = p;
	struct iio_dev *indio_dev = pf->indio_dev;
	struct itg3200 *st = iio_priv(indio_dev);
	/*
	 * Ensure correct alignment and padding including for the
	 * timestamp that may be inserted.
	 */
	struct {
		__be16 buf[ITG3200_SCAN_ELEMENTS];
		s64 ts __aligned(8);
	} scan;

	int ret = itg3200_read_all_channels(st->i2c, scan.buf);
	if (ret < 0)
		goto error_ret;

	iio_push_to_buffers_with_timestamp(indio_dev, &scan, pf->timestamp);

error_ret:
	iio_trigger_notify_done(indio_dev->trig);

	return IRQ_HANDLED;
}

int itg3200_buffer_configure(struct iio_dev *indio_dev)
{
	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
		itg3200_trigger_handler, NULL);
}

void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
{
	iio_triggered_buffer_cleanup(indio_dev);
}


static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
		bool state)
{
	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
	int ret;
	u8 msc;

	ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
	if (ret)
		goto error_ret;

	if (state)
		msc |= ITG3200_IRQ_DATA_RDY_ENABLE;
	else
		msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;

	ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
	if (ret)
		goto error_ret;

error_ret:
	return ret;

}

static const struct iio_trigger_ops itg3200_trigger_ops = {
	.set_trigger_state = &itg3200_data_rdy_trigger_set_state,
};

int itg3200_probe_trigger(struct iio_dev *indio_dev)
{
	int ret;
	struct itg3200 *st = iio_priv(indio_dev);

	st->trig = iio_trigger_alloc(&st->i2c->dev, "%s-dev%d", indio_dev->name,
				     iio_device_id(indio_dev));
	if (!st->trig)
		return -ENOMEM;

	ret = request_irq(st->i2c->irq,
			  &iio_trigger_generic_data_rdy_poll,
			  IRQF_TRIGGER_RISING,
			  "itg3200_data_rdy",
			  st->trig);
	if (ret)
		goto error_free_trig;


	st->trig->ops = &itg3200_trigger_ops;
	iio_trigger_set_drvdata(st->trig, indio_dev);
	ret = iio_trigger_register(st->trig);
	if (ret)
		goto error_free_irq;

	/* select default trigger */
	indio_dev->trig = iio_trigger_get(st->trig);

	return 0;

error_free_irq:
	free_irq(st->i2c->irq, st->trig);
error_free_trig:
	iio_trigger_free(st->trig);
	return ret;
}

void itg3200_remove_trigger(struct iio_dev *indio_dev)
{
	struct itg3200 *st = iio_priv(indio_dev);

	iio_trigger_unregister(st->trig);
	free_irq(st->i2c->irq, st->trig);
	iio_trigger_free(st->trig);
}
Commit	Line	Data
d2912cb1	1	// SPDX-License-Identifier: GPL-2.0-only
9dbf091d MS	2	/*
	3	* itg3200_buffer.c -- support InvenSense ITG3200
	4	* Digital 3-Axis Gyroscope driver
	5	*
	6	* Copyright (c) 2011 Christian Strobel <christian.strobel@iis.fraunhofer.de>
	7	* Copyright (c) 2011 Manuel Stahl <manuel.stahl@iis.fraunhofer.de>
	8	* Copyright (c) 2012 Thorsten Nowak <thorsten.nowak@iis.fraunhofer.de>
9dbf091d MS	9	*/
	10
	11	#include <linux/slab.h>
	12	#include <linux/i2c.h>
	13	#include <linux/interrupt.h>
	14
	15	#include <linux/iio/iio.h>
	16	#include <linux/iio/buffer.h>
	17	#include <linux/iio/trigger.h>
	18	#include <linux/iio/trigger_consumer.h>
	19	#include <linux/iio/triggered_buffer.h>
	20	#include <linux/iio/gyro/itg3200.h>
	21
	22
	23	static int itg3200_read_all_channels(struct i2c_client i2c, __be16 buf)
	24	{
	25	u8 tx = 0x80 \| ITG3200_REG_TEMP_OUT_H;
	26	struct i2c_msg msg[2] = {
	27	{
	28	.addr = i2c->addr,
	29	.flags = i2c->flags,
	30	.len = 1,
	31	.buf = &tx,
	32	},
	33	{
	34	.addr = i2c->addr,
	35	.flags = i2c->flags \| I2C_M_RD,
	36	.len = ITG3200_SCAN_ELEMENTS * sizeof(s16),
	37	.buf = (char *)&buf,
	38	},
	39	};
	40
	41	return i2c_transfer(i2c->adapter, msg, 2);
	42	}
	43
	44	static irqreturn_t itg3200_trigger_handler(int irq, void *p)
	45	{
	46	struct iio_poll_func *pf = p;
	47	struct iio_dev *indio_dev = pf->indio_dev;
	48	struct itg3200 *st = iio_priv(indio_dev);
10ab7cfd JC	49	/*
	50	* Ensure correct alignment and padding including for the
	51	* timestamp that may be inserted.
	52	*/
	53	struct {
	54	__be16 buf[ITG3200_SCAN_ELEMENTS];
	55	s64 ts __aligned(8);
	56	} scan;
	57
	58	int ret = itg3200_read_all_channels(st->i2c, scan.buf);
9dbf091d MS	59	if (ret < 0)
	60	goto error_ret;
	61
10ab7cfd	62	iio_push_to_buffers_with_timestamp(indio_dev, &scan, pf->timestamp);
9dbf091d	63
67fe2958	64	error_ret:
9dbf091d MS	65	iio_trigger_notify_done(indio_dev->trig);
9dbf091d MS	66
9dbf091d MS	67	return IRQ_HANDLED;
	68	}
	69
	70	int itg3200_buffer_configure(struct iio_dev *indio_dev)
	71	{
	72	return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
	73	itg3200_trigger_handler, NULL);
	74	}
	75
	76	void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
	77	{
	78	iio_triggered_buffer_cleanup(indio_dev);
	79	}
	80
	81
	82	static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
	83	bool state)
	84	{
1e9663c6	85	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
9dbf091d MS	86	int ret;
	87	u8 msc;
	88
	89	ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
	90	if (ret)
	91	goto error_ret;
	92
	93	if (state)
	94	msc \|= ITG3200_IRQ_DATA_RDY_ENABLE;
	95	else
	96	msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;
	97
	98	ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
	99	if (ret)
	100	goto error_ret;
	101
	102	error_ret:
	103	return ret;
	104
	105	}
	106
	107	static const struct iio_trigger_ops itg3200_trigger_ops = {
9dbf091d MS	108	.set_trigger_state = &itg3200_data_rdy_trigger_set_state,
	109	};
	110
	111	int itg3200_probe_trigger(struct iio_dev *indio_dev)
	112	{
	113	int ret;
	114	struct itg3200 *st = iio_priv(indio_dev);
	115
995071d3	116	st->trig = iio_trigger_alloc(&st->i2c->dev, "%s-dev%d", indio_dev->name,
15ea2878	117	iio_device_id(indio_dev));
9dbf091d MS	118	if (!st->trig)
	119	return -ENOMEM;
	120
	121	ret = request_irq(st->i2c->irq,
	122	&iio_trigger_generic_data_rdy_poll,
	123	IRQF_TRIGGER_RISING,
	124	"itg3200_data_rdy",
	125	st->trig);
	126	if (ret)
	127	goto error_free_trig;
	128
	129
9dbf091d	130	st->trig->ops = &itg3200_trigger_ops;
1e9663c6	131	iio_trigger_set_drvdata(st->trig, indio_dev);
9dbf091d MS	132	ret = iio_trigger_register(st->trig);
	133	if (ret)
	134	goto error_free_irq;
	135
	136	/* select default trigger */
0b4dce2e	137	indio_dev->trig = iio_trigger_get(st->trig);
9dbf091d MS	138
	139	return 0;
	140
	141	error_free_irq:
	142	free_irq(st->i2c->irq, st->trig);
	143	error_free_trig:
	144	iio_trigger_free(st->trig);
	145	return ret;
	146	}
	147
	148	void itg3200_remove_trigger(struct iio_dev *indio_dev)
	149	{
	150	struct itg3200 *st = iio_priv(indio_dev);
	151
	152	iio_trigger_unregister(st->trig);
	153	free_irq(st->i2c->irq, st->trig);
	154	iio_trigger_free(st->trig);
	155	}