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