cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

itg3200_buffer.c (3555B)

      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	/*
     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
     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	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
    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}