path: root/drivers/platform/chrome/cros_ec_sensorhub_ring.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Driver for Chrome OS EC Sensor hub FIFO.
 *
 * Copyright 2020 Google LLC
 */

#include <linux/delay.h>
#include <linux/device.h>
#include <linux/iio/iio.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/platform_data/cros_ec_commands.h>
#include <linux/platform_data/cros_ec_proto.h>
#include <linux/platform_data/cros_ec_sensorhub.h>
#include <linux/platform_device.h>
#include <linux/sort.h>
#include <linux/slab.h>

#define CREATE_TRACE_POINTS
#include "cros_ec_sensorhub_trace.h"

/* Precision of fixed point for the m values from the filter */
#define M_PRECISION BIT(23)

/* Only activate the filter once we have at least this many elements. */
#define TS_HISTORY_THRESHOLD 8

/*
 * If we don't have any history entries for this long, empty the filter to
 * make sure there are no big discontinuities.
 */
#define TS_HISTORY_BORED_US 500000

/* To measure by how much the filter is overshooting, if it happens. */
#define FUTURE_TS_ANALYTICS_COUNT_MAX 100

static inline int
cros_sensorhub_send_sample(struct cros_ec_sensorhub *sensorhub,
			   struct cros_ec_sensors_ring_sample *sample)
{
	cros_ec_sensorhub_push_data_cb_t cb;
	int id = sample->sensor_id;
	struct iio_dev *indio_dev;

	if (id >= sensorhub->sensor_num)
		return -EINVAL;

	cb = sensorhub->push_data[id].push_data_cb;
	if (!cb)
		return 0;

	indio_dev = sensorhub->push_data[id].indio_dev;

	if (sample->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
		return 0;

	return cb(indio_dev, sample->vector, sample->timestamp);
}

/**
 * cros_ec_sensorhub_register_push_data() - register the callback to the hub.
 *
 * @sensorhub : Sensor Hub object
 * @sensor_num : The sensor the caller is interested in.
 * @indio_dev : The iio device to use when a sample arrives.
 * @cb : The callback to call when a sample arrives.
 *
 * The callback cb will be used by cros_ec_sensorhub_ring to distribute events
 * from the EC.
 *
 * Return: 0 when callback is registered.
 *         EINVAL is the sensor number is invalid or the slot already used.
 */
int cros_ec_sensorhub_register_push_data(struct cros_ec_sensorhub *sensorhub,
					 u8 sensor_num,
					 struct iio_dev *indio_dev,
					 cros_ec_sensorhub_push_data_cb_t cb)
{
	if (sensor_num >= sensorhub->sensor_num)
		return -EINVAL;
	if (sensorhub->push_data[sensor_num].indio_dev)
		return -EINVAL;

	sensorhub->push_data[sensor_num].indio_dev = indio_dev;
	sensorhub->push_data[sensor_num].push_data_cb = cb;

	return 0;
}
EXPORT_SYMBOL_GPL(cros_ec_sensorhub_register_push_data);

void cros_ec_sensorhub_unregister_push_data(struct cros_ec_sensorhub *sensorhub,
					    u8 sensor_num)
{
	sensorhub->push_data[sensor_num].indio_dev = NULL;
	sensorhub->push_data[sensor_num].push_data_cb = NULL;
}
EXPORT_SYMBOL_GPL(cros_ec_sensorhub_unregister_push_data);

/**
 * cros_ec_sensorhub_ring_fifo_enable() - Enable or disable interrupt generation
 *					  for FIFO events.
 * @sensorhub: Sensor Hub object
 * @on: true when events are requested.
 *
 * To be called before sleeping or when no one is listening.
 * Return: 0 on success, or an error when we can not communicate with the EC.
 *
 */
int cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub *sensorhub,
				       bool on)
{
	int ret, i;

	mutex_lock(&sensorhub->cmd_lock);
	if (sensorhub->tight_timestamps)
		for (i = 0; i < sensorhub->sensor_num; i++)
			sensorhub->batch_state[i].last_len = 0;

	sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INT_ENABLE;
	sensorhub->params->fifo_int_enable.enable = on;

	sensorhub->msg->outsize = sizeof(struct ec_params_motion_sense);
	sensorhub->msg->insize = sizeof(struct ec_response_motion_sense);

	ret = cros_ec_cmd_xfer_status(sensorhub->ec->ec_dev, sensorhub->msg);
	mutex_unlock(&sensorhub->cmd_lock);

	/* We expect to receive a payload of 4 bytes, ignore. */
	if (ret > 0)
		ret = 0;

	return ret;
}

static void cros_ec_sensor_ring_median_swap(s64 *a, s64 *b)
{
	s64 tmp = *a;
	*a = *b;
	*b = tmp;
}

/*
 * cros_ec_sensor_ring_median: Gets median of an array of numbers
 *
 * It's implemented using the quickselect algorithm, which achieves an
 * average time complexity of O(n) the middle element. In the worst case,
 * the runtime of quickselect could regress to O(n^2). To mitigate this,
 * algorithms like median-of-medians exist, which can guarantee O(n) even
 * in the worst case. However, these algorithms come with a higher
 * overhead and are more complex to implement, making quickselect a
 * pragmatic choice for our use case.
 *
 * Warning: the input array gets modified!
 */
static s64 cros_ec_sensor_ring_median(s64 *array, size_t length)
{
	int lo = 0;
	int hi = length - 1;

	while (lo <= hi) {
		int mid = lo + (hi - lo) / 2;
		int pivot, i;

		if (array[lo] > array[mid])
			cros_ec_sensor_ring_median_swap(&array[lo], &array[mid]);
		if (array[lo] > array[hi])
			cros_ec_sensor_ring_median_swap(&array[lo], &array[hi]);
		if (array[mid] < array[hi])
			cros_ec_sensor_ring_median_swap(&array[mid], &array[hi]);

		pivot = array[hi];
		i = lo - 1;

		for (int j = lo; j < hi; j++)
			if (array[j] < pivot)
				cros_ec_sensor_ring_median_swap(&array[++i],