diff --git a/ps2.c b/ps2.c
index dd5b24129fa..f7aaf96e3fa 100644
--- a/ps2.c
+++ b/ps2.c
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010 Jun WAKO <wakojun@gmail.com>
+Copyright (c) 2010,2011 Jun WAKO <wakojun@gmail.com>
 
 This software is licensed with a Modified BSD License.
 All of this is supposed to be Free Software, Open Source, DFSG-free,
@@ -36,12 +36,13 @@ POSSIBILITY OF SUCH DAMAGE.
 */
 #include <stdbool.h>
 #include <avr/io.h>
+#include <avr/interrupt.h>
 #include <util/delay.h>
 #include "ps2.h"
-#include "print.h"
 #include "debug.h"
 
 
+static uint8_t recv_data(void);
 static inline void clock_lo(void);
 static inline void clock_hi(void);
 static inline bool clock_in(void);
@@ -52,6 +53,8 @@ static inline uint16_t wait_clock_lo(uint16_t us);
 static inline uint16_t wait_clock_hi(uint16_t us);
 static inline uint16_t wait_data_lo(uint16_t us);
 static inline uint16_t wait_data_hi(uint16_t us);
+static inline void idle(void);
+static inline void inhibit(void);
 
 
 /*
@@ -79,38 +82,38 @@ http://www.mcamafia.de/pdf/ibm_hitrc07.pdf
     } \
 } while (0)
 
-#define WAIT_NORETRY(stat, us, err) do { \
-    if (!wait_##stat(us)) { \
-        ps2_error = err; \
-        return 0; \
-    } \
-} while (0)
-
 
 uint8_t ps2_error = PS2_ERR_NONE;
 
 
 void ps2_host_init(void)
 {
-    /* inhibit */
-    clock_lo();
-    data_hi();
+#ifdef PS2_INT_ENABLE
+    PS2_INT_ENABLE();
+    idle();
+#else
+    inhibit();
+#endif
 }
 
 uint8_t ps2_host_send(uint8_t data)
 {
-    bool parity = true;
+    bool parity;
+RETRY:
+    parity = true;
     ps2_error = 0;
 
-    /* request to send */
-    clock_lo();
+    /* terminate a transmission if we have */
+    inhibit();
     _delay_us(100);
+
     /* start bit [1] */
     data_lo();
     clock_hi();
     WAIT(clock_lo, 15000, 1);
     /* data [2-9] */
     for (uint8_t i = 0; i < 8; i++) {
+        _delay_us(15);
         if (data&(1<<i)) {
             parity = !parity;
             data_hi();
@@ -121,44 +124,145 @@ uint8_t ps2_host_send(uint8_t data)
         WAIT(clock_lo, 50, 3);
     }
     /* parity [10] */
+    _delay_us(15);
     if (parity) { data_hi(); } else { data_lo(); }
     WAIT(clock_hi, 50, 4);
     WAIT(clock_lo, 50, 5);
     /* stop bit [11] */
+    _delay_us(15);
     data_hi();
     /* ack [12] */
     WAIT(data_lo, 50, 6);
     WAIT(clock_lo, 50, 7);
+
+    /* wait for idle state */
     WAIT(clock_hi, 50, 8);
     WAIT(data_hi, 50, 9);
 
-    /* inhibit device to send */
-    clock_lo();
+    uint8_t res = ps2_host_recv_response();
+    if (res == 0xFE && data != 0xFE)
+        goto RETRY;
 
-    return 1;
+    inhibit();
+    return res;
 ERROR:
-    /* inhibit device to send */
-    data_hi();
-    clock_lo();
+    inhibit();
     return 0;
 }
 
+/* receive data when host want else inhibit communication */
+uint8_t ps2_host_recv_response(void)
+{
+    uint8_t data = 0;
+
+    /* terminate a transmission if we have */
+    inhibit();
+    _delay_us(100);
+
+    /* release lines(idle state) */
+    idle();
+
+    /* wait start bit */
+    wait_clock_lo(2000);
+    data = recv_data();
+
+    inhibit();
+    return data;
+}
+
+#ifndef PS2_INT_VECT
 uint8_t ps2_host_recv(void)
+{
+    return ps2_host_recv_response();
+}
+#else
+/* ring buffer to store ps/2 key data */
+#define PBUF_SIZE 8
+static uint8_t pbuf[PBUF_SIZE];
+static uint8_t pbuf_head = 0;
+static uint8_t pbuf_tail = 0;
+static inline void pbuf_enqueue(uint8_t data)
+{
+    if (!data)
+        return;
+    uint8_t next = (pbuf_head + 1) % PBUF_SIZE;
+    if (next != pbuf_tail) {
+        pbuf[pbuf_head] = data;
+        pbuf_head = next;
+    } else {
+        print("pbuf: full\n");
+    }
+}
+static inline uint8_t pbuf_dequeue(void)
+{
+    uint8_t val = 0;
+    uint8_t sreg = SREG;
+    cli();
+    if (pbuf_head != pbuf_tail) {
+        val = pbuf[pbuf_tail];
+        pbuf_tail = (pbuf_tail + 1) % PBUF_SIZE;
+    }
+    SREG = sreg;
+    return val;
+}
+
+/* get data received by interrupt */
+uint8_t ps2_host_recv(void)
+{
+    return pbuf_dequeue();
+}
+
+ISR(PS2_INT_VECT)
+{
+PORTC = 0xFF;
+    /* interrupt means start bit comes */
+    pbuf_enqueue(recv_data());
+
+    /* release lines(idle state) */
+    idle();
+    _delay_us(5);
+PORTC = 0x00;
+}
+#endif
+
+
+/*
+static void ps2_reset(void)
+{
+    ps2_host_send(0xFF);
+    if (ps2_host_recv_response() == 0xFA) {
+        _delay_ms(1000);
+        ps2_host_recv_response();
+    }
+}
+*/
+
+/* send LED state to keyboard */
+void ps2_host_set_led(uint8_t led)
+{
+#ifdef PS2_INT_DISABLE
+    PS2_INT_DISABLE();
+#endif
+    ps2_host_send(0xED);
+    ps2_host_recv_response();
+    ps2_host_send(led);
+    ps2_host_recv_response();
+#ifdef PS2_INT_ENABLE
+    PS2_INT_ENABLE();
+    idle();
+#endif
+}
+
+
+/* called after start bit comes */
+static uint8_t recv_data(void)
 {
     uint8_t data = 0;
     bool parity = true;
     ps2_error = 0;
 
-    /* terminate a transmission if we have */
-    clock_lo();
-    _delay_us(100);
-
-    /* release lines(idle state) */
-    clock_hi();
-    data_hi();
-
     /* start bit [1] */
-    WAIT(clock_lo, 2000, 1);    // How long should we wait?
+    WAIT(clock_lo, 1, 1);
     WAIT(data_lo, 1, 2);
     WAIT(clock_hi, 50, 3);
 
@@ -185,18 +289,11 @@ uint8_t ps2_host_recv(void)
     WAIT(data_hi, 1, 9);
     WAIT(clock_hi, 50, 10);
 
-    /* inhibit device to send */
-    clock_lo();
-
     return data;
 ERROR:
-    /* inhibit device to send */
-    data_hi();
-    clock_lo();
     return 0;
 }
 
-
 static inline void clock_lo()
 {
     PS2_CLOCK_PORT &= ~(1<<PS2_CLOCK_BIT);
@@ -252,3 +349,17 @@ static inline uint16_t wait_data_hi(uint16_t us)
     while (!data_in() && us)  { asm(""); _delay_us(1); us--; }
     return us;
 }
+
+/* idle state that device can send */
+static inline void idle(void)
+{
+    clock_hi();
+    data_hi();
+}
+
+/* inhibit device to send */
+static inline void inhibit(void)
+{
+    clock_lo();
+    data_hi();
+}
diff --git a/ps2.h b/ps2.h
index 5ae28a95cf6..7a8bbd2665f 100644
--- a/ps2.h
+++ b/ps2.h
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010 Jun WAKO <wakojun@gmail.com>
+Copyright (c) 2010,2011 Jun WAKO <wakojun@gmail.com>
 
 This software is licensed with a Modified BSD License.
 All of this is supposed to be Free Software, Open Source, DFSG-free,
@@ -66,11 +66,13 @@ POSSIBILITY OF SUCH DAMAGE.
 
 extern uint8_t ps2_error;
 
-/* host side */
+/* host role */
 void ps2_host_init(void);
-uint8_t ps2_host_send(uint8_t);
+uint8_t ps2_host_send(uint8_t data);
+uint8_t ps2_host_recv_response(void);
 uint8_t ps2_host_recv(void);
+void ps2_host_set_led(uint8_t usb_led);
 
-/* TODO: device side */
+/* device role */
 
 #endif
diff --git a/ps2_vusb/config.h b/ps2_vusb/config.h
index 1d2a2830713..639a1ac7199 100644
--- a/ps2_vusb/config.h
+++ b/ps2_vusb/config.h
@@ -23,14 +23,38 @@
 #   define MOUSEKEY_DELAY_TIME 255
 #endif
 
-/* PS/2 mouse */
+/* PS/2 lines */
 #define PS2_CLOCK_PORT  PORTD
 #define PS2_CLOCK_PIN   PIND
 #define PS2_CLOCK_DDR   DDRD
-#define PS2_CLOCK_BIT   6
+#define PS2_CLOCK_BIT   3
 #define PS2_DATA_PORT   PORTD
 #define PS2_DATA_PIN    PIND
 #define PS2_DATA_DDR    DDRD
 #define PS2_DATA_BIT    7
 
+/* External interrupt for PS/2 clock line (optional) */
+#define PS2_INT_ENABLE()  do {  \
+    EIMSK |= (1<<INT1);         \
+    EICRA |= ((1<<ISC11) | (0<<ISC10)); \
+    EIFR |= (1<<INTF1);         \
+} while (0)
+#define PS2_INT_DISABLE() do {  \
+    EIMSK &= ~(1<<INT1);        \
+} while (0)
+#define PS2_INT_VECT    INT1_vect
+
+/* Pin Change interrupt for PS/2 clock line (optional)
+#define PS2_INT_ENABLE()  do {  \
+    PCMSK2 |= (1<<PCINT22);     \
+    PCICR |= (1<<PCIE2);        \
+    PCIFR |= (1<<PCIF2);        \
+} while (0)
+#define PS2_INT_DISABLE() do {  \
+    PCMSK2 &= ~(1<<PCINT22);    \
+    PCICR &= ~(1<<PCIE);        \
+} while (0)
+#define PS2_INT_VECT    PCINT2_vect
+*/
+
 #endif
