From 3b0fb96364a4e5369aa3f335ef45027b7cbc059d Mon Sep 17 00:00:00 2001
From: Dreaded_X <tim.huizinga@gmail.com>
Date: Mon, 18 Jan 2021 17:58:30 +0100
Subject: [PATCH] Rewrote send and receive routines using coroutines

---
 src/keyboard.c | 275 +++++++++++++++++++++++++------------------------
 1 file changed, 140 insertions(+), 135 deletions(-)

diff --git a/src/keyboard.c b/src/keyboard.c
index 9e8c21c..e157985 100644
--- a/src/keyboard.c
+++ b/src/keyboard.c
@@ -1,4 +1,5 @@
 #include <avr/io.h>
+#include <stdint.h>
 #include <util/delay.h>
 
 #include "keyboard.h"
@@ -10,8 +11,7 @@ volatile struct {
 	struct FIFO buffer;
 
 	uint8_t sending : 1;
-	uint16_t value;
-	uint8_t counter;
+	uint8_t cmd;
 
 	struct {
 		uint8_t shift : 1;
@@ -22,7 +22,7 @@ volatile struct {
 	uint8_t released : 1;
 } keyboard = {0};
 
-// Send a command to the keyboard (BLOCKS)
+// Send a command to the keyboard (THIS BLOCKS)
 void send_keyboard_cmd(uint8_t value) {
 	// Pull clock low
 	DDRD |= (1<<KEYBOARD_CLK);
@@ -38,7 +38,7 @@ void send_keyboard_cmd(uint8_t value) {
 	DDRD &= ~(1<<KEYBOARD_CLK);
 
 	keyboard.sending = 1;
-	keyboard.value = value;
+	keyboard.cmd = value;
 
 	while (keyboard.sending) {};
 }
@@ -64,161 +64,166 @@ uint8_t parity(uint8_t value) {
 	return (~value) & 1;
 }
 
-void receive_scancode_bit(void (*callback)(uint8_t)) {
-	keyboard.value |= ((PIND >> KEYBOARD_DATA) & 1) << keyboard.counter;
-	keyboard.counter++;
+void process_scancode(uint8_t value, void (*callback)(uint8_t)) {
+	switch (value) {
+		// Left and right shift
+		case 18:
+		case 89:
+			keyboard.modifier.shift = !keyboard.released;
+			keyboard.released = 0;
+			break;
 
-	// Check that the first bit is 0
-	if (keyboard.counter == 1 && keyboard.value & 1) {
-		keyboard.counter = 0;
-		keyboard.value = 0;
+			// Caps lock
+		case 0x58:
+			if (!keyboard.released) {
+				keyboard.modifier.caps_lock = !keyboard.modifier.caps_lock;
 
-		return;
-	}
+				queue_keyboard_cmd(0xED);
+				queue_keyboard_cmd(keyboard.modifier.caps_lock << 2);
+			}
+			keyboard.released = 0;
+			break;
 
-	// Check the parity
-	if (keyboard.counter == 10 && parity(keyboard.value >> 1) != ((keyboard.value >> 9) & 1)) {
-		keyboard.counter = 0;
-		keyboard.value = 0;
+			// Left ctrl
+		case 0x14:
+			keyboard.modifier.ctrl = !keyboard.released;
+			keyboard.released = 0;
+			break;
 
-		return;
-	}
+		case 240:
+			keyboard.released = 1;
+			break;
 
-	// Check that the last bit is 1
-	if (keyboard.counter == 11 && !((keyboard.value >> 10) & 1)) {
-		keyboard.counter = 0;
-		keyboard.value = 0;
+		default:
+			if (!keyboard.released) {
+				char c = convert_scancode(keyboard.modifier.shift, value & 127);
 
-		return;
-	}
-
-	// Process the scancode
-	if (keyboard.counter == 11) {
-		uint8_t value = keyboard.value >> 1;
-
-		keyboard.counter = 0;
-		keyboard.value = 0;
-
-		switch (value) {
-			// Left and right shift
-			case 18:
-			case 89:
-				keyboard.modifier.shift = !keyboard.released;
-				keyboard.released = 0;
-				break;
-
-				// Caps lock
-			case 0x58:
-				if (!keyboard.released) {
-					keyboard.modifier.caps_lock = !keyboard.modifier.caps_lock;
-
-					queue_keyboard_cmd(0xED);
-					queue_keyboard_cmd(keyboard.modifier.caps_lock << 2);
+				if (keyboard.modifier.caps_lock && !keyboard.modifier.shift && c >= 'a' && c <= 'z') {
+					c -= 0x20;
 				}
-				keyboard.released = 0;
-				break;
 
-				// Left ctrl
-			case 0x14:
-				keyboard.modifier.ctrl = !keyboard.released;
-				keyboard.released = 0;
-				break;
+				if (keyboard.modifier.ctrl) {
+					switch (c) {
+						case 'c':
+							c = 0x03;
+							break;
 
-			case 240:
-				keyboard.released = 1;
-				break;
+						case 'e':
+							c = 0x05;
+							break;
 
-			default:
-				if (!keyboard.released) {
-					char c = convert_scancode(keyboard.modifier.shift, value & 127);
+						case 'p':
+							c = 0x10;
+							break;
 
-					if (keyboard.modifier.caps_lock && !keyboard.modifier.shift && c >= 'a' && c <= 'z') {
-						c -= 0x20;
-					}
+						case 'r':
+							c = 0x12;
+							break;
 
-					if (keyboard.modifier.ctrl) {
-						switch (c) {
-							case 'c':
-								c = 0x03;
-								break;
+						case 's':
+							c = 0x13;
+							break;
 
-							case 'e':
-								c = 0x05;
-								break;
+						case 'u':
+							c = 0x15;
+							break;
 
-							case 'p':
-								c = 0x10;
-								break;
+						case 'x':
+							c = 0x18;
+							break;
 
-							case 'r':
-								c = 0x12;
-								break;
+						case 'z':
+							c = 0x1A;
+							break;
 
-							case 's':
-								c = 0x13;
-								break;
-
-							case 'u':
-								c = 0x15;
-								break;
-
-							case 'x':
-								c = 0x18;
-								break;
-
-							case 'z':
-								c = 0x1A;
-								break;
-
-							default:
-								c = 0x00;
-								break;
-						}
-					}
-
-					if (c) {
-						callback(c);
+						default:
+							c = 0x00;
+							break;
 					}
 				}
-				keyboard.released = 0;
-				break;
-		}
+
+				if (c) {
+					callback(c);
+				}
+			}
+			keyboard.released = 0;
+			break;
 	}
 }
 
-void send_cmd_bit() {
-	if (keyboard.counter == 8) {
-		// Send parity
-		if (parity(keyboard.value)) {
-			PORTD |= (1<<KEYBOARD_DATA);
-		} else {
-			PORTD &= ~(1<<KEYBOARD_DATA);
-		}
+void receive_scancode_bit(void (*callback)(uint8_t)) {
+	uint8_t in = (PIND >> KEYBOARD_DATA) & 1;
 
-		++keyboard.counter;
-	} else if (keyboard.counter == 9) {
-		// Release data line
-		PORTD &= ~(1<<KEYBOARD_DATA);
-		DDRD &= ~(1<<KEYBOARD_DATA);
+	CO_BEGIN;
 
-		++keyboard.counter;
-	} else if (keyboard.counter == 10) {
-		// Wait for data to go low as ack
-		if (!((PIND >> KEYBOARD_DATA) & 1)) {
-			keyboard.counter = 0;
-			keyboard.value = 0;
-			keyboard.sending = 0;
-		}
-	} else {
-		// Send bit
-		if ((keyboard.value >> keyboard.counter) & 1) {
-			PORTD |= (1<<KEYBOARD_DATA);
-		} else {
-			PORTD &= ~(1<<KEYBOARD_DATA);
-		}
-
-		++keyboard.counter;
+	// Check that the first bit is 0
+	if (in != 0) {
+		CO_BREAK;
 	}
+
+	CO_YIELD;
+
+	static uint8_t value;
+	value = 0;
+
+	static int i = 0;
+	for (i = 0; i < 8; ++i) {
+		value |=  in << i;
+		CO_YIELD;
+	}
+
+	// Check the parity
+	if (in != parity(value)) {
+		CO_BREAK;
+	}
+
+	CO_YIELD;
+
+	// Check that the last bit is 1
+	if (in != 1) {
+		CO_BREAK;
+	}
+
+	process_scancode(value, callback);
+
+	CO_END;
+}
+
+void send_cmd_bit() {
+	CO_BEGIN;
+
+	static int i;
+	for (i = 0; i < 8; ++i) {
+		// Send bit
+		if ((keyboard.cmd >> i) & 1) {
+			PORTD |= (1<<KEYBOARD_DATA);
+		} else {
+			PORTD &= ~(1<<KEYBOARD_DATA);
+		}
+		CO_YIELD;
+	}
+
+	// Send parity
+	if (parity(keyboard.cmd)) {
+		PORTD |= (1<<KEYBOARD_DATA);
+	} else {
+		PORTD &= ~(1<<KEYBOARD_DATA);
+	}
+
+	CO_YIELD;
+
+	// Release data line
+	PORTD &= ~(1<<KEYBOARD_DATA);
+	DDRD &= ~(1<<KEYBOARD_DATA);
+
+	CO_YIELD;
+
+	// Wait for data to go low as ack
+	if (!((PIND >> KEYBOARD_DATA) & 1)) {
+		keyboard.sending = 0;
+	}
+
+	CO_END;
 }
 
 void keyboard_interrupt(void (*callback)(uint8_t)) {