diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 16ce82c995c9210c32342f029472924e14c6df98..e0126d331af8f93ba1801401728a72fa7bcd2e87 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 ea5deb2971fcdf158e9bab50d976184d0f10d5ec..234fdaa60aa3d0ebbeb2677c46189deb32321022 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 d5f82b44f3890ec03562a2ce62030a4e8f5771f5..796707eaf749592618c7d3d4fd344b97d2689274 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 55e7e2066d8e69cce2b082ab71cc50b48c8586e8..3f119187464fc0696596c9022cfd46c0d81654ea 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 ec22a5058e1c0b2b829284246c8cf7bcec374b76..fd1024947d94e58e3725f4505198a142fd357e3f 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);