From: richardbarry <richardbarry@1d2547de-c912-0410-9cb9-b8ca96c0e9e2>
Date: Tue, 6 May 2008 11:51:13 +0000 (+0000)
Subject: Update to allow nesting.
X-Git-Tag: V5.0.2~60
X-Git-Url: https://git.sur5r.net/?a=commitdiff_plain;h=d6dadc0cc345ce595c3f169b7fc354e4d9c51b83;p=freertos

Update to allow nesting.

git-svn-id: https://svn.code.sf.net/p/freertos/code/trunk@337 1d2547de-c912-0410-9cb9-b8ca96c0e9e2
---

diff --git a/Source/portable/MPLAB/PIC32MX/ISR_Support.h b/Source/portable/MPLAB/PIC32MX/ISR_Support.h
index 208d7b80e..757b1ba4e 100644
--- a/Source/portable/MPLAB/PIC32MX/ISR_Support.h
+++ b/Source/portable/MPLAB/PIC32MX/ISR_Support.h
@@ -49,12 +49,9 @@
 
 #include "FreeRTOSConfig.h"
 
-#define portCONTEXT_SIZE 136
-#define portEXL_AND_IE_BITS	0x03
-
+#define portCONTEXT_SIZE 132
 #define portEPC_STACK_LOCATION	124
 #define portSTATUS_STACK_LOCATION 128
-#define portCAUSE_STACK_LOCATION 132
 
 /******************************************************************/ 	
 .macro	portSAVE_CONTEXT
@@ -64,26 +61,16 @@
 	original values in case of interrupt nesting. */
 	mfc0		k0, _CP0_CAUSE
 	addiu		sp,	sp, -portCONTEXT_SIZE
-	sw			k0, portCAUSE_STACK_LOCATION(sp)
 	mfc0		k1, _CP0_STATUS
 
 	/* Also save s6 and s5 so we can use them during this interrupt.  Any
 	nesting interrupts should maintain the values of these registers
-	accross the ISR. */
+	across the ISR. */
 	sw			s6, 44(sp)
 	sw			s5, 40(sp)
 	sw			k1, portSTATUS_STACK_LOCATION(sp)
 
-	/* Enable interrupts above the current priority.  SysCall interrupts
-	enable priorities above configKERNEL_INTERRUPT_PRIORITY, so first 
-	check if the interrupt was a system call (32). */
-	add			s6, zero, k0
-	and			s6, s6, 32
-	beq			s6, zero, .+20 /* Not a system call, mask up to the current interrupt priority. */
-	nop
-	addiu		k0, zero, configKERNEL_INTERRUPT_PRIORITY /* Was a system call, mask only to kernel priority. */
-	beq			zero, zero, .+12
-	nop
+	/* Enable interrupts above the current priority. */
 	srl			k0, k0, 0xa
 	ins 		k1, k0, 10, 6
 	ins			k1, zero, 1, 4
@@ -97,7 +84,7 @@
 
 	/* If the nesting count is 0 then swap to the the system stack, otherwise
 	the system stack is already being used. */
-	bne			s6, zero, .+16
+	bne			s6, zero, .+20
 	nop
 
 	/* Swap to the system stack. */
@@ -108,14 +95,15 @@
 	addiu		s6, s6, 1
 	sw			s6, 0(k0)
 
-	/* s6 holds the EPC value, we may want this during the context switch. */
+	/* s6 holds the EPC value, this is saved after interrupts are re-enabled. */
 	mfc0 		s6, _CP0_EPC
 
 	/* Re-enable interrupts. */
 	mtc0		k1, _CP0_STATUS
 
 	/* Save the context into the space just created.  s6 is saved again
-	here as it now contains the EPC value. */
+	here as it now contains the EPC value.  No other s registers need be
+	saved. */
 	sw			ra,	120(s5)
 	sw			s8, 116(s5)
 	sw			t9, 112(s5)
@@ -134,38 +122,30 @@
 	sw			a0, 60(s5)
 	sw			v1, 56(s5)
 	sw			v0, 52(s5)
