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