Merge tag 'io_uring-5.7-2020-05-22' of git://git.kernel.dk/linux-block

[thirdparty/linux.git] / drivers / platform / chrome / cros_ec_sensorhub_ring.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Driver for Chrome OS EC Sensor hub FIFO.
 *
 * Copyright 2020 Google LLC
 */

#include <linux/delay.h>
#include <linux/device.h>
#include <linux/iio/iio.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/platform_data/cros_ec_commands.h>
#include <linux/platform_data/cros_ec_proto.h>
#include <linux/platform_data/cros_ec_sensorhub.h>
#include <linux/platform_device.h>
#include <linux/sort.h>
#include <linux/slab.h>

/* Precision of fixed point for the m values from the filter */
#define M_PRECISION BIT(23)

/* Only activate the filter once we have at least this many elements. */
#define TS_HISTORY_THRESHOLD 8

/*
 * If we don't have any history entries for this long, empty the filter to
 * make sure there are no big discontinuities.
 */
#define TS_HISTORY_BORED_US 500000

/* To measure by how much the filter is overshooting, if it happens. */
#define FUTURE_TS_ANALYTICS_COUNT_MAX 100

static inline int
cros_sensorhub_send_sample(struct cros_ec_sensorhub *sensorhub,
                           struct cros_ec_sensors_ring_sample *sample)
{
        cros_ec_sensorhub_push_data_cb_t cb;
        int id = sample->sensor_id;
        struct iio_dev *indio_dev;

        if (id >= sensorhub->sensor_num)
                return -EINVAL;

        cb = sensorhub->push_data[id].push_data_cb;
        if (!cb)
                return 0;

        indio_dev = sensorhub->push_data[id].indio_dev;

        if (sample->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
                return 0;

        return cb(indio_dev, sample->vector, sample->timestamp);
}

/**
 * cros_ec_sensorhub_register_push_data() - register the callback to the hub.
 *
 * @sensorhub : Sensor Hub object
 * @sensor_num : The sensor the caller is interested in.
 * @indio_dev : The iio device to use when a sample arrives.
 * @cb : The callback to call when a sample arrives.
 *
 * The callback cb will be used by cros_ec_sensorhub_ring to distribute events
 * from the EC.
 *
 * Return: 0 when callback is registered.
 *         EINVAL is the sensor number is invalid or the slot already used.
 */
int cros_ec_sensorhub_register_push_data(struct cros_ec_sensorhub *sensorhub,
                                         u8 sensor_num,
                                         struct iio_dev *indio_dev,
                                         cros_ec_sensorhub_push_data_cb_t cb)
{
        if (sensor_num >= sensorhub->sensor_num)
                return -EINVAL;
        if (sensorhub->push_data[sensor_num].indio_dev)
                return -EINVAL;

        sensorhub->push_data[sensor_num].indio_dev = indio_dev;
        sensorhub->push_data[sensor_num].push_data_cb = cb;

        return 0;
}
EXPORT_SYMBOL_GPL(cros_ec_sensorhub_register_push_data);

void cros_ec_sensorhub_unregister_push_data(struct cros_ec_sensorhub *sensorhub,
                                            u8 sensor_num)
{
        sensorhub->push_data[sensor_num].indio_dev = NULL;
        sensorhub->push_data[sensor_num].push_data_cb = NULL;
}
EXPORT_SYMBOL_GPL(cros_ec_sensorhub_unregister_push_data);

/**
 * cros_ec_sensorhub_ring_fifo_enable() - Enable or disable interrupt generation
 *                                        for FIFO events.
 * @sensorhub: Sensor Hub object
 * @on: true when events are requested.
 *
 * To be called before sleeping or when noone is listening.
 * Return: 0 on success, or an error when we can not communicate with the EC.
 *
 */
int cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub *sensorhub,
                                       bool on)
{
        int ret, i;

        mutex_lock(&sensorhub->cmd_lock);
        if (sensorhub->tight_timestamps)
                for (i = 0; i < sensorhub->sensor_num; i++)
                        sensorhub->batch_state[i].last_len = 0;

        sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INT_ENABLE;
        sensorhub->params->fifo_int_enable.enable = on;

        sensorhub->msg->outsize = sizeof(struct ec_params_motion_sense);
        sensorhub->msg->insize = sizeof(struct ec_response_motion_sense);

        ret = cros_ec_cmd_xfer_status(sensorhub->ec->ec_dev, sensorhub->msg);
        mutex_unlock(&sensorhub->cmd_lock);

        /* We expect to receive a payload of 4 bytes, ignore. */
        if (ret > 0)
                ret = 0;

        return ret;
}

