1 files changed, 75 insertions, 14 deletions

diff --git a/drivers/mtd/hbmc-am654.c b/drivers/mtd/hbmc-am654.c
index 846b0e832b..ec375fd54e 100644
--- a/drivers/mtd/hbmc-am654.c
+++ b/drivers/mtd/hbmc-am654.c
@@ -12,27 +12,32 @@
 
 #define FSS_SYSC_REG	0x4
 
-#define HYPERBUS_CALIB_COUNT 25
+#define HYPERBUS_CALIB_COUNT 64
+#define HYPERBUS_CALIB_MIN_TRIES 48
+#define HYPERBUS_CALIB_SUCCESS_COUNT 4
+#define HYPERBUS_CALIB_SIZE 64
+
+#define HYPERBUS_CFG_DLL_STAT_REG 4
+#define HYPERBUS_CFG_MDLL_LOCK BIT(0)
+#define HYPERBUS_CFG_SDLL_LOCK BIT(1)
+#define HYPERBUS_CFG_RAM_STAT_REG 8
+#define HYPERBUS_CFG_RAM_FIFO_INIT_DONE BIT(0)
 
 struct am654_hbmc_priv {
 	void __iomem *mmiobase;
+	void __iomem *cfgregbase;
 	bool calibrated;
 };
 
-/* Calibrate by looking for "QRY" string within the CFI space */
-static int am654_hyperbus_calibrate(struct udevice *dev)
+/* confirm calibration by looking for "QRY" string within the CFI space */
+static int am654_hyperbus_calibrate_check(struct udevice *dev)
 {
 	struct am654_hbmc_priv *priv = dev_get_priv(dev);
 	int count = HYPERBUS_CALIB_COUNT;
 	int pass_count = 0;
+	int ret = -EIO;
 	u16 qry[3];
 
-	if (priv->calibrated)
-		return 0;
-
-	writew(0xF0, priv->mmiobase);
-	writew(0x98, priv->mmiobase + 0xaa);
-
 	while (count--) {
 		qry[0] = readw(priv->mmiobase + 0x20);
 		qry[1] = readw(priv->mmiobase + 0x22);
@@ -42,13 +47,67 @@ static int am654_hyperbus_calibrate(struct udevice *dev)
 			pass_count++;
 		else
 			pass_count = 0;
-		if (pass_count == 5)
+
+		if (pass_count > HYPERBUS_CALIB_SUCCESS_COUNT)
+			return 0;
+	}
+
+	return ret;
+}
+
+static int am654_hyperbus_calibrate(struct udevice *dev)
+{
+	struct am654_hbmc_priv *priv = dev_get_priv(dev);
+	char verifybuf[HYPERBUS_CALIB_SIZE];
+	char rdbuf[HYPERBUS_CALIB_SIZE];
+	int tries = HYPERBUS_CALIB_COUNT;
+	int pass_count = 0;
+	unsigned int reg;
+	int ret = -EIO;
+
+	if (priv->calibrated)
+		return 0;
+
+	if (!priv->cfgregbase || !priv->mmiobase)
+		return ret;
+
+	reg = readl(priv->cfgregbase + HYPERBUS_CFG_RAM_STAT_REG);
+	if (!(reg & HYPERBUS_CFG_RAM_FIFO_INIT_DONE)) {
+		dev_err(dev, "Hyperbus RAM FIFO init failed\n");
+		return ret;
+	}
+
+	writew(0xF0, priv->mmiobase);
+	writew(0x98, priv->mmiobase + 0xaa);
+
+	/*
+	 * Perform calibration by reading 64 bytes from CFI region and
+	 * compare with previously read data, and ensure Delay Locked Loop(DLL)
+	 * is stabilized.
+	 */
+	while (tries--) {
+		memcpy(rdbuf, priv->mmiobase, HYPERBUS_CALIB_SIZE);
+		if (!memcmp(rdbuf, verifybuf, HYPERBUS_CALIB_SIZE)) {
+			reg = readl(priv->cfgregbase + HYPERBUS_CFG_DLL_STAT_REG);
+			if ((reg & HYPERBUS_CFG_MDLL_LOCK) &&
+			    (reg & HYPERBUS_CFG_SDLL_LOCK))
+				pass_count++;
+			else
+				pass_count = 0;
+		}
+		memcpy(verifybuf, rdbuf, HYPERBUS_CALIB_SIZE);
+		if (pass_count > HYPERBUS_CALIB_SUCCESS_COUNT &&
+		    tries < HYPERBUS_CALIB_MIN_TRIES)
 			break;
 	}
+
+	if (tries > 0)
+		ret = am654_hyperbus_calibrate_check(dev);
+
 	writew(0xF0, priv->mmiobase);
 	writew(0xFF, priv->mmiobase);
 
-	return pass_count == 5;
+	return ret;
 }
 
 static int am654_select_hbmc(struct udevice *dev)
@@ -68,7 +127,9 @@ static int am654_hbmc_probe(struct udevice *dev)
 	struct am654_hbmc_priv *priv = dev_get_priv(dev);
 	int ret;
 
-	priv->mmiobase = devfdt_remap_addr_index(dev, 1);
+	priv->cfgregbase = devfdt_remap_addr_index(dev, 0);
+	priv->mmiobase = devfdt_remap_addr_index(dev, 2);
+
 	if (dev_read_bool(dev, "mux-controls")) {
 		ret = am654_select_hbmc(dev);
 		if (ret) {
@@ -79,9 +140,9 @@ static int am654_hbmc_probe(struct udevice *dev)
 
 	if (!priv->calibrated) {
 		ret = am654_hyperbus_calibrate(dev);
-		if (!ret) {
+		if (IS_ERR_VALUE(ret)) {
 			dev_err(dev, "Calibration Failed\n");
-			return -EIO;
+			return ret;
 		}
 	}
 	priv->calibrated = true;