Fix ps2_host_recv_response

3 files changed, 12 insertions, 13 deletions

diff --git a/common/print.h b/common/print.h
index a828328b6..930e84be9 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 a4c27f5d5..aa0c38c68 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 1e2925889..5ab377877 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;