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