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