path: root/common/motion_sense_fifo.c

/* Copyright 2019 The ChromiumOS Authors
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "accelgyro.h"
#include "console.h"
#include "hwtimer.h"
#include "math_util.h"
#include "mkbp_event.h"
#include "motion_sense_fifo.h"
#include "online_calibration.h"
#include "stdbool.h"
#include "tablet_mode.h"
#include "task.h"
#include "util.h"

#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ##args)

/*
 * Adjustment in us to ec rate when calculating interrupt interval:
 * To be sure the EC will send an interrupt even if it finishes processing
 * events slightly earlier than the previous period.
 */
#define MOTION_SENSOR_INT_ADJUSTMENT_US \
	(CONFIG_MOTION_MIN_SENSE_WAIT_TIME * MSEC / 10)

/**
 * Staged metadata for the fifo queue.
 * @read_ts: The timestamp at which the staged data was read. This value will
 *	serve as the upper bound for spreading
 * @count: The total number of motion_sense_fifo entries that are currently
 *	staged.
 * @sample_count: The total number of sensor readings per sensor that are
 *	currently staged.
 * @requires_spreading: Flag used to shortcut the commit process. This should be
 *	true iff at least one of sample_count[] > 1
 */
struct fifo_staged {
	uint32_t read_ts;
	uint16_t count;
	uint8_t sample_count[MAX_MOTION_SENSORS];
	uint8_t requires_spreading;
};

/**
 * Timestamp state metadata for maintaining spreading between commits.
 * @prev: The previous timestamp that was added to the FIFO
 * @next: The predicted next timestamp that will be added to the FIFO
 */
struct timestamp_state {
	uint32_t prev;
	uint32_t next;
};

/** Queue to hold the data to be sent to the AP. */
static struct queue fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO_SIZE,
				      struct ec_response_motion_sensor_data);
/** Count of the number of entries lost due to a small queue. */
static int fifo_lost;
/*
 * How many vector events are lost in the FIFO since last time
 * FIFO info has been transmitted.
 */
static uint16_t fifo_sensor_lost[MAX_MOTION_SENSORS];

/** Metadata for the fifo, used for staging and spreading data. */
static struct fifo_staged fifo_staged;

/**
 * Cached expected timestamp per sensor. If a sensor's timestamp pre-dates this
 * timestamp it will be fast forwarded.
 */
static struct timestamp_state next_timestamp[MAX_MOTION_SENSORS];

/**
 * Expected data periods:
 * copy of collection rate, updated when ODR changes.
 */
static uint32_t expected_data_periods[MAX_MOTION_SENSORS];

/**
 * Calculated data periods:
 * can be different from collection rate when spreading.
 */
static uint32_t data_periods[MAX_MOTION_SENSORS];

/**
 * Bitmap telling which sensors have valid entries in the next_timestamp array.
 */
static uint32_t next_timestamp_initialized;

/** Need to bypass the FIFO for an important message. */
static int bypass_needed;

/** Need to wake up the AP. */
static int wake_up_needed;

/** Need to interrupt the AP. */
static int ap_interrupt_needed;

/**
 * Timestamp of the first event put in the fifo during the
 * last motion_task invocation.
 */
uint32_t ts_last_int[MAX_MOTION_SENSORS];

/**
 * Check whether or not a give sensor data entry is a timestamp or not.
 *
 * @param data The data entry to check.
 * @return 1 if the entry is a timestamp, 0 otherwise.
 */
static inline int
is_timestamp(const struct ec_response_motion_sensor_data *data)
{
	return data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
}

/**
 * Check whether or not a given sensor data entry contains sensor data or not.
 *
 * @param data The data entry to check.
 * @return True if the entry contains data, false otherwise.
 */
static inline bool is_data(const struct ec_response_motion_sensor_data *data)
{
	return (data->flags & (MOTIONSENSE_SENSOR_FLAG_TIMESTAMP |
			       MOTIONSENSE_SENSOR_FLAG_ODR)) == 0;
}

