diff --git a/Makefile b/Makefile
index cb948f5..fb02001 100644
--- a/Makefile
+++ b/Makefile
@@ -21,7 +21,7 @@ all: create_build_dir $(VERSION_HEADER) $(SUBDIRS) $(BUILDDIR)rtlplayground.bin
 create_build_dir:
 	mkdir -p $(BUILDDIR)
 
-SRCS = rtlplayground.c rtl837x_flash.c rtl837x_phy.c rtl837x_port.c cmd_parser.c html_data.c rtl837x_igmp.c rtl837x_stp.c dhcp.c  machine.c
+SRCS = rtlplayground.c rtl837x_flash.c rtl837x_phy.c rtl837x_port.c cmd_parser.c html_data.c rtl837x_igmp.c rtl837x_stp.c rtl837x_pins.c dhcp.c machine.c
 OBJS = ${SRCS:%.c=$(BUILDDIR)%.rel}
 OBJS += uip/$(BUILDDIR)/timer.rel uip/$(BUILDDIR)/uip-fw.rel uip/$(BUILDDIR)/uip-neighbor.rel uip/$(BUILDDIR)/uip-split.rel uip/$(BUILDDIR)/uip.rel uip/$(BUILDDIR)/uip_arp.rel uip/$(BUILDDIR)/uiplib.rel httpd/$(BUILDDIR)/httpd.rel httpd/$(BUILDDIR)/page_impl.rel
 
diff --git a/cmd_parser.c b/cmd_parser.c
index ed7d159..f02cb09 100644
--- a/cmd_parser.c
+++ b/cmd_parser.c
@@ -42,6 +42,7 @@ extern __xdata struct dhcp_state dhcp_state;
 
 __xdata uint8_t vlan_names[VLAN_NAMES_SIZE];
 __xdata uint16_t vlan_ptr;
+extern __xdata uint16_t management_vlan;
 __xdata uint8_t gpio_last_value[8] = { 0 };
 
 // Temporatly for str to hex convertion value.
@@ -289,16 +290,20 @@ void parse_vlan(void)
 			vlan_delete(vlan);
 			return;
 		}
+		if (cmd_words_b[2] > 0 && cmd_compare(2, "mgmt")) {
+			management_vlan = vlan;
+			if (!vlan) 
+				print_string("Management VLAN disabled\n");
+			else
+				print_string("Management VLAN set to "); print_short(management_vlan); write_char('\n');
+			return;
+		}
 		uint8_t w = 2;
