drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

   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
  26 static 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
  36 int 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
  92 reset_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  */
 103 irqreturn_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  */
 120 irqreturn_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(&indio_dev->mlock);
 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
 180 end_session:
 181         mutex_unlock(&indio_dev->mlock);
 182         iio_trigger_notify_done(indio_dev->trig);
 183
 184         return IRQ_HANDLED;
 185
 186 flush_fifo:
 187         /* Flush HW and SW FIFOs. */
 188         inv_reset_fifo(indio_dev);
 189         mutex_unlock(&indio_dev->mlock);
 190         iio_trigger_notify_done(indio_dev->trig);
 191
 192         return IRQ_HANDLED;
 193 }