LXR linux/drivers/iio/gyro/itg3200

   1/*
   2 * itg3200_buffer.c -- support InvenSense ITG3200
   3 *                     Digital 3-Axis Gyroscope driver
   4 *
   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>
   8 *
   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.
  12 */
  13
  14#include <linux/slab.h>
  15#include <linux/i2c.h>
  16#include <linux/interrupt.h>
  17
  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>
  24
  25
  26static int itg3200_read_all_channels(struct i2c_client *i2c, __be16 *buf)
  27{
  28        u8 tx = 0x80 | ITG3200_REG_TEMP_OUT_H;
  29        struct i2c_msg msg[2] = {
  30                {
  31                        .addr = i2c->addr,
  32                        .flags = i2c->flags,
  33                        .len = 1,
  34                        .buf = &tx,
  35                },
  36                {
  37                        .addr = i2c->addr,
  38                        .flags = i2c->flags | I2C_M_RD,
  39                        .len = ITG3200_SCAN_ELEMENTS * sizeof(s16),
  40                        .buf = (char *)&buf,
  41                },
  42        };
  43
  44        return i2c_transfer(i2c->adapter, msg, 2);
  45}
  46
  47static irqreturn_t itg3200_trigger_handler(int irq, void *p)
  48{
  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)];
  53
  54        int ret = itg3200_read_all_channels(st->i2c, buf);
  55        if (ret < 0)
  56                goto error_ret;
  57
  58        if (indio_dev->scan_timestamp)
  59                memcpy(buf + indio_dev->scan_bytes - sizeof(s64),
  60                                &pf->timestamp, sizeof(pf->timestamp));
  61
  62        iio_push_to_buffers(indio_dev, (u8 *)buf);
  63        iio_trigger_notify_done(indio_dev->trig);
  64
  65error_ret:
  66        return IRQ_HANDLED;
  67}
  68
  69int itg3200_buffer_configure(struct iio_dev *indio_dev)
  70{
  71        return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
  72                itg3200_trigger_handler, NULL);
  73}
  74
  75void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
  76{
  77        iio_triggered_buffer_cleanup(indio_dev);
  78}
  79
  80
  81static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
  82                bool state)
  83{
  84        struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
  85        int ret;
  86        u8 msc;
  87
  88        ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
  89        if (ret)
  90                goto error_ret;
  91
  92        if (state)
  93                msc |= ITG3200_IRQ_DATA_RDY_ENABLE;
  94        else
  95                msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;
  96
  97        ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
  98        if (ret)
  99                goto error_ret;
 100
 101error_ret:
 102        return ret;
 103
 104}
 105
 106static const struct iio_trigger_ops itg3200_trigger_ops = {
 107        .owner = THIS_MODULE,
 108        .set_trigger_state = &itg3200_data_rdy_trigger_set_state,
 109};
 110
 111int 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("%s-dev%d", indio_dev->name,
 117                                     indio_dev->id);
 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->dev.parent = &st->i2c->dev;
 131        st->trig->ops = &itg3200_trigger_ops;
 132        iio_trigger_set_drvdata(st->trig, indio_dev);
 133        ret = iio_trigger_register(st->trig);
 134        if (ret)
 135                goto error_free_irq;
 136
 137        /* select default trigger */
 138        indio_dev->trig = st->trig;
 139
 140        return 0;
 141
 142error_free_irq:
 143        free_irq(st->i2c->irq, st->trig);
 144error_free_trig:
 145        iio_trigger_free(st->trig);
 146        return ret;
 147}
 148
 149void itg3200_remove_trigger(struct iio_dev *indio_dev)
 150{
 151        struct itg3200 *st = iio_priv(indio_dev);
 152
 153        iio_trigger_unregister(st->trig);
 154        free_irq(st->i2c->irq, st->trig);
 155        iio_trigger_free(st->trig);
 156}
 157