LXR linux/drivers/iio/gyro/itg3200

   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 <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>
   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
  23static 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
  44static 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
  55        iio_push_to_buffers_with_timestamp(indio_dev, buf, pf->timestamp);
  56
  57        iio_trigger_notify_done(indio_dev->trig);
  58
  59error_ret:
  60        return IRQ_HANDLED;
  61}
  62
  63int 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
  69void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
  70{
  71        iio_triggered_buffer_cleanup(indio_dev);
  72}
  73
  74
  75static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
  76                bool state)
  77{
  78        struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
  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
  95error_ret:
  96        return ret;
  97
  98}
  99
 100static const struct iio_trigger_ops itg3200_trigger_ops = {
 101        .set_trigger_state = &itg3200_data_rdy_trigger_set_state,
 102};
 103
 104int 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;
 125        iio_trigger_set_drvdata(st->trig, indio_dev);
 126        ret = iio_trigger_register(st->trig);
 127        if (ret)
 128                goto error_free_irq;
 129
 130        /* select default trigger */
 131        indio_dev->trig = iio_trigger_get(st->trig);
 132
 133        return 0;
 134
 135error_free_irq:
 136        free_irq(st->i2c->irq, st->trig);
 137error_free_trig:
 138        iio_trigger_free(st->trig);
 139        return ret;
 140}
 141
 142void 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}
 150