diff --git a/portable/GCC/ARM_CM7/r0p1/port.c b/portable/GCC/ARM_CM7/r0p1/port.c
index ac067274d..60c2f9d83 100644
--- a/portable/GCC/ARM_CM7/r0p1/port.c
+++ b/portable/GCC/ARM_CM7/r0p1/port.c
@@ -272,15 +272,19 @@ void vPortSVCHandler( void )
 
 static void prvPortStartFirstTask( void )
 {
-    /* Start the first task.  This also clears the bit that indicates the FPU is
-     * in use in case the FPU was used before the scheduler was started - which
-     * would otherwise result in the unnecessary leaving of space in the SVC stack
-     * for lazy saving of FPU registers. */
+#ifndef configKEEP_MAIN_STACK
     __asm volatile (
         " ldr r0, =0xE000ED08   \n" /* Use the NVIC offset register to locate the stack. */
         " ldr r0, [r0]          \n"
         " ldr r0, [r0]          \n"
         " msr msp, r0           \n" /* Set the msp back to the start of the stack. */
+        );
+#endif
+    /* Start the first task.  This also clears the bit that indicates the FPU is
+     * in use in case the FPU was used before the scheduler was started - which
+     * would otherwise result in the unnecessary leaving of space in the SVC stack
+     * for lazy saving of FPU registers. */
+    __asm volatile (
         " mov r0, #0            \n" /* Clear the bit that indicates the FPU is in use, see comment above. */
         " msr control, r0       \n"
         " cpsie i               \n" /* Globally enable interrupts. */