[PATCH] arm: Check FEAT_CCIDX when parsing ccsidr_el1 register

[Lukasz Wiecaszek]

Current Cache Size ID Register (ccsidr_el1) has two "flavors"
depending on whether FEAT_CCIDX is implemented or not.
When FEAT_CCIDX is implemented Associativity parameter
is coded on bits [23:3] and NumSets parameter on bits [55:32].
When FEAT_CCIDX is not implemented then Associativity parameter
is coded on bits [12:3] and NumSets parameter on bits [27:13].
Current U-Boot code does not check whether FEAT_CCIDX is implemented
and always parses ccsidr_el1 as if FEAT_CCIDX was not implemented.
This is of course wrong on systems where FEAT_CCIDX is implemented.
This patch fixes that problems and tests whether FEAT_CCIDX
is implemented or not and accordingly parses the ccsidr_el1 register.

Signed-off-by: Lukasz Wiecaszek <lukasz.wiecaszek at gmail.com>
---
 arch/arm/cpu/armv8/cache.S | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/arch/arm/cpu/armv8/cache.S b/arch/arm/cpu/armv8/cache.S
index 3fe935cf28..c9e46859b4 100644
--- a/arch/arm/cpu/armv8/cache.S
+++ b/arch/arm/cpu/armv8/cache.S
@@ -20,6 +20,7 @@
  *
  * x0: cache level
  * x1: 0 clean & invalidate, 1 invalidate only
+ * x16: FEAT_CCIDX
  * x2~x9: clobbered
  */
 .pushsection .text.__asm_dcache_level, "ax"
@@ -29,8 +30,14 @@ ENTRY(__asm_dcache_level)
 	isb				/* sync change of cssidr_el1 */
 	mrs	x6, ccsidr_el1		/* read the new cssidr_el1 */
 	ubfx	x2, x6,  #0,  #3	/* x2 <- log2(cache line size)-4 */
+	cbz	x16, 3f			/* check for FEAT_CCIDX */
+	ubfx	x3, x6,  #3, #21	/* x3 <- number of cache ways - 1 */
+	ubfx	x4, x6, #32, #24	/* x4 <- number of cache sets - 1 */
+	b	4f
+3:
 	ubfx	x3, x6,  #3, #10	/* x3 <- number of cache ways - 1 */
 	ubfx	x4, x6, #13, #15	/* x4 <- number of cache sets - 1 */
+4:
 	add	x2, x2, #4		/* x2 <- log2(cache line size) */
 	clz	w5, w3			/* bit position of #ways */
 	/* x12 <- cache level << 1 */
@@ -74,6 +81,8 @@ ENTRY(__asm_dcache_all)
 	ubfx	x11, x10, #24, #3	/* x11 <- loc */
 	cbz	x11, finished		/* if loc is 0, exit */
 	mov	x15, lr
+	mrs	x16, s3_0_c0_c7_2	/* read value of id_aa64mmfr2_el1*/
+	ubfx	x16, x16, #20, #4	/* save FEAT_CCIDX identifier in x16 */
 	mov	x0, #0			/* start flush at cache level 0 */
 	/* x0  <- cache level */
 	/* x10 <- clidr_el1 */
-- 
2.44.0