1 files changed, 62 insertions, 0 deletions
diff --git a/c/src/lib/libbsp/arm/xm-tms570/startup/cpucounterread.c b/c/src/lib/libbsp/arm/xm-tms570/startup/cpucounterread.c
new file mode 100644
index 0000000000..4549dae882
--- /dev/null
+++ b/c/src/lib/libbsp/arm/xm-tms570/startup/cpucounterread.c
@@ -0,0 +1,62 @@
+/*
+ * @file cpucounterread.c
+ *
+ * @author Miguel Masmano <mmasmano@fentiss.com>
+ *
+ * @copyright
+ * Copyright 2016 Fent Innovative Software Solutions (FENTISS). 
+ * All rights reserved.
+ *
+ * Copyright 2017 embedded brains GmbH.
+ *
+ * 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/counter.h>
+
+#include <libcpu/arm-cp15.h>
+
+#include <bsp.h>
+#include <xm.h>
+
+#ifdef XM_TMS570_USE_PMU_FOR_CPU_COUNTER
+/* PMCCNTR */
+ARM_CP15_TEXT_SECTION static inline uint32_t
+arm_cp15_get_performance_monitors_cycle_count(void)
+{
+  ARM_SWITCH_REGISTERS;
+  uint32_t val;
+
+  __asm__ volatile (
+    ARM_SWITCH_TO_ARM
+    "mrc p15, 0, %[val], c9, c13, 0\n"
+    ARM_SWITCH_BACK
+    : [val] "=&r" (val) ARM_SWITCH_ADDITIONAL_OUTPUT
+  );
+
+  return val;
+}
+#endif
+
+CPU_Counter_ticks _CPU_Counter_read(void)
+{
+#ifdef XM_TMS570_USE_PMU_FOR_CPU_COUNTER
+  return arm_cp15_get_performance_monitors_cycle_count();
+#else
+  xmTime_t cTime;
+  XM_get_time(XM_HW_CLOCK, &cTime);
+
+  return (CPU_Counter_ticks)cTime;
+#endif
+}
+
+void xm_tms570_cpu_counter_initialize(void)
+{
+#ifdef XM_TMS570_USE_PMU_FOR_CPU_COUNTER
+  rtems_counter_initialize_converter(XM_TMS570_GCLK);
+#else
+  rtems_counter_initialize_converter(1000000);
+#endif
+}