From 82fd1f8da9add2d74532cf78d224485f0042d00d Mon Sep 17 00:00:00 2001
From: Kumar Gala <galak@kernel.crashing.org>
Date: Thu, 19 Mar 2009 02:53:01 -0500
Subject: [PATCH] 85xx: Add support for e500mc cache stashing

The e500mc core supports the ability to stash into the L1 or L2 cache,
however we need to uniquely identify the caches with an id.

We use the following equation to set the various stash-ids:

32 + coreID*2 + 0(L1) or 1(L2)

The 0 (for L1) or 1 (for L2) matches the CT field used be various cache
control instructions.

Signed-off-by: Kumar Gala <galak@kernel.crashing.org>
---
 cpu/mpc85xx/cpu_init.c |  5 +++++
 cpu/mpc85xx/fdt.c      | 18 ++++++++++++++++++
 cpu/mpc85xx/release.S  | 13 +++++++++++++
 cpu/mpc85xx/start.S    |  8 +++++++-
 4 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 0041a60df9..16ce82c995 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 de2dcac816..5f9aeb10b6 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 433ff02544..55eca72869 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 6a865283d4..7e60e67c3a 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 */
-- 
2.39.5