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

/*
 * itg3200_buffer.c -- support InvenSense ITG3200
 *                     Digital 3-Axis Gyroscope driver
 *
 * Copyright (c) 2011 Christian Strobel <[email protected]>
 * Copyright (c) 2011 Manuel Stahl <[email protected]>
 * Copyright (c) 2012 Thorsten Nowak <[email protected]>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#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);
	__be16 buf[ITG3200_SCAN_ELEMENTS + sizeof(s64)/sizeof(u16)];

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

	iio_push_to_buffers_with_timestamp(indio_dev, buf, pf->timestamp);

	iio_trigger_notify_done(indio_dev->trig);

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