diff --git a/components/freertos/FreeRTOS-openocd.c b/components/freertos/FreeRTOS-openocd.c
new file mode 100644
index 0000000000..6177f02057
--- /dev/null
+++ b/components/freertos/FreeRTOS-openocd.c
@@ -0,0 +1,23 @@
+/*
+ * Since at least FreeRTOS V7.5.3 uxTopUsedPriority is no longer
+ * present in the kernel, so it has to be supplied by other means for
+ * OpenOCD's threads awareness.
+ *
+ * Add this file to your project, and, if you're using --gc-sections,
+ * ``--undefined=uxTopUsedPriority'' (or
+ * ``-Wl,--undefined=uxTopUsedPriority'' when using gcc for final
+ * linking) to your LDFLAGS; same with all the other symbols you need.
+ */
+
+#include "FreeRTOS.h"
+#include "sdkconfig.h"
+
+#ifdef __GNUC__
+#define USED __attribute__((used))
+#else
+#define USED
+#endif
+
+#ifdef CONFIG_FREERTOS_DEBUG_OCDAWARE
+const int USED uxTopUsedPriority = configMAX_PRIORITIES - 1;
+#endif
\ No newline at end of file
diff --git a/components/freertos/Kconfig b/components/freertos/Kconfig
index 89227a7ddf..4149e6f868 100644
--- a/components/freertos/Kconfig
+++ b/components/freertos/Kconfig
@@ -121,5 +121,12 @@ config FREERTOS_DEBUG_OCDAWARE
 		The FreeRTOS panic and unhandled exception handers can detect a JTAG OCD debugger and
 		instead of panicking, have the debugger stop on the offending instruction.
 
+config FREERTOS_BREAK_ON_SCHEDULER_START_JTAG
+	bool "Stop program on scheduler start when JTAG/OCD is detected"
+	depends on FREERTOS_DEBUG_OCDAWARE
+	default y
+	help
+		If JTAG/OCD is connected, stop execution when the scheduler is started and the first
+		task is executed.
 
 endmenu
diff --git a/components/freertos/Makefile b/components/freertos/Makefile
index a2cdb00229..e4003eb146 100644
--- a/components/freertos/Makefile
+++ b/components/freertos/Makefile
@@ -2,6 +2,7 @@
 # Component Makefile
 #
 
+COMPONENT_ADD_LDFLAGS = -l$(COMPONENT_NAME) -Wl,--undefined=uxTopUsedPriority
 COMPONENT_ADD_INCLUDEDIRS := include
 COMPONENT_PRIV_INCLUDEDIRS := include/freertos
 
diff --git a/components/freertos/include/freertos/panic.h b/components/freertos/include/freertos/panic.h
new file mode 100644
index 0000000000..9e902ed20b
--- /dev/null
+++ b/components/freertos/include/freertos/panic.h
@@ -0,0 +1,7 @@
+#ifndef PANIC_H
+#define PANIC_H
+
+void setBreakpointIfJtag(void *fn);
+
+
+#endif
\ No newline at end of file
diff --git a/components/freertos/include/freertos/portmacro.h b/components/freertos/include/freertos/portmacro.h
index bb574a1aae..807412fa88 100644
--- a/components/freertos/include/freertos/portmacro.h
+++ b/components/freertos/include/freertos/portmacro.h
@@ -118,8 +118,11 @@ typedef unsigned portBASE_TYPE	UBaseType_t;
 // portbenchmark
 #include "portbenchmark.h"
 
+#include "sdkconfig.h"
 
 #define portMUX_DEBUG
+#define portFIRST_TASK_HOOK CONFIG_FREERTOS_BREAK_ON_SCHEDULER_START_JTAG
+
 
 typedef struct {
 	volatile uint32_t mux;
diff --git a/components/freertos/panic.c b/components/freertos/panic.c
index 4711e9efe9..4c0f8fcacc 100644
--- a/components/freertos/panic.c
+++ b/components/freertos/panic.c
@@ -142,6 +142,17 @@ void panicHandler(XtExcFrame *frame) {
 	commonErrorHandler(frame);
 }
 
+static void setFirstBreakpoint(uint32_t pc) {
+	asm(
+		"wsr.ibreaka0 %0\n" \
+		"rsr.ibreakenable a3\n" \
+		"movi a4,1\n" \
+		"or a4, a4, a3\n" \
+		"wsr.ibreakenable a4\n" \
+		::"r"(pc):"a3","a4");
+	return;
+}
+
 void xt_unhandled_exception(XtExcFrame *frame) {
 	int *regs=(int*)frame;
 	int x;
@@ -158,14 +169,7 @@ void xt_unhandled_exception(XtExcFrame *frame) {
 		panicPutStr(". Setting bp and returning..\r\n");
 		//Stick a hardware breakpoint on the address the handler returns to. This way, the OCD debugger
 		//will kick in exactly at the context the error happened.
-		asm(
-			"wsr.ibreaka0 %0\n" \
-			"rsr.ibreakenable a3\n" \
-			"movi a4,1\n" \
-			"or a4, a4, a3\n" \
-			"wsr.ibreakenable a4\n" \
-			::"r"(regs[1]):"a3","a4");
-		return;
+		setFirstBreakpoint(regs[1]);
 	}
 	panicPutStr(". Exception was unhandled.\r\n");
 	commonErrorHandler(frame);
@@ -209,3 +213,9 @@ void commonErrorHandler(XtExcFrame *frame) {
 	while(1);
 #endif
 }
+
+
+void setBreakpointIfJtag(void *fn) {
+	if (!inOCDMode()) return;
+	setFirstBreakpoint((uint32_t)fn);
+}
diff --git a/components/freertos/port.c b/components/freertos/port.c
index 8f0a617af1..2220373f3d 100644
--- a/components/freertos/port.c
+++ b/components/freertos/port.c
@@ -101,6 +101,8 @@
 #include "FreeRTOS.h"
 #include "task.h"
 
+#include "panic.h"
+
 /* Defined in portasm.h */
 extern void _frxt_tick_timer_init(void);
 
@@ -385,6 +387,10 @@ portBASE_TYPE vPortCPUReleaseMutex(portMUX_TYPE *mux) {
 	return ret;
 }
 
-
+#if CONFIG_FREERTOS_BREAK_ON_SCHEDULER_START_JTAG
+void vPortFirstTaskHook(TaskFunction_t function) {
+	setBreakpointIfJtag(function);
+}
+#endif
 
 
diff --git a/components/freertos/tasks.c b/components/freertos/tasks.c
index fff7f49f8d..ca0baa9d5b 100644
--- a/components/freertos/tasks.c
+++ b/components/freertos/tasks.c
@@ -445,6 +445,11 @@ to its original value when it is released. */
 	extern void vApplicationTickHook( void );
 #endif
 
+#if  portFIRST_TASK_HOOK
+	extern void vPortFirstTaskHook(TaskFunction_t taskfn);
+#endif
+
+
 /* File private functions. --------------------------------*/
 
 /*
@@ -707,6 +712,12 @@ BaseType_t i;
 						/* Schedule if nothing is scheduled yet, or overwrite a task of lower prio. */
 						if ( pxCurrentTCB[i] == NULL || pxCurrentTCB[i]->uxPriority <= uxPriority )
 						{
+#if portFIRST_TASK_HOOK
+							if ( i == 0) {
+								vPortFirstTaskHook(pxTaskCode);
+							}
+#endif /* configFIRST_TASK_HOOK */
+
 							pxCurrentTCB[i] = pxNewTCB;
 							break;
 						}