diff --git a/rtl837x_common.h b/rtl837x_common.h
index a568276..a84ed6a 100644
--- a/rtl837x_common.h
+++ b/rtl837x_common.h
@@ -60,7 +60,8 @@ void print_short(uint16_t a);
 void print_byte(uint8_t a);
 void print_sfr_data(void);
 void print_phy_data(void);
-void phy_write(uint16_t phy_mask, uint8_t dev_id, uint16_t reg, uint16_t v);
+void phy_write_mask(uint16_t phy_mask, uint8_t dev_id, uint16_t reg, uint16_t v);
+void phy_write(uint8_t phy_id, uint8_t dev_id, uint16_t reg, uint16_t v);
 void phy_read(uint8_t phy_id, uint8_t dev_id, uint16_t reg);
 void reg_read(uint16_t reg_addr);
 void reg_read_m(uint16_t reg_addr);
diff --git a/rtl837x_phy.c b/rtl837x_phy.c
index 7deb67c..65b9d54 100644
--- a/rtl837x_phy.c
+++ b/rtl837x_phy.c
@@ -80,7 +80,7 @@ void rtl8224_phy_enable(void) __banked
 	pval |= 0x0c;
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
 
-	phy_write(0x1, 0x1e, 0xa90, pval);
+	phy_write_mask(0x1, 0x1e, 0xa90, pval);
 
 	phy_read(0, 0x1e, 0xa90);
 	pval = SFR_DATA_8;
@@ -105,7 +105,7 @@ void phy_config(uint8_t phy) __banked
 	pval = SFR_DATA_8;
 	pval <<= 8;
 	pval |= SFR_DATA_0 & 0xfe;
-	phy_write(bit_mask[phy], 0x1e, 0x75f3, pval);
+	phy_write(phy, 0x1e, 0x75f3, pval);
 	delay(20);
 
 //	p081e.697a:ffff P000100.1e00697a:ffc1 / p031e.697a:0003 P000008.1e00697a:0001
@@ -114,7 +114,7 @@ void phy_config(uint8_t phy) __banked
 	pval = SFR_DATA_8;
 	pval <<= 8;
 	pval |= SFR_DATA_0 & 0xc0 | 0x01;
-	phy_write(bit_mask[phy], 0x1e, 0x697a, pval);
+	phy_write(phy, 0x1e, 0x697a, pval);
 	delay(20);
 
 //	p031f.a432:0811 P000008.1f00a432:0831
@@ -123,7 +123,7 @@ void phy_config(uint8_t phy) __banked
 	pval = SFR_DATA_8;
 	pval <<= 8;
 	pval |= SFR_DATA_0 | 0x20;
-	phy_write(bit_mask[phy], 0x1f, 0xa432, pval);
+	phy_write(phy, 0x1f, 0xa432, pval);
 
 //	p0307.003e:0000 P000008.0700003e:0001
 	// EEE avertisment 2 register MMMD 7.0x003e, set bit 0: 2.5G has EEE capability
@@ -131,7 +131,7 @@ void phy_config(uint8_t phy) __banked
 	pval = SFR_DATA_8;
 	pval <<= 8;
 	pval |= SFR_DATA_0 | 0x1;
-	phy_write(bit_mask[phy], 0x7, 0x3e, pval);
+	phy_write(phy, 0x7, 0x3e, pval);
 	delay(20);
 
 //	p031f.a442:043c P000008.1f00a442:0430
@@ -140,11 +140,11 @@ void phy_config(uint8_t phy) __banked
 	pval = SFR_DATA_8;
 	pval <<= 8;
 	pval |= SFR_DATA_0 & 0xf3;
-	phy_write(bit_mask[phy], 0x1f, 0xa442, pval);
+	phy_write(phy, 0x1f, 0xa442, pval);
 	delay(20);
 
 	// P000100.1e0075b5:e084
-	phy_write(bit_mask[phy], 0x1e, 0x75b5, 0xe084);
+	phy_write(phy, 0x1e, 0x75b5, 0xe084);
 	delay(20);
 
 //	p031e.75b2:0000 P000008.1e0075b2:0060
