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/poll.h> 23 #include <linux/math64.h> 24 #include <asm/unaligned.h> 25 #include "inv_mpu_iio.h" 26 27 /** 28 * inv_mpu6050_update_period() - Update chip internal period estimation 29 * 30 * @st: driver state 31 * @timestamp: the interrupt timestamp 32 * @nb: number of data set in the fifo 33 * 34 * This function uses interrupt timestamps to estimate the chip period and 35 * to choose the data timestamp to come. 36 */ 37 static void inv_mpu6050_update_period(struct inv_mpu6050_state *st, 38 s64 timestamp, size_t nb) 39 { 40 /* Period boundaries for accepting timestamp */ 41 const s64 period_min = 42 (NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100; 43 const s64 period_max = 44 (NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100; 45 const s32 divider = INV_MPU6050_FREQ_DIVIDER(st); 46 s64 delta, interval; 47 bool use_it_timestamp = false; 48 49 if (st->it_timestamp == 0) { 50 /* not initialized, forced to use it_timestamp */ 51 use_it_timestamp = true; 52 } else if (nb == 1) { 53 /* 54 * Validate the use of it timestamp by checking if interrupt 55 * has been delayed. 56 * nb > 1 means interrupt was delayed for more than 1 sample, 57 * so it's obviously not good. 58 * Compute the chip period between 2 interrupts for validating. 59 */ 60 delta = div_s64(timestamp - st->it_timestamp, divider); 61 if (delta > period_min && delta < period_max) { 62 /* update chip period and use it timestamp */ 63 st->chip_period = (st->chip_period + delta) / 2; 64 use_it_timestamp = true; 65 } 66 } 67 68 if (use_it_timestamp) { 69 /* 70 * Manage case of multiple samples in the fifo (nb > 1): 71 * compute timestamp corresponding to the first sample using 72 * estimated chip period. 73 */ 74 interval = (nb - 1) * st->chip_period * divider; 75 st->data_timestamp = timestamp - interval; 76 } 77 78 /* save it timestamp */ 79 st->it_timestamp = timestamp; 80 } 81 82 /** 83 * inv_mpu6050_get_timestamp() - Return the current data timestamp 84 * 85 * @st: driver state 86 * @return: current data timestamp 87 * 88 * This function returns the current data timestamp and prepares for next one. 89 */ 90 static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st) 91 { 92 s64 ts; 93 94 /* return current data timestamp and increment */ 95 ts = st->data_timestamp; 96 st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st); 97 98 return ts; 99 } 100 101 int inv_reset_fifo(struct iio_dev *indio_dev) 102 { 103 int result; 104 u8 d; 105 struct inv_mpu6050_state *st = iio_priv(indio_dev); 106 107 /* reset it timestamp validation */ 108 st->it_timestamp = 0; 109 110 /* disable interrupt */ 111 result = regmap_write(st->map, st->reg->int_enable, 0); 112 if (result) { 113 dev_err(regmap_get_device(st->map), "int_enable failed %d\n", 114 result); 115 return result; 116 } 117 /* disable the sensor output to FIFO */ 118 result = regmap_write(st->map, st->reg->fifo_en, 0); 119 if (result) 120 goto reset_fifo_fail; 121 /* disable fifo reading */ 122 result = regmap_write(st->map, st->reg->user_ctrl, 123 st->chip_config.user_ctrl); 124 if (result) 125 goto reset_fifo_fail; 126 127 /* reset FIFO*/ 128 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST; 129 result = regmap_write(st->map, st->reg->user_ctrl, d); 130 if (result) 131 goto reset_fifo_fail; 132 133 /* enable interrupt */ 134 if (st->chip_config.accl_fifo_enable || 135 st->chip_config.gyro_fifo_enable) { 136 result = regmap_write(st->map, st->reg->int_enable, 137 INV_MPU6050_BIT_DATA_RDY_EN); 138 if (result) 139 return result; 140 } 141 /* enable FIFO reading */ 142 d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN; 143 result = regmap_write(st->map, st->reg->user_ctrl, d); 144 if (result) 145 goto reset_fifo_fail; 146 /* enable sensor output to FIFO */ 147 d = 0; 148 if (st->chip_config.gyro_fifo_enable) 149 d |= INV_MPU6050_BITS_GYRO_OUT; 150 if (st->chip_config.accl_fifo_enable) 151 d |= INV_MPU6050_BIT_ACCEL_OUT; 152 result = regmap_write(st->map, st->reg->fifo_en, d); 153 if (result) 154 goto reset_fifo_fail; 155 156 return 0; 157 158 reset_fifo_fail: 159 dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result); 160 result = regmap_write(st->map, st->reg->int_enable, 161 INV_MPU6050_BIT_DATA_RDY_EN); 162 163 return result; 164 } 165 166 /** 167 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO. 168 */ 169 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) 170 { 171 struct iio_poll_func *pf = p; 172 struct iio_dev *indio_dev = pf->indio_dev; 173 struct inv_mpu6050_state *st = iio_priv(indio_dev); 174 size_t bytes_per_datum; 175 int result; 176 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE]; 177 u16 fifo_count; 178 s64 timestamp; 179 int int_status; 180 size_t i, nb; 181 182 mutex_lock(&st->lock); 183 184 /* ack interrupt and check status */ 185 result = regmap_read(st->map, st->reg->int_status, &int_status); 186 if (result) { 187 dev_err(regmap_get_device(st->map), 188 "failed to ack interrupt\n"); 189 goto flush_fifo; 190 } 191 /* handle fifo overflow by reseting fifo */ 192 if (int_status & INV_MPU6050_BIT_FIFO_OVERFLOW_INT) 193 goto flush_fifo; 194 if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) { 195 dev_warn(regmap_get_device(st->map), 196 "spurious interrupt with status 0x%x\n", int_status); 197 goto end_session; 198 } 199 200 if (!(st->chip_config.accl_fifo_enable | 201 st->chip_config.gyro_fifo_enable)) 202 goto end_session; 203 bytes_per_datum = 0; 204 if (st->chip_config.accl_fifo_enable) 205 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 206 207 if (st->chip_config.gyro_fifo_enable) 208 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 209 210 /* 211 * read fifo_count register to know how many bytes are inside the FIFO 212 * right now 213 */ 214 result = regmap_bulk_read(st->map, st->reg->fifo_count_h, data, 215 INV_MPU6050_FIFO_COUNT_BYTE); 216 if (result) 217 goto end_session; 218 fifo_count = get_unaligned_be16(&data[0]); 219 /* compute and process all complete datum */ 220 nb = fifo_count / bytes_per_datum; 221 inv_mpu6050_update_period(st, pf->timestamp, nb); 222 for (i = 0; i < nb; ++i) { 223 result = regmap_bulk_read(st->map, st->reg->fifo_r_w, 224 data, bytes_per_datum); 225 if (result) 226 goto flush_fifo; 227 /* skip first samples if needed */ 228 if (st->skip_samples) { 229 st->skip_samples--; 230 continue; 231 } 232 timestamp = inv_mpu6050_get_timestamp(st); 233 iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp); 234 } 235 236 end_session: 237 mutex_unlock(&st->lock); 238 iio_trigger_notify_done(indio_dev->trig); 239 240 return IRQ_HANDLED; 241 242 flush_fifo: 243 /* Flush HW and SW FIFOs. */ 244 inv_reset_fifo(indio_dev); 245 mutex_unlock(&st->lock); 246 iio_trigger_notify_done(indio_dev->trig); 247 248 return IRQ_HANDLED; 249 } 250