UBUNTU: SAUCE: (no-up) ata: Disparity/10b8b Error cleaning loop for Mustang boards.

BugLink: http://bugs.launchpad.net/bugs/1350087

Due to fact that we have different margin across different boards,
so the BERR rate is high across boards. So we need to clean these
erros by digital resetting the PHY.

Signed-off-by: Loc Ho <lho@apm.com>
Signed-off-by: Suman Tripathi <stripathi@apm.com>
(not yet submitted)
Signed-off-by: dann frazier <dann.frazier@canonical.com>

Conflicts:
	drivers/ata/ahci_xgene.c

2 files changed, 180 insertions, 2 deletions

diff --git a/arch/arm64/boot/dts/apm/apm-storm.dtsi b/arch/arm64/boot/dts/apm/apm-storm.dtsi
index f1ad9c2ab2e9..7bdbd8734cd0 100644
--- a/arch/arm64/boot/dts/apm/apm-storm.dtsi
+++ b/arch/arm64/boot/dts/apm/apm-storm.dtsi
@@ -549,6 +549,7 @@
 			      <0x0 0x1f210000 0x0 0x1000>,
 			      <0x0 0x1f21d000 0x0 0x1000>,
 			      <0x0 0x1f21e000 0x0 0x1000>,
+			      <0x0 0x1f21a000 0x0 0x100>,
 			      <0x0 0x1f217000 0x0 0x1000>;
 			interrupts = <0x0 0x86 0x4>;
 			dma-coherent;
@@ -564,6 +565,7 @@
 			      <0x0 0x1f220000 0x0 0x1000>,
 			      <0x0 0x1f22d000 0x0 0x1000>,
 			      <0x0 0x1f22e000 0x0 0x1000>,
+			      <0x0 0x1f22a000 0x0 0x100>,
 			      <0x0 0x1f227000 0x0 0x1000>;
 			interrupts = <0x0 0x87 0x4>;
 			dma-coherent;
@@ -578,7 +580,8 @@
 			reg = <0x0 0x1a800000 0x0 0x1000>,
 			      <0x0 0x1f230000 0x0 0x1000>,
 			      <0x0 0x1f23d000 0x0 0x1000>,
-			      <0x0 0x1f23e000 0x0 0x1000>;
+			      <0x0 0x1f23e000 0x0 0x1000>,
+			      <0x0 0x1f23a000 0x0 0x100>;
 			interrupts = <0x0 0x88 0x4>;
 			dma-coherent;
 			status = "ok";
diff --git a/drivers/ata/ahci_xgene.c b/drivers/ata/ahci_xgene.c
index feeb8f1e2fe8..79d83c1ef50d 100644
--- a/drivers/ata/ahci_xgene.c
+++ b/drivers/ata/ahci_xgene.c
@@ -81,16 +81,138 @@
 /* Max retry for link down */
 #define MAX_LINK_DOWN_RETRY 3
 
+#define RXTX_REG7			0x00e
+#define  RXTX_REG7_RESETB_RXD_MASK	0x00000100
+#define  RXTX_REG7_RESETB_RXA_MASK	0x00000080
+#define SATA_ENET_SDS_IND_CMD_REG	0x0000003c
+#define  CFG_IND_WR_CMD_MASK		0x00000001
+#define  CFG_IND_RD_CMD_MASK		0x00000002
+#define  CFG_IND_CMD_DONE_MASK		0x00000004
+#define  CFG_IND_ADDR_SET(dst, src) \
+		(((dst) & ~0x003ffff0) | (((u32) (src) << 4) & 0x003ffff0))
+#define SATA_ENET_SDS_IND_RDATA_REG	0x00000040
+#define SATA_ENET_SDS_IND_WDATA_REG	0x00000044
+#define SERDES_PLL_INDIRECT_OFFSET	0x0000
+#define SERDES_PLL_REF_INDIRECT_OFFSET	0x20000
+#define SERDES_INDIRECT_OFFSET		0x0400
+#define SERDES_LANE_STRIDE		0x0200
+#define SERDES_LANE_X4_STRIDE		0x30000
+
 struct xgene_ahci_context {
 	struct ahci_host_priv *hpriv;
 	struct device *dev;
 	u8 last_cmd[MAX_AHCI_CHN_PERCTR]; /* tracking the last command issued*/
 	void __iomem *csr_core;		/* Core CSR address of IP */
+	void __iomem *csr_sds;		/* serdes CSR address of IP */
 	void __iomem *csr_diag;		/* Diag CSR address of IP */
 	void __iomem *csr_axi;		/* AXI CSR address of IP */
 	void __iomem *csr_mux;		/* MUX CSR address of IP */
 };
 
