1 files changed, 83 insertions, 2 deletions

diff --git a/arch/arm/mach-kirkwood/board-dt.c b/arch/arm/mach-kirkwood/board-dt.c
index d8b666c39fd1..27c1877e6dda 100644
--- a/arch/arm/mach-kirkwood/board-dt.c
+++ b/arch/arm/mach-kirkwood/board-dt.c
@@ -13,6 +13,8 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_net.h>
 #include <linux/of_platform.h>
 #include <linux/clk-provider.h>
 #include <linux/clocksource.h>
@@ -58,6 +60,85 @@ static void __init kirkwood_legacy_clk_init(void)
 	clk_prepare_enable(clk);
 }
 
+#define MV643XX_ETH_MAC_ADDR_LOW	0x0414
+#define MV643XX_ETH_MAC_ADDR_HIGH	0x0418
+
+static void __init kirkwood_dt_eth_fixup(void)
+{
+	struct device_node *np;
+
+	/*
+	 * The ethernet interfaces forget the MAC address assigned by u-boot
+	 * if the clocks are turned off. Usually, u-boot on kirkwood boards
+	 * has no DT support to properly set local-mac-address property.
+	 * As a workaround, we get the MAC address from mv643xx_eth registers
+	 * and update the port device node if no valid MAC address is set.
+	 */
+	for_each_compatible_node(np, NULL, "marvell,kirkwood-eth-port") {
+		struct device_node *pnp = of_get_parent(np);
+		struct clk *clk;
+		struct property *pmac;
+		void __iomem *io;
+		u8 *macaddr;
+		u32 reg;
+
+		if (!pnp)
+			continue;
+
+		/* skip disabled nodes or nodes with valid MAC address*/
+		if (!of_device_is_available(pnp) || of_get_mac_address(np))
+			goto eth_fixup_skip;
+
+		clk = of_clk_get(pnp, 0);
+		if (IS_ERR(clk))
+			goto eth_fixup_skip;
+
+		io = of_iomap(pnp, 0);
+		if (!io)
+			goto eth_fixup_no_map;
+
+		/* ensure port clock is not gated to not hang CPU */
+		clk_prepare_enable(clk);
+
+		/* store MAC address register contents in local-mac-address */
+		pr_err(FW_INFO "%s: local-mac-address is not set\n",
+		       np->full_name);
+
+		pmac = kzalloc(sizeof(*pmac) + 6, GFP_KERNEL);
+		if (!pmac)
+			goto eth_fixup_no_mem;
+
+		pmac->value = pmac + 1;
+		pmac->length = 6;
+		pmac->name = kstrdup("local-mac-address", GFP_KERNEL);
+		if (!pmac->name) {
+			kfree(pmac);
+			goto eth_fixup_no_mem;
+		}
+
+		macaddr = pmac->value;
+		reg = readl(io + MV643XX_ETH_MAC_ADDR_HIGH);
+		macaddr[0] = (reg >> 24) & 0xff;
+		macaddr[1] = (reg >> 16) & 0xff;
+		macaddr[2] = (reg >> 8) & 0xff;
+		macaddr[3] = reg & 0xff;
+
+		reg = readl(io + MV643XX_ETH_MAC_ADDR_LOW);
+		macaddr[4] = (reg >> 8) & 0xff;
+		macaddr[5] = reg & 0xff;
+
+		of_update_property(np, pmac);
+
+eth_fixup_no_mem:
+		iounmap(io);
+		clk_disable_unprepare(clk);
+eth_fixup_no_map:
+		clk_put(clk);
+eth_fixup_skip:
+		of_node_put(pnp);
+	}
+}
+
 static void __init kirkwood_dt_time_init(void)
 {
 	of_clk_init(NULL);
@@ -82,12 +163,12 @@ static void __init kirkwood_dt_init(void)
 	kirkwood_l2_init();
 
 	kirkwood_cpufreq_init();
-
+	kirkwood_cpuidle_init();
 	/* Setup clocks for legacy devices */
 	kirkwood_legacy_clk_init();
 
 	kirkwood_pm_init();
-	kirkwood_cpuidle_init();
+	kirkwood_dt_eth_fixup();
 
 #ifdef CONFIG_KEXEC
 	kexec_reinit = kirkwood_enable_pcie;