armv7m/v8m:Restore the CSSELR state before setting.

crafcat7 · xiaoxiang781216 · commit 9c2bc0edbea1 · 2023-07-15T15:11:07.000+08:00
Signed-off-by: chenrun1 &lt;chenrun1@xiaomi.com&gt;
diff --git a/arch/arm/src/armv7-a/cp15_cacheops.c b/arch/arm/src/armv7-a/cp15_cacheops.c
@@ -52,9 +52,7 @@ static inline uint32_t cp15_cache_get_info(uint32_t *sets, uint32_t *ways,
 
   csselr = CP15_GET(CSSELR);
 
-  csselr = (csselr & ~0x01) | (icache & 0x01);
-
-  CP15_SET(CSSELR, csselr);
+  CP15_SET(CSSELR, (csselr & ~0x01) | (icache & 0x01));
 
   ccsidr = CP15_GET(CCSIDR);
 
@@ -68,6 +66,8 @@ static inline uint32_t cp15_cache_get_info(uint32_t *sets, uint32_t *ways,
       *ways = ((ccsidr >> 3) & 0x3ff) + 1;
     }
 
+  CP15_SET(CSSELR, csselr);   /* restore csselr */
+
   return (1 << ((ccsidr & 0x7) + 2)) * 4;
 }
 
diff --git a/arch/arm/src/armv7-m/arm_cache.c b/arch/arm/src/armv7-m/arm_cache.c
@@ -129,17 +129,20 @@ static size_t up_get_cache_linesize(bool icache)
 
   if (icache)
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_ICACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_ICACHE, NVIC_CSSELR);
     }
   else
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_DCACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_DCACHE, NVIC_CSSELR);
     }
 
-  putreg32(csselr, NVIC_CSSELR);
   ccsidr = getreg32(NVIC_CCSIDR);
   sshift = CCSIDR_LSSHIFT(ccsidr) + 4;   /* log2(cache-line-size-in-bytes) */
 
+  putreg32(csselr, NVIC_CSSELR);    /* restore csselr */
+
   return 1 << sshift;
 }
 
@@ -170,11 +173,13 @@ static size_t up_get_cache_size(bool icache)
 
   if (icache)
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_ICACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_ICACHE, NVIC_CSSELR);
     }
   else
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_DCACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_DCACHE, NVIC_CSSELR);
     }
 
   ccsidr = getreg32(NVIC_CCSIDR);
@@ -183,6 +188,8 @@ static size_t up_get_cache_size(bool icache)
   sshift = CCSIDR_LSSHIFT(ccsidr) + 4;   /* log2(cache-line-size-in-bytes) */
   line   = 1 << sshift;
 
+  putreg32(csselr, NVIC_CSSELR);    /* restore csselr */
+
   return sets * ways * line;
 }
 #endif
diff --git a/arch/arm/src/armv7-r/cp15_cacheops.c b/arch/arm/src/armv7-r/cp15_cacheops.c
@@ -52,9 +52,7 @@ static inline uint32_t cp15_cache_get_info(uint32_t *sets, uint32_t *ways,
 
   csselr = CP15_GET(CSSELR);
 
-  csselr = (csselr & ~0x01) | (icache & 0x01);
-
-  CP15_SET(CSSELR, csselr);
+  CP15_SET(CSSELR, (csselr & ~0x01) | (icache & 0x01));
 
   ccsidr = CP15_GET(CCSIDR);
 
@@ -68,6 +66,8 @@ static inline uint32_t cp15_cache_get_info(uint32_t *sets, uint32_t *ways,
       *ways = ((ccsidr >> 3) & 0x3ff) + 1;
     }
 
+  CP15_SET(CSSELR, csselr);   /* restore csselr */
+
   return (1 << ((ccsidr & 0x7) + 2)) * 4;
 }
 
diff --git a/arch/arm/src/armv8-m/arm_cache.c b/arch/arm/src/armv8-m/arm_cache.c
@@ -129,17 +129,20 @@ static size_t up_get_cache_linesize(bool icache)
 
   if (icache)
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_ICACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_ICACHE, NVIC_CSSELR);
     }
   else
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_DCACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_DCACHE, NVIC_CSSELR);
     }
 
-  putreg32(csselr, NVIC_CSSELR);
   ccsidr = getreg32(NVIC_CCSIDR);
   sshift = CCSIDR_LSSHIFT(ccsidr) + 4;   /* log2(cache-line-size-in-bytes) */
 
+  putreg32(csselr, NVIC_CSSELR);    /* restore csselr */
+
   return 1 << sshift;
 }
 
@@ -170,11 +173,13 @@ static size_t up_get_cache_size(bool icache)
 
   if (icache)
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_ICACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_ICACHE, NVIC_CSSELR);
     }
   else
     {
-      csselr = (csselr & ~NVIC_CSSELR_IND) | NVIC_CSSELR_IND_DCACHE;
+      putreg32((csselr & ~NVIC_CSSELR_IND) |
+                NVIC_CSSELR_IND_DCACHE, NVIC_CSSELR);
     }
 
   ccsidr = getreg32(NVIC_CCSIDR);
