diff --git a/Core/Src/main.c b/Core/Src/main.c
index 7b46fc9..f370b2d 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -77,15 +77,7 @@ static void MX_SDIO_SD_Init(void);
 static void MX_USART1_UART_Init(void);
 
 /* USER CODE BEGIN PFP */
-int _write(int file, char *ptr, int len)
-{
-  if ((CoreDebug->DEMCR & CoreDebug_DEMCR_TRCENA_Msk) && (ITM->TCR & ITM_TCR_ITMENA_Msk))
-  {
-    for (int i = 0; i < len; i++)
-    {
-      ITM_SendChar((*ptr++));
-    }
-  }
+int _write(int file, char *ptr, int len) for (int i = 0; i < len; i++) ITM_SendChar((*ptr++));
   return len;
 }
 /* USER CODE END PFP */
@@ -103,10 +95,14 @@ 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;
+  if (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk)
+  {
+    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 */