diff --git a/include/arch.h b/include/arch.h
index 8024818ea7196c82a3f3e68ea715622c0256018f..094b2306de01db25afbf1e77dda32b91a9071f35 100644
--- a/include/arch.h
+++ b/include/arch.h
@@ -10,8 +10,8 @@ class Arch {
 		void setup();
 		void idle_loop();
 		void idle();
-		void delay_us(unsigned char const us);
-		void delay_ms(unsigned char const ms);
+		void delay_us(unsigned int const us);
+		void delay_ms(unsigned int const ms);
 };
 
 extern Arch arch;
diff --git a/src/arch/arduino-nano/arch.cc b/src/arch/arduino-nano/arch.cc
index 300963781e3140d33cf1c91da75685539a510c48..b0343485e2a62c90e1bf1215b7c75e6dccbed203 100644
--- a/src/arch/arduino-nano/arch.cc
+++ b/src/arch/arduino-nano/arch.cc
@@ -67,12 +67,12 @@ void Arch::idle(void)
 	asm("wdr");
 }
 
-void Arch::delay_us(unsigned char const us)
+void Arch::delay_us(unsigned int const us)
 {
 	_delay_us(us);
 }
 
-void Arch::delay_ms(unsigned char const ms)
+void Arch::delay_ms(unsigned int const ms)
 {
 	_delay_ms(ms);
 }
diff --git a/src/arch/blinkenrocket/arch.cc b/src/arch/blinkenrocket/arch.cc
index 80426792ed0e370be6ae6e2c91398743d890b69e..1261515e97f946f15d402ca270f9d8e8e6b9251e 100644
--- a/src/arch/blinkenrocket/arch.cc
+++ b/src/arch/blinkenrocket/arch.cc
@@ -64,12 +64,12 @@ void Arch::idle(void)
 	asm("wdr");
 }
 
-void Arch::delay_us(unsigned char const us)
+void Arch::delay_us(unsigned int const us)
 {
 	_delay_us(us);
 }
 
-void Arch::delay_ms(unsigned char const ms)
+void Arch::delay_ms(unsigned int const ms)
 {
 	_delay_ms(ms);
 }
diff --git a/src/arch/msp430fr5969lp/arch.cc b/src/arch/msp430fr5969lp/arch.cc
index b475c5f2beb5ba950beefcd55973d5e70200847d..b5b23b6a4744ba2b00345bedef830e11adb1f7fc 100644
--- a/src/arch/msp430fr5969lp/arch.cc
+++ b/src/arch/msp430fr5969lp/arch.cc
@@ -93,15 +93,15 @@ extern void loop();
 volatile char run_loop = 0;
 #endif
 
-void Arch::delay_us(unsigned char const us)
+void Arch::delay_us(unsigned int const us)
 {
-	for (int i = 0; i < us/10; i++) {
+	for (unsigned int i = 0; i < us/10; i++) {
 		__delay_cycles(160);
 	}
 }
-void Arch::delay_ms(unsigned char const ms)
+void Arch::delay_ms(unsigned int const ms)
 {
-	for (int i = 0; i < ms; i++) {
+	for (unsigned int i = 0; i < ms; i++) {
 		__delay_cycles(16000);
 	}
 }
diff --git a/src/driver/soft_i2c.cc b/src/driver/soft_i2c.cc
index e69bd2c13c8af629448b0e2c730f577b2c3095e7..39754a315bdba0e454a3f07fb96a81ddf5611552 100644
--- a/src/driver/soft_i2c.cc
+++ b/src/driver/soft_i2c.cc
@@ -27,6 +27,12 @@
 
 #ifndef SOFTI2C_TIMER
 
+#if F_I2C < 50000
+#define I2C_WAIT arch.delay_us((500000UL / F_I2C) - 10)
+#else
+#define I2C_WAIT
+#endif
+
 signed char SoftI2C::setup()
 {
 	SDA_HIGH;
@@ -38,20 +44,20 @@ void SoftI2C::start()
 {
 	SDA_HIGH;
 	SCL_HIGH;
-	//
+	I2C_WAIT;
 	SDA_LOW;
-	//
+	I2C_WAIT;
 	SCL_LOW;
 }
 
 void SoftI2C::stop()
 {
 	SCL_LOW;
-	//
+	I2C_WAIT;
 	SDA_LOW;
-	//
+	I2C_WAIT;
 	SCL_HIGH;
-	//
+	I2C_WAIT;
 	SDA_HIGH;
 }
 
@@ -65,17 +71,17 @@ bool SoftI2C::tx(unsigned char byte)
 			SDA_LOW;
 		}
 		byte <<= 1;
-		//
+		I2C_WAIT;
 		SCL_HIGH;
 		while (!gpio.read(scl)) ;
-		//
+		I2C_WAIT;
 		if (i == 8) {
 			if (!gpio.read(sda)) {
 				got_ack = 1;
 			}
 		}
 		SCL_LOW;
-		//
+		I2C_WAIT;
 	}
 	return got_ack;
 }
@@ -85,16 +91,16 @@ unsigned char SoftI2C::rx(bool send_ack)
 	unsigned char byte = 0;
 	SDA_HIGH;
 	for (unsigned char i = 0; i <= 8; i++) {
-		//
+		I2C_WAIT;
 		SCL_HIGH;
 		while (!gpio.read(scl)) ;
-		//
+		I2C_WAIT;
 		if ((i < 8) && gpio.read(sda)) {
 			byte |= 1 << (7 - i);
 		}
-		//
+		I2C_WAIT;
 		SCL_LOW;
-		//
+		I2C_WAIT;
 		if ((i == 7) && send_ack) {
 			SDA_LOW;
 		} else if ((i == 8) && send_ack) {