/**
 * Convenience function to get the head of the fifo. This function makes no
 * guarantee on whether or not the entry is valid.
 *
 * @return Pointer to the head of the fifo.
 */
static inline struct ec_response_motion_sensor_data *get_fifo_head(void)
{
	return ((struct ec_response_motion_sensor_data *)fifo.buffer) +
	       (fifo.state->head & fifo.buffer_units_mask);
}

/**
 * Pop one entry from the motion sense fifo. Poping will give priority to
 * committed data (data residing between the head and tail of the queue). If no
 * committed data is available (all the data is staged), then this function will
 * remove the oldest staged data by moving both the head and tail.
 *
 * As a side-effect of this function, it'll updated any appropriate lost and
 * count variables.
 *
 * WARNING: This function MUST be called from within a locked context of
 * g_sensor_mutex.
 */
static void fifo_pop(void)
{
	struct ec_response_motion_sensor_data *head = get_fifo_head();
	const size_t initial_count = queue_count(&fifo);

	/* Check that we have something to pop. */
	if (!initial_count && !fifo_staged.count)
		return;

	/*
	 * If all the data is staged (nothing in the committed queue), we'll
	 * need to move the head and the tail over to simulate poping from the
	 * staged data.
	 */
	if (!initial_count)
		queue_advance_tail(&fifo, 1);

	/*
	 * If we're about to pop a wakeup flag, we should remember it as though
	 * it was committed.
	 */
	if (head->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
		wake_up_needed = 1;
	/*
	 * By not using queue_remove_unit we're avoiding an un-necessary memcpy.
	 */
	queue_advance_head(&fifo, 1);
	fifo_lost++;

	/* Increment lost counter if we have valid data. */
	if (!is_timestamp(head))
		fifo_sensor_lost[head->sensor_num]++;

	/*
	 * We're done if the initial count was non-zero and we only advanced the
	 * head. Else, decrement the staged count and update staged metadata.
	 */
	if (initial_count)
		return;

	fifo_staged.count--;

	/* If we removed a timestamp there's nothing else for us to do. */
	if (is_timestamp(head))
		return;

	/*
	 * Decrement sample count, if the count was 2 before, we might not need
	 * to spread anymore. Loop through and check.
	 */
	if (--fifo_staged.sample_count[head->sensor_num] < 2) {
		int i;

		fifo_staged.requires_spreading = 0;
		for (i = 0; i < MAX_MOTION_SENSORS; i++) {
			if (fifo_staged.sample_count[i] > 1) {
				fifo_staged.requires_spreading = 1;
				break;
			}
		}
	}
}

/**
 * Make sure that the fifo has at least 1 empty spot to stage data into.
 */
static void fifo_ensure_space(void)
{
	/* If we already have space just bail. */
	if (queue_space(&fifo) > fifo_staged.count)
		return;

	/*
	 * Pop at least 1 spot, but if all the following conditions are met we
	 * will continue to pop:
	 * 1. We're operating with tight timestamps.
	 * 2. The new head isn't a timestamp.
	 * 3. We have data that we can possibly pop.
	 *
	 * Removing more than one entry is needed because if we are using tight
	 * timestamps and we pop a timestamp, then the next head is data, the AP
	 * would assign a bad timestamp to it.
	 */
	do {
		fifo_pop();
	} while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
		 !is_timestamp(get_fifo_head()) &&
		 queue_count(&fifo) + fifo_staged.count);
}

/**
 * Test if a given timestamp is the first timestamp seen by a given sensor
 * number.
 *
 * @param sensor_num the sensor index to test.
 * @return True if the given sensor index has not seen a timestamp yet.
 */
static inline bool is_new_timestamp(uint8_t sensor_num)
{
	return sensor_num < MAX_MOTION_SENSORS &&
	       !(next_timestamp_initialized & BIT(sensor_num));
}

