1 files changed, 17 insertions, 22 deletions

diff --git a/converter/ibm4704_usb/matrix.c b/converter/ibm4704_usb/matrix.c
index 0bfda2b158..5f928d716f 100644
--- a/converter/ibm4704_usb/matrix.c
+++ b/converter/ibm4704_usb/matrix.c
@@ -69,41 +69,34 @@ static void enable_break(void)
 {
     uint8_t ret;
     print("Enable break: ");
-    // valid scancode: 00-77h
-    for (uint8_t code = 0; code < 0x78; code++) {
-        while (ibm4704_send(0x80|code) != 0) {
-            print("z");
-            _delay_us(500);
-        }
-        _delay_us(2000);
-        ret = ibm4704_recv();
-        if (ret != 0xff) {
+    // valid scancode: 00-79h
+    for (uint8_t code = 0; code < 0x7A; code++) {
+        while (ibm4704_send(0x80|code)) _delay_ms(1);
+        // get none when ok, get FD when out of bound
+        _delay_ms(5);
+        if ((ret = ibm4704_recv()) != 0xff) {
             xprintf("c%02X:r%02X ", code, ret);
         }
-        _delay_us(1000);
+        _delay_ms(1);
     }
     _delay_us(1000);
-    while (ibm4704_send(0xFF) != 0) { _delay_us(500); } // End
+    while (ibm4704_send(0xFF)) { _delay_ms(1); } // End
     print("End\n");
 }
 
 void matrix_init(void)
 {
-    uint8_t ret;
     debug_enable = true;
 
     ibm4704_init();
     matrix_clear();
 
-    // read keyboard id
-    while ((ret = ibm4704_recv()) == 0xFF) {
-        ibm4704_send(0xFE);
-        _delay_us(100);
-    }
+    _delay_ms(2000);    // wait for starting up debug console
 
-    _delay_ms(2000);    // wait for starting up debug console 
     print("IBM 4704 converter\n");
-    xprintf("Keyboard ID: %02X\n", ret);
+    while (ibm4704_send(0xFE)) _delay_ms(1);    // resend
+    _delay_ms(5);
+    xprintf("Keyboard ID: %02X\n", ibm4704_recv());
     enable_break();
 }
 
@@ -116,14 +109,16 @@ uint8_t matrix_scan(void)
     if (code==0xFF) {
         // Not receivd
         return 0;
-    } else if ((code&0x78)==0x78) {
-        // 0xFF-F8 and 0x7F-78 is not scancode
-        xprintf("Error: %0X\n", code);
+    } else if ((code&0x7F) >= 0x7A) {
+        // 0xFF-FA and 0x7F-7A is not scancode
+        xprintf("Error: %02X\n", code);
         matrix_clear();
         return 0;
     } else if (code&0x80) {
+        dprintf("%02X\n", code);
         matrix_make(code);
     } else {
+        dprintf("%02X\n", code);
         matrix_break(code);
     }
     return 1;