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 17 #include <linux/iio/common/inv_sensors_timestamp.h> 18 19 #include "inv_mpu_iio.h" 20 21 static int inv_reset_fifo(struct iio_dev *indio_dev) 22 { 23 int result; 24 struct inv_mpu6050_state *st = iio_priv(indio_dev); 25 26 /* disable fifo and reenable it */ 27 inv_mpu6050_prepare_fifo(st, false); 28 result = inv_mpu6050_prepare_fifo(st, true); 29 if (result) 30 goto reset_fifo_fail; 31 32 return 0; 33 34 reset_fifo_fail: 35 dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result); 36 result = regmap_write(st->map, st->reg->int_enable, 37 INV_MPU6050_BIT_DATA_RDY_EN); 38 39 return result; 40 } 41 42 /* 43 * inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO. 44 */ 45 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p) 46 { 47 struct iio_poll_func *pf = p; 48 struct iio_dev *indio_dev = pf->indio_dev; 49 struct inv_mpu6050_state *st = iio_priv(indio_dev); 50 size_t bytes_per_datum; 51 int result; 52 u16 fifo_count; 53 u32 fifo_period; 54 s64 timestamp; 55 u8 data[INV_MPU6050_OUTPUT_DATA_SIZE]; 56 int int_status; 57 size_t i, nb; 58 59 mutex_lock(&st->lock); 60 61 /* ack interrupt and check status */ 62 result = regmap_read(st->map, st->reg->int_status, &int_status); 63 if (result) { 64 dev_err(regmap_get_device(st->map), 65 "failed to ack interrupt\n"); 66 goto flush_fifo; 67 } 68 if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) 69 goto end_session; 70 71 if (!(st->chip_config.accl_fifo_enable | 72 st->chip_config.gyro_fifo_enable | 73 st->chip_config.magn_fifo_enable)) 74 goto end_session; 75 bytes_per_datum = 0; 76 if (st->chip_config.accl_fifo_enable) 77 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 78 79 if (st->chip_config.gyro_fifo_enable) 80 bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR; 81 82 if (st->chip_config.temp_fifo_enable) 83 bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR; 84 85 if (st->chip_config.magn_fifo_enable) 86 bytes_per_datum += INV_MPU9X50_BYTES_MAGN; 87 88 /* 89 * read fifo_count register to know how many bytes are inside the FIFO 90 * right now 91 */ 92 result = regmap_bulk_read(st->map, st->reg->fifo_count_h, 93 st->data, INV_MPU6050_FIFO_COUNT_BYTE); 94 if (result) 95 goto end_session; 96 fifo_count = be16_to_cpup((__be16 *)&st->data[0]); 97 98 /* 99 * Handle fifo overflow by resetting fifo. 100 * Reset if there is only 3 data set free remaining to mitigate 101 * possible delay between reading fifo count and fifo data. 102 */ 103 nb = 3 * bytes_per_datum; 104 if (fifo_count >= st->hw->fifo_size - nb) { 105 dev_warn(regmap_get_device(st->map), "fifo overflow reset\n"); 106 goto flush_fifo; 107 } 108 109 /* compute and process only all complete datum */ 110 nb = fifo_count / bytes_per_datum; 111 fifo_count = nb * bytes_per_datum; 112 /* Each FIFO data contains all sensors, so same number for FIFO and sensor data */ 113 fifo_period = NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider); 114 inv_sensors_timestamp_interrupt(&st->timestamp, fifo_period, nb, nb, pf->timestamp); 115 inv_sensors_timestamp_apply_odr(&st->timestamp, fifo_period, nb, 0); 116 117 /* clear internal data buffer for avoiding kernel data leak */ 118 memset(data, 0, sizeof(data)); 119 120 /* read all data once and process every samples */ 121 result = regmap_noinc_read(st->map, st->reg->fifo_r_w, st->data, fifo_count); 122 if (result) 123 goto flush_fifo; 124 for (i = 0; i < nb; ++i) { 125 /* skip first samples if needed */ 126 if (st->skip_samples) { 127 st->skip_samples--; 128 continue; 129 } 130 memcpy(data, &st->data[i * bytes_per_datum], bytes_per_datum); 131 timestamp = inv_sensors_timestamp_pop(&st->timestamp); 132 iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp); 133 } 134 135 end_session: 136 mutex_unlock(&st->lock); 137 iio_trigger_notify_done(indio_dev->trig); 138 139 return IRQ_HANDLED; 140 141 flush_fifo: 142 /* Flush HW and SW FIFOs. */ 143 inv_reset_fifo(indio_dev); 144 mutex_unlock(&st->lock); 145 iio_trigger_notify_done(indio_dev->trig); 146 147 return IRQ_HANDLED; 148 } 149