@@ -153,7 +153,7 @@ void phy_config(uint8_t phy) __banked
 	pval = SFR_DATA_8;
 	pval <<= 8;
 	pval |= SFR_DATA_0 | 0x60;
-	phy_write(bit_mask[phy], 0x1e, 0x75b2, pval);
+	phy_write(phy, 0x1e, 0x75b2, pval);
 	delay(20);
 
 //	p081f.d040:ffff P000100.1f00d040:feff
@@ -162,7 +162,7 @@ void phy_config(uint8_t phy) __banked
 	pval = (SFR_DATA_8 & 0xfc) | 0x02;
 	pval <<= 8;
 	pval |= SFR_DATA_0;
-	phy_write(bit_mask[phy], 0x1e, 0xd040, pval);
+	phy_write(phy, 0x1e, 0xd040, pval);
 	delay(20);
 
 //	p081f.a400:ffff P000100.1f00a400:ffff, then: p081f.a400:ffff P000100.1f00a400:bfff
@@ -174,14 +174,14 @@ void phy_config(uint8_t phy) __banked
 	pval = SFR_DATA_8 | 0x40;
 	pval <<= 8;
 	pval |= SFR_DATA_0;
-	phy_write(bit_mask[phy], 0x1f, 0xa400, pval);
+	phy_write(phy, 0x1f, 0xa400, pval);
 	delay(20);
 
 	phy_read(phy, 0x1f, 0xa400);
 	pval = SFR_DATA_8 & 0xbf;
 	pval <<= 8;
 	pval |= SFR_DATA_0;
-	phy_write(bit_mask[phy], 0x1f, 0xa400, pval);
+	phy_write(phy, 0x1f, 0xa400, pval);
 	delay(20);
 
 	print_string("\r\n  phy config done\r\n");
