cros_ec_sensorhub_ring.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * Driver for Chrome OS EC Sensor hub FIFO.
   4 *
   5 * Copyright 2020 Google LLC
   6 */
   7
   8#include <linux/delay.h>
   9#include <linux/device.h>
  10#include <linux/iio/iio.h>
  11#include <linux/kernel.h>
  12#include <linux/module.h>
  13#include <linux/platform_data/cros_ec_commands.h>
  14#include <linux/platform_data/cros_ec_proto.h>
  15#include <linux/platform_data/cros_ec_sensorhub.h>
  16#include <linux/platform_device.h>
  17#include <linux/sort.h>
  18#include <linux/slab.h>
  19
  20#define CREATE_TRACE_POINTS
  21#include "cros_ec_sensorhub_trace.h"
  22
  23/* Precision of fixed point for the m values from the filter */
  24#define M_PRECISION BIT(23)
  25
  26/* Only activate the filter once we have at least this many elements. */
  27#define TS_HISTORY_THRESHOLD 8
  28
  29/*
  30 * If we don't have any history entries for this long, empty the filter to
  31 * make sure there are no big discontinuities.
  32 */
  33#define TS_HISTORY_BORED_US 500000
  34
  35/* To measure by how much the filter is overshooting, if it happens. */
  36#define FUTURE_TS_ANALYTICS_COUNT_MAX 100
  37
  38static inline int
  39cros_sensorhub_send_sample(struct cros_ec_sensorhub *sensorhub,
  40			   struct cros_ec_sensors_ring_sample *sample)
  41{
  42	cros_ec_sensorhub_push_data_cb_t cb;
  43	int id = sample->sensor_id;
  44	struct iio_dev *indio_dev;
  45
  46	if (id >= sensorhub->sensor_num)
  47		return -EINVAL;
  48
  49	cb = sensorhub->push_data[id].push_data_cb;
  50	if (!cb)
  51		return 0;
  52
  53	indio_dev = sensorhub->push_data[id].indio_dev;
  54
  55	if (sample->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
  56		return 0;
  57
  58	return cb(indio_dev, sample->vector, sample->timestamp);
  59}
  60
  61/**
  62 * cros_ec_sensorhub_register_push_data() - register the callback to the hub.
  63 *
  64 * @sensorhub : Sensor Hub object
  65 * @sensor_num : The sensor the caller is interested in.
  66 * @indio_dev : The iio device to use when a sample arrives.
  67 * @cb : The callback to call when a sample arrives.
  68 *
  69 * The callback cb will be used by cros_ec_sensorhub_ring to distribute events
  70 * from the EC.
  71 *
  72 * Return: 0 when callback is registered.
  73 *         EINVAL is the sensor number is invalid or the slot already used.
  74 */
  75int cros_ec_sensorhub_register_push_data(struct cros_ec_sensorhub *sensorhub,
  76					 u8 sensor_num,
  77					 struct iio_dev *indio_dev,
  78					 cros_ec_sensorhub_push_data_cb_t cb)
  79{
  80	if (sensor_num >= sensorhub->sensor_num)
  81		return -EINVAL;
  82	if (sensorhub->push_data[sensor_num].indio_dev)
  83		return -EINVAL;
  84
  85	sensorhub->push_data[sensor_num].indio_dev = indio_dev;
  86	sensorhub->push_data[sensor_num].push_data_cb = cb;
  87
  88	return 0;
  89}
  90EXPORT_SYMBOL_GPL(cros_ec_sensorhub_register_push_data);
  91
  92void cros_ec_sensorhub_unregister_push_data(struct cros_ec_sensorhub *sensorhub,
  93					    u8 sensor_num)
  94{
  95	sensorhub->push_data[sensor_num].indio_dev = NULL;
  96	sensorhub->push_data[sensor_num].push_data_cb = NULL;
  97}
  98EXPORT_SYMBOL_GPL(cros_ec_sensorhub_unregister_push_data);
  99
 100/**
 101 * cros_ec_sensorhub_ring_fifo_enable() - Enable or disable interrupt generation
 102 *					  for FIFO events.
 103 * @sensorhub: Sensor Hub object
 104 * @on: true when events are requested.
 105 *
 106 * To be called before sleeping or when no one is listening.
 107 * Return: 0 on success, or an error when we can not communicate with the EC.
 108 *
 109 */
 110int cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub *sensorhub,
 111				       bool on)
 112{
 113	int ret, i;
 114
 115	mutex_lock(&sensorhub->cmd_lock);
 116	if (sensorhub->tight_timestamps)
 117		for (i = 0; i < sensorhub->sensor_num; i++)
 118			sensorhub->batch_state[i].last_len = 0;
 119
 120	sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INT_ENABLE;
 121	sensorhub->params->fifo_int_enable.enable = on;
 122
 123	sensorhub->msg->outsize = sizeof(struct ec_params_motion_sense);
 124	sensorhub->msg->insize = sizeof(struct ec_response_motion_sense);
 125
 126	ret = cros_ec_cmd_xfer_status(sensorhub->ec->ec_dev, sensorhub->msg);
 127	mutex_unlock(&sensorhub->cmd_lock);
 128
 129	/* We expect to receive a payload of 4 bytes, ignore. */
 130	if (ret > 0)
 131		ret = 0;
 132	/*
 133	 * Some platforms (such as Smaug) don't support the FIFO_INT_ENABLE
 134	 * command and the interrupt is always enabled.  In the case, it
 135	 * returns -EINVAL.
 136	 *
 137	 * N.B: there is no danger of -EINVAL meaning any other invalid
 138	 * parameter since fifo_int_enable.enable is a bool and can never
 139	 * be in an invalid range.
 140	 */
 141	else if (ret == -EINVAL)
 142		ret = 0;
 143
 144	return ret;
 145}
 146
 147static void cros_ec_sensor_ring_median_swap(s64 *a, s64 *b)
 148{
 149	s64 tmp = *a;
 150	*a = *b;
 151	*b = tmp;
 152}
 153
 154/*
 155 * cros_ec_sensor_ring_median: Gets median of an array of numbers
 156 *
 157 * It's implemented using the quickselect algorithm, which achieves an
 158 * average time complexity of O(n) the middle element. In the worst case,
 159 * the runtime of quickselect could regress to O(n^2). To mitigate this,
 160 * algorithms like median-of-medians exist, which can guarantee O(n) even
 161 * in the worst case. However, these algorithms come with a higher
 162 * overhead and are more complex to implement, making quickselect a
 163 * pragmatic choice for our use case.
 164 *
 165 * Warning: the input array gets modified!
 166 */
 167static s64 cros_ec_sensor_ring_median(s64 *array, size_t length)
 168{
 169	int lo = 0;
 170	int hi = length - 1;
 171
 172	while (lo <= hi) {
 173		int mid = lo + (hi - lo) / 2;
 174		int pivot, i;
 175
 176		if (array[lo] > array[mid])
 177			cros_ec_sensor_ring_median_swap(&array[lo], &array[mid]);
 178		if (array[lo] > array[hi])
 179			cros_ec_sensor_ring_median_swap(&array[lo], &array[hi]);
 180		if (array[mid] < array[hi])
 181			cros_ec_sensor_ring_median_swap(&array[mid], &array[hi]);
 182
 183		pivot = array[hi];
 184		i = lo - 1;
 185
 186		for (int j = lo; j < hi; j++)
 187			if (array[j] < pivot)
 188				cros_ec_sensor_ring_median_swap(&array[++i], &array[j]);
 189
 190		/* The pivot's index corresponds to i+1. */
 191		cros_ec_sensor_ring_median_swap(&array[i + 1], &array[hi]);
 192		if (i + 1 == length / 2)
 193			return array[i + 1];
 194		if (i + 1 > length / 2)
 195			hi = i;
 196		else
 197			lo = i + 2;
 198	}
 199
 200	/* Should never reach here. */
 201	return -1;
 202}
 203
 204/*
 205 * IRQ Timestamp Filtering
 206 *
 207 * Lower down in cros_ec_sensor_ring_process_event(), for each sensor event
 208 * we have to calculate it's timestamp in the AP timebase. There are 3 time
 209 * points:
 210 *   a - EC timebase, sensor event
 211 *   b - EC timebase, IRQ
 212 *   c - AP timebase, IRQ
 213 *   a' - what we want: sensor even in AP timebase
 214 *
 215 * While a and b are recorded at accurate times (due to the EC real time
 216 * nature); c is pretty untrustworthy, even though it's recorded the
 217 * first thing in ec_irq_handler(). There is a very good chance we'll get
 218 * added latency due to:
 219 *   other irqs
 220 *   ddrfreq
 221 *   cpuidle
 222 *
 223 * Normally a' = c - b + a, but if we do that naive math any jitter in c
 224 * will get coupled in a', which we don't want. We want a function
 225 * a' = cros_ec_sensor_ring_ts_filter(a) which will filter out outliers in c.
 226 *
 227 * Think of a graph of AP time(b) on the y axis vs EC time(c) on the x axis.
 228 * The slope of the line won't be exactly 1, there will be some clock drift
 229 * between the 2 chips for various reasons (mechanical stress, temperature,
 230 * voltage). We need to extrapolate values for a future x, without trusting
 231 * recent y values too much.
 232 *
 233 * We use a median filter for the slope, then another median filter for the
 234 * y-intercept to calculate this function:
 235 *   dx[n] = x[n-1] - x[n]
 236 *   dy[n] = x[n-1] - x[n]
 237 *   m[n] = dy[n] / dx[n]
 238 *   median_m = median(m[n-k:n])
 239 *   error[i] = y[n-i] - median_m * x[n-i]
 240 *   median_error = median(error[:k])
 241 *   predicted_y = median_m * x + median_error
 242 *
 243 * Implementation differences from above:
 244 * - Redefined y to be actually c - b, this gives us a lot more precision
 245 * to do the math. (c-b)/b variations are more obvious than c/b variations.
 246 * - Since we don't have floating point, any operations involving slope are
 247 * done using fixed point math (*M_PRECISION)
 248 * - Since x and y grow with time, we keep zeroing the graph (relative to
 249 * the last sample), this way math involving *x[n-i] will not overflow
 250 * - EC timestamps are kept in us, it improves the slope calculation precision
 251 */
 252
 253/**
 254 * cros_ec_sensor_ring_ts_filter_update() - Update filter history.
 255 *
 256 * @state: Filter information.
 257 * @b: IRQ timestamp, EC timebase (us)
 258 * @c: IRQ timestamp, AP timebase (ns)
 259 *
 260 * Given a new IRQ timestamp pair (EC and AP timebases), add it to the filter
 261 * history.
 262 */
 263static void
 264cros_ec_sensor_ring_ts_filter_update(struct cros_ec_sensors_ts_filter_state
 265				     *state,
 266				     s64 b, s64 c)
 267{
 268	s64 x, y;
 269	s64 dx, dy;
 270	s64 m; /* stored as *M_PRECISION */
 271	s64 *m_history_copy = state->temp_buf;
 272	s64 *error = state->temp_buf;
 273	int i;
 274
 275	/* we trust b the most, that'll be our independent variable */
 276	x = b;
 277	/* y is the offset between AP and EC times, in ns */
 278	y = c - b * 1000;
 279
 280	dx = (state->x_history[0] + state->x_offset) - x;
 281	if (dx == 0)
 282		return; /* we already have this irq in the history */
 283	dy = (state->y_history[0] + state->y_offset) - y;
 284	m = div64_s64(dy * M_PRECISION, dx);
 285
 286	/* Empty filter if we haven't seen any action in a while. */
 287	if (-dx > TS_HISTORY_BORED_US)
 288		state->history_len = 0;
 289
 290	/* Move everything over, also update offset to all absolute coords .*/
 291	for (i = state->history_len - 1; i >= 1; i--) {
 292		state->x_history[i] = state->x_history[i - 1] + dx;
 293		state->y_history[i] = state->y_history[i - 1] + dy;
 294
 295		state->m_history[i] = state->m_history[i - 1];
 296		/*
 297		 * Also use the same loop to copy m_history for future
 298		 * median extraction.
 299		 */
 300		m_history_copy[i] = state->m_history[i - 1];
 301	}
 302
 303	/* Store the x and y, but remember offset is actually last sample. */
 304	state->x_offset = x;
 305	state->y_offset = y;
 306	state->x_history[0] = 0;
 307	state->y_history[0] = 0;
 308
 309	state->m_history[0] = m;
 310	m_history_copy[0] = m;
 311
 312	if (state->history_len < CROS_EC_SENSORHUB_TS_HISTORY_SIZE)
 313		state->history_len++;
 314
 315	/* Precalculate things for the filter. */
 316	if (state->history_len > TS_HISTORY_THRESHOLD) {
 317		state->median_m =
 318		    cros_ec_sensor_ring_median(m_history_copy,
 319					       state->history_len - 1);
 320
 321		/*
 322		 * Calculate y-intercepts as if m_median is the slope and
 323		 * points in the history are on the line. median_error will
 324		 * still be in the offset coordinate system.
 325		 */
 326		for (i = 0; i < state->history_len; i++)
 327			error[i] = state->y_history[i] -
 328				div_s64(state->median_m * state->x_history[i],
 329					M_PRECISION);
 330		state->median_error =
 331			cros_ec_sensor_ring_median(error, state->history_len);
 332	} else {
 333		state->median_m = 0;
 334		state->median_error = 0;
 335	}
 336	trace_cros_ec_sensorhub_filter(state, dx, dy);
 337}
 338
 339/**
 340 * cros_ec_sensor_ring_ts_filter() - Translate EC timebase timestamp to AP
 341 *                                   timebase
 342 *
 343 * @state: filter information.
 344 * @x: any ec timestamp (us):
 345 *
 346 * cros_ec_sensor_ring_ts_filter(a) => a' event timestamp, AP timebase
 347 * cros_ec_sensor_ring_ts_filter(b) => calculated timestamp when the EC IRQ
 348 *                           should have happened on the AP, with low jitter
 349 *
 350 * Note: The filter will only activate once state->history_len goes
 351 * over TS_HISTORY_THRESHOLD. Otherwise it'll just do the naive c - b + a
 352 * transform.
 353 *
 354 * How to derive the formula, starting from:
 355 *   f(x) = median_m * x + median_error
 356 * That's the calculated AP - EC offset (at the x point in time)
 357 * Undo the coordinate system transform:
 358 *   f(x) = median_m * (x - x_offset) + median_error + y_offset
 359 * Remember to undo the "y = c - b * 1000" modification:
 360 *   f(x) = median_m * (x - x_offset) + median_error + y_offset + x * 1000
 361 *
 362 * Return: timestamp in AP timebase (ns)
 363 */
 364static s64
 365cros_ec_sensor_ring_ts_filter(struct cros_ec_sensors_ts_filter_state *state,
 366			      s64 x)
 367{
 368	return div_s64(state->median_m * (x - state->x_offset), M_PRECISION)
 369	       + state->median_error + state->y_offset + x * 1000;
 370}
 371
 372/*
 373 * Since a and b were originally 32 bit values from the EC,
 374 * they overflow relatively often, casting is not enough, so we need to
 375 * add an offset.
 376 */
 377static void
 378cros_ec_sensor_ring_fix_overflow(s64 *ts,
 379				 const s64 overflow_period,
 380				 struct cros_ec_sensors_ec_overflow_state
 381				 *state)
 382{
 383	s64 adjust;
 384
 385	*ts += state->offset;
 386	if (abs(state->last - *ts) > (overflow_period / 2)) {
 387		adjust = state->last > *ts ? overflow_period : -overflow_period;
 388		state->offset += adjust;
 389		*ts += adjust;
 390	}
 391	state->last = *ts;
 392}
 393
 394static void
 395cros_ec_sensor_ring_check_for_past_timestamp(struct cros_ec_sensorhub
 396					     *sensorhub,
 397					     struct cros_ec_sensors_ring_sample
 398					     *sample)
 399{
 400	const u8 sensor_id = sample->sensor_id;
 401
 402	/* If this event is earlier than one we saw before... */
 403	if (sensorhub->batch_state[sensor_id].newest_sensor_event >
 404	    sample->timestamp)
 405		/* mark it for spreading. */
 406		sample->timestamp =
 407			sensorhub->batch_state[sensor_id].last_ts;
 408	else
 409		sensorhub->batch_state[sensor_id].newest_sensor_event =
 410			sample->timestamp;
 411}
 412
 413/**
 414 * cros_ec_sensor_ring_process_event() - Process one EC FIFO event
 415 *
 416 * @sensorhub: Sensor Hub object.
 417 * @fifo_info: FIFO information from the EC (includes b point, EC timebase).
 418 * @fifo_timestamp: EC IRQ, kernel timebase (aka c).
 419 * @current_timestamp: calculated event timestamp, kernel timebase (aka a').
 420 * @in: incoming FIFO event from EC (includes a point, EC timebase).
 421 * @out: outgoing event to user space (includes a').
 422 *
 423 * Process one EC event, add it in the ring if necessary.
 424 *
 425 * Return: true if out event has been populated.
 426 */
 427static bool
 428cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub,
 429				const struct ec_response_motion_sense_fifo_info
 430				*fifo_info,
 431				const ktime_t fifo_timestamp,
 432				ktime_t *current_timestamp,
 433				struct ec_response_motion_sensor_data *in,
 434				struct cros_ec_sensors_ring_sample *out)
 435{
 436	const s64 now = cros_ec_get_time_ns();
 437	int axis, async_flags;
 438
 439	/* Do not populate the filter based on asynchronous events. */
 440	async_flags = in->flags &
 441		(MOTIONSENSE_SENSOR_FLAG_ODR | MOTIONSENSE_SENSOR_FLAG_FLUSH);
 442
 443	if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP && !async_flags) {
 444		s64 a = in->timestamp;
 445		s64 b = fifo_info->timestamp;
 446		s64 c = fifo_timestamp;
 447
 448		cros_ec_sensor_ring_fix_overflow(&a, 1LL << 32,
 449					  &sensorhub->overflow_a);
 450		cros_ec_sensor_ring_fix_overflow(&b, 1LL << 32,
 451					  &sensorhub->overflow_b);
 452
 453		if (sensorhub->tight_timestamps) {
 454			cros_ec_sensor_ring_ts_filter_update(
 455					&sensorhub->filter, b, c);
 456			*current_timestamp = cros_ec_sensor_ring_ts_filter(
 457					&sensorhub->filter, a);
 458		} else {
 459			s64 new_timestamp;
 460
 461			/*
 462			 * Disable filtering since we might add more jitter
 463			 * if b is in a random point in time.
 464			 */
 465			new_timestamp = c - b * 1000 + a * 1000;
 466			/*
 467			 * The timestamp can be stale if we had to use the fifo
 468			 * info timestamp.
 469			 */
 470			if (new_timestamp - *current_timestamp > 0)
 471				*current_timestamp = new_timestamp;
 472		}
 473		trace_cros_ec_sensorhub_timestamp(in->timestamp,
 474						  fifo_info->timestamp,
 475						  fifo_timestamp,
 476						  *current_timestamp,
 477						  now);
 478	}
 479
 480	if (in->flags & MOTIONSENSE_SENSOR_FLAG_ODR) {
 481		if (sensorhub->tight_timestamps) {
 482			sensorhub->batch_state[in->sensor_num].last_len = 0;
 483			sensorhub->batch_state[in->sensor_num].penul_len = 0;
 484		}
 485		/*
 486		 * ODR change is only useful for the sensor_ring, it does not
 487		 * convey information to clients.
 488		 */
 489		return false;
 490	}
 491
 492	if (in->flags & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
 493		out->sensor_id = in->sensor_num;
 494		out->timestamp = *current_timestamp;
 495		out->flag = in->flags;
 496		if (sensorhub->tight_timestamps)
 497			sensorhub->batch_state[out->sensor_id].last_len = 0;
 498		/*
 499		 * No other payload information provided with
 500		 * flush ack.
 501		 */
 502		return true;
 503	}
 504
 505	if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP)
 506		/* If we just have a timestamp, skip this entry. */
 507		return false;
 508
 509	/* Regular sample */
 510	out->sensor_id = in->sensor_num;
 511	trace_cros_ec_sensorhub_data(in->sensor_num,
 512				     fifo_info->timestamp,
 513				     fifo_timestamp,
 514				     *current_timestamp,
 515				     now);
 516
 517	if (*current_timestamp - now > 0) {
 518		/*
 519		 * This fix is needed to overcome the timestamp filter putting
 520		 * events in the future.
 521		 */
 522		sensorhub->future_timestamp_total_ns +=
 523			*current_timestamp - now;
 524		if (++sensorhub->future_timestamp_count ==
 525				FUTURE_TS_ANALYTICS_COUNT_MAX) {
 526			s64 avg = div_s64(sensorhub->future_timestamp_total_ns,
 527					sensorhub->future_timestamp_count);
 528			dev_warn_ratelimited(sensorhub->dev,
 529					     "100 timestamps in the future, %lldns shaved on average\n",
 530					     avg);
 531			sensorhub->future_timestamp_count = 0;
 532			sensorhub->future_timestamp_total_ns = 0;
 533		}
 534		out->timestamp = now;
 535	} else {
 536		out->timestamp = *current_timestamp;
 537	}
 538
 539	out->flag = in->flags;
 540	for (axis = 0; axis < 3; axis++)
 541		out->vector[axis] = in->data[axis];
 542
 543	if (sensorhub->tight_timestamps)
 544		cros_ec_sensor_ring_check_for_past_timestamp(sensorhub, out);
 545	return true;
 546}
 547
 548/*
 549 * cros_ec_sensor_ring_spread_add: Calculate proper timestamps then add to
 550 *                                 ringbuffer.
 551 *
 552 * This is the new spreading code, assumes every sample's timestamp
 553 * precedes the sample. Run if tight_timestamps == true.
 554 *
 555 * Sometimes the EC receives only one interrupt (hence timestamp) for
 556 * a batch of samples. Only the first sample will have the correct
 557 * timestamp. So we must interpolate the other samples.
 558 * We use the previous batch timestamp and our current batch timestamp
 559 * as a way to calculate period, then spread the samples evenly.
 560 *
 561 * s0 int, 0ms
 562 * s1 int, 10ms
 563 * s2 int, 20ms
 564 * 30ms point goes by, no interrupt, previous one is still asserted
 565 * downloading s2 and s3
 566 * s3 sample, 20ms (incorrect timestamp)
 567 * s4 int, 40ms
 568 *
 569 * The batches are [(s0), (s1), (s2, s3), (s4)]. Since the 3rd batch
 570 * has 2 samples in them, we adjust the timestamp of s3.
 571 * s2 - s1 = 10ms, so s3 must be s2 + 10ms => 20ms. If s1 would have
 572 * been part of a bigger batch things would have gotten a little
 573 * more complicated.
 574 *
 575 * Note: we also assume another sensor sample doesn't break up a batch
 576 * in 2 or more partitions. Example, there can't ever be a sync sensor
 577 * in between S2 and S3. This simplifies the following code.
 578 */
 579static void
 580cros_ec_sensor_ring_spread_add(struct cros_ec_sensorhub *sensorhub,
 581			       unsigned long sensor_mask,
 582			       struct cros_ec_sensors_ring_sample *last_out)
 583{
 584	struct cros_ec_sensors_ring_sample *batch_start, *next_batch_start;
 585	int id;
 586
 587	for_each_set_bit(id, &sensor_mask, sensorhub->sensor_num) {
 588		for (batch_start = sensorhub->ring; batch_start < last_out;
 589		     batch_start = next_batch_start) {
 590			/*
 591			 * For each batch (where all samples have the same
 592			 * timestamp).
 593			 */
 594			int batch_len, sample_idx;
 595			struct cros_ec_sensors_ring_sample *batch_end =
 596				batch_start;
 597			struct cros_ec_sensors_ring_sample *s;
 598			s64 batch_timestamp = batch_start->timestamp;
 599			s64 sample_period;
 600
 601			/*
 602			 * Skip over batches that start with the sensor types
 603			 * we're not looking at right now.
 604			 */
 605			if (batch_start->sensor_id != id) {
 606				next_batch_start = batch_start + 1;
 607				continue;
 608			}
 609
 610			/*
 611			 * Do not start a batch
 612			 * from a flush, as it happens asynchronously to the
 613			 * regular flow of events.
 614			 */
 615			if (batch_start->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
 616				cros_sensorhub_send_sample(sensorhub,
 617							   batch_start);
 618				next_batch_start = batch_start + 1;
 619				continue;
 620			}
 621
 622			if (batch_start->timestamp <=
 623			    sensorhub->batch_state[id].last_ts) {
 624				batch_timestamp =
 625					sensorhub->batch_state[id].last_ts;
 626				batch_len = sensorhub->batch_state[id].last_len;
 627
 628				sample_idx = batch_len;
 629
 630				sensorhub->batch_state[id].last_ts =
 631				  sensorhub->batch_state[id].penul_ts;
 632				sensorhub->batch_state[id].last_len =
 633				  sensorhub->batch_state[id].penul_len;
 634			} else {
 635				/*
 636				 * Push first sample in the batch to the,
 637				 * kfifo, it's guaranteed to be correct, the
 638				 * rest will follow later on.
 639				 */
 640				sample_idx = 1;
 641				batch_len = 1;
 642				cros_sensorhub_send_sample(sensorhub,
 643							   batch_start);
 644				batch_start++;
 645			}
 646
 647			/* Find all samples have the same timestamp. */
 648			for (s = batch_start; s < last_out; s++) {
 649				if (s->sensor_id != id)
 650					/*
 651					 * Skip over other sensor types that
 652					 * are interleaved, don't count them.
 653					 */
 654					continue;
 655				if (s->timestamp != batch_timestamp)
 656					/* we discovered the next batch */
 657					break;
 658				if (s->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
 659					/* break on flush packets */
 660					break;
 661				batch_end = s;
 662				batch_len++;
 663			}
 664
 665			if (batch_len == 1)
 666				goto done_with_this_batch;
 667
 668			/* Can we calculate period? */
 669			if (sensorhub->batch_state[id].last_len == 0) {
 670				dev_warn(sensorhub->dev, "Sensor %d: lost %d samples when spreading\n",
 671					 id, batch_len - 1);
 672				goto done_with_this_batch;
 673				/*
 674				 * Note: we're dropping the rest of the samples
 675				 * in this batch since we have no idea where
 676				 * they're supposed to go without a period
 677				 * calculation.
 678				 */
 679			}
 680
 681			sample_period = div_s64(batch_timestamp -
 682				sensorhub->batch_state[id].last_ts,
 683				sensorhub->batch_state[id].last_len);
 684			dev_dbg(sensorhub->dev,
 685				"Adjusting %d samples, sensor %d last_batch @%lld (%d samples) batch_timestamp=%lld => period=%lld\n",
 686				batch_len, id,
 687				sensorhub->batch_state[id].last_ts,
 688				sensorhub->batch_state[id].last_len,
 689				batch_timestamp,
 690				sample_period);
 691
 692			/*
 693			 * Adjust timestamps of the samples then push them to
 694			 * kfifo.
 695			 */
 696			for (s = batch_start; s <= batch_end; s++) {
 697				if (s->sensor_id != id)
 698					/*
 699					 * Skip over other sensor types that
 700					 * are interleaved, don't change them.
 701					 */
 702					continue;
 703
 704				s->timestamp = batch_timestamp +
 705					sample_period * sample_idx;
 706				sample_idx++;
 707
 708				cros_sensorhub_send_sample(sensorhub, s);
 709			}
 710
 711done_with_this_batch:
 712			sensorhub->batch_state[id].penul_ts =
 713				sensorhub->batch_state[id].last_ts;
 714			sensorhub->batch_state[id].penul_len =
 715				sensorhub->batch_state[id].last_len;
 716
 717			sensorhub->batch_state[id].last_ts =
 718				batch_timestamp;
 719			sensorhub->batch_state[id].last_len = batch_len;
 720
 721			next_batch_start = batch_end + 1;
 722		}
 723	}
 724}
 725
 726/*
 727 * cros_ec_sensor_ring_spread_add_legacy: Calculate proper timestamps then
 728 * add to ringbuffer (legacy).
 729 *
 730 * Note: This assumes we're running old firmware, where timestamp
 731 * is inserted after its sample(s)e. There can be several samples between
 732 * timestamps, so several samples can have the same timestamp.
 733 *
 734 *                        timestamp | count
 735 *                        -----------------
 736 *          1st sample --> TS1      | 1
 737 *                         TS2      | 2
 738 *                         TS2      | 3
 739 *                         TS3      | 4
 740 *           last_out -->
 741 *
 742 *
 743 * We spread time for the samples using period p = (current - TS1)/4.
 744 * between TS1 and TS2: [TS1+p/4, TS1+2p/4, TS1+3p/4, current_timestamp].
 745 *
 746 */
 747static void
 748cros_ec_sensor_ring_spread_add_legacy(struct cros_ec_sensorhub *sensorhub,
 749				      unsigned long sensor_mask,
 750				      s64 current_timestamp,
 751				      struct cros_ec_sensors_ring_sample
 752				      *last_out)
 753{
 754	struct cros_ec_sensors_ring_sample *out;
 755	int i;
 756
 757	for_each_set_bit(i, &sensor_mask, sensorhub->sensor_num) {
 758		s64 timestamp;
 759		int count = 0;
 760		s64 time_period;
 761
 762		for (out = sensorhub->ring; out < last_out; out++) {
 763			if (out->sensor_id != i)
 764				continue;
 765
 766			/* Timestamp to start with */
 767			timestamp = out->timestamp;
 768			out++;
 769			count = 1;
 770			break;
 771		}
 772		for (; out < last_out; out++) {
 773			/* Find last sample. */
 774			if (out->sensor_id != i)
 775				continue;
 776			count++;
 777		}
 778		if (count == 0)
 779			continue;
 780
 781		/* Spread uniformly between the first and last samples. */
 782		time_period = div_s64(current_timestamp - timestamp, count);
 783
 784		for (out = sensorhub->ring; out < last_out; out++) {
 785			if (out->sensor_id != i)
 786				continue;
 787			timestamp += time_period;
 788			out->timestamp = timestamp;
 789		}
 790	}
 791
 792	/* Push the event into the kfifo */
 793	for (out = sensorhub->ring; out < last_out; out++)
 794		cros_sensorhub_send_sample(sensorhub, out);
 795}
 796
 797/**
 798 * cros_ec_sensorhub_ring_handler() - The trigger handler function
 799 *
 800 * @sensorhub: Sensor Hub object.
 801 *
 802 * Called by the notifier, process the EC sensor FIFO queue.
 803 */
 804static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub)
 805{
 806	struct ec_response_motion_sense_fifo_info *fifo_info =
 807		sensorhub->fifo_info;
 808	struct cros_ec_dev *ec = sensorhub->ec;
 809	ktime_t fifo_timestamp, current_timestamp;
 810	int i, j, number_data, ret;
 811	unsigned long sensor_mask = 0;
 812	struct ec_response_motion_sensor_data *in;
 813	struct cros_ec_sensors_ring_sample *out, *last_out;
 814
 815	mutex_lock(&sensorhub->cmd_lock);
 816
 817	/* Get FIFO information if there are lost vectors. */
 818	if (fifo_info->total_lost) {
 819		int fifo_info_length =
 820			sizeof(struct ec_response_motion_sense_fifo_info) +
 821			sizeof(u16) * sensorhub->sensor_num;
 822
 823		/* Need to retrieve the number of lost vectors per sensor */
 824		sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
 825		sensorhub->msg->outsize = 1;
 826		sensorhub->msg->insize = fifo_info_length;
 827
 828		if (cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg) < 0)
 829			goto error;
 830
 831		memcpy(fifo_info, &sensorhub->resp->fifo_info,
 832		       fifo_info_length);
 833
 834		/*
 835		 * Update collection time, will not be as precise as the
 836		 * non-error case.
 837		 */
 838		fifo_timestamp = cros_ec_get_time_ns();
 839	} else {
 840		fifo_timestamp = sensorhub->fifo_timestamp[
 841			CROS_EC_SENSOR_NEW_TS];
 842	}
 843
 844	if (fifo_info->count > sensorhub->fifo_size ||
 845	    fifo_info->size != sensorhub->fifo_size) {
 846		dev_warn(sensorhub->dev,
 847			 "Mismatch EC data: count %d, size %d - expected %d\n",
 848			 fifo_info->count, fifo_info->size,
 849			 sensorhub->fifo_size);
 850		goto error;
 851	}
 852
 853	/* Copy elements in the main fifo */
 854	current_timestamp = sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS];
 855	out = sensorhub->ring;
 856	for (i = 0; i < fifo_info->count; i += number_data) {
 857		sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_READ;
 858		sensorhub->params->fifo_read.max_data_vector =
 859			fifo_info->count - i;
 860		sensorhub->msg->outsize =
 861			sizeof(struct ec_params_motion_sense);
 862		sensorhub->msg->insize =
 863			sizeof(sensorhub->resp->fifo_read) +
 864			sensorhub->params->fifo_read.max_data_vector *
 865			  sizeof(struct ec_response_motion_sensor_data);
 866		ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
 867		if (ret < 0) {
 868			dev_warn(sensorhub->dev, "Fifo error: %d\n", ret);
 869			break;
 870		}
 871		number_data = sensorhub->resp->fifo_read.number_data;
 872		if (number_data == 0) {
 873			dev_dbg(sensorhub->dev, "Unexpected empty FIFO\n");
 874			break;
 875		}
 876		if (number_data > fifo_info->count - i) {
 877			dev_warn(sensorhub->dev,
 878				 "Invalid EC data: too many entry received: %d, expected %d\n",
 879				 number_data, fifo_info->count - i);
 880			break;
 881		}
 882		if (out + number_data >
 883		    sensorhub->ring + fifo_info->count) {
 884			dev_warn(sensorhub->dev,
 885				 "Too many samples: %d (%zd data) to %d entries for expected %d entries\n",
 886				 i, out - sensorhub->ring, i + number_data,
 887				 fifo_info->count);
 888			break;
 889		}
 890
 891		for (in = sensorhub->resp->fifo_read.data, j = 0;
 892		     j < number_data; j++, in++) {
 893			if (cros_ec_sensor_ring_process_event(
 894						sensorhub, fifo_info,
 895						fifo_timestamp,
 896						&current_timestamp,
 897						in, out)) {
 898				sensor_mask |= BIT(in->sensor_num);
 899				out++;
 900			}
 901		}
 902	}
 903	mutex_unlock(&sensorhub->cmd_lock);
 904	last_out = out;
 905
 906	if (out == sensorhub->ring)
 907		/* Unexpected empty FIFO. */
 908		goto ring_handler_end;
 909
 910	/*
 911	 * Check if current_timestamp is ahead of the last sample. Normally,
 912	 * the EC appends a timestamp after the last sample, but if the AP
 913	 * is slow to respond to the IRQ, the EC may have added new samples.
 914	 * Use the FIFO info timestamp as last timestamp then.
 915	 */
 916	if (!sensorhub->tight_timestamps &&
 917	    (last_out - 1)->timestamp == current_timestamp)
 918		current_timestamp = fifo_timestamp;
 919
 920	/* Warn on lost samples. */
 921	if (fifo_info->total_lost)
 922		for (i = 0; i < sensorhub->sensor_num; i++) {
 923			if (fifo_info->lost[i]) {
 924				dev_warn_ratelimited(sensorhub->dev,
 925						     "Sensor %d: lost: %d out of %d\n",
 926						     i, fifo_info->lost[i],
 927						     fifo_info->total_lost);
 928				if (sensorhub->tight_timestamps)
 929					sensorhub->batch_state[i].last_len = 0;
 930			}
 931		}
 932
 933	/*
 934	 * Spread samples in case of batching, then add them to the
 935	 * ringbuffer.
 936	 */
 937	if (sensorhub->tight_timestamps)
 938		cros_ec_sensor_ring_spread_add(sensorhub, sensor_mask,
 939					       last_out);
 940	else
 941		cros_ec_sensor_ring_spread_add_legacy(sensorhub, sensor_mask,
 942						      current_timestamp,
 943						      last_out);
 944
 945ring_handler_end:
 946	sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] = current_timestamp;
 947	return;
 948
 949error:
 950	mutex_unlock(&sensorhub->cmd_lock);
 951}
 952
 953static int cros_ec_sensorhub_event(struct notifier_block *nb,
 954				   unsigned long queued_during_suspend,
 955				   void *_notify)
 956{
 957	struct cros_ec_sensorhub *sensorhub;
 958	struct cros_ec_device *ec_dev;
 959
 960	sensorhub = container_of(nb, struct cros_ec_sensorhub, notifier);
 961	ec_dev = sensorhub->ec->ec_dev;
 962
 963	if (ec_dev->event_data.event_type != EC_MKBP_EVENT_SENSOR_FIFO)
 964		return NOTIFY_DONE;
 965
 966	if (ec_dev->event_size != sizeof(ec_dev->event_data.data.sensor_fifo)) {
 967		dev_warn(ec_dev->dev, "Invalid fifo info size\n");
 968		return NOTIFY_DONE;
 969	}
 970
 971	if (queued_during_suspend)
 972		return NOTIFY_OK;
 973
 974	memcpy(sensorhub->fifo_info, &ec_dev->event_data.data.sensor_fifo.info,
 975	       sizeof(*sensorhub->fifo_info));
 976	sensorhub->fifo_timestamp[CROS_EC_SENSOR_NEW_TS] =
 977		ec_dev->last_event_time;
 978	cros_ec_sensorhub_ring_handler(sensorhub);
 979
 980	return NOTIFY_OK;
 981}
 982
 983/**
 984 * cros_ec_sensorhub_ring_allocate() - Prepare the FIFO functionality if the EC
 985 *				       supports it.
 986 *
 987 * @sensorhub : Sensor Hub object.
 988 *
 989 * Return: 0 on success.
 990 */
 991int cros_ec_sensorhub_ring_allocate(struct cros_ec_sensorhub *sensorhub)
 992{
 993	int fifo_info_length =
 994		sizeof(struct ec_response_motion_sense_fifo_info) +
 995		sizeof(u16) * sensorhub->sensor_num;
 996
 997	/* Allocate the array for lost events. */
 998	sensorhub->fifo_info = devm_kzalloc(sensorhub->dev, fifo_info_length,
 999					    GFP_KERNEL);
1000	if (!sensorhub->fifo_info)
1001		return -ENOMEM;
1002
1003	/*
1004	 * Allocate the callback area based on the number of sensors.
1005	 * Add one for the sensor ring.
1006	 */
1007	sensorhub->push_data = devm_kcalloc(sensorhub->dev,
1008			sensorhub->sensor_num,
1009			sizeof(*sensorhub->push_data),
1010			GFP_KERNEL);
1011	if (!sensorhub->push_data)
1012		return -ENOMEM;
1013
1014	sensorhub->tight_timestamps = cros_ec_check_features(
1015			sensorhub->ec,
1016			EC_FEATURE_MOTION_SENSE_TIGHT_TIMESTAMPS);
1017
1018	if (sensorhub->tight_timestamps) {
1019		sensorhub->batch_state = devm_kcalloc(sensorhub->dev,
1020				sensorhub->sensor_num,
1021				sizeof(*sensorhub->batch_state),
1022				GFP_KERNEL);
1023		if (!sensorhub->batch_state)
1024			return -ENOMEM;
1025	}
1026
1027	return 0;
1028}
1029
1030/**
1031 * cros_ec_sensorhub_ring_add() - Add the FIFO functionality if the EC
1032 *				  supports it.
1033 *
1034 * @sensorhub : Sensor Hub object.
1035 *
1036 * Return: 0 on success.
1037 */
1038int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
1039{
1040	struct cros_ec_dev *ec = sensorhub->ec;
1041	int ret;
1042	int fifo_info_length =
1043		sizeof(struct ec_response_motion_sense_fifo_info) +
1044		sizeof(u16) * sensorhub->sensor_num;
1045
1046	/* Retrieve FIFO information */
1047	sensorhub->msg->version = 2;
1048	sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
1049	sensorhub->msg->outsize = 1;
1050	sensorhub->msg->insize = fifo_info_length;
1051
1052	ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
1053	if (ret < 0)
1054		return ret;
1055
1056	/*
1057	 * Allocate the full fifo. We need to copy the whole FIFO to set
1058	 * timestamps properly.
1059	 */
1060	sensorhub->fifo_size = sensorhub->resp->fifo_info.size;
1061	sensorhub->ring = devm_kcalloc(sensorhub->dev, sensorhub->fifo_size,
1062				       sizeof(*sensorhub->ring), GFP_KERNEL);
1063	if (!sensorhub->ring)
1064		return -ENOMEM;
1065
1066	sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] =
1067		cros_ec_get_time_ns();
1068
1069	/* Register the notifier that will act as a top half interrupt. */
1070	sensorhub->notifier.notifier_call = cros_ec_sensorhub_event;
1071	ret = blocking_notifier_chain_register(&ec->ec_dev->event_notifier,
1072					       &sensorhub->notifier);
1073	if (ret < 0)
1074		return ret;
1075
1076	/* Start collection samples. */
1077	return cros_ec_sensorhub_ring_fifo_enable(sensorhub, true);
1078}
1079
1080void cros_ec_sensorhub_ring_remove(void *arg)
1081{
1082	struct cros_ec_sensorhub *sensorhub = arg;
1083	struct cros_ec_device *ec_dev = sensorhub->ec->ec_dev;
1084
1085	/* Disable the ring, prevent EC interrupt to the AP for nothing. */
1086	cros_ec_sensorhub_ring_fifo_enable(sensorhub, false);
1087	blocking_notifier_chain_unregister(&ec_dev->event_notifier,
1088					   &sensorhub->notifier);
1089}