diff --git a/keyboard/kimera/config.h b/keyboard/kimera/config.h
index 2ae301f6..d85e789b 100644
--- a/keyboard/kimera/config.h
+++ b/keyboard/kimera/config.h
@@ -90,6 +90,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define NO_ACTION_MACRO
 //#define NO_ACTION_FUNCTION
 
-#define NO_SUSPEND_POWER_DOWN
+//#define NO_SUSPEND_POWER_DOWN
 
 #endif
diff --git a/keyboard/kimera/i2c_wrapper.c b/keyboard/kimera/i2c_wrapper.c
index 07f87838..9ab9a967 100644
--- a/keyboard/kimera/i2c_wrapper.c
+++ b/keyboard/kimera/i2c_wrapper.c
@@ -18,78 +18,22 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include <avr/interrupt.h>
 #include <avr/wdt.h>
 #include <util/delay.h>
+#include "timer.h"
 #include "i2cmaster.h"
 #include "i2c_wrapper.h"
 #include "debug.h"
 
-#define wdt_intr_enable(value)   \
-__asm__ __volatile__ (  \
-    "in __tmp_reg__,__SREG__" "\n\t"    \
-    "cli" "\n\t"    \
-    "wdr" "\n\t"    \
-    "sts %0,%1" "\n\t"  \
-    "out __SREG__,__tmp_reg__" "\n\t"   \
-    "sts %0,%2" "\n\t" \
-    : /* no outputs */  \
-    : "M" (_SFR_MEM_ADDR(_WD_CONTROL_REG)), \
-    "r" (_BV(_WD_CHANGE_BIT) | _BV(WDE)), \
-    "r" ((uint8_t) ((value & 0x08 ? _WD_PS3_MASK : 0x00) | \
-        _BV(WDIE) | (value & 0x07)) ) \
-    : "r0"  \
-)
-
-#define SCL_CLOCK       400000L
-#define SCL_DURATION    (1000000L/SCL_CLOCK)/2
-
-static uint8_t i2c_wdt_enabled = 0;
-
-extern uint8_t i2c_force_stop;
-
-static void wdt_init(void);
-
 void i2c_wrapper_init(void)
 {
+    dprintf("I2C Init\n");
+
+    /* init timer */
+    timer_init();
+
     /* init i2c */
     i2c_init();
-
-    /* init watch dog */
-    wdt_init();
 }
 
 void i2c_wrapper_task(void)
 {
-    /* reset watch dog counter */
-    wdt_reset();
-}
-
-void wdt_init(void)
-{
-    cli();
-    wdt_reset();
-    wdt_intr_enable(WDTO_2S);
-    sei();
-}
-
-ISR(WDT_vect)
-{
-    xprintf("i2c timeout\n");
-
-    /* let slave to release SDA */
-    TWCR = 0;
-    DDRD |=  (1<<PD0);
-    DDRD &= ~(1<<PD1);
-    if (!(PIND & (1<<PD1))) {
-        for (uint8_t i = 0; i < 9; i++) {
-            PORTD &= ~(1<<PD0);
-            _delay_us(SCL_DURATION);
-            PORTD |= (1<<PD0);
-            _delay_us(SCL_DURATION);
-        }
-    }
-
-    /* send stop condition */
-    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
-
-    /* escape from loop */
-    i2c_force_stop = 1;
 }
diff --git a/keyboard/kimera/twimaster.c b/keyboard/kimera/twimaster.c
index d55386c5..d7b613d6 100644
--- a/keyboard/kimera/twimaster.c
+++ b/keyboard/kimera/twimaster.c
@@ -8,8 +8,10 @@
  **************************************************************************/
 #include <inttypes.h>
 #include <compat/twi.h>
+#include <util/delay.h>
 
 #include <i2cmaster.h>
+#include "timer.h"
 #include "debug.h"
 
 
@@ -20,9 +22,30 @@
 
 /* I2C clock in Hz */
 #define SCL_CLOCK  400000L
+#define SCL_DURATION    (1000000L/SCL_CLOCK)/2
 
 volatile uint8_t i2c_force_stop = 0;
+#define TIMEOUT 3000
 #define CHECK_FORCE_STOP() if(i2c_force_stop){i2c_force_stop=0;break;}
