bsp/leon3: Add LEON3_IRQAMP_PROBE_TIMESTAMP

4 files changed, 209 insertions, 118 deletions

diff --git a/bsps/sparc/leon3/clock/ckinit.c b/bsps/sparc/leon3/clock/ckinit.c
index 66a79a6a42..13e14e33d6 100644
--- a/bsps/sparc/leon3/clock/ckinit.c
+++ b/bsps/sparc/leon3/clock/ckinit.c
@@ -64,11 +64,29 @@
 /* LEON3 Timer system interrupt number */
 static int clkirq;
 
-static void (*leon3_tc_tick)(void);
-
 static struct timecounter leon3_tc;
 
-#ifdef RTEMS_PROFILING
+static void leon3_tc_tick_default(void)
+{
+#if !defined(RTEMS_SMP)
+  SPARC_Counter *counter;
+  rtems_interrupt_level level;
+
+  counter = &_SPARC_Counter_mutable;
+  rtems_interrupt_local_disable(level);
+
+  grlib_store_32(&LEON3_IrqCtrl_Regs->iclear, counter->pending_mask);
+  counter->accumulated += counter->interval;
+
+  rtems_interrupt_local_enable(level);
+#endif
+
+  rtems_timecounter_tick();
+}
+
+#if defined(RTEMS_PROFILING)
+static void (*leon3_tc_tick)(void) = leon3_tc_tick_default;
+
 #define IRQMP_TIMESTAMP_S1_S2 ((1U << 25) | (1U << 26))
 
 static void leon3_tc_tick_irqmp_timestamp(void)
@@ -86,11 +104,9 @@ static void leon3_tc_tick_irqmp_timestamp(void)
 
   rtems_timecounter_tick();
 }