static int cros_ec_sensor_ring_median_cmp(const void *pv1, const void *pv2)
{
        s64 v1 = *(s64 *)pv1;
        s64 v2 = *(s64 *)pv2;

        if (v1 > v2)
                return 1;
        else if (v1 < v2)
                return -1;
        else
                return 0;
}

/*
 * cros_ec_sensor_ring_median: Gets median of an array of numbers
 *
 * For now it's implemented using an inefficient > O(n) sort then return
 * the middle element. A more optimal method would be something like
 * quickselect, but given that n = 64 we can probably live with it in the
 * name of clarity.
 *
 * Warning: the input array gets modified (sorted)!
 */
static s64 cros_ec_sensor_ring_median(s64 *array, size_t length)
{
        sort(array, length, sizeof(s64), cros_ec_sensor_ring_median_cmp, NULL);
        return array[length / 2];
}

/*
 * IRQ Timestamp Filtering
 *
 * Lower down in cros_ec_sensor_ring_process_event(), for each sensor event
 * we have to calculate it's timestamp in the AP timebase. There are 3 time
 * points:
 *   a - EC timebase, sensor event
 *   b - EC timebase, IRQ
 *   c - AP timebase, IRQ
 *   a' - what we want: sensor even in AP timebase
 *
 * While a and b are recorded at accurate times (due to the EC real time
 * nature); c is pretty untrustworthy, even though it's recorded the
 * first thing in ec_irq_handler(). There is a very good change we'll get
 * added lantency due to:
 *   other irqs
 *   ddrfreq
 *   cpuidle
 *
 * Normally a' = c - b + a, but if we do that naive math any jitter in c
 * will get coupled in a', which we don't want. We want a function
 * a' = cros_ec_sensor_ring_ts_filter(a) which will filter out outliers in c.
 *
 * Think of a graph of AP time(b) on the y axis vs EC time(c) on the x axis.
 * The slope of the line won't be exactly 1, there will be some clock drift
 * between the 2 chips for various reasons (mechanical stress, temperature,
 * voltage). We need to extrapolate values for a future x, without trusting
 * recent y values too much.
 *
 * We use a median filter for the slope, then another median filter for the
 * y-intercept to calculate this function:
 *   dx[n] = x[n-1] - x[n]
 *   dy[n] = x[n-1] - x[n]
 *   m[n] = dy[n] / dx[n]
 *   median_m = median(m[n-k:n])
 *   error[i] = y[n-i] - median_m * x[n-i]
 *   median_error = median(error[:k])
 *   predicted_y = median_m * x + median_error
 *
 * Implementation differences from above:
 * - Redefined y to be actually c - b, this gives us a lot more precision
 * to do the math. (c-b)/b variations are more obvious than c/b variations.
 * - Since we don't have floating point, any operations involving slope are
 * done using fixed point math (*M_PRECISION)
 * - Since x and y grow with time, we keep zeroing the graph (relative to
 * the last sample), this way math involving *x[n-i] will not overflow
 * - EC timestamps are kept in us, it improves the slope calculation precision
 */

/**
 * cros_ec_sensor_ring_ts_filter_update() - Update filter history.
 *
 * @state: Filter information.
 * @b: IRQ timestamp, EC timebase (us)
 * @c: IRQ timestamp, AP timebase (ns)
 *
 * Given a new IRQ timestamp pair (EC and AP timebases), add it to the filter
 * history.
 */
static void
cros_ec_sensor_ring_ts_filter_update(struct cros_ec_sensors_ts_filter_state
                                     *state,
                                     s64 b, s64 c)
{
        s64 x, y;
        s64 dx, dy;
        s64 m; /* stored as *M_PRECISION */
        s64 *m_history_copy = state->temp_buf;
        s64 *error = state->temp_buf;
        int i;

        /* we trust b the most, that'll be our independent variable */
        x = b;
        /* y is the offset between AP and EC times, in ns */
        y = c - b * 1000;

        dx = (state->x_history[0] + state->x_offset) - x;
        if (dx == 0)
                return; /* we already have this irq in the history */
        dy = (state->y_history[0] + state->y_offset) - y;
        m = div64_s64(dy * M_PRECISION, dx);

        /* Empty filter if we haven't seen any action in a while. */
        if (-dx > TS_HISTORY_BORED_US)
                state->history_len = 0;

        /* Move everything over, also update offset to all absolute coords .*/
        for (i = state->history_len - 1; i >= 1; i--) {
                state->x_history[i] = state->x_history[i - 1] + dx;
                state->y_history[i] = state->y_history[i - 1] + dy;

                state->m_history[i] = state->m_history[i - 1];
                /*
                 * Also use the same loop to copy m_history for future
                 * median extraction.
                 */
                m_history_copy[i] = state->m_history[i - 1];
        }

        /* Store the x and y, but remember offset is actually last sample. */
        state->x_offset = x;
        state->y_offset = y;
        state->x_history[0] = 0;
        state->y_history[0] = 0;

        state->m_history[0] = m;
        m_history_copy[0] = m;

        if (state->history_len < CROS_EC_SENSORHUB_TS_HISTORY_SIZE)
                state->history_len++;

        /* Precalculate things for the filter. */
        if (state->history_len > TS_HISTORY_THRESHOLD) {
                state->median_m =
                    cros_ec_sensor_ring_median(m_history_copy,
                                               state->history_len - 1);

                /*
                 * Calculate y-intercepts as if m_median is the slope and
                 * points in the history are on the line. median_error will
                 * still be in the offset coordinate system.
                 */
                for (i = 0; i < state->history_len; i++)
                        error[i] = state->y_history[i] -
                                div_s64(state->median_m * state->x_history[i],
                                        M_PRECISION);
                state->median_error =
                        cros_ec_sensor_ring_median(error, state->history_len);
        } else {
                state->median_m = 0;
                state->median_error = 0;
        }
}

