4 files changed, 43 insertions, 1 deletions
diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 0041a60..16ce82c 100644
--- a/cpu/mpc85xx/cpu_init.c
+++ b/cpu/mpc85xx/cpu_init.c
@@ -357,6 +357,11 @@ int cpu_init_r(void)
 	while (mfspr(SPRN_L2CSR0) & (L2CSR0_L2FI|L2CSR0_L2LFC))
 		;
 
+#ifdef CONFIG_SYS_CACHE_STASHING
+	/* set stash id to (coreID) * 2 + 32 + L2 (1) */
+	mtspr(SPRN_L2CSR1, (32 + 1));
+#endif
+
 	/* enable the cache */
 	mtspr(SPRN_L2CSR0, CONFIG_SYS_INIT_L2CSR0);
 
diff --git a/cpu/mpc85xx/fdt.c b/cpu/mpc85xx/fdt.c
index de2dcac..5f9aeb1 100644
--- a/cpu/mpc85xx/fdt.c
+++ b/cpu/mpc85xx/fdt.c
@@ -197,6 +197,15 @@ static inline void ft_fixup_l2cache(void *blob)
 			goto next;
 		}
 
+#ifdef CONFIG_SYS_CACHE_STASHING
+		{
+			u32 *reg = (u32 *)fdt_getprop(blob, off, "reg", 0);
+			if (reg)
+				fdt_setprop_cell(blob, l2_off, "cache-stash-id",
+					 (*reg * 2) + 32 + 1);
+		}
+#endif
+
 		fdt_setprop(blob, l2_off, "cache-unified", NULL, 0);
 		fdt_setprop_cell(blob, l2_off, "cache-block-size", line_size);
 		fdt_setprop_cell(blob, l2_off, "cache-size", size);
@@ -252,6 +261,15 @@ static inline void ft_fixup_cache(void *blob)
 		fdt_setprop_cell(blob, off, "d-cache-size", dsize);
 		fdt_setprop_cell(blob, off, "d-cache-sets", dnum_sets);
 
+#ifdef CONFIG_SYS_CACHE_STASHING
+		{
+			u32 *reg = (u32 *)fdt_getprop(blob, off, "reg", 0);
+			if (reg)
+				fdt_setprop_cell(blob, off, "cache-stash-id",
+					 (*reg * 2) + 32 + 0);
+		}
+#endif
+
 		/* i-side config */
 		isize = (l1cfg1 & 0x7ff) * 1024;
 		inum_ways = ((l1cfg1 >> 11) & 0xff) + 1;
diff --git a/cpu/mpc85xx/release.S b/cpu/mpc85xx/release.S
index 433ff02..55eca72 100644
--- a/cpu/mpc85xx/release.S
+++ b/cpu/mpc85xx/release.S
@@ -99,6 +99,13 @@ __secondary_start_page:
 	slwi	r8,r4,5
 	add	r10,r3,r8
 
+#if defined(CONFIG_E500MC) && defined(CONFIG_SYS_CACHE_STASHING)
+	/* set stash id to (coreID) * 2 + 32 + L1 CT (0) */
+	slwi	r8,r4,1
+	addi	r8,r8,32
+	mtspr	L1CSR2,r8
+#endif
+
 #ifdef CONFIG_BACKSIDE_L2_CACHE
 	/* Enable/invalidate the L2 cache */
 	msync
@@ -110,6 +117,12 @@ __secondary_start_page:
 	and.	r1,r3,r2
 	bne	1b
 
+#ifdef CONFIG_SYS_CACHE_STASHING
+	/* set stash id to (coreID) * 2 + 32 + L2 (1) */
+	addi	r3,r8,1
+	mtspr	SPRN_L2CSR1,r3
+#endif
+
 	lis	r3,CONFIG_SYS_INIT_L2CSR0@h
 	ori	r3,r3,CONFIG_SYS_INIT_L2CSR0@l
 	mtspr	SPRN_L2CSR0,r3
diff --git a/cpu/mpc85xx/start.S b/cpu/mpc85xx/start.S
index 6a86528..7e60e67 100644
--- a/cpu/mpc85xx/start.S
+++ b/cpu/mpc85xx/start.S
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004, 2007-2009 Freescale Semiconductor.
+ * Copyright 2004, 2007-2009 Freescale Semiconductor, Inc.
  * Copyright (C) 2003  Motorola,Inc.
  *
  * See file CREDITS for list of people who contributed to this
@@ -102,6 +102,12 @@ _start_e500:
 	 *
 	 */
 
+#if defined(CONFIG_E500MC) && defined(CONFIG_SYS_CACHE_STASHING)
+	/* set stash id to (coreID) * 2 + 32 + L1 CT (0) */
+	li	r2,(32 + 0)
+	mtspr	L1CSR2,r2
+#endif
+
 	lis	r2,L1CSR0_CPE@H	/* enable parity */
 	ori	r2,r2,L1CSR0_DCE
 	mtspr	L1CSR0,r2	/* enable L1 Dcache */