Commit b9b05664 authored by Gwendal Grignou's avatar Gwendal Grignou Committed by Enric Balletbo i Serra
Browse files

platform/chrome: cros_ec_sensorhub: Add code to spread timestmap 



EC FIFO can send sensor events in batch. Spread them based on
previous (TSa) and currnet timestamp (TSb)

  EC FIFO                             iio events
+-----------+
| TSa       |
+-----------+             +---------------------------------------+
| event 1   |             | event 1 | TSb - (TSb - TSa)/n * (n-1) |
+-----------+             +---------------------------------------+
| event 2   |             | event 2 | TSb - (TSb - TSa)/n * (n-2) |
+-----------+             +---------------------------------------+
|  ...      |  ------>    |  ....   |                             |
+-----------+             +---------------------------------------+
| event n-1 |             | event 2 | TSb - (TSb - TSa)/n         |
+-----------+             +---------------------------------------+
| event n   |             | event 2 | TSb                         |
+-----------+             +---------------------------------------+
| TSb       |
+-----------+

Signed-off-by: default avatarGwendal Grignou <gwendal@chromium.org>
Acked-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
Acked-by: default avatarLee Jones <lee.jones@linaro.org>
Acked-by: default avatarAndy Shevchenko <andy.shevchenko@gmail.com>
Signed-off-by: default avatarEnric Balletbo i Serra <enric.balletbo@collabora.com>
parent 145d59ba

drivers/platform/chrome/cros_ec_sensorhub_ring.c

+109 −8
Original line number Diff line number Diff line
@@ -190,6 +190,101 @@ cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub, 
	return true;

}



/*

 * cros_ec_sensor_ring_spread_add: Calculate proper timestamps then add to

 * ringbuffer.

 *

 * 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(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

 *
@@ -204,6 +299,7 @@ static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub) 
	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;
@@ -289,10 +385,12 @@ static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub) 
						sensorhub, fifo_info,

						fifo_timestamp,

						&current_timestamp,

						in, out))

						in, out)) {

				sensor_mask |= BIT(in->sensor_num);

				out++;

			}

		}

	}

	mutex_unlock(&sensorhub->cmd_lock);

	last_out = out;
@@ -301,10 +399,10 @@ static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub) 
		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.

	 * 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 ((last_out - 1)->timestamp == current_timestamp)

		current_timestamp = fifo_timestamp;
@@ -319,9 +417,12 @@ static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub) 
						     fifo_info->total_lost);

		}



	/* Push the event into the FIFO. */

	for (out = sensorhub->ring; out < last_out; out++)

		cros_sensorhub_send_sample(sensorhub, out);

	/*

	 * Spread samples in case of batching, then add them to the

	 * ringbuffer.

	 */

	cros_ec_sensor_ring_spread_add(sensorhub, sensor_mask,

				       current_timestamp, last_out);



ring_handler_end:

	sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] = current_timestamp;

CRA Git | Maintained and supported by SUSTech CRA and CCSE