/**
 * cros_ec_sensor_ring_ts_filter() - Translate EC timebase timestamp to AP
 *                                   timebase
 *
 * @state: filter information.
 * @x: any ec timestamp (us):
 *
 * cros_ec_sensor_ring_ts_filter(a) => a' event timestamp, AP timebase
 * cros_ec_sensor_ring_ts_filter(b) => calculated timestamp when the EC IRQ
 *                           should have happened on the AP, with low jitter
 *
 * Note: The filter will only activate once state->history_len goes
 * over TS_HISTORY_THRESHOLD. Otherwise it'll just do the naive c - b + a
 * transform.
 *
 * How to derive the formula, starting from:
 *   f(x) = median_m * x + median_error
 * That's the calculated AP - EC offset (at the x point in time)
 * Undo the coordinate system transform:
 *   f(x) = median_m * (x - x_offset) + median_error + y_offset
 * Remember to undo the "y = c - b * 1000" modification:
 *   f(x) = median_m * (x - x_offset) + median_error + y_offset + x * 1000
 *
 * Return: timestamp in AP timebase (ns)
 */
static s64
cros_ec_sensor_ring_ts_filter(struct cros_ec_sensors_ts_filter_state *state,
                              s64 x)
{
        return div_s64(state->median_m * (x - state->x_offset), M_PRECISION)
               + state->median_error + state->y_offset + x * 1000;
}

/*
 * Since a and b were originally 32 bit values from the EC,
 * they overflow relatively often, casting is not enough, so we need to
 * add an offset.
 */
static void
cros_ec_sensor_ring_fix_overflow(s64 *ts,
                                 const s64 overflow_period,
                                 struct cros_ec_sensors_ec_overflow_state
                                 *state)
{
        s64 adjust;

        *ts += state->offset;
        if (abs(state->last - *ts) > (overflow_period / 2)) {
                adjust = state->last > *ts ? overflow_period : -overflow_period;
                state->offset += adjust;
                *ts += adjust;
        }
        state->last = *ts;
}

static void
cros_ec_sensor_ring_check_for_past_timestamp(struct cros_ec_sensorhub
                                             *sensorhub,
                                             struct cros_ec_sensors_ring_sample
                                             *sample)
{
        const u8 sensor_id = sample->sensor_id;

        /* If this event is earlier than one we saw before... */
        if (sensorhub->batch_state[sensor_id].newest_sensor_event >
            sample->timestamp)
                /* mark it for spreading. */
                sample->timestamp =
                        sensorhub->batch_state[sensor_id].last_ts;
        else
                sensorhub->batch_state[sensor_id].newest_sensor_event =
                        sample->timestamp;
}

/**
 * cros_ec_sensor_ring_process_event() - Process one EC FIFO event
 *
 * @sensorhub: Sensor Hub object.
 * @fifo_info: FIFO information from the EC (includes b point, EC timebase).
 * @fifo_timestamp: EC IRQ, kernel timebase (aka c).
 * @current_timestamp: calculated event timestamp, kernel timebase (aka a').
 * @in: incoming FIFO event from EC (includes a point, EC timebase).
 * @out: outgoing event to user space (includes a').
 *
 * Process one EC event, add it in the ring if necessary.
 *
 * Return: true if out event has been populated.
 */