-	sw			s7, 48(s5)
 	sw			s6, portEPC_STACK_LOCATION(s5)
-	/* s5 and s6 has already been saved. */
-	sw			s4,	36(s5)
-	sw			s3, 32(s5)
-	sw			s2, 28(s5)
-	sw			s1, 24(s5)
-	sw			s0, 20(s5)
 	sw			$1, 16(s5)
 
-	/* s7 is used as a scratch register. */
-	mfhi		s7
-	sw			s7, 12(s5)
-	mflo		s7
-	sw			s7, 8(s5)
+	/* s6 is used as a scratch register. */
+	mfhi		s6
+	sw			s6, 12(s5)
+	mflo		s6
+	sw			s6, 8(s5)
 
 	/* Each task maintains its own nesting count. */
-	la			s7, uxCriticalNesting
-	lw			s7, (s7)
-	sw			s7, 4(s5)
-
-	/* Update the TCB stack pointer value if the nesting count is 1. */
-	la			s7, uxInterruptNesting
-	lw			s7, (s7)
-	addiu		s7, s7, -1
-	bne			s7, zero, .+24 /* Dont save the stack pointer to the task or swap stacks. */
+	la			s6, uxCriticalNesting
+	lw			s6, (s6)
+	sw			s6, 4(s5)
+
+	/* Update the task stack pointer value if nesting is zero. */
+	la			s6, uxInterruptNesting
+	lw			s6, (s6)
+	addiu		s6, s6, -1
+	bne			s6, zero, .+20
 	nop
 
-	/* Save the stack pointer to the task. */
-	la			s7, pxCurrentTCB
-	lw			s7, (s7)
-	sw			s5, (s7)
+	/* Save the stack pointer. */
+	la			s6, uxSavedTaskStackPointer
+	sw			s5, (s6)
 
 	.endm
 	
@@ -174,35 +154,23 @@
 
 	/* Restore the stack pointer from the TCB.  This is only done if the
 	nesting count is 1. */
-	la			s7, uxInterruptNesting
-	lw			s7, (s7)
-	addiu		s7, s7, -1
-	bne			s7, zero, .+24 /* Dont load the stack pointer. */
+	la			s6, uxInterruptNesting
+	lw			s6, (s6)
+	addiu		s6, s6, -1
+	bne			s6, zero, .+20
 	nop
-	la			s0, pxCurrentTCB
-	lw			s0, (s0)
-	lw			s5, (s0)
+	la			s6, uxSavedTaskStackPointer
+	lw			s5, (s6)
 	
-	/* Restore the context, the first item of which is the critical nesting
-	depth. */
-	la			s0, uxCriticalNesting
-	lw			s1, 4(s5)
-	sw			s1, (s0)
-
-	/* Restore the rest of the context. */
-	lw			s0, 8(s5)
-	mtlo		s0
-	lw			s0, 12(s5)
-	mthi		s0
+	/* Restore the context. */
+	lw			s6, 8(s5)
+	mtlo		s6
+	lw			s6, 12(s5)
+	mthi		s6
 	lw			$1, 16(s5)
-	lw			s0, 20(s5)
-	lw			s1, 24(s5)
-	lw			s2, 28(s5)
-	lw			s3, 32(s5)
-	lw			s4, 36(s5)
-	/* s5 is loaded later. */
+	/* s6 is loaded as it was used as a scratch register and therefore saved
+	as part of the interrupt context. */
 	lw			s6, 44(s5)
-	lw			s7, 48(s5)
 	lw			v0, 52(s5)
 	lw			v1, 56(s5)
 	lw			a0, 60(s5)
@@ -231,8 +199,15 @@
 	addiu		k1, k1, -1
 	sw			k1, 0(k0)
 
