3 files changed, 63 insertions, 16 deletions

diff --git a/arch/powerpc/cpu/mpc85xx/p5040_ids.c b/arch/powerpc/cpu/mpc85xx/p5040_ids.c
index d4343ef..98a568f 100644
--- a/arch/powerpc/cpu/mpc85xx/p5040_ids.c
+++ b/arch/powerpc/cpu/mpc85xx/p5040_ids.c
@@ -49,23 +49,23 @@ int liodn_tbl_sz = ARRAY_SIZE(liodn_tbl);
 
 #ifdef CONFIG_SYS_DPAA_FMAN
 struct liodn_id_table fman1_liodn_tbl[] = {
-	SET_FMAN_RX_1G_LIODN(1, 0, 6),
-	SET_FMAN_RX_1G_LIODN(1, 1, 7),
-	SET_FMAN_RX_1G_LIODN(1, 2, 8),
-	SET_FMAN_RX_1G_LIODN(1, 3, 9),
-	SET_FMAN_RX_1G_LIODN(1, 4, 10),
-	SET_FMAN_RX_10G_LIODN(1, 0, 11),
+	SET_FMAN_RX_1G_LIODN(1, 0, 11),
+	SET_FMAN_RX_1G_LIODN(1, 1, 12),
+	SET_FMAN_RX_1G_LIODN(1, 2, 13),
+	SET_FMAN_RX_1G_LIODN(1, 3, 14),
+	SET_FMAN_RX_1G_LIODN(1, 4, 15),
+	SET_FMAN_RX_10G_LIODN(1, 0, 16),
 };
 int fman1_liodn_tbl_sz = ARRAY_SIZE(fman1_liodn_tbl);
 
 #if (CONFIG_SYS_NUM_FMAN == 2)
 struct liodn_id_table fman2_liodn_tbl[] = {
-	SET_FMAN_RX_1G_LIODN(2, 0, 12),
-	SET_FMAN_RX_1G_LIODN(2, 1, 13),
-	SET_FMAN_RX_1G_LIODN(2, 2, 14),
-	SET_FMAN_RX_1G_LIODN(2, 3, 15),
-	SET_FMAN_RX_1G_LIODN(2, 4, 16),
-	SET_FMAN_RX_10G_LIODN(2, 0, 17),
+	SET_FMAN_RX_1G_LIODN(2, 0, 17),
+	SET_FMAN_RX_1G_LIODN(2, 1, 18),
+	SET_FMAN_RX_1G_LIODN(2, 2, 19),
+	SET_FMAN_RX_1G_LIODN(2, 3, 20),
+	SET_FMAN_RX_1G_LIODN(2, 4, 21),
+	SET_FMAN_RX_10G_LIODN(2, 0, 22),
 };
 int fman2_liodn_tbl_sz = ARRAY_SIZE(fman2_liodn_tbl);
 #endif
diff --git a/arch/powerpc/cpu/mpc85xx/portals.c b/arch/powerpc/cpu/mpc85xx/portals.c
index 98815f8..ec3b292 100644
--- a/arch/powerpc/cpu/mpc85xx/portals.c
+++ b/arch/powerpc/cpu/mpc85xx/portals.c
@@ -14,9 +14,46 @@
 #include <asm/fsl_portals.h>
 #include <asm/fsl_liodn.h>
 
+#define MAX_BPORTALS (CONFIG_SYS_BMAN_CINH_SIZE / CONFIG_SYS_BMAN_SP_CINH_SIZE)
+#define MAX_QPORTALS (CONFIG_SYS_QMAN_CINH_SIZE / CONFIG_SYS_QMAN_SP_CINH_SIZE)
+static void inhibit_portals(void __iomem *addr, int max_portals,
+			int arch_max_portals, int portal_cinh_size)
+{
+	uint32_t val;
+	int i;
+
+	/* arch_max_portals is the maximum based on memory size. This includes
+	 * the reserved memory in the SoC.  max_portals the number of physical
+	 * portals in the SoC */
+	if (max_portals > arch_max_portals) {
+		printf("ERROR: portal config error\n");
+		max_portals = arch_max_portals;
+	}
+
+	for (i = 0; i < max_portals; i++) {
+		out_be32(addr, -1);
+		val = in_be32(addr);
+		if (!val) {
+			printf("ERROR: Stopped after %d portals\n", i);
+			goto done;
+		}
+		addr += portal_cinh_size;
+	}
+#ifdef DEBUG
+	printf("Cleared %d portals\n", i);
+#endif
+done:
+
+	return;
+}
+
 void setup_portals(void)
 {
 	ccsr_qman_t *qman = (void *)CONFIG_SYS_FSL_QMAN_ADDR;
+	void __iomem *bpaddr = (void *)CONFIG_SYS_BMAN_CINH_BASE +
+				CONFIG_SYS_BMAN_SWP_ISDR_REG;
+	void __iomem *qpaddr = (void *)CONFIG_SYS_QMAN_CINH_BASE +
+				CONFIG_SYS_QMAN_SWP_ISDR_REG;
 #ifdef CONFIG_FSL_CORENET
 	int i;
 
@@ -38,6 +75,12 @@ void setup_portals(void)
 	out_be32(&qman->qcsp_bare, (u32)(CONFIG_SYS_QMAN_MEM_PHYS >> 32));
 #endif
 	out_be32(&qman->qcsp_bar, (u32)CONFIG_SYS_QMAN_MEM_PHYS);
+
+	/* Change default state of BMan ISDR portals to all 1s */
+	inhibit_portals(bpaddr, CONFIG_SYS_BMAN_NUM_PORTALS, MAX_BPORTALS,
+			CONFIG_SYS_BMAN_SP_CINH_SIZE);
+	inhibit_portals(qpaddr, CONFIG_SYS_QMAN_NUM_PORTALS, MAX_QPORTALS,
+			CONFIG_SYS_QMAN_SP_CINH_SIZE);
 }
 
 /* Update portal containter to match LAW setup of portal in phy map */
diff --git a/arch/powerpc/cpu/mpc85xx/tlb.c b/arch/powerpc/cpu/mpc85xx/tlb.c
index 4adba95..8e0508f 100644
--- a/arch/powerpc/cpu/mpc85xx/tlb.c
+++ b/arch/powerpc/cpu/mpc85xx/tlb.c
@@ -299,12 +299,16 @@ unsigned int setup_ddr_tlbs_phys(phys_addr_t p_addr,
 {
 	unsigned int ram_tlb_address = (unsigned int)CONFIG_SYS_DDR_SDRAM_BASE;
 	u64 memsize = (u64)memsize_in_meg << 20;
+	u64 size;
 
-	memsize = min(memsize, (u64)CONFIG_MAX_MEM_MAPPED);
-	memsize = tlb_map_range(ram_tlb_address, p_addr, memsize, TLB_MAP_RAM);
+	size = min(memsize, (u64)CONFIG_MAX_MEM_MAPPED);
+	size = tlb_map_range(ram_tlb_address, p_addr, size, TLB_MAP_RAM);
 
-	if (memsize)
-		print_size(memsize, " left unmapped\n");
+	if (size || memsize > CONFIG_MAX_MEM_MAPPED) {
+		print_size(memsize > CONFIG_MAX_MEM_MAPPED ?
+			   memsize - CONFIG_MAX_MEM_MAPPED + size : size,
+			   " left unmapped\n");
+	}
 
 	return memsize_in_meg;
 }