[PATCH] bsp/leon3: Add and use cache register functions

[Sebastian Huber]

---
 c/src/lib/libbsp/sparc/leon3/include/leon.h     |   42 +++++++++++++++++++++++
 c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c    |   16 +-------
 c/src/lib/libbsp/sparc/leon3/startup/bspstart.c |    9 +----
 3 files changed, 46 insertions(+), 21 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index a2c9bc6..476ed5c 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -357,6 +357,48 @@ static inline uint32_t leon3_get_cpu_count(
   return ((mpstat >> LEON3_IRQMPSTATUS_CPUNR) & 0xf)  + 1;
 }
 
+static inline void leon3_set_system_register(uint32_t addr, uint32_t val)
+{
+  __asm__ volatile(
+    "sta %1, [%0] 2"
+    :
+    : "r" (addr), "r" (val)
+  );
+}
+
+static inline uint32_t leon3_get_system_register(uint32_t addr)
+{
+  uint32_t val;
+
+  __asm__ volatile(
+    "lda [%1] 2, %0"
+    : "=r" (val)
+    : "r" (addr)
+  );
+
+  return val;
+}
+
+static inline void leon3_set_cache_control_register(uint32_t val)
+{
+  leon3_set_system_register(0x0, val);
+}
+
+static inline uint32_t leon3_get_cache_control_register(void)
+{
+  return leon3_get_system_register(0x0);
+}
+
+static inline uint32_t leon3_get_inst_cache_config_register(void)
+{
+  return leon3_get_system_register(0x8);
+}
+
+static inline uint32_t leon3_get_data_cache_config_register(void)
+{
+  return leon3_get_system_register(0xc);
+}
+
 #endif /* !ASM */
 
 #ifdef __cplusplus
diff --git a/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
index 426dc46..2441f52 100644
--- a/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
+++ b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
@@ -19,18 +19,6 @@
 #include <rtems/score/smpimpl.h>
 #include <stdlib.h>
 
-static inline void sparc_leon3_set_cctrl( unsigned int val )
-{
-  __asm__ volatile( "sta %0, [%%g0] 2" : : "r" (val) );
-}
-
-static inline unsigned int sparc_leon3_get_cctrl( void )
-{
-  unsigned int v = 0;
-  __asm__ volatile( "lda [%%g0] 2, %0" : "=r" (v) : "0" (v) );
-  return v;
-}
-
 static rtems_isr bsp_inter_processor_interrupt(
   rtems_vector_number vector
 )
@@ -40,7 +28,7 @@ static rtems_isr bsp_inter_processor_interrupt(
 
 void leon3_secondary_cpu_initialize(uint32_t cpu)
 {
-  sparc_leon3_set_cctrl( 0x80000F );
+  leon3_set_cache_control_register(0x80000F);
   /* Unmask IPI interrupts at Interrupt controller for this CPU */
   LEON3_IrqCtrl_Regs->mask[cpu] |= 1 << LEON3_MP_IRQ;
 
@@ -53,7 +41,7 @@ uint32_t _CPU_SMP_Initialize( uint32_t configured_cpu_count )
   uint32_t used_cpu_count;
   uint32_t cpu;
 
-  sparc_leon3_set_cctrl( 0x80000F );
+  leon3_set_cache_control_register(0x80000F);
 
   max_cpu_count = leon3_get_cpu_count(LEON3_IrqCtrl_Regs);
   used_cpu_count = configured_cpu_count < max_cpu_count ?
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
index 64ab4d8..5ecea6c 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
@@ -32,7 +32,7 @@ uint32_t LEON3_Cpu_Index = 0;
 /*
  * set_snooping
  *
- * Read the data cache configuration register to determine if
+ * Read the cache control register to determine if
  * bus snooping is available and enabled. This is needed for some
  * drivers so that they can select the most efficient copy routines.
  *
@@ -40,12 +40,7 @@ uint32_t LEON3_Cpu_Index = 0;
 
 static inline int set_snooping(void)
 {
-  int tmp;
-  __asm__ (" lda [%%g0] 2, %0 "
-      : "=r"(tmp)
-      :
-  );
-  return (tmp >> 23) & 1;
+  return (leon3_get_cache_control_register() >> 23) & 1;
 }
 
 /*
-- 
1.7.7