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 
itg3200_read_all_channels(struct i2c_client * i2c,__be16 * buf)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 
itg3200_trigger_handler(int irq,void * p)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);
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 
64 error_ret:
65 	iio_trigger_notify_done(indio_dev->trig);
66 
67 	return IRQ_HANDLED;
68 }
69 
itg3200_buffer_configure(struct iio_dev * indio_dev)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 
itg3200_buffer_unconfigure(struct iio_dev * indio_dev)76 void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
77 {
78 	iio_triggered_buffer_cleanup(indio_dev);
79 }
80 
81 
itg3200_data_rdy_trigger_set_state(struct iio_trigger * trig,bool state)82 static 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 
102 error_ret:
103 	return ret;
104 
105 }
106 
107 static const struct iio_trigger_ops itg3200_trigger_ops = {
108 	.set_trigger_state = &itg3200_data_rdy_trigger_set_state,
109 };
110 
itg3200_probe_trigger(struct iio_dev * indio_dev)111 int 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 
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 
itg3200_remove_trigger(struct iio_dev * indio_dev)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 }
156