+#define CHECK_TIMEOUT_PRE() \
+    uint16_t start; \
+    uint8_t once = 1;
+#define CHECK_TIMEOUT_PRE2() \
+    once = 1;
+#define CHECK_TIMEOUT(retval) { \
+    if (once) { \
+        start = timer_read(); \
+        once = 0; \
+    } \
+    else { \
+        if (timer_elapsed(start) >= TIMEOUT) { \
+            i2c_forceStop(); \
+            return retval; \
+        } \
+    } \
+}
+
+static void i2c_forceStop(void);
 
 /*************************************************************************
   Initialization of the I2C bus interface. Need to be called only once
@@ -49,7 +72,8 @@ unsigned char i2c_start(unsigned char address)
     TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
 
     // wait until transmission completed
-    while(!(TWCR & (1<<TWINT))) { CHECK_FORCE_STOP(); };
+    CHECK_TIMEOUT_PRE();
+    while(!(TWCR & (1<<TWINT))) { CHECK_TIMEOUT(2); };
 
     // check value of TWI Status Register. Mask prescaler bits.
     twst = TW_STATUS & 0xF8;
@@ -60,7 +84,8 @@ unsigned char i2c_start(unsigned char address)
     TWCR = (1<<TWINT) | (1<<TWEN);
 
     // wail until transmission completed and ACK/NACK has been received
-    while(!(TWCR & (1<<TWINT))) { CHECK_FORCE_STOP(); };
+    CHECK_TIMEOUT_PRE2();
+    while(!(TWCR & (1<<TWINT))) { CHECK_TIMEOUT(2); };
 
     // check value of TWI Status Register. Mask prescaler bits.
     twst = TW_STATUS & 0xF8;
@@ -109,7 +134,8 @@ void i2c_start_wait(unsigned char address)
             TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
 
             // wait until stop condition is executed and bus released
-            while(TWCR & (1<<TWSTO)) { CHECK_FORCE_STOP(); };
+            CHECK_TIMEOUT_PRE();
+            while(TWCR & (1<<TWSTO)) { CHECK_TIMEOUT(); };
 
             continue;
         }
@@ -144,7 +170,8 @@ void i2c_stop(void)
     TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
 
     // wait until stop condition is executed and bus released
-    while(TWCR & (1<<TWSTO)) { CHECK_FORCE_STOP(); };
+    CHECK_TIMEOUT_PRE();
+    while(TWCR & (1<<TWSTO)) { CHECK_TIMEOUT(); };
 
 }/* i2c_stop */
 
@@ -165,7 +192,8 @@ unsigned char i2c_write( unsigned char data )
     TWCR = (1<<TWINT) | (1<<TWEN);
 
     // wait until transmission completed
-    while(!(TWCR & (1<<TWINT))) { CHECK_FORCE_STOP() };
+    CHECK_TIMEOUT_PRE();
+    while(!(TWCR & (1<<TWINT))) { CHECK_TIMEOUT(2) };
 
     // check value of TWI Status Register. Mask prescaler bits
     twst = TW_STATUS & 0xF8;
@@ -183,7 +211,8 @@ Return:  byte read from I2C device
 unsigned char i2c_readAck(void)
 {
     TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
-    while(!(TWCR & (1<<TWINT))) { CHECK_FORCE_STOP(); };
+    CHECK_TIMEOUT_PRE();
+    while(!(TWCR & (1<<TWINT))) { CHECK_TIMEOUT(2); };
 
     return TWDR;
 
@@ -198,8 +227,32 @@ Return:  byte read from I2C device
 unsigned char i2c_readNak(void)
 {
     TWCR = (1<<TWINT) | (1<<TWEN);
-    while(!(TWCR & (1<<TWINT))) { CHECK_FORCE_STOP(); };
+    CHECK_TIMEOUT_PRE();
+    while(!(TWCR & (1<<TWINT))) { CHECK_TIMEOUT(2); };
 
     return TWDR;
 
 }/* i2c_readNak */
+
+void i2c_forceStop(void)
+{
+    xprintf("i2c timeout\n");
+
+    /* let slave to release SDA */
+    TWCR = 0;
+    DDRD |=  (1<<PD0);
+    DDRD &= ~(1<<PD1);
+    _delay_us(30);
+    if ((PIND & (1<<PD1)) == 0) {
+        for (uint8_t i = 0; i < 9; i++) {
+            PORTD &= ~(1<<PD0);
+            _delay_us(SCL_DURATION);
+            PORTD |= (1<<PD0);
+            _delay_us(SCL_DURATION);
+        }
+    }
+    DDRD &= ~(1<<PD0);
+
+    /* send stop condition */
+    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
+}