+static void ahci_sds_wr(void __iomem *csr_base, u32 indirect_cmd_reg,
+		   	u32 indirect_data_reg, u32 addr, u32 data)
+{
+	unsigned long deadline = jiffies + HZ;
+	u32 val;
+	u32 cmd;
+
+	cmd = CFG_IND_WR_CMD_MASK | CFG_IND_CMD_DONE_MASK;
+	cmd = CFG_IND_ADDR_SET(cmd, addr);
+	writel(data, csr_base + indirect_data_reg);
+	readl(csr_base + indirect_data_reg); /* Force a barrier */
+	writel(cmd, csr_base + indirect_cmd_reg);
+	readl(csr_base + indirect_cmd_reg); /* Force a barrier */
+	do {
+		val = readl(csr_base + indirect_cmd_reg);
+	} while (!(val & CFG_IND_CMD_DONE_MASK)&&
+		 time_before(jiffies, deadline));
+	if (!(val & CFG_IND_CMD_DONE_MASK))
+		pr_err("SDS WR timeout at 0x%p offset 0x%08X value 0x%08X\n",
+		       csr_base + indirect_cmd_reg, addr, data);
+}
+
+static void ahci_sds_rd(void __iomem *csr_base, u32 indirect_cmd_reg,
+		   	u32 indirect_data_reg, u32 addr, u32 *data)
+{
+	unsigned long deadline = jiffies + HZ;
+	u32 val;
+	u32 cmd;
+
+	cmd = CFG_IND_RD_CMD_MASK | CFG_IND_CMD_DONE_MASK;
+	cmd = CFG_IND_ADDR_SET(cmd, addr);
+	writel(cmd, csr_base + indirect_cmd_reg);
+	readl(csr_base + indirect_cmd_reg); /* Force a barrier */
+	do {
+		val = readl(csr_base + indirect_cmd_reg);
+	} while (!(val & CFG_IND_CMD_DONE_MASK)&&
+		 time_before(jiffies, deadline));
+	*data = readl(csr_base + indirect_data_reg);
+	if (!(val & CFG_IND_CMD_DONE_MASK))
+		pr_err("SDS WR timeout at 0x%p offset 0x%08X value 0x%08X\n",
+		       csr_base + indirect_cmd_reg, addr, *data);
+}
+
+static void ahci_serdes_wr(struct xgene_ahci_context *ctx, int lane, 
+			   u32 reg, u32 data)
+{
+	void __iomem *sds_base = ctx->csr_sds;
+	u32 cmd_reg;
+	u32 wr_reg;
+	u32 rd_reg;
+	u32 val;
+	
+	cmd_reg = SATA_ENET_SDS_IND_CMD_REG;
+	wr_reg = SATA_ENET_SDS_IND_WDATA_REG;
+	rd_reg = SATA_ENET_SDS_IND_RDATA_REG;
+
+	reg += (lane / 4) * SERDES_LANE_X4_STRIDE;
+	reg += SERDES_INDIRECT_OFFSET;
+	reg += (lane % 4) * SERDES_LANE_STRIDE;
+	ahci_sds_wr(sds_base, cmd_reg, wr_reg, reg, data);
+	ahci_sds_rd(sds_base, cmd_reg, rd_reg, reg, &val);
+	pr_debug("SERDES WR addr 0x%X value 0x%08X <-> 0x%08X\n", reg, data,
+		 val);
+}
+
+static void ahci_serdes_rd(struct xgene_ahci_context *ctx, int lane, 
+			  u32 reg, u32 *data)
+{
+	void __iomem *sds_base = ctx->csr_sds;
+	u32 cmd_reg;
+	u32 rd_reg;
+
+	cmd_reg = SATA_ENET_SDS_IND_CMD_REG;
+	rd_reg = SATA_ENET_SDS_IND_RDATA_REG;
+
+	reg += (lane / 4) * SERDES_LANE_X4_STRIDE;
+	reg += SERDES_INDIRECT_OFFSET;
+	reg += (lane % 4) * SERDES_LANE_STRIDE;
+	ahci_sds_rd(sds_base, cmd_reg, rd_reg, reg, data);
+	pr_debug("SERDES RD addr 0x%X value 0x%08X\n", reg, *data);
+}
+
+static void ahci_serdes_clrbits(struct xgene_ahci_context *ctx, int lane, 
+				u32 reg, u32 bits)
+{
+	u32 val;
+
+	ahci_serdes_rd(ctx, lane, reg, &val);
+	val &= ~bits;
+	ahci_serdes_wr(ctx, lane, reg, val);
+}
+
+static void ahci_serdes_setbits(struct xgene_ahci_context *ctx, int lane, 
+				u32 reg, u32 bits)
+{
+	u32 val;
+
+	ahci_serdes_rd(ctx, lane, reg, &val);
+	val |= bits;
+	ahci_serdes_wr(ctx, lane, reg, val);
+}
+
+
+
 static int xgene_ahci_init_memram(struct xgene_ahci_context *ctx)
 {
 	dev_dbg(ctx->dev, "Release memory from shutdown\n");
@@ -229,6 +351,30 @@ static void xgene_ahci_set_phy_cfg(struct xgene_ahci_context *ctx, int channel)
 	writel(val, mmio + PORTRANSCFG);
 }
 
