new/kernel-rt/drivers/iio/imu/inv_icm40605/icm40605_ring.c

// SPDX-License-Identifier: GPL-2.0
/* Copyright (C) 2021 Rockchip Electronics Co., Ltd */

#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 <asm/unaligned.h>
#include "icm40605.h"

/**
 *  icm40605_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 icm40605_update_period(struct icm40605_data *st,
				   s64 timestamp, size_t nb)
{
	s64 interval;

	if (st->it_timestamp != 0)
		st->interrupt_period = div_s64((timestamp - st->it_timestamp), nb);

	interval = (nb - 1) * st->interrupt_period;
	st->data_timestamp = timestamp - interval;
	/* save it timestamp */
	st->it_timestamp = timestamp;
}

/**
 *  icm40605_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 icm40605_get_timestamp(struct icm40605_data *st)
{
	s64 ts;

	/* return current data timestamp and increment */
	ts = st->data_timestamp;
	st->data_timestamp += st->interrupt_period;

	return ts;
}

int icm40605_reset_fifo(struct iio_dev *indio_dev)
{
	int ret;
	struct icm40605_data  *st = iio_priv(indio_dev);

	/* reset it timestamp validation */
	st->it_timestamp = 0;
	ret = regmap_write(st->regmap, MPUREG_REG_BANK_SEL, ICM40605_BANK0);
	if (ret) {
		dev_err(regmap_get_device(st->regmap), "sel_bank0 fail: %d\n",
			ret);
		return ret;
	}
	/* disable interrupt */
	ret = regmap_write(st->regmap, MPUREG_INT_SOURCE0_REG, 0x0);
	if (ret) {
		dev_err(regmap_get_device(st->regmap), "int_enable failed %d\n",
			ret);
		goto reset_fifo_fail;
	}
	/* disable the sensor output to FIFO */
	ret = regmap_write(st->regmap, MPUREG_FIFO_CONFIG_REG, 0x0);
	if (ret)
		goto reset_fifo_fail;

	/* reset FIFO*/
	ret = regmap_write(st->regmap, MPUREG_SIGNAL_PATH_RESET_REG, BIT_FIFO_FLUSH);
	if (ret)
		goto reset_fifo_fail;

	/* enable sensor output to FIFO */
	ret = regmap_write(st->regmap, MPUREG_FIFO_CONFIG_REG, 0x80);
	if (ret)
		goto reset_fifo_fail;

	ret = regmap_write(st->regmap, MPUREG_TMST_CONFIG_REG, 0x31);
	if (ret) {
		dev_err(regmap_get_device(st->regmap), "set MPUREG_TMST_CONFIG_REG fail: %d, val: 0x39\n",
			ret);
		goto reset_fifo_fail;
	}

	ret = regmap_write(st->regmap, MPUREG_FIFO_CONFIG2_REG, BIT_FIFO_WM5);
	if (ret)
		goto reset_fifo_fail;

	/* enable interrupt */
	ret = regmap_write(st->regmap, MPUREG_INT_SOURCE0_REG,
			   BIT_INT_RESET_DONE_INT1_EN|BIT_INT_FIFO_THS_INT1_EN);
	if (ret)
		goto reset_fifo_fail;

	ret = regmap_write(st->regmap, MPUREG_FIFO_CONFIG1_REG, 0xf);
	if (ret)
		goto reset_fifo_fail;

	ret = regmap_write(st->regmap, MPUREG_INT_CONFIG1_REG, 0x10);
	if (ret)
		goto reset_fifo_fail;

	st->interrupt_period = st->standard_period;
	return 0;

reset_fifo_fail:
	dev_err(regmap_get_device(st->regmap), "%s :reset fifo failed %d\n", __func__, ret);
	ret = regmap_write(st->regmap, MPUREG_INT_SOURCE0_REG,
			   BIT_INT_RESET_DONE_INT1_EN|
			   BIT_INT_FIFO_THS_INT1_EN);

	return ret;
}


/**
 * icm40605_read_fifo() - Transfer data from hardware FIFO to KFIFO.
 */
