1079 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * Driver for Chrome OS EC Sensor hub FIFO.4 *5 * Copyright 2020 Google LLC6 */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_POINTS21#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 828 29/*30 * If we don't have any history entries for this long, empty the filter to31 * make sure there are no big discontinuities.32 */33#define TS_HISTORY_BORED_US 50000034 35/* To measure by how much the filter is overshooting, if it happens. */36#define FUTURE_TS_ANALYTICS_COUNT_MAX 10037 38static inline int39cros_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 object65 * @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 events70 * 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 generation102 * for FIFO events.103 * @sensorhub: Sensor Hub object104 * @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 numbers145 *146 * It's implemented using the quickselect algorithm, which achieves an147 * 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) even150 * in the worst case. However, these algorithms come with a higher151 * overhead and are more complex to implement, making quickselect a152 * 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 else186 lo = i + 2;187 }188 189 /* Should never reach here. */190 return -1;191}192 193/*194 * IRQ Timestamp Filtering195 *196 * Lower down in cros_ec_sensor_ring_process_event(), for each sensor event197 * we have to calculate it's timestamp in the AP timebase. There are 3 time198 * points:199 * a - EC timebase, sensor event200 * b - EC timebase, IRQ201 * c - AP timebase, IRQ202 * a' - what we want: sensor even in AP timebase203 *204 * While a and b are recorded at accurate times (due to the EC real time205 * nature); c is pretty untrustworthy, even though it's recorded the206 * first thing in ec_irq_handler(). There is a very good chance we'll get207 * added latency due to:208 * other irqs209 * ddrfreq210 * cpuidle211 *212 * Normally a' = c - b + a, but if we do that naive math any jitter in c213 * will get coupled in a', which we don't want. We want a function214 * 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 drift218 * between the 2 chips for various reasons (mechanical stress, temperature,219 * voltage). We need to extrapolate values for a future x, without trusting220 * recent y values too much.221 *222 * We use a median filter for the slope, then another median filter for the223 * 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_error231 *232 * Implementation differences from above:233 * - Redefined y to be actually c - b, this gives us a lot more precision234 * 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 are236 * done using fixed point math (*M_PRECISION)237 * - Since x and y grow with time, we keep zeroing the graph (relative to238 * the last sample), this way math involving *x[n-i] will not overflow239 * - EC timestamps are kept in us, it improves the slope calculation precision240 */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 filter250 * history.251 */252static void253cros_ec_sensor_ring_ts_filter_update(struct cros_ec_sensors_ts_filter_state254 *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 future287 * 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 and312 * points in the history are on the line. median_error will313 * 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 AP330 * timebase331 *332 * @state: filter information.333 * @x: any ec timestamp (us):334 *335 * cros_ec_sensor_ring_ts_filter(a) => a' event timestamp, AP timebase336 * cros_ec_sensor_ring_ts_filter(b) => calculated timestamp when the EC IRQ337 * should have happened on the AP, with low jitter338 *339 * Note: The filter will only activate once state->history_len goes340 * over TS_HISTORY_THRESHOLD. Otherwise it'll just do the naive c - b + a341 * transform.342 *343 * How to derive the formula, starting from:344 * f(x) = median_m * x + median_error345 * 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_offset348 * Remember to undo the "y = c - b * 1000" modification:349 * f(x) = median_m * (x - x_offset) + median_error + y_offset + x * 1000350 *351 * Return: timestamp in AP timebase (ns)352 */353static s64354cros_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 to364 * add an offset.365 */366static void367cros_ec_sensor_ring_fix_overflow(s64 *ts,368 const s64 overflow_period,369 struct cros_ec_sensors_ec_overflow_state370 *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 void384cros_ec_sensor_ring_check_for_past_timestamp(struct cros_ec_sensorhub385 *sensorhub,386 struct cros_ec_sensors_ring_sample387 *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 else398 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 event404 *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 bool417cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub,418 const struct ec_response_motion_sense_fifo_info419 *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 jitter452 * 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 fifo457 * 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 not476 * 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 with489 * 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 putting509 * 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 to539 * ringbuffer.540 *541 * This is the new spreading code, assumes every sample's timestamp542 * precedes the sample. Run if tight_timestamps == true.543 *544 * Sometimes the EC receives only one interrupt (hence timestamp) for545 * a batch of samples. Only the first sample will have the correct546 * timestamp. So we must interpolate the other samples.547 * We use the previous batch timestamp and our current batch timestamp548 * as a way to calculate period, then spread the samples evenly.549 *550 * s0 int, 0ms551 * s1 int, 10ms552 * s2 int, 20ms553 * 30ms point goes by, no interrupt, previous one is still asserted554 * downloading s2 and s3555 * s3 sample, 20ms (incorrect timestamp)556 * s4 int, 40ms557 *558 * The batches are [(s0), (s1), (s2, s3), (s4)]. Since the 3rd batch559 * 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 have561 * been part of a bigger batch things would have gotten a little562 * more complicated.563 *564 * Note: we also assume another sensor sample doesn't break up a batch565 * in 2 or more partitions. Example, there can't ever be a sync sensor566 * in between S2 and S3. This simplifies the following code.567 */568static void569cros_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 same581 * 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 types592 * 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 batch601 * from a flush, as it happens asynchronously to the602 * 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, the627 * 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 that641 * 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 samples664 * in this batch since we have no idea where665 * they're supposed to go without a period666 * 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 to683 * kfifo.684 */685 for (s = batch_start; s <= batch_end; s++) {686 if (s->sensor_id != id)687 /*688 * Skip over other sensor types that689 * 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 then717 * add to ringbuffer (legacy).718 *719 * Note: This assumes we're running old firmware, where timestamp720 * is inserted after its sample(s)e. There can be several samples between721 * timestamps, so several samples can have the same timestamp.722 *723 * timestamp | count724 * -----------------725 * 1st sample --> TS1 | 1726 * TS2 | 2727 * TS2 | 3728 * TS3 | 4729 * 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 void737cros_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_sample741 *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 function788 *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 the825 * 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 AP902 * 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 the924 * ringbuffer.925 */926 if (sensorhub->tight_timestamps)927 cros_ec_sensor_ring_spread_add(sensorhub, sensor_mask,928 last_out);929 else930 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 EC974 * 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 EC1021 * 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 set1047 * 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