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