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, 55 st->chip_config.user_ctrl); 56 if (result) 57 goto reset_fifo_fail; 58 59 /* reset FIFO*/ 60 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST; 61 result = regmap_write(st->map, st->reg->user_ctrl, d); 62 if (result) 63 goto reset_fifo_fail; 64 65 /* clear timestamps fifo */ 66 inv_clear_kfifo(st); 67 68 /* enable interrupt */ 69 if (st->chip_config.accl_fifo_enable || 70 st->chip_config.gyro_fifo_enable) { 71 result = regmap_write(st->map, st->reg->int_enable, 72 INV_MPU6050_BIT_DATA_RDY_EN); 73 if (result) 74 return result; 75 } 76 /* enable FIFO reading */ 77 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN; 78 result = regmap_write(st->map, st->reg->user_ctrl, d); 79 if (result) 80 goto reset_fifo_fail; 81 /* enable sensor output to FIFO */ 82 d = 0; 83 if (st->chip_config.gyro_fifo_enable) 84 d |= INV_MPU6050_BITS_GYRO_OUT; 85 if (st->chip_config.accl_fifo_enable) 86 d |= INV_MPU6050_BIT_ACCEL_OUT; 87 result = regmap_write(st->map, st->reg->fifo_en, d); 88 if (result) 89 goto reset_fifo_fail; 90 91 return 0; 92 93 reset_fifo_fail: 94 dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result); 95 result = regmap_write(st->map, st->reg->int_enable, 96 INV_MPU6050_BIT_DATA_RDY_EN); 97 98 return result; 99 } 100 101 /** 102 * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt. 103 */ 104 irqreturn_t inv_mpu6050_irq_handler(int irq, void *p) 105 { 106 struct iio_poll_func *pf = p; 107 struct iio_dev *indio_dev = pf->indio_dev; 108 struct inv_mpu6050_state *st = iio_priv(indio_dev); 109 s64 timestamp; 110 111 timestamp = iio_get_time_ns(indio_dev); 112 kfifo_in_spinlocked(&st->timestamps, ×tamp, 1, 113 &st->time_stamp_lock); 114 115 return IRQ_WAKE_THREAD; 116 } 117 118 /** 119 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO. 120 */ 121 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) 122 { 123 struct iio_poll_func *pf = p; 124 struct iio_dev *indio_dev = pf->indio_dev; 125 struct inv_mpu6050_state *st = iio_priv(indio_dev); 126 size_t bytes_per_datum; 127 int result; 128 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE]; 129 u16 fifo_count; 130 s64 timestamp; 131 int int_status; 132 133 mutex_lock(&st->lock); 134 135 /* ack interrupt and check status */ 136 result = regmap_read(st->map, st->reg->int_status, &int_status); 137 if (result) { 138 dev_err(regmap_get_device(st->map), 139 "failed to ack interrupt\n"); 140 goto flush_fifo; 141 } 142 if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) { 143 dev_warn(regmap_get_device(st->map), 144 "spurious interrupt with status 0x%x\n", int_status); 145 goto end_session; 146 } 147 148 if (!(st->chip_config.accl_fifo_enable | 149 st->chip_config.gyro_fifo_enable)) 150 goto end_session; 151 bytes_per_datum = 0; 152 if (st->chip_config.accl_fifo_enable) 153 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 154 155 if (st->chip_config.gyro_fifo_enable) 156 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 157 158 /* 159 * read fifo_count register to know how many bytes are inside the FIFO 160 * right now 161 */ 162 result = regmap_bulk_read(st->map, st->reg->fifo_count_h, data, 163 INV_MPU6050_FIFO_COUNT_BYTE); 164 if (result) 165 goto end_session; 166 fifo_count = be16_to_cpup((__be16 *)(&data[0])); 167 if (fifo_count < bytes_per_datum) 168 goto end_session; 169 /* fifo count can't be an odd number. If it is odd, reset the FIFO. */ 170 if (fifo_count & 1) 171 goto flush_fifo; 172 if (fifo_count > INV_MPU6050_FIFO_THRESHOLD) 173 goto flush_fifo; 174 /* Timestamp mismatch. */ 175 if (kfifo_len(&st->timestamps) > 176 fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR) 177 goto flush_fifo; 178 do { 179 result = regmap_bulk_read(st->map, st->reg->fifo_r_w, 180 data, bytes_per_datum); 181 if (result) 182 goto flush_fifo; 183 184 result = kfifo_out(&st->timestamps, ×tamp, 1); 185 /* when there is no timestamp, put timestamp as 0 */ 186 if (result == 0) 187 timestamp = 0; 188 189 /* skip first samples if needed */ 190 if (st->skip_samples) 191 st->skip_samples--; 192 else 193 iio_push_to_buffers_with_timestamp(indio_dev, data, 194 timestamp); 195 196 fifo_count -= bytes_per_datum; 197 } while (fifo_count >= bytes_per_datum); 198 199 end_session: 200 mutex_unlock(&st->lock); 201 iio_trigger_notify_done(indio_dev->trig); 202 203 return IRQ_HANDLED; 204 205 flush_fifo: 206 /* Flush HW and SW FIFOs. */ 207 inv_reset_fifo(indio_dev); 208 mutex_unlock(&st->lock); 209 iio_trigger_notify_done(indio_dev->trig); 210 211 return IRQ_HANDLED; 212 } 213