static bool
cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub,
                                const struct ec_response_motion_sense_fifo_info
                                *fifo_info,
                                const ktime_t fifo_timestamp,
                                ktime_t *current_timestamp,
                                struct ec_response_motion_sensor_data *in,
                                struct cros_ec_sensors_ring_sample *out)
{
        const s64 now = cros_ec_get_time_ns();
        int axis, async_flags;

        /* Do not populate the filter based on asynchronous events. */
        async_flags = in->flags &
                (MOTIONSENSE_SENSOR_FLAG_ODR | MOTIONSENSE_SENSOR_FLAG_FLUSH);

        if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP && !async_flags) {
                s64 a = in->timestamp;
                s64 b = fifo_info->timestamp;
                s64 c = fifo_timestamp;

                cros_ec_sensor_ring_fix_overflow(&a, 1LL << 32,
                                          &sensorhub->overflow_a);
                cros_ec_sensor_ring_fix_overflow(&b, 1LL << 32,
                                          &sensorhub->overflow_b);

                if (sensorhub->tight_timestamps) {
                        cros_ec_sensor_ring_ts_filter_update(
                                        &sensorhub->filter, b, c);
                        *current_timestamp = cros_ec_sensor_ring_ts_filter(
                                        &sensorhub->filter, a);
                } else {
                        s64 new_timestamp;

                        /*
                         * Disable filtering since we might add more jitter
                         * if b is in a random point in time.
                         */
                        new_timestamp = fifo_timestamp -
                                        fifo_info->timestamp  * 1000 +
                                        in->timestamp * 1000;
                        /*
                         * The timestamp can be stale if we had to use the fifo
                         * info timestamp.
                         */
                        if (new_timestamp - *current_timestamp > 0)
                                *current_timestamp = new_timestamp;
                }
        }

        if (in->flags & MOTIONSENSE_SENSOR_FLAG_ODR) {
                if (sensorhub->tight_timestamps) {
                        sensorhub->batch_state[in->sensor_num].last_len = 0;
                        sensorhub->batch_state[in->sensor_num].penul_len = 0;
                }
                /*
                 * ODR change is only useful for the sensor_ring, it does not
                 * convey information to clients.
                 */
                return false;
        }

        if (in->flags & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
                out->sensor_id = in->sensor_num;
                out->timestamp = *current_timestamp;
                out->flag = in->flags;
                if (sensorhub->tight_timestamps)
                        sensorhub->batch_state[out->sensor_id].last_len = 0;
                /*
                 * No other payload information provided with
                 * flush ack.
                 */
                return true;
        }

        if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP)
                /* If we just have a timestamp, skip this entry. */
                return false;

        /* Regular sample */
        out->sensor_id = in->sensor_num;
        if (*current_timestamp - now > 0) {
                /*
                 * This fix is needed to overcome the timestamp filter putting
                 * events in the future.
                 */
                sensorhub->future_timestamp_total_ns +=
                        *current_timestamp - now;
                if (++sensorhub->future_timestamp_count ==
                                FUTURE_TS_ANALYTICS_COUNT_MAX) {
                        s64 avg = div_s64(sensorhub->future_timestamp_total_ns,
                                        sensorhub->future_timestamp_count);
                        dev_warn_ratelimited(sensorhub->dev,
                                             "100 timestamps in the future, %lldns shaved on average\n",
                                             avg);
                        sensorhub->future_timestamp_count = 0;
                        sensorhub->future_timestamp_total_ns = 0;
                }
                out->timestamp = now;
        } else {
                out->timestamp = *current_timestamp;
        }

        out->flag = in->flags;
        for (axis = 0; axis < 3; axis++)
                out->vector[axis] = in->data[axis];

        if (sensorhub->tight_timestamps)
                cros_ec_sensor_ring_check_for_past_timestamp(sensorhub, out);
        return true;
}

/*
 * cros_ec_sensor_ring_spread_add: Calculate proper timestamps then add to
 *                                 ringbuffer.
 *
 * This is the new spreading code, assumes every sample's timestamp
 * preceeds the sample. Run if tight_timestamps == true.
 *
 * Sometimes the EC receives only one interrupt (hence timestamp) for
 * a batch of samples. Only the first sample will have the correct
 * timestamp. So we must interpolate the other samples.
 * We use the previous batch timestamp and our current batch timestamp
 * as a way to calculate period, then spread the samples evenly.
 *
 * s0 int, 0ms
 * s1 int, 10ms
 * s2 int, 20ms
 * 30ms point goes by, no interrupt, previous one is still asserted
 * downloading s2 and s3
 * s3 sample, 20ms (incorrect timestamp)
 * s4 int, 40ms
 *
 * The batches are [(s0), (s1), (s2, s3), (s4)]. Since the 3rd batch
 * has 2 samples in them, we adjust the timestamp of s3.
 * s2 - s1 = 10ms, so s3 must be s2 + 10ms => 20ms. If s1 would have
 * been part of a bigger batch things would have gotten a little
 * more complicated.
 *
 * Note: we also assume another sensor sample doesn't break up a batch
 * in 2 or more partitions. Example, there can't ever be a sync sensor
 * in between S2 and S3. This simplifies the following code.
 */