-	lw			k1, portSTATUS_STACK_LOCATION(s5)
-	lw			k0, portEPC_STACK_LOCATION(s5)
+	/* Restore the critical nesting count. */
+	la			k0, uxCriticalNesting
+	lw			k1, 4(s5)
+	sw			k1, (k0)
+
+	/* If the critical nesting is not zero then set status as if within
+	a critical section. */
+	lw			k0, portSTATUS_STACK_LOCATION(s5)
+	lw			k1, portEPC_STACK_LOCATION(s5)
 
 	/* Leave the stack how we found it.  First load sp from s5, then restore
 	s5 from the stack. */
@@ -240,9 +215,9 @@
 	lw			s5, 40(sp)
 	addiu		sp,	sp,	portCONTEXT_SIZE
 
-	mtc0		k1, _CP0_STATUS
+	mtc0		k0, _CP0_STATUS
 	ehb
-	mtc0 		k0, _CP0_EPC
+	mtc0 		k1, _CP0_EPC
 	eret 
 	nop
 
diff --git a/Source/portable/MPLAB/PIC32MX/port.c b/Source/portable/MPLAB/PIC32MX/port.c
index 01bcf4a0d..8ab29fc68 100644
--- a/Source/portable/MPLAB/PIC32MX/port.c
+++ b/Source/portable/MPLAB/PIC32MX/port.c
@@ -51,9 +51,6 @@
  * Implementation of functions defined in portable.h for the PIC32MX port.
   *----------------------------------------------------------*/
 
-/* Library includes. */
-#include <string.h>
-
 /* Scheduler include files. */
 #include "FreeRTOS.h"
 #include "task.h"
@@ -64,12 +61,13 @@
 /* Bits within various registers. */
 #define portIE_BIT					( 0x00000001 )
 #define portEXL_BIT					( 0x00000002 )
+#define portSW0_ENABLE				( 0x00000100 )
 #define portIPL_SHIFT				( 10 )
-#define portALL_IPL_BITS			( 0x1f << portIPL_SHIFT )
+#define portALL_IPL_BITS			( 0x3f << portIPL_SHIFT )
 
 /* The EXL bit is set to ensure interrupts do not occur while the context of
 the first task is being restored. */
-#define portINITIAL_SR				( portIE_BIT | portEXL_BIT )
+#define portINITIAL_SR				( portIE_BIT | portEXL_BIT | portSW0_ENABLE )
 
 /* Records the nesting depth of calls to portENTER_CRITICAL(). */
 unsigned portBASE_TYPE uxCriticalNesting = 0x55555555;
@@ -78,6 +76,13 @@ unsigned portBASE_TYPE uxCriticalNesting = 0x55555555;
 decremented to 0 when the first task starts. */
 volatile unsigned portBASE_TYPE uxInterruptNesting = 0x01;
 
+/* Used to store the original interrupt mask when the mask level is temporarily
+raised during an ISR. */
+volatile unsigned portBASE_TYPE uxSavedStatusRegister = 0;
+
+/* Stores the task stack pointer when a switch is made to use the system stack. */
+unsigned portBASE_TYPE uxSavedTaskStackPointer = 0;
+
 /* The stack used by interrupt service routines that cause a context switch. */
 portSTACK_TYPE xISRStack[ configISR_STACK_SIZE ] = { 0 };
 