-#endif
 
 static void leon3_tc_tick_irqmp_timestamp_init(void)
 {
-#ifdef RTEMS_PROFILING
   /*
    * Ignore the first clock interrupt, since it contains the sequential system
    * initialization time.  Do the timestamp initialization on the fly.
@@ -117,32 +133,18 @@ static void leon3_tc_tick_irqmp_timestamp_init(void)
   if (done) {
     leon3_tc_tick = leon3_tc_tick_irqmp_timestamp;
   }
-#endif
-
-  rtems_timecounter_tick();
-}
-
-static void leon3_tc_tick_default(void)
-{
-#if !defined(RTEMS_SMP)
-  SPARC_Counter *counter;
-  rtems_interrupt_level level;
-
-  counter = &_SPARC_Counter_mutable;
-  rtems_interrupt_local_disable(level);
-
-  grlib_store_32(&LEON3_IrqCtrl_Regs->iclear, counter->pending_mask);
-  counter->accumulated += counter->interval;
-
-  rtems_interrupt_local_enable(level);
-#endif
 
   rtems_timecounter_tick();
 }
+#endif /* RTEMS_PROFILING */
 
 static void leon3_tc_do_tick(void)
 {
+#if defined(RTEMS_PROFILING)
   (*leon3_tc_tick)();
+#else
+  leon3_tc_tick_default();
+#endif
 }
 
 #define Adjust_clkirq_for_node() do { clkirq += LEON3_CLOCK_INDEX; } while(0)
@@ -185,9 +187,87 @@ static void bsp_clock_handler_install(rtems_interrupt_handler isr)
 #define Clock_driver_support_set_interrupt_affinity(online_processors) \
   bsp_interrupt_set_affinity(clkirq, online_processors)
 
+static void leon3_clock_use_up_counter(struct timecounter *tc)
+{
+  tc->tc_get_timecount = _SPARC_Get_timecount_asr23;
+  tc->tc_frequency = leon3_up_counter_frequency();
+
+#if defined(RTEMS_PROFILING)
+  if (irqamp_get_timestamp_registers(LEON3_IrqCtrl_Regs) == NULL) {
+    bsp_fatal(LEON3_FATAL_CLOCK_NO_IRQMP_TIMESTAMP_SUPPORT);
+  }
+
+  leon3_tc_tick = leon3_tc_tick_irqmp_timestamp_init;
+#endif
+
+  rtems_timecounter_install(tc);
+}
+
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
+static void leon3_clock_use_irqamp_timestamp(
+  struct timecounter *tc,
+  irqamp_timestamp *irqmp_ts
+)
+{
+  tc->tc_get_timecount = _SPARC_Get_timecount_up;
+#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
+  tc->tc_frequency = leon3_processor_local_bus_frequency();
+#else
+  tc->tc_frequency = ambapp_freq_get(ambapp_plb(), LEON3_Timer_Adev);
+#endif
+
+#if defined(RTEMS_PROFILING)
+  leon3_tc_tick = leon3_tc_tick_irqmp_timestamp_init;
+#endif
+
+  /*
+   * At least one TSISEL field must be non-zero to enable the timestamp
+   * counter.  Use an arbitrary interrupt source.
+   */
+  grlib_store_32(&irqmp_ts->itstmpc, IRQAMP_ITSTMPC_TSISEL(1));
+
+  rtems_timecounter_install(tc);
+}
+#endif
+
+static void leon3_clock_use_gptimer(
+  struct timecounter *tc,
+  gptimer_timer *timer
+)
+{
+#ifdef RTEMS_SMP
+  /*
+   * The GR712RC for example has no timestamp unit in the interrupt
+   * controller.  At least on SMP configurations we must use a second timer
+   * in free running mode for the timecounter.  The timer is initialized by
+   * leon3_counter_initialize().
+   */
+  tc->tc_get_timecount = _SPARC_Get_timecount_down;
+#else
+  SPARC_Counter *counter;
+
+  counter = &_SPARC_Counter_mutable;
+  counter->read_isr_disabled = _SPARC_Counter_read_clock_isr_disabled;
+  counter->read = _SPARC_Counter_read_clock;
+  counter->counter_register = &timer->tcntval;
+  counter->pending_register = &LEON3_IrqCtrl_Regs->ipend;
+  counter->pending_mask = UINT32_C(1) << clkirq;
+  counter->accumulated = rtems_configuration_get_microseconds_per_tick();
+  counter->interval = rtems_configuration_get_microseconds_per_tick();
+
+  tc->tc_get_timecount = _SPARC_Get_timecount_clock;
+#endif
+
+  tc->tc_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER,
+
+  rtems_timecounter_install(tc);
+}
+
 static void leon3_clock_initialize(void)
 {
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
   irqamp_timestamp *irqmp_ts;
+#endif
   gptimer_timer *timer;
   struct timecounter *tc;
 
@@ -202,73 +282,29 @@ static void leon3_clock_initialize(void)
     GPTIMER_TCTRL_EN | GPTIMER_TCTRL_RS | GPTIMER_TCTRL_LD | GPTIMER_TCTRL_IE
   );
 
-  irqmp_ts = irqamp_get_timestamp_registers(LEON3_IrqCtrl_Regs);
   tc = &leon3_tc;
+  tc->tc_counter_mask = 0xffffffff;
+  tc->tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+
   leon3_up_counter_enable();
 
   if (leon3_up_counter_is_available()) {
     /* Use the LEON4 up-counter if available */
-    tc->tc_get_timecount = _SPARC_Get_timecount_asr23;
-    tc->tc_frequency = leon3_up_counter_frequency();
+    leon3_clock_use_up_counter(tc);
+    return;
+  }
 
-#ifdef RTEMS_PROFILING
-    if (irqmp_ts == NULL) {
-      bsp_fatal(LEON3_FATAL_CLOCK_NO_IRQMP_TIMESTAMP_SUPPORT);
-    }
-#endif
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
+  irqmp_ts = irqamp_get_timestamp_registers(LEON3_IrqCtrl_Regs);
 
-    leon3_tc_tick = leon3_tc_tick_irqmp_timestamp_init;
-  } else if (irqmp_ts != NULL) {
+  if (irqmp_ts != NULL) {
     /* Use the interrupt controller timestamp counter if available */
-    tc->tc_get_timecount = _SPARC_Get_timecount_up;
-#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
-    tc->tc_frequency = leon3_processor_local_bus_frequency();
-#else
-    tc->tc_frequency = ambapp_freq_get(ambapp_plb(), LEON3_Timer_Adev);
-#endif
-
-    leon3_tc_tick = leon3_tc_tick_irqmp_timestamp_init;
-
-    /*
-     * At least one TSISEL field must be non-zero to enable the timestamp
-     * counter.  Use an arbitrary interrupt source.
-     */
-    grlib_store_32(&irqmp_ts->itstmpc, IRQAMP_ITSTMPC_TSISEL(1));
-  } else {
-#ifdef RTEMS_SMP
-    /*
-     * The GR712RC for example has no timestamp unit in the interrupt
-     * controller.  At least on SMP configurations we must use a second timer
-     * in free running mode for the timecounter.
-     */
-    grlib_store_32(
-      &LEON3_Timer_Regs->timer[LEON3_COUNTER_GPTIMER_INDEX].tctrl,
-      GPTIMER_TCTRL_EN | GPTIMER_TCTRL_IE
-    );
-
-    tc->tc_get_timecount = _SPARC_Get_timecount_down;
-#else
-    SPARC_Counter *counter;
-
-    counter = &_SPARC_Counter_mutable;
-    counter->read_isr_disabled = _SPARC_Counter_read_clock_isr_disabled;
-    counter->read = _SPARC_Counter_read_clock;
-    counter->counter_register = &timer->tcntval;
-    counter->pending_register = grlib_load_32(&LEON3_IrqCtrl_Regs->ipend);
-    counter->pending_mask = UINT32_C(1) << clkirq;
-    counter->accumulated = rtems_configuration_get_microseconds_per_tick();
-    counter->interval = rtems_configuration_get_microseconds_per_tick();
-
-    tc->tc_get_timecount = _SPARC_Get_timecount_clock;
-#endif
-
-    tc->tc_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER,
-    leon3_tc_tick = leon3_tc_tick_default;
+    leon3_clock_use_irqamp_timestamp(tc, irqmp_ts);
+    return;
   }
+#endif
 
-  tc->tc_counter_mask = 0xffffffff;
-  tc->tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
-  rtems_timecounter_install(tc);
+  leon3_clock_use_gptimer(tc, timer);
 }
 
 #define Clock_driver_support_initialize_hardware() \
