diff --git a/board/keymile/common/common.c b/board/keymile/common/common.c
index 61276d22a286bd43b5718b944c4356cdb0241dcb..e47928d0f7c0cffad4e275512b94fa0db0f5fa72 100644
--- a/board/keymile/common/common.c
+++ b/board/keymile/common/common.c
@@ -34,6 +34,7 @@
 #if defined(CONFIG_HARD_I2C) || defined(CONFIG_SOFT_I2C)
 #include <i2c.h>
 #endif
+#include <asm/io.h>
 
 extern int i2c_soft_read_pin (void);
 
@@ -400,7 +401,7 @@ static void set_scl (int state)
 
 static int get_sda (void)
 {
-	return i2c_soft_read_pin ();
+	return I2C_READ;
 }
 
 static int get_scl (void)
diff --git a/drivers/i2c/soft_i2c.c b/drivers/i2c/soft_i2c.c
index 508d3d7d784be3ea6322f7e378806413b43d5e41..ebe60e23380e662422b38c0c6e2df4f5aaeae9dc 100644
--- a/drivers/i2c/soft_i2c.c
+++ b/drivers/i2c/soft_i2c.c
@@ -28,6 +28,7 @@
 #include <common.h>
 #ifdef	CONFIG_MPC8260			/* only valid for MPC8260 */
 #include <ioports.h>
+#include <asm/io.h>
 #endif
 #ifdef	CONFIG_AT91RM9200		/* need this for the at91rm9200 */
 #include <asm/io.h>
@@ -39,6 +40,9 @@
 #ifdef CONFIG_LPC2292
 #include <asm/arch/hardware.h>
 #endif
+#ifdef	CONFIG_MPC866			/* only valid for MPC866 */
+#include <asm/io.h>
+#endif
 #include <i2c.h>
 
 /* #define	DEBUG_I2C	*/
diff --git a/include/configs/mgsuvd.h b/include/configs/mgsuvd.h
index 416e4db0daa2e4f2b7b9263dc6649b2d07c3b66e..1604285da0b3fbb3b424db06c66a01a615620ab5 100644
--- a/include/configs/mgsuvd.h
+++ b/include/configs/mgsuvd.h
@@ -346,8 +346,8 @@
 /*
  * Software (bit-bang) I2C driver configuration
  */
-#define I2C_BASE_DIR	(CONFIG_SYS_PIGGY_BASE + 0x04)
-#define I2C_BASE_PORT	(CONFIG_SYS_PIGGY_BASE + 0x09)
+#define I2C_BASE_DIR	((u16 *)(CONFIG_SYS_PIGGY_BASE + 0x04))
+#define I2C_BASE_PORT	((u8 *)(CONFIG_SYS_PIGGY_BASE + 0x09))
 
 #define SDA_BIT		0x40
 #define SCL_BIT		0x80
@@ -356,18 +356,18 @@
 
 #define I2C_ACTIVE	do {} while (0)
 #define I2C_TRISTATE	do {} while (0)
-#define I2C_READ	i2c_soft_read_pin ()
+#define I2C_READ	((in_8(I2C_BASE_PORT) & SDA_BIT) == SDA_BIT)
 #define I2C_SDA(bit)	if(bit) { \
-				*(unsigned short *)(I2C_BASE_DIR) &=  ~SDA_CONF; \
+				clrbits(be16, I2C_BASE_DIR, SDA_CONF); \
 			} else { \
-				*(unsigned char *)(I2C_BASE_PORT) &= ~SDA_BIT; \
-				*(unsigned short *)(I2C_BASE_DIR) |= SDA_CONF; \
+				clrbits(8, I2C_BASE_PORT, SDA_BIT); \
+				setbits(be16, I2C_BASE_DIR, SDA_CONF); \
 			}
 #define I2C_SCL(bit)	if(bit) { \
-				*(unsigned short *)(I2C_BASE_DIR) &=  ~SCL_CONF; \
+				clrbits(be16, I2C_BASE_DIR, SCL_CONF); \
 			} else { \
-				*(unsigned char *)(I2C_BASE_PORT) &= ~SCL_BIT; \
-				*(unsigned short *)(I2C_BASE_DIR) |= SCL_CONF; \
+				clrbits(8, I2C_BASE_PORT, SCL_BIT); \
+				setbits(be16, I2C_BASE_DIR, SCL_CONF); \
 			}
 #define I2C_DELAY	udelay(50)	/* 1/4 I2C clock duration */