[net-next RFC PATCH 1/6] net: phy: add support for defining multiple PHY IDs in PHY driver

[Christian Marangi <ansuelsmth@gmail.com>]

Some PHY driver might implement the same OPs for different PHY ID and
using a mask is not enough to match similar PHYs.

To reduce code duplication, add support for defining multiple PHY IDs in
PHY driver struct.

Introduce a new variable in phy_driver struct, .ids, where a table array of
mdio_device_id can be defined to reference multiple PHY IDs (with their
own masks) supporting the same group of OPs and flags.

Introduce a new variable in phy_device, .dev_id, where the matching
mdio_device_id is stored. PHYs supporting multiple PHYs for one PHY
driver struct, should use this instead of matching for phy_id.

Single PHY ID implementation is still supported and dev_id is filled
with the data from phy_driver in this case.

Signed-off-by: Christian Marangi <ansuelsmth@gmail.com>
---
 drivers/net/phy/phy_device.c | 94 ++++++++++++++++++++++++++++++------
 include/linux/phy.h          |  8 ++-
 2 files changed, 85 insertions(+), 17 deletions(-)

diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index d63dca535746..9b96357e4de8 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -522,12 +522,74 @@ static int phy_scan_fixups(struct phy_device *phydev)
 	return 0;
 }
 
+static int phy_driver_match_id(struct phy_driver *phydrv, u32 id,
+			       const struct mdio_device_id **dev_id)
+{
+	const struct mdio_device_id *ids = phydrv->ids;
+
+	/* PHY driver might provide an array of different PHY IDs and
+	 * masks. Walk them if this is the case and compare with ID.
+	 */
+	if (ids) {
+		/* From mdio_device_id struct phy_id_mask MUST
+		 * be used as sentinel.
+		 */
+		while (ids->phy_id_mask) {
+			if (phy_id_compare(id, ids->phy_id, ids->phy_id_mask)) {
+				if (dev_id)
+					*dev_id = ids;
+
+				return 1;
+			}
+
+			ids++;
+		}
+	}
+
+	if (phy_id_compare(id, phydrv->phy_id, phydrv->phy_id_mask))
+		return 1;
+
+	return 0;
+}
+
+/**
+ * phy_driver_match - match a phydriver with a given PHY istance
+ * @phydrv: PHY driver to compare with
+ * @phydev: PHY istance to use for comparison. Either PHY ID will be used or
+ *   with C45 PHY ID is extracted from Package regs.
+ * @dev_id: Pointer where to store pointer to a matchin mdio_device_id.
+ *   mdio_device_id are assumed to be statically allocated for each PHY driver,
+ *   hence the reference to this struct is returned here.
+ *
+ * Returns 1 if matching, 0 otherwise. dev_id can be passed as NULL to skip
+ * referecing a matching mdio_device_id if found.
+ */
+static int phy_driver_match(struct phy_driver *phydrv, struct phy_device *phydev,
+			    const struct mdio_device_id **dev_id)
+{
+	const int num_ids = ARRAY_SIZE(phydev->c45_ids.device_ids);
+	int i;
+
+	if (!phydev->is_c45)
+		return phy_driver_match_id(phydrv, phydev->phy_id,
+					   dev_id);
+
+	for (i = 1; i < num_ids; i++) {
+		if (phydev->c45_ids.device_ids[i] == 0xffffffff)
+			continue;
+
+		if (phy_driver_match_id(phydrv, phydev->c45_ids.device_ids[i],
+					dev_id))
+			return 1;
+	}
+
+	return 0;
+}
+
 static int phy_bus_match(struct device *dev, struct device_driver *drv)
 {
 	struct phy_device *phydev = to_phy_device(dev);
 	struct phy_driver *phydrv = to_phy_driver(drv);
-	const int num_ids = ARRAY_SIZE(phydev->c45_ids.device_ids);
-	int i;
 
 	if (!(phydrv->mdiodrv.flags & MDIO_DEVICE_IS_PHY))
 		return 0;
@@ -535,20 +597,7 @@ static int phy_bus_match(struct device *dev, struct device_driver *drv)
 	if (phydrv->match_phy_device)
 		return phydrv->match_phy_device(phydev);
 
-	if (phydev->is_c45) {
-		for (i = 1; i < num_ids; i++) {
-			if (phydev->c45_ids.device_ids[i] == 0xffffffff)
-				continue;
-
-			if (phy_id_compare(phydev->c45_ids.device_ids[i],
-					   phydrv->phy_id, phydrv->phy_id_mask))
-				return 1;
-		}
-		return 0;
-	} else {
-		return phy_id_compare(phydev->phy_id, phydrv->phy_id,
-				      phydrv->phy_id_mask);
-	}
+	return phy_driver_match(phydrv, phydev, NULL);
 }
 
 static ssize_t