diff --git a/bsps/sparc/leon3/start/cpucounter.c b/bsps/sparc/leon3/start/cpucounter.c
index 57fd487009..18fc8099f2 100644
--- a/bsps/sparc/leon3/start/cpucounter.c
+++ b/bsps/sparc/leon3/start/cpucounter.c
@@ -39,60 +39,89 @@ uint32_t _CPU_Counter_frequency(void)
   return leon3_counter_frequency;
 }
 
+static void leon3_counter_use_up_counter(SPARC_Counter *counter)
+{
+  counter->read_isr_disabled = _SPARC_Counter_read_asr23;
+  counter->read = _SPARC_Counter_read_asr23;
+
+  leon3_counter_frequency = leon3_up_counter_frequency();
+}
+
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
+static void leon3_counter_use_irqamp_timestamp(
+  SPARC_Counter *counter,
+  irqamp_timestamp *irqmp_ts
+)
+{
+  counter->read_isr_disabled = _SPARC_Counter_read_up;
+  counter->read = _SPARC_Counter_read_up;
+  counter->counter_register = &irqmp_ts->itcnt;
+
+  /* Enable interrupt timestamping for an arbitrary interrupt line */
+  grlib_store_32(&irqmp_ts->itstmpc, IRQAMP_ITSTMPC_TSISEL(1));
+
+#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
+  leon3_counter_frequency = leon3_processor_local_bus_frequency();
+#else
+  leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_IrqCtrl_Adev);
+#endif
+}
+#endif
+
+static void leon3_counter_use_gptimer(SPARC_Counter *counter, gptimer *gpt)
+{
+  gptimer_timer *timer;
+
+  timer = &gpt->timer[LEON3_COUNTER_GPTIMER_INDEX];
+  counter->read_isr_disabled = _SPARC_Counter_read_down;
+  counter->read = _SPARC_Counter_read_down;
+  counter->counter_register = &timer->tcntval;
+
+  /* Make timer free running */
+  grlib_store_32(&timer->trldval, 0xffffffff);
+  grlib_store_32(&timer->tctrl, GPTIMER_TCTRL_EN | GPTIMER_TCTRL_RS);
+
+#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
+  leon3_counter_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER;
+#else
+  leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_Timer_Adev) /
+    (grlib_load_32(&gpt->sreload) + 1);
+#endif
+}
+
 static void leon3_counter_initialize(void)
 {
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
   irqamp_timestamp *irqmp_ts;
+#endif
   gptimer *gpt;
   SPARC_Counter *counter;
 
-  irqmp_ts = irqamp_get_timestamp_registers(LEON3_IrqCtrl_Regs);
-  gpt = LEON3_Timer_Regs;
   counter = &_SPARC_Counter_mutable;
 
   leon3_up_counter_enable();
 
   if (leon3_up_counter_is_available()) {
     /* Use the LEON4 up-counter if available */
-    counter->read_isr_disabled = _SPARC_Counter_read_asr23;
-    counter->read = _SPARC_Counter_read_asr23;
-
-    leon3_counter_frequency = leon3_up_counter_frequency();
-  } else if (irqmp_ts != NULL) {
-    /* Use the interrupt controller timestamp counter if available */
-    counter->read_isr_disabled = _SPARC_Counter_read_up;
-    counter->read = _SPARC_Counter_read_up;
-    counter->counter_register = &irqmp_ts->itcnt;
+    leon3_counter_use_up_counter(counter);
+    return;
+  }
 
-    /* Enable interrupt timestamping for an arbitrary interrupt line */
-    grlib_store_32(&irqmp_ts->itstmpc, IRQAMP_ITSTMPC_TSISEL(1));
+#if defined(LEON3_IRQAMP_PROBE_TIMESTAMP)
+  irqmp_ts = irqamp_get_timestamp_registers(LEON3_IrqCtrl_Regs);
 
-#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
-    leon3_counter_frequency = leon3_processor_local_bus_frequency();
-#else
-    leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_IrqCtrl_Adev);
+  if (irqmp_ts != NULL) {
+    /* Use the interrupt controller timestamp counter if available */
+    leon3_counter_use_irqamp_timestamp(counter, irqmp_ts);
+    return;
+  }
 #endif
