drivers/iio/gyro/itg3200_buffer.c

   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
  26 static 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
  47 static 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         /*
  53          * Ensure correct alignment and padding including for the
  54          * timestamp that may be inserted.
  55          */
  56         struct {
  57                 __be16 buf[ITG3200_SCAN_ELEMENTS];
  58                 s64 ts __aligned(8);
  59         } scan;
  60
  61         int ret = itg3200_read_all_channels(st->i2c, scan.buf);
  62         if (ret < 0)
  63                 goto error_ret;
  64
  65         iio_push_to_buffers_with_timestamp(indio_dev, &scan, pf->timestamp);
  66
  67         iio_trigger_notify_done(indio_dev->trig);
  68
  69 error_ret:
  70         return IRQ_HANDLED;
  71 }
  72
  73 int itg3200_buffer_configure(struct iio_dev *indio_dev)
  74 {
  75         return iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
  76                 itg3200_trigger_handler, NULL);
  77 }
  78
  79 void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)
  80 {
  81         iio_triggered_buffer_cleanup(indio_dev);
  82 }
  83
  84
  85 static int itg3200_data_rdy_trigger_set_state(struct iio_trigger *trig,
  86                 bool state)
  87 {
  88         struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
  89         int ret;
  90         u8 msc;
  91
  92         ret = itg3200_read_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, &msc);
  93         if (ret)
  94                 goto error_ret;
  95
  96         if (state)
  97                 msc |= ITG3200_IRQ_DATA_RDY_ENABLE;
  98         else
  99                 msc &= ~ITG3200_IRQ_DATA_RDY_ENABLE;
 100
 101         ret = itg3200_write_reg_8(indio_dev, ITG3200_REG_IRQ_CONFIG, msc);
 102         if (ret)
 103                 goto error_ret;
 104
 105 error_ret:
 106         return ret;
 107
 108 }
 109
 110 static const struct iio_trigger_ops itg3200_trigger_ops = {
 111         .owner = THIS_MODULE,
 112         .set_trigger_state = &itg3200_data_rdy_trigger_set_state,
 113 };
 114
 115 int itg3200_probe_trigger(struct iio_dev *indio_dev)
 116 {
 117         int ret;
 118         struct itg3200 *st = iio_priv(indio_dev);
 119
 120         st->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
 121                                      indio_dev->id);
 122         if (!st->trig)
 123                 return -ENOMEM;
 124
 125         ret = request_irq(st->i2c->irq,
 126                           &iio_trigger_generic_data_rdy_poll,
 127                           IRQF_TRIGGER_RISING,
 128                           "itg3200_data_rdy",
 129                           st->trig);
 130         if (ret)
 131                 goto error_free_trig;
 132
 133
 134         st->trig->dev.parent = &st->i2c->dev;
 135         st->trig->ops = &itg3200_trigger_ops;
 136         iio_trigger_set_drvdata(st->trig, indio_dev);
 137         ret = iio_trigger_register(st->trig);
 138         if (ret)
 139                 goto error_free_irq;
 140
 141         /* select default trigger */
 142         indio_dev->trig = iio_trigger_get(st->trig);
 143
 144         return 0;
 145
 146 error_free_irq:
 147         free_irq(st->i2c->irq, st->trig);
 148 error_free_trig:
 149         iio_trigger_free(st->trig);
 150         return ret;
 151 }
 152
 153 void itg3200_remove_trigger(struct iio_dev *indio_dev)
 154 {
 155         struct itg3200 *st = iio_priv(indio_dev);
 156
 157         iio_trigger_unregister(st->trig);
 158         free_irq(st->i2c->irq, st->trig);
 159         iio_trigger_free(st->trig);
 160 }