diff --git a/ps2_vusb/host.h b/ps2_vusb/host.h
index b4b9eefbe19..fa7e79910bc 100644
--- a/ps2_vusb/host.h
+++ b/ps2_vusb/host.h
@@ -29,6 +29,7 @@ typedef struct {
 } report_mouse_t;
 
 
+extern uint8_t host_keyboard_led;
 void host_keyboard_send(report_keyboard_t *report);
 void host_mouse_send(report_mouse_t *report);
 
diff --git a/ps2_vusb/host_vusb.c b/ps2_vusb/host_vusb.c
index cda9048789c..060d23c1867 100644
--- a/ps2_vusb/host_vusb.c
+++ b/ps2_vusb/host_vusb.c
@@ -6,7 +6,7 @@
 #include "host_vusb.h"
 
 
-#define KBUF_SIZE 8
+#define KBUF_SIZE 16
 static report_keyboard_t kbuf[KBUF_SIZE];
 static uint8_t kbuf_head = 0;
 static uint8_t kbuf_tail = 0;
@@ -14,12 +14,18 @@ static uint8_t kbuf_tail = 0;
 
 void host_vusb_keyboard_send()
 {
+    while (usbInterruptIsReady() && kbuf_head != kbuf_tail) {
+        usbSetInterrupt((void *)&kbuf[kbuf_tail], sizeof(report_keyboard_t));
+        kbuf_tail = (kbuf_tail + 1) % KBUF_SIZE;
+    }
+/*
     if (kbuf_head != kbuf_tail) {
         if (usbInterruptIsReady()) {
             usbSetInterrupt((void *)&kbuf[kbuf_tail], sizeof(report_keyboard_t));
             kbuf_tail = (kbuf_tail + 1) % KBUF_SIZE;
         }
     }
+*/
 }
 
 void host_keyboard_send(report_keyboard_t *report)
