From 9d26053f1c14da79336a64f800305660d1a71180 Mon Sep 17 00:00:00 2001
From: tmk <nobody@nowhere>
Date: Tue, 26 Nov 2013 14:31:57 +0900
Subject: [PATCH] Fix ps2_host_recv_response

---
 common/print.h             |  4 ----
 converter/ps2_usb/matrix.c | 10 +++++-----
 protocol/ps2_busywait.c    | 11 +++++++----
 3 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/common/print.h b/common/print.h
index a828328b..930e84be 100644
--- a/common/print.h
+++ b/common/print.h
@@ -40,10 +40,6 @@
 #endif
 #define println(s)              print_P(PSTR(s "\n"))
 
-#ifndef AVR_LIBC_PRINTF
-#define printf(f, ...)          xprintf(f, ##__VA_ARGS__)
-#endif
-
 /* for old name */
 #define pdec(data)              print_dec(data)
 #define pdec16(data)            print_dec(data)
diff --git a/converter/ps2_usb/matrix.c b/converter/ps2_usb/matrix.c
index a4c27f5d..aa0c38c6 100644
--- a/converter/ps2_usb/matrix.c
+++ b/converter/ps2_usb/matrix.c
@@ -217,7 +217,7 @@ uint8_t matrix_scan(void)
                         if (code < 0x80) {
                             matrix_make(code);
                         } else {
-                            printf("unexpected scan code at INIT: %02X\n", code);
+                            xprintf("unexpected scan code at INIT: %02X\n", code);
                             clear_keyboard();
                         }
                         state = INIT;
@@ -239,7 +239,7 @@ uint8_t matrix_scan(void)
                         if (code < 0x80) {
                             matrix_make(code|0x80);
                         } else {
-                            printf("unexpected scan code at E0: %02X\n", code);
+                            xprintf("unexpected scan code at E0: %02X\n", code);
                             clear_keyboard();
                         }
                         state = INIT;
@@ -259,7 +259,7 @@ uint8_t matrix_scan(void)
                     if (code < 0x80) {
                         matrix_break(code);
                     } else {
-                        printf("unexpected scan code at F0: %02X\n", code);
+                        xprintf("unexpected scan code at F0: %02X\n", code);
                         clear_keyboard();
                     }
                     state = INIT;
@@ -275,7 +275,7 @@ uint8_t matrix_scan(void)
                         if (code < 0x80) {
                             matrix_break(code|0x80);
                         } else {
-                            printf("unexpected scan code at E0_F0: %02X\n", code);
+                            xprintf("unexpected scan code at E0_F0: %02X\n", code);
                             clear_keyboard();
                         }
                         state = INIT;
@@ -371,7 +371,7 @@ uint8_t matrix_scan(void)
 
     if (ps2_error > PS2_ERR_STARTBIT3) {
         uint8_t ret = ps2_host_send(PS2_RESEND);
-        printf("Resend: %02X\n", ret);
+        xprintf("Resend: %02X\n", ret);
     }
     return 1;
 }
diff --git a/protocol/ps2_busywait.c b/protocol/ps2_busywait.c
index 1e292588..5ab37787 100644
--- a/protocol/ps2_busywait.c
+++ b/protocol/ps2_busywait.c
@@ -104,6 +104,7 @@ uint8_t ps2_host_send(uint8_t data)
     WAIT(clock_hi, 50, 8);
     WAIT(data_hi, 50, 9);
 
+    inhibit();
     res = ps2_host_recv_response();
 ERROR:
     inhibit();
@@ -113,12 +114,14 @@ ERROR:
 /* receive data when host want else inhibit communication */
 uint8_t ps2_host_recv_response(void)
 {
-    // TODO:
     // Command might take 20ms to response([3]p.21)
     // TrackPoint might take 25ms ([5]2.7)
+    // 250 * 100us(wait for start bit in ps2_host_recv)
     uint8_t data = 0;
-    uint8_t try = 200;
-    while (try-- && (data = ps2_host_recv())) ;
+    uint8_t try = 250;
+    do {
+        data = ps2_host_recv();
+    } while (try-- && ps2_error);
     return data;
 }
 
@@ -172,7 +175,7 @@ uint8_t ps2_host_recv(void)
     return data;
 ERROR:
     if (ps2_error > PS2_ERR_STARTBIT3) {
-        printf("x%02X\n", ps2_error);
+        xprintf("x%02X\n", ps2_error);
     }
     inhibit();
     return 0;