[U-Boot,07/14] net: gem: Fix gem driver on 1Gbps LAN

diff --git a/arch/arm/cpu/armv7/zynq/slcr.c b/arch/arm/cpu/armv7/zynq/slcr.c
index 788a8fd..5a8674a 100644
--- a/arch/arm/cpu/armv7/zynq/slcr.c
+++ b/arch/arm/cpu/armv7/zynq/slcr.c
@@ -61,3 +61,29 @@  void zynq_slcr_cpu_reset(void)

 	writel(1, &slcr_base->pss_rst_ctrl);
 }
+
+/* Setup clk for network */
+void zynq_slcr_gem_clk_setup(u32 gem_id, u32 rclk, u32 clk)
+{
+	zynq_slcr_unlock();
+
+	if (gem_id > 1) {
+		printf("Non existing GEM id %d\n", gem_id);
+		goto out;
+	}
+
+	if (gem_id) {
+		/* Set divisors for appropriate frequency in GEM_CLK_CTRL */
+		writel(clk, &slcr_base->gem1_clk_ctrl);
+		/* Configure GEM_RCLK_CTRL */
+		writel(rclk, &slcr_base->gem1_rclk_ctrl);
+	} else {
+		/* Set divisors for appropriate frequency in GEM_CLK_CTRL */
+		writel(clk, &slcr_base->gem0_clk_ctrl);
+		/* Configure GEM_RCLK_CTRL */
+		writel(rclk, &slcr_base->gem0_rclk_ctrl);
+	}
+
+out:
+	zynq_slcr_lock();
+}
diff --git a/arch/arm/include/asm/arch-zynq/hardware.h b/arch/arm/include/asm/arch-zynq/hardware.h
index 1b72773..bee7b09 100644
--- a/arch/arm/include/asm/arch-zynq/hardware.h
+++ b/arch/arm/include/asm/arch-zynq/hardware.h
@@ -33,7 +33,12 @@  struct slcr_regs {
 	u32 scl; /* 0x0 */
 	u32 slcr_lock; /* 0x4 */
 	u32 slcr_unlock; /* 0x8 */
-	u32 reserved1[125];
+	u32 reserved0[75];
+	u32 gem0_rclk_ctrl; /* 0x138 */
+	u32 gem1_rclk_ctrl; /* 0x13c */
+	u32 gem0_clk_ctrl; /* 0x140 */
+	u32 gem1_clk_ctrl; /* 0x144 */
+	u32 reserved1[46];
 	u32 pss_rst_ctrl; /* 0x200 */
 	u32 reserved2[15];
 	u32 fpga_rst_ctrl; /* 0x240 */
diff --git a/arch/arm/include/asm/arch-zynq/sys_proto.h b/arch/arm/include/asm/arch-zynq/sys_proto.h
index e788900..57128dc 100644
--- a/arch/arm/include/asm/arch-zynq/sys_proto.h
+++ b/arch/arm/include/asm/arch-zynq/sys_proto.h
@@ -26,5 +26,6 @@ 
 extern void zynq_slcr_lock(void);
 extern void zynq_slcr_unlock(void);
 extern void zynq_slcr_cpu_reset(void);
+extern void zynq_slcr_gem_clk_setup(u32 gem_id, u32 rclk, u32 clk);

 #endif /* _SYS_PROTO_H_ */
diff --git a/drivers/net/zynq_gem.c b/drivers/net/zynq_gem.c
index 7758cf8..2d717e9 100644
--- a/drivers/net/zynq_gem.c
+++ b/drivers/net/zynq_gem.c
@@ -33,6 +33,7 @@ 
 #include <phy.h>
 #include <miiphy.h>
 #include <watchdog.h>
+#include <asm/arch/sys_proto.h>

 #if !defined(CONFIG_PHYLIB)
 # error XILINX_GEM_ETHERNET requires PHYLIB
@@ -67,13 +68,14 @@ 
 #define ZYNQ_GEM_NWCTRL_MDEN_MASK	0x00000010 /* Enable MDIO port */
 #define ZYNQ_GEM_NWCTRL_STARTTX_MASK	0x00000200 /* Start tx (tx_go) */

-#define ZYNQ_GEM_NWCFG_SPEED		0x00000001 /* 100 Mbps operation */
-#define ZYNQ_GEM_NWCFG_FDEN		0x00000002 /* Full Duplex mode */
-#define ZYNQ_GEM_NWCFG_FSREM		0x00020000 /* FCS removal */
+#define ZYNQ_GEM_NWCFG_SPEED100		0x000000001 /* 100 Mbps operation */
+#define ZYNQ_GEM_NWCFG_SPEED1000	0x000000400 /* 1Gbps operation */
+#define ZYNQ_GEM_NWCFG_FDEN		0x000000002 /* Full Duplex mode */
+#define ZYNQ_GEM_NWCFG_FSREM		0x000020000 /* FCS removal */
 #define ZYNQ_GEM_NWCFG_MDCCLKDIV	0x000080000 /* Div pclk by 32, 80MHz */
+#define ZYNQ_GEM_NWCFG_MDCCLKDIV2	0x0000c0000 /* Div pclk by 48, 120MHz */

-#define ZYNQ_GEM_NWCFG_INIT		(ZYNQ_GEM_NWCFG_SPEED | \
-					ZYNQ_GEM_NWCFG_FDEN | \
+#define ZYNQ_GEM_NWCFG_INIT		(ZYNQ_GEM_NWCFG_FDEN | \
 					ZYNQ_GEM_NWCFG_FSREM | \
 					ZYNQ_GEM_NWCFG_MDCCLKDIV)