irqreturn_t icm40605_read_fifo(int irq, void *p)
{
	struct iio_poll_func *pf = p;
	struct iio_dev *indio_dev = pf->indio_dev;
	struct icm40605_data *st = iio_priv(indio_dev);
	size_t bytes_per_datum;
	int result, int_status, interval_time;
	u8 data[ICM40605_OUTPUT_DATA_SIZE_PULS_ONE], i;
	u16 fifo_count;
	u32 data_len;
	s64 timestamp = 0;

	mutex_lock(&st->lock);

	result = regmap_read(st->regmap, MPUREG_GYRO_CONFIG0_REG, &interval_time);
	if (result) {
		dev_err(regmap_get_device(st->regmap),
			"failed get interval_time\n");
		goto flush_fifo;
	}

	switch (interval_time & 0x0f) {
	case 0xA:
		interval_time = 40000;
		break;
	case 0x9:
		interval_time = 20000;
		break;
	case 0x8:
		interval_time = 10000;
		break;
	case 0x7:
		interval_time = 5000;
		break;
	case 0x6:
		interval_time = 1000;
		break;
	case 0x5:
		interval_time = 500;
		break;
	case 0x4:
		interval_time = 250;
		break;
	case 0x3:
		interval_time = 125;
		break;
	default:
		interval_time = 5000;
	}

	/* ack interrupt and check status */
	result = regmap_read(st->regmap, MPUREG_INT_STATUS, &int_status);
	if (result) {
		dev_err(regmap_get_device(st->regmap),
			"failed to ack interrupt\n");
		goto flush_fifo;
	}

	if (!(int_status & BIT_STATUS_FIFO_THS)) {
		dev_warn(regmap_get_device(st->regmap),
			 "spurious interrupt with status 0x%x\n",
			 int_status);
		if (!(int_status & (BIT_STATUS_DRDY | BIT_STATUS_FIFO_THS)))
			goto flush_fifo;
	}

	if ((int_status & BIT_STATUS_FIFO_FULL)) {
		dev_warn(regmap_get_device(st->regmap), "the fifo is full\n");
		goto flush_fifo;
	}

	bytes_per_datum = ICM40605_FIFO_DATUM;
	result = regmap_bulk_read(st->regmap, MPUREG_FIFO_BYTE_COUNT1_REG,
				  data, ICM40605_FIFO_COUNT_BYTE);
	if (result) {
		dev_err(regmap_get_device(st->regmap), "read fifo count fail: %d\n", result);
		goto end_session;
	}

	fifo_count = (data[0] << 8) | data[1];
	if (fifo_count > ICM40605_FIFO_COUNT_LIMIT) {
		dev_warn(regmap_get_device(st->regmap),
			 "fifo overflow reset, cnt: %u\n",
			 fifo_count);
		goto flush_fifo;
	}


	icm40605_update_period(st, pf->timestamp, fifo_count);

	data_len = bytes_per_datum * fifo_count;

	result = regmap_bulk_read(st->regmap, MPUREG_FIFO_DATA_REG,
				  st->data_buff, data_len);
	if (result) {
		dev_err(regmap_get_device(st->regmap),
			"regmap_bulk_read failed\n");
		goto flush_fifo;
	}

	for (i = 0; i < fifo_count; ++i) {
		/* skip first samples if needed */
		if (st->skip_samples) {
			st->skip_samples--;
			continue;
		}
		memcpy(data, st->data_buff+i*16, bytes_per_datum);
		timestamp = icm40605_get_timestamp(st);
		iio_push_to_buffers_with_timestamp(indio_dev, &(data[1]), timestamp);
	}

end_session:
	mutex_unlock(&st->lock);
	iio_trigger_notify_done(indio_dev->trig);

	return IRQ_HANDLED;

flush_fifo:
	/* Flush HW and SW FIFOs. */
	dev_info(regmap_get_device(st->regmap), "flush info\n");
	icm40605_reset_fifo(indio_dev);
	mutex_unlock(&st->lock);
	iio_trigger_notify_done(indio_dev->trig);

	return IRQ_HANDLED;
}