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