diff --git a/Makefile b/Makefile
index e5dc27d53be949e01a71ac1b00046b045dbd2d5f..f6b1f0eb28e3cefecf537df02dd6ecfffbf875d3 100644
--- a/Makefile
+++ b/Makefile
@@ -32,6 +32,10 @@ ifneq ($(findstring softi2c,${drivers}), )
 	COMMON_FLAGS += -DDRIVER_SOFTI2C
 endif
 
+ifeq (${softi2c_pullup}, 1)
+	COMMON_FLAGS += -DSOFTI2C_PULLUP
+endif
+
 ifeq (${timer_cycles}, 1)
 	COMMON_FLAGS += -DTIMER_CYCLES
 endif
diff --git a/include/arch/msp430fr5969lp/driver/gpio.h b/include/arch/msp430fr5969lp/driver/gpio.h
index caed74abce5ea79b8bbc6afdebca58976ef95d85..71dab889fd2e1f6dbd8bee5466787826bf3f2343 100644
--- a/include/arch/msp430fr5969lp/driver/gpio.h
+++ b/include/arch/msp430fr5969lp/driver/gpio.h
@@ -63,6 +63,29 @@ class GPIO {
 				PJDIR &= ~(1 << (pin - pj_0));
 			}
 		}
+		inline void input(unsigned char const pin, unsigned char const pull) {
+			if (pin < p2_0) {
+				P1DIR &= ~(1 << pin);
+				P1OUT = pull ? (P1OUT | (1 << pin)) : (P1OUT & ~(1 << pin));
+				P1REN |= (1 << pin);
+			} else if (pin < p3_0) {
+				P2DIR &= ~(1 << (pin - p2_0));
+				P2OUT = pull ? (P2OUT | (1 << (pin - p2_0))) : (P2OUT & ~(1 << (pin - p2_0)));
+				P2REN |= (1 << (pin - p2_0));
+			} else if (pin < p4_0) {
+				P3DIR &= ~(1 << (pin - p3_0));
+				P3OUT = pull ? (P3OUT | (1 << (pin - p3_0))) : (P3OUT & ~(1 << (pin - p3_0)));
+				P3REN |= (1 << (pin - p3_0));
+			} else if (pin < pj_0) {
+				P4DIR &= ~(1 << (pin - p4_0));
+				P4OUT = pull ? (P4OUT | (1 << (pin - p4_0))) : (P4OUT & ~(1 << (pin - p4_0)));
+				P4REN |= (1 << (pin - p4_0));
+			} else if (pin < PIN_INVALID) {
+				PJDIR &= ~(1 << (pin - pj_0));
+				PJOUT = pull ? (PJOUT | (1 << (pin - pj_0))) : (PJOUT & ~(1 << (pin - pj_0)));
+				PJREN |= (1 << (pin - pj_0));
+			}
+		}
 		inline void output(unsigned char const pin) {
 			if (pin < p2_0) {
 				P1DIR |= (1 << pin);
@@ -76,6 +99,24 @@ class GPIO {
 				PJDIR |= (1 << (pin - pj_0));
 			}
 		}
+		inline void output(unsigned char const pin, unsigned char const value) {
+			if (pin < p2_0) {
+				P1OUT = value ? (P1OUT | (1 << pin)) : (P1OUT & ~(1 << pin));
+				P1DIR |= (1 << pin);
+			} else if (pin < p3_0) {
+				P2OUT = value ? (P2OUT | (1 << (pin - p2_0))) : (P2OUT & ~(1 << (pin - p2_0)));
+				P2DIR |= (1 << (pin - p2_0));
+			} else if (pin < p4_0) {
+				P3OUT = value ? (P3OUT | (1 << (pin - p3_0))) : (P3OUT & ~(1 << (pin - p3_0)));
+				P3DIR |= (1 << (pin - p3_0));
+			} else if (pin < pj_0) {
+				P4OUT = value ? (P4OUT | (1 << (pin - p4_0))) : (P4OUT & ~(1 << (pin - p4_0)));
+				P4DIR |= (1 << (pin - p4_0));
+			} else if (pin < PIN_INVALID) {
+				PJOUT = value ? (PJOUT | (1 << (pin - pj_0))) : (PJOUT & ~(1 << (pin - pj_0)));
+				PJDIR |= (1 << (pin - pj_0));
+			}
+		}
 		inline unsigned char read(unsigned char const pin) {
 			if (pin < p2_0) {
 				return P1IN & (1 << pin);
diff --git a/src/arch/msp430fr5969lp/arch.cc b/src/arch/msp430fr5969lp/arch.cc
index 9ea49b7c2aee54763b8b99aff7b12125960c0aad..ddb84a4a46bdead7f306a5b033d0281510aff0d1 100644
--- a/src/arch/msp430fr5969lp/arch.cc
+++ b/src/arch/msp430fr5969lp/arch.cc
@@ -72,6 +72,13 @@ extern void loop();
 volatile char run_loop = 0;
 #endif
 
+void Arch::delay_us(unsigned char const us)
+{
+	for (int i = 0; i < us/10; i++) {
+		__delay_cycles(160);
+	}
+}
+
 void Arch::idle_loop(void)
 {
 	while (1) {
diff --git a/src/arch/msp430fr5969lp/driver/i2c.cc b/src/arch/msp430fr5969lp/driver/i2c.cc
index fffc28c7b70fdfecdf705ddd689f574d736a0e23..55f85910d12d7bfc07c063dc0b7eeac10ab65cdc 100644
--- a/src/arch/msp430fr5969lp/driver/i2c.cc
+++ b/src/arch/msp430fr5969lp/driver/i2c.cc
@@ -16,7 +16,7 @@ signed char I2C::setup()
 	UCB0CTL1 &= ~UCSWRST;
 	UCB0I2CSA = 0;
 
-	__delay_cycles(1600);
+	arch.delay_us(100);
 
 	if (UCB0STAT & UCBBUSY)
 		return -1;
diff --git a/src/driver/soft_i2c.cc b/src/driver/soft_i2c.cc
index 94ab2613fe17543cf09020601afa3874a6244ddb..e7b85482badddd3e29ef52353da943d5e31fa73f 100644
--- a/src/driver/soft_i2c.cc
+++ b/src/driver/soft_i2c.cc
@@ -1,32 +1,45 @@
 #include "driver/soft_i2c.h"
 #include "driver/gpio.h"
+#include "arch.h"
+
+#ifdef SOFTI2C_PULLUP
+#define SDA_HIGH gpio.input(sda, 1)
+#define SDA_LOW gpio.output(sda, 0)
+#define SCL_HIGH gpio.input(scl, 1)
+#define SCL_LOW gpio.output(scl, 0)
+#else
+#define SDA_HIGH gpio.input(sda)
+#define SDA_LOW gpio.output(sda)
+#define SCL_HIGH gpio.input(scl)
+#define SCL_LOW gpio.output(scl)
+#endif
 
 signed char SoftI2C::setup()
 {
-	gpio.write(sda, 0);
-	gpio.write(scl, 0);
+	SDA_HIGH;
+	SCL_HIGH;
 	return 0;
 }
 
 void SoftI2C::start()
 {
-	gpio.input(sda);
-	gpio.input(scl);
+	SDA_HIGH;
+	SCL_HIGH;
 	//
-	gpio.output(sda);
+	SDA_LOW;
 	//
-	gpio.output(scl);
+	SCL_LOW;
 }
 
 void SoftI2C::stop()
 {
-	gpio.output(scl);
+	SCL_LOW;
 	//
-	gpio.output(sda);
+	SDA_LOW;
 	//
-	gpio.input(scl);
+	SCL_HIGH;
 	//
-	gpio.input(sda);
+	SDA_HIGH;
 }
 
 bool SoftI2C::tx(unsigned char byte)
@@ -34,20 +47,20 @@ bool SoftI2C::tx(unsigned char byte)
 	unsigned char got_ack = 0;
 	for (unsigned char i = 0; i <= 8; i++) {
 		if ((byte & 0x80) || (i == 8)) {
-			gpio.input(sda);
+			SDA_HIGH;
 		} else {
-			gpio.output(sda);
+			SDA_LOW;
 		}
 		byte <<= 1;
 		//
-		gpio.input(scl);
+		SCL_HIGH;
 		//
 		if (i == 8) {
 			if (!gpio.read(sda)) {
 				got_ack = 1;
 			}
 		}
-		gpio.output(scl);
+		SCL_LOW;
 		//
 	}
 	return got_ack;
@@ -56,21 +69,21 @@ bool SoftI2C::tx(unsigned char byte)
 unsigned char SoftI2C::rx(bool send_ack)
 {
 	unsigned char byte = 0;
-	gpio.input(sda);
+	SDA_HIGH;
 	for (unsigned char i = 0; i <= 8; i++) {
 		//
-		gpio.input(scl);
+		SCL_HIGH;
 		//
 		if ((i < 8) && gpio.read(sda)) {
 			byte |= 1 << (7 - i);
 		}
 		//
-		gpio.output(scl);
+		SCL_LOW;
 		//
 		if ((i == 7) && send_ack) {
-			gpio.output(sda);
+			SDA_LOW;
 		} else if ((i == 8) && send_ack) {
-			gpio.input(sda);
+			SDA_HIGH;
 		}
 	}
 	return byte;