static void
cros_ec_sensor_ring_spread_add(struct cros_ec_sensorhub *sensorhub,
                               unsigned long sensor_mask,
                               struct cros_ec_sensors_ring_sample *last_out)
{
        struct cros_ec_sensors_ring_sample *batch_start, *next_batch_start;
        int id;

        for_each_set_bit(id, &sensor_mask, sensorhub->sensor_num) {
                for (batch_start = sensorhub->ring; batch_start < last_out;
                     batch_start = next_batch_start) {
                        /*
                         * For each batch (where all samples have the same
                         * timestamp).
                         */
                        int batch_len, sample_idx;
                        struct cros_ec_sensors_ring_sample *batch_end =
                                batch_start;
                        struct cros_ec_sensors_ring_sample *s;
                        s64 batch_timestamp = batch_start->timestamp;
                        s64 sample_period;

                        /*
                         * Skip over batches that start with the sensor types
                         * we're not looking at right now.
                         */
                        if (batch_start->sensor_id != id) {
                                next_batch_start = batch_start + 1;
                                continue;
                        }

                        /*
                         * Do not start a batch
                         * from a flush, as it happens asynchronously to the
                         * regular flow of events.
                         */
                        if (batch_start->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
                                cros_sensorhub_send_sample(sensorhub,
                                                           batch_start);
                                next_batch_start = batch_start + 1;
                                continue;
                        }

                        if (batch_start->timestamp <=
                            sensorhub->batch_state[id].last_ts) {
                                batch_timestamp =
                                        sensorhub->batch_state[id].last_ts;
                                batch_len = sensorhub->batch_state[id].last_len;

                                sample_idx = batch_len;

                                sensorhub->batch_state[id].last_ts =
                                  sensorhub->batch_state[id].penul_ts;
                                sensorhub->batch_state[id].last_len =
                                  sensorhub->batch_state[id].penul_len;
                        } else {
                                /*
                                 * Push first sample in the batch to the,
                                 * kifo, it's guaranteed to be correct, the
                                 * rest will follow later on.
                                 */
                                sample_idx = 1;
                                batch_len = 1;
                                cros_sensorhub_send_sample(sensorhub,
                                                           batch_start);
                                batch_start++;
                        }

                        /* Find all samples have the same timestamp. */
                        for (s = batch_start; s < last_out; s++) {
                                if (s->sensor_id != id)
                                        /*
                                         * Skip over other sensor types that
                                         * are interleaved, don't count them.
                                         */
                                        continue;
                                if (s->timestamp != batch_timestamp)
                                        /* we discovered the next batch */
                                        break;
                                if (s->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
                                        /* break on flush packets */
                                        break;
                                batch_end = s;
                                batch_len++;
                        }

                        if (batch_len == 1)
                                goto done_with_this_batch;

                        /* Can we calculate period? */
                        if (sensorhub->batch_state[id].last_len == 0) {
                                dev_warn(sensorhub->dev, "Sensor %d: lost %d samples when spreading\n",
                                         id, batch_len - 1);
                                goto done_with_this_batch;
                                /*
                                 * Note: we're dropping the rest of the samples
                                 * in this batch since we have no idea where
                                 * they're supposed to go without a period
                                 * calculation.
                                 */
                        }

                        sample_period = div_s64(batch_timestamp -
                                sensorhub->batch_state[id].last_ts,
                                sensorhub->batch_state[id].last_len);
                        dev_dbg(sensorhub->dev,
                                "Adjusting %d samples, sensor %d last_batch @%lld (%d samples) batch_timestamp=%lld => period=%lld\n",
                                batch_len, id,
                                sensorhub->batch_state[id].last_ts,
                                sensorhub->batch_state[id].last_len,
                                batch_timestamp,
                                sample_period);

                        /*
                         * Adjust timestamps of the samples then push them to
                         * kfifo.
                         */
                        for (s = batch_start; s <= batch_end; s++) {
                                if (s->sensor_id != id)
                                        /*
                                         * Skip over other sensor types that
                                         * are interleaved, don't change them.
                                         */
                                        continue;

                                s->timestamp = batch_timestamp +
                                        sample_period * sample_idx;
                                sample_idx++;

                                cros_sensorhub_send_sample(sensorhub, s);
                        }

done_with_this_batch:
                        sensorhub->batch_state[id].penul_ts =
                                sensorhub->batch_state[id].last_ts;
                        sensorhub->batch_state[id].penul_len =
                                sensorhub->batch_state[id].last_len;

                        sensorhub->batch_state[id].last_ts =
                                batch_timestamp;
                        sensorhub->batch_state[id].last_len = batch_len;

                        next_batch_start = batch_end + 1;
                }
        }
}

/*
 * cros_ec_sensor_ring_spread_add_legacy: Calculate proper timestamps then
 * add to ringbuffer (legacy).
 *
 * Note: This assumes we're running old firmware, where every sample's timestamp
 * is after the sample. Run if tight_timestamps == false.
 *
 * If there is a sample with a proper timestamp
 *
 *                        timestamp | count
 *                        -----------------
 * older_unprocess_out --> TS1      | 1
 *                         TS1      | 2
 *                out -->  TS1      | 3
 *           next_out -->  TS2      |
 *
 * We spread time for the samples [older_unprocess_out .. out]
 * between TS1 and TS2: [TS1+1/4, TS1+2/4, TS1+3/4, TS2].
 *
 * If we reach the end of the samples, we compare with the
 * current timestamp:
 *
 * older_unprocess_out --> TS1      | 1
 *                         TS1      | 2
 *                 out --> TS1      | 3
 *
 * We know have [TS1+1/3, TS1+2/3, current timestamp]
 */
static void
cros_ec_sensor_ring_spread_add_legacy(struct cros_ec_sensorhub *sensorhub,
                                      unsigned long sensor_mask,
                                      s64 current_timestamp,
                                      struct cros_ec_sensors_ring_sample
                                      *last_out)
{
        struct cros_ec_sensors_ring_sample *out;
        int i;

        for_each_set_bit(i, &sensor_mask, sensorhub->sensor_num) {
                s64 older_timestamp;
                s64 timestamp;
                struct cros_ec_sensors_ring_sample *older_unprocess_out =
                        sensorhub->ring;
                struct cros_ec_sensors_ring_sample *next_out;
                int count = 1;

                for (out = sensorhub->ring; out < last_out; out = next_out) {
                        s64 time_period;

                        next_out = out + 1;
                        if (out->sensor_id != i)
                                continue;

                        /* Timestamp to start with */
                        older_timestamp = out->timestamp;

                        /* Find next sample. */
                        while (next_out < last_out && next_out->sensor_id != i)
                                next_out++;

                        if (next_out >= last_out) {
                                timestamp = current_timestamp;
                        } else {
                                timestamp = next_out->timestamp;
                                if (timestamp == older_timestamp) {
                                        count++;
                                        continue;
                                }
                        }

                        /*
                         * The next sample has a new timestamp, spread the
                         * unprocessed samples.
                         */
                        if (next_out < last_out)
                                count++;
                        time_period = div_s64(timestamp - older_timestamp,
                                              count);

                        for (; older_unprocess_out <= out;
                                        older_unprocess_out++) {
                                if (older_unprocess_out->sensor_id != i)
                                        continue;
                                older_timestamp += time_period;
                                older_unprocess_out->timestamp =
                                        older_timestamp;
                        }
                        count = 1;
                        /* The next_out sample has a valid timestamp, skip. */
                        next_out++;
                        older_unprocess_out = next_out;
                }
        }

        /* Push the event into the kfifo */
        for (out = sensorhub->ring; out < last_out; out++)
                cros_sensorhub_send_sample(sensorhub, out);
}

/**
 * cros_ec_sensorhub_ring_handler() - The trigger handler function
 *
 * @sensorhub: Sensor Hub object.
 *
 * Called by the notifier, process the EC sensor FIFO queue.
 */
static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub)
{
        struct ec_response_motion_sense_fifo_info *fifo_info =
                sensorhub->fifo_info;
        struct cros_ec_dev *ec = sensorhub->ec;
        ktime_t fifo_timestamp, current_timestamp;
        int i, j, number_data, ret;
        unsigned long sensor_mask = 0;
        struct ec_response_motion_sensor_data *in;
        struct cros_ec_sensors_ring_sample *out, *last_out;

        mutex_lock(&sensorhub->cmd_lock);

        /* Get FIFO information if there are lost vectors. */
        if (fifo_info->total_lost) {
                int fifo_info_length =
                        sizeof(struct ec_response_motion_sense_fifo_info) +
                        sizeof(u16) * sensorhub->sensor_num;

                /* Need to retrieve the number of lost vectors per sensor */
                sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
                sensorhub->msg->outsize = 1;
                sensorhub->msg->insize = fifo_info_length;

                if (cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg) < 0)
                        goto error;

                memcpy(fifo_info, &sensorhub->resp->fifo_info,
                       fifo_info_length);

                /*
                 * Update collection time, will not be as precise as the
                 * non-error case.
                 */
                fifo_timestamp = cros_ec_get_time_ns();
        } else {
                fifo_timestamp = sensorhub->fifo_timestamp[
                        CROS_EC_SENSOR_NEW_TS];
        }

        if (fifo_info->count > sensorhub->fifo_size ||
            fifo_info->size != sensorhub->fifo_size) {
                dev_warn(sensorhub->dev,
                         "Mismatch EC data: count %d, size %d - expected %d\n",
                         fifo_info->count, fifo_info->size,
                         sensorhub->fifo_size);
                goto error;
        }

        /* Copy elements in the main fifo */
        current_timestamp = sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS];
        out = sensorhub->ring;
        for (i = 0; i < fifo_info->count; i += number_data) {
                sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_READ;
                sensorhub->params->fifo_read.max_data_vector =
                        fifo_info->count - i;
                sensorhub->msg->outsize =
                        sizeof(struct ec_params_motion_sense);
                sensorhub->msg->insize =
                        sizeof(sensorhub->resp->fifo_read) +
                        sensorhub->params->fifo_read.max_data_vector *
                          sizeof(struct ec_response_motion_sensor_data);
                ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
                if (ret < 0) {
                        dev_warn(sensorhub->dev, "Fifo error: %d\n", ret);
                        break;
                }
                number_data = sensorhub->resp->fifo_read.number_data;
                if (number_data == 0) {
                        dev_dbg(sensorhub->dev, "Unexpected empty FIFO\n");
                        break;
                }
                if (number_data > fifo_info->count - i) {
                        dev_warn(sensorhub->dev,
                                 "Invalid EC data: too many entry received: %d, expected %d\n",
                                 number_data, fifo_info->count - i);
                        break;
                }
                if (out + number_data >
                    sensorhub->ring + fifo_info->count) {
                        dev_warn(sensorhub->dev,
                                 "Too many samples: %d (%zd data) to %d entries for expected %d entries\n",
                                 i, out - sensorhub->ring, i + number_data,
                                 fifo_info->count);
                        break;
                }

                for (in = sensorhub->resp->fifo_read.data, j = 0;
                     j < number_data; j++, in++) {
                        if (cros_ec_sensor_ring_process_event(
                                                sensorhub, fifo_info,
                                                fifo_timestamp,
                                                &current_timestamp,
                                                in, out)) {
                                sensor_mask |= BIT(in->sensor_num);
                                out++;
                        }
                }
        }
        mutex_unlock(&sensorhub->cmd_lock);
        last_out = out;

        if (out == sensorhub->ring)
                /* Unexpected empty FIFO. */
                goto ring_handler_end;

        /*
         * Check if current_timestamp is ahead of the last sample. Normally,
         * the EC appends a timestamp after the last sample, but if the AP
         * is slow to respond to the IRQ, the EC may have added new samples.
         * Use the FIFO info timestamp as last timestamp then.
         */
        if (!sensorhub->tight_timestamps &&
            (last_out - 1)->timestamp == current_timestamp)
                current_timestamp = fifo_timestamp;

        /* Warn on lost samples. */
        if (fifo_info->total_lost)
                for (i = 0; i < sensorhub->sensor_num; i++) {
                        if (fifo_info->lost[i]) {
                                dev_warn_ratelimited(sensorhub->dev,
                                                     "Sensor %d: lost: %d out of %d\n",
                                                     i, fifo_info->lost[i],
                                                     fifo_info->total_lost);
                                if (sensorhub->tight_timestamps)
                                        sensorhub->batch_state[i].last_len = 0;
                        }
                }

        /*
         * Spread samples in case of batching, then add them to the
         * ringbuffer.
         */
        if (sensorhub->tight_timestamps)
                cros_ec_sensor_ring_spread_add(sensorhub, sensor_mask,
                                               last_out);
        else
                cros_ec_sensor_ring_spread_add_legacy(sensorhub, sensor_mask,
                                                      current_timestamp,
                                                      last_out);

