From 84ea393dd65e9faef3ba62f79b06c8cdfaa13e65 Mon Sep 17 00:00:00 2001
From: Erick Ahmed <engineering@erickahmed.com>
Date: Wed, 17 Jun 2026 18:04:48 +0200
Subject: [PATCH] Implement ITM debug via ST-Link

- Enable Instrumentation Trace Macroblock
- Remove PB3 initialization conflict with ITM
- Add a bunch of debug prints
---
 Core/Inc/main.h | 18 ++++++++++++++++++
 Core/Src/main.c | 33 ++++++++++++++++++++++++---------
 2 files changed, 42 insertions(+), 9 deletions(-)

diff --git a/Core/Inc/main.h b/Core/Inc/main.h
index 8f472c0..4cc7d5f 100644
--- a/Core/Inc/main.h
+++ b/Core/Inc/main.h
@@ -24,6 +24,24 @@
 extern "C" {
 #endif
 
+#define DEBUG
+#ifdef DEBUG
+static inline void ITM_Print(const char *str) {
+    while (*str) {
+        uint32_t timeout = 1000;
+        while ((ITM->PORT[0].u32 == 0) && (timeout-- > 0));
+
+        if (timeout > 0) {
+            ITM->PORT[0].u8 = (uint8_t)(*str);
+        }
+        str++;
+    }
+}
+#define DEBUG_PRINT(x) ITM_Print(x)
+#else
+#define DEBUG_PRINT(x)
+#endif
+
 /* Includes ------------------------------------------------------------------*/
 #include "stm32f4xx_hal.h"
 #include "canlog.h"
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 7d631ed..d538f06 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -91,7 +91,11 @@ int main(void)
 {
 
   /* USER CODE BEGIN 1 */
-
+  CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+  ITM->LAR = 0xC5ACCE55;
+  ITM->TER = 1 << 0;
+  ITM->TCR = ITM_TCR_ITMENA_Msk | ITM_TCR_SYNCENA_Msk | ITM_TCR_SWOENA_Msk;
+  DEBUG_PRINT("Booting system\r\n");
   /* USER CODE END 1 */
 
   /* MCU Configuration--------------------------------------------------------*/
@@ -100,6 +104,7 @@ int main(void)
   HAL_Init();
 
   /* USER CODE BEGIN Init */
+  DEBUG_PRINT("Configuring system clock\r\n");
 
   /* USER CODE END Init */
 
@@ -107,7 +112,7 @@ int main(void)
   SystemClock_Config();
 
   /* USER CODE BEGIN SysInit */
-
+  DEBUG_PRINT("Initializing configured peripherals...\r\n");
   /* USER CODE END SysInit */
 
   /* Initialize all configured peripherals */
@@ -118,12 +123,15 @@ int main(void)
   MX_SDIO_SD_Init();
   MX_USART1_UART_Init();
   /* USER CODE BEGIN 2 */
+  DEBUG_PRINT("Initializing CAN bus logger\r\n");
   CAN_Logger_Init(&hcan1, &hcan2);
   /* USER CODE END 2 */
 
   /* Init scheduler */
   osKernelInitialize();
 
+  DEBUG_PRINT("Creating RTOS entities\r\n");
+
   /* USER CODE BEGIN RTOS_TASKS */
   const osThreadAttr_t CAN1rxAttributes = {
     .name = "CAN1rx",
@@ -183,6 +191,7 @@ int main(void)
   /* USER CODE END RTOS_EVENTS */
 
   /* Start scheduler */
+  DEBUG_PRINT("Starting RTOS init scheduler\r\n");
   osKernelStart();
 
   /* We should never get here as control is now taken by the scheduler */
@@ -194,6 +203,7 @@ int main(void)
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
+    DEBUG_PRINT("ERROR: RTOS scheduler crashed!\r\n");
     /* USER CODE END 3 */
   }
 }
@@ -276,7 +286,7 @@ static void MX_CAN1_Init(void)
     Error_Handler();
   }
   /* USER CODE BEGIN CAN1_Init 2 */
-
+  DEBUG_PRINT("CAN1 initialized!\r\n");
   /* USER CODE END CAN1_Init 2 */
 
 }
@@ -313,7 +323,7 @@ static void MX_CAN2_Init(void)
     Error_Handler();
   }
   /* USER CODE BEGIN CAN2_Init 2 */
-
+  DEBUG_PRINT("CAN2 initialized!\r\n");
   /* USER CODE END CAN2_Init 2 */
 
 }
@@ -349,7 +359,7 @@ static void MX_SDIO_SD_Init(void)
     Error_Handler();
   }
   /* USER CODE BEGIN SDIO_Init 2 */
-
+  DEBUG_PRINT("SDIO initialized!\r\n");
   /* USER CODE END SDIO_Init 2 */
 
 }
@@ -382,7 +392,7 @@ static void MX_USART1_UART_Init(void)
     Error_Handler();
   }
   /* USER CODE BEGIN USART1_Init 2 */
-
+  DEBUG_PRINT("USART1 initialized!\r\n");
   /* USER CODE END USART1_Init 2 */
 
 }
@@ -404,6 +414,9 @@ static void MX_DMA_Init(void)
   HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 5, 0);
   HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);
 
+  /* USER CODE BEGIN MX_DMA_Init 1 */
+  DEBUG_PRINT("DMA initialized!\r\n");
+  /* USER CODE END MX_DMA_Init 1 */
 }
 
 /**
@@ -466,15 +479,15 @@ static void MX_GPIO_Init(void)
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
-  /*Configure GPIO pins : PB3 PB4 PB5 */
-  GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5;
+  /*Configure GPIO pins : PB4 PB5 */
+  GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_5;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
   /* USER CODE BEGIN MX_GPIO_Init_2 */
-
+  DEBUG_PRINT("GPIO initialized!\r\n");
   /* USER CODE END MX_GPIO_Init_2 */
 }
 
@@ -509,6 +522,8 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
   */
 void Error_Handler(void)
 {
+  DEBUG_PRINT("ERROR: entering error handler\r\n");
+
   /* USER CODE BEGIN Error_Handler_Debug */
   /* User can add his own implementation to report the HAL error return state */
   __disable_irq();