1 files changed, 53 insertions, 35 deletions

diff --git a/ardkbd.c b/ardkbd.c
index 0b62bda..02c23a5 100644
--- a/ardkbd.c
+++ b/ardkbd.c
@@ -36,41 +36,58 @@ static inline bool kbd_clock() {
 }
 
 
+static inline void kbd_clock_down() {
+	DDRC |= 0x02;
+	PORTC &= ~0x02;
+}
+
+static inline void kbd_clock_in() {
+	DDRC &= ~0x02;
+	PORTC |= 0x02;
+}
+
+static inline void kbd_data_set(bool state) {
+	if (state)
+		PORTC |= 0x01;
+	else
+		PORTC &= ~0x01;
+}
+
+static inline void kbd_data_out() {
+	DDRC |= 0x01;
+}
+
+static inline void kbd_data_in() {
+	DDRC &= ~0x01;
+	PORTC |= 0x01;
+}
+
+
+static inline void kbd_wait() {
+	while (kbd_state) {}
+}
+
+
 ISR(PCINT1_vect) {
 	if (kbd_clock())
 		return;
 
 	if (kbd_state < 0) {
 		if (kbd_state >= -8) {
-			if (kbd_output & (1 << (-1-kbd_state)))
-				PORTC |= 0x01;
-			else
-				PORTC &= ~0x01;
-
-			kbd_state--;
-			return;
+			kbd_data_set(kbd_output & _BV(-1-kbd_state));
 		}
-
-		if (kbd_state == -9) {
-			if ((__builtin_popcount(kbd_output) & 1) == 0)
-				PORTC |= 0x01;
-			else
-				PORTC &= ~0x01;
-
-			kbd_state--;
-			return;
+		else if (kbd_state == -9) {
+			kbd_data_set(!(__builtin_popcount(kbd_output) & 1));
 		}
-
-		if (kbd_state == -10) {
-			kbd_state--;
-
-			DDRC &= ~0x01;
-			PORTC |= 0x01;
+		else if (kbd_state == -10) {
+			kbd_data_in();
+		}
+		else {
+			kbd_state = 0;
 			return;
 		}
 
-		kbd_state = 0;
-
+		kbd_state--;
 		return;
 	}
 
@@ -86,7 +103,8 @@ ISR(PCINT1_vect) {
 	}
 
 	if (kbd_state <= 8) {
-		kbd_input |= (data << (kbd_state-1));
+		if (data)
+			kbd_input |= _BV(kbd_state-1);
 		kbd_state++;
 		return;
 	}
@@ -139,26 +157,26 @@ ISR(PCINT1_vect) {
 
 
 void kbd_send(uint8_t command) {
-	while (kbd_state) {} /* wait for idle */
+	kbd_wait();
 
-	DDRC |= 0x02;
-	PORTC &= ~0x02;
+	kbd_clock_down();
 	_delay_us(100);
 
-	DDRC |= 0x01;
-	PORTC &= ~0x01;
+	kbd_data_out();
+	kbd_data_set(false);
 	_delay_us(10);
 
-	DDRC &= ~0x02;
-	PORTC |= 0x02;
+	kbd_clock_in();
 
 	kbd_output = command;
 	kbd_state = -1;
 
-	/* wait for idle */
-	while (kbd_state) {}
+	kbd_wait();
+
+	/* wait for ack */
 	while (!kbd_state) {}
-	while (kbd_state) {}
+
+	kbd_wait();
 }