GNU Linux-libre 4.9.282-gnu1
[releases.git] / drivers / iio / gyro / itg3200_buffer.c
1 /*
2  * itg3200_buffer.c -- support InvenSense ITG3200
3  *                     Digital 3-Axis Gyroscope driver
4  *
5  * Copyright (c) 2011 Christian Strobel <christian.strobel@iis.fraunhofer.de>
6  * Copyright (c) 2011 Manuel Stahl <manuel.stahl@iis.fraunhofer.de>
7  * Copyright (c) 2012 Thorsten Nowak <thorsten.nowak@iis.fraunhofer.de>
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         /*
53          * Ensure correct alignment and padding including for the
54          * timestamp that may be inserted.
55          */
56         struct {
57                 __be16 buf[ITG3200_SCAN_ELEMENTS];
58                 s64 ts __aligned(8);
59         } scan;
60
61         int ret = itg3200_read_all_channels(st->i2c, scan.buf);
62         if (ret < 0)
63                 goto error_ret;
64
65         iio_push_to_buffers_with_timestamp(indio_dev, &scan, pf->timestamp);
66
67         iio_trigger_notify_done(indio_dev->trig);
68
69 error_ret:
70         return IRQ_HANDLED;
71 }
72
73 int itg3200_buffer_configure(struct iio_dev *indio_dev)
74 {
75         return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
76                 itg3200_trigger_handler, NULL);
77 }
78
79 void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
80 {
81         iio_triggered_buffer_cleanup(indio_dev);
82 }
83
84
85 static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
86                 bool state)
87 {
88         struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
89         int ret;
90         u8 msc;
91
92         ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
93         if (ret)
94                 goto error_ret;
95
96         if (state)
97                 msc |= ITG3200_IRQ_DATA_RDY_ENABLE;
98         else
99                 msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;
100
101         ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
102         if (ret)
103                 goto error_ret;
104
105 error_ret:
106         return ret;
107
108 }
109
110 static const struct iio_trigger_ops itg3200_trigger_ops = {
111         .owner = THIS_MODULE,
112         .set_trigger_state = &itg3200_data_rdy_trigger_set_state,
113 };
114
115 int itg3200_probe_trigger(struct iio_dev *indio_dev)
116 {
117         int ret;
118         struct itg3200 *st = iio_priv(indio_dev);
119
120         st->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
121                                      indio_dev->id);
122         if (!st->trig)
123                 return -ENOMEM;
124
125         ret = request_irq(st->i2c->irq,
126                           &iio_trigger_generic_data_rdy_poll,
127                           IRQF_TRIGGER_RISING,
128                           "itg3200_data_rdy",
129                           st->trig);
130         if (ret)
131                 goto error_free_trig;
132
133
134         st->trig->dev.parent = &st->i2c->dev;
135         st->trig->ops = &itg3200_trigger_ops;
136         iio_trigger_set_drvdata(st->trig, indio_dev);
137         ret = iio_trigger_register(st->trig);
138         if (ret)
139                 goto error_free_irq;
140
141         /* select default trigger */
142         indio_dev->trig = iio_trigger_get(st->trig);
143
144         return 0;
145
146 error_free_irq:
147         free_irq(st->i2c->irq, st->trig);
148 error_free_trig:
149         iio_trigger_free(st->trig);
150         return ret;
151 }
152
153 void itg3200_remove_trigger(struct iio_dev *indio_dev)
154 {
155         struct itg3200 *st = iio_priv(indio_dev);
156
157         iio_trigger_unregister(st->trig);
158         free_irq(st->i2c->irq, st->trig);
159         iio_trigger_free(st->trig);
160 }