@@ -203,13 +203,13 @@ void phy_config_8224(void) __banked
 	pval &= 0x0fe0;
 	pval |= 0x000d;
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
-	phy_write(0x01, 0x1e, 0x7b20, pval);
+	phy_write_mask(0x01, 0x1e, 0x7b20, pval);
 
 	uint8_t i = 0;
 	while (rtl8224_ca[i]) {
-		phy_write(0x1, 0x1e, 0x400, rtl8224_ca[i]);
+		phy_write_mask(0x1, 0x1e, 0x400, rtl8224_ca[i]);
 		i++;
-		phy_write(0x1, 0x1e, 0x3f8, rtl8224_ca[i]);
+		phy_write_mask(0x1, 0x1e, 0x3f8, rtl8224_ca[i]);
 		i++;
 		do {
 			phy_read(0, 0x1e, 0x3f8);
@@ -230,39 +230,39 @@ void phy_set_mode(uint8_t port, uint8_t speed, uint8_t flow_control, uint8_t dup
 	phy_read(port, 0x1f, 0xa610);
 	v = (((uint16_t)SFR_DATA_8) << 8) | SFR_DATA_0;
 	if (speed == PHY_OFF) {
-		phy_write(bit_mask[port], 0x1f, 0xa610, v | 0x0800);
+		phy_write(port, 0x1f, 0xa610, v | 0x0800);
 		return;
 	}
 	// Port is on, make sure of it:
 	if (v & 0x0800)
-		phy_write(bit_mask[port], 0x1f, 0xa610, v & 0xf7ff);
+		phy_write(port, 0x1f, 0xa610, v & 0xf7ff);
 
 	if (speed == PHY_SPEED_AUTO) {
 			// AN Advertisement Register (MMD 7.0x0010)
-			phy_write(bit_mask[port], 0x07, 0x10, 0x1001);	// bits 0-4: 0x1 (802.3 supported), Extended Next Page format used
+			phy_write(port, 0x07, 0x10, 0x1001);	// bits 0-4: 0x1 (802.3 supported), Extended Next Page format used
 			// Multi-GBASE-TBASE-T AN Control 1 Register (MMD 7.0x0020)
-			phy_write(bit_mask[port], 0x07, 0x20, 0x6081);	// bit 14: SLAVE, bit 13: Multi-Port device, bit 8: 2.5GBit available, 1: LD Loop timin enableed
-			phy_write(bit_mask[port], 0x07, 0x00, 0x3200);	// Restart AN
+			phy_write(port, 0x07, 0x20, 0x6081);	// bit 14: SLAVE, bit 13: Multi-Port device, bit 8: 2.5GBit available, 1: LD Loop timin enableed
+			phy_write(port, 0x07, 0x00, 0x3200);	// Restart AN
 	} else {
 		// AN Control Register (MMD 7.0x0000)
-		phy_write(bit_mask[port], 0x07, 0x00, 0x2000);	// Clear bit 12: No Autoneg, Set Extended Pages (bit 13)
+		phy_write(port, 0x07, 0x00, 0x2000);	// Clear bit 12: No Autoneg, Set Extended Pages (bit 13)
 		// AN Advertisement Register (MMD 7.0x0010)
-		phy_write(bit_mask[port], 0x07, 0x10, 0x1001);	// bits 0-4: 0x1 (802.3 supported), Extended Next Page format used
+		phy_write(port, 0x07, 0x10, 0x1001);	// bits 0-4: 0x1 (802.3 supported), Extended Next Page format used
 		if (speed == PHY_SPEED_1G) {
 			// Multi-GBASE-TBASE-T AN Control 1 Register (MMD 7.0x0020)
-			phy_write(bit_mask[port], 0x07, 0x20, 0x6001);	// bit 14: SLAVE, bit 13: Multi-Port device, 1: LD Loop timin enableed
+			phy_write(port, 0x07, 0x20, 0x6001);	// bit 14: SLAVE, bit 13: Multi-Port device, 1: LD Loop timin enableed
 			// GBCR (1000Base-T Control Register, MMD 31.0xA412)
 			phy_read(port, 0x1f, 0xa412);
 			v = (((uint16_t)SFR_DATA_8) << 8) | SFR_DATA_0;
-			phy_write(bit_mask[port], 0x1f, 0xa412, v | 0x0200);
+			phy_write(port, 0x1f, 0xa412, v | 0x0200);
 		} else if (speed == PHY_SPEED_2G5) {
 			// Multi-GBASE-TBASE-T AN Control 1 Register (MMD 7.0x0020)
-			phy_write(bit_mask[port], 0x07, 0x20, 0x6081);	// bit 14: SLAVE, bit 13: Multi-Port device, bit 8: 2.5GBit available, 1: LD Loop timin enableed
+			phy_write(port, 0x07, 0x20, 0x6081);	// bit 14: SLAVE, bit 13: Multi-Port device, bit 8: 2.5GBit available, 1: LD Loop timin enableed
 			// GBCR (1000Base-T Control Register, MMD 31.0xA412)
 			phy_read(port, 0x1f, 0xa412);
 			v = (((uint16_t)SFR_DATA_8) << 8) | SFR_DATA_0;
-			phy_write(bit_mask[port], 0x1f, 0xa412, v & 0xfdff);
+			phy_write(port, 0x1f, 0xa412, v & 0xfdff);
 		}
-		phy_write(bit_mask[port], 0x07, 0x00, 0x3200);	// Enable AN
+		phy_write(port, 0x07, 0x00, 0x3200);	// Enable AN
 	}
 }
diff --git a/rtlplayground.c b/rtlplayground.c
index 57dc0f9..5075095 100644
--- a/rtlplayground.c
+++ b/rtlplayground.c
@@ -1079,10 +1079,10 @@ void setup_clock(void)
 
 
 /*
- * Write a register reg of phy phy_id, in page page
+ * Write a register reg of multipule phys, using a mask to select them, in page page
  * Data to be written is in v
  */
-void phy_write(uint16_t phy_mask, uint8_t dev_id, uint16_t reg, uint16_t v)
+void phy_write_mask(uint16_t phy_mask, uint8_t dev_id, uint16_t reg, uint16_t v)
 {
 #ifdef REGDBG
 	print_string("P"); print_byte(phy_mask>>8); print_byte(phy_mask); print_byte(dev_id); write_char('.'); print_byte(reg>>8); print_byte(reg); write_char(':');
@@ -1099,6 +1099,30 @@ void phy_write(uint16_t phy_mask, uint8_t dev_id, uint16_t reg, uint16_t v)
 	} while (SFR_EXEC_STATUS != 0);
 }
 
+/*
+ * Write a register reg of phy, using a mask to select them, in page page
+ * Data to be written is in v
+ */
+void phy_write(uint8_t phy_id, uint8_t dev_id, uint16_t reg, uint16_t v)
+{
+	uint16_t phy_mask =  bit_mask[phy_id];
+#ifdef REGDBG
+	print_string("P"); print_byte(phy_mask>>8); print_byte(phy_mask); print_byte(dev_id); write_char('.'); print_byte(reg>>8); print_byte(reg); write_char(':');
+	print_byte(v>>8); print_byte(v); write_char(' ');
+#endif
+
+	SFR_DATA_8 = v >> 8;			// SFR_A6
+	SFR_DATA_0 = v;				// SFR_A7
+	SFR_SMI_PHYMASK = phy_mask;		// SFR_C5
+	SFR_SMI_REG_H = reg >> 8;		// SFR_C2
+	SFR_SMI_REG_L = reg;			// SFR_C3
+
+	SFR_SMI_DEV = (phy_mask >> 8) | dev_id  << 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);
+}
+
 
 /*
  * Read a phy register via MDIO clause 45
@@ -1199,7 +1223,7 @@ void sds_init(void)
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
 	delay(10);
 
-	phy_write(0x1, 0x1e, 0xd, pval);
+	phy_write_mask(0x1, 0x1e, 0xd, pval);
 
 	phy_read(0, 0x1e, 0xd);
 	pval = SFR_DATA_8;
@@ -1211,7 +1235,7 @@ void sds_init(void)
 	pval &= 0xfff0;
 	REG_WRITE(0x2f4, 0, 0, pval >> 8, pval);
 
-	phy_write(0x1, 0x1e, 0xd, pval);
+	phy_write_mask(0x1, 0x1e, 0xd, pval);
 }
 
 
@@ -1420,7 +1444,7 @@ void rtl8373_init(void)
 	rtl8224_phy_enable();
 
 	// Disable PHYs for configuration
-	phy_write(0xff,0x1f,0xa610,0x2858);
+	phy_write_mask(0xff,0x1f,0xa610,0x2858);
 
 	// Set bits 0x13 and 0x14 of 0x5fd4
 	// r5fd4:0002914a R5fd4-001a914a
@@ -1448,7 +1472,7 @@ void rtl8373_init(void)
 	// TODO: patch the PHYs
 
 	// Re-enable PHY after configuration
-	phy_write(0xff,0x1f,0xa610,0x2058);
+	phy_write_mask(0xff,0x1f,0xa610,0x2058);
 
 	// Enables MAC access
 	// Set bits 0xc-0x14 of 0x632c to 0x1f8, see rtl8372_init
@@ -1512,7 +1536,7 @@ void rtl8372_init(void)
 	reg_write_m(0xa90);
 
 	// Disable PHYs for configuration
-	phy_write(0xf0,0x1f,0xa610,0x2858);
+	phy_write_mask(0xf0,0x1f,0xa610,0x2858);
 
 	// Set bits 0x13 and 0x14 of 0x5fd4
 	// r5fd4:0002914a R5fd4-001a914a
@@ -1539,7 +1563,7 @@ void rtl8372_init(void)
 	// TODO: patch the PHYs
 
 	// Re-enable PHY after configuration
-	phy_write(0xf0,0x1f,0xa610,0x2058);
+	phy_write_mask(0xf0,0x1f,0xa610,0x2058);
 
 	// Enables MAC access
 	// Set bits 0xc-0x14 of 0x632c to 0x1f8, see rtl8372_init