[PATCH net-next v5 3/6] net: phy: add additional helper for PHY package

[Christian Marangi <ansuelsmth@gmail.com>]

Add additional MMD helper for modify/set/clear and poll register in
addition of read and write. These additional helper follow the same exact
implementation of the normal PHY helper.

Also add a helper to derive the offset of the passed phydev in the PHY
package from the PHY package base address.

These additional helper limit complex code in the PHY driver that requires
additional operation on the PHY package like specific PHY calibration that
needs to operate on a specific PHY address of the package.

Signed-off-by: Christian Marangi <ansuelsmth@gmail.com>
---
 drivers/net/phy/phy_package.c | 57 +++++++++++++++++++++++++++
 drivers/net/phy/phylib.h      | 73 +++++++++++++++++++++++++++++++++++
 2 files changed, 130 insertions(+)

diff --git a/drivers/net/phy/phy_package.c b/drivers/net/phy/phy_package.c
index 735806c5bea8..0a649e2f6f74 100644
--- a/drivers/net/phy/phy_package.c
+++ b/drivers/net/phy/phy_package.c
@@ -75,6 +75,19 @@ void phy_package_unlock(struct phy_device *phydev)
 }
 EXPORT_SYMBOL_GPL(phy_package_unlock);
 
+/**
+ * phy_package_unlock - get the PHY offset relative to PHY package base addr
+ * @phydev: PHY device that has joined the package
+ */
+u8 phy_package_offset(struct phy_device *phydev)
+{
+	struct phy_package_shared *shared = phydev->shared;
+	u8 base_addr = shared->base_addr;
+
+	return phydev->mdio.addr - base_addr;
+}
+EXPORT_SYMBOL_GPL(phy_package_offset);
+
 static int phy_package_address(struct phy_device *phydev,
 			       unsigned int addr_offset)
 {
@@ -179,6 +192,50 @@ int __phy_package_write_mmd(struct phy_device *phydev,
 }
 EXPORT_SYMBOL(__phy_package_write_mmd);
 
+/**
+ * __phy_package_modify_mmd - modify MMD reg relative to PHY package base addr
+ * @phydev: The phy_device struct
+ * @addr_offset: The offset to be added to PHY package base_addr
+ * @devad: The MMD to write to
+ * @regnum: The register on the MMD to write
+ * @mask: bit mask of bits to clear
+ * @set: new value of bits set in mask to write to @regnum
+ *
+ * Convenience helper for modifying a register of an MMD on a given PHY
+ * using the PHY package base address. The base address is added to
+ * the addr_offset value.
+ *
+ * Same calling rules as for __phy_modify_mmd();
+ *
+ * NOTE: It's assumed that the entire PHY package is either C22 or C45.
+ */
+int __phy_package_modify_mmd(struct phy_device *phydev,
+			     unsigned int addr_offset, int devad,
+			     u32 regnum, u16 mask, u16 set)
+{
+	int addr = phy_package_address(phydev, addr_offset);
+	int new, ret;
+
+	if (addr < 0)
+		return addr;
+
+	if (regnum > (u16)~0 || devad > 32)
+		return -EINVAL;
+
+	ret = __phy_package_read_mmd(phydev, addr_offset, devad, regnum);
+	if (ret < 0)
+		return ret;
+
+	new = (ret & ~mask) | set;
+	if (new == ret)
+		return 0;
+
+	ret = __phy_package_write_mmd(phydev, addr_offset, devad, regnum, new);
+
+	return ret < 0 ? ret : 0;
+}
+EXPORT_SYMBOL(__phy_package_modify_mmd);
+
 static bool __phy_package_set_once(struct phy_device *phydev, unsigned int b)
 {
 	struct phy_package_shared *shared = phydev->shared;
diff --git a/drivers/net/phy/phylib.h b/drivers/net/phy/phylib.h
index c6e26ac6b28f..6258b1f84bff 100644
--- a/drivers/net/phy/phylib.h
+++ b/drivers/net/phy/phylib.h
@@ -12,8 +12,10 @@ struct mii_bus;
 
 struct device_node *phy_package_get_node(struct phy_device *phydev);
 void *phy_package_get_priv(struct phy_device *phydev);
+
 void phy_package_lock(struct phy_device *phydev);
 void phy_package_unlock(struct phy_device *phydev);
+u8 phy_package_offset(struct phy_device *phydev);
 int __phy_package_read(struct phy_device *phydev, unsigned int addr_offset,
 		       u32 regnum);
 int __phy_package_write(struct phy_device *phydev, unsigned int addr_offset,
@@ -24,6 +26,77 @@ int __phy_package_read_mmd(struct phy_device *phydev,
 int __phy_package_write_mmd(struct phy_device *phydev,
 			    unsigned int addr_offset, int devad,
 			    u32 regnum, u16 val);
+int __phy_package_modify_mmd(struct phy_device *phydev,
+			     unsigned int addr_offset, int devad,
+			     u32 regnum, u16 mask, u16 set);
+
+/**
+ * __phy_package_set_bits_mmd - Convenience function for setting bits in a register
+ * on MMD relative to PHY package base addr
+ * @phydev: the phy_device struct
+ * @addr_offset: the offset to be added to PHY package base_addr
+ * @devad: the MMD containing register to modify
+ * @regnum: register number to modify
+ * @val: bits to set
+ */
+static inline int __phy_package_set_bits_mmd(struct phy_device *phydev,
+					     unsigned int addr_offset, int devad,
+					     u32 regnum, u16 val)
+{
+	return __phy_package_modify_mmd(phydev, addr_offset, devad, regnum, 0, val);
+}
+
+/**
+ * __phy_package_clear_bits_mmd - Convenience function for clearing bits in a register
+ * on MMD relative to PHY package base addr
+ * @phydev: the phy_device struct
+ * @addr_offset: the offset to be added to PHY package base_addr
+ * @devad: the MMD containing register to modify
+ * @regnum: register number to modify
+ * @val: bits to clear
+ */
+static inline int __phy_package_clear_bits_mmd(struct phy_device *phydev,
+					       unsigned int addr_offset, int devad,
+					       u32 regnum, u16 val)
+{
+	return __phy_package_modify_mmd(phydev, addr_offset, devad, regnum, val, 0);
+}
+
+/**
+ * phy_read_mmd_poll_timeout - Periodically poll a PHY register relative to PHY package
+ *                             until a condition is met or a timeout occurs
+ *
+ * @phydev: The phy_device struct
+ * @addr_offset: The offset to be added to PHY package base_addr
+ * @devaddr: The MMD to read from
+ * @regnum: The register on the MMD to read
+ * @val: Variable to read the register into
+ * @cond: Break condition (usually involving @val)
+ * @sleep_us: Maximum time to sleep between reads in us (0 tight-loops). Please
+ *            read usleep_range() function description for details and
+ *            limitations.
+ * @timeout_us: Timeout in us, 0 means never timeout
+ * @sleep_before_read: if it is true, sleep @sleep_us before read.
+ *
+ * Returns: 0 on success and -ETIMEDOUT upon a timeout. In either
+ * case, the last read value at @args is stored in @val. Must not
+ * be called from atomic context if sleep_us or timeout_us are used.
+ */
+#define __phy_package_read_mmd_poll_timeout(phydev, addr_offset, devaddr, regnum, val, \
+					    cond, sleep_us, timeout_us, sleep_before_read) \
+({ \
+	int __ret, __val; \
+	__ret = read_poll_timeout(__val = __phy_package_read_mmd, val, \
+				  __val < 0 || (cond), \
+				  sleep_us, timeout_us, sleep_before_read, \
+				  phydev, addr_offset, devaddr, regnum); \
+	if (__val < 0) \
+		__ret = __val; \
+	if (__ret) \
+		phydev_err(phydev, "%s failed: %d\n", __func__, __ret); \
+	__ret; \
+})
+
 bool phy_package_init_once(struct phy_device *phydev);
 bool phy_package_probe_once(struct phy_device *phydev);
 int phy_package_join(struct phy_device *phydev, int base_addr, size_t priv_size);
-- 
2.55.0