From 5d369c85725062d2e716742180887068f1b2ccaa Mon Sep 17 00:00:00 2001
From: Pavel Pisa <pisa@cmp.felk.cvut.cz>
Date: Sat, 3 Sep 2016 14:10:52 +0200
Subject: arm/raspberrypi: use GPU/system timer as clock source.

It has advantage that it is designed as free-running counter
with compare registers and can easily serve for for both
timecounter and tick interrupt.
---
 c/src/lib/libbsp/arm/raspberrypi/clock/clockdrv.c | 98 ++++++++++++++---------
 1 file changed, 62 insertions(+), 36 deletions(-)

diff --git a/c/src/lib/libbsp/arm/raspberrypi/clock/clockdrv.c b/c/src/lib/libbsp/arm/raspberrypi/clock/clockdrv.c
index 4ce795fb30..72c85b1669 100644
--- a/c/src/lib/libbsp/arm/raspberrypi/clock/clockdrv.c
+++ b/c/src/lib/libbsp/arm/raspberrypi/clock/clockdrv.c
@@ -10,6 +10,7 @@
  * BCM2835 Clock driver
  *
  * Copyright (c) 2013 Alan Cudmore
+ * Copyright (c) 2016 Pavel Pisa
  *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
@@ -21,71 +22,96 @@
 #include <rtems.h>
 #include <bsp.h>
 #include <bsp/irq.h>
+#include <bsp/irq-generic.h>
 #include <bsp/raspberrypi.h>
+#include <rtems/timecounter.h>
 
 /* This is defined in ../../../shared/clockdrv_shell.h */
 void Clock_isr(rtems_irq_hdl_param arg);
 
+static struct timecounter raspberrypi_tc;
+
+static uint32_t raspberrypi_clock_get_timecount(struct timecounter *tc)
+{
+  return BCM2835_REG(BCM2835_GPU_TIMER_CLO);
+}
+
 static void raspberrypi_clock_at_tick(void)
 {
-   BCM2835_REG(BCM2835_TIMER_CLI) = 0;
+  uint32_t act_val;
+  uint32_t next_cmp = BCM2835_REG(BCM2835_GPU_TIMER_C3);
+  next_cmp += rtems_configuration_get_microseconds_per_tick();
+  BCM2835_REG(BCM2835_GPU_TIMER_C3) = next_cmp;
+  act_val = BCM2835_REG(BCM2835_GPU_TIMER_CLO);
+
+  /*
+   * Clear interrupt only if there is time left to the next tick.
+   * If time of the next tick has already passed then interrupt
+   * request stays active and fires immediately after current tick
+   * processing is finished.
+   */
+  if ((int32_t)(next_cmp - act_val) > 0)
+    BCM2835_REG(BCM2835_GPU_TIMER_CS) = BCM2835_GPU_TIMER_CS_M3;
 }
 
-static void raspberrypi_clock_handler_install(void)
+static void raspberrypi_clock_handler_install_isr(
+  rtems_isr_entry clock_isr
+)
 {
   rtems_status_code sc = RTEMS_SUCCESSFUL;
 
-  sc = rtems_interrupt_handler_install(
-    BCM2835_IRQ_ID_TIMER_0,
-    "Clock",
-    RTEMS_INTERRUPT_UNIQUE,
-    (rtems_interrupt_handler) Clock_isr,
-    NULL
-  );
-  if (sc != RTEMS_SUCCESSFUL) {
+  if (clock_isr != NULL) {
+    sc = rtems_interrupt_handler_install(
+      BCM2835_IRQ_ID_GPU_TIMER_M3,
+      "Clock",
+      RTEMS_INTERRUPT_UNIQUE,
+      (rtems_interrupt_handler) clock_isr,
+      NULL
+    );
+  } else {
+    /* Remove interrupt handler */
+    sc = rtems_interrupt_handler_remove(
+      BCM2835_IRQ_ID_GPU_TIMER_M3,
+      (rtems_interrupt_handler) Clock_isr,
+      NULL
+    );
+  }
+  if ( sc != RTEMS_SUCCESSFUL ) {
     rtems_fatal_error_occurred(0xdeadbeef);
   }
 }
 
-static void raspberrypi_clock_initialize(void)
+static void raspberrypi_clock_initialize_hardware(void)
 {
-   BCM2835_REG(BCM2835_TIMER_CTL) = 0x003E0000;
-   BCM2835_REG(BCM2835_TIMER_LOD) =
-                rtems_configuration_get_microseconds_per_tick() - 1;
-   BCM2835_REG(BCM2835_TIMER_RLD) =
-                rtems_configuration_get_microseconds_per_tick() - 1;
-   BCM2835_REG(BCM2835_TIMER_DIV) = BCM2835_TIMER_PRESCALE;
-   BCM2835_REG(BCM2835_TIMER_CLI) = 0;
-   BCM2835_REG(BCM2835_TIMER_CTL) = 0x003E00A2;
+  uint32_t next_cmp = BCM2835_REG(BCM2835_GPU_TIMER_CLO);
+  next_cmp += rtems_configuration_get_microseconds_per_tick();
+  BCM2835_REG(BCM2835_GPU_TIMER_C3) = next_cmp;
+  BCM2835_REG(BCM2835_GPU_TIMER_CS) = BCM2835_GPU_TIMER_CS_M3;
+
+  raspberrypi_tc.tc_get_timecount = raspberrypi_clock_get_timecount;
+  raspberrypi_tc.tc_counter_mask = 0xffffffff;
+  raspberrypi_tc.tc_frequency = 1000000; /* 1 MHz */
+  raspberrypi_tc.tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+  rtems_timecounter_install(&raspberrypi_tc);
 }
 
 static void raspberrypi_clock_cleanup(void)
 {
-  rtems_status_code sc = RTEMS_SUCCESSFUL;
-
-  /* Remove interrupt handler */
-  sc = rtems_interrupt_handler_remove(
-    BCM2835_IRQ_ID_TIMER_0,
-    (rtems_interrupt_handler) Clock_isr,
-    NULL
-  );
-  if (sc != RTEMS_SUCCESSFUL) {
-    rtems_fatal_error_occurred(0xdeadbeef);
-  }
+  bsp_interrupt_vector_disable(BCM2835_IRQ_ID_GPU_TIMER_M3);
 }
 
 #define Clock_driver_support_at_tick() raspberrypi_clock_at_tick()
 
-#define Clock_driver_support_initialize_hardware() raspberrypi_clock_initialize()
+#define Clock_driver_support_initialize_hardware() raspberrypi_clock_initialize_hardware()
 
-#define Clock_driver_support_install_isr(isr, old_isr) \
+#define Clock_driver_support_shutdown_hardware() raspberrypi_clock_cleanup()
+
+#define Clock_driver_support_install_isr(clock_isr, old_isr) \
   do {                                                 \
-    raspberrypi_clock_handler_install();               \
+    raspberrypi_clock_handler_install_isr(clock_isr);  \
     old_isr = NULL;                                    \
   } while (0)
 
-#define Clock_driver_support_shutdown_hardware() raspberrypi_clock_cleanup()
-
-#define CLOCK_DRIVER_USE_DUMMY_TIMECOUNTER
+#define CLOCK_DRIVER_USE_ONLY_BOOT_PROCESSOR 1
 
 #include "../../../shared/clockdrv_shell.h"
-- 
cgit v1.2.3