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