From d48ccf161296e2aabba3b46e35579787f3f1bf27 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Mario=20H=C3=BCttel?= <mario.huettel@gmx.net>
Date: Sun, 1 Jan 2023 20:01:04 +0100
Subject: [PATCH] Fix #50: Enable core cycle counter

---
 .../reflow-controller/main-cycle-counter.h    | 22 +++++++++++++--
 stm-firmware/main-cycle-counter.c             | 28 ++++++++++++++++++-
 stm-firmware/main.c                           |  4 +--
 stm-firmware/ui/shell.c                       |  6 ++--
 4 files changed, 52 insertions(+), 8 deletions(-)

diff --git a/stm-firmware/include/reflow-controller/main-cycle-counter.h b/stm-firmware/include/reflow-controller/main-cycle-counter.h
index 90564ad..a9123c1 100644
--- a/stm-firmware/include/reflow-controller/main-cycle-counter.h
+++ b/stm-firmware/include/reflow-controller/main-cycle-counter.h
@@ -30,10 +30,16 @@
 #include <stdint.h>
 
 /**
- * @brief Initialize/Reset the main cycle counter to 0.
+ * @brief Initialize the main cycle counter and reset to 0. This also enables the core cycle counter
  * This function can be called multiple times.
  */
-void main_cycle_counter_init(void);
+void main_and_core_cycle_counter_init(void);
+
+/**
+ * @brief Reset the main cycle counter.
+ * @note This does not reset the core cycle counter
+ */
+void main_cycle_counter_reset(void);
 
 /**
  * @brief Increment the main cycle counter by 1
@@ -46,6 +52,18 @@ void main_cycle_counter_inc(void);
  */
 uint64_t main_cycle_counter_get(void);
 
+/**
+ * @brief Reset the core cycle counter to 0
+ */
+void core_cycle_counter_reset(void);
+
+/**
+ * @brief Get the current value of the core cycle counter
+ * 
+ * @return uint32_t Counter value
+ */
+uint32_t core_cycle_counter_get(void);
+
 #endif /* __MAIN_CYCLE_COUNTER_H__ */
 
 /** @} */
diff --git a/stm-firmware/main-cycle-counter.c b/stm-firmware/main-cycle-counter.c
index bb0d4e8..da55809 100644
--- a/stm-firmware/main-cycle-counter.c
+++ b/stm-firmware/main-cycle-counter.c
@@ -25,6 +25,7 @@
 
 #include <reflow-controller/main-cycle-counter.h>
 #include <helper-macros/helper-macros.h>
+#include <stm32/stm32f4xx.h>
 
 /**
  * @brief Variable storing the main cycle counter.
@@ -33,7 +34,22 @@
  */
 static uint64_t IN_SECTION(.ccm.bss) main_cycle_counter;
 
-void main_cycle_counter_init(void)
+void main_and_core_cycle_counter_init(void)
+{
+	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;	
+
+	/* Enable the core cycle counter if available on current processor */
+	if (!(DWT->CTRL & DWT_CTRL_NOCYCCNT_Msk)) {
+		/* Cycle counter is available. Enable */
+		DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+	}
+
+	/* Reset the counters to 0 */
+	main_cycle_counter_reset();
+	core_cycle_counter_reset();
+}	
+
+void main_cycle_counter_reset(void)
 {
 	main_cycle_counter = 0ULL;
 }
@@ -48,4 +64,14 @@ uint64_t main_cycle_counter_get(void)
 	return main_cycle_counter;
 }
 
+void core_cycle_counter_reset(void)
+{
+	DWT->CYCCNT = 0UL;
+}
+
+uint32_t core_cycle_counter_get(void)
+{
+	return DWT->CYCCNT;
+}
+
 /** @} */
diff --git a/stm-firmware/main.c b/stm-firmware/main.c
index 5573003..0923a0b 100644
--- a/stm-firmware/main.c
+++ b/stm-firmware/main.c
@@ -268,8 +268,8 @@ int main(void)
 	/** - Print motd to shell */
 	shell_print_motd(shell_handle);
 
-	/** - Set the main cycle counter to 0 */
-	main_cycle_counter_init();
+	/** - Set the main cycle counter to 0 and activate the core cycle counter if available */
+	main_and_core_cycle_counter_init();
 
 	/** - Do a loop over the following */
 	while (1) {
diff --git a/stm-firmware/ui/shell.c b/stm-firmware/ui/shell.c
index 06f9722..b82a42c 100644
--- a/stm-firmware/ui/shell.c
+++ b/stm-firmware/ui/shell.c
@@ -892,7 +892,7 @@ shellmatta_retCode_t shell_cmd_cycle_count(const shellmatta_handle_t handle, con
 
 
 	counter = main_cycle_counter_get();
-	core_cycle_count = DWT->CYCCNT;
+	core_cycle_count = core_cycle_counter_get();
 	if (hex) {
 		shellmatta_printf(handle, "Main loop: 0x%016"PRIX64"\r\n", counter);
 		shellmatta_printf(handle, "CPU cycles: 0x%08"PRIX32"\r\n", core_cycle_count);
@@ -901,8 +901,8 @@ shellmatta_retCode_t shell_cmd_cycle_count(const shellmatta_handle_t handle, con
 		shellmatta_printf(handle, "CPU cycles: %"PRIu32"\r\n", core_cycle_count);
 	}
 	if (clear) {
-		main_cycle_counter_init();
-		DWT->CYCCNT = 0UL;
+		main_cycle_counter_reset();
+		core_cycle_counter_reset();
 	}
 	return SHELLMATTA_OK;
 }