diff --git a/Source/portable/GCC/ColdFire_V2/port.c b/Source/portable/GCC/ColdFire_V2/port.c
index 315562165c..409d8be262 100644
--- a/Source/portable/GCC/ColdFire_V2/port.c
+++ b/Source/portable/GCC/ColdFire_V2/port.c
@@ -125,17 +125,12 @@ portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE * pxTopOfStack, pdTASK_COD
 
 portBASE_TYPE xPortStartScheduler( void )
 {
+extern void vPortStartFirstTask( void );
+
 	ulCriticalNesting = 0UL;
 
 	vApplicationSetupInterrupts();
-
-	asm volatile(
-					"move.l		pxCurrentTCB, %sp				\n\t"\
-					"move.l		(%sp), %sp						\n\t"\
-					"movem.l	(%sp), %d0-%fp					\n\t"\
-					"lea.l		%sp@(60), %sp					\n\t"\
-					"rte										    "
-				);
+	vPortStartFirstTask();
 
 	return pdFALSE;
 }
@@ -173,6 +168,13 @@ void vPortClearInterruptMaskFromISR( unsigned portBASE_TYPE uxSavedInterruptMask
 {
 }
 
+void vPortClearYield( void )
+{
+	/* -32 as we are using the high word of the 64bit mask. */
+	MCF_INTC0_INTFRCH &= ~( 1UL << ( configYIELD_INTERRUPT_VECTOR - 32UL ) );
+}
+
+
 
 
 
diff --git a/Source/portable/GCC/ColdFire_V2/portasm.S b/Source/portable/GCC/ColdFire_V2/portasm.S
index aee34d3c8b..84be3b7298 100644
--- a/Source/portable/GCC/ColdFire_V2/portasm.S
+++ b/Source/portable/GCC/ColdFire_V2/portasm.S
@@ -9,9 +9,31 @@
  */
 
     .global ulPortSetIPL
+    .global __cs3_isr_interrupt_127
+    .global __cs3_isr_interrupt_119
+    .global vPortStartFirstTask
 
     .text
 
+.macro portSAVE_CONTEXT
+
+	lea.l		(-60, %sp), %sp
+	movem.l		%d0-%fp, (%sp)
+	move.l		pxCurrentTCB, %a0
+	move.l		%sp, (%a0)
+
+	.endm
+
+.macro portRESTORE_CONTEXT
+
+	move.l		pxCurrentTCB, %sp
+	move.l		(%sp), %sp
+	movem.l		(%sp), %d0-%fp
+	lea.l		%sp@(60), %sp
+	rte
+
+	.endm
+
 /********************************************************************/
 /*
  * This routines changes the IPL to the value passed into the routine.
@@ -47,6 +69,20 @@ ulPortSetIPL:
     rts
 
 /********************************************************************/
+
+/* Yield interrupt. */
+__cs3_isr_interrupt_127:
+	portSAVE_CONTEXT
+	jsr vPortClearYield
+	jsr vTaskSwitchContext
+	portRESTORE_CONTEXT
+
+/********************************************************************/
+
+
+vPortStartFirstTask:
+	portRESTORE_CONTEXT
+
     .end
 
 
diff --git a/Source/portable/GCC/ColdFire_V2/portmacro.h b/Source/portable/GCC/ColdFire_V2/portmacro.h
index 3cd00193d4..248d8bdd01 100644
--- a/Source/portable/GCC/ColdFire_V2/portmacro.h
+++ b/Source/portable/GCC/ColdFire_V2/portmacro.h
@@ -106,7 +106,8 @@ extern void vPortClearInterruptMaskFromISR( unsigned portBASE_TYPE );
 
 /* Task utilities. */
 
-#define portYIELD()
+#define portYIELD()			MCF_INTC0_INTFRCH |= ( 1UL << ( configYIELD_INTERRUPT_VECTOR - 32UL ) ); portNOP(); portNOP(); portNOP(); /* -32 as we are using the high word of the 64bit mask. */
+
 
 
 #define portNOP()	asm volatile ( 	"nop" )