/**
 * Stage a single data unit to the motion sense fifo. Note that for the AP to
 * see this data, it must be committed.
 *
 * @param data The data to stage.
 * @param sensor The sensor that generated the data
 * @param valid_data The number of readable data entries in the data.
 *   sensor can be NULL (for activity sensors). valid_data must be 0 then.
 */
static void fifo_stage_unit(struct ec_response_motion_sensor_data *data,
			    struct motion_sensor_t *sensor, int valid_data)
{
	struct queue_chunk chunk;
	int i;

	mutex_lock(&g_sensor_mutex);

	for (i = 0; i < valid_data; i++)
		sensor->xyz[i] = data->data[i];

	/*
	 * For timestamps, update the next value of the sensor's timestamp
	 * if this timestamp is considered new.
	 */
	if (data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP &&
	    is_new_timestamp(data->sensor_num)) {
		next_timestamp[data->sensor_num].next =
			next_timestamp[data->sensor_num].prev = data->timestamp;
		next_timestamp_initialized |= BIT(data->sensor_num);
	}

	/* For valid sensors, check if AP really needs this data */
	if (valid_data) {
		int removed = 0;

		if (sensor->oversampling_ratio == 0) {
			removed = 1;
		} else {
			removed = sensor->oversampling++;
			sensor->oversampling %= sensor->oversampling_ratio;
		}
		if (removed) {
			mutex_unlock(&g_sensor_mutex);
			if (IS_ENABLED(CONFIG_ONLINE_CALIB) &&
			    !is_new_timestamp(data->sensor_num))
				online_calibration_process_data(
					data, sensor,
					next_timestamp[data->sensor_num].next);
			return;
		}
	}

	/* Make sure we have room for the data */
	fifo_ensure_space();

	if (IS_ENABLED(CONFIG_TABLET_MODE))
		data->flags |= (tablet_get_mode() ?
					MOTIONSENSE_SENSOR_FLAG_TABLET_MODE :
					0);

	/*
	 * Get the next writable block in the fifo. We don't need to lock this
	 * because it will always be past the tail and thus the AP will never
	 * read this until motion_sense_fifo_commit_data() is called.
	 */
	chunk = queue_get_write_chunk(&fifo, fifo_staged.count);

	if (!chunk.buffer) {
		/*
		 * This should never happen since we already ensured there was
		 * space, but if there was a bug, we don't want to write to
		 * address 0. Just don't add any data to the queue instead.
		 */
		CPRINTS("Failed to get write chunk for new fifo data!");
		mutex_unlock(&g_sensor_mutex);
		return;
	}

	/*
	 * Save the data to the writable block and increment count. This data
	 * will now reside AFTER the tail of the queue and will not be visible
	 * to the AP until the motion_sense_fifo_commit_data() function is
	 * called. Because count is incremented, the following staged data will
	 * be written to the next available block and this one will remain
	 * staged.
	 */
	memcpy(chunk.buffer, data, fifo.unit_bytes);
	fifo_staged.count++;

	/*
	 * If we're using tight timestamps, and the current entry isn't a
	 * timestamp we'll increment the sample_count for the given sensor.
	 * If the new per-sensor sample count is greater than 1, we'll need to
	 * spread.
	 */
	if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) && !is_timestamp(data) &&
	    ++fifo_staged.sample_count[data->sensor_num] > 1)
		fifo_staged.requires_spreading = 1;

	mutex_unlock(&g_sensor_mutex);
}

/**
 * Stage an entry representing a single timestamp.
 *
 * @param timestamp The timestamp to add to the fifo.
 * @param sensor_num The sensor number that this timestamp came from (use 0xff
 *	  for unknown).
 */
static void fifo_stage_timestamp(uint32_t timestamp, uint8_t sensor_num)
{
	struct ec_response_motion_sensor_data vector;

	vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
	vector.timestamp = timestamp;
	vector.sensor_num = sensor_num;
	fifo_stage_unit(&vector, NULL, 0);
}

