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