Replace the CLZ function with a CLZ intrinsic in the Cortex-R4 port layer.

Add EDS support in the PIC24 port layer.
Remove unnecessary EHB instructions from PIC32 port.
In the PIC32 port assembly code, replace the &= code with a single ins instruction.

FreeRTOS/Source/portable/CCS/ARM_Cortex-R4/portASM.asm

@@ -233,15 +233,6 @@ vPortPreemptiveTick:
         ; Restore the context of the task selected to execute.
         portRESTORE_CONTEXT
 
-;-------------------------------------------------------------------------------
-
-		.def ulPortCountLeadingZeros
-
-ulPortCountLeadingZeros:
-
-		CLZ		R0, R0
-		BX		LR
-
 ;-------------------------------------------------------------------------------
 
 	.if (__TI_VFP_SUPPORT__)

FreeRTOS/Source/portable/CCS/ARM_Cortex-R4/portmacro.h

@@ -122,9 +122,6 @@ extern void vPortYield( void );
 /* Architecture specific optimisations. */
 #if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1
 
-	/* Generic helper function. */
-	unsigned long ulPortCountLeadingZeros( unsigned long ulBitmap );
-
 	/* Check the configuration. */
 	#if( configMAX_PRIORITIES > 32 )
 		#error configUSE_PORT_OPTIMISED_TASK_SELECTION can only be set to 1 when configMAX_PRIORITIES is less than or equal to 32.  It is very rare that a system requires more than 10 to 15 difference priorities as tasks that share a priority will time slice.
@@ -136,7 +133,7 @@ extern void vPortYield( void );
 
 	/*-----------------------------------------------------------*/
 
-	#define portGET_HIGHEST_PRIORITY( uxTopPriority, uxReadyPriorities ) uxTopPriority = ( 31 - ulPortCountLeadingZeros( ( uxReadyPriorities ) ) )
+	#define portGET_HIGHEST_PRIORITY( uxTopPriority, uxReadyPriorities ) uxTopPriority = ( 31 - __clz( ( uxReadyPriorities ) ) )
 
 #endif /* configUSE_PORT_OPTIMISED_TASK_SELECTION */
 

FreeRTOS/Source/portable/MPLAB/PIC24_dsPIC/port.c

@@ -103,6 +103,27 @@ unsigned portBASE_TYPE uxCriticalNesting = 0xef;
 
 #ifdef MPLAB_PIC24_PORT
 
+    #ifdef __HAS_EDS__
+		#define portRESTORE_CONTEXT()                                                                                                                   \
+					asm volatile(	"MOV	_pxCurrentTCB, W0		\n"	/* Restore the stack pointer for the task. */				\
+							"MOV	[W0], W15				\n"																\
+							"POP	W0						\n"	/* Restore the critical nesting counter for the task. */	\
+							"MOV	W0, _uxCriticalNesting	\n"																\
+							"POP	DSWPAG					\n"																\
+							"POP    DSRPAG                                  \n"                                             \
+							"POP	CORCON					\n"																\
+							"POP	TBLPAG					\n"																\
+							"POP	RCOUNT					\n"	/* Restore the registers from the stack. */					\
+							"POP	W14						\n"																\
+							"POP.D	W12						\n"																\
+							"POP.D	W10						\n"																\
+							"POP.D	W8						\n"																\
+							"POP.D	W6						\n"																\
+							"POP.D	W4						\n"																\
+							"POP.D	W2						\n"																\
+							"POP.D	W0						\n"																\
+							"POP	SR						  " );
+	#else /* __HAS_EDS__ */
 		#define portRESTORE_CONTEXT()																						\
 			asm volatile(	"MOV	_pxCurrentTCB, W0		\n"	/* Restore the stack pointer for the task. */				\
 							"MOV	[W0], W15				\n"																\
@@ -121,7 +142,7 @@ unsigned portBASE_TYPE uxCriticalNesting = 0xef;
 							"POP.D	W2						\n"																\
 							"POP.D	W0						\n"																\
 							"POP	SR						  " );
-
+		#endif /* __HAS_EDS__ */
 #endif /* MPLAB_PIC24_PORT */
 
 #ifdef MPLAB_DSPIC_PORT
@@ -235,8 +256,16 @@ const portSTACK_TYPE xInitialStack[] =
 
 	*pxTopOfStack = CORCON;
 	pxTopOfStack++;
+
+	#if defined(__HAS_EDS__)
+		*pxTopOfStack = DSRPAG;
+		pxTopOfStack++;
+		*pxTopOfStack = DSWPAG;
+		pxTopOfStack++;
+	#else /* __HAS_EDS__ */
 		*pxTopOfStack = PSVPAG;
 		pxTopOfStack++;
+	#endif /* __HAS_EDS__ */
 
 	/* Finally the critical nesting depth. */
 	*pxTopOfStack = 0x00;

FreeRTOS/Source/portable/MPLAB/PIC24_dsPIC/portasm_PIC24.S

@@ -88,7 +88,12 @@ _vPortYield:
 		PUSH	TBLPAG
 
 		PUSH	CORCON
+		#ifdef __HAS_EDS__
+			PUSH    DSRPAG
+			PUSH    DSWPAG
+		#else
 			PUSH	PSVPAG
+		#endif /* __HAS_EDS__ */
 		MOV		_uxCriticalNesting, W0		/* Save the critical nesting counter for the task. */
 		PUSH	W0
 		MOV		_pxCurrentTCB, W0			/* Save the new top of stack into the TCB. */
@@ -100,7 +105,12 @@ _vPortYield:
 		MOV		[W0], W15
 		POP		W0							/* Restore the critical nesting counter for the task. */
 		MOV		W0, _uxCriticalNesting
+#ifdef __HAS_EDS__
+        POP     DSWPAG
+        POP     DSRPAG
+#else
 		POP		PSVPAG
+#endif /* __HAS_EDS__ */
 		POP		CORCON
 		POP		TBLPAG
 		POP		RCOUNT						/* Restore the registers from the stack. */

FreeRTOS/Source/portable/MPLAB/PIC32MX/ISR_Support.h

@@ -224,7 +224,6 @@
 
 	mtc0		k0, _CP0_STATUS
 	mtc0 		k1, _CP0_EPC
-	ehb
 	eret 
 	nop
 

FreeRTOS/Source/portable/MPLAB/PIC32MX/port_asm.S

@@ -222,8 +222,7 @@ vPortYieldISR:
 
 	/* Clear the software interrupt in the core. */
 	mfc0		s6, _CP0_CAUSE
-	addiu       s4,zero,-257
+	ins			s6, zero, 8, 1
-	and			s6, s6, s4
 	mtc0		s6, _CP0_CAUSE
 
 	/* Clear the interrupt in the interrupt controller. */
@@ -298,7 +297,6 @@ vPortYieldISR:
 
 	mtc0		k1, _CP0_STATUS
 	mtc0 		k0, _CP0_EPC
-	ehb
 	eret
 	nop