[U-Boot] [PATCH 4/8] mpc86xx: Add support to populate addr map based on BATs

[Becky Bruce <beckyb@kernel.crashing.org>]

If CONFIG_ADDR_MAP is enabled, update the address map
whenever we write a bat.

Signed-off-by: Becky Bruce <beckyb@kernel.crashing.org>
---
 cpu/mpc86xx/cpu_init.c |   27 +++++++++++++++++++++++++++
 include/asm-ppc/mmu.h  |   16 +++++++++++++---
 lib_ppc/bat_rw.c       |   28 ++++++++++++++++++++++++++++
 lib_ppc/board.c        |    2 +-
 4 files changed, 69 insertions(+), 4 deletions(-)

diff --git a/cpu/mpc86xx/cpu_init.c b/cpu/mpc86xx/cpu_init.c
index a7e6036..4f29122 100644
--- a/cpu/mpc86xx/cpu_init.c
+++ b/cpu/mpc86xx/cpu_init.c
@@ -154,3 +154,30 @@ void setup_bats(void)
 
 	return;
 }
+
+#ifdef CONFIG_ADDR_MAP
+/* Initialize address mapping array */
+void init_addr_map(void)
+{
+	int i;
+	ppc_bat_t bat = DBAT0;
+	phys_size_t size;
+	unsigned long upper, lower;
+
+	for (i = 0; i < CONFIG_SYS_NUM_ADDR_MAP; i++, bat++) {
+		if (read_bat(bat, &upper, &lower) != -1) {
+			if (!BATU_VALID(upper))
+				size = 0;
+			else
+				size = BATU_SIZE(upper);
+			addrmap_set_entry(BATU_VADDR(upper), BATL_PADDR(lower),
+					  size, i);
+		}
+#ifdef CONFIG_HIGH_BATS
+		/* High bats are not contiguous with low BAT numbers */
+		if (bat == DBAT3)
+			bat = DBAT4 - 1;
+#endif
+	}
+}
+#endif
diff --git a/include/asm-ppc/mmu.h b/include/asm-ppc/mmu.h
index ce04e62..fa92b90 100644
--- a/include/asm-ppc/mmu.h
+++ b/include/asm-ppc/mmu.h
@@ -138,6 +138,10 @@ typedef struct _MMU_context {
 extern void _tlbie(unsigned long va);	/* invalidate a TLB entry */
 extern void _tlbia(void);		/* invalidate all TLB entries */
 
+#ifdef CONFIG_ADDR_MAP
+extern void init_addr_map(void);
+#endif
+
 typedef enum {
 	IBAT0 = 0, IBAT1, IBAT2, IBAT3,
 	DBAT0, DBAT1, DBAT2, DBAT3,
@@ -203,6 +207,14 @@ extern void print_bats(void);
 #define BPP_RX	0x01		/* Read only */
 #define BPP_RW	0x02		/* Read/write */
 
+/* Macros to get values from BATs, once data is in the BAT register format */
+#define BATU_VALID(x) (x & 0x3)
+#define BATU_VADDR(x) (x & 0xfffe0000)
+#define BATL_PADDR(x) ((phys_addr_t)((x & 0xfffe0000)		\
+				     | ((x & 0x0e00ULL) << 24)	\
+				     | ((x & 0x04ULL) << 30)))
+#define BATU_SIZE(x) (1UL << (fls((x & BATU_BL_MAX) >> 2) + 17))
+
 /* Used to set up SDR1 register */
 #define HASH_TABLE_SIZE_64K	0x00010000
 #define HASH_TABLE_SIZE_128K	0x00020000
