From 94e9411b9dda182dd63d53ba6ea640c98b35db5f Mon Sep 17 00:00:00 2001
From: Kumar Gala <galak@kernel.crashing.org>
Date: Thu, 12 Nov 2009 10:26:16 -0600
Subject: [PATCH] ppc/85xx: Add tracking of TLB CAM usage

We need to track which TLB CAM entries are used to allow us to
"dynamically" allocate entries later in the code.  For example the SPD
DDR code today hard codes which TLB entries it uses.  We can now make
that pick entries that are free.

Signed-off-by: Kumar Gala <galak@kernel.crashing.org>
---
 cpu/mpc85xx/cpu_init.c        |  1 +
 cpu/mpc85xx/tlb.c             | 64 +++++++++++++++++++++++++++++++++++
 include/asm-ppc/config.h      |  7 ++++
 include/asm-ppc/global_data.h |  3 ++
 include/asm-ppc/mmu.h         |  2 ++
 5 files changed, 77 insertions(+)

diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 16ce82c995..e0126d331a 100644
--- a/cpu/mpc85xx/cpu_init.c
+++ b/cpu/mpc85xx/cpu_init.c
@@ -246,6 +246,7 @@ void cpu_init_f (void)
 #ifdef CONFIG_FSL_CORENET
 	corenet_tb_init();
 #endif
+	init_used_tlb_cams();
 }
 
 
diff --git a/cpu/mpc85xx/tlb.c b/cpu/mpc85xx/tlb.c
index ea5deb2971..234fdaa60a 100644
--- a/cpu/mpc85xx/tlb.c
+++ b/cpu/mpc85xx/tlb.c
@@ -56,12 +56,74 @@ void init_tlbs(void)
 }
 
 #ifndef CONFIG_NAND_SPL
+static inline void use_tlb_cam(u8 idx)
+{
+	int i = idx / 32;
+	int bit = idx % 32;
+
+	gd->used_tlb_cams[i] |= (1 << bit);
+}
+
+static inline void free_tlb_cam(u8 idx)
+{
+	int i = idx / 32;
+	int bit = idx % 32;
+
+	gd->used_tlb_cams[i] &= ~(1 << bit);
+}
+
+void init_used_tlb_cams(void)
+{
+	int i;
+	unsigned int num_cam = mfspr(SPRN_TLB1CFG) & 0xfff;
+
+	for (i = 0; i < ((CONFIG_SYS_NUM_TLBCAMS+31)/32); i++)
+		gd->used_tlb_cams[i] = 0;
+
+	/* walk all the entries */
+	for (i = 0; i < num_cam; i++) {
+		u32 _mas1;
+
+		mtspr(MAS0, FSL_BOOKE_MAS0(1, i, 0));
+
+		asm volatile("tlbre;isync");
+		_mas1 = mfspr(MAS1);
+
+		/* if the entry isn't valid skip it */
+		if ((_mas1 & MAS1_VALID))
+			use_tlb_cam(i);
+	}
+}
+
+int find_free_tlbcam(void)
+{
+	int i;
+	u32 idx;
+
+	for (i = 0; i < ((CONFIG_SYS_NUM_TLBCAMS+31)/32); i++) {
+		idx = ffz(gd->used_tlb_cams[i]);
+
+		if (idx != 32)
+			break;
+	}
+
+	idx += i * 32;
+
+	if (idx >= CONFIG_SYS_NUM_TLBCAMS)
+		return -1;
+
+	return idx;
+}
+
 void set_tlb(u8 tlb, u32 epn, u64 rpn,
 	     u8 perms, u8 wimge,
 	     u8 ts, u8 esel, u8 tsize, u8 iprot)
 {
 	u32 _mas0, _mas1, _mas2, _mas3, _mas7;
 
+	if (tlb == 1)
+		use_tlb_cam(esel);
+
 	_mas0 = FSL_BOOKE_MAS0(tlb, esel, 0);
 	_mas1 = FSL_BOOKE_MAS1(1, iprot, 0, ts, tsize);
 	_mas2 = FSL_BOOKE_MAS2(epn, wimge);
@@ -80,6 +142,8 @@ void disable_tlb(u8 esel)
 {
 	u32 _mas0, _mas1, _mas2, _mas3, _mas7;
 
+	free_tlb_cam(esel);
+
 	_mas0 = FSL_BOOKE_MAS0(1, esel, 0);
 	_mas1 = 0;
 	_mas2 = 0;
diff --git a/include/asm-ppc/config.h b/include/asm-ppc/config.h
index d5f82b44f3..796707eaf7 100644
--- a/include/asm-ppc/config.h
+++ b/include/asm-ppc/config.h
@@ -63,6 +63,13 @@
 #define CONFIG_TSECV2
 #endif
 
+/* Number of TLB CAM entries we have on FSL Book-E chips */
+#if defined(CONFIG_E500MC)
+#define CONFIG_SYS_NUM_TLBCAMS	64
+#elif defined(CONFIG_E500)
+#define CONFIG_SYS_NUM_TLBCAMS	16
+#endif
+
 /* Relocation to SDRAM works on all PPC boards */
 #define CONFIG_RELOC_FIXUP_WORKS
 
diff --git a/include/asm-ppc/global_data.h b/include/asm-ppc/global_data.h
index 55e7e2066d..3f11918746 100644
--- a/include/asm-ppc/global_data.h
+++ b/include/asm-ppc/global_data.h
@@ -107,6 +107,9 @@ typedef	struct	global_data {
 #if defined(CONFIG_FSL_LAW)
 	u32 used_laws;
 #endif
+#if defined(CONFIG_E500)
+	u32 used_tlb_cams[(CONFIG_SYS_NUM_TLBCAMS+31)/32];
+#endif
 #if defined(CONFIG_MPC5xxx)
 	unsigned long	ipb_clk;
 	unsigned long	pci_clk;
diff --git a/include/asm-ppc/mmu.h b/include/asm-ppc/mmu.h
index ec22a5058e..fd1024947d 100644
--- a/include/asm-ppc/mmu.h
+++ b/include/asm-ppc/mmu.h
@@ -479,6 +479,8 @@ extern void disable_tlb(u8 esel);
 extern void invalidate_tlb(u8 tlb);
 extern void init_tlbs(void);
 extern int find_tlb_idx(void *addr, u8 tlbsel);
+extern void init_used_tlb_cams(void);
+extern int find_free_tlbcam(void);
 
 extern unsigned int setup_ddr_tlbs(unsigned int memsize_in_meg);