dpaa: Support c45 phys

2 files changed, 105 insertions, 66 deletions

diff --git a/rtemsbsd/powerpc/include/linux/phy.h b/rtemsbsd/powerpc/include/linux/phy.h
index 0c1991ce..f933f5fd 100644
--- a/rtemsbsd/powerpc/include/linux/phy.h
+++ b/rtemsbsd/powerpc/include/linux/phy.h
@@ -80,9 +80,11 @@ typedef enum {
 #define	SUPPORTED_MII			(1U << 8)
 #define	SUPPORTED_Pause			(1U << 9)
 
+#define MII_ADDR_C45 (1 << 30)
+
 struct mdio_bus {
-	int (*read)(struct mdio_bus *bus, int phy, int reg);
-	int (*write)(struct mdio_bus *bus, int phy, int reg, int val);
+	int (*read)(struct mdio_bus *bus, int addr, int reg);
+	int (*write)(struct mdio_bus *bus, int addr, int reg, int val);
 	SLIST_ENTRY(mdio_bus) next;
 	int node;
 };
@@ -91,27 +93,14 @@ struct phy_device {
 	struct {
 		struct device dev;
 		int addr;
+		int is_c45;
 		struct mdio_bus *bus;
 	} mdio;
 };
 
-static inline int
-phy_read(struct phy_device *phy_dev, int reg)
-{
-	struct mdio_bus *mdio_dev;
-
-	mdio_dev = phy_dev->mdio.bus;
-	return ((*mdio_dev->read)(mdio_dev, phy_dev->mdio.addr, (int)reg));
-}
+int phy_read(struct phy_device *phy_dev, int reg);
 
-static inline int
-phy_write(struct phy_device *phy_dev, int reg, int val)
-{
-	struct mdio_bus *mdio_dev;
-
-	mdio_dev = phy_dev->mdio.bus;
-	return ((*mdio_dev->write)(mdio_dev, phy_dev->mdio.addr, reg, val));
-}
+int phy_write(struct phy_device *phy_dev, int reg, int val);
 
 #ifdef __cplusplus
 }
diff --git a/rtemsbsd/sys/powerpc/fdt_phy.c b/rtemsbsd/sys/powerpc/fdt_phy.c
index 04fad064..220c5a7d 100644
--- a/rtemsbsd/sys/powerpc/fdt_phy.c
+++ b/rtemsbsd/sys/powerpc/fdt_phy.c
@@ -55,7 +55,35 @@ static struct {
 	.instances = SLIST_HEAD_INITIALIZER(mdio.instances)
 };
 
-MTX_SYSINIT(mdio_mutex, &mdio.mutex, "FDT MDIO", MTX_DEF);
+MTX_SYSINIT(mdio_mutex, &mdio.mutex, "MDIO", MTX_DEF);
+
+int
+phy_read(struct phy_device *phy_dev, int reg)
+{
+	struct mdio_bus *mdio_dev;
+	int val;
+
+	mdio_dev = phy_dev->mdio.bus;
+	MDIO_LOCK();
+	val = (*mdio_dev->read)(mdio_dev, phy_dev->mdio.addr,
+	    reg | phy_dev->mdio.is_c45);
+	MDIO_UNLOCK();
+	return (val);
+}
+
+int
+phy_write(struct phy_device *phy_dev, int reg, int val)
+{
+	struct mdio_bus *mdio_dev;
+	int err;
+
+	mdio_dev = phy_dev->mdio.bus;
+	MDIO_LOCK();
+	err = (*mdio_dev->write)(mdio_dev, phy_dev->mdio.addr,
+	    reg | phy_dev->mdio.is_c45, val);
+	MDIO_UNLOCK();
+	return (err);
+}
 
 static uint64_t
 fdt_get_address(const void *fdt, int node)
@@ -157,78 +185,92 @@ fman_mdio_wait(volatile struct fman_mdio_regs *regs)
 }
 
 static int
