drivers/iio/gyro/itg3200_buffer.c

   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 <[email protected]>
   7  * Copyright (c) 2011 Manuel Stahl <[email protected]>
   8  * Copyright (c) 2012 Thorsten Nowak <[email protected]>
   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         /*
  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
  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
  76 void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
  77 {
  78         iio_triggered_buffer_cleanup(indio_dev);
  79 }
  80
  81
  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
 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
 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 }