powerpc/mpc85xx: Revise workaround for DDR-A003

Existing workaround only handles one RDIMM on reference design. In case
of two RDIMMs being used, the workaround requires two separate writes to
DDR_SDRAM_MD_CNTL register.

This patch also restores two debug registers changed by the workaround.

Signed-off-by: York Sun <yorksun@freescale.com>
CC: Ben Collins <ben.c@servergy.com>
CC: James Yang <James.Yang@freescale.com>

drivers/ddr/fsl/mpc85xx_ddr_gen3.c

@@ -39,6 +39,9 @@ void fsl_ddr_set_memctl_regs(const fsl_ddr_cfg_regs_t *regs,
 	unsigned int csn_bnds_backup = 0, cs_sa, cs_ea, *csn_bnds_t;
 	int csn = -1;
 #endif
+#ifdef CONFIG_SYS_FSL_ERRATUM_DDR_A003
+	u32 save1, save2;
+#endif
 
 	switch (ctrl_num) {
 	case 0:
@@ -197,6 +200,8 @@ step2:
 		out_be32(&ddr->ddr_wrlvl_cntl, regs->ddr_wrlvl_cntl & 0x7fffffff);
 		out_be32(&ddr->sdram_cfg_2, regs->ddr_sdram_cfg_2 & 0xffffffeb);
 		out_be32(&ddr->mtcr, 0);
+		save1 = in_be32(&ddr->debug[12]);
+		save2 = in_be32(&ddr->debug[21]);
 		out_be32(&ddr->debug[12], 0x00000015);
 		out_be32(&ddr->debug[21], 0x24000000);
 		out_be32(&ddr->sdram_interval, regs->ddr_sdram_interval & 0xffff);
@@ -214,6 +219,18 @@ step2:
 				0x04000000		|
 				MD_CNTL_WRCW		|
 				MD_CNTL_MD_VALUE(0x02));
+#if (CONFIG_DIMM_SLOTS_PER_CTLR == 2)
+			if (!(regs->cs[2].config & SDRAM_CS_CONFIG_EN))
+				break;
+			while (in_be32(&ddr->sdram_md_cntl) & MD_CNTL_MD_EN)
+				;
+			out_be32(&ddr->sdram_md_cntl,
+				 MD_CNTL_MD_EN		|
+				 MD_CNTL_CS_SEL_CS2_CS3	|
+				 0x04000000		|
+				 MD_CNTL_WRCW		|
+				 MD_CNTL_MD_VALUE(0x02));
+#endif
 			break;
 		case 0x00100000:
 			out_be32(&ddr->sdram_md_cntl,
@@ -222,6 +239,18 @@ step2:
 				0x04000000		|
 				MD_CNTL_WRCW		|
 				MD_CNTL_MD_VALUE(0x0a));
+#if (CONFIG_DIMM_SLOTS_PER_CTLR == 2)
+			if (!(regs->cs[2].config & SDRAM_CS_CONFIG_EN))
+				break;
+			while (in_be32(&ddr->sdram_md_cntl) & MD_CNTL_MD_EN)
+				;
+			out_be32(&ddr->sdram_md_cntl,
+				 MD_CNTL_MD_EN		|
+				 MD_CNTL_CS_SEL_CS2_CS3	|
+				 0x04000000		|
+				 MD_CNTL_WRCW		|
+				 MD_CNTL_MD_VALUE(0x0a));
+#endif
 			break;
 		case 0x00200000:
 			out_be32(&ddr->sdram_md_cntl,
@@ -230,6 +259,18 @@ step2:
 				0x04000000		|
 				MD_CNTL_WRCW		|
 				MD_CNTL_MD_VALUE(0x12));
+#if (CONFIG_DIMM_SLOTS_PER_CTLR == 2)
+			if (!(regs->cs[2].config & SDRAM_CS_CONFIG_EN))
+				break;
+			while (in_be32(&ddr->sdram_md_cntl) & MD_CNTL_MD_EN)
+				;
+			out_be32(&ddr->sdram_md_cntl,
+				 MD_CNTL_MD_EN		|
+				 MD_CNTL_CS_SEL_CS2_CS3	|
+				 0x04000000		|
+				 MD_CNTL_WRCW		|
+				 MD_CNTL_MD_VALUE(0x12));
+#endif
 			break;
 		case 0x00300000:
 			out_be32(&ddr->sdram_md_cntl,
@@ -238,6 +279,18 @@ step2:
 				0x04000000		|
 				MD_CNTL_WRCW		|
 				MD_CNTL_MD_VALUE(0x1a));
+#if (CONFIG_DIMM_SLOTS_PER_CTLR == 2)
+			if (!(regs->cs[2].config & SDRAM_CS_CONFIG_EN))
+				break;
+			while (in_be32(&ddr->sdram_md_cntl) & MD_CNTL_MD_EN)
+				;
+			out_be32(&ddr->sdram_md_cntl,
+				 MD_CNTL_MD_EN		|
+				 MD_CNTL_CS_SEL_CS2_CS3	|
+				 0x04000000		|
+				 MD_CNTL_WRCW		|
+				 MD_CNTL_MD_VALUE(0x1a));
+#endif
 			break;
 		default:
 			out_be32(&ddr->sdram_md_cntl,
@@ -246,6 +299,18 @@ step2:
 				0x04000000		|
 				MD_CNTL_WRCW		|
 				MD_CNTL_MD_VALUE(0x02));
+#if (CONFIG_DIMM_SLOTS_PER_CTLR == 2)
+			if (!(regs->cs[2].config & SDRAM_CS_CONFIG_EN))
+				break;
+			while (in_be32(&ddr->sdram_md_cntl) & MD_CNTL_MD_EN)
+				;
+			out_be32(&ddr->sdram_md_cntl,
+				 MD_CNTL_MD_EN		|
+				 MD_CNTL_CS_SEL_CS2_CS3	|
+				 0x04000000		|
+				 MD_CNTL_WRCW		|
+				 MD_CNTL_MD_VALUE(0x02));
+#endif
 			printf("Unsupported RC10\n");
 			break;
 		}
@@ -259,8 +324,8 @@ step2:
 		out_be32(&ddr->ddr_zq_cntl, regs->ddr_zq_cntl);
 		out_be32(&ddr->ddr_wrlvl_cntl, regs->ddr_wrlvl_cntl);
 		out_be32(&ddr->sdram_cfg_2, regs->ddr_sdram_cfg_2);
-		out_be32(&ddr->debug[12], 0x0);
-		out_be32(&ddr->debug[21], 0x0);
+		out_be32(&ddr->debug[12], save1);
+		out_be32(&ddr->debug[21], save2);
 		out_be32(&ddr->sdram_interval, regs->ddr_sdram_interval);
 
 	}