From 562b0a014afd9b0553510ef8ae3f6900644d4c2a Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Thu, 24 Nov 2016 10:49:17 +0100
Subject: bsps/arm: Add Cortex-M DWT CPU counter

---
 c/src/lib/libbsp/arm/atsam/Makefile.am             |  2 +-
 .../arm/shared/armv7m/startup/armv7m-cpucounter.c  | 55 ++++++++++++++++++++++
 c/src/lib/libbsp/shared/include/fatal.h            |  1 +
 3 files changed, 57 insertions(+), 1 deletion(-)
 create mode 100644 c/src/lib/libbsp/arm/shared/armv7m/startup/armv7m-cpucounter.c

diff --git a/c/src/lib/libbsp/arm/atsam/Makefile.am b/c/src/lib/libbsp/arm/atsam/Makefile.am
index abbb12a0bd..b0029d0cff 100644
--- a/c/src/lib/libbsp/arm/atsam/Makefile.am
+++ b/c/src/lib/libbsp/arm/atsam/Makefile.am
@@ -334,7 +334,6 @@ libbsp_a_SOURCES += ../../shared/bootcard.c
 libbsp_a_SOURCES += ../../shared/bspclean.c
 libbsp_a_SOURCES += ../../shared/bspgetworkarea.c
 libbsp_a_SOURCES += ../../shared/bsppredriverhook.c
-libbsp_a_SOURCES += ../../shared/cpucounterread.c
 libbsp_a_SOURCES += ../../shared/cpucounterdiff.c
 libbsp_a_SOURCES += ../../shared/gnatinstallhandler.c
 libbsp_a_SOURCES += ../../shared/sbrk.c
@@ -394,6 +393,7 @@ libbsp_a_SOURCES += libraries/libboard/source/dbg_console.c
 # Startup
 libbsp_a_SOURCES += ../shared/startup/bsp-start-memcpy.S
 libbsp_a_SOURCES += ../shared/armv7m/startup/bspreset.c
+libbsp_a_SOURCES += ../shared/armv7m/startup/armv7m-cpucounter.c
 libbsp_a_SOURCES += startup/bspstart.c
 libbsp_a_SOURCES += startup/bspstarthooks.c
 libbsp_a_SOURCES += startup/pin-config.c
diff --git a/c/src/lib/libbsp/arm/shared/armv7m/startup/armv7m-cpucounter.c b/c/src/lib/libbsp/arm/shared/armv7m/startup/armv7m-cpucounter.c
new file mode 100644
index 0000000000..16e971f85a
--- /dev/null
+++ b/c/src/lib/libbsp/arm/shared/armv7m/startup/armv7m-cpucounter.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2016 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.org/license/LICENSE.
+ */
+
+#include <rtems/score/armv7m.h>
+#include <rtems/counter.h>
+#include <rtems/sysinit.h>
+
+#include <bsp.h>
+#include <bsp/fatal.h>
+
+CPU_Counter_ticks _CPU_Counter_read(void)
+{
+  volatile ARMV7M_DWT *dwt = _ARMV7M_DWT;
+
+  return dwt->cyccnt;
+}
+
+static void armv7m_cpu_counter_initialize(void)
+{
+  volatile ARMV7M_DWT *dwt = _ARMV7M_DWT;
+  uint32_t dwt_ctrl;
+
+  dwt_ctrl = dwt->ctrl;
+
+  if ((dwt_ctrl & ARMV7M_DWT_CTRL_NOCYCCNT) == 0) {
+    #ifdef BSP_ARMV7M_SYSTICK_FREQUENCY
+      uint64_t freq = BSP_ARMV7M_SYSTICK_FREQUENCY;
+    #else
+      volatile ARMV7M_Systick *systick = _ARMV7M_Systick;
+      uint64_t freq = ARMV7M_SYSTICK_CALIB_TENMS_GET(systick->calib) * 100ULL;
+    #endif
+
+    dwt->ctrl = dwt_ctrl | ARMV7M_DWT_CTRL_CYCCNTENA;
+    rtems_counter_initialize_converter(freq);
+  } else {
+    bsp_fatal(BSP_ARM_ARMV7M_CPU_COUNTER_INIT);
+  }
+}
+
+RTEMS_SYSINIT_ITEM(
+  armv7m_cpu_counter_initialize,
+  RTEMS_SYSINIT_BSP_START,
+  RTEMS_SYSINIT_ORDER_FIRST
+);
diff --git a/c/src/lib/libbsp/shared/include/fatal.h b/c/src/lib/libbsp/shared/include/fatal.h
index 8220a732f0..fdb7797650 100644
--- a/c/src/lib/libbsp/shared/include/fatal.h
+++ b/c/src/lib/libbsp/shared/include/fatal.h
@@ -49,6 +49,7 @@ typedef enum {
   BSP_ARM_PL111_FATAL_SEM_CREATE,
   BSP_ARM_PL111_FATAL_SEM_RELEASE,
   BSP_ARM_A9MPCORE_FATAL_CLOCK_SMP_INIT,
+  BSP_ARM_ARMV7M_CPU_COUNTER_INIT,
 
   /* LEON3 fatal codes */
   LEON3_FATAL_NO_IRQMP_CONTROLLER = BSP_FATAL_CODE_BLOCK(2),
-- 
cgit v1.2.3