@@ -3410,9 +3459,22 @@ static int phy_probe(struct device *dev)
 	struct phy_device *phydev = to_phy_device(dev);
 	struct device_driver *drv = phydev->mdio.dev.driver;
 	struct phy_driver *phydrv = to_phy_driver(drv);
+	const struct mdio_device_id *dev_id = NULL;
+	struct mdio_device_id *phy_dev_id;
 	int err = 0;
 
 	phydev->drv = phydrv;
+	phy_dev_id = (struct mdio_device_id *)&phydev->dev_id;
+	/* Fill the mdio_device_id for the PHY istance.
+	 * If PHY driver provide an array of PHYs, search the right one,
+	 * in the other case fill it with the phy_driver data.
+	 */
+	if (phy_driver_match(phydrv, phydev, &dev_id) && dev_id) {
+		memcpy(phy_dev_id, dev_id, sizeof(*dev_id));
+	} else {
+		phy_dev_id->phy_id = phydrv->phy_id;
+		phy_dev_id->phy_id_mask = phydrv->phy_id_mask;
+	}
 
 	/* Disable the interrupt if the PHY doesn't support it
 	 * but the interrupt is still a valid one
diff --git a/include/linux/phy.h b/include/linux/phy.h
index c2dda21b39e1..f0313b9e0173 100644
--- a/include/linux/phy.h
+++ b/include/linux/phy.h
@@ -547,6 +547,7 @@ struct macsec_ops;
  * @drv: Pointer to the driver for this PHY instance
  * @devlink: Create a link between phy dev and mac dev, if the external phy
  *           used by current mac interface is managed by another mac interface.
+ * @dev_id: The matched device ID for this PHY instance
  * @phy_id: UID for this device found during discovery
  * @c45_ids: 802.3-c45 Device Identifiers if is_c45.
  * @is_c45:  Set to true if this PHY uses clause 45 addressing.
@@ -645,6 +646,7 @@ struct phy_device {
 
 	struct device_link *devlink;
 
+	const struct mdio_device_id dev_id;
 	u32 phy_id;
 
 	struct phy_c45_device_ids c45_ids;
@@ -885,6 +887,8 @@ struct phy_led {
  * struct phy_driver - Driver structure for a particular PHY type
  *
  * @mdiodrv: Data common to all MDIO devices
+ * @ids: array of mdio device IDs to match this driver (terminated with
+ *   zero phy_id_mask)
  * @phy_id: The result of reading the UID registers of this PHY
  *   type, and ANDing them with the phy_id_mask.  This driver
  *   only works for PHYs with IDs which match this field
@@ -906,6 +910,7 @@ struct phy_led {
  */
 struct phy_driver {
 	struct mdio_driver_common mdiodrv;
+	const struct mdio_device_id *ids;
 	u32 phy_id;
 	char *name;
 	u32 phy_id_mask;
@@ -1206,7 +1211,8 @@ static inline bool phy_id_compare(u32 id1, u32 id2, u32 mask)
  */
 static inline bool phydev_id_compare(struct phy_device *phydev, u32 id)
 {
-	return phy_id_compare(id, phydev->phy_id, phydev->drv->phy_id_mask);
+	return phy_id_compare(id, phydev->dev_id.phy_id,
+			      phydev->dev_id.phy_id_mask);
 }
 
 /* A Structure for boards to register fixups with the PHY Lib */
-- 
2.43.0