drivers/platform/chrome/cros_ec_sensorhub_ring.c

   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(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
 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(&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  */
 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(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  */
 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(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(&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  */
 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                                                            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
 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(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  */
 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->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                                                 &current_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
 934 ring_handler_end:
 935         sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] = current_timestamp;
 936         return;
 937
 938 error:
 939         mutex_unlock(&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(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  */
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->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
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, false);
1076         blocking_notifier_chain_unregister(&ec_dev->event_notifier,
1077                                            &sensorhub->notifier);
1078 }