/**
 * Peek into the staged data at a given offset. This function performs no bound
 * checking and is purely for confinience.
 *
 * @param offset The offset into the staged data to peek into.
 * @return Pointer to the entry at the given offset.
 */
static inline struct ec_response_motion_sensor_data *
peek_fifo_staged(size_t offset)
{
	return (struct ec_response_motion_sensor_data *)queue_get_write_chunk(
		       &fifo, offset)
		.buffer;
}

void motion_sense_fifo_init(void)
{
	if (IS_ENABLED(CONFIG_ONLINE_CALIB))
		online_calibration_init();
}

int motion_sense_fifo_interrupt_needed(void)
{
	return ap_interrupt_needed;
}

int motion_sense_fifo_bypass_needed(void)
{
	return bypass_needed;
}

int motion_sense_fifo_wake_up_needed(void)
{
	return wake_up_needed;
}

void motion_sense_fifo_reset_needed_flags(void)
{
	int i;

	if (ap_interrupt_needed) {
		ap_interrupt_needed = 0;
		/*
		 * The FIFO is emptied, note timestamp of the last event sent
		 * as we start counting the delay based on that timestamp.
		 */
		for (i = 0; i < MAX_MOTION_SENSORS; i++)
			if (!is_new_timestamp(i))
				ts_last_int[i] = next_timestamp[i].prev;
	}
	wake_up_needed = 0;
	bypass_needed = 0;
}

void motion_sense_fifo_insert_async_event(struct motion_sensor_t *sensor,
					  enum motion_sense_async_event event)
{
	struct ec_response_motion_sensor_data vector;

	vector.flags = event;
	vector.timestamp = __hw_clock_source_read();
	vector.sensor_num = sensor - motion_sensors;

	fifo_stage_unit(&vector, sensor, 0);
	motion_sense_fifo_commit_data();
}

inline void motion_sense_fifo_add_timestamp(uint32_t timestamp)
{
	fifo_stage_timestamp(timestamp, 0xff);
	motion_sense_fifo_commit_data();
}

void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
				  struct motion_sensor_t *sensor,
				  int valid_data, uint32_t time)
{
	int id = data->sensor_num;

	if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS)) {
		/* First entry, save the time for spreading later. */
		if (!fifo_staged.count)
			fifo_staged.read_ts = __hw_clock_source_read();
		fifo_stage_timestamp(time, data->sensor_num);
	}
	if (sensor->config[SENSOR_CONFIG_AP].ec_rate > 0 &&
	    time_after(time, ts_last_int[id] +
				     sensor->config[SENSOR_CONFIG_AP].ec_rate -
				     MOTION_SENSOR_INT_ADJUSTMENT_US)) {
		ap_interrupt_needed = 1;
	}
	fifo_stage_unit(data, sensor, valid_data);
}

