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 
38 static inline int
cros_sensorhub_send_sample(struct cros_ec_sensorhub * sensorhub,struct cros_ec_sensors_ring_sample * sample)39 cros_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  */
cros_ec_sensorhub_register_push_data(struct cros_ec_sensorhub * sensorhub,u8 sensor_num,struct iio_dev * indio_dev,cros_ec_sensorhub_push_data_cb_t cb)75 int 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 }
90 EXPORT_SYMBOL_GPL(cros_ec_sensorhub_register_push_data);
91 
cros_ec_sensorhub_unregister_push_data(struct cros_ec_sensorhub * sensorhub,u8 sensor_num)92 void 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 }
98 EXPORT_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  */
cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub * sensorhub,bool on)110 int 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 
cros_ec_sensor_ring_median_cmp(const void * pv1,const void * pv2)136 static 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  */
cros_ec_sensor_ring_median(s64 * array,size_t length)159 static 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  */
224 static void
cros_ec_sensor_ring_ts_filter_update(struct cros_ec_sensors_ts_filter_state * state,s64 b,s64 c)225 cros_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  */
325 static s64
cros_ec_sensor_ring_ts_filter(struct cros_ec_sensors_ts_filter_state * state,s64 x)326 cros_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  */
338 static void
cros_ec_sensor_ring_fix_overflow(s64 * ts,const s64 overflow_period,struct cros_ec_sensors_ec_overflow_state * state)339 cros_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 
355 static void
cros_ec_sensor_ring_check_for_past_timestamp(struct cros_ec_sensorhub * sensorhub,struct cros_ec_sensors_ring_sample * sample)356 cros_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  */
388 static bool
cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub * sensorhub,const struct ec_response_motion_sense_fifo_info * fifo_info,const ktime_t fifo_timestamp,ktime_t * current_timestamp,struct ec_response_motion_sensor_data * in,struct cros_ec_sensors_ring_sample * out)389 cros_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  */
540 static void
cros_ec_sensor_ring_spread_add(struct cros_ec_sensorhub * sensorhub,unsigned long sensor_mask,struct cros_ec_sensors_ring_sample * last_out)541 cros_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 
672 done_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  */
708 static void
cros_ec_sensor_ring_spread_add_legacy(struct cros_ec_sensorhub * sensorhub,unsigned long sensor_mask,s64 current_timestamp,struct cros_ec_sensors_ring_sample * last_out)709 cros_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  */
cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub * sensorhub)765 static 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 
906 ring_handler_end:
907 	sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] = current_timestamp;
908 	return;
909 
910 error:
911 	mutex_unlock(&sensorhub->cmd_lock);
912 }
913 
cros_ec_sensorhub_event(struct notifier_block * nb,unsigned long queued_during_suspend,void * _notify)914 static 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  */
cros_ec_sensorhub_ring_allocate(struct cros_ec_sensorhub * sensorhub)952 int 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  */
cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub * sensorhub)999 int 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 
cros_ec_sensorhub_ring_remove(void * arg)1041 void 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 }
1051