ring_handler_end:
        sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] = current_timestamp;
        return;

error:
        mutex_unlock(&sensorhub->cmd_lock);
}

static int cros_ec_sensorhub_event(struct notifier_block *nb,
                                   unsigned long queued_during_suspend,
                                   void *_notify)
{
        struct cros_ec_sensorhub *sensorhub;
        struct cros_ec_device *ec_dev;

        sensorhub = container_of(nb, struct cros_ec_sensorhub, notifier);
        ec_dev = sensorhub->ec->ec_dev;

        if (ec_dev->event_data.event_type != EC_MKBP_EVENT_SENSOR_FIFO)
                return NOTIFY_DONE;

        if (ec_dev->event_size != sizeof(ec_dev->event_data.data.sensor_fifo)) {
                dev_warn(ec_dev->dev, "Invalid fifo info size\n");
                return NOTIFY_DONE;
        }

        if (queued_during_suspend)
                return NOTIFY_OK;

        memcpy(sensorhub->fifo_info, &ec_dev->event_data.data.sensor_fifo.info,
               sizeof(*sensorhub->fifo_info));
        sensorhub->fifo_timestamp[CROS_EC_SENSOR_NEW_TS] =
                ec_dev->last_event_time;
        cros_ec_sensorhub_ring_handler(sensorhub);

        return NOTIFY_OK;
}

