projects / linux-block.git / blame
| Commit | Line | Data |
|---|---|---|
| d2912cb1 | 1 | // SPDX-License-Identifier: GPL-2.0-only |
| 9dbf091d MS |
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> | |
| 9dbf091d MS |
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 | ||
| 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 | ||
| 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 | __be16 buf[ITG3200_SCAN_ELEMENTS + sizeof(s64)/sizeof(u16)]; | |
| 50 | ||
| 51 | int ret = itg3200_read_all_channels(st->i2c, buf); | |
| 52 | if (ret < 0) | |
| 53 | goto error_ret; | |
| 54 | ||
| 36e0371e | 55 | iio_push_to_buffers_with_timestamp(indio_dev, buf, pf->timestamp); |
| 9dbf091d | 56 | |
| 9dbf091d MS |
57 | iio_trigger_notify_done(indio_dev->trig); |
| 58 | ||
| 59 | error_ret: | |
| 60 | return IRQ_HANDLED; | |
| 61 | } | |
| 62 | ||
| 63 | int itg3200_buffer_configure(struct iio_dev *indio_dev) | |
| 64 | { | |
| 65 | return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time, | |
| 66 | itg3200_trigger_handler, NULL); | |
| 67 | } | |
| 68 | ||
| 69 | void itg3200_buffer_unconfigure(struct iio_dev *indio_dev) | |
| 70 | { | |
| 71 | iio_triggered_buffer_cleanup(indio_dev); | |
| 72 | } | |
| 73 | ||
| 74 | ||
| 75 | static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig, | |
| 76 | bool state) | |
| 77 | { | |
| 1e9663c6 | 78 | struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig); |
| 9dbf091d MS |
79 | int ret; |
| 80 | u8 msc; | |
| 81 | ||
| 82 | ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc); | |
| 83 | if (ret) | |
| 84 | goto error_ret; | |
| 85 | ||
| 86 | if (state) | |
| 87 | msc |= ITG3200_IRQ_DATA_RDY_ENABLE; | |
| 88 | else | |
| 89 | msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE; | |
| 90 | ||
| 91 | ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc); | |
| 92 | if (ret) | |
| 93 | goto error_ret; | |
| 94 | ||
| 95 | error_ret: | |
| 96 | return ret; | |
| 97 | ||
| 98 | } | |
| 99 | ||
| 100 | static const struct iio_trigger_ops itg3200_trigger_ops = { | |
| 9dbf091d MS |
101 | .set_trigger_state = &itg3200_data_rdy_trigger_set_state, |
| 102 | }; | |
| 103 | ||
| 104 | int itg3200_probe_trigger(struct iio_dev *indio_dev) | |
| 105 | { | |
| 106 | int ret; | |
| 107 | struct itg3200 *st = iio_priv(indio_dev); | |
| 108 | ||
| 109 | st->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name, | |
| 110 | indio_dev->id); | |
| 111 | if (!st->trig) | |
| 112 | return -ENOMEM; | |
| 113 | ||
| 114 | ret = request_irq(st->i2c->irq, | |
| 115 | &iio_trigger_generic_data_rdy_poll, | |
| 116 | IRQF_TRIGGER_RISING, | |
| 117 | "itg3200_data_rdy", | |
| 118 | st->trig); | |
| 119 | if (ret) | |
| 120 | goto error_free_trig; | |
| 121 | ||
| 122 | ||
| 123 | st->trig->dev.parent = &st->i2c->dev; | |
| 124 | st->trig->ops = &itg3200_trigger_ops; | |
| 1e9663c6 | 125 | iio_trigger_set_drvdata(st->trig, indio_dev); |
| 9dbf091d MS |
126 | ret = iio_trigger_register(st->trig); |
| 127 | if (ret) | |
| 128 | goto error_free_irq; | |
| 129 | ||
| 130 | /* select default trigger */ | |
| 0b4dce2e | 131 | indio_dev->trig = iio_trigger_get(st->trig); |
| 9dbf091d MS |
132 | |
| 133 | return 0; | |
| 134 | ||
| 135 | error_free_irq: | |
| 136 | free_irq(st->i2c->irq, st->trig); | |
| 137 | error_free_trig: | |
| 138 | iio_trigger_free(st->trig); | |
| 139 | return ret; | |
| 140 | } | |
| 141 | ||
| 142 | void itg3200_remove_trigger(struct iio_dev *indio_dev) | |
| 143 | { | |
| 144 | struct itg3200 *st = iio_priv(indio_dev); | |
| 145 | ||
| 146 | iio_trigger_unregister(st->trig); | |
| 147 | free_irq(st->i2c->irq, st->trig); | |
| 148 | iio_trigger_free(st->trig); | |
| 149 | } |