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

---
 .../arm/shared/armv7m/startup/armv7m-cpucounter.c  | 55 ++++++++++++++++++++++
 1 file changed, 55 insertions(+)
 create mode 100644 c/src/lib/libbsp/arm/shared/armv7m/startup/armv7m-cpucounter.c

(limited to 'c/src/lib/libbsp/arm/shared')

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
+);
-- 
cgit v1.2.3