diff --git a/httpd/page_impl.c b/httpd/page_impl.c
index 288e4f4..363f96b 100644
--- a/httpd/page_impl.c
+++ b/httpd/page_impl.c
@@ -178,7 +178,7 @@ void sfp_send_data(uint8_t slot, uint8_t reg, uint8_t len)
 	}
 
 	reg_read_m(RTL837X_REG_I2C_CTRL);
-	sfr_mask_data(1, 0xfc, machine.sfp_port[slot].i2c == 0 ? SCL_PIN << 5 | SDA_PIN_0 << 2 : SCL_PIN << 5 | SDA_PIN_1 << 2 );
+	sfr_mask_data(1, 0xfc, machine.sfp_port[slot].i2c_bus.scl << 5 | machine.sfp_port[slot].i2c_bus.sda << 2);
 	reg_write_m(RTL837X_REG_I2C_CTRL);
 
 	REG_WRITE(RTL837X_REG_I2C_IN, 0, 0, 0, reg);
diff --git a/machine.c b/machine.c
index fadd346..57ce9f6 100644
--- a/machine.c
+++ b/machine.c
@@ -12,12 +12,14 @@ __code const struct machine machine = {
 	.is_sfp = {0, 0, 0, 2, 0, 0, 0, 0, 1},
 	.sfp_port[0].pin_detect = 50,
 	.sfp_port[0].pin_los = 10,
+	.sfp_port[0].pin_tx_disable = 0xFF,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 1,
+	.sfp_port[0].i2c_bus = { .sda = 3, .scl = 3 },
 	.sfp_port[1].pin_detect = 30,
 	.sfp_port[1].pin_los = 37,
+	.sfp_port[1].pin_tx_disable = 0xFF,
 	.sfp_port[1].sds = 0,
-	.sfp_port[1].i2c = 0,
+	.sfp_port[1].i2c_bus = { .sda = 4, .scl = 3 },
 	.reset_pin = 46,
 };
 #elif defined MACHINE_KP_9000_6XH_X
