common: motion_sense: Fix pop logic
The logic for popping data from the staged partition of the fifo
was incorrect. We never ended up decrementing the count.
BUG=b:135239484
BRANCH=None
TEST=Added code in motion_sense_init() to fake staging data into
the fifo. This replicated the issue, with the fix the issue was
resolved.
Change-Id: Ic4a0338131defbdfa44e1121d26ee3c5e8238b3b
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1665213
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
Reviewed-by: Enrico Granata <egranata@chromium.org>
Commit-Queue: Jack Rosenthal <jrosenth@chromium.org>
diff --git a/common/motion_sense.c b/common/motion_sense.c
index ce3f137..fe2d9e8 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -20,6 +20,7 @@
#include "mkbp_event.h"
#include "motion_sense.h"
#include "motion_lid.h"
+#include "panic.h"
#include "power.h"
#include "queue.h"
#include "tablet_mode.h"
@@ -114,7 +115,7 @@
*/
struct fifo_staged {
uint32_t read_ts;
- uint8_t count;
+ uint16_t count;
uint8_t sample_count[SENSOR_COUNT];
uint8_t requires_spreading;
};
@@ -169,8 +170,11 @@
if (!is_timestamp(head))
motion_sensors[head->sensor_num].lost++;
- /* Only continue if we removed from staged. */
- if (!initial_count)
+ /*
+ * We're done if the initial count was non-zero and we only advanced the
+ * head. Else, decrement the staged count and update staged metadata.
+ */
+ if (initial_count)
return;
fifo_staged.count--;
@@ -271,6 +275,9 @@
chunk = queue_get_write_chunk(
&motion_sense_fifo, fifo_staged.count);
+ if (!chunk.buffer)
+ panic("Failed to get write chunk for new fifo data");
+
/*
* Save the data to the writable block and increment count. This data
* will now reside AFTER the tail of the queue and will not be visible
@@ -281,6 +288,7 @@
*/
memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes);
fifo_staged.count++;
+
/*
* If we're using tight timestamps, and the current entry isn't a
* timestamp we'll increment the sample_count for the given sensor.
@@ -709,6 +717,7 @@
sensor->state = SENSOR_INITIALIZED;
motion_sense_set_data_rate(sensor);
}
+
return ret;
}
