diff --git a/source/Core/BSP/Magic/FreeRTOSConfig.h b/source/Core/BSP/Magic/FreeRTOSConfig.h
index 3b51d441..9127e00d 100644
--- a/source/Core/BSP/Magic/FreeRTOSConfig.h
+++ b/source/Core/BSP/Magic/FreeRTOSConfig.h
@@ -28,18 +28,14 @@
 #define configUSE_COUNTING_SEMAPHORES           1
 #define configGENERATE_RUN_TIME_STATS           0
 #define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
-#define configUSE_STATS_FORMATTING_FUNCTIONS    2
+#define configUSE_STATS_FORMATTING_FUNCTIONS    0
 #define configUSE_TICKLESS_IDLE                 0
 
 /* Co-routine definitions. */
-#define configUSE_CO_ROUTINES           0
-#define configMAX_CO_ROUTINE_PRIORITIES (2)
+#define configUSE_CO_ROUTINES 0
 
 /* Software timer definitions. */
-#define configUSE_TIMERS             0
-#define configTIMER_TASK_PRIORITY    (configMAX_PRIORITIES - 1)
-#define configTIMER_QUEUE_LENGTH     8
-#define configTIMER_TASK_STACK_DEPTH (160)
+#define configUSE_TIMERS 0
 
 /* Task priorities.  Allow these to be overridden. */
 #ifndef uartPRIMARY_PRIORITY
@@ -53,9 +49,6 @@ extern "C" {
 header file. */
 void vAssertCalled(void);
 
-void vPortEnterCritical(void);
-void vPortExitCritical(void);
-
 #define configASSERT(x) \
   if ((x) == 0)         \
   vAssertCalled()
@@ -68,9 +61,9 @@ void vApplicationSleep(uint32_t xExpectedIdleTime);
 #define portSUPPRESS_TICKS_AND_SLEEP(xExpectedIdleTime) vApplicationSleep(xExpectedIdleTime)
 #endif
 
-#define INCLUDE_vTaskPrioritySet            1
-#define INCLUDE_uxTaskPriorityGet           1
-#define INCLUDE_vTaskDelete                 1
+#define INCLUDE_vTaskPrioritySet            0
+#define INCLUDE_uxTaskPriorityGet           0
+#define INCLUDE_vTaskDelete                 0
 #define INCLUDE_vTaskSuspend                1
 #define INCLUDE_xResumeFromISR              1
 #define INCLUDE_vTaskDelayUntil             1
diff --git a/source/Core/BSP/Magic/Setup.cpp b/source/Core/BSP/Magic/Setup.cpp
index f1adf194..2675d9cd 100644
--- a/source/Core/BSP/Magic/Setup.cpp
+++ b/source/Core/BSP/Magic/Setup.cpp
@@ -119,6 +119,3 @@ void vAssertCalled(void) {
   while (1)
     ;
 }
-
-void vPortEnterCritical(void) {}
-void vPortExitCritical(void) {}
diff --git a/source/Core/Drivers/USBPD.cpp b/source/Core/Drivers/USBPD.cpp
index 15b279ae..023947e3 100644
--- a/source/Core/Drivers/USBPD.cpp
+++ b/source/Core/Drivers/USBPD.cpp
@@ -4,6 +4,7 @@
 
 #include "BSP_PD.h"
 #include "FreeRTOS.h"
+#include "bflb_platform.h"
 #include "fusb302b.h"
 #include "main.hpp"
 #include "pd.h"
@@ -18,9 +19,11 @@
 
 void ms_delay(uint32_t delayms) {
   // Convert ms -> ticks
+  MSG((char *)"USB PD Delay %d\r\n", delayms);
   TickType_t ticks = delayms / portTICK_PERIOD_MS;
 
   vTaskDelay(ticks ? ticks : 1); /* Minimum delay = 1 tick */
+  MSG((char *)"USB PD Delay %d...Done\r\n", delayms);
 }
 uint32_t get_ms_timestamp() {
   // Convert ticks -> ms