diff --git a/cpu/mpc85xx/tlb.c b/cpu/mpc85xx/tlb.c
index f3d3080c44eb09e2555707d76496a473f1e7d8bd..0497a29ba83f78e91385d03f8f3650a35bbf7764 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)
 {
diff --git a/include/asm-ppc/mmu.h b/include/asm-ppc/mmu.h
index fa92b90c3378008cff199f2180031e0a7492961b..eda2959b7d0a42eddeb36d13f577af252eb83a86 100644
--- a/include/asm-ppc/mmu.h
+++ b/include/asm-ppc/mmu.h
@@ -387,8 +387,10 @@ extern void print_bats(void);
  * FSL Book-E support
  */
 
-#define MAS0_TLBSEL(x)	((x << 28) & 0x30000000)
-#define MAS0_ESEL(x)	((x << 16) & 0x0FFF0000)
+#define MAS0_TLBSEL_MSK	0x30000000
+#define MAS0_TLBSEL(x)	((x << 28) & MAS0_TLBSEL_MSK)
+#define MAS0_ESEL_MSK	0x0FFF0000
+#define MAS0_ESEL(x)	((x << 16) & MAS0_ESEL_MSK)
 #define MAS0_NV(x)	((x) & 0x00000FFF)
 
 #define MAS1_VALID	0x80000000
@@ -474,6 +476,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);
+extern int find_tlb_idx(void *addr, u8 tlbsel);
 
 extern unsigned int setup_ddr_tlbs(unsigned int memsize_in_meg);