156 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * itg3200_buffer.c -- support InvenSense ITG32004 * Digital 3-Axis Gyroscope driver5 *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 /*50 * Ensure correct alignment and padding including for the51 * 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 64error_ret:65 iio_trigger_notify_done(indio_dev->trig);66 67 return IRQ_HANDLED;68}69 70int 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 76void itg3200_buffer_unconfigure(struct iio_dev *indio_dev)77{78 iio_triggered_buffer_cleanup(indio_dev);79}80 81 82static 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 else96 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 102error_ret:103 return ret;104 105}106 107static const struct iio_trigger_ops itg3200_trigger_ops = {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(&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 141error_free_irq:142 free_irq(st->i2c->irq, st->trig);143error_free_trig:144 iio_trigger_free(st->trig);145 return ret;146}147 148void 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}156