From befc326c7f6cdfc7f86d6d290c75eb80b417dfc1 Mon Sep 17 00:00:00 2001
From: Dreaded_X <tim.huizinga@gmail.com>
Date: Mon, 18 Jan 2021 17:25:25 +0100
Subject: [PATCH] Instead of delay, wait for keyboard to respond

---
 include/coroutine.h |  6 ++++++
 include/fifo.h      |  5 +----
 include/keyboard.h  |  5 +----
 include/scancode.h  |  5 +----
 src/keyboard.c      |  1 +
 src/main.c          | 12 ++++++++++--
 6 files changed, 20 insertions(+), 14 deletions(-)
 create mode 100644 include/coroutine.h

diff --git a/include/coroutine.h b/include/coroutine.h
new file mode 100644
index 0000000..5634728
--- /dev/null
+++ b/include/coroutine.h
@@ -0,0 +1,6 @@
+#pragma once
+
+#define CO_BEGIN  static unsigned state = 0; switch (state) { case 0:
+#define CO_YIELD  do { state = __LINE__; return; case __LINE__:; } while (0)
+#define CO_BREAK  do { state = 0; return; } while (0)
+#define CO_END    } state = 0
diff --git a/include/fifo.h b/include/fifo.h
index 7aa0e56..94d8df9 100644
--- a/include/fifo.h
+++ b/include/fifo.h
@@ -1,5 +1,4 @@
-#ifndef FIFO_h
-#define FIFO_h
+#pragma once
 
 #include <stdint.h>
 
@@ -16,5 +15,3 @@ void FIFO_push(volatile struct FIFO* fifo, uint8_t value);
 uint8_t FIFO_pop(volatile struct FIFO* fifo);
 int FIFO_size(volatile struct FIFO* fifo);
 void FIFO_clear(volatile struct FIFO* fifo);
-
-#endif
diff --git a/include/keyboard.h b/include/keyboard.h
index 7ae4570..42b4d7b 100644
--- a/include/keyboard.h
+++ b/include/keyboard.h
@@ -1,5 +1,4 @@
-#ifndef KEYBOARD_h
-#define KEYBOARD_h
+#pragma once
 
 #include <stdint.h>
 
@@ -12,5 +11,3 @@ void queue_keyboard_cmd(uint8_t value);
 void send_keyboard_cmd_queue();
 
 void keyboard_interrupt(void (*callback)(uint8_t));
-
-#endif
diff --git a/include/scancode.h b/include/scancode.h
index 4bdf82b..00a2fda 100644
--- a/include/scancode.h
+++ b/include/scancode.h
@@ -1,8 +1,5 @@
-#ifndef SCANCODE_H
-#define SCANCODE_H
+#pragma once
 
 #include <stdint.h>
 
 uint8_t convert_scancode(uint8_t shift, uint8_t code);
-
-#endif
diff --git a/src/keyboard.c b/src/keyboard.c
index 8574007..9e8c21c 100644
--- a/src/keyboard.c
+++ b/src/keyboard.c
@@ -4,6 +4,7 @@
 #include "keyboard.h"
 #include "scancode.h"
 #include "fifo.h"
+#include "coroutine.h"
 
 volatile struct {
 	struct FIFO buffer;
diff --git a/src/main.c b/src/main.c
index 9b11eba..fd3dbab 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,5 +1,6 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
+#include <stdint.h>
 #include <util/delay.h>
 #include <stddef.h>
 
@@ -57,6 +58,8 @@ void I2C_requested() {
 }
 
 int main() {
+	/* FIFO_clear(&buffer); */
+
 	// Configure pins as output
 	DDRD |= (1<<RDY) | (1<<CLK) | (1<<DATA);
 	PORTD |= (1<<RDY);
@@ -74,8 +77,13 @@ int main() {
 
 	sei();
 
-	/* // Wait for the keyboard to be ready */
-	_delay_ms(500);
+	send_keyboard_cmd(0xFF);
+
+	while(FIFO_size(&buffer)) {};
+	if (FIFO_pop(&buffer) != 0xFF) {
+		send_keyboard_cmd(0xED);
+		send_keyboard_cmd(0x06);
+	}
 
 	// Set the keyboard repeat rate (and delay)
 	send_keyboard_cmd(0xF3);