perf bench futex: Cache align the worker struct
[linux/fpc-iii.git] / drivers / iio / gyro / itg3200_buffer.c
blobeef50e91f17cfe56930357e4404eafd11a7836d4
1 /*
2 * itg3200_buffer.c -- support InvenSense ITG3200
3 * Digital 3-Axis Gyroscope driver
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>
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.
14 #include <linux/slab.h>
15 #include <linux/i2c.h>
16 #include <linux/interrupt.h>
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>
26 static int itg3200_read_all_channels(struct i2c_client *i2c, __be16 *buf)
28 u8 tx = 0x80 | ITG3200_REG_TEMP_OUT_H;
29 struct i2c_msg msg[2] = {
31 .addr = i2c->addr,
32 .flags = i2c->flags,
33 .len = 1,
34 .buf = &tx,
37 .addr = i2c->addr,
38 .flags = i2c->flags | I2C_M_RD,
39 .len = ITG3200_SCAN_ELEMENTS * sizeof(s16),
40 .buf = (char *)&buf,
44 return i2c_transfer(i2c->adapter, msg, 2);
47 static irqreturn_t itg3200_trigger_handler(int irq, void *p)
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)];
54 int ret = itg3200_read_all_channels(st->i2c, buf);
55 if (ret < 0)
56 goto error_ret;
58 iio_push_to_buffers_with_timestamp(indio_dev, buf, pf->timestamp);
60 iio_trigger_notify_done(indio_dev->trig);
62 error_ret:
63 return IRQ_HANDLED;
66 int itg3200_buffer_configure(struct iio_dev *indio_dev)
68 return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
69 itg3200_trigger_handler, NULL);
72 void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
74 iio_triggered_buffer_cleanup(indio_dev);
78 static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
79 bool state)
81 struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
82 int ret;
83 u8 msc;
85 ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
86 if (ret)
87 goto error_ret;
89 if (state)
90 msc |= ITG3200_IRQ_DATA_RDY_ENABLE;
91 else
92 msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;
94 ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
95 if (ret)
96 goto error_ret;
98 error_ret:
99 return ret;
103 static const struct iio_trigger_ops itg3200_trigger_ops = {
104 .owner = THIS_MODULE,
105 .set_trigger_state = &itg3200_data_rdy_trigger_set_state,
108 int itg3200_probe_trigger(struct iio_dev *indio_dev)
110 int ret;
111 struct itg3200 *st = iio_priv(indio_dev);
113 st->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
114 indio_dev->id);
115 if (!st->trig)
116 return -ENOMEM;
118 ret = request_irq(st->i2c->irq,
119 &iio_trigger_generic_data_rdy_poll,
120 IRQF_TRIGGER_RISING,
121 "itg3200_data_rdy",
122 st->trig);
123 if (ret)
124 goto error_free_trig;
127 st->trig->dev.parent = &st->i2c->dev;
128 st->trig->ops = &itg3200_trigger_ops;
129 iio_trigger_set_drvdata(st->trig, indio_dev);
130 ret = iio_trigger_register(st->trig);
131 if (ret)
132 goto error_free_irq;
134 /* select default trigger */
135 indio_dev->trig = iio_trigger_get(st->trig);
137 return 0;
139 error_free_irq:
140 free_irq(st->i2c->irq, st->trig);
141 error_free_trig:
142 iio_trigger_free(st->trig);
143 return ret;
146 void itg3200_remove_trigger(struct iio_dev *indio_dev)
148 struct itg3200 *st = iio_priv(indio_dev);
150 iio_trigger_unregister(st->trig);
151 free_irq(st->i2c->irq, st->trig);
152 iio_trigger_free(st->trig);