diff options
Diffstat (limited to 'drivers/platform/chrome/cros_ec_sensorhub_ring.c')
| -rw-r--r-- | drivers/platform/chrome/cros_ec_sensorhub_ring.c | 196 |
1 files changed, 110 insertions, 86 deletions
diff --git a/drivers/platform/chrome/cros_ec_sensorhub_ring.c b/drivers/platform/chrome/cros_ec_sensorhub_ring.c index 24e48d96ed76..a10579144c34 100644 --- a/drivers/platform/chrome/cros_ec_sensorhub_ring.c +++ b/drivers/platform/chrome/cros_ec_sensorhub_ring.c @@ -17,6 +17,9 @@ #include <linux/sort.h> #include <linux/slab.h> +#define CREATE_TRACE_POINTS +#include "cros_ec_sensorhub_trace.h" + /* Precision of fixed point for the m values from the filter */ #define M_PRECISION BIT(23) @@ -100,7 +103,7 @@ EXPORT_SYMBOL_GPL(cros_ec_sensorhub_unregister_push_data); * @sensorhub: Sensor Hub object * @on: true when events are requested. * - * To be called before sleeping or when noone is listening. + * To be called before sleeping or when no one is listening. * Return: 0 on success, or an error when we can not communicate with the EC. * */ @@ -126,37 +129,76 @@ int cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub *sensorhub, /* We expect to receive a payload of 4 bytes, ignore. */ if (ret > 0) ret = 0; + /* + * Some platforms (such as Smaug) don't support the FIFO_INT_ENABLE + * command and the interrupt is always enabled. In the case, it + * returns -EINVAL. + * + * N.B: there is no danger of -EINVAL meaning any other invalid + * parameter since fifo_int_enable.enable is a bool and can never + * be in an invalid range. + */ + else if (ret == -EINVAL) + ret = 0; return ret; } -static int cros_ec_sensor_ring_median_cmp(const void *pv1, const void *pv2) +static void cros_ec_sensor_ring_median_swap(s64 *a, s64 *b) { - s64 v1 = *(s64 *)pv1; - s64 v2 = *(s64 *)pv2; - - if (v1 > v2) - return 1; - else if (v1 < v2) - return -1; - else - return 0; + s64 tmp = *a; + *a = *b; + *b = tmp; } /* * 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. + * It's implemented using the quickselect algorithm, which achieves an + * average time complexity of O(n) the middle element. In the worst case, + * the runtime of quickselect could regress to O(n^2). To mitigate this, + * algorithms like median-of-medians exist, which can guarantee O(n) even + * in the worst case. However, these algorithms come with a higher + * overhead and are more complex to implement, making quickselect a + * pragmatic choice for our use case. * - * Warning: the input array gets modified (sorted)! + * Warning: the input array gets modified! */ 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]; + int lo = 0; + int hi = length - 1; + + while (lo <= hi) { + int mid = lo + (hi - lo) / 2; + int pivot, i; + + if (array[lo] > array[mid]) + cros_ec_sensor_ring_median_swap(&array[lo], &array[mid]); + if (array[lo] > array[hi]) + cros_ec_sensor_ring_median_swap(&array[lo], &array[hi]); + if (array[mid] < array[hi]) + cros_ec_sensor_ring_median_swap(&array[mid], &array[hi]); + + pivot = array[hi]; + i = lo - 1; + + for (int j = lo; j < hi; j++) + if (array[j] < pivot) + cros_ec_sensor_ring_median_swap(&array[++i], &array[j]); + + /* The pivot's index corresponds to i+1. */ + cros_ec_sensor_ring_median_swap(&array[i + 1], &array[hi]); + if (i + 1 == length / 2) + return array[i + 1]; + if (i + 1 > length / 2) + hi = i; + else + lo = i + 2; + } + + /* Should never reach here. */ + return -1; } /* @@ -172,8 +214,8 @@ static s64 cros_ec_sensor_ring_median(s64 *array, size_t length) * * 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: + * first thing in ec_irq_handler(). There is a very good chance we'll get + * added latency due to: * other irqs * ddrfreq * cpuidle @@ -291,6 +333,7 @@ cros_ec_sensor_ring_ts_filter_update(struct cros_ec_sensors_ts_filter_state state->median_m = 0; state->median_error = 0; } + trace_cros_ec_sensorhub_filter(state, dx, dy); } /** @@ -419,9 +462,7 @@ cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub, * 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; + new_timestamp = c - b * 1000 + a * 1000; /* * The timestamp can be stale if we had to use the fifo * info timestamp. @@ -429,6 +470,11 @@ cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub, if (new_timestamp - *current_timestamp > 0) *current_timestamp = new_timestamp; } + trace_cros_ec_sensorhub_timestamp(in->timestamp, + fifo_info->timestamp, + fifo_timestamp, + *current_timestamp, + now); } if (in->flags & MOTIONSENSE_SENSOR_FLAG_ODR) { @@ -462,6 +508,12 @@ cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub, /* Regular sample */ out->sensor_id = in->sensor_num; + trace_cros_ec_sensorhub_data(in->sensor_num, + fifo_info->timestamp, + fifo_timestamp, + *current_timestamp, + now); + if (*current_timestamp - now > 0) { /* * This fix is needed to overcome the timestamp filter putting @@ -498,7 +550,7 @@ cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub, * ringbuffer. * * This is the new spreading code, assumes every sample's timestamp - * preceeds the sample. Run if tight_timestamps == true. + * precedes 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 @@ -582,7 +634,7 @@ cros_ec_sensor_ring_spread_add(struct cros_ec_sensorhub *sensorhub, } else { /* * Push first sample in the batch to the, - * kifo, it's guaranteed to be correct, the + * kfifo, it's guaranteed to be correct, the * rest will follow later on. */ sample_idx = 1; @@ -675,29 +727,22 @@ done_with_this_batch: * 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 + * Note: This assumes we're running old firmware, where timestamp + * is inserted after its sample(s)e. There can be several samples between + * timestamps, so several samples can have the same 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]. + * 1st sample --> TS1 | 1 + * TS2 | 2 + * TS2 | 3 + * TS3 | 4 + * last_out --> * - * 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 spread time for the samples using period p = (current - TS1)/4. + * between TS1 and TS2: [TS1+p/4, TS1+2p/4, TS1+3p/4, current_timestamp]. * - * 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, @@ -710,58 +755,37 @@ cros_ec_sensor_ring_spread_add_legacy(struct cros_ec_sensorhub *sensorhub, 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; + int count = 0; + s64 time_period; - for (out = sensorhub->ring; out < last_out; out = next_out) { - s64 time_period; - - next_out = out + 1; + for (out = sensorhub->ring; out < last_out; out++) { 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++; + timestamp = out->timestamp; + out++; + count = 1; + break; + } + for (; out < last_out; out++) { + /* Find last sample. */ + if (out->sensor_id != i) + continue; + count++; + } + if (count == 0) + continue; - if (next_out >= last_out) { - timestamp = current_timestamp; - } else { - timestamp = next_out->timestamp; - if (timestamp == older_timestamp) { - count++; - continue; - } - } + /* Spread uniformly between the first and last samples. */ + time_period = div_s64(current_timestamp - timestamp, count); - /* - * 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; + for (out = sensorhub->ring; out < last_out; out++) { + if (out->sensor_id != i) + continue; + timestamp += time_period; + out->timestamp = timestamp; } } |