[PATCH] platform: cros_ec: sensorhub: Simplify legacy timestamp spreading

[Gwendal Grignou]

On some machine (nami), interrupt latency cause samples to appear
to be from the future and are pegged to the current time.
We would see samples with this pattern:

[t, t + ~5ms, t + ~10ms, t + ~10ms + 100us, t + ~10ms + 200us],
                             (current now)      (current now)
(t is the last timestamp time)

Last 2 samples would be barely spread, causing applications to
complain.
We now spread the entire sequence. This is not great: in the example
the sensor was supposed to send samples every 5ms, it now appears to
send one every 2.5ms, but it is slightly closer to reality:

sampling time in the example above
At sensor level

1             2             3             4            5
+-----5ms-----+-----5ms-----+-----5ms-----+----5ms-----+---> t

Before, at host level
                       1             2             3 4 5
--interrupt delay------+-----5ms-----+-----5ms-----+-+-+---> t

Afer, at host level
                       1       2       3       4       5
--interrupt delay------+-2.5ms-+-2.5ms-+-2.5ms-+-2.5ms-+---> t

Signed-off-by: Gwendal Grignou <gwendal@xxxxxxxxxxxx>
---
 .../platform/chrome/cros_ec_sensorhub_ring.c  | 94 +++++++------------
 1 file changed, 33 insertions(+), 61 deletions(-)

diff --git a/drivers/platform/chrome/cros_ec_sensorhub_ring.c b/drivers/platform/chrome/cros_ec_sensorhub_ring.c
index 1b9637655e63e..0b0bf93073c30 100644
--- a/drivers/platform/chrome/cros_ec_sensorhub_ring.c
+++ b/drivers/platform/chrome/cros_ec_sensorhub_ring.c
@@ -719,29 +719,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 perod 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,
@@ -754,58 +747,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;
-
-		for (out = sensorhub->ring; out < last_out; out = next_out) {
-			s64 time_period;
+		int count = 0;
+		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;
 		}
 	}
 
-- 
2.28.0.rc0.142.g3c755180ce-goog