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