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>

2 files changed, 65 insertions, 0 deletions

diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 16ce82c..e0126d3 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 ea5deb2..234fdaa 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;