diff --git a/lib/at24c256_i2c.c b/lib/at24c256_i2c.c
index 8530070..f823f7c 100644
--- a/lib/at24c256_i2c.c
+++ b/lib/at24c256_i2c.c
@@ -12,7 +12,7 @@ static int  _is_16bits = 0;
 
 int at24_i2c_init(uint32_t scl, uint32_t sda, uint8_t addr, int bits)
 {
-    _i2c = i2c_new(scl, sda);
+    _i2c = i2c_new(scl, sda, 100000); /* 100K */
     _address = addr;
     _is_16bits = bits;
     return 0;
diff --git a/lib/dwt.h b/lib/dwt.h
index 69fe838..9b84fc5 100644
--- a/lib/dwt.h
+++ b/lib/dwt.h
@@ -4,8 +4,9 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
+#include <stdint.h>
 
-extern unsigned int SystemCoreClock;
+extern uint32_t SystemCoreClock;
 
 #define DWT_CR           (*(volatile unsigned int *)0xE0001000)
 #define DWT_CYCCNT       (*(volatile unsigned int *)0xE0001004)
@@ -13,6 +14,13 @@ extern unsigned int SystemCoreClock;
 #define DEM_CR_TRCENA    (1 << 24)
 #define DWT_CR_CYCCNTENA (1 << 0)
 
+static inline int __attribute__((unused)) dwt_is_enabled(void)
+{
+	if (DEM_CR & DEM_CR_TRCENA)
+		return 1;
+	return 0;
+}
+
 static inline void __attribute__((unused)) dwt_init(void)
 {
 	DEM_CR |= DEM_CR_TRCENA; /* enable DWT */
diff --git a/lib/i2c.c b/lib/i2c.c
index 3affc02..21ad04f 100644
--- a/lib/i2c.c
+++ b/lib/i2c.c
@@ -2,7 +2,7 @@
 #include "stm32f4xx.h"
 #include "gpio.h"
 #include "i2c.h"
-#include "utils.h"
+#include "dwt.h"
 
 struct i2c {
     GPIO_TypeDef *scl_port;
@@ -10,6 +10,8 @@ struct i2c {
 
     GPIO_TypeDef *sda_port;
     uint32_t sda;
+
+	unsigned int ticks;
 };
 
 #define DELAY_TICKS (4)
@@ -23,7 +25,7 @@ extern GPIO_TypeDef *gpio_port(uint32_t pin);
 #define	SCL_IN(i2c) ((((i2c)->scl_port)-IDR) & (((i2c)->sda) & PIN_MASK))
 #define SDA_IN(i2c) ((((i2c)->sda_port)->IDR) & (((i2c)->sda) & PIN_MASK))
 
-i2c_t i2c_new(uint32_t scl, uint32_t sda)
+i2c_t i2c_new(uint32_t scl, uint32_t sda, int speed)
 {
     i2c_t i2c = malloc(sizeof *i2c);
     if (i2c == NULL) {
@@ -40,6 +42,12 @@ i2c_t i2c_new(uint32_t scl, uint32_t sda)
     SCL_H(i2c);
     SDA_H(i2c);
 
+	if (speed > 0) {
+		i2c->ticks = 1000000 / speed / 2;
+	} else {
+		i2c->ticks = DELAY_TICKS;
+	}
+
     return (i2c);
 }
 
@@ -50,118 +58,169 @@ void i2c_free(i2c_t i2c)
 
 int i2c_start(i2c_t i2c)
 {
+	int en, err = 0;
+
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
+
 	SDA_H(i2c);
 	SCL_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+	dwt_wait_us(i2c->ticks);
 
     if (!SDA_IN(i2c)) { /* sda busy ? */
-        return -1;
+		err = -1;
+		goto err;
     }
 
     SDA_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
     if (SDA_IN(i2c)) { /* sda hi, error. */
-        return -2;
+		err = -2;
+		goto err;
     }
 
     SDA_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+	dwt_wait_us(i2c->ticks);
 
-    return 0;
+err:
+	if (!en) {
+		dwt_close();
+	}
+
+    return err;
 }
 
 void i2c_stop(i2c_t i2c)
 {
+	int en;
+
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
+
 	SCL_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SDA_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SCL_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SDA_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
+
+	if (!en) {
+		dwt_close();
+	}
 }
 
 void i2c_ack(i2c_t i2c)
 {
+	int en;
+
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
+
 	SCL_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SDA_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SCL_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SCL_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
+	if (!en) {
+		dwt_close();
+	}
 }
 
 void i2c_nack(i2c_t i2c)
 {
+	int en;
+
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
+
 	SCL_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SDA_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SCL_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SCL_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
+	if (!en) {
+		dwt_close();
+	}
 }
 
 int i2c_wait_ack(i2c_t i2c) 
 {
-    int err;
+    int en, err;
+
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
 
 	SCL_L(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SDA_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
 	SCL_H(i2c);
 
-    delay_microseconds(DELAY_TICKS);
+    dwt_wait_us(i2c->ticks);
 
     err = SDA_IN(i2c);
 
     SCL_L(i2c);
+	if (!en) {
+		dwt_close();
+	}
 
     return err;
 }
 
 int i2c_send_byte(i2c_t i2c, uint8_t byte)
 {
-    int i = 8;
+    int en, i = 8;
 
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
 	while(i--) {
 		SCL_L(i2c);
 
-        delay_microseconds(DELAY_TICKS);
+		dwt_wait_us(i2c->ticks);
 
 		if(byte & 0x80)
 			SDA_H(i2c);
@@ -170,21 +229,28 @@ int i2c_send_byte(i2c_t i2c, uint8_t byte)
 
 		byte <<= 1;
 
-        delay_microseconds(DELAY_TICKS);
+		dwt_wait_us(i2c->ticks);
 
 		SCL_H(i2c);
 
-        delay_microseconds(DELAY_TICKS);
+		dwt_wait_us(i2c->ticks);
 	}
 	SCL_L(i2c);
+	if (!en) {
+		dwt_close();
+	}
     return 0;
 }
 
 uint8_t i2c_recv_byte(i2c_t i2c)
 {
-	int i = 8;
+	int en, i = 8;
     uint8_t rd = 0;
 
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
+
 	SDA_H(i2c);
 
 	while(i--) {
@@ -192,17 +258,20 @@ uint8_t i2c_recv_byte(i2c_t i2c)
 
 		SCL_L(i2c);
 
-        delay_microseconds(DELAY_TICKS);
+		dwt_wait_us(i2c->ticks);
 
 		SCL_H(i2c);
 
-        delay_microseconds(DELAY_TICKS);
+		dwt_wait_us(i2c->ticks);
 
 		if (SDA_IN(i2c)) {
             rd |= 0x01;
 		}
 	}
 	SCL_L(i2c);
+	if (!en) {
+		dwt_close();
+	}
     return (rd);
 }
 
diff --git a/lib/i2c.h b/lib/i2c.h
index 41e20ab..ac77313 100644
--- a/lib/i2c.h
+++ b/lib/i2c.h
@@ -7,7 +7,7 @@ extern "C" {
 
 typedef struct i2c *i2c_t;
 
-i2c_t i2c_new(uint32_t scl, uint32_t sda);
+i2c_t i2c_new(uint32_t scl, uint32_t sda, int speed);
 
 void  i2c_free(i2c_t);
 
diff --git a/lib/stdio_ymodem.c b/lib/stdio_ymodem.c
index e7fd7b8..06f6e84 100644
--- a/lib/stdio_ymodem.c
+++ b/lib/stdio_ymodem.c
@@ -54,7 +54,7 @@ int stdio_ymodem_recv(void *user,
         int (*_on_info)(void *user, const char *name, unsigned int size),
         int (*_on_data)(void *user, const void *data, unsigned int len))
 {
-	int err;
+	int err, en;
     struct stdio_ymodem modem = {
         .xfer = {
             .getc    = do_getc,
@@ -67,9 +67,13 @@ int stdio_ymodem_recv(void *user,
         .user = user
     };
 
-	dwt_init();
+	if (!(en = dwt_is_enabled())) {
+		dwt_init();
+	}
     err = ymodem_recv(&modem.xfer);
-	dwt_close();
+	if (!en) {
+		dwt_close();
+	}
 	return (err);
 }
 
diff --git a/targets/stm32f429-disc/main.c b/targets/stm32f429-disc/main.c
index f94944d..7f86cd6 100644
--- a/targets/stm32f429-disc/main.c
+++ b/targets/stm32f429-disc/main.c
@@ -133,7 +133,7 @@ int main()
 	iis_setup(&i2s0, IIS_STD_PHILIP, 44100);
 	iis_set_tx_completion(&i2s0, tx_done, NULL);
 
-	i2c = i2c_new(PB6, PB7);
+	i2c = i2c_new(PB6, PB7, 100000);
 
 	wm8978_init_with_i2c(i2c);