@@ -227,7 +229,7 @@  static int zynq_gem_setup_mac(struct eth_device *dev)

 static int zynq_gem_init(struct eth_device *dev, bd_t * bis)
 {
-	u32 i;
+	u32 i, rclk, clk = 0;
 	struct phy_device *phydev;
 	const u32 stat_size = (sizeof(struct zynq_gem_regs) -
 				offsetof(struct zynq_gem_regs, stat)) / 4;
@@ -277,16 +279,11 @@  static int zynq_gem_init(struct eth_device *dev, bd_t * bis)
 		/* Write RxBDs to IP */
 		writel((u32)&(priv->rx_bd), &regs->rxqbase);

-		/* MAC Setup */
-		/* Setup Network Configuration register */
-		writel(ZYNQ_GEM_NWCFG_INIT, &regs->nwcfg);
-
 		/* Setup for DMA Configuration register */
 		writel(ZYNQ_GEM_DMACR_INIT, &regs->dmacr);

 		/* Setup for Network Control register, MDIO, Rx and Tx enable */
-		setbits_le32(&regs->nwctrl, ZYNQ_GEM_NWCTRL_MDEN_MASK |
-			ZYNQ_GEM_NWCTRL_RXEN_MASK | ZYNQ_GEM_NWCTRL_TXEN_MASK);
+		setbits_le32(&regs->nwctrl, ZYNQ_GEM_NWCTRL_MDEN_MASK);

 		priv->init++;
 	}
@@ -294,12 +291,38 @@  static int zynq_gem_init(struct eth_device *dev, bd_t * bis)
 	/* interface - look at tsec */
 	phydev = phy_connect(priv->bus, priv->phyaddr, dev, 0);

-	phydev->supported &= supported;
+	phydev->supported = supported | ADVERTISED_Pause |
+			    ADVERTISED_Asym_Pause;
 	phydev->advertising = phydev->supported;
 	priv->phydev = phydev;
 	phy_config(phydev);
 	phy_startup(phydev);

+	switch (phydev->speed) {
+	case SPEED_1000:
+		writel(ZYNQ_GEM_NWCFG_INIT | ZYNQ_GEM_NWCFG_SPEED1000,
+		       &regs->nwcfg);
+		rclk = (0 << 4) | (1 << 0);
+		clk = (1 << 20) | (8 << 8) | (0 << 4) | (1 << 0);
+		break;
+	case SPEED_100:
+		clrsetbits_le32(&regs->nwcfg, ZYNQ_GEM_NWCFG_SPEED1000,
+				ZYNQ_GEM_NWCFG_INIT | ZYNQ_GEM_NWCFG_SPEED100);
+		rclk = 1 << 0;
+		clk = (5 << 20) | (8 << 8) | (0 << 4) | (1 << 0);
+		break;
+	case SPEED_10:
+		rclk = 1 << 0;
+		/* FIXME untested */
+		clk = (5 << 20) | (8 << 8) | (0 << 4) | (1 << 0);
+		break;
+	}
+	/* FIXME maybe better to define gem address in hardware.h */
+	zynq_slcr_gem_clk_setup(dev->iobase != 0xE000B000, rclk, clk);
+
+	setbits_le32(&regs->nwctrl, ZYNQ_GEM_NWCTRL_RXEN_MASK |
+					ZYNQ_GEM_NWCTRL_TXEN_MASK);
+
 	return 0;
 }

@@ -380,8 +403,8 @@  static void zynq_gem_halt(struct eth_device *dev)
 {
 	struct zynq_gem_regs *regs = (struct zynq_gem_regs *)dev->iobase;

-	/* Disable the receiver & transmitter */
-	writel(0, &regs->nwctrl);
+	clrsetbits_le32(&regs->nwctrl, ZYNQ_GEM_NWCTRL_RXEN_MASK |
+						ZYNQ_GEM_NWCTRL_TXEN_MASK, 0);
 }

 static int zynq_gem_miiphyread(const char *devname, uchar addr,