From c5995a418423cc151eb418f492709ef62fa3c249 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Ren=C3=A9=20van=20Dorst?= <opensource@vdorst.com>
Date: Sun, 31 Aug 2025 22:59:09 +0200
Subject: [PATCH] Add: phy_modify() to modify phy_registers.

Makes it easier to modify set/clear bit in a phy register.
Simulair like Linux PHY-core: https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git/tree/drivers/net/phy/phy-core.c#n551
---
 rtl837x_common.h |  1 +
 rtlplayground.c  | 35 +++++++++++++++++++++++++++++++++++
 2 files changed, 36 insertions(+)

diff --git a/rtl837x_common.h b/rtl837x_common.h
index a84ed6a..46f480d 100644
--- a/rtl837x_common.h
+++ b/rtl837x_common.h
@@ -63,6 +63,7 @@ void print_phy_data(void);
 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 phy_modify(uint8_t phy_id, uint8_t dev_id, uint16_t reg, uint16_t mask, uint16_t set);
 void reg_read(uint16_t reg_addr);
 void reg_read_m(uint16_t reg_addr);
 void reg_write(uint16_t reg_addr);
diff --git a/rtlplayground.c b/rtlplayground.c
index 1ec02d8..c140362 100644
--- a/rtlplayground.c
+++ b/rtlplayground.c
@@ -1133,6 +1133,41 @@ void phy_read(uint8_t phy_id, uint8_t dev_id, uint16_t reg)
 #endif
 }
 
+/*
+ * Modify a register reg of phy phy_id, in page page
+ * Set: bit mask of bits to set.
+ * Mask: bit mask of bits to clear.
+
+ * Note: We assume that the registers `SFR_SMI_REG_U16`, `SFR_SMI_PHY` and `SFR_SMI_DEV` 
+ * keep there value, and dont have to be rewritten everytime.
+ */
+void phy_modify(uint8_t phy_id, uint8_t dev_id, uint16_t reg, uint16_t mask, uint16_t set)
+{
+	uint8_t smi_phy = dev_id << 3 | 2;
+
+	// Read the data
+	SFR_SMI_REG_U16 = reg;		// c2, c2
+	SFR_SMI_PHY = phy_id;		// a5
+	SFR_SMI_DEV = smi_phy;		// c4
+	SFR_EXEC_GO = SFR_EXEC_READ_SMI;
+	do {
+	} while (SFR_EXEC_STATUS != 0);
+
+	// Modify the reed data.
+	// TODO: Check if we directly can modify SFR register directly.
+	uint16_t data = SFR_DATA_U16 & ~(mask);
+	data |= ~(set);
+
+	uint16_t phy_mask = bit_mask[phy_id];
+
+	// Write it back
+	SFR_SMI_REG_U16 = data;
+	SFR_SMI_PHYMASK = phy_mask;		// SFR_C5
+	SFR_SMI_DEV = smi_phy | (phy_mask >> 8);
+	SFR_EXEC_GO = SFR_EXEC_WRITE_SMI;
+	do {
+	} while (SFR_EXEC_STATUS != 0);
+}
 
 void nic_setup(void)
 {