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