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