-		write_char('#');
-		print_byte(cmd_words_b[w] );
-		write_char('#'); write_char(cmd_buffer[cmd_words_b[w]]);
 		if (cmd_words_b[w] > 0 && isletter(cmd_buffer[cmd_words_b[w]])) {
 			register uint8_t i = 0;
 			vlan_names[vlan_ptr++] = hex[(vlan >> 8) & 0xf];
 			vlan_names[vlan_ptr++] = hex[(vlan >> 4) & 0xf] ;
 			vlan_names[vlan_ptr++] = hex[vlan & 0xf];
-			print_string("COPYING: >");
 			while(cmd_buffer[cmd_words_b[w] + i] != ' ') {
 				write_char(cmd_buffer[cmd_words_b[w] + i]);
 				vlan_names[vlan_ptr++] = cmd_buffer[cmd_words_b[w] + i++];
diff --git a/httpd/page_impl.c b/httpd/page_impl.c
index 2362a00..aa7e38f 100644
--- a/httpd/page_impl.c
+++ b/httpd/page_impl.c
@@ -5,6 +5,7 @@
 #include "rtl837x_regs.h"
 #include "rtl837x_port.h"
 #include "rtl837x_flash.h"
+#include "rtl837x_pins.h"
 #include "uip.h"
 #include "html_data.h"
 #include <stdint.h>
@@ -178,7 +179,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, i2c_bus_from_scl_pin(machine.sfp_port[slot].i2c.scl) << 5 | i2c_bus_from_sda_pin(machine.sfp_port[slot].i2c.sda) << 2);
 	reg_write_m(RTL837X_REG_I2C_CTRL);
 
 	REG_WRITE(RTL837X_REG_I2C_IN, 0, 0, 0, reg);
@@ -607,16 +608,16 @@ void send_status(void)
 			}
 		} else {
 			slen += strtox(outbuf + slen, ",\"isSFP\":0,\"enabled\":");
-			phy_read(i, 0x1f, 0xa610);
+			phy_read(i, PHY_MMD31, 0xa610);
 			bool_to_html(SFR_DATA_8 == 0x20);
 			slen += strtox(outbuf + slen, ",\"adv\":\"");
-			phy_read(i, PHY_MMD_AN, 0x20);
+			phy_read(i, PHY_MMD_AN, PHY_ANEG_MGBASE_CTRL);
 			uint16_t w = SFR_DATA_U16;
 			bool_to_html(!!(w & 0x80));		// 2500BaseN-Full
-			phy_read(i, PHY_MMD_CTRL, 0xa412);
+			phy_read(i, PHY_MMD31, PHY_MMD31_GBCR);
 			w = SFR_DATA_U16;
 			bool_to_html(!!(w & 0x0200));		// 1000Base-Full
-			phy_read(i, PHY_MMD_AN, 0x10);
+			phy_read(i, PHY_MMD_AN, PHY_ANEG_ADV);
 			w = SFR_DATA_U16;
 			bool_to_html(!!(w & 0x0100));		// 100Base-Full
 			bool_to_html(!!(w & 0x80));		// 100Base-Half
diff --git a/machine.c b/machine.c
index fadd346..84f1d2c 100644
--- a/machine.c
+++ b/machine.c
@@ -1,4 +1,5 @@
 #include "machine.h"
+#include "rtl837x_pins.h"
 
 #ifdef MACHINE_KP_9000_6XHML_X2
 __code const struct machine machine = {
@@ -10,15 +11,17 @@ __code const struct machine machine = {
 	.log_to_phys_port = {0, 0, 0, 5, 1, 2, 3, 4, 6},
 	.phys_to_log_port = {4, 5, 6, 7, 3, 8, 0, 0, 0},
 	.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_detect = GPIO50_I2C_SCL2_UART1_TX,
+	.sfp_port[0].pin_los = GPIO10_LED10,
+	.sfp_port[0].pin_tx_disable = GPIO_NA,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 1,
-	.sfp_port[1].pin_detect = 30,
-	.sfp_port[1].pin_los = 37,
+	.sfp_port[0].i2c = { .sda = GPIO41_I2C_SDA3_MDIO1, .scl = GPIO40_I2C_SCL3_MDC1 },
+	.sfp_port[1].pin_detect = GPIO30_ACL_BIT3_EN,
+	.sfp_port[1].pin_los = GPIO37,
+	.sfp_port[1].pin_tx_disable = GPIO_NA,
 	.sfp_port[1].sds = 0,
-	.sfp_port[1].i2c = 0,
-	.reset_pin = 46,
+	.sfp_port[1].i2c = { .sda = GPIO39_I2C_SDA4, .scl = GPIO40_I2C_SCL3_MDC1 },
+	.reset_pin = GPIO46_I2C_SCL0,
 };
 #elif defined MACHINE_KP_9000_6XH_X
 __code const struct machine machine = {
@@ -30,10 +33,12 @@ __code const struct machine machine = {
 	.log_to_phys_port = {0, 0, 0, 5, 1, 2, 3, 4, 6},
 	.phys_to_log_port = {4, 5, 6, 7, 3, 8, 0, 0, 0},
 	.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_detect = GPIO30_ACL_BIT3_EN,
+	.sfp_port[0].pin_los = GPIO37,
+	.sfp_port[0].pin_tx_disable = GPIO_NA,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0,
+	.sfp_port[0].i2c = { .sda = GPIO39_I2C_SDA4, .scl = GPIO40_I2C_SCL3_MDC1 },
+	.reset_pin = GPIO_NA,
 };
 #elif defined MACHINE_KP_9000_9XH_X_EU
 __code const struct machine machine = {
@@ -45,11 +50,14 @@ __code const struct machine machine = {
 	.log_to_phys_port = {1, 2, 3, 4, 5, 6, 7, 8, 9},
 	.phys_to_log_port = {0, 1, 2, 3, 4, 5, 6, 7, 8},
 	.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_detect = GPIO30_ACL_BIT3_EN,
+	.sfp_port[0].pin_los = GPIO37,
+	.sfp_port[0].pin_tx_disable = GPIO_NA,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0,
+	.sfp_port[0].i2c = { .sda = GPIO39_I2C_SDA4, .scl = GPIO40_I2C_SCL3_MDC1 },
+	.reset_pin = GPIO_NA,
 };
+
 #elif defined MACHINE_SWGT024_V2_0
 __code const struct machine machine = {
 	.machine_name = "SWGT024 V2.0",
@@ -61,17 +69,20 @@ __code const struct machine machine = {
 	.phys_to_log_port = {4, 5, 6, 7, 8, 3, 0, 0, 0},
 	.is_sfp= {0, 0, 0, 2, 0, 0, 0, 0, 1},
 	// Left SFP port (J4)
-	.sfp_port[0].pin_detect = 30,
-	.sfp_port[0].pin_los = 37,
+	.sfp_port[0].pin_detect = GPIO30_ACL_BIT3_EN,
+	.sfp_port[0].pin_los = GPIO37,
+	.sfp_port[0].pin_tx_disable = GPIO_NA,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0, /* GPIO 39 */
+	.sfp_port[0].i2c = { .sda = GPIO39_I2C_SDA4, .scl = GPIO40_I2C_SCL3_MDC1 }, /* GPIO 39 */
 	// Right SFP port (J2)
-	.sfp_port[1].pin_detect = 50,
-	.sfp_port[1].pin_los = 51,
+	.sfp_port[1].pin_detect = GPIO50_I2C_SCL2_UART1_TX,
+	.sfp_port[1].pin_los = GPIO51_I2C_SDA2_UART1_RX,
+	.sfp_port[1].pin_tx_disable = GPIO_NA,
 	.sfp_port[1].sds = 0,
-	.sfp_port[1].i2c = 1, /* GPIO 40 */
-	.reset_pin = 36,
+	.sfp_port[1].i2c = { .sda = GPIO41_I2C_SDA3_MDIO1, .scl = GPIO40_I2C_SCL3_MDC1 }, /* GPIO 40 */
+	.reset_pin = GPIO36_PWM_OUT,
 };
+
 #elif defined DEFAULT_8C_1SFP
 __code const struct machine machine = {
 	.machine_name = "8+1 SFP Port Switch",
@@ -82,10 +93,11 @@ __code const struct machine machine = {
 	.log_to_phys_port = {1, 2, 3, 4, 5, 6, 7, 8, 9},
 	.phys_to_log_port = {0, 1, 2, 3, 4, 5, 6, 7, 8},
 	.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_detect = GPIO30_ACL_BIT3_EN,
+	.sfp_port[0].pin_los = GPIO37,
+	.sfp_port[0].pin_tx_disable = GPIO_NA,
 	.sfp_port[0].sds = 1,
-	.sfp_port[0].i2c = 0,
+	.sfp_port[0].i2c = { .sda = GPIO39_I2C_SDA4, .scl = GPIO40_I2C_SCL3_MDC1 },
+	.reset_pin = GPIO_NA,
 };
-
 #endif
diff --git a/machine.h b/machine.h
index 22d046f..b1051b3 100644
--- a/machine.h
+++ b/machine.h
@@ -16,12 +16,19 @@
 
 // #define DEFAULT_5C_1SFP
 
+typedef struct {
+	// GPIO pins for SDA/SCL
+	uint8_t sda; 
+	uint8_t scl;
+} i2c_bus_t;
+
 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;
+	i2c_bus_t i2c;
 };
 
 typedef struct machine {
@@ -38,4 +45,10 @@ typedef struct machine {
 	int8_t reset_pin;
 };
 
+typedef struct machine_runtime
+{
+	uint8_t isRTL8373 : 1;
+	uint8_t isN : 1;
+};
+
 #endif
diff --git a/phy.h b/phy.h
index 53c0409..f359fac 100644
--- a/phy.h
+++ b/phy.h
@@ -13,13 +13,17 @@
  */
 #define PHY_MMD_PMAPMD	1
 #define PHY_MMD_AN	7
-#define PHY_SDS_CTRL	30
-#define PHY_MMD_CTRL	31
+#define PHY_MMD30	30
+#define PHY_MMD31	31
 
 /*
  * Define registers in Auto-Negotiation page
  */
 #define PHY_ANEG_CTRL		0x00
+#define PHY_ANEG_ADV		0x10
+#define PHY_ANEG_LP_ABILITY	0x13
+#define PHY_ANEG_MGBASE_CTRL 0x20
+#define PHY_ANEG_MGBASE_ADV 0x21
 #define PHY_EEE_ADV		0x3c
 #define PHY_EEE_LP_ABILITY	0x3d
 #define PHY_EEE_ADV2		0x3e
@@ -29,6 +33,17 @@
 #define PHY_EEE_BIT_1G		0x04
 #define PHY_EEE_BIT_100M	0x02
 
+/*
+ * MMD 31 Registers
+ */
+#define PHY_MMD31_FEDCR     0xa400
+#define PHY_MMD31_GBCR      0xa412
+#define PHY_MMD31_GANLPAR   0xa414
+#define PHY_MMD31_PHYCR2    0xa432
+#define PHY_MMD31_PHYSR     0xa434
+
+
+
 /*
  * Define registers in Control page
  */
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_igmp.c b/rtl837x_igmp.c
index ad16b2b..21dadf4 100644
--- a/rtl837x_igmp.c
+++ b/rtl837x_igmp.c
@@ -24,7 +24,7 @@ extern __code struct machine machine;
 
 extern __xdata uint8_t cpuPort;
 extern __xdata uint8_t sfr_data[4];
-
+extern __xdata struct machine_runtime machine_detected;
 extern __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE + 2];
 
 __xdata uint16_t idx;
@@ -90,8 +90,8 @@ void igmp_setup(void) __banked
 	REG_SET(RTL837X_IPV6_PORT_MC_LM_ACT, LOOKUP_MISS_FLOOD);
 
 	// Define ports where unknown MC addresses are flooded to:
-	REG_SET(RTL837X_IPV4_UNKN_MC_FLD_PMSK, machine.isRTL8373? PMASK_9: PMASK_6);
-	REG_SET(RTL837X_IPV6_UNKN_MC_FLD_PMSK, machine.isRTL8373? PMASK_9: PMASK_6);
+	REG_SET(RTL837X_IPV4_UNKN_MC_FLD_PMSK, machine_detected.isRTL8373? PMASK_9: PMASK_6);
+	REG_SET(RTL837X_IPV6_UNKN_MC_FLD_PMSK, machine_detected.isRTL8373? PMASK_9: PMASK_6);
 
 	// Enable lookup of IPv4 MC addresses in table
 	reg_bit_set(RTL837X_L2_CTRL, L2_CTRL_LUT_IPMC_HASH);
@@ -126,7 +126,7 @@ void igmp_setup(void) __banked
 
 /*	// Allow all physical ports to be dynamic router ports
 	reg_read_m(RTL837X_IGMP_ROUTER_PORT);
-	if (isRTL8373) {
+	if (machine_detected.isRTL8373) {
 		REG_WRITE(RTL837X_IGMP_ROUTER_PORT, PMASK_9 >> 8, PMASK_9 & 0xff, sfr_data[1], sfr_data[0]);
 	} else {
 		REG_WRITE(RTL837X_IGMP_ROUTER_PORT, PMASK_6 >> 8, PMASK_6 & 0xff, sfr_data[1], sfr_data[0]);
@@ -142,8 +142,8 @@ void igmp_enable(void) __banked
 	REG_SET(RTL837X_IGMP_TRAP_CFG, IGMP_CPU_PORT | IGMP_TRAP_PRIORITY);
 
 	// Drop unknown IP-MC packets
-	REG_SET(RTL837X_IPV4_PORT_MC_LM_ACT, machine.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
-//	REG_SET(RTL837X_IPV6_PORT_MC_LM_ACT, machine.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
+	REG_SET(RTL837X_IPV4_PORT_MC_LM_ACT, machine_detected.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
+//	REG_SET(RTL837X_IPV6_PORT_MC_LM_ACT, machine_detected.isRTL8373? LOOKUP_MISS_DROP_9: LOOKUP_MISS_DROP_6);
 
 	// Configure per-port IGMP configuration, bits 0-10 enable MC protocol snooping,
 	// bits 16-24 configure max MC group used by that port. Trap to CPU (10)
diff --git a/rtl837x_phy.c b/rtl837x_phy.c
index ae68ab6..97a4909 100644
--- a/rtl837x_phy.c
+++ b/rtl837x_phy.c
@@ -15,14 +15,24 @@
 #include "rtl837x_regs.h"
 #include "rtl837x_phy.h"
 #include "phy.h"
+#include "machine.h"
 
 #pragma codeseg BANK2
 #pragma constseg BANK2
 
 extern __code uint16_t bit_mask[16];
+extern __code const struct machine machine;
+extern __xdata struct machine_runtime machine_detected;
 
-
-__code uint16_t rtl8224_ca[42] = {
+// SDS-settings for RTL8224 first SerDes which is connected to the RTL837x-SOC.
+// Array contrains register-value, and SDS-CMD, which already encodes (sds_index, page, reg).
+// This array is used in phy_config_8224().
+//
+// Note: Adding `Swapping the RX for N-devices`-setting on the end of the array, didn't work.
+// Setting will apply but still no packets flow.
+// Settings are `0x2000, 0xc10c`,
+__code uint16_t rtl8224_sds0_setttings[42] = {
+	// SDS_DATA, SDS_CMD
 	0x4480, 0xc842,
 	0x0400, 0xc9c2,
 	0x6d02, 0xcc42,
@@ -46,48 +56,30 @@ __code uint16_t rtl8224_ca[42] = {
 	0, 0
 };
 
-__code uint16_t rtl8224_cb[60] = {
-	0xc45c, 0xc18c, 0x8040,
-	0x0030, 0xc040, 0x8040,
-	0x0010, 0xc040, 0x8040,
-	0x0050, 0xc040, 0x8040,
-	0x00d0, 0xc040, 0x8040,
-	0x0cd0, 0xc040, 0x8040,
-	0x04d0, 0xc040, 0x8040,
-	0x04d0, 0xc040, 0x8040,
-	0x0cd0, 0xc040, 0x8040,
-	0x00d0, 0xc040, 0x8040,
-	0x00d0, 0xc040, 0x8040,
-	0x0050, 0xc040, 0x8040,
-	0x0010, 0xc040, 0x8040,
-	0x0010, 0xc040, 0x8040,
-	0x0030, 0xc040, 0x8040,
-	0x0000, 0xc040, 0x803e,
-	0x000b, 0xc03e, 0x803e,
-	0x0000, 0xc03e, 0x8042,
-	0x4906, 0xc042, 0x82ec,
-	0xffff,0,0
-};
-
 void rtl8224_phy_enable(void) __banked
 {
 	uint16_t pval;
 
 	// p001e.0a90:00f3 R02f8-000000f3 R02f4-000000fc P000001.1e000a90:00fc
 	print_string("\r\nrtl8224_phy_enable called\r\n");
-	phy_read(RTL8224_PHY_ID, 0x1e, 0xa90);
+	phy_read(RTL8224_PHY_ID, PHY_MMD30, RTL837X_CFG_PHY_MDI_REVERSE);
 	pval = SFR_DATA_U16;
 
 	// PHY Initialization:
 	REG_WRITE(0x2f8, 0, 0, pval >> 8, pval);
-
 	pval &= 0xfff0;
 	pval |= 0x0c;
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
 
-	phy_write(RTL8224_PHY_ID, 0x1e, 0xa90, pval);
+	phy_write(RTL8224_PHY_ID, PHY_MMD30, RTL837X_CFG_PHY_MDI_REVERSE, pval);
 	delay(50);
 
+	if (machine_detected.isN) {
+		print_string("  N-settings");
+		// TX_POLARITY_SWAP
+		rtl8224_write_reg_u16(RTL837X_CFG_PHY_TX_POLARITY_SWAP, 0x596A);
+	}
+
 	print_string("\r\nrtl8224_phy_enable done\r\n");
 }
 
@@ -100,50 +92,50 @@ void phy_config(uint8_t phy) __banked
 	delay(20);
 	// PHY configuration: External 8221B?
 	//	p081e.75f3:ffff P000100.1e0075f3:fffe
-	phy_modify(phy, 0x1e, 0x75f3, 0x0001, 0x0000);
+	phy_modify(phy, PHY_MMD30, 0x75f3, 0x0001, 0x0000);
 	delay(20);
 
 	//	p081e.697a:ffff P000100.1e00697a:ffc1 / p031e.697a:0003 P000008.1e00697a:0001
 	// SERDES OPTION 1 Register (MMD 30.0x6) bits 0-5: 0x01: Set HiSGMII+SGMII
-	phy_modify(phy, 0x1e, 0x697a, 0x003f, 0x0001);
+	phy_modify(phy, PHY_MMD30, 0x697a, 0x003f, 0x0001);
 	delay(20);
 
 	//	p031f.a432:0811 P000008.1f00a432:0831
 	// PHYCR2 PHY Specific Control Register 2, MMD 31. 0xA432), set bit 5: enable EEE
-	phy_modify(phy, 0x1f, 0xa432, 0x0000, 0x0020);
+	phy_modify(phy, PHY_MMD31, PHY_MMD31_PHYCR2, 0x0000, 0x0020);
 
 	//	p0307.003e:0000 P000008.0700003e:0001
 	// EEE avertisment 2 register MMMD 7.0x003e, set bit 0: 2.5G has EEE capability
-	phy_modify(phy, 0x7, 0x3e, 0x0000, 0x0001);
+	phy_modify(phy, PHY_MMD_AN, PHY_EEE_ADV2, 0x0000, 0x0001);
 	delay(20);
 
 	//	p031f.a442:043c P000008.1f00a442:0430
 	// Unknown, but clear bits 2/3
-	phy_modify(phy, 0x1f, 0xa442, 0x000c, 0x0000);
+	phy_modify(phy, PHY_MMD31, 0xa442, 0x000c, 0x0000);
 	delay(20);
 
 	// P000100.1e0075b5:e084
-	phy_write(phy, 0x1e, 0x75b5, 0xe084);
+	phy_write(phy, PHY_MMD30, 0x75b5, 0xe084);
 	delay(20);
 
 	//	p031e.75b2:0000 P000008.1e0075b2:0060
 	// set bits 5/6
-	phy_modify(phy, 0x1e, 0x75b2, 0x0000, 0x0060);
+	phy_modify(phy, PHY_MMD30, 0x75b2, 0x0000, 0x0060);
 	delay(20);
 
 	//	p081f.d040:ffff P000100.1f00d040:feff
 	// LCR6 (LED Control Register 6, MMD 31.D040), set bits 8/9 to 0b10
-	phy_modify(phy, 0x1e, 0xd040, 0x0300, 0x0200);
+	phy_modify(phy, PHY_MMD30, 0xd040, 0x0300, 0x0200);
 	delay(20);
 
 	//	p081f.a400:ffff P000100.1f00a400:ffff, then: p081f.a400:ffff P000100.1f00a400:bfff
 	//	p031f.a400:1040 P000008.1f00a400:5040, then: p031f.a400:5040 P000008.1f00a400:1040
 	// FEDCR (Fast Ethernet Duplex Control Register, MMD 31.0xA400)
 	// Set bit 14, sleep, then clear again, according to the datasheet these bits are reserved
-	phy_modify(phy, 0x1f, 0xa400, 0x0000, 0x4000);
+	phy_modify(phy, PHY_MMD31, PHY_MMD31_FEDCR, 0x0000, 0x4000);
 	delay(20);
 
-	phy_modify(phy, 0x1f, 0xa400, 0x4000, 0x0000);
+	phy_modify(phy, PHY_MMD31, PHY_MMD31_FEDCR, 0x4000, 0x0000);
 	delay(20);
 
 	print_string("\r\n  phy config done\r\n");
@@ -153,10 +145,18 @@ void phy_config(uint8_t phy) __banked
 void phy_config_8224(void) __banked
 {
 	uint16_t pval;
-	print_string("\r\nphy_config_8224 called\r\n");
+	print_string("\r\nphy_config_8224 called\r\nRTL8224 ID: ");
+
+	// Print RTL8224 chip id
+	rtl8224_read_reg_u16(RTL837X_REG_CHIP_ID + 1);
+	print_short(SFR_DATA_U16);
+	rtl8224_read_reg_u16(RTL837X_REG_CHIP_ID);
+	print_byte(SFR_DATA_U16 >> 8);
+	print_byte(SFR_DATA_U16);
+	write_char('\n');
 
 	// p001e.7b20:0bff R02f8-00000bff R02f4-00000bed P000001.1e007b20:0bed
-	phy_read(RTL8224_PHY_ID, 0x1e, 0x7b20);
+	phy_read(RTL8224_PHY_ID, PHY_MMD30, 0x7b20);
 	pval = SFR_DATA_U16;
 
 	REG_WRITE(0x2f8, 0, 0, pval >> 8, pval);
@@ -164,16 +164,16 @@ void phy_config_8224(void) __banked
 	pval |= 0x000d;
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
 
-	phy_write(RTL8224_PHY_ID, 0x1e, 0x7b20, pval);
+	phy_write(RTL8224_PHY_ID, PHY_MMD30, 0x7b20, pval);
 
 	uint8_t i = 0;
-	while (rtl8224_ca[i]) {
-		phy_write(RTL8224_PHY_ID, 0x1e, 0x400, rtl8224_ca[i]);
+	while (rtl8224_sds0_setttings[i]) {
+		rtl8224_write_reg_u16(RTL837X_SDS_INDACS_WRITE_DATA, rtl8224_sds0_setttings[i]);
 		i++;
-		phy_write(RTL8224_PHY_ID, 0x1e, 0x3f8, rtl8224_ca[i]);
+		rtl8224_write_reg_u16(RTL837X_SDS_INDACS_CMD, rtl8224_sds0_setttings[i]);
 		i++;
 		do {
-			phy_read(RTL8224_PHY_ID, 0x1e, 0x3f8);
+			rtl8224_read_reg_u16(0x3f8);
 		} while (SFR_DATA_8 & 0x80);
 	}
 
@@ -189,66 +189,66 @@ void phy_config_8224(void) __banked
 void phy_set_speed(uint8_t port, uint8_t speed, uint8_t duplex) __banked
 {
 	uint16_t v;
-	phy_read(port, PHY_MMD_CTRL, 0xa610);
+	phy_read(port, PHY_MMD31, 0xa610);
 	v = SFR_DATA_U16;
 	if (speed == PHY_OFF) {
-		phy_write(port, PHY_MMD_CTRL, 0xa610, v | 0x0800);
+		phy_write(port, PHY_MMD31, 0xa610, v | 0x0800);
 		return;
 	}
 	// Port is on, make sure of it:
 	if (v & 0x0800)
-		phy_write(port, PHY_MMD_CTRL, 0xa610, v & 0xf7ff);
+		phy_write(port, PHY_MMD31, 0xa610, v & 0xf7ff);
 
 	if (speed == PHY_SPEED_AUTO) {
 			// AN Advertisement Register (MMD 7.0x0010)
 			// bits 0-4: 0x1 (802.3 supported), Extended Next Page format used
-			phy_write(port, PHY_MMD_AN, 0x10, 0x15e1);
+			phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x15e1);
 			// Multi-GBASE-TBASE-T AN Control 1 Register (MMD 7.0x0020)
 			// bit 14: SLAVE, bit 13: Multi-Port device, bit 8: 2.5GBit available, 1: LD
-			phy_write(port, PHY_MMD_AN, 0x20, 0x6081);
+			phy_write(port, PHY_MMD_AN, PHY_ANEG_MGBASE_CTRL, 0x6081);
 			// GBCR (1000Base-T Control Register, MMD 31.0xA412)
-			phy_modify(port, PHY_MMD_CTRL, 0xa412, 0x0000, 0x0200); // Loop timing enabled
-			phy_write(port, PHY_MMD_AN, 0x00, 0x3200);	// Restart AN
+			phy_modify(port, PHY_MMD31, PHY_MMD31_GBCR, 0x0000, 0x0200); // Loop timing enabled
+			phy_write(port, PHY_MMD31, PHY_ANEG_CTRL, 0x3200);	// Restart AN
 	} else {
 		// AN Control Register (MMD 7.0x0000)
-		phy_write(port, PHY_MMD_AN, 0x00, 0x2000);	// Clear bit 12: No Autoneg, Set Extended Pages (bit 13)
+		phy_write(port, PHY_MMD31, PHY_ANEG_CTRL, 0x2000);	// Clear bit 12: No Autoneg, Set Extended Pages (bit 13)
 		if (speed == PHY_SPEED_10M) {
-			phy_write(port, PHY_MMD_AN, 0x20, 0x6001);
+			phy_write(port, PHY_MMD_AN, PHY_ANEG_MGBASE_CTRL, 0x6001);
 			if (!duplex)
-				phy_write(port, PHY_MMD_AN, 0x10, 0x1421);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x1421);
 			else if (duplex == 1)
-				phy_write(port, PHY_MMD_AN, 0x10, 0x1441);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x1441);
 			else
-				phy_write(port, PHY_MMD_AN, 0x10, 0x1461);
-			phy_modify(port, PHY_MMD_CTRL, 0xa412, 0x0200, 0x0000);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x1461);
+			phy_modify(port, PHY_MMD31, PHY_MMD31_GBCR, 0x0200, 0x0000);
 		} else if (speed == PHY_SPEED_100M) {
-			phy_write(port, PHY_MMD_AN, 0x20, 0x6001);
+			phy_write(port, PHY_MMD_AN, PHY_ANEG_MGBASE_CTRL, 0x6001);
 			if (!duplex)
-				phy_write(port, PHY_MMD_AN, 0x10, 0x1481);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x1481);
 			if (duplex == 1)
-				phy_write(port, PHY_MMD_AN, 0x10, 0x1501);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x1501);
 			else
-				phy_write(port, PHY_MMD_AN, 0x10, 0x1581);
-			phy_modify(port, PHY_MMD_CTRL, 0xa412, 0x0200, 0x0000);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x1581);
+			phy_modify(port, PHY_MMD31, PHY_MMD31_GBCR, 0x0200, 0x0000);
 		} else {
 			// AN Advertisement Register (MMD 7.0x0010)
 			// bits 0-4: 0x1 (802.3 supported), Extended Next Page format used
-			phy_write(port, PHY_MMD_AN, 0x10, 0x1001);
+			phy_write(port, PHY_MMD_AN, PHY_ANEG_ADV, 0x1001);
 			if (speed == PHY_SPEED_1G) {
 				// Multi-GBASE-TBASE-T AN Control 1 Register (MMD 7.0x0020)
 				// bit 14: SLAVE, bit 13: Multi-Port device, 1: LD Loop timin enableed
-				phy_write(port, PHY_MMD_AN, 0x20, 0x6001);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_MGBASE_CTRL, 0x6001);
 				// GBCR (1000Base-T Control Register, MMD 31.0xA412)
-				phy_modify(port, PHY_MMD_CTRL, 0xa412, 0x0000, 0x0200);
+				phy_modify(port, PHY_MMD31, PHY_MMD31_GBCR, 0x0000, 0x0200);
 			} else if (speed == PHY_SPEED_2G5) {
 				// Multi-GBASE-TBASE-T AN Control 1 Register (MMD 7.0x0020)
 				// bit 14: SLAVE, bit 13: Multi-Port device, bit 8: 2.5GBit available, 1: LD Loop timin enableed
-				phy_write(port, PHY_MMD_AN, 0x20, 0x6081);
+				phy_write(port, PHY_MMD_AN, PHY_ANEG_MGBASE_CTRL, 0x6081);
 				// GBCR (1000Base-T Control Register, MMD 31.0xA412)
-				phy_modify(port, PHY_MMD_CTRL, 0xa412, 0x0200, 0x0000);
+				phy_modify(port, PHY_MMD31, PHY_MMD31_GBCR, 0x0200, 0x0000);
 			}
 		}
-		phy_write(port, PHY_MMD_AN, 0x00, 0x3000);	// Enable AN
+		phy_write(port, PHY_MMD31, PHY_ANEG_CTRL, 0x3000);	// Enable AN
 	}
 }
 
@@ -256,36 +256,36 @@ void phy_set_speed(uint8_t port, uint8_t speed, uint8_t duplex) __banked
 void phy_set_duplex(uint8_t port, uint8_t fullduplex) __banked
 {
 	uint16_t v;
-	phy_read(port, PHY_MMD_AN, 0x00);
+	phy_read(port, PHY_MMD31, PHY_ANEG_CTRL);
 	v = SFR_DATA_U16;	
 	if (!(v & 0x1000)) { // AN disabled, we are in forced mode
-		phy_read(port, PHY_MMD_CTRL, 0xa400);
+		phy_read(port, PHY_MMD31, PHY_MMD31_FEDCR);
 		v = SFR_DATA_U16;
 		if (fullduplex)
 			v |= 0x0100;
 		else
 			v &= 0xfeff;
-		phy_write(port, PHY_MMD_CTRL, 0xa400, v);
+		phy_write(port, PHY_MMD31, PHY_MMD31_FEDCR, v);
 		return;
 	}
 	// Disable AN
-	phy_write(port, PHY_MMD_AN, 0x00, 0x2000);
-	phy_read(port, PHY_MMD_AN, 0x10);
+	phy_write(port, PHY_MMD31, PHY_ANEG_CTRL, 0x2000);
+	phy_read(port, PHY_MMD_AN, PHY_ANEG_ADV);
 	v = SFR_DATA_U16;
 	if (v & 0x0060) {
 		if (fullduplex)
-			phy_modify(port, PHY_MMD_AN, 0x10, 0xffbf, 0x0040);
+			phy_modify(port, PHY_MMD_AN, PHY_ANEG_ADV, 0xffbf, 0x0040);
 		else
-			phy_modify(port, PHY_MMD_AN, 0x10, 0xffdf, 0x0020);
+			phy_modify(port, PHY_MMD_AN, PHY_ANEG_ADV, 0xffdf, 0x0020);
 	}
 	if (v & 0x0180) {
 		if (fullduplex)
-			phy_modify(port, PHY_MMD_AN, 0x10, 0xfeff, 0x0100);
+			phy_modify(port, PHY_MMD_AN, PHY_ANEG_ADV, 0xfeff, 0x0100);
 		else
-			phy_modify(port, PHY_MMD_AN, 0x10, 0xff7f, 0x0080);
+			phy_modify(port, PHY_MMD_AN, PHY_ANEG_ADV, 0xff7f, 0x0080);
 	}
 	// Restart AN
-	phy_write(port, PHY_MMD_AN, 0x00, 0x3000);
+	phy_write(port, PHY_MMD31, PHY_ANEG_CTRL, 0x3000);
 }
 
 
@@ -295,7 +295,7 @@ void phy_show(uint8_t port) __banked
 
 	// The actual PHY speed is in a Realtek propriatary register
 	print_string("\nLink speed: ");
-	phy_read(port, PHY_MMD_CTRL, 0xA434);
+	phy_read(port, PHY_MMD31, PHY_MMD31_PHYSR);
 	v = SFR_DATA_U16;
 	switch(((v & 0x0600) >> 7) | ((v & 0x0030) >> 4)) {
 	case 0:
@@ -327,7 +327,7 @@ void phy_show(uint8_t port) __banked
 	else
 		print_string(" half duplex");
 
-	phy_read(port,  PHY_MMD_AN, 0x00);
+	phy_read(port,  PHY_MMD31, PHY_ANEG_CTRL);
 	v = SFR_DATA_U16;
 	if (!(v & 0x1000)) { // AN disabled, we are in forced mode
 		phy_read(port, PHY_MMD_PMAPMD, 0);
@@ -363,7 +363,7 @@ void phy_show(uint8_t port) __banked
 		default:
 			print_string("Unknown\n");
 		}
-		phy_read(port, PHY_MMD_CTRL, 0xa400);
+		phy_read(port, PHY_MMD31, PHY_MMD31_FEDCR);
 		v = SFR_DATA_U16;
 		print_string("Duplex: "); print_short(v); print_string(" enabled: ");
 		if (v & 0x100)
@@ -374,7 +374,7 @@ void phy_show(uint8_t port) __banked
 
 	} else {
 		print_string("\nAN enabled, advertising:");
-		phy_read(port, PHY_MMD_AN, 0x10);
+		phy_read(port, PHY_MMD_AN, PHY_ANEG_ADV);
 		v = SFR_DATA_U16;
 		if (v & 0x0020)
 			print_string(" 10Base-Half");
@@ -384,16 +384,16 @@ void phy_show(uint8_t port) __banked
 			print_string(" 100Base-Half");
 		if (v & 0x0100)
 			print_string(" 100Base-Full");
-		phy_read(port, PHY_MMD_CTRL, 0xa412);
+		phy_read(port, PHY_MMD31, PHY_MMD31_GBCR);
 		v = SFR_DATA_U16;
 		if (v & 0x0200)
 			print_string(" 1000Base-Full");
-		phy_read(port, PHY_MMD_AN, 0x20);
+		phy_read(port, PHY_MMD_AN, PHY_ANEG_MGBASE_CTRL);
 		v = SFR_DATA_U16;
 		if (v & 0x0080)
 			print_string(" 2500BaseN-Full");
 	}
-	phy_read(port, PHY_MMD_AN, 0x13);
+	phy_read(port, PHY_MMD_AN, PHY_ANEG_LP_ABILITY);
 	v = SFR_DATA_U16;
 	print_string("\nLink Partner advertises:");
 	if (v & 0x0020)
@@ -404,13 +404,13 @@ void phy_show(uint8_t port) __banked
 		print_string(" 100Base-Half");
 	if (v & 0x0100)
 		print_string(" 100Base-Full");
-	phy_read(port, PHY_MMD_CTRL, 0xa414);
+	phy_read(port, PHY_MMD31, PHY_MMD31_GANLPAR);
 	v = SFR_DATA_U16;
 	if (v & 0x0400)
 		print_string(" 1000Base-Half");
 	if (v & 0x0800)
 		print_string(" 1000Base-Full");
-	phy_read(port, PHY_MMD_AN, 33);
+	phy_read(port, PHY_MMD_AN, PHY_ANEG_MGBASE_ADV);
 	v = SFR_DATA_U16;
 	if (v & 0x0020)
 		print_string(" 2500Base-Full");
@@ -425,15 +425,88 @@ void phy_show(uint8_t port) __banked
 void phy_reset(uint8_t port) __banked
 {
 	uint16_t v;
-	phy_read(port, PHY_MMD_CTRL, 0xa610);
+	phy_read(port, PHY_MMD31, 0xa610);
 	v = SFR_DATA_U16;
 	// If PHY off, do nothing
 	if (v & 0x0800)
 		return;
 
 	// Disable PHY
-	phy_write(port, PHY_MMD_CTRL, 0xa610, v | 0x0800);
+	phy_write(port, PHY_MMD31, 0xa610, v | 0x0800);
 	delay(2);
 	// Re-enable PHY
-	phy_write(port, PHY_MMD_CTRL, 0xa610, v & 0xf7ff);
+	phy_write(port, PHY_MMD31, 0xa610, v & 0xf7ff);
+}
+
+// Read RTL8224 register.
+// Registers names are the same as on the RTL837x.
+// Reading only reads the lower 16-bit part of the 32-bit register.
+// When also needing read the upper 16-bits, use register address + 1.
+// Readed values it return via sfr-data.
+void inline rtl8224_read_reg_u16(uint16_t reg) __banked
+{
+	//	void phy_read(uint8_t phy_id, uint8_t dev_id, uint16_t reg)
+	// phy_read(RTL8224_PHY_ID, PHY_MMD30, reg);
+
+	SFR_SMI_REG_U16 = reg;		// c2, c2
+
+	SFR_SMI_PHY = RTL8224_PHY_ID;		// a5
+	SFR_SMI_DEV = PHY_MMD30 << 3 | 2;	// c4
+
+	SFR_EXEC_GO = SFR_EXEC_READ_SMI;
+	do {
+	} while (SFR_EXEC_STATUS != 0);
+}
+
+// Write RTL8224 register.
+// Registers names are the same as on the RTL837x.
+// Writing only the lower 16-bit part of the 32-bit register.
+// When also needing to write the upper 16-bits, use register address + 1.
+void inline rtl8224_write_reg_u16(uint16_t reg, uint16_t val) __banked
+{
+	SFR_DATA_U16 = val;			    // SFR_A6, SFR_A7
+	SFR_SMI_REG_U16 = reg;			// SFR_C2, SFR_C3
+
+	//void phy_write(uint8_t phy_id, uint8_t dev_id, uint16_t reg, uint16_t v)
+	// phy_write(RTL8224_PHY_ID, PHY_MMD30, reg, val);
+
+	uint16_t phy_mask = bit_mask[RTL8224_PHY_ID];
+
+	SFR_SMI_PHYMASK = phy_mask;		// SFR_C5
+	SFR_SMI_DEV = (phy_mask >> 8) | PHY_MMD30 << 3 | 2; // SFR_C4: bit 2 can also be set for some option
+	SFR_EXEC_GO = SFR_EXEC_WRITE_SMI;
+	do {
+	} while (SFR_EXEC_STATUS != 0);
+}
+
+// // Modify RTL8224 register.
+// // Registers names are the same as on the RTL837x.
+// // Modifies only the lower 16-bit part of the 32-bit register.
+// // When also needing to modifie the upper 16-bits, use register address + 1.
+// void rtl8224_modify_reg_u16(uint16_t reg, uint16_t clear, uint16_t set) __banked
+// {
+// 	phy_read(RTL8224_PHY_ID, PHY_MMD30, reg);
+// 	uint16_t pval = SFR_DATA_U16;
+// 	pval &= ~(clear);
+// 	pval |= set;
+// 	phy_write(RTL8224_PHY_ID, PHY_MMD30, reg, pval);
+// }
+
+
+// Write to the RTL8224 SDS registers.
+void rtl8224_sds_write(uint16_t sds_cmd, uint16_t value) __banked
+{
+	// Wait for command bit is cleared
+	do {
+		rtl8224_read_reg_u16(RTL837X_SDS_INDACS_CMD);
+	} while (SFR_DATA_8 & 0x80);
+
+	rtl8224_write_reg_u16(RTL837X_SDS_INDACS_WRITE_DATA, value);
+
+	rtl8224_write_reg_u16(RTL837X_SDS_INDACS_CMD, sds_cmd);
+
+	// Wait for command bit is cleared
+	do {
+		rtl8224_read_reg_u16(RTL837X_SDS_INDACS_CMD);
+	} while (SFR_DATA_8 & 0x80);
 }
diff --git a/rtl837x_phy.h b/rtl837x_phy.h
index d5bd2de..0b01e45 100644
--- a/rtl837x_phy.h
+++ b/rtl837x_phy.h
@@ -17,5 +17,15 @@ void phy_set_speed(uint8_t port, uint8_t speed, uint8_t duplex) __banked;
 void phy_set_duplex(uint8_t port, uint8_t fullduplex) __banked;
 void phy_show(uint8_t port) __banked;
 void phy_reset(uint8_t port) __banked;
+void rtl8224_read_reg_u16(uint16_t reg) __banked;
+void rtl8224_write_reg_u16(uint16_t reg, uint16_t val) __banked;
+void rtl8224_sds_write(uint16_t sds_cmd, uint16_t val) __banked;
+
+#define	RTL8224_SDS_WRITE(sds_id, page, reg, v) uint16_t _sdscmd = (uint16_t)(sds_id & 0x01) | (1 << 14) | (1 << 15); \
+	_sdscmd |= (page & 0x3F) << 1; \
+	_sdscmd |= ((uint16_t)(reg & 0x1f)) << 7; \
+	print_string("CMD: "); print_short(_sdscmd); \
+	write_char('-'); print_short(v); \
+    rtl8224_sds_write(_sdscmd, v);
 
 #endif
diff --git a/rtl837x_pins.c b/rtl837x_pins.c
new file mode 100644
index 0000000..a6cbe06
--- /dev/null
+++ b/rtl837x_pins.c
@@ -0,0 +1,120 @@
+#include "rtl837x_pins.h"
+#include "rtl837x_common.h"
+#include "rtl837x_regs.h"
+
+uint8_t i2c_bus_from_sda_pin(uint8_t sda_pin) {
+	switch (sda_pin) {
+		case GPIO47_I2C_SDA0:
+			return 0;
+		case GPIO49_I2C_SDA1:
+			return 1;
+		case GPIO51_I2C_SDA2_UART1_RX:
+			return 2;
+		case GPIO41_I2C_SDA3_MDIO1:
+			return 3;
+		case GPIO39_I2C_SDA4:
+			return 4;
+		default:
+			return 0xFF;
+	}
+}
+
+uint8_t i2c_bus_from_scl_pin(uint8_t scl_pin) {
+	switch (scl_pin) {
+		case GPIO46_I2C_SCL0:
+			return 0;
+		case GPIO48_I2C_SCL1:
+			return 1;
+		case GPIO50_I2C_SCL2_UART1_TX:
+			return 2;
+		case GPIO40_I2C_SCL3_MDC1:
+			return 3;
+		default:
+			return 0xFF;
+	}
+}
+
+/* Returns RTL837X_REG_GPIO_XX_OUTPUT register address */
+static uint16_t gpio_output_reg(uint8_t pin) {
+	return pin < 32 ? RTL837X_REG_GPIO_00_31_OUTPUT : RTL837X_REG_GPIO_32_63_OUTPUT;
+} 
+
+/* Returns RTL837X_REG_GPIO_XX_DIRECTION register address */
+static uint16_t gpio_direction_reg(uint8_t pin) {
+	return pin < 32 ? RTL837X_REG_GPIO_00_31_DIRECTION : RTL837X_REG_GPIO_32_63_DIRECTION;
+} 
+
+
+/* Enable GPIO functions for pin */
+static void gpio_mux_setup(uint8_t pin)
+{
+	// Some GPIOs require setting MUX registers to enable GPIO
+	switch (pin) {
+		case GPIO10_LED10:
+			reg_bit_clear(RTL837X_PIN_MUX_0, 10);
+			break;
+		case GPIO30_ACL_BIT3_EN:
+			reg_bit_clear(RTL837X_PIN_MUX_2, 3);
+			break;
+		case GPIO36_PWM_OUT:
+			reg_bit_set(RTL837X_PIN_MUX_1, 30);
+			break;
+		case GPIO37:
+		case GPIO38:
+			// Intentionally empty, always GPIO
+			break;
+		case GPIO46_I2C_SCL0:
+			// Bit 7-8 0b00 -> GPIO
+			reg_read_m(RTL837X_PIN_MUX_1);
+			sfr_mask_data(0, 0x80, 0x00);
+			sfr_mask_data(1, 0x01, 0x00);
+			reg_write_m(RTL837X_PIN_MUX_1);	
+			break;
+		case GPIO50_I2C_SCL2_UART1_TX:
+			// 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);	
+			break;
+		case GPIO51_I2C_SDA2_UART1_RX:
+			// 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);
+			break;
+		case GPIO54_ACL_BIT2_EN:
+			reg_bit_clear(RTL837X_PIN_MUX_2, 2);
+			break;
+		case GPIO_NA:
+			print_string("Attemped to assign GPIO function to N/A pin!");
+			break;
+		default:
+			print_string("GPIO MUX setup not implemented for pin="); print_byte(pin); print_string("\n");
+	}
+}
+
+void gpio_input_setup(uint8_t pin) {
+	if (pin == GPIO_NA) { 
+		return;
+	}
+
+	gpio_mux_setup(pin);
+	reg_bit_clear(gpio_direction_reg(pin), (pin % 32));
+}
+
+void gpio_output_setup(uint8_t pin, __xdata uint8_t initial_val) {
+	if (pin == GPIO_NA) { 
+		return;
+	}
+	gpio_mux_setup(pin);
+
+	// We need to setup value before enabling output on PIN
+	if (initial_val) {
+		reg_bit_set(gpio_output_reg(pin), (pin % 32));
+	} else {
+		reg_bit_clear(gpio_output_reg(pin), (pin % 32));
+	}
+
+	reg_bit_set(gpio_direction_reg(pin), (pin % 32));
+}
diff --git a/rtl837x_pins.h b/rtl837x_pins.h
new file mode 100644
index 0000000..b8768f7
--- /dev/null
+++ b/rtl837x_pins.h
@@ -0,0 +1,93 @@
+#ifndef _RTL837X_PINS_H_
+#define _RTL837X_PINS_H_
+
+#include <stdint.h>
+
+#define GPIO0_LED0                0
+#define GPIO1_LED1                1
+#define GPIO2_LED2                2
+#define GPIO3_LED3                3
+#define GPIO4_LED4                4
+#define GPIO5_LED5                5
+#define GPIO6_LED6                6
+#define GPIO7_LED7                7
+#define GPIO8_LED8                8
+#define GPIO9_LED9                9
+#define GPIO10_LED10              10
+#define GPIO11_LED11              11
+#define GPIO12_LED12              12
+#define GPIO13_LED13              13
+#define GPIO14_LED14              14
+#define GPIO15_LED15              15
+#define GPIO16_LED16              16
+#define GPIO17_LED17              17
+#define GPIO18_LED18              18
+#define GPIO19_LED19              19
+#define GPIO20_LED20              20
+#define GPIO21_LED21              21
+#define GPIO22_LED22              22
+#define GPIO23_LED23              23
+#define GPIO24_LED24              24
+#define GPIO25_LED25              25
+#define GPIO26_LED26              26
+#define GPIO27_LED27              27
+#define GPIO28_SYS_LED            28
+#define GPIO29_GLB_RLDP_LED_EN    29
+#define GPIO30_ACL_BIT3_EN        30
+#define GPIO31_UART0_TX           31
+#define GPIO32_UART0_RX           32
+#define GPIO33_INT                33
+#define GPIO34_MDC0               34
+#define GPIO35_MDIO0              35
+#define GPIO36_PWM_OUT            36
+#define GPIO37                    37
+#define GPIO38                    38
+#define GPIO39_I2C_SDA4           39
+#define GPIO40_I2C_SCL3_MDC1      40
+#define GPIO41_I2C_SDA3_MDIO1     41
+#define GPIO42_SPI                42
+#define GPIO43_SPI                43
+#define GPIO44_SPI                44
+#define GPIO45_SPI                45
+#define GPIO46_I2C_SCL0           46
+#define GPIO47_I2C_SDA0           47
+#define GPIO48_I2C_SCL1           48
+#define GPIO49_I2C_SDA1           49
+#define GPIO50_I2C_SCL2_UART1_TX  50
+#define GPIO51_I2C_SDA2_UART1_RX  51
+#define GPIO52_ACL_BIT0_EN        52
+#define GPIO53_ACL_BIT1_EN        53
+#define GPIO54_ACL_BIT2_EN        54
+#define GPIO55_PTP_CLK_IN         55
+#define GPIO56_PTP_CLK_OUT        56
+#define GPIO57_PTP_TOD_OUT        57
+#define GPIO58_PTP_PPS_OUT        58
+#define GPIO59_PTP_TOD_IN         59
+#define GPIO60_PTP_PPS_IN         60
+#define GPIO61_SYNCELOCK0         61
+#define GPIO62_SYNCELOCK1         62
+#define GPIO63_MDIO               63
+
+/* Not available GPIO */
+#define GPIO_NA                   0xFF
+
+/* Convert SDA PIN GPIO to I2C bus number */
+uint8_t i2c_bus_from_sda_pin(uint8_t sda_pin);
+
+/* Convert SCL PIN GPIO to I2C bus number */
+uint8_t i2c_bus_from_scl_pin(uint8_t scl_pin);
+
+/*
+ * Setup a GPIO pin as input
+ * pin: GPIO pin number 0-63
+ */
+void gpio_input_setup(uint8_t pin);
+
+/*
+ * Setup a GPIO pin as output
+ * pin: GPIO pin number 0-63
+ * initial_val: 1 for bit set in RTL837X_REG_GPIO_xx_OUTPUT, 0 for bit not set
+ */
+void gpio_output_setup(uint8_t pin, __xdata uint8_t initial_val);
+
+#endif
diff --git a/rtl837x_port.c b/rtl837x_port.c
index 3be1ba3..d0e7611 100644
--- a/rtl837x_port.c
+++ b/rtl837x_port.c
@@ -24,6 +24,7 @@ extern __code struct machine machine;
 extern __xdata uint8_t sfr_data[4];
 extern __xdata uint16_t vlan_ptr;
 extern __xdata uint8_t vlan_names[VLAN_NAMES_SIZE];
+extern __xdata struct machine_runtime machine_detected;
 
 __xdata	uint32_t l2_head;
 
@@ -133,7 +134,7 @@ void vlan_create(register uint16_t vlan, register uint16_t members, register uin
 
 	uint16_t a = (~members) ^ tagged ^ members;
 	// On RTL8372, port-bits 0-2 must be 0, although they are not members
-	if (!machine.isRTL8373) {
+	if (!machine_detected.isRTL8373) {
 		a &= 0x1f8;
 		tagged &= 0x3f8;
 	}
@@ -164,7 +165,7 @@ void vlan_setup(void) __banked
 	vlan_names[0] = 0;
 
 	// Initialize VLAN table for VLAN 1, by disabling that entry
-	REG_SET(RTL837x_TBL_DATA_IN_A, machine.isRTL8373? 0x0007ffff : 0x0007e3f8);
+	REG_SET(RTL837x_TBL_DATA_IN_A, machine_detected.isRTL8373? 0x0007ffff : 0x0007e3f8);
 
 	REG_SET(RTL837X_TBL_CTRL, 0x00010303);
 	do {
@@ -208,7 +209,7 @@ void vlan_setup(void) __banked
 	REG_SET(RTL837X_VLAN_L2_LRN_DIS_1, 0);
 
 	// Enable VLAN 1: Ports 0-9, i.e. including the CPU port are untagged members
-	REG_SET(RTL837x_TBL_DATA_IN_A, machine.isRTL8373? 0x0207ffff : 0x0207e3f8);	// 02: Entry valid, 7...: membership
+	REG_SET(RTL837x_TBL_DATA_IN_A, machine_detected.isRTL8373? 0x0207ffff : 0x0207e3f8);	// 02: Entry valid, 7...: membership
 
 	REG_SET(RTL837X_TBL_CTRL, 0x00010303);	// Write VLAN 1
 	do {
@@ -240,7 +241,7 @@ uint8_t port_l2_forget(void) __banked
 	REG_SET(RTL837x_L2_TBL_FLUSH_CNF, 0x0);
 
 	// Flush L2 table for all ports by setting the ports and the flush-exec bit (bit 16)
-	REG_SET(RTL837x_L2_TBL_FLUSH_CTRL, L2_TBL_FLUSH_EXEC | (machine.isRTL8373 ? PMASK_9 : PMASK_6));
+	REG_SET(RTL837x_L2_TBL_FLUSH_CTRL, L2_TBL_FLUSH_EXEC | (machine_detected.isRTL8373 ? PMASK_9 : PMASK_6));
 
 	// Wait for flush completed
 	do {
@@ -333,7 +334,7 @@ void port_l2_setup(void) __banked
 
 		// All ports may communicate with each other and CPU-Port
 		reg = RTL837X_PORT_ISOLATION_BASE + (i << 2);
-		REG_SET(reg, PMASK_CPU | (machine.isRTL8373? PMASK_9 : PMASK_6));
+		REG_SET(reg, PMASK_CPU | (machine_detected.isRTL8373? PMASK_9 : PMASK_6));
 	}
 	// When maximim entries learned, then simply flood the packet
 	reg_bit_set(RTL837X_L2_LRN_PORT_CONSTRT_ACT, 0);
@@ -349,26 +350,16 @@ void port_stats_print(void) __banked
 		write_char('0' + machine.log_to_phys_port[i]); write_char('\t');
 
 		if (!machine.is_sfp[i]) {
-			phy_read(i, 0x1f, 0xa610);
+			phy_read(i, PHY_MMD31, 0xa610);
 			if (SFR_DATA_8 == 0x20)
 				print_string("On\t");
 			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/rtl837x_regs.h b/rtl837x_regs.h
index 9f38fa7..cd7a2eb 100644
--- a/rtl837x_regs.h
+++ b/rtl837x_regs.h
@@ -1,7 +1,8 @@
 #ifndef _RTL837X_REGS_H_
 #define _RTL837X_REGS_H_
 
-#define RTL837X_REG_CHIP_INFO		0x000c
+#define RTL837X_REG_CHIP_ID		0x0004
+#define RTL837X_REG_CHIP_INFO	0x000c
 #define RTL837X_REG_RESET		0x0024
 #define RESET_SOC_BIT			0
 #define RESET_NIC_BIT			2
@@ -35,7 +36,20 @@
 #define RTL837X_REG_SEC_COUNTER2 0x06f8
 // Used for counting seconds
 
-#define RTL837X_REG_SDS_MODES 0x7b20
+
+/*
+ * SDS
+ */
+#define RTL837X_SDS_INDACS_CMD			0x3F8
+#define RTL837X_SDS_INDACS_WRITE_DATA	0x400
+#define RTL837X_REG_SDS_MODES			0x7b20
+
+/*
+ * PHY
+ */
+ #define RTL837X_CFG_PHY_TX_POLARITY_SWAP	0xA94
+ #define RTL837X_CFG_PHY_MDI_REVERSE		0xA90
+
 /*
  * 5 Bits each give the state of the 2 SerDes of the RTL8372
  * Values are:
diff --git a/rtlplayground.c b/rtlplayground.c
index 041282f..b2bc882 100644
--- a/rtlplayground.c
+++ b/rtlplayground.c
@@ -8,6 +8,7 @@
 #include "rtl837x_regs.h"
 #include "rtl837x_common.h"
 #include "rtl837x_flash.h"
+#include "rtl837x_pins.h"
 #include "rtl837x_phy.h"
 #include "rtl837x_port.h"
 #include "rtl837x_stp.h"
@@ -18,12 +19,14 @@
 #include "uip/uip.h"
 #include "uip/uip_arp.h"
 #include "machine.h"
+#include "phy.h"
 
 
 extern __code const struct machine machine;
 
 extern __xdata uint16_t crc_value;
 __xdata uint8_t crc_testbytes[10];
+__xdata struct machine_runtime machine_detected;
 void crc16(__xdata uint8_t *v) __naked;
 
 // Upload Firmware to 1M
@@ -105,6 +108,7 @@ __xdata uint8_t rx_headers[16]; // Packet header(s) on RX
 __xdata uint8_t uip_buf[UIP_CONF_BUFFER_SIZE+2];
 
 __xdata uint16_t rx_packet_vlan;
+__xdata uint16_t management_vlan;
 __xdata uint8_t tx_seq;
 
 __xdata uint8_t stpEnabled;
@@ -115,7 +119,6 @@ __code uint16_t bit_mask[16] = {
 };
 
 
-__xdata uint8_t was_offline;
 __xdata uint8_t linkbits_last[4];
 __xdata uint8_t linkbits_last_p89;
 __xdata uint8_t sfp_pins_last;
@@ -683,7 +686,7 @@ void sds_config_mac(uint8_t sds, uint8_t mode)
 	case 2:
 		sfr_mask_data(1, 0xfc, 0x02 << 2);
 	}
-	if (machine.isRTL8373) // Set 3rd SERDES Mode to 0x2 for RTL8224
+	if (machine_detected.isRTL8373) // Set 3rd SERDES Mode to 0x2 for RTL8224
 		sfr_mask_data(1, 0xfc, 0x02 << 2);
 	else
 		sfr_data[2] &= 0x03;
@@ -805,7 +808,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, i2c_bus_from_scl_pin(machine.sfp_port[slot].i2c.scl) << 5 | i2c_bus_from_sda_pin(machine.sfp_port[slot].i2c.sda) << 2);
 	reg_write_m(RTL837X_REG_I2C_CTRL);
 
 	REG_WRITE(RTL837X_REG_I2C_IN, 0, 0, 0, reg);
@@ -921,12 +924,14 @@ void handle_rx(void)
 			    tcpip_output();
 			}
 		} else if (uip_buf[ETHERTYPE_OFFSET] == 0x08 && uip_buf[ETHERTYPE_OFFSET + 1] == 0x00) { // TCP?
-			uip_arp_ipin();	// Learn MAC addresses in TCP packets
-			uip_input();
-			if (uip_len) {
-				// Add ethernet frame
-				uip_arp_out();
-				tcpip_output();
+			if (!management_vlan || management_vlan == rx_packet_vlan) {
+				uip_arp_ipin();	// Learn MAC addresses in TCP packets
+				uip_input();
+				if (uip_len) {
+					// Add ethernet frame
+					uip_arp_out();
+					tcpip_output();
+				}
 			}
 		} else {
 #ifdef RXTXDBG
@@ -1005,6 +1010,15 @@ bool gpio_pin_test(uint8_t pin)
 	return sfr_data[3-((pin >> 3) & 3)] & (1 << (pin & 7));
 }
 
+/* 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);
+		gpio_output_setup(machine.sfp_port[sfp].pin_tx_disable, 0);
+	}
+}
 
 void handle_sfp(void)
 {
@@ -1098,7 +1112,7 @@ void idle(void)
 		print_byte(linkbits_last[2]); print_byte(linkbits_last[3]);
 		print_string(">\n");
 		linkbits_last_p89 = linkbits_p89;
-		if (!machine.isRTL8373 && machine.n_sfp != 2) {
+		if (!machine_detected.isRTL8373 && machine.n_sfp != 2) {
 			uint8_t p5 = sfr_data[2] >> 4;
 			uint8_t p5_last = linkbits_last[2] >> 4;
 			cpy_4(linkbits_last, sfr_data);
@@ -1379,7 +1393,7 @@ void sds_init(void)
 	p001e.000d:0010 R02f8-00000010 R02f4-0000001a P000001.1e00000d:b7fe
 	p001e.000d:0010 p001e.000d:0010	R02f8-00000010 R02f4-00000010 P000001.1e00000d:b7fe
 */
-	phy_read(0, 0x1e, 0xd);
+	phy_read(0, PHY_MMD30, 0xd);
 	uint16_t pval = SFR_DATA_U16;
 
 	// PHY Initialization:
@@ -1391,9 +1405,9 @@ void sds_init(void)
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
 	delay(10);
 
-	phy_write_mask(0x1, 0x1e, 0xd, pval);
+	phy_write_mask(0x1, PHY_MMD30, 0xd, pval);
 
-	phy_read(0, 0x1e, 0xd);
+	phy_read(0, PHY_MMD30, 0xd);
 	pval = SFR_DATA_U16;
 
 	REG_WRITE(0x2f8, 0, 0, pval >> 8, pval);
@@ -1401,7 +1415,40 @@ void sds_init(void)
 	pval &= 0xfff0;
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
 
-	phy_write_mask(0x1, 0x1e, 0xd, pval);
+	phy_write_mask(0x1, PHY_MMD30, 0xd, pval);
+
+	if (machine_detected.isN) {
+		uint16_t pval;
+
+		print_string("  N-settings");
+		// Serdes 0 RX PN swap for 64B/66B
+		sds_read(1, 6, 2);
+		pval = SFR_DATA_U16;
+		sds_write_v(1, 6, 2, pval | 0x2000);
+
+		// Serdes 1 RX PN swap for 8B/10B
+		sds_read(1, 0, 0);
+		pval = SFR_DATA_U16;
+		sds_write_v(1, 0, 0, pval | 0x200);
+
+		// Serdes 0 RX PN swap for 64B/66B
+		sds_read(0, 6, 2);
+		pval = SFR_DATA_U16;
+		sds_write_v(0, 6, 2, pval | 0x2000);
+
+		if (machine_detected.isRTL8373) {
+			// RTL8224: Serdes 0 RX PN swap for 64B/66B
+			// We assume that RTL8373N always paired with RTL8224N.
+			// This sds register value is 0x0000 at reset.
+			// So only write to it.
+			RTL8224_SDS_WRITE(0, 6, 2, 0x2000);
+		} else {
+			// Serdes 0 RX PN swap for 8B/10B
+			sds_read(0, 0, 0);
+			pval = SFR_DATA_U16;
+			sds_write_v(0, 0, 0, pval | 0x200);
+		}
+	}
 }
 
 
@@ -1501,7 +1548,6 @@ void led_config(void)
 	reg_write_m(RTL837X_REG_LED3_0_SET1);
 }
 
-
 void rtl8373_revision(void)
 {
 	reg_read_m(RTL837X_REG_CHIP_INFO);
@@ -1548,14 +1594,23 @@ void rtl8373_init(void)
 	pval = SFR_DATA_U16;
 
 	// r0a90:000000f3 R0a90-000000fc
-	reg_read_m(0xa90);
+	reg_read_m(RTL837X_CFG_PHY_MDI_REVERSE);
 	sfr_mask_data(0, 0x0f,0x0c);
-	reg_write_m(0xa90);
+	reg_write_m(RTL837X_CFG_PHY_MDI_REVERSE);
+
+	if (machine_detected.isN) {
+		print_string("  TX_POLARITY_SWAP\n");
+		// FOR N-Version: #TX_POLARITY_SWAP
+		reg_read_m(RTL837X_CFG_PHY_TX_POLARITY_SWAP);
+			sfr_data[2] = 0x59;
+			sfr_data[3] = 0x6a;
+		reg_write_m(RTL837X_CFG_PHY_TX_POLARITY_SWAP);
+	}
 
 	rtl8224_phy_enable();
 
 	// Disable PHYs for configuration
-	phy_write_mask(0xff,0x1f,0xa610,0x2858);
+	phy_write_mask(0xff,PHY_MMD31,0xa610,0x2858);
 
 	// Set bits 0x13 and 0x14 of 0x5fd4
 	// r5fd4:0002914a R5fd4-001a914a
@@ -1585,7 +1640,7 @@ void rtl8373_init(void)
 	// TODO: patch the PHYs
 
 	// Re-enable PHY after configuration
-	phy_write_mask(0xff,0x1f,0xa610,0x2058);
+	phy_write_mask(0xff,PHY_MMD31,0xa610,0x2058);
 
 	// Enables MAC access
 	// Set bits 0xc-0x14 of 0x632c to 0x1f8, see rtl8372_init
@@ -1616,12 +1671,12 @@ void rtl8372_init(void)
 	reg_write_m(RTL837X_REG_SDS_MODES);
 
 	// r0a90:000000f3 R0a90-000000fc
-	reg_read_m(0xa90);
-	sfr_mask_data(0, 0x0f,0x0c);
-	reg_write_m(0xa90);
+	reg_read_m(RTL837X_CFG_PHY_MDI_REVERSE);
+	sfr_mask_data(0, 0x0f, 0x0c);
+	reg_write_m(RTL837X_CFG_PHY_MDI_REVERSE);
 
 	// Disable PHYs for configuration
-	phy_write_mask(0xf0,0x1f,0xa610,0x2858);
+	phy_write_mask(0xf0,PHY_MMD31,0xa610,0x2858);
 
 	// Set bits 0x13 and 0x14 of 0x5fd4
 	// r5fd4:0002914a R5fd4-001a914a
@@ -1648,7 +1703,7 @@ void rtl8372_init(void)
 	// TODO: patch the PHYs
 
 	// Re-enable PHY after configuration
-	phy_write_mask(0xf0,0x1f,0xa610,0x2058);
+	phy_write_mask(0xf0,PHY_MMD31,0xa610,0x2058);
 
 	// Enables MAC access
 	// Set bits 0xc-0x14 of 0x632c to 0x1f8, see rtl8372_init
@@ -1678,7 +1733,7 @@ void init_smi(void)
 	REG_SET(RTL837X_REG_SMI_MAC_TYPE, machine.n_sfp == 2 ? 0x00005515 : 0x00005555);
 
 	// Configure polling of all PHYs by the MAC to detect link-state changes
-	if (machine.isRTL8373) {
+	if (machine_detected.isRTL8373) {
 		REG_SET(RTL837X_REG_SMI_PORT_POLLING, 0xff);
 	} else {
 		REG_SET(RTL837X_REG_SMI_PORT_POLLING, machine.n_sfp == 2 ? 0xf0 : 0x1f8);
@@ -1689,7 +1744,7 @@ void init_smi(void)
 	reg_write_m(RTL837X_REG_SMI_CTRL);
 	delay(50);
 
-	if (!machine.isRTL8373) {
+	if (!machine_detected.isRTL8373) {
 		// Change I2C addresses for SMI of the non-existent PHYs
 		// r6450:000020e6 R6450-000000e6
 		reg_read_m(RTL837X_REG_SMI_PORT6_9_ADDR);
@@ -1765,11 +1820,61 @@ 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 = i2c_bus_from_scl_pin(machine.sfp_port[sfp].i2c.scl);
+		const uint8_t sda_bus = i2c_bus_from_sda_pin(machine.sfp_port[sfp].i2c.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');
+		switch (scl_bus) {
+			case 3:
+				// Bit 5-6 0b10 -> SCL (implies enabled SDA on bus 3)
+				sfr_mask_data(0, 0x60, 0x40);
+				break;
+			case 2: 
+				// Bit 15-16 0b01 -> SCL
+				sfr_mask_data(1, 0x80, 0x80);
+				sfr_mask_data(2, 0x01, 0x00);
+				break;
+			case 1:
+				// Bit 11-12 0b01 -> SCL
+				sfr_mask_data(1, 0x18, 0x08);
+				break;
+			case 0:
+				// Bit 7-8 0b01 -> SCL
+				sfr_mask_data(0, 0x80, 0x80);
+				sfr_mask_data(1, 0x01, 0x00);
+				break;
+			default:
+				print_string("Invalid SCL bus number: "); print_byte(scl_bus); write_char('\n');
+		}
+
+		switch (sda_bus) {
+			case 4:
+				// Bit 29 0b0 -> SDA
+				sfr_mask_data(3, 0x20, 0x00);
+				break;
+			case 3:
+				// Bit 5-6 0b10 -> SDA (implies enabled SCL on bus 3)
+				sfr_mask_data(0, 0x60, 0x40);
+				break;
+			case 2:
+				// Bit 17-18 0b01 -> SDA
+				sfr_mask_data(2, 0x06, 0x02);
+				break;
+			case 1:
+				// Bit 13-14 0b01 -> SDA
+				sfr_mask_data(1, 0x60, 0x20);
+				break;
+			case 0:
+				// Bit 9-10 0b01 -> SDA
+				sfr_mask_data(1, 0x06, 0x02);
+				break;
+			default:
+				print_string("Invalid SDA bus number: "); print_byte(sda_bus); write_char('\n');
+		}
+	}
+	reg_write_m(RTL837X_PIN_MUX_1);	
 }
 
 
@@ -1810,17 +1915,27 @@ void bootloader(void)
 	// We have not detected any link
 	linkbits_last[0] = linkbits_last[1] = linkbits_last[2] = linkbits_last[3] = linkbits_last_p89 = 0;
 
-	print_string("Detecting CPU: ");
-	reg_read_m(0x4);
-	if (sfr_data[1] == 0x73) { // Register was 0x83730000
-		print_string("RTL8373\n");
-		if (!machine.isRTL8373)
-			print_string("INCORRECT MACHINE!");
-		rtl8224_enable();  // Power on the RTL8224
+	machine_detected.isRTL8373 = 0;
+	machine_detected.isN = 0;
+	print_string("Detecting CPU: RTL837");
+	reg_read_m(RTL837X_REG_CHIP_ID);
+	if (sfr_data[1] == 0x73) { // Register was 0x8373xx00
+		machine_detected.isRTL8373 = 1;
+		write_char('3');
 	} else {
-		print_string("RTL8372\n");
-		if (machine.isRTL8373)
-			print_string("INCORRECT MACHINE!");
+		write_char('2');
+	}
+	// Detect non-N/N chip, 0xxxxx70xx
+	if (sfr_data[2] == 0x70) {
+		machine_detected.isN = 1;
+		write_char('N');
+	}
+	write_char('\n');
+	if (machine.isRTL8373 != machine_detected.isRTL8373) {
+		print_string("INCORRECT MACHINE!");
+	}
+	if (machine_detected.isRTL8373) {
+		rtl8224_enable();  // Power on the RTL8224
 	}
 
 	// Print SW version
@@ -1840,7 +1955,7 @@ void bootloader(void)
 	REG_SET(RTL837X_PIN_MUX_2, 0x0); // Disable pins for ACL
 	init_smi();
 	rtl8373_revision();
-	if (machine.isRTL8373)
+	if (machine_detected.isRTL8373)
 		rtl8373_init();
 	else
 		rtl8372_init();
@@ -1950,7 +2065,7 @@ void bootloader(void)
 	uip_arp_init();
 	httpd_init();
 
-	was_offline = 1;
+	management_vlan = 0; // Disabled
 
 	setup_i2c();
 
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);
   }