7 files changed, 73 insertions, 0 deletions
diff --git a/c/src/lib/libbsp/sparc/erc32/Makefile.am b/c/src/lib/libbsp/sparc/erc32/Makefile.am
index 677c44e2b7..08c6a8b0e6 100644
--- a/c/src/lib/libbsp/sparc/erc32/Makefile.am
+++ b/c/src/lib/libbsp/sparc/erc32/Makefile.am
@@ -35,6 +35,7 @@ libbsp_a_SOURCES += ../../shared/bspclean.c ../../shared/bsplibc.c \
     ../../shared/sbrk.c startup/setvec.c startup/spurious.c \
     startup/erc32mec.c startup/boardinit.S startup/bspidle.c \
     startup/bspdelay.c ../../sparc/shared/startup/early_malloc.c
+libbsp_a_SOURCES += ../../shared/cpucounterread.c
 # ISR Handler
 libbsp_a_SOURCES += ../../sparc/shared/irq_asm.S
 # gnatsupp
diff --git a/c/src/lib/libbsp/sparc/leon2/Makefile.am b/c/src/lib/libbsp/sparc/leon2/Makefile.am
index c08f159f81..8bbe69b201 100644
--- a/c/src/lib/libbsp/sparc/leon2/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon2/Makefile.am
@@ -52,6 +52,7 @@ libbsp_a_SOURCES += ../../shared/bspclean.c ../../shared/bsplibc.c \
     ../../shared/sbrk.c startup/setvec.c startup/spurious.c startup/bspidle.c \
     ../../shared/bspinit.c startup/bspdelay.c \
     ../../sparc/shared/startup/early_malloc.c
+libbsp_a_SOURCES += ../../shared/cpucounterread.c
 # ISR Handler
 libbsp_a_SOURCES += ../../sparc/shared/irq_asm.S
 # gnatsupp
diff --git a/c/src/lib/libbsp/sparc/leon3/Makefile.am b/c/src/lib/libbsp/sparc/leon3/Makefile.am
index 1a24cbfd4b..b543f17e75 100644
--- a/c/src/lib/libbsp/sparc/leon3/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon3/Makefile.am
@@ -38,6 +38,7 @@ libbsp_a_SOURCES += ../../shared/bspclean.c ../../shared/bsplibc.c \
     ../../sparc/shared/startup/bspgetworkarea.c ../../shared/sbrk.c startup/setvec.c \
     startup/spurious.c startup/bspidle.S startup/bspdelay.c \
     ../../shared/bspinit.c ../../sparc/shared/startup/early_malloc.c
+libbsp_a_SOURCES += startup/cpucounter.c
 
 # ISR Handler
 libbsp_a_SOURCES += ../../sparc/shared/irq_asm.S
diff --git a/c/src/lib/libbsp/sparc/leon3/include/bsp.h b/c/src/lib/libbsp/sparc/leon3/include/bsp.h
index b0a17305af..18ae87f239 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/bsp.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/bsp.h
@@ -209,6 +209,10 @@ extern void BSP_shared_interrupt_unmask(int irq);
  */
 extern void BSP_shared_interrupt_mask(int irq);
 
+typedef enum {
+  LEON3_FATAL_CPU_COUNTER_INIT
+} leon3_fatal_code;
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index deff75ddd5..84567e46cd 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -317,6 +317,8 @@ int apbuart_inbyte_nonblocking(struct apbuart_regs *regs);
  */
 void leon3_secondary_cpu_initialize(uint32_t cpu);
 
+void leon3_cpu_counter_initialize(void);
+
 void bsp_debug_uart_init(void);
 
 #endif /* !ASM */
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
index 1e8d12c7a1..4b9450d2df 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
@@ -79,6 +79,7 @@ void bsp_start( void )
    * interrupt support
    */
   amba_initialize();
+  leon3_cpu_counter_initialize();
 
   /* find debug UART for printk() */
   bsp_debug_uart_init();
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
new file mode 100644
index 0000000000..2863f359e7
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/leon3/startup/cpucounter.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <leon.h>
+
+#include <rtems/counter.h>
+
+static volatile struct gptimer_regs *leon3_cpu_counter_gpt;
+
+void leon3_cpu_counter_initialize(void)
+{
+  struct ambapp_dev *adev;
+  int idx = 1;
+  volatile struct gptimer_regs *gpt;
+  unsigned new_prescaler = 8;
+  unsigned prescaler;
+  uint32_t frequency;
+
+  adev = (void *) ambapp_for_each(
+    &ambapp_plb,
+    OPTIONS_ALL | OPTIONS_APB_SLVS,
+    VENDOR_GAISLER,
+    GAISLER_GPTIMER,
+    ambapp_find_by_idx,
+    &idx
+  );
+  if (adev == NULL) {
+    rtems_fatal(RTEMS_FATAL_SOURCE_BSP_SPECIFIC, LEON3_FATAL_CPU_COUNTER_INIT);
+  }
+
+  gpt = (volatile struct gptimer_regs *) DEV_TO_APB(adev)->start;
+
+  prescaler = gpt->scaler_reload + 1;
+  gpt->scaler_reload = new_prescaler - 1;
+  gpt->timer[0].reload = 0xffffffff;
+  gpt->timer[0].ctrl = LEON3_GPTIMER_EN | LEON3_GPTIMER_RL
+    | LEON3_GPTIMER_LD;
+
+  leon3_cpu_counter_gpt = gpt;
+
+  /* Assume that GRMON initialized the timer to 1MHz */
+  frequency = UINT32_C(1000000) * (prescaler / new_prescaler);
+  rtems_counter_initialize_converter(frequency);
+}
+
+CPU_Counter_ticks _CPU_Counter_read(void)
+{
+  volatile struct gptimer_regs *gpt = leon3_cpu_counter_gpt;
+
+  return 0U - gpt->timer[0].value;
+}