ppc/85xx: Add a simple function to search the TLB

Allow us to search the TLB array based on an address.  This is useful
if we want to change an entry but dont know where it happens to be
located.

For example, the boot page mapping we use on MP or the flash TLB that
we change the WIMGE settings for after we've relocated.

Signed-off-by: Kumar Gala <galak@kernel.crashing.org>

1 files changed, 27 insertions, 0 deletions

diff --git a/cpu/mpc85xx/tlb.c b/cpu/mpc85xx/tlb.c
index f3d3080c44..0497a29ba8 100644
--- a/cpu/mpc85xx/tlb.c
+++ b/cpu/mpc85xx/tlb.c
@@ -106,6 +106,33 @@ void init_tlbs(void)
 	return ;
 }
 
+static void tlbsx (const volatile unsigned *addr)
+{
+	__asm__ __volatile__ ("tlbsx 0,%0" : : "r" (addr), "m" (*addr));
+}
+
+/* return -1 if we didn't find anything */
+int find_tlb_idx(void *addr, u8 tlbsel)
+{
+	u32 _mas0, _mas1;
+
+	/* zero out Search PID, AS */
+	mtspr(MAS6, 0);
+
+	tlbsx(addr);
+
+	_mas0 = mfspr(MAS0);
+	_mas1 = mfspr(MAS1);
+
+	/* we found something, and its in the TLB we expect */
+	if ((MAS1_VALID & _mas1) &&
+		(MAS0_TLBSEL(tlbsel) == (_mas0 & MAS0_TLBSEL_MSK))) {
+		return ((_mas0 & MAS0_ESEL_MSK) >> 16);
+	}
+
+	return -1;
+}
+
 #ifdef CONFIG_ADDR_MAP
 void init_addr_map(void)
 {