@@ -28,14 +34,20 @@ void host_keyboard_send(report_keyboard_t *report)
     if (next != kbuf_tail) {
         kbuf[kbuf_head] = *report;
         kbuf_head = next;
+        print("kbuf: "); phex(kbuf_head); phex(kbuf_tail); print("\n");
+    } else {
+        print("kbuf: full\n");
+        // hmm...
+        /*
+        matrix_init();
+        kbuf_head = 0;
+        kbuf_tail = 0;
+        */
     }
 }
 
 void host_mouse_send(report_mouse_t *report)
 {
-    // dirty hack to send twice a loop :(
-    //while (!usbInterruptIsReady3()) usbPoll();
-
     if (usbInterruptIsReady3()) {
         usbSetInterrupt3((void *)report, sizeof(*report));
     } else {
@@ -46,30 +58,51 @@ void host_mouse_send(report_mouse_t *report)
 
 
 
+static struct {
+    uint16_t        len;
+    enum {
+        NONE,
+        SET_LED
+    }               kind;
+} last_req;
+
+uint8_t host_keyboard_led = 0;
+static uchar    idleRate;
 
-static uchar    idleRate;   /* repeat rate for keyboards, never used for mice */
 usbMsgLen_t usbFunctionSetup(uchar data[8])
 {
 usbRequest_t    *rq = (void *)data;
 
-    print("Setup: ");
+    //print("Setup: ");
     if((rq->bmRequestType & USBRQ_TYPE_MASK) == USBRQ_TYPE_CLASS){    /* class request type */
+        /*
         print("CLASS: ");
-        phex(rq->bRequest);
+        phex(rq->bRequest); print(" ");
+        phex16(rq->wValue.word); print(" ");
+        phex16(rq->wIndex.word); print(" ");
+        phex16(rq->wLength.word); print(" ");
+        */
         if(rq->bRequest == USBRQ_HID_GET_REPORT){
-            print("GET_REPORT");
+            print(" GET_REPORT");
             /* we only have one report type, so don't look at wValue */
             usbMsgPtr = (void *)keyboard_report;
             return sizeof(*keyboard_report);
         }else if(rq->bRequest == USBRQ_HID_GET_IDLE){
-            print("GET_IDLE: ");
+            print(" GET_IDLE: ");
             phex(idleRate);
             usbMsgPtr = &idleRate;
             return 1;
         }else if(rq->bRequest == USBRQ_HID_SET_IDLE){
             idleRate = rq->wValue.bytes[1];
-            print("SET_IDLE: ");
+            print(" SET_IDLE: ");
             phex(idleRate);
+        }else if(rq->bRequest == USBRQ_HID_SET_REPORT){
+            //print(" SET_REPORT: ");
+            if (rq->wValue.word == 0x0200 && rq->wIndex.word == 0) {
+                last_req.kind = SET_LED;
+                last_req.len = rq->wLength.word;
+            }
+            return USB_NO_MSG; // to get data in usbFunctionWrite
         }
         print("\n");
     }else{
@@ -79,6 +112,26 @@ usbRequest_t    *rq = (void *)data;
     return 0;   /* default for not implemented requests: return no data back to host */
 }
 
+uchar usbFunctionWrite(uchar *data, uchar len)
+{
+    if (last_req.len == 0) {
+        return -1;
+    }
+    switch (last_req.kind) {
+        case SET_LED:
+            //print("SET_LED\n");
+            host_keyboard_led = data[0];
+            last_req.len = 0;
+            return 1;
+            break;
+        case NONE:
+        default:
+            return -1;
+            break;
+    }
+    return 1;
+}
+
 
 PROGMEM uchar keyboard_hid_report[] = {
     0x05, 0x01,          // Usage Page (Generic Desktop),
diff --git a/ps2_vusb/host_vusb.h b/ps2_vusb/host_vusb.h
index c9b1d77671f..f09ad58228a 100644
--- a/ps2_vusb/host_vusb.h
+++ b/ps2_vusb/host_vusb.h
@@ -4,4 +4,3 @@
 void host_vusb_keyboard_send(void);
 
 #endif
-
diff --git a/ps2_vusb/keyboard.c b/ps2_vusb/keyboard.c
index 95f65887f88..c480908f847 100644
--- a/ps2_vusb/keyboard.c
+++ b/ps2_vusb/keyboard.c
@@ -1,12 +1,30 @@
 #include "usb_keycodes.h"
 #include "host.h"
+#include "ps2.h"
+#include "usb.h"
 #include "keyboard.h"
+#include "print.h"
 
 static report_keyboard_t report0;
 static report_keyboard_t report1;
 static report_keyboard_t *report = &report0;
 static report_keyboard_t *report_prev = &report1;
 
+
+void keyboard_set_led(uint8_t usb_led)
+{
+    uint8_t ps2_led = 0;
+    if (usb_led & (1<<USB_LED_SCROLL_LOCK))
+        ps2_led |= (1<<PS2_LED_SCROLL_LOCK);
+    if (usb_led & (1<<USB_LED_NUM_LOCK))
+        ps2_led |= (1<<PS2_LED_NUM_LOCK);
+    if (usb_led & (1<<USB_LED_CAPS_LOCK))
+        ps2_led |= (1<<PS2_LED_CAPS_LOCK);
+    print("ps2_led: "); phex(ps2_led); print("\n");
+
+    ps2_host_set_led(ps2_led);
+}
+
 void keyboard_send(void)
 {
     host_keyboard_send(report);
diff --git a/ps2_vusb/keyboard.h b/ps2_vusb/keyboard.h
index bc6b2149370..d8604bb9569 100644
--- a/ps2_vusb/keyboard.h
+++ b/ps2_vusb/keyboard.h
@@ -6,6 +6,7 @@
 #include "host.h"
 
 
+void keyboard_set_led(uint8_t led);
 void keyboard_send(void);
 bool keyboard_has_key(void);
 void keyboard_add_mod(uint8_t mod);
diff --git a/ps2_vusb/main.c b/ps2_vusb/main.c
index e692924e794..16dea9aaaf0 100644
--- a/ps2_vusb/main.c
+++ b/ps2_vusb/main.c
@@ -28,9 +28,13 @@
 #include "host_vusb.h"
 #include "timer.h"
 
+#define DEBUGP_INIT() do { DDRC = 0xFF; } while (0)
+#define DEBUGP(x) do { PORTC = x; } while (0)
 
+static uint8_t last_led = 0;
 int main(void)
 {
+    DEBUGP_INIT();
     wdt_enable(WDTO_1S);
     /* Even if you don't use the watchdog, turn it off here. On newer devices,
      * the status of the watchdog (on/off, period) is PRESERVED OVER RESET!
@@ -60,10 +64,12 @@ int main(void)
 
     uint8_t fn_bits = 0;
     while (1) {                /* main event loop */
+        DEBUGP(0x01);
         wdt_reset();
         usbPoll();
         host_vusb_keyboard_send();
 
+        DEBUGP(0x02);
         matrix_scan();
         fn_bits = 0;
         keyboard_swap_report();
@@ -94,10 +100,16 @@ int main(void)
                 }
             }
         }
+        DEBUGP(0x03);
         layer_switching(fn_bits);
         if (matrix_is_modified()) {
             keyboard_send();
         }
         mousekey_send();
+
+        if (last_led != host_keyboard_led) {
+            keyboard_set_led(host_keyboard_led);
+            last_led = host_keyboard_led;
+        }
     }
 }
diff --git a/ps2_vusb/matrix.c b/ps2_vusb/matrix.c
index bd9b92446cf..f04f6b7674d 100644
--- a/ps2_vusb/matrix.c
+++ b/ps2_vusb/matrix.c
@@ -60,8 +60,6 @@ static bool matrix_has_ghost_in_row(uint8_t row);
 #endif
 static void matrix_make(uint8_t code);
 static void matrix_break(uint8_t code);
-static void ps2_reset(void);
-static void ps2_set_leds(uint8_t leds);
 
 
 inline
@@ -79,19 +77,7 @@ uint8_t matrix_cols(void)
 void matrix_init(void)
 {
     ps2_host_init();
-    _delay_ms(1000);
 
-    // flush LEDs
-/*
-    ps2_set_leds(1<<PS2_LED_NUM_LOCK);
-    _delay_ms(100);
-    ps2_set_leds(1<<PS2_LED_NUM_LOCK|1<<PS2_LED_CAPS_LOCK);
-    _delay_ms(100);
-    ps2_set_leds(1<<PS2_LED_NUM_LOCK|1<<PS2_LED_CAPS_LOCK|1<<PS2_LED_SCROLL_LOCK);
-    _delay_ms(300);
-    ps2_set_leds(0x00);
-*/
-    
     // initialize matrix state: all keys off
     for (uint8_t i=0; i < MATRIX_ROWS; i++) matrix[i] = 0x00;
 
@@ -190,10 +176,7 @@ uint8_t matrix_scan(void)
     }
 
     uint8_t code;
-    code = ps2_host_recv();
-    if (code == 0x00) return 0;
-    //while ((code = ps2_host_recv())) {
-//phex(code); print(" ");
+    while ((code = ps2_host_recv())) {
         switch (state) {
             case INIT:
                 switch (code) {
@@ -350,26 +333,7 @@ uint8_t matrix_scan(void)
             default:
                 state = INIT;
         }
-    //}
-//print("|");
-
-    // handle LED indicators
-/*
-    static uint8_t prev_leds = 0;
-    if (prev_leds != usb_keyboard_leds) {
-        uint8_t leds = 0;
-        if (usb_keyboard_leds&(1<<USB_LED_SCROLL_LOCK))
-            leds |= (1<<PS2_LED_SCROLL_LOCK);
-        if (usb_keyboard_leds&(1<<USB_LED_NUM_LOCK))
-            leds |= (1<<PS2_LED_NUM_LOCK);
-        if (usb_keyboard_leds&(1<<USB_LED_CAPS_LOCK))
-            leds |= (1<<PS2_LED_CAPS_LOCK);
-
-        ps2_set_leds(leds);
-        prev_leds = usb_keyboard_leds;
     }
-*/
-
     return 1;
 }
 
@@ -479,19 +443,3 @@ static void matrix_break(uint8_t code)
         //print("matrix_break: "); phex(code); print("\n");
     }
 }
