LXR linux/drivers/iio/imu/inv_mpu6050/inv_mpu

   1/*
   2* Copyright (C) 2012 Invensense, Inc.
   3*
   4* This software is licensed under the terms of the GNU General Public
   5* License version 2, as published by the Free Software Foundation, and
   6* may be copied, distributed, and modified under those terms.
   7*
   8* This program is distributed in the hope that it will be useful,
   9* but WITHOUT ANY WARRANTY; without even the implied warranty of
  10* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11* GNU General Public License for more details.
  12*/
  13
  14#include <linux/module.h>
  15#include <linux/slab.h>
  16#include <linux/i2c.h>
  17#include <linux/err.h>
  18#include <linux/delay.h>
  19#include <linux/sysfs.h>
  20#include <linux/jiffies.h>
  21#include <linux/irq.h>
  22#include <linux/interrupt.h>
  23#include <linux/kfifo.h>
  24#include <linux/poll.h>
  25#include "inv_mpu_iio.h"
  26
  27int inv_reset_fifo(struct iio_dev *indio_dev)
  28{
  29        int result;
  30        u8 d;
  31        struct inv_mpu6050_state  *st = iio_priv(indio_dev);
  32
  33        /* disable interrupt */
  34        result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
  35        if (result) {
  36                dev_err(&st->client->dev, "int_enable failed %d\n", result);
  37                return result;
  38        }
  39        /* disable the sensor output to FIFO */
  40        result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
  41        if (result)
  42                goto reset_fifo_fail;
  43        /* disable fifo reading */
  44        result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
  45        if (result)
  46                goto reset_fifo_fail;
  47
  48        /* reset FIFO*/
  49        result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
  50                                        INV_MPU6050_BIT_FIFO_RST);
  51        if (result)
  52                goto reset_fifo_fail;
  53        /* enable interrupt */
  54        if (st->chip_config.accl_fifo_enable ||
  55            st->chip_config.gyro_fifo_enable) {
  56                result = inv_mpu6050_write_reg(st, st->reg->int_enable,
  57                                        INV_MPU6050_BIT_DATA_RDY_EN);
  58                if (result)
  59                        return result;
  60        }
  61        /* enable FIFO reading and I2C master interface*/
  62        result = inv_mpu6050_write_reg(st, st->reg->user_ctrl,
  63                                        INV_MPU6050_BIT_FIFO_EN);
  64        if (result)
  65                goto reset_fifo_fail;
  66        /* enable sensor output to FIFO */
  67        d = 0;
  68        if (st->chip_config.gyro_fifo_enable)
  69                d |= INV_MPU6050_BITS_GYRO_OUT;
  70        if (st->chip_config.accl_fifo_enable)
  71                d |= INV_MPU6050_BIT_ACCEL_OUT;
  72        result = inv_mpu6050_write_reg(st, st->reg->fifo_en, d);
  73        if (result)
  74                goto reset_fifo_fail;
  75
  76        return 0;
  77
  78reset_fifo_fail:
  79        dev_err(&st->client->dev, "reset fifo failed %d\n", result);
  80        result = inv_mpu6050_write_reg(st, st->reg->int_enable,
  81                                        INV_MPU6050_BIT_DATA_RDY_EN);
  82
  83        return result;
  84}
  85
  86static void inv_clear_kfifo(struct inv_mpu6050_state *st)
  87{
  88        unsigned long flags;
  89
  90        /* take the spin lock sem to avoid interrupt kick in */
  91        spin_lock_irqsave(&st->time_stamp_lock, flags);
  92        kfifo_reset(&st->timestamps);
  93        spin_unlock_irqrestore(&st->time_stamp_lock, flags);
  94}
  95
  96/**
  97 * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
  98 */
  99irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
 100{
 101        struct iio_poll_func *pf = p;
 102        struct iio_dev *indio_dev = pf->indio_dev;
 103        struct inv_mpu6050_state *st = iio_priv(indio_dev);
 104        s64 timestamp;
 105
 106        timestamp = iio_get_time_ns();
 107        kfifo_in_spinlocked(&st->timestamps, &timestamp, 1,
 108                                &st->time_stamp_lock);
 109
 110        return IRQ_WAKE_THREAD;
 111}
 112
 113/**
 114 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
 115 */
 116irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 117{
 118        struct iio_poll_func *pf = p;
 119        struct iio_dev *indio_dev = pf->indio_dev;
 120        struct inv_mpu6050_state *st = iio_priv(indio_dev);
 121        size_t bytes_per_datum;
 122        int result;
 123        u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
 124        u16 fifo_count;
 125        s64 timestamp;
 126
 127        mutex_lock(&indio_dev->mlock);
 128        if (!(st->chip_config.accl_fifo_enable |
 129                st->chip_config.gyro_fifo_enable))
 130                goto end_session;
 131        bytes_per_datum = 0;
 132        if (st->chip_config.accl_fifo_enable)
 133                bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 134
 135        if (st->chip_config.gyro_fifo_enable)
 136                bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
 137
 138        /*
 139         * read fifo_count register to know how many bytes inside FIFO
 140         * right now
 141         */
 142        result = i2c_smbus_read_i2c_block_data(st->client,
 143                                       st->reg->fifo_count_h,
 144                                       INV_MPU6050_FIFO_COUNT_BYTE, data);
 145        if (result != INV_MPU6050_FIFO_COUNT_BYTE)
 146                goto end_session;
 147        fifo_count = be16_to_cpup((__be16 *)(&data[0]));
 148        if (fifo_count < bytes_per_datum)
 149                goto end_session;
 150        /* fifo count can't be odd number, if it is odd, reset fifo*/
 151        if (fifo_count & 1)
 152                goto flush_fifo;
 153        if (fifo_count >  INV_MPU6050_FIFO_THRESHOLD)
 154                goto flush_fifo;
 155        /* Timestamp mismatch. */
 156        if (kfifo_len(&st->timestamps) >
 157                fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
 158                        goto flush_fifo;
 159        while (fifo_count >= bytes_per_datum) {
 160                result = i2c_smbus_read_i2c_block_data(st->client,
 161                                                       st->reg->fifo_r_w,
 162                                                       bytes_per_datum, data);
 163                if (result != bytes_per_datum)
 164                        goto flush_fifo;
 165
 166                result = kfifo_out(&st->timestamps, &timestamp, 1);
 167                /* when there is no timestamp, put timestamp as 0 */
 168                if (0 == result)
 169                        timestamp = 0;
 170
 171                result = iio_push_to_buffers_with_timestamp(indio_dev, data,
 172                        timestamp);
 173                if (result)
 174                        goto flush_fifo;
 175                fifo_count -= bytes_per_datum;
 176        }
 177
 178end_session:
 179        mutex_unlock(&indio_dev->mlock);
 180        iio_trigger_notify_done(indio_dev->trig);
 181
 182        return IRQ_HANDLED;
 183
 184flush_fifo:
 185        /* Flush HW and SW FIFOs. */
 186        inv_reset_fifo(indio_dev);
 187        inv_clear_kfifo(st);
 188        mutex_unlock(&indio_dev->mlock);
 189        iio_trigger_notify_done(indio_dev->trig);
 190
 191        return IRQ_HANDLED;
 192}
 193