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