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