@@ -462,9 +474,7 @@ extern void set_tlb(u8 tlb, u32 epn, u64 rpn,
 extern void disable_tlb(u8 esel);
 extern void invalidate_tlb(u8 tlb);
 extern void init_tlbs(void);
-#ifdef CONFIG_ADDR_MAP
-extern void init_addr_map(void);
-#endif
+
 extern unsigned int setup_ddr_tlbs(unsigned int memsize_in_meg);
 
 #define SET_TLB_ENTRY(_tlb, _epn, _rpn, _perms, _wimge, _ts, _esel, _sz, _iprot) \
diff --git a/lib_ppc/bat_rw.c b/lib_ppc/bat_rw.c
index a40b377..c48c240 100644
--- a/lib_ppc/bat_rw.c
+++ b/lib_ppc/bat_rw.c
@@ -27,14 +27,23 @@
 #include <asm/mmu.h>
 #include <asm/io.h>
 
+#ifdef CONFIG_ADDR_MAP
+#include <addr_map.h>
+#endif
+
+DECLARE_GLOBAL_DATA_PTR;
+
 int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 {
+	int batn = -1;
+
 	sync();
 
 	switch (bat) {
 	case DBAT0:
 		mtspr (DBAT0L, lower);
 		mtspr (DBAT0U, upper);
+		batn = 0;
 		break;
 	case IBAT0:
 		mtspr (IBAT0L, lower);
@@ -43,6 +52,7 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 	case DBAT1:
 		mtspr (DBAT1L, lower);
 		mtspr (DBAT1U, upper);
+		batn = 1;
 		break;
 	case IBAT1:
 		mtspr (IBAT1L, lower);
@@ -51,6 +61,7 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 	case DBAT2:
 		mtspr (DBAT2L, lower);
 		mtspr (DBAT2U, upper);
+		batn = 2;
 		break;
 	case IBAT2:
 		mtspr (IBAT2L, lower);
@@ -59,6 +70,7 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 	case DBAT3:
 		mtspr (DBAT3L, lower);
 		mtspr (DBAT3U, upper);
+		batn = 3;
 		break;
 	case IBAT3:
 		mtspr (IBAT3L, lower);
@@ -68,6 +80,7 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 	case DBAT4:
 		mtspr (DBAT4L, lower);
 		mtspr (DBAT4U, upper);
+		batn = 4;
 		break;
 	case IBAT4:
 		mtspr (IBAT4L, lower);
@@ -76,6 +89,7 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 	case DBAT5:
 		mtspr (DBAT5L, lower);
 		mtspr (DBAT5U, upper);
+		batn = 5;
 		break;
 	case IBAT5:
 		mtspr (IBAT5L, lower);
@@ -84,6 +98,7 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 	case DBAT6:
 		mtspr (DBAT6L, lower);
 		mtspr (DBAT6U, upper);
+		batn = 6;
 		break;
 	case IBAT6:
 		mtspr (IBAT6L, lower);
@@ -92,6 +107,7 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 	case DBAT7:
 		mtspr (DBAT7L, lower);
 		mtspr (DBAT7U, upper);
+		batn = 7;
 		break;
 	case IBAT7:
 		mtspr (IBAT7L, lower);
@@ -102,6 +118,18 @@ int write_bat (ppc_bat_t bat, unsigned long upper, unsigned long lower)
 		return (-1);
 	}
 
+#ifdef CONFIG_ADDR_MAP
+	if ((gd->flags & GD_FLG_RELOC) && (batn >= 0)) {
+		phys_size_t size;
+		if (!BATU_VALID(upper))
+			size = 0;
+		else
+			size = BATU_SIZE(upper);
+		addrmap_set_entry(BATU_VADDR(upper), BATL_PADDR(lower),
+				  size, batn);
+	}
+#endif
+
 	sync();
 	isync();
 
diff --git a/lib_ppc/board.c b/lib_ppc/board.c
index df1cf13..2262bb4 100644
--- a/lib_ppc/board.c
+++ b/lib_ppc/board.c
@@ -685,7 +685,7 @@ void board_init_r (gd_t *id, ulong dest_addr)
 	 */
 	trap_init (dest_addr);
 
-#if defined(CONFIG_ADDR_MAP) && defined(CONFIG_E500)
+#ifdef CONFIG_ADDR_MAP
 	init_addr_map();
 #endif
 
-- 
1.5.6.6