OpenCloudOS-Kernel/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

209 lines
5.5 KiB
C

// SPDX-License-Identifier: GPL-2.0-only
/*
* Copyright (C) 2012 Invensense, Inc.
*/
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/poll.h>
#include <linux/math64.h>
#include "inv_mpu_iio.h"
/**
* 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)
{
/* 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;
}
/* 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;
}
static int inv_reset_fifo(struct iio_dev *indio_dev)
{
int result;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
/* disable fifo and reenable it */
inv_mpu6050_prepare_fifo(st, false);
result = inv_mpu6050_prepare_fifo(st, true);
if (result)
goto reset_fifo_fail;
return 0;
reset_fifo_fail:
dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
result = regmap_write(st->map, st->reg->int_enable,
INV_MPU6050_BIT_DATA_RDY_EN);
return result;
}
/*
* inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
*/
irqreturn_t inv_mpu6050_read_fifo(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);
size_t bytes_per_datum;
int result;
u16 fifo_count;
s64 timestamp;
int int_status;
size_t i, nb;
mutex_lock(&st->lock);
/* ack interrupt and check status */
result = regmap_read(st->map, st->reg->int_status, &int_status);
if (result) {
dev_err(regmap_get_device(st->map),
"failed to ack interrupt\n");
goto flush_fifo;
}
if (!(int_status & INV_MPU6050_BIT_RAW_DATA_RDY_INT))
goto end_session;
if (!(st->chip_config.accl_fifo_enable |
st->chip_config.gyro_fifo_enable |
st->chip_config.magn_fifo_enable))
goto end_session;
bytes_per_datum = 0;
if (st->chip_config.accl_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
if (st->chip_config.gyro_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
if (st->chip_config.temp_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_TEMP_SENSOR;
if (st->chip_config.magn_fifo_enable)
bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
/*
* read fifo_count register to know how many bytes are inside the FIFO
* right now
*/
result = regmap_bulk_read(st->map, st->reg->fifo_count_h,
st->data, INV_MPU6050_FIFO_COUNT_BYTE);
if (result)
goto end_session;
fifo_count = be16_to_cpup((__be16 *)&st->data[0]);
/*
* Handle fifo overflow by resetting fifo.
* Reset if there is only 3 data set free remaining to mitigate
* possible delay between reading fifo count and fifo data.
*/
nb = 3 * bytes_per_datum;
if (fifo_count >= st->hw->fifo_size - nb) {
dev_warn(regmap_get_device(st->map), "fifo overflow reset\n");
goto flush_fifo;
}
/* 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_noinc_read(st->map, st->reg->fifo_r_w,
st->data, bytes_per_datum);
if (result)
goto flush_fifo;
/* skip first samples if needed */
if (st->skip_samples) {
st->skip_samples--;
continue;
}
timestamp = inv_mpu6050_get_timestamp(st);
iio_push_to_buffers_with_timestamp(indio_dev, st->data, timestamp);
}
end_session:
mutex_unlock(&st->lock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
flush_fifo:
/* Flush HW and SW FIFOs. */
inv_reset_fifo(indio_dev);
mutex_unlock(&st->lock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}