iio/gyro/itg3200_buffer.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * itg3200_buffer.c -- support InvenSense ITG3200
0004  *                     Digital 3-Axis Gyroscope driver
0005  *
0006  * Copyright (c) 2011 Christian Strobel <christian.strobel@iis.fraunhofer.de>
0007  * Copyright (c) 2011 Manuel Stahl <manuel.stahl@iis.fraunhofer.de>
0008  * Copyright (c) 2012 Thorsten Nowak <thorsten.nowak@iis.fraunhofer.de>
0009  */
0010
0011 #include <linux/slab.h>
0012 #include <linux/i2c.h>
0013 #include <linux/interrupt.h>
0014
0015 #include <linux/iio/iio.h>
0016 #include <linux/iio/buffer.h>
0017 #include <linux/iio/trigger.h>
0018 #include <linux/iio/trigger_consumer.h>
0019 #include <linux/iio/triggered_buffer.h>
0020 #include <linux/iio/gyro/itg3200.h>
0021
0022
0023 static int itg3200_read_all_channels(struct i2c_client *i2c, __be16 *buf)
0024 {
0025     u8 tx = 0x80 | ITG3200_REG_TEMP_OUT_H;
0026     struct i2c_msg msg[2] = {
0027         {
0028             .addr = i2c->addr,
0029             .flags = i2c->flags,
0030             .len = 1,
0031             .buf = &tx,
0032         },
0033         {
0034             .addr = i2c->addr,
0035             .flags = i2c->flags | I2C_M_RD,
0036             .len = ITG3200_SCAN_ELEMENTS * sizeof(s16),
0037             .buf = (char *)&buf,
0038         },
0039     };
0040
0041     return i2c_transfer(i2c->adapter, msg, 2);
0042 }
0043
0044 static irqreturn_t itg3200_trigger_handler(int irq, void *p)
0045 {
0046     struct iio_poll_func *pf = p;
0047     struct iio_dev *indio_dev = pf->indio_dev;
0048     struct itg3200 *st = iio_priv(indio_dev);
0049     /*
0050      * Ensure correct alignment and padding including for the
0051      * timestamp that may be inserted.
0052      */
0053     struct {
0054         __be16 buf[ITG3200_SCAN_ELEMENTS];
0055         s64 ts __aligned(8);
0056     } scan;
0057
0058     int ret = itg3200_read_all_channels(st->i2c, scan.buf);
0059     if (ret < 0)
0060         goto error_ret;
0061
0062     iio_push_to_buffers_with_timestamp(indio_dev, &scan, pf->timestamp);
0063
0064 error_ret:
0065     iio_trigger_notify_done(indio_dev->trig);
0066
0067     return IRQ_HANDLED;
0068 }
0069
0070 int itg3200_buffer_configure(struct iio_dev *indio_dev)
0071 {
0072     return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
0073         itg3200_trigger_handler, NULL);
0074 }
0075
0076 void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
0077 {
0078     iio_triggered_buffer_cleanup(indio_dev);
0079 }
0080
0081
0082 static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
0083         bool state)
0084 {
0085     struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
0086     int ret;
0087     u8 msc;
0088
0089     ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
0090     if (ret)
0091         goto error_ret;
0092
0093     if (state)
0094         msc |= ITG3200_IRQ_DATA_RDY_ENABLE;
0095     else
0096         msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;
0097
0098     ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
0099     if (ret)
0100         goto error_ret;
0101
0102 error_ret:
0103     return ret;
0104
0105 }
0106
0107 static const struct iio_trigger_ops itg3200_trigger_ops = {
0108     .set_trigger_state = &itg3200_data_rdy_trigger_set_state,
0109 };
0110
0111 int itg3200_probe_trigger(struct iio_dev *indio_dev)
0112 {
0113     int ret;
0114     struct itg3200 *st = iio_priv(indio_dev);
0115
0116     st->trig = iio_trigger_alloc(&st->i2c->dev, "%s-dev%d", indio_dev->name,
0117                      iio_device_id(indio_dev));
0118     if (!st->trig)
0119         return -ENOMEM;
0120
0121     ret = request_irq(st->i2c->irq,
0122               &iio_trigger_generic_data_rdy_poll,
0123               IRQF_TRIGGER_RISING,
0124               "itg3200_data_rdy",
0125               st->trig);
0126     if (ret)
0127         goto error_free_trig;
0128
0129
0130     st->trig->ops = &itg3200_trigger_ops;
0131     iio_trigger_set_drvdata(st->trig, indio_dev);
0132     ret = iio_trigger_register(st->trig);
0133     if (ret)
0134         goto error_free_irq;
0135
0136     /* select default trigger */
0137     indio_dev->trig = iio_trigger_get(st->trig);
0138
0139     return 0;
0140
0141 error_free_irq:
0142     free_irq(st->i2c->irq, st->trig);
0143 error_free_trig:
0144     iio_trigger_free(st->trig);
0145     return ret;
0146 }
0147
0148 void itg3200_remove_trigger(struct iio_dev *indio_dev)
0149 {
0150     struct itg3200 *st = iio_priv(indio_dev);
0151
0152     iio_trigger_unregister(st->trig);
0153     free_irq(st->i2c->irq, st->trig);
0154     iio_trigger_free(st->trig);
0155 }