-
-static void ps2_reset(void)
-{
-    ps2_host_send(0xFF);
-    if (ps2_host_recv() != 0xFA) return;
-    _delay_ms(1000);
-    if (ps2_host_recv() != 0xAA) return;
-}
-
-static void ps2_set_leds(uint8_t leds)
-{
-        ps2_host_send(0xED);
-        if (ps2_host_recv() != 0xFA) return;        // 0xFA
-        ps2_host_send(leds);
-        if (ps2_host_recv() != 0xFA) return;        // 0xFA
-}
diff --git a/ps2_vusb/usbconfig.h b/ps2_vusb/usbconfig.h
index 84fc6fd3fb3..abc31ced54b 100644
--- a/ps2_vusb/usbconfig.h
+++ b/ps2_vusb/usbconfig.h
@@ -121,7 +121,7 @@ section at the end of this file).
  * The value is in milliamperes. [It will be divided by two since USB
  * communicates power requirements in units of 2 mA.]
  */
-#define USB_CFG_IMPLEMENT_FN_WRITE      0
+#define USB_CFG_IMPLEMENT_FN_WRITE      1
 /* Set this to 1 if you want usbFunctionWrite() to be called for control-out
  * transfers. Set it to 0 if you don't need it and want to save a couple of
  * bytes.