@@ -32,8 +34,9 @@ __code const struct machine machine = {
 	.is_sfp = {0, 0, 0, 0, 0, 0, 0, 0, 1},
 	.sfp_port[0].pin_detect = 30,
 	.sfp_port[0].pin_los = 37,
+	.sfp_port[0].pin_tx_disable = 0xFF,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0,
+	.sfp_port[0].i2c_bus = { .sda = 4, .scl = 3 },
 };
 #elif defined MACHINE_KP_9000_9XH_X_EU
 __code const struct machine machine = {
@@ -47,9 +50,11 @@ __code const struct machine machine = {
 	.is_sfp = {0, 0, 0, 0, 0, 0, 0, 0, 1},
 	.sfp_port[0].pin_detect = 30,
 	.sfp_port[0].pin_los = 37,
+	.sfp_port[0].pin_tx_disable = 0xFF,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0,
+	.sfp_port[0].i2c_bus = { .sda = 4, .scl = 3 },
 };
+
 #elif defined MACHINE_SWGT024_V2_0
 __code const struct machine machine = {
 	.machine_name = "SWGT024 V2.0",
@@ -63,15 +68,18 @@ __code const struct machine machine = {
 	// Left SFP port (J4)
 	.sfp_port[0].pin_detect = 30,
 	.sfp_port[0].pin_los = 37,
+	.sfp_port[0].pin_tx_disable = 0xFF,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0, /* GPIO 39 */
+	.sfp_port[0].i2c_bus = { .sda = 4, .scl = 3 }, /* GPIO 39 */
 	// Right SFP port (J2)
 	.sfp_port[1].pin_detect = 50,
 	.sfp_port[1].pin_los = 51,
+	.sfp_port[1].pin_tx_disable = 0xFF,
 	.sfp_port[1].sds = 0,
-	.sfp_port[1].i2c = 1, /* GPIO 40 */
+	.sfp_port[1].i2c_bus = { .sda = 3, .scl = 3 }, /* GPIO 40 */
 	.reset_pin = 36,
 };
+
 #elif defined DEFAULT_8C_1SFP
 __code const struct machine machine = {
 	.machine_name = "8+1 SFP Port Switch",
@@ -84,8 +92,8 @@ __code const struct machine machine = {
 	.is_sfp = {0, 0, 0, 0, 0, 0, 0, 0, 1},
 	.sfp_port[0].pin_detect = 30,
 	.sfp_port[0].pin_los = 37,
+	.sfp_port[0].pin_tx_disable = 0xFF,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0,
+	.sfp_port[0].i2c_bus = { .sda = 4, .scl = 3 },
 };
-
 #endif
diff --git a/machine.h b/machine.h
index d653d66..0c8176e 100644
--- a/machine.h
+++ b/machine.h
@@ -16,12 +16,21 @@
 
 // #define DEFAULT_5C_1SFP
 
+struct i2c_bus {
+	// These are I2C bus identifiers refer to GPIO MUX document
+	// for GPIO pin assignments for given bus numbers
+	uint8_t sda : 3; // SDA pin number 0-4
+	uint8_t scl : 3; // SCL pin number 0-3
+	uint8_t reserved : 2;
+};
+
 struct sfp_port
 {
 	uint8_t pin_detect; // gpio number 0-63, 0xFF = don't have it?
 	uint8_t pin_los; // gpio number 0-63, 0xFF = don't have it?
+	uint8_t pin_tx_disable; // gpio number 0-63, 0xFF = not present
 	uint8_t sds;
-	uint8_t i2c;
+	struct i2c_bus i2c_bus;
 };
 
 typedef struct machine {
diff --git a/rtl837x_common.h b/rtl837x_common.h
index 0b0db2e..3ff6fb8 100644
--- a/rtl837x_common.h
+++ b/rtl837x_common.h
@@ -7,11 +7,6 @@
 
 #define SYS_TICK_HZ 200
 
-// SCL and SDA pin numbers for SFP cage 0 and SFP cage 1
-#define SCL_PIN		3
-#define SDA_PIN_0	4
-#define SDA_PIN_1	3
-
 #define CPU_PORT        9
 
 // Define Port-masks for 9-port devices and 6-port devices
diff --git a/rtl837x_port.c b/rtl837x_port.c
index c4b542c..dbd1715 100644
--- a/rtl837x_port.c
+++ b/rtl837x_port.c
@@ -356,20 +356,10 @@ void port_stats_print(void) __banked
 			else
 				print_string("Off\t");
 		} else {  // An SFP Module
-			if (i != 3) {
-				reg_read_m(RTL837X_REG_GPIO_00_31_INPUT);
-				if (!(sfr_data[0] & 0x40)) {
-					print_string("SFP OK\t");
-				} else {
-					print_string("NO SFP\t");
-				}
+			if (!gpio_pin_test(machine.sfp_port[machine.is_sfp[i]-1].pin_detect)) {
+				print_string("SFP IN\t");
 			} else {
-				reg_read_m(RTL837X_REG_GPIO_32_63_INPUT);
-				if (!(sfr_data[1] & 0x04)) {
-					print_string("SFP OK\t");
-				} else {
-					print_string("NO SFP\t");
-				}
+				print_string("NO SFP\t");
 			}
 		}
 
diff --git a/rtlplayground.c b/rtlplayground.c
index e9712b8..90deb1a 100644
--- a/rtlplayground.c
+++ b/rtlplayground.c
@@ -807,7 +807,7 @@ uint8_t sfp_read_reg(uint8_t slot, uint8_t reg)
 	}
 
 	reg_read_m(RTL837X_REG_I2C_CTRL);
-	sfr_mask_data(1, 0xfc, machine.sfp_port[slot].i2c == 0 ? SCL_PIN << 5 | SDA_PIN_0 << 2 : SCL_PIN << 5 | SDA_PIN_1 << 2 );
+	sfr_mask_data(1, 0xfc, machine.sfp_port[slot].i2c_bus.scl << 5 | machine.sfp_port[slot].i2c_bus.sda << 2);
 	reg_write_m(RTL837X_REG_I2C_CTRL);
 
 	REG_WRITE(RTL837X_REG_I2C_IN, 0, 0, 0, reg);
@@ -1009,6 +1009,59 @@ bool gpio_pin_test(uint8_t pin)
 	return sfr_data[3-((pin >> 3) & 3)] & (1 << (pin & 7));
 }
 
+void gpio_mux_setup(uint8_t pin)
+{
+	// Some GPIOs require setting MUX registers to enable GPIO
+	if (pin == 36) {
+		reg_bit_set(RTL837X_PIN_MUX_1, 30);
+	} else if ( pin == 50 ) {
+		// Bit 15-16 0b00 -> GPIO
+		reg_read_m(RTL837X_PIN_MUX_1);
+		sfr_mask_data(1, 0x80, 0x00);
+		sfr_mask_data(2, 0x01, 0x00);
+		reg_write_m(RTL837X_PIN_MUX_1);	
+	} else if ( pin == 51 ) {
+		// Bit 17-18 0b00 -> GPIO
+		reg_read_m(RTL837X_PIN_MUX_1);
+		sfr_mask_data(2, 0x06, 0x00);
+		reg_write_m(RTL837X_PIN_MUX_1);	
+	} else if ( pin == 54 ) {
+		reg_bit_clear(RTL837X_PIN_MUX_2, 2);
+	}
+}
+
+/*
+ * Setup a GPIO pin as input
+ * pin: GPIO pin number 0-63
+ */
+void gpio_input_setup(uint8_t pin) {
+	gpio_mux_setup(pin);
+	reg_bit_clear(pin < 32 ? RTL837X_REG_GPIO_00_31_DIRECTION : RTL837X_REG_GPIO_32_63_DIRECTION, (pin % 32));
+}
+
+
+/*
+ * Setup a GPIO pin as output
+ * pin: GPIO pin number 0-63
+ */
+void gpio_output_setup(uint8_t pin) {
+	gpio_mux_setup(pin);
+	// Default output to low
+	reg_bit_clear(pin < 32 ? RTL837X_REG_GPIO_00_31_OUTPUT : RTL837X_REG_GPIO_32_63_OUTPUT, (pin % 32));
+	reg_bit_set(pin < 32 ? RTL837X_REG_GPIO_00_31_DIRECTION : RTL837X_REG_GPIO_32_63_DIRECTION, (pin % 32));
+}
+
+/* Inititalize SFP GPIOs */
+void setup_sfp_gpio(void)
+{
+	for (uint8_t sfp = 0; sfp < machine.n_sfp; sfp++) {
+		gpio_input_setup(machine.sfp_port[sfp].pin_detect);
+		gpio_input_setup(machine.sfp_port[sfp].pin_los);
+		if (machine.sfp_port[sfp].pin_tx_disable != 0xFF) {
+			gpio_output_setup(machine.sfp_port[sfp].pin_tx_disable);
+		}
+	}
+}
 
 void handle_sfp(void)
 {
@@ -1538,7 +1591,6 @@ void led_config(void)
 	reg_write_m(RTL837X_REG_LED3_0_SET1);
 }
 
-
 void rtl8373_revision(void)
 {
 	reg_read_m(RTL837X_REG_CHIP_INFO);
@@ -1811,11 +1863,50 @@ void setup_i2c(void)
 
 	REG_SET(RTL837X_REG_I2C_CTRL2, 0);
 
-	// HW Control register, enable I2C?
+	// HW Control register, enable I2C depending on PIN configuration
 	reg_read_m(RTL837X_PIN_MUX_1);
-	sfr_mask_data(3, 0x20, 0x00); // Clear bit 29
-	sfr_mask_data(0, 0x60, 0x40); // Set bits 5-6 to 0b10
-	reg_write_m(RTL837X_PIN_MUX_1);
+	for (uint8_t sfp = 0; sfp < machine.n_sfp; sfp++) {
+		const uint8_t scl_bus = machine.sfp_port[sfp].i2c_bus.scl;
+		const uint8_t sda_bus = machine.sfp_port[sfp].i2c_bus.sda;
+		print_string("Configuring I2C for SFP idx="); print_byte(sfp); print_string(" SCL="); print_byte(scl_bus); print_string(", SDA="); print_byte(sda_bus); write_char('\n');
+		if (scl_bus == 3) {
+			// Bit 5-6 0b10 -> SCL (implies enabled SDA on bus 3)
+			sfr_mask_data(0, 0x60, 0x40);
+		} else if (scl_bus == 2) {
+			// Bit 15-16 0b01 -> SCL
+			sfr_mask_data(1, 0x80, 0x80);
+			sfr_mask_data(2, 0x01, 0x00);
+		} else if (scl_bus == 1) {
+			// Bit 11-12 0b01 -> SCL
+			sfr_mask_data(1, 0x18, 0x08);
+		} else if (scl_bus == 0) {
+			// Bit 7-8 0b01 -> SCL
+			sfr_mask_data(0, 0x80, 0x80);
+			sfr_mask_data(1, 0x01, 0x00);
+		} else {
+			print_string("Invalid SCL bus number: "); print_byte(scl_bus); write_char('\n');
+		}
+
+		if (sda_bus == 4) {
+			// Bit 29 0b0 -> SDA
+			sfr_mask_data(3, 0x20, 0x00);
+		} else if (sda_bus == 3) {
+			// Bit 5-6 0b10 -> SDA (implies enabled SCL on bus 3)
+			sfr_mask_data(0, 0x60, 0x40);
+		} else if (sda_bus == 2) {
+			// Bit 17-18 0b01 -> SDA
+			sfr_mask_data(2, 0x06, 0x02);
+		} else if (sda_bus == 1) {
+			// Bit 13-14 0b01 -> SDA
+			sfr_mask_data(1, 0x60, 0x20);
+		} else if (sda_bus == 0) {
+			// Bit 9-10 0b01 -> SDA
+			sfr_mask_data(1, 0x06, 0x02);
+		} else {
+			print_string("Invalid SDA bus number: "); print_byte(sda_bus); write_char('\n');
+		}
+	}
+	reg_write_m(RTL837X_PIN_MUX_1);	
 }
 
 
@@ -2009,6 +2100,7 @@ void bootloader(void)
 	management_vlan = 0; // Disabled
 
 	setup_i2c();
+	setup_sfp_gpio();
 
 	print_string(greeting);
 
diff --git a/uip/uip_arp.c b/uip/uip_arp.c
index ca8ad1e..160dab1 100644
--- a/uip/uip_arp.c
+++ b/uip/uip_arp.c
@@ -142,7 +142,7 @@ static __xdata u8_t tmpage;
 void
 uip_arp_init(void) __banked
 {
-  print_string("uip_arp_init called");
+  print_string("uip_arp_init called\n");
   for(uint8_t i = 0; i < UIP_ARPTAB_SIZE; ++i) {
     memset(arp_table[i].ipaddr, 0, 4);
   }