void motion_sense_fifo_commit_data(void)
{
	struct ec_response_motion_sensor_data *data;
	int i, window, sensor_num;

	/* Nothing staged, no work to do. */
	if (!fifo_staged.count)
		return;

	mutex_lock(&g_sensor_mutex);
	/*
	 * If per-sensor event counts are never more than 1, no spreading is
	 * needed. This will also catch cases where tight timestamps aren't
	 * used.
	 */
	if (!fifo_staged.requires_spreading)
		goto commit_data_end;

	data = peek_fifo_staged(0);

	/*
	 * Spreading only makes sense if tight timestamps are used. In such case
	 * entries are expected to be ordered: timestamp then data. If the first
	 * entry isn't a timestamp we must have gotten out of sync. Just commit
	 * all the data and skip the spreading.
	 */
	if (!is_timestamp(data)) {
		CPRINTS("Spreading skipped, first entry is not a timestamp");
		fifo_staged.requires_spreading = 0;
		goto commit_data_end;
	}

	window = time_until(data->timestamp, fifo_staged.read_ts);

	/* Update the data_periods as needed for this flush. */
	for (i = 0; i < MAX_MOTION_SENSORS; i++) {
		int period;

		/* Skip empty sensors. */
		if (!fifo_staged.sample_count[i])
			continue;

		period = expected_data_periods[i];
		/*
		 * Clamp the sample period to the MIN of collection_rate and the
		 * window length / (sample count - 1).
		 */
		if (window && fifo_staged.sample_count[i] > 1)
			period =
				MIN(period,
				    window / (fifo_staged.sample_count[i] - 1));
		data_periods[i] = period;
	}

commit_data_end:
	/*
	 * Conditionally spread the timestamps.
	 *
	 * If we got this far that means that the tight timestamps config is
	 * enabled. This means that we can expect the staged entries to have 1
	 * or more timestamps followed by exactly 1 data entry. We'll loop
	 * through the timestamps until we get to data. We only need to update
	 * the timestamp right before it to keep things correct.
	 */
	for (i = 0; i < fifo_staged.count; i++) {
		data = peek_fifo_staged(i);
		if (data->flags & MOTIONSENSE_SENSOR_FLAG_BYPASS_FIFO)
			bypass_needed = 1;
		if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
			wake_up_needed = 1;

		/*
		 * Skip non-data entries, we don't know the sensor number yet.
		 */
		if (!is_data(data))
			continue;

		/* Get the sensor number and point to the timestamp entry. */
		sensor_num = data->sensor_num;
		data = peek_fifo_staged(i - 1);

		/* Verify we're pointing at a timestamp. */
		if (!is_timestamp(data)) {
			CPRINTS("FIFO entries out of order,"
				" expected timestamp");
			continue;
		}

		/*
		 * If this is the first time we're seeing a timestamp for this
		 * sensor or the timestamp is after our computed next, skip
		 * ahead.
		 */
		if (is_new_timestamp(sensor_num) ||
		    time_after(data->timestamp,
			       next_timestamp[sensor_num].prev)) {
			next_timestamp[sensor_num].next = data->timestamp;
			next_timestamp_initialized |= BIT(sensor_num);
		}

		/* Spread the timestamp and compute the expected next. */
		data->timestamp = next_timestamp[sensor_num].next;
		next_timestamp[sensor_num].prev =
			next_timestamp[sensor_num].next;
		next_timestamp[sensor_num].next +=
			fifo_staged.requires_spreading ?
				data_periods[sensor_num] :
				expected_data_periods[sensor_num];

		/* Update online calibration if enabled. */
		data = peek_fifo_staged(i);
		if (IS_ENABLED(CONFIG_ONLINE_CALIB))
			online_calibration_process_data(
				data, &motion_sensors[sensor_num],
				next_timestamp[sensor_num].prev);
	}

	/* Advance the tail and clear the staged metadata. */
	queue_advance_tail(&fifo, fifo_staged.count);

	/* Reset metadata for next staging cycle. */
	memset(&fifo_staged, 0, sizeof(fifo_staged));

	mutex_unlock(&g_sensor_mutex);
}

void motion_sense_fifo_get_info(
	struct ec_response_motion_sense_fifo_info *fifo_info, int reset)
{
	int i;

	mutex_lock(&g_sensor_mutex);
	fifo_info->size = fifo.buffer_units;
	fifo_info->count = queue_count(&fifo);
	fifo_info->total_lost = fifo_lost;
	for (i = 0; i < MAX_MOTION_SENSORS; i++) {
		fifo_info->lost[i] = fifo_sensor_lost[i];
	}
	mutex_unlock(&g_sensor_mutex);
#ifdef CONFIG_MKBP_EVENT
	fifo_info->timestamp = mkbp_last_event_time;
#endif

	if (reset) {
		fifo_lost = 0;
		memset(fifo_sensor_lost, 0, sizeof(fifo_sensor_lost));
	}
}

