[PATCH 2/2] imx6-mmdc: Revert calibration configuration on failure

[John Watts <contact@jookia.org>]

When calibration fails it will clobber the existing configuration,
even if the configuration was working but not ideal.

Reverting calibration from a known bad value to a possibly good value
gives a better chance of the memory working.

I've tested this on the Novena board and had good success with it.
U-Boot implements a similar solution in its write level calibration.

Signed-off-by: John Watts <contact@jookia.org>
---
 arch/arm/mach-imx/imx6-mmdc.c | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/arch/arm/mach-imx/imx6-mmdc.c b/arch/arm/mach-imx/imx6-mmdc.c
index 908771626a..0d50b39566 100644
--- a/arch/arm/mach-imx/imx6-mmdc.c
+++ b/arch/arm/mach-imx/imx6-mmdc.c
@@ -25,11 +25,18 @@ static bool wlcalib_failed(void __iomem *ips)
 
 int mmdc_do_write_level_calibration(void)
 {
+	u32 ldectrl[4] = {0};
 	u32 esdmisc_val, zq_val;
 	int errorcount = 0;
 	u32 val;
 	u32 ddr_mr1 = 0x4;
 
+	/* Store current calibration data in case of failure */
+	ldectrl[0] = readl(P0_IPS + MPWLDECTRL0);
+	ldectrl[1] = readl(P0_IPS + MPWLDECTRL1);
+	ldectrl[2] = readl(P1_IPS + MPWLDECTRL0);
+	ldectrl[3] = readl(P1_IPS + MPWLDECTRL1);
+
 	/* disable DDR logic power down timer */
 	val = readl((P0_IPS + MDPDC));
 	val &= 0xffff00ff;
@@ -69,8 +76,14 @@ int mmdc_do_write_level_calibration(void)
 	while (readl(P0_IPS + MPWLGCR) & 0x00000001);
 
 	/* check for any errors on both PHYs */
-	if (wlcalib_failed(P0_IPS) || wlcalib_failed(P1_IPS))
+	if (wlcalib_failed(P0_IPS) || wlcalib_failed(P1_IPS)) {
+		pr_debug("Calibration failed, rolling back calibration data\n");
+		writel(ldectrl[0], P0_IPS + MPWLDECTRL0);
+		writel(ldectrl[1], P0_IPS + MPWLDECTRL1);
+		writel(ldectrl[2], P1_IPS + MPWLDECTRL0);
+		writel(ldectrl[3], P1_IPS + MPWLDECTRL1);
 		errorcount++;
+	}
 
 	pr_debug("Write leveling calibration completed\n");
 
-- 
2.39.0