Linux Audio

Check our new training course

Loading...
  1// SPDX-License-Identifier: GPL-2.0-only
  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>
  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
 23static 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
 44static 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);
 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);
 59	if (ret < 0)
 60		goto error_ret;
 61
 62	iio_push_to_buffers_with_timestamp(indio_dev, &scan, pf->timestamp);
 63
 64error_ret:
 65	iio_trigger_notify_done(indio_dev->trig);
 66
 67	return IRQ_HANDLED;
 68}
 69
 70int 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
 76void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
 77{
 78	iio_triggered_buffer_cleanup(indio_dev);
 79}
 80
 81
 82static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
 83		bool state)
 84{
 85	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 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
102error_ret:
103	return ret;
104
105}
106
107static const struct iio_trigger_ops itg3200_trigger_ops = {
108	.set_trigger_state = &itg3200_data_rdy_trigger_set_state,
109};
110
111int itg3200_probe_trigger(struct iio_dev *indio_dev)
112{
113	int ret;
114	struct itg3200 *st = iio_priv(indio_dev);
115
116	st->trig = iio_trigger_alloc(&st->i2c->dev, "%s-dev%d", indio_dev->name,
117				     iio_device_id(indio_dev));
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
130	st->trig->ops = &itg3200_trigger_ops;
131	iio_trigger_set_drvdata(st->trig, indio_dev);
132	ret = iio_trigger_register(st->trig);
133	if (ret)
134		goto error_free_irq;
135
136	/* select default trigger */
137	indio_dev->trig = iio_trigger_get(st->trig);
138
139	return 0;
140
141error_free_irq:
142	free_irq(st->i2c->irq, st->trig);
143error_free_trig:
144	iio_trigger_free(st->trig);
145	return ret;
146}
147
148void 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}