Update to allow nesting.

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)
+	la			s6, uxCriticalNesting
+	lw			s6, (s6)
+	sw			s6, 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. */
+	/* 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