/**
 * cros_ec_sensorhub_ring_allocate() - Prepare the FIFO functionality if the EC
 *                                     supports it.
 *
 * @sensorhub : Sensor Hub object.
 *
 * Return: 0 on success.
 */
int cros_ec_sensorhub_ring_allocate(struct cros_ec_sensorhub *sensorhub)
{
        int fifo_info_length =
                sizeof(struct ec_response_motion_sense_fifo_info) +
                sizeof(u16) * sensorhub->sensor_num;

        /* Allocate the array for lost events. */
        sensorhub->fifo_info = devm_kzalloc(sensorhub->dev, fifo_info_length,
                                            GFP_KERNEL);
        if (!sensorhub->fifo_info)
                return -ENOMEM;

        /*
         * Allocate the callback area based on the number of sensors.
         * Add one for the sensor ring.
         */
        sensorhub->push_data = devm_kcalloc(sensorhub->dev,
                        sensorhub->sensor_num,
                        sizeof(*sensorhub->push_data),
                        GFP_KERNEL);
        if (!sensorhub->push_data)
                return -ENOMEM;

        sensorhub->tight_timestamps = cros_ec_check_features(
                        sensorhub->ec,
                        EC_FEATURE_MOTION_SENSE_TIGHT_TIMESTAMPS);

        if (sensorhub->tight_timestamps) {
                sensorhub->batch_state = devm_kcalloc(sensorhub->dev,
                                sensorhub->sensor_num,
                                sizeof(*sensorhub->batch_state),
                                GFP_KERNEL);
                if (!sensorhub->batch_state)
                        return -ENOMEM;
        }

        return 0;
}