+static void xgene_ahci_force_port_phy_rdy(struct xgene_ahci_context *ctx,
+				     int channel, int force)
+{
+	void __iomem *mmio = ctx->hpriv->mmio;
+	u32 val;
+
+	val = readl(mmio + PORTCFG);
+	val = PORTADDR_SET(val, channel == 0 ? 2 : 3);
+	writel(val, mmio + PORTCFG);
+	readl(mmio + PORTCFG);	/* Force a barrier */
+	val = readl(mmio + PORTPHY1CFG);
+	val = PORTPHY1CFG_FRCPHYRDY_SET(val, force);
+	writel(val, mmio + PORTPHY1CFG);
+}
+
+static void xgene_ahci_phy_reset_rxd(struct xgene_ahci_context *ctx, int lane)
+{
+	/* Reset digital Rx */
+	ahci_serdes_clrbits(ctx, lane, RXTX_REG7, RXTX_REG7_RESETB_RXD_MASK);
+	/* As per PHY design spec, the reset requires a minimum of 100us. */
+	usleep_range(100, 150);
+	ahci_serdes_setbits(ctx, lane, RXTX_REG7, RXTX_REG7_RESETB_RXD_MASK);
+}
+
 /**
  * xgene_ahci_do_hardreset - Issue the actual COMRESET
  * @link: link to reset
@@ -290,6 +436,7 @@ static int xgene_ahci_do_hardreset(struct ata_link *link,
 	int link_down_retry = 0;
 	int rc;
 	u32 val, sstatus;
+	int i;
 
 	do {
 		/* clear D2H reception area to properly wait for D2H FIS */
@@ -309,6 +456,28 @@ static int xgene_ahci_do_hardreset(struct ata_link *link,
 	} while (link_down_retry++ < MAX_LINK_DOWN_RETRY &&
 		 (sstatus & 0xff) == 0x1);
 
+	if (*online) {
+		/* Clear SER_DISPARITY/SER_10B_8B_ERR if set due to errata */
+		for (i = 0; i < 5; i++) {
+			/* Check if error bit set */
+			val = readl(port_mmio + PORT_SCR_ERR);
+			if (!(val & (SERR_DISPARITY | SERR_10B_8B_ERR)))
+				break;
+			/* Clear any error due to errata */
+			xgene_ahci_force_port_phy_rdy(ctx, ap->port_no, 1);
+			/* Reset the PHY Rx path */
+			xgene_ahci_phy_reset_rxd(ctx, ap->port_no);	
+			xgene_ahci_force_port_phy_rdy(ctx, ap->port_no, 0);
+			/* Clear all errors */
+			val = readl(port_mmio + PORT_SCR_ERR);
+			writel(val, port_mmio + PORT_SCR_ERR);
+		}
+	} 
+
+	val = readl(port_mmio + PORT_SCR_ERR);
+	if (val & (SERR_DISPARITY | SERR_10B_8B_ERR))
+		dev_warn(ctx->dev, "link has error\n");
+
 	/* clear all errors if any pending */
 	val = readl(port_mmio + PORT_SCR_ERR);
 	writel(val, port_mmio + PORT_SCR_ERR);
@@ -482,8 +651,14 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 	if (IS_ERR(ctx->csr_axi))
 		return PTR_ERR(ctx->csr_axi);
 
-	/* Retrieve the optional IP mux resource */
+	/* Retrive the serdes csr resource */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 4);
+	ctx->csr_sds = devm_ioremap(dev, res->start, resource_size(res));
+	if (IS_ERR(ctx->csr_sds))
+		return PTR_ERR(ctx->csr_sds);
+
+	/* Retrieve the optional IP mux resource */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 5);
 	if (res) {
 		void __iomem *csr = devm_ioremap_resource(dev, res);
 		if (IS_ERR(csr))