-fman_mdio_read(struct mdio_bus *base, int phy, int reg)
+fman_mdio_setup(volatile struct fman_mdio_regs *regs, int addr, int reg,
+    uint32_t *mdio_ctrl_p)
 {
-	struct fman_mdio_bus *fm;
-	volatile struct fman_mdio_regs *regs;
-	int val;
+	uint32_t mdio_cfg;
+	uint32_t mdio_ctrl;
+	uint32_t reg_addr;
 	int err;
 
-	fm = (struct fman_mdio_bus *)base;
-	regs = fm->regs;
-
-	MDIO_LOCK();
-
 	err = fman_mdio_wait(regs);
-	if (err == 0) {
-		uint32_t mdio_cfg;
-		uint32_t mdio_ctrl;
+	if (err != 0) {
+		return (err);
+	}
 
-		mdio_cfg = regs->mdio_cfg;
+	mdio_cfg = regs->mdio_cfg;
+	if ((reg & MII_ADDR_C45) != 0) {
+		reg_addr = (uint32_t)(reg >> 16);
+		mdio_cfg |= MDIO_CFG_ENC45;
+	} else {
+		reg_addr = (uint32_t)reg;
 		mdio_cfg &= ~MDIO_CFG_ENC45;
-		regs->mdio_cfg = mdio_cfg;
+	}
+	regs->mdio_cfg = mdio_cfg;
 
-		mdio_ctrl = MDIO_CTRL_PHY_ADDR(phy) | MDIO_CTRL_REG_ADDR(reg);
-		regs->mdio_ctrl = mdio_ctrl;
-		mdio_ctrl |= MDIO_CTRL_READ;
-		regs->mdio_ctrl = mdio_ctrl;
+	mdio_ctrl = MDIO_CTRL_PHY_ADDR(addr) | MDIO_CTRL_REG_ADDR(reg_addr);
+	regs->mdio_ctrl = mdio_ctrl;
 
+	if ((reg & MII_ADDR_C45) != 0) {
+		regs->mdio_addr = (uint32_t)(reg & 0xffff);
 		err = fman_mdio_wait(regs);
-		if (err == 0 && (regs->mdio_cfg & MDIO_CFG_RD_ERR) == 0) {
-			val = (int)(regs->mdio_data & 0xffff);
-		} else {
-			val = 0xffff;
+		if (err != 0) {
+			return (err);
 		}
-	} else {
-		val = 0xffff;
 	}
 
-	MDIO_UNLOCK();
-
-	return (val);
+	*mdio_ctrl_p = mdio_ctrl;
+	return (0);
 }
 
 static int
-fman_mdio_write(struct mdio_bus *base, int phy, int reg, int val)
+fman_mdio_read(struct mdio_bus *base, int addr, int reg)
 {
 	struct fman_mdio_bus *fm;
 	volatile struct fman_mdio_regs *regs;
+	uint32_t mdio_ctrl;
+	int val;
 	int err;
 
 	fm = (struct fman_mdio_bus *)base;
 	regs = fm->regs;
 
-	MDIO_LOCK();
+	err = fman_mdio_setup(regs, addr, reg, &mdio_ctrl);
+	if (err != 0) {
+		return (-1);
+	}
+
+	mdio_ctrl |= MDIO_CTRL_READ;
+	regs->mdio_ctrl = mdio_ctrl;
 
 	err = fman_mdio_wait(regs);
-	if (err == 0) {
-		uint32_t mdio_cfg;
-		uint32_t mdio_ctrl;
+	if (err == 0 && (regs->mdio_cfg & MDIO_CFG_RD_ERR) == 0) {
+		val = (int)(regs->mdio_data & 0xffff);
+	} else {
+		val = -1;
+	}
 
-		mdio_cfg = regs->mdio_cfg;
-		mdio_cfg &= ~MDIO_CFG_ENC45;
-		regs->mdio_cfg = mdio_cfg;
+	return (val);
+}
 
-		mdio_ctrl = MDIO_CTRL_PHY_ADDR(phy) | MDIO_CTRL_REG_ADDR(reg);
-		regs->mdio_ctrl = mdio_ctrl;
+static int
+fman_mdio_write(struct mdio_bus *base, int addr, int reg, int val)
+{
+	struct fman_mdio_bus *fm;
+	volatile struct fman_mdio_regs *regs;
+	uint32_t mdio_ctrl;
+	int err;
 
-		regs->mdio_data = (uint32_t)(val & 0xffff);
+	fm = (struct fman_mdio_bus *)base;
+	regs = fm->regs;
 
-		fman_mdio_wait(regs);
+	err = fman_mdio_setup(regs, addr, reg, &mdio_ctrl);
+	if (err != 0) {
+		return (0);
 	}
 
-	MDIO_UNLOCK();
-
+	regs->mdio_data = (uint32_t)(val & 0xffff);
+	fman_mdio_wait(regs);
 	return (0);
 }
 
@@ -290,7 +332,7 @@ find_mdio_bus(const void *fdt, int mdio_node,
 }
 
 static struct phy_device *
-phy_obtain(const void *fdt, int mdio_node, int phy)
+phy_obtain(const void *fdt, int is_c45, int mdio_node, int addr)
 {
 	struct phy_device *phy_dev;
 	int err;
@@ -300,7 +342,8 @@ phy_obtain(const void *fdt, int mdio_node, int phy)
 		return (NULL);
 	}
 
-	phy_dev->mdio.addr = phy;
+	phy_dev->mdio.addr = addr;
+	phy_dev->mdio.is_c45 = is_c45;
 	MDIO_LOCK();
 	err = find_mdio_bus(fdt, mdio_node, phy_dev);
 	MDIO_UNLOCK();
@@ -317,21 +360,28 @@ struct phy_device *
 of_phy_find_device(struct device_node *dn)
 {
 	const void *fdt;
-	const fdt32_t *phy;
+	const fdt32_t *addr;
 	int len;
+	int is_c45;
 	int mdio_node;
 
 	fdt = bsp_fdt_get();
 
-	phy = fdt_getprop(fdt, dn->offset, "reg", &len);
-	if (phy == NULL || len != sizeof(*phy)) {
+	addr = fdt_getprop(fdt, dn->offset, "reg", &len);
+	if (addr == NULL || len != sizeof(*addr)) {
 		return (NULL);
 	}
 
+	if (of_device_is_compatible(dn, "ethernet-phy-ieee802.3-c45")) {
+		is_c45 = MII_ADDR_C45;
+	} else {
+		is_c45 = 0;
+	}
+
 	mdio_node = fdt_parent_offset(fdt, dn->offset);
 	if (mdio_node < 0) {
 		return (NULL);
 	}
 
-	return (phy_obtain(fdt, mdio_node, (int)fdt32_to_cpu(*phy)));
+	return (phy_obtain(fdt, is_c45, mdio_node, (int)fdt32_to_cpu(*addr)));
 }