summaryrefslogtreecommitdiffstats
path: root/drivers/iio/imu
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/imu')
-rw-r--r--drivers/iio/imu/inv_mpu6050/inv_mpu_core.c31
-rw-r--r--drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h36
-rw-r--r--drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c143
3 files changed, 131 insertions, 79 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 42618fe4f83e..0f6efe913b5f 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -20,8 +20,6 @@
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
-#include <linux/kfifo.h>
-#include <linux/spinlock.h>
#include <linux/iio/iio.h>
#include <linux/acpi.h>
#include <linux/platform_device.h>
@@ -84,7 +82,7 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = {
static const struct inv_mpu6050_chip_config chip_config_6050 = {
.fsr = INV_MPU6050_FSR_2000DPS,
.lpf = INV_MPU6050_FILTER_20HZ,
- .fifo_rate = INV_MPU6050_INIT_FIFO_RATE,
+ .divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE),
.gyro_fifo_enable = false,
.accl_fifo_enable = false,
.accl_fs = INV_MPU6050_FS_02G,
@@ -280,7 +278,7 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
if (result)
goto error_power_off;
- d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1;
+ d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE);
result = regmap_write(st->map, st->reg->sample_rate_div, d);
if (result)
goto error_power_off;
@@ -297,6 +295,13 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
memcpy(&st->chip_config, hw_info[st->chip_type].config,
sizeof(struct inv_mpu6050_chip_config));
+ /*
+ * Internal chip period is 1ms (1kHz).
+ * Let's use at the beginning the theorical value before measuring
+ * with interrupt timestamps.
+ */
+ st->chip_period = NSEC_PER_MSEC;
+
return inv_mpu6050_set_power_itg(st, false);
error_power_off:
@@ -630,7 +635,7 @@ static ssize_t
inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
const char *buf, size_t count)
{
- s32 fifo_rate;
+ int fifo_rate;
u8 d;
int result;
struct iio_dev *indio_dev = dev_to_iio_dev(dev);
@@ -646,8 +651,13 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
if (result)
return result;
+ /* compute the chip sample rate divider */
+ d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(fifo_rate);
+ /* compute back the fifo rate to handle truncation cases */
+ fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(d);
+
mutex_lock(&st->lock);
- if (fifo_rate == st->chip_config.fifo_rate) {
+ if (d == st->chip_config.divider) {
result = 0;
goto fifo_rate_fail_unlock;
}
@@ -655,11 +665,10 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
if (result)
goto fifo_rate_fail_unlock;
- d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1;
result = regmap_write(st->map, st->reg->sample_rate_div, d);
if (result)
goto fifo_rate_fail_power_off;
- st->chip_config.fifo_rate = fifo_rate;
+ st->chip_config.divider = d;
result = inv_mpu6050_set_lpf(st, fifo_rate);
if (result)
@@ -687,7 +696,7 @@ inv_fifo_rate_show(struct device *dev, struct device_attribute *attr,
unsigned fifo_rate;
mutex_lock(&st->lock);
- fifo_rate = st->chip_config.fifo_rate;
+ fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
mutex_unlock(&st->lock);
return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate);
@@ -1005,7 +1014,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
indio_dev->modes = INDIO_BUFFER_TRIGGERED;
result = devm_iio_triggered_buffer_setup(dev, indio_dev,
- inv_mpu6050_irq_handler,
+ iio_pollfunc_store_time,
inv_mpu6050_read_fifo,
NULL);
if (result) {
@@ -1018,8 +1027,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
return result;
}
- INIT_KFIFO(st->timestamps);
- spin_lock_init(&st->time_stamp_lock);
result = devm_iio_device_register(dev, indio_dev);
if (result) {
dev_err(dev, "IIO register fail %d\n", result);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index c54da777945d..de8391693e17 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -12,8 +12,6 @@
*/
#include <linux/i2c.h>
#include <linux/i2c-mux.h>
-#include <linux/kfifo.h>
-#include <linux/spinlock.h>
#include <linux/mutex.h>
#include <linux/iio/iio.h>
#include <linux/iio/buffer.h>
@@ -88,7 +86,7 @@ enum inv_devices {
* @accl_fs: accel full scale range.
* @accl_fifo_enable: enable accel data output
* @gyro_fifo_enable: enable gyro data output
- * @fifo_rate: FIFO update rate.
+ * @divider: chip sample rate divider (sample rate divider - 1)
*/
struct inv_mpu6050_chip_config {
unsigned int fsr:2;
@@ -96,7 +94,7 @@ struct inv_mpu6050_chip_config {
unsigned int accl_fs:2;
unsigned int accl_fifo_enable:1;
unsigned int gyro_fifo_enable:1;
- u16 fifo_rate;
+ u8 divider;
u8 user_ctrl;
};
@@ -116,40 +114,40 @@ struct inv_mpu6050_hw {
/*
* struct inv_mpu6050_state - Driver state variables.
- * @TIMESTAMP_FIFO_SIZE: fifo size for timestamp.
* @lock: Chip access lock.
* @trig: IIO trigger.
* @chip_config: Cached attribute information.
* @reg: Map of important registers.
* @hw: Other hardware-specific information.
* @chip_type: chip type.
- * @time_stamp_lock: spin lock to time stamp.
* @plat_data: platform data (deprecated in favor of @orientation).
* @orientation: sensor chip orientation relative to main hardware.
- * @timestamps: kfifo queue to store time stamp.
* @map regmap pointer.
* @irq interrupt number.
* @irq_mask the int_pin_cfg mask to configure interrupt type.
+ * @chip_period: chip internal period estimation (~1kHz).
+ * @it_timestamp: timestamp from previous interrupt.
+ * @data_timestamp: timestamp for next data sample.
*/
struct inv_mpu6050_state {
-#define TIMESTAMP_FIFO_SIZE 16
struct mutex lock;
struct iio_trigger *trig;
struct inv_mpu6050_chip_config chip_config;
const struct inv_mpu6050_reg_map *reg;
const struct inv_mpu6050_hw *hw;
enum inv_devices chip_type;
- spinlock_t time_stamp_lock;
struct i2c_mux_core *muxc;
struct i2c_client *mux_client;
unsigned int powerup_count;
struct inv_mpu6050_platform_data plat_data;
struct iio_mount_matrix orientation;
- DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE);
struct regmap *map;
int irq;
u8 irq_mask;
unsigned skip_samples;
+ s64 chip_period;
+ s64 it_timestamp;
+ s64 data_timestamp;
};
/*register and associated bit definition*/
@@ -174,6 +172,7 @@ struct inv_mpu6050_state {
#define INV_MPU6050_REG_RAW_GYRO 0x43
#define INV_MPU6050_REG_INT_STATUS 0x3A
+#define INV_MPU6050_BIT_FIFO_OVERFLOW_INT 0x10
#define INV_MPU6050_BIT_RAW_DATA_RDY_INT 0x01
#define INV_MPU6050_REG_USER_CTRL 0x6A
@@ -198,7 +197,6 @@ struct inv_mpu6050_state {
#define INV_MPU6050_BYTES_PER_3AXIS_SENSOR 6
#define INV_MPU6050_FIFO_COUNT_BYTE 2
-#define INV_MPU6050_FIFO_THRESHOLD 500
/* mpu6500 registers */
#define INV_MPU6500_REG_ACCEL_CONFIG_2 0x1D
@@ -231,13 +229,24 @@ struct inv_mpu6050_state {
#define INV_MPU6050_LATCH_INT_EN 0x20
#define INV_MPU6050_BIT_BYPASS_EN 0x2
+/* Allowed timestamp period jitter in percent */
+#define INV_MPU6050_TS_PERIOD_JITTER 4
/* init parameters */
#define INV_MPU6050_INIT_FIFO_RATE 50
-#define INV_MPU6050_TIME_STAMP_TOR 5
#define INV_MPU6050_MAX_FIFO_RATE 1000
#define INV_MPU6050_MIN_FIFO_RATE 4
-#define INV_MPU6050_ONE_K_HZ 1000
+
+/* chip internal frequency: 1KHz */
+#define INV_MPU6050_INTERNAL_FREQ_HZ 1000
+/* return the frequency divider (chip sample rate divider + 1) */
+#define INV_MPU6050_FREQ_DIVIDER(st) \
+ ((st)->chip_config.divider + 1)
+/* chip sample rate divider to fifo rate */
+#define INV_MPU6050_FIFO_RATE_TO_DIVIDER(fifo_rate) \
+ ((INV_MPU6050_INTERNAL_FREQ_HZ / (fifo_rate)) - 1)
+#define INV_MPU6050_DIVIDER_TO_FIFO_RATE(divider) \
+ (INV_MPU6050_INTERNAL_FREQ_HZ / ((divider) + 1))
#define INV_MPU6050_REG_WHOAMI 117
@@ -300,7 +309,6 @@ enum inv_mpu6050_clock_sel_e {
NUM_CLK
};
-irqreturn_t inv_mpu6050_irq_handler(int irq, void *p);
irqreturn_t inv_mpu6050_read_fifo(int irq, void *p);
int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type);
int inv_reset_fifo(struct iio_dev *indio_dev);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 1795418438e4..548e042f7b5b 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -19,18 +19,83 @@
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
-#include <linux/kfifo.h>
#include <linux/poll.h>
+#include <linux/math64.h>
+#include <asm/unaligned.h>
#include "inv_mpu_iio.h"
-static void inv_clear_kfifo(struct inv_mpu6050_state *st)
+/**
+ * inv_mpu6050_update_period() - Update chip internal period estimation
+ *
+ * @st: driver state
+ * @timestamp: the interrupt timestamp
+ * @nb: number of data set in the fifo
+ *
+ * This function uses interrupt timestamps to estimate the chip period and
+ * to choose the data timestamp to come.
+ */
+static void inv_mpu6050_update_period(struct inv_mpu6050_state *st,
+ s64 timestamp, size_t nb)
{
- unsigned long flags;
+ /* Period boundaries for accepting timestamp */
+ const s64 period_min =
+ (NSEC_PER_MSEC * (100 - INV_MPU6050_TS_PERIOD_JITTER)) / 100;
+ const s64 period_max =
+ (NSEC_PER_MSEC * (100 + INV_MPU6050_TS_PERIOD_JITTER)) / 100;
+ const s32 divider = INV_MPU6050_FREQ_DIVIDER(st);
+ s64 delta, interval;
+ bool use_it_timestamp = false;
+
+ if (st->it_timestamp == 0) {
+ /* not initialized, forced to use it_timestamp */
+ use_it_timestamp = true;
+ } else if (nb == 1) {
+ /*
+ * Validate the use of it timestamp by checking if interrupt
+ * has been delayed.
+ * nb > 1 means interrupt was delayed for more than 1 sample,
+ * so it's obviously not good.
+ * Compute the chip period between 2 interrupts for validating.
+ */
+ delta = div_s64(timestamp - st->it_timestamp, divider);
+ if (delta > period_min && delta < period_max) {
+ /* update chip period and use it timestamp */
+ st->chip_period = (st->chip_period + delta) / 2;
+ use_it_timestamp = true;
+ }
+ }
+
+ if (use_it_timestamp) {
+ /*
+ * Manage case of multiple samples in the fifo (nb > 1):
+ * compute timestamp corresponding to the first sample using
+ * estimated chip period.
+ */
+ interval = (nb - 1) * st->chip_period * divider;
+ st->data_timestamp = timestamp - interval;
+ }
- /* take the spin lock sem to avoid interrupt kick in */
- spin_lock_irqsave(&st->time_stamp_lock, flags);
- kfifo_reset(&st->timestamps);
- spin_unlock_irqrestore(&st->time_stamp_lock, flags);
+ /* save it timestamp */
+ st->it_timestamp = timestamp;
+}
+
+/**
+ * inv_mpu6050_get_timestamp() - Return the current data timestamp
+ *
+ * @st: driver state
+ * @return: current data timestamp
+ *
+ * This function returns the current data timestamp and prepares for next one.
+ */
+static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
+{
+ s64 ts;
+
+ /* return current data timestamp and increment */
+ ts = st->data_timestamp;
+ st->data_timestamp += st->chip_period * INV_MPU6050_FREQ_DIVIDER(st);
+
+ return ts;
}
int inv_reset_fifo(struct iio_dev *indio_dev)
@@ -39,6 +104,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
u8 d;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
+ /* reset it timestamp validation */
+ st->it_timestamp = 0;
+
/* disable interrupt */
result = regmap_write(st->map, st->reg->int_enable, 0);
if (result) {
@@ -62,9 +130,6 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
if (result)
goto reset_fifo_fail;
- /* clear timestamps fifo */
- inv_clear_kfifo(st);
-
/* enable interrupt */
if (st->chip_config.accl_fifo_enable ||
st->chip_config.gyro_fifo_enable) {
@@ -99,23 +164,6 @@ reset_fifo_fail:
}
/**
- * inv_mpu6050_irq_handler() - Cache a timestamp at each data ready interrupt.
- */
-irqreturn_t inv_mpu6050_irq_handler(int irq, void *p)
-{
- struct iio_poll_func *pf = p;
- struct iio_dev *indio_dev = pf->indio_dev;
- struct inv_mpu6050_state *st = iio_priv(indio_dev);
- s64 timestamp;
-
- timestamp = iio_get_time_ns(indio_dev);
- kfifo_in_spinlocked(&st->timestamps, &timestamp, 1,
- &st->time_stamp_lock);
-
- return IRQ_WAKE_THREAD;
-}
-
-/**
* inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
*/
irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
@@ -129,6 +177,7 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
u16 fifo_count;
s64 timestamp;
int int_status;
+ size_t i, nb;
mutex_lock(&st->lock);
@@ -139,6 +188,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
"failed to ack interrupt\n");
goto flush_fifo;
}
+ /* handle fifo overflow by reseting fifo */
+ if (int_status & INV_MPU6050_BIT_FIFO_OVERFLOW_INT)
+ goto flush_fifo;
if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT)) {
dev_warn(regmap_get_device(st->map),
"spurious interrupt with status 0x%x\n", int_status);
@@ -163,38 +215,23 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
INV_MPU6050_FIFO_COUNT_BYTE);
if (result)
goto end_session;
- fifo_count = be16_to_cpup((__be16 *)(&data[0]));
- if (fifo_count < bytes_per_datum)
- goto end_session;
- /* fifo count can't be an odd number. If it is odd, reset the FIFO. */
- if (fifo_count & 1)
- goto flush_fifo;
- if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
- goto flush_fifo;
- /* Timestamp mismatch. */
- if (kfifo_len(&st->timestamps) >
- fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
- goto flush_fifo;
- do {
+ fifo_count = get_unaligned_be16(&data[0]);
+ /* compute and process all complete datum */
+ nb = fifo_count / bytes_per_datum;
+ inv_mpu6050_update_period(st, pf->timestamp, nb);
+ for (i = 0; i < nb; ++i) {
result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
data, bytes_per_datum);
if (result)
goto flush_fifo;
-
- result = kfifo_out(&st->timestamps, &timestamp, 1);
- /* when there is no timestamp, put timestamp as 0 */
- if (result == 0)
- timestamp = 0;
-
/* skip first samples if needed */
- if (st->skip_samples)
+ if (st->skip_samples) {
st->skip_samples--;
- else
- iio_push_to_buffers_with_timestamp(indio_dev, data,
- timestamp);
-
- fifo_count -= bytes_per_datum;
- } while (fifo_count >= bytes_per_datum);
+ continue;
+ }
+ timestamp = inv_mpu6050_get_timestamp(st);
+ iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp);
+ }
end_session:
mutex_unlock(&st->lock);
generated by cgit v1.2.3-55-g7522 (git 2.39.0) at 2024-07-23 02:06:22 +0200