@@ -88,11 +93,10 @@ const portBASE_TYPE * const xISRStackTop = &( xISRStack[ configISR_STACK_SIZE -
 /* Place the prototype here to ensure the interrupt vector is correctly installed. */
 extern void __attribute__( (interrupt(ipl1), vector(_TIMER_1_VECTOR))) vT1InterruptHandler( void );
 
-/* 
- * General exception handler that will be called for all general exceptions
- * other than SYS.  This should be overridden by a user provided handler.
+/*
+ * The software interrupt handler that performs the yield.
  */
-void vApplicationGeneralExceptionHandler( unsigned portLONG ulCause, unsigned portLONG ulStatus ) __attribute__((weak));
+void __attribute__( (interrupt(ipl1), vector(_CORE_SOFTWARE_0_VECTOR))) vPortYieldISR( void );
 
 /*-----------------------------------------------------------*/
 
@@ -156,7 +160,7 @@ unsigned portLONG ulStatus;
 
 	/* Mask interrupts at and below the kernel interrupt priority. */
 	ulStatus = _CP0_GET_STATUS();
-	ulStatus |= ( configKERNEL_INTERRUPT_PRIORITY << portIPL_SHIFT );
+	ulStatus |= ( configMAX_SYSCALL_INTERRUPT_PRIORITY << portIPL_SHIFT );
 	_CP0_SET_STATUS( ulStatus );
 
 	/* Once interrupts are disabled we can access the nesting count directly. */
@@ -186,14 +190,18 @@ unsigned portLONG ulStatus;
 portBASE_TYPE xPortStartScheduler( void )
 {
 extern void vPortStartFirstTask( void );
+extern void *pxCurrentTCB;
 
-	memset( xISRStack, 0x5a, configISR_STACK_SIZE * sizeof( portSTACK_TYPE ) );
+	/* Setup the software interrupt. */
+	mConfigIntCoreSW0( CSW_INT_ON | CSW_INT_PRIOR_1 | CSW_INT_SUB_PRIOR_0 );
 
 	/* Setup the timer to generate the tick.  Interrupts will have been 
 	disabled by the time we get here. */
 	prvSetupTimerInterrupt();
 
-	/* Kick off the highest priority task that has been created so far. */
+	/* Kick off the highest priority task that has been created so far. 
+	Its stack location is loaded into uxSavedTaskStackPointer. */
+	uxSavedTaskStackPointer = *( unsigned portBASE_TYPE * ) pxCurrentTCB;
 	vPortStartFirstTask();
 
 	/* Should never get here as the tasks will now be executing. */
@@ -201,12 +209,49 @@ extern void vPortStartFirstTask( void );
 }
 /*-----------------------------------------------------------*/
 
-void vApplicationGeneralExceptionHandler( unsigned portLONG ulCause, unsigned portLONG ulStatus )
+void vPortYield( void )
+{
+unsigned portLONG ulStatus;
+
+	SetCoreSW0();
+
+	/* Unmask all interrupts. */
+	ulStatus = _CP0_GET_STATUS();
+	ulStatus &= ~portALL_IPL_BITS;
+	_CP0_SET_STATUS( ulStatus );
+}
+/*-----------------------------------------------------------*/
+
+void vPortIncrementTick( void )
+{
+	vPortSetInterruptMaskFromISR();
+		vTaskIncrementTick();
+	vPortClearInterruptMaskFromISR();
+	
+	/* If we are using the preemptive scheduler then we might want to select
+	a different task to execute. */
+	#if configUSE_PREEMPTION == 1
+		SetCoreSW0();
+	#endif /* configUSE_PREEMPTION */
+
+	/* Clear timer 0 interrupt. */
+	mT1ClearIntFlag();
+}
+/*-----------------------------------------------------------*/
+
+void vPortSetInterruptMaskFromISR( void )
+{
+	asm volatile ( "di" );
+	uxSavedStatusRegister = _CP0_GET_STATUS() | 0x01;
+	_CP0_SET_STATUS( ( uxSavedStatusRegister | ( configMAX_SYSCALL_INTERRUPT_PRIORITY << portIPL_SHIFT ) ) );
+}
+/*-----------------------------------------------------------*/
+
+void vPortClearInterruptMaskFromISR( void )
 {
-	/* This function is declared weak and should be overridden by the users
-	application. */
-	while( 1 );
+	_CP0_SET_STATUS( uxSavedStatusRegister );
 }
+/*-----------------------------------------------------------*/
 
 
 
diff --git a/Source/portable/MPLAB/PIC32MX/port_asm.S b/Source/portable/MPLAB/PIC32MX/port_asm.S
index 8aed3ac91..a1b26f8f8 100644
--- a/Source/portable/MPLAB/PIC32MX/port_asm.S
+++ b/Source/portable/MPLAB/PIC32MX/port_asm.S
@@ -59,12 +59,11 @@
  	.extern pxCurrentTCB
  	.extern uxCriticalNesting
  	.extern vTaskSwitchContext
- 	.extern vTaskIncrementTick
-	.extern vApplicationGeneralExceptionHandler
+ 	.extern vPortIncrementTick
 	.extern xISRStackTop
  	
  	.global vPortStartFirstTask
-	.global _general_exception_context
+	.global vPortYieldISR
 	.global vT1InterruptHandler
 
 
@@ -79,21 +78,9 @@ vT1InterruptHandler:
 
 	portSAVE_CONTEXT
 
-	jal 		vTaskIncrementTick
+	jal 		vPortIncrementTick
 	nop
 
-	/* If we are using the preemptive scheduler then we might want to select
-	a different task to execute. */
-	#if configUSE_PREEMPTION == 1
-		jal 		vTaskSwitchContext
-		nop
-	#endif /* configUSE_PREEMPTION */
-
-	/* Clear timer 0 interrupt. */
-	la			s1, IFS0CLR
-	addiu       s0,zero,_IFS0_T1IF_MASK
-	sw			s0, 0(s1)
-
 	portRESTORE_CONTEXT
 
 	.end vT1InterruptHandler
@@ -113,58 +100,214 @@ vPortStartFirstTask:
 
 	.end xPortStartScheduler
 
+
+
 /*******************************************************************/
 
  	.section 	.FreeRTOS, "ax", @progbits
  	.set		noreorder
 	.set 		noat
- 	.ent		_general_exception_context
+ 	.ent		vPortYieldISR
+
+vPortYieldISR:
+
+	/* Make room for the context. First save the current status so we can 
+	manipulate it, and the cause and EPC registers so we capture their 
+	original values in case of interrupt nesting. */
+	mfc0		k0, _CP0_CAUSE
+	addiu		sp,	sp, -portCONTEXT_SIZE
+	mfc0		k1, _CP0_STATUS
+
+	/* Also save s6 and s5 so we can use them during this interrupt.  Any
+	nesting interrupts should maintain the values of these registers
+	accross the ISR. */
+	sw			s6, 44(sp)
+	sw			s5, 40(sp)
+	sw			k1, portSTATUS_STACK_LOCATION(sp)
+
+	/* Enable interrupts above the current priority. */
+	srl			k0, k0, 0xa
+	ins 		k1, k0, 10, 6
+	ins			k1, zero, 1, 4
+
+	/* s5 is used as the frame pointer. */
+	add			s5, zero, sp
+
+	/* Swap to the system stack.  This is not conditional on the nesting
+	count as this interrupt is always the lowest priority and therefore
+	the nesting is always 0. */
+	la			sp, xISRStackTop
+	lw			sp, (sp)
+
+	/* Increment and save the nesting count in case this gets preempted. */
+	la			k0, uxInterruptNesting
+	lw			s6, (k0)
+	addiu		s6, s6, 1
+	sw			s6, 0(k0)
+
+	/* s6 holds the EPC value, this is saved with the rest of the context
+	after interrupts are enabled. */
+	mfc0 		s6, _CP0_EPC
+
+	/* Re-enable interrupts. */
+	mtc0		k1, _CP0_STATUS
+
+	/* Save the context into the space just created.  s6 is saved again
+	here as it now contains the EPC value. */
+	sw			ra,	120(s5)
+	sw			s8, 116(s5)
+	sw			t9, 112(s5)
+	sw			t8,	108(s5)
+	sw			t7,	104(s5)
+	sw			t6, 100(s5)
+	sw			t5, 96(s5)
+	sw			t4, 92(s5)
+	sw			t3, 88(s5)
+	sw			t2, 84(s5)
+	sw			t1, 80(s5)
+	sw			t0, 76(s5)
+	sw			a3, 72(s5)
+	sw			a2, 68(s5)
+	sw			a1, 64(s5)
+	sw			a0, 60(s5)
+	sw			v1, 56(s5)
+	sw			v0, 52(s5)
+	sw			s7, 48(s5)
+	sw			s6, portEPC_STACK_LOCATION(s5)
+	/* s5 and s6 has already been saved. */
+	sw			s4,	36(s5)
+	sw			s3, 32(s5)
+	sw			s2, 28(s5)
+	sw			s1, 24(s5)
+	sw			s0, 20(s5)
+	sw			$1, 16(s5)
+
+	/* s7 is used as a scratch register as this should always be saved across
+	nesting interrupts. */
+	mfhi		s7
+	sw			s7, 12(s5)
+	mflo		s7
+	sw			s7, 8(s5)
+
+	/* Each task maintains its own nesting count. */
+	la			s7, uxCriticalNesting
+	lw			s7, (s7)
+	sw			s7, 4(s5)
+
+	/* Save the stack pointer to the task. */
+	la			s7, pxCurrentTCB
+	lw			s7, (s7)
+	sw			s5, (s7)
+
+	/* Set the interrupt mask to the max priority that can use the API. */
+	di
+	mfc0		s7, _CP0_STATUS
+	ori			s7, s7, 1
+	ori			s6, s7, configMAX_SYSCALL_INTERRUPT_PRIORITY << 10
 
-_general_exception_context:
+	/* This mtc0 re-enables interrupts, but only above 
+	configMAX_SYSCALL_INTERRUPT_PRIORITY. */
+	mtc0		s6, _CP0_STATUS
 
-	/* Save the context of the current task. */
-	portSAVE_CONTEXT
+	/* Clear the software interrupt in the core. */
+	mfc0		s6, _CP0_CAUSE
+	addiu       s4,zero,-257
+	and			s6, s6, s4
+	mtc0		s6, _CP0_CAUSE
 
-	/* Was this handler caused by a syscall?  The original Cause
-	value was saved to the stack as it could change as interrupts
-	nest.  Use of k registers must be protected from use by nesting
-	interrupts. */
-	lw			s7, portCAUSE_STACK_LOCATION(s5)
-	andi		s7, s7, portEXC_CODE_MASK
-	addi		s7, s7, -( _EXCCODE_SYS << 2 )
+	/* Clear the interrupt in the interrupt controller. */
+	la			s6, IFS0CLR
+	addiu		s4, zero, 2
+	sw			s4, (s6)
 
-	/* Yes - call the SYSCALL handler to select a new task to execute. */
-	beq         s7, zero, SyscallHandler
+	jal			vTaskSwitchContext
 	nop
 
-	/* No - call the application handler to handle all other types of 
-	exception.  Pass the status and cause to the application provided 
-	handler.  Interrupts are disabled during the execution of the user
-	defined handler. */
+	/* Clear the interrupt mask again.  The saved status value is still in s7. */
+	mtc0		s7, _CP0_STATUS
+
+	/* Restore the stack pointer from the TCB. */
+	la			s0, pxCurrentTCB
+	lw			s0, (s0)
+	lw			s5, (s0)
+
+	/* Restore the rest of the context. */
+	lw			s0, 8(s5)
+	mtlo		s0
+	lw			s0, 12(s5)
+	mthi		s0
+	lw			$1, 16(s5)
+	lw			s0, 20(s5)
+	lw			s1, 24(s5)
+	lw			s2, 28(s5)
+	lw			s3, 32(s5)
+	lw			s4, 36(s5)
+	/* s5 is loaded later. */
+	lw			s6, 44(s5)
+	lw			s7, 48(s5)
+	lw			v0, 52(s5)
+	lw			v1, 56(s5)
+	lw			a0, 60(s5)
+	lw			a1, 64(s5)
+	lw			a2, 68(s5)
+	lw			a3, 72(s5)
+	lw			t0, 76(s5)
+	lw			t1, 80(s5)
+	lw			t2, 84(s5)
+	lw			t3, 88(s5)
+	lw			t4, 92(s5)
+	lw			t5, 96(s5)
+	lw			t6, 100(s5)
+	lw			t7, 104(s5)
+	lw			t8, 108(s5)
+	lw			t9, 112(s5)
+	lw			s8, 116(s5)
+	lw			ra, 120(s5)
+
+	/* Protect access to the k registers, and others. */
 	di
-	lw			a1, portSTATUS_STACK_LOCATION(s5)
-	lw			a0, portCAUSE_STACK_LOCATION(s5)
-	jal			vApplicationGeneralExceptionHandler
+
+	/* Decrement the nesting count. */
+	la			k0, uxInterruptNesting
+	lw			k1, (k0)
+	addiu		k1, k1, -1
+	sw			k1, 0(k0)
+
+	/* Switch back to use the real stack pointer. */
+	add			sp, zero, s5
+
+	/* Restore the critical nesting	depth. */
+	la			s5, uxCriticalNesting
+	lw			k0, 4(sp)
+	sw			k0, (s5)
+
+	/* If the critical nesting is not zero and a yield is not pended
+	then set status as if within a critical section. */
+	lw			s5, portSTATUS_STACK_LOCATION(sp)
+	beq			k0, zero, .+28
 	nop
-	ei
-	beq 		zero, zero, FinishExceptionHandler
+	mfc0		k1, _CP0_CAUSE
+	andi		k1, k1, 256
+	bne			k1, zero, .+12
 	nop
+	or			s5, s5, (configMAX_SYSCALL_INTERRUPT_PRIORITY<<10)
 
-SyscallHandler:
+	lw			k0, portEPC_STACK_LOCATION(sp)
 
-	/* Adjust the return that was placed onto the stack to be the 
-	address of the instruction following the syscall.  s6 already
-	contains the EPC value. */
-	addi		s6, 4
-	sw			s6, portEPC_STACK_LOCATION(s5)
+	mtc0		s5, _CP0_STATUS
+	ehb	
 
-	jal			vTaskSwitchContext
-	nop
+	/* Restore the real s5 value. */
+	lw			s5, 40(sp)
 
-FinishExceptionHandler:
-	portRESTORE_CONTEXT
+	/* Remove stack frame. */
+	addiu		sp,	sp,	portCONTEXT_SIZE
+
+	mtc0 		k0, _CP0_EPC
+	eret 
+	nop
 
-	.end		_general_exception_context
+	.end		vPortYieldISR
 
 
 
diff --git a/Source/portable/MPLAB/PIC32MX/portmacro.h b/Source/portable/MPLAB/PIC32MX/portmacro.h
index 0629d7be7..1c22d5104 100644
--- a/Source/portable/MPLAB/PIC32MX/portmacro.h
+++ b/Source/portable/MPLAB/PIC32MX/portmacro.h
@@ -99,11 +99,17 @@ extern void vPortEnterCritical( void );
 extern void vPortExitCritical( void );
 #define portENTER_CRITICAL()		vPortEnterCritical()
 #define portEXIT_CRITICAL()			vPortExitCritical()
+
+extern void vPortSetInterruptMaskFromISR();
+extern void vPortClearInterruptMaskFromISR();
+#define portSET_INTERRUPT_MASK_FROM_ISR() vPortSetInterruptMaskFromISR()
+#define portCLEAR_INTERRUPT_MASK_FROM_ISR() vPortClearInterruptMaskFromISR()
+
 /*-----------------------------------------------------------*/
 
 /* Task utilities. */
-#define portYIELD() asm volatile ( 	"ehb \r\n"	\
-									"SYSCALL \r\n" )
+extern void vPortYield( void );
+#define portYIELD()	vPortYield()
 
 #define portNOP()				asm volatile ( 	"nop" )
 
@@ -114,7 +120,7 @@ extern void vPortExitCritical( void );
 #define portTASK_FUNCTION( vFunction, pvParameters ) void vFunction( void *pvParameters )
 /*-----------------------------------------------------------*/
 
-#define portEND_SWITCHING_ISR( vSwitchRequired ) if( vSwitchRequired ) vTaskSwitchContext()
+#define portEND_SWITCHING_ISR( vSwitchRequired ) if( vSwitchRequired ) SetCoreSW0()
 
 #ifdef __cplusplus
 }