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/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
  28 int 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
  79 reset_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
  87 static 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  */
 100 irqreturn_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  */
 117 irqreturn_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
 181 end_session:
 182         mutex_unlock(&indio_dev->mlock);
 183         iio_trigger_notify_done(indio_dev->trig);
 184
 185         return IRQ_HANDLED;
 186
 187 flush_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 }