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, ×tamp, 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(&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, ×tamp, 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(&st->lock); 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(&st->lock); 190 iio_trigger_notify_done(indio_dev->trig); 191 192 return IRQ_HANDLED; 193 } 194