@@ -183,6 +188,8 @@ static size_t up_get_cache_size(bool icache)
   sshift = CCSIDR_LSSHIFT(ccsidr) + 4;   /* log2(cache-line-size-in-bytes) */
   line   = 1 << sshift;
 
+  putreg32(csselr, NVIC_CSSELR);    /* restore csselr */
+
   return sets * ways * line;
 }
 #endif

Original file line number	Diff line number	Diff line change
`@@ -52,9 +52,7 @@ static inline uint32_t cp15_cache_get_info(uint32_t sets, uint32_t ways,`
`52`	`52`
`53`	`53`	`csselr = CP15_GET(CSSELR);`
`54`	`54`
`55`		`- csselr = (csselr & ~0x01) \| (icache & 0x01);`
`56`		`-`
`57`		`- CP15_SET(CSSELR, csselr);`
	`55`	`+ CP15_SET(CSSELR, (csselr & ~0x01) \| (icache & 0x01));`
`58`	`56`
`59`	`57`	`ccsidr = CP15_GET(CCSIDR);`
`60`	`58`
`@@ -68,6 +66,8 @@ static inline uint32_t cp15_cache_get_info(uint32_t sets, uint32_t ways,`
`68`	`66`	`*ways = ((ccsidr >> 3) & 0x3ff) + 1;`
`69`	`67`	`}`
`70`	`68`
	`69`	`+ CP15_SET(CSSELR, csselr); /* restore csselr */`
	`70`	`+`
`71`	`71`	`return (1 << ((ccsidr & 0x7) + 2)) * 4;`
`72`	`72`	`}`
`73`	`73`
Original file line number	Diff line number	Diff line change
`@@ -129,17 +129,20 @@ static size_t up_get_cache_linesize(bool icache)`
`129`	`129`
`130`	`130`	`if (icache)`
`131`	`131`	`{`
`132`		`- csselr = (csselr & ~NVIC_CSSELR_IND) \| NVIC_CSSELR_IND_ICACHE;`
	`132`	`+ putreg32((csselr & ~NVIC_CSSELR_IND) \|`
	`133`	`+ NVIC_CSSELR_IND_ICACHE, NVIC_CSSELR);`
`133`	`134`	`}`
`134`	`135`	`else`
`135`	`136`	`{`
`136`		`- csselr = (csselr & ~NVIC_CSSELR_IND) \| NVIC_CSSELR_IND_DCACHE;`
	`137`	`+ putreg32((csselr & ~NVIC_CSSELR_IND) \|`
	`138`	`+ NVIC_CSSELR_IND_DCACHE, NVIC_CSSELR);`
`137`	`139`	`}`
`138`	`140`
`139`		`- putreg32(csselr, NVIC_CSSELR);`
`140`	`141`	`ccsidr = getreg32(NVIC_CCSIDR);`
`141`	`142`	`sshift = CCSIDR_LSSHIFT(ccsidr) + 4; /* log2(cache-line-size-in-bytes) */`
`142`	`143`
	`144`	`+ putreg32(csselr, NVIC_CSSELR); /* restore csselr */`
	`145`	`+`
`143`	`146`	`return 1 << sshift;`
`144`	`147`	`}`
`145`	`148`
`@@ -170,11 +173,13 @@ static size_t up_get_cache_size(bool icache)`
`170`	`173`
`171`	`174`	`if (icache)`
`172`	`175`	`{`
`173`		`- csselr = (csselr & ~NVIC_CSSELR_IND) \| NVIC_CSSELR_IND_ICACHE;`
	`176`	`+ putreg32((csselr & ~NVIC_CSSELR_IND) \|`
	`177`	`+ NVIC_CSSELR_IND_ICACHE, NVIC_CSSELR);`
`174`	`178`	`}`
`175`	`179`	`else`
`176`	`180`	`{`
`177`		`- csselr = (csselr & ~NVIC_CSSELR_IND) \| NVIC_CSSELR_IND_DCACHE;`
	`181`	`+ putreg32((csselr & ~NVIC_CSSELR_IND) \|`
	`182`	`+ NVIC_CSSELR_IND_DCACHE, NVIC_CSSELR);`
`178`	`183`	`}`
`179`	`184`
`180`	`185`	`ccsidr = getreg32(NVIC_CCSIDR);`
`@@ -183,6 +188,8 @@ static size_t up_get_cache_size(bool icache)`
`183`	`188`	`sshift = CCSIDR_LSSHIFT(ccsidr) + 4; /* log2(cache-line-size-in-bytes) */`
`184`	`189`	`line = 1 << sshift;`
`185`	`190`
	`191`	`+ putreg32(csselr, NVIC_CSSELR); /* restore csselr */`
	`192`	`+`
`186`	`193`	`return sets * ways * line;`
`187`	`194`	`}`
`188`	`195`	`#endif`