-  } else if (gpt != NULL) {
-    gptimer_timer *timer;
-    uint32_t       tctrl;
-
-    /* Fall back to the first GPTIMER if available */
-    timer = &gpt->timer[LEON3_COUNTER_GPTIMER_INDEX];
-    counter->read_isr_disabled = _SPARC_Counter_read_down;
-    counter->read = _SPARC_Counter_read_down;
-    counter->counter_register = &timer->tcntval;
 
-    /* Enable timer just in case no clock driver is configured */
-    grlib_store_32(&timer->trldval, 0xffffffff);
-    tctrl = grlib_load_32(&timer->tctrl);
-    tctrl |= GPTIMER_TCTRL_EN | GPTIMER_TCTRL_RS | GPTIMER_TCTRL_LD;
-    grlib_store_32(&timer->tctrl, tctrl);
+  gpt = LEON3_Timer_Regs;
 
-#if defined(LEON3_PLB_FREQUENCY_DEFINED_BY_GPTIMER)
-    leon3_counter_frequency = LEON3_GPTIMER_0_FREQUENCY_SET_BY_BOOT_LOADER;
-#else
-    leon3_counter_frequency = ambapp_freq_get(ambapp_plb(), LEON3_Timer_Adev) /
-      (grlib_load_32(&gpt->sreload) + 1);
-#endif
+  if (gpt != NULL) {
+    /* Fall back to the first GPTIMER if available */
+    leon3_counter_use_gptimer(counter, gpt);
   }
 }
 
diff --git a/spec/build/bsps/sparc/leon3/grp.yml b/spec/build/bsps/sparc/leon3/grp.yml
index 7bb09d268f..9d9f7d2750 100644
--- a/spec/build/bsps/sparc/leon3/grp.yml
+++ b/spec/build/bsps/sparc/leon3/grp.yml
@@ -39,6 +39,8 @@ links:
 - role: build-dependency
   uid: optirqampbase
 - role: build-dependency
+  uid: optirqampts
+- role: build-dependency
   uid: optconirq
 - role: build-dependency
   uid: optleon3smp
diff --git a/spec/build/bsps/sparc/leon3/optirqampts.yml b/spec/build/bsps/sparc/leon3/optirqampts.yml
new file mode 100644
index 0000000000..adcfeaf850
--- /dev/null
+++ b/spec/build/bsps/sparc/leon3/optirqampts.yml
@@ -0,0 +1,24 @@
+SPDX-License-Identifier: CC-BY-SA-4.0 OR BSD-2-Clause
+copyrights:
+- Copyright (C) 2021 embedded brains GmbH & Co. KG
+actions:
+- get-boolean: null
+- define-condition: null
+build-type: option
+default:
+- enabled-by:
+  - sparc/gr712rc
+  - sparc/gr740
+  - sparc/ut699
+  - sparc/ut700
+  value: false
+- enabled-by: true
+  value: true
+enabled-by: true
+links: []
+name: LEON3_IRQAMP_PROBE_TIMESTAMP
+description: |
+  If this option is set to true, then the interrupt timestamping feature of the
+  IRQ(A)MP is probed.  If it is available, then it may be used for the CPU
+  counter and interrupt profiling.
+type: build