/* LCOV_EXCL_START - function cannot be tested due to limitations with mkbp */
static int motion_sense_get_next_event(uint8_t *out)
{
	union ec_response_get_next_data *data =
		(union ec_response_get_next_data *)out;
	/* out is not padded. It has one byte for the event type */
	motion_sense_fifo_get_info(&data->sensor_fifo.info, 0);
	return sizeof(data->sensor_fifo);
}
/* LCOV_EXCL_STOP */
DECLARE_EVENT_SOURCE(EC_MKBP_EVENT_SENSOR_FIFO, motion_sense_get_next_event);

inline int motion_sense_fifo_over_thres(void)
{
	int result;

	mutex_lock(&g_sensor_mutex);
	result = queue_space(&fifo) < CONFIG_ACCEL_FIFO_THRES;
	mutex_unlock(&g_sensor_mutex);

	return result;
}

int motion_sense_fifo_read(int capacity_bytes, int max_count, void *out,
			   uint16_t *out_size)
{
	int count;

	mutex_lock(&g_sensor_mutex);
	count = MIN(capacity_bytes / fifo.unit_bytes,
		    MIN(queue_count(&fifo), max_count));
	count = queue_remove_units(&fifo, out, count);
	mutex_unlock(&g_sensor_mutex);
	*out_size = count * fifo.unit_bytes;

	return count;
}

void motion_sense_fifo_reset(void)
{
	static uint8_t fifo_info_buffer
		[sizeof(struct ec_response_motion_sense_fifo_info) +
		 sizeof(uint16_t) * MAX_MOTION_SENSORS];
	struct ec_response_motion_sense_fifo_info *fifo_info =
		(void *)fifo_info_buffer;

	next_timestamp_initialized = 0;
	memset(&fifo_staged, 0, sizeof(fifo_staged));
	motion_sense_fifo_init();
	queue_init(&fifo);
	motion_sense_fifo_get_info(fifo_info, /*reset=*/true);
}

void motion_sense_set_data_period(int sensor_num, uint32_t data_period)
{
	expected_data_periods[sensor_num] = data_period;
	/*
	 * Reset the timestamp:
	 * - Avoid overflow when the sensor has been disabled for a long
	 * time.
	 * - First ODR setting.
	 * We may not send the first sample on time, but that is acceptable
	 * for CTS.
	 */
	ts_last_int[sensor_num] = __hw_clock_source_read();
	next_timestamp_initialized &= ~BIT(sensor_num);
}

#ifdef CONFIG_CMD_ACCEL_FIFO
static int motion_sense_read_fifo(int argc, char **argv)
{
	int count, i;
	struct ec_response_motion_sensor_data v;

	if (argc < 1)
		return EC_ERROR_PARAM_COUNT;

	/* Limit the amount of data to avoid saturating the UART buffer */
	count = MIN(queue_count(&fifo), 16);
	for (i = 0; i < count; i++) {
		queue_peek_units(&fifo, &v, i, 1);
		if (v.flags & (MOTIONSENSE_SENSOR_FLAG_TIMESTAMP |
			       MOTIONSENSE_SENSOR_FLAG_FLUSH)) {
			uint64_t timestamp;

			memcpy(&timestamp, v.data, sizeof(v.data));
			ccprintf("Timestamp: 0x%016llx%s\n", timestamp,
				 (v.flags & MOTIONSENSE_SENSOR_FLAG_FLUSH ?
					  " - Flush" :
					  ""));
		} else {
			ccprintf("%d %d: %-5d %-5d %-5d\n", i, v.sensor_num,
				 v.data[X], v.data[Y], v.data[Z]);
		}
	}
	return EC_SUCCESS;
}

DECLARE_CONSOLE_COMMAND(fiforead, motion_sense_read_fifo, "id",
			"Read Fifo sensor");
#endif /* defined(CONFIG_CMD_ACCEL_FIFO) */