/**
 * cros_ec_sensorhub_ring_add() - Add the FIFO functionality if the EC
 *                                supports it.
 *
 * @sensorhub : Sensor Hub object.
 *
 * Return: 0 on success.
 */
int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
{
        struct cros_ec_dev *ec = sensorhub->ec;
        int ret;
        int fifo_info_length =
                sizeof(struct ec_response_motion_sense_fifo_info) +
                sizeof(u16) * sensorhub->sensor_num;

        /* Retrieve FIFO information */
        sensorhub->msg->version = 2;
        sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
        sensorhub->msg->outsize = 1;
        sensorhub->msg->insize = fifo_info_length;

        ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
        if (ret < 0)
                return ret;

        /*
         * Allocate the full fifo. We need to copy the whole FIFO to set
         * timestamps properly.
         */
        sensorhub->fifo_size = sensorhub->resp->fifo_info.size;
        sensorhub->ring = devm_kcalloc(sensorhub->dev, sensorhub->fifo_size,
                                       sizeof(*sensorhub->ring), GFP_KERNEL);
        if (!sensorhub->ring)
                return -ENOMEM;

        sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] =
                cros_ec_get_time_ns();

        /* Register the notifier that will act as a top half interrupt. */
        sensorhub->notifier.notifier_call = cros_ec_sensorhub_event;
        ret = blocking_notifier_chain_register(&ec->ec_dev->event_notifier,
                                               &sensorhub->notifier);
        if (ret < 0)
                return ret;

        /* Start collection samples. */
        return cros_ec_sensorhub_ring_fifo_enable(sensorhub, true);
}

void cros_ec_sensorhub_ring_remove(void *arg)
{
        struct cros_ec_sensorhub *sensorhub = arg;
        struct cros_ec_device *ec_dev = sensorhub->ec->ec_dev;

        /* Disable the ring, prevent EC interrupt to the AP for nothing. */
        cros_ec_sensorhub_ring_fifo_enable(sensorhub, false);
        blocking_notifier_chain_unregister(&ec_dev->event_notifier,
                                           &sensorhub->notifier);
}