stm32: fix keyboard FIFO

When the FIFO is empty, returns the last read entry not the next one.

also rewrite the FIFO index increment to generate slightly better code.

Signed-off-by: Vincent Palatin <vpalatin@chromium.org>

BUG=chrome-os-partner:8869
TEST=On Snow, in U-Boot using "stdin=mkbp-keyb" type on internal
keyboard and see the correct text.

Change-Id: I189d230053de40dd563ce672db82dd6217e545e3
(cherry picked from commit 38bab6b9f1fc74d53960b62054792f7885fd994a)

2 files changed, 12 insertions, 13 deletions

diff --git a/board/snow/board.h b/board/snow/board.h
index 67156dc0ec..dac98a22ed 100644
--- a/board/snow/board.h
+++ b/board/snow/board.h
@@ -17,6 +17,9 @@
 /* use I2C for host communication */
 #define CONFIG_I2C
 
+/* By default, enable all console messages except keyboard */
+#define CC_DEFAULT	(CC_ALL & ~CC_MASK(CC_KEYSCAN))
+
 #define USB_CHARGE_PORT_COUNT 0
 
 /* EC drives 13 outputs to keyboard matrix */
diff --git a/chip/stm32/keyboard_scan.c b/chip/stm32/keyboard_scan.c
index 2784ae1dc2..3a302969e0 100644
--- a/chip/stm32/keyboard_scan.c
+++ b/chip/stm32/keyboard_scan.c
@@ -76,8 +76,8 @@ void board_keyboard_suppress_noise(void)
 		__attribute__((weak, alias("__board_keyboard_suppress_noise")));
 
 #define KB_FIFO_DEPTH		16	/* FIXME: this is pretty huge */
-static int kb_fifo_start;		/* first entry */
-static int kb_fifo_end;			/* last entry */
+static uint32_t kb_fifo_start;		/* first entry */
+static uint32_t kb_fifo_end;			/* last entry */
 static uint32_t kb_fifo_entries;	/* number of existing entries */
 static uint8_t kb_fifo[KB_FIFO_DEPTH][KB_OUTPUTS];
 
@@ -111,10 +111,7 @@ static int kb_fifo_add(uint8_t *buffp)
 
 	memcpy(kb_fifo[kb_fifo_end], buffp, KB_OUTPUTS);
 
-	if (kb_fifo_end == KB_FIFO_DEPTH - 1)
-		kb_fifo_end = 0;
-	else
-		kb_fifo_end++;
+	kb_fifo_end = (kb_fifo_end + 1) % KB_FIFO_DEPTH;
 
 	atomic_add(&kb_fifo_entries, 1);
 
@@ -129,10 +126,11 @@ kb_fifo_push_done:
   */
 static int kb_fifo_remove(uint8_t *buffp)
 {
-	memcpy(buffp, kb_fifo[kb_fifo_start], KB_OUTPUTS);
-
 	if (!kb_fifo_entries) {
-		CPRINTF("%s: No entries remaining in FIFO\n", __func__);
+		/* no entry remaining in FIFO : return last known state */
+		memcpy(buffp, kb_fifo[(kb_fifo_start - 1) % KB_FIFO_DEPTH],
+		       KB_OUTPUTS);
+
 		/*
 		 * Bail out without changing any FIFO indices and let the
 		 * caller know something strange happened. The buffer will
@@ -140,11 +138,9 @@ static int kb_fifo_remove(uint8_t *buffp)
 		 */
 		return EC_ERROR_UNKNOWN;
 	}
+	memcpy(buffp, kb_fifo[kb_fifo_start], KB_OUTPUTS);
 
-	if (kb_fifo_start ==  KB_FIFO_DEPTH - 1)
-		kb_fifo_start = 0;
-	else
-		kb_fifo_start++;
+	kb_fifo_start = (kb_fifo_start + 1) % KB_FIFO_DEPTH;
 
 	atomic_sub(&kb_fifo_entries, 1);