From 7e5450acd1fc80f1b046bf11e9dde5c425bce270 Mon Sep 17 00:00:00 2001
From: Richard Barry <ribarry@amazon.com>
Date: Mon, 11 Feb 2008 21:02:40 +0000
Subject: [PATCH] Change the critical section handling (Fujitsu 32bit port).

---
 Source/portable/Softune/MB91460/port.c      | 51 +++++++++++++++++++--
 Source/portable/Softune/MB91460/portmacro.h | 26 ++++-------
 2 files changed, 56 insertions(+), 21 deletions(-)

diff --git a/Source/portable/Softune/MB91460/port.c b/Source/portable/Softune/MB91460/port.c
index 00de8a98c..4c1d425fc 100644
--- a/Source/portable/Softune/MB91460/port.c
+++ b/Source/portable/Softune/MB91460/port.c
@@ -51,6 +51,10 @@ any details of its type. */
 typedef void tskTCB;
 extern volatile tskTCB * volatile pxCurrentTCB;
 
+/* Constants required to handle critical sections. */
+#define portNO_CRITICAL_NESTING		( ( unsigned portBASE_TYPE ) 0 )
+volatile unsigned portLONG ulCriticalNesting = 9999UL;
+
 /*-----------------------------------------------------------*/
 
 
@@ -63,6 +67,10 @@ extern volatile tskTCB * volatile pxCurrentTCB;
 	 STM1 (R14,R13,R12,R11,R10,R9,R8)			;Store R14-R8
 	 ST MDH, @-R15								;Store MDH
 	 ST MDL, @-R15								;Store MDL
+
+	 LDI #_ulCriticalNesting, R0				;Get the address of the critical nesting counter
+	 LD @R0, R0									;Get the value of the critical nesting counter
+	 ST R0, @-R15								;Store the critical nesting value to the user stack.
 	 
 	 ANDCCR #0xDF								;Switch back to system stack
 	 LD @R15+,R0								;Store PC to R0 
@@ -98,6 +106,10 @@ extern volatile tskTCB * volatile pxCurrentTCB;
 
 	 ORCCR #0x20								;Switch back to retreive the remaining context
 
+	 LDI #_ulCriticalNesting, R0				;Get the address of the critical nesting counter
+	 LD @R15+, R1								;Get the saved critical nesting value
+	 ST R1, @R0									;Save the critical nesting value into the ulCriticalNesting variable
+
 	 LD @R15+, MDL								;Restore MDL
 	 LD @R15+, MDH								;Restore MDH
 	 LDM1 (R14,R13,R12,R11,R10,R9,R8)			;Restore R14-R8
@@ -124,8 +136,6 @@ static void prvSetupTimerInterrupt( void );
  */
 portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE pxCode, void *pvParameters )
 {
-unsigned portSHORT usAddress;
-
 	/* Place a few bytes of known values on the bottom of the stack. 
 	This is just useful for debugging. */
 
@@ -185,8 +195,13 @@ unsigned portSHORT usAddress;
 	*pxTopOfStack = ( portSTACK_TYPE ) 0x22220000;	/* MDL */
 	pxTopOfStack--;
 
-	usAddress = ( unsigned portSHORT ) pxCode;
-	*pxTopOfStack = ( portSTACK_TYPE ) usAddress ;	/* PC */
+	/* The task starts with its ulCriticalNesting variable set to 0, interrupts
+	being enabled. */
+	*pxTopOfStack = portNO_CRITICAL_NESTING;
+	pxTopOfStack--;
+
+	/* The start of the task code. */
+	*pxTopOfStack = ( portSTACK_TYPE ) pxCode;	/* PC */
 	pxTopOfStack--;
 	 
     /* PS - User Mode, USP, ILM=31, Interrupts enabled */
@@ -351,3 +366,31 @@ const unsigned portSHORT usReloadValue = ( unsigned portSHORT ) ( ( ( configPER_
 	RETI
 
 #pragma endasm
+
+/*-----------------------------------------------------------*/
+
+void vPortEnterCritical( void )
+{
+	/* Disable interrupts */
+	portDISABLE_INTERRUPTS();
+
+	/* Now interrupts are disabled ulCriticalNesting can be accessed
+	 directly.  Increment ulCriticalNesting to keep a count of how many times
+	 portENTER_CRITICAL() has been called. */
+	ulCriticalNesting++;
+}
+/*-----------------------------------------------------------*/
+
+void vPortExitCritical( void )
+{
+	if( ulCriticalNesting > portNO_CRITICAL_NESTING )
+	{
+		ulCriticalNesting--;
+		if( ulCriticalNesting == portNO_CRITICAL_NESTING )
+		{
+			/* Enable all interrupt/exception. */
+			portENABLE_INTERRUPTS();
+		}
+	}
+}
+/*-----------------------------------------------------------*/
diff --git a/Source/portable/Softune/MB91460/portmacro.h b/Source/portable/Softune/MB91460/portmacro.h
index d9f07ab42..cd8642332 100644
--- a/Source/portable/Softune/MB91460/portmacro.h
+++ b/Source/portable/Softune/MB91460/portmacro.h
@@ -59,7 +59,7 @@
 #define portDOUBLE		double
 #define portLONG		long
 #define portSHORT		int
-#define portSTACK_TYPE	unsigned portSHORT
+#define portSTACK_TYPE	unsigned portLONG
 #define portBASE_TYPE	long
 
 /* This is required since SOFTUNE doesn't support inline directive as is. */
@@ -75,20 +75,12 @@
 /*-----------------------------------------------------------*/	
 
 /* Critical section management. */
-
-
-#define portENTER_CRITICAL() \
-	__asm(" ST PS,@-R15 "); \
-	__asm(" ANDCCR #0xef "); \
-
-
-#define portEXIT_CRITICAL()	 \
-	__asm(" LD @R15+,PS "); \
-
-
-#define portDISABLE_INTERRUPTS()  __DI();
-
-#define portENABLE_INTERRUPTS()  __EI();
+void vPortEnterCritical( void );
+void vPortExitCritical( void );
+#define portENTER_CRITICAL() vPortEnterCritical()
+#define portEXIT_CRITICAL() vPortExitCritical()
+#define portDISABLE_INTERRUPTS() __DI();
+#define portENABLE_INTERRUPTS() __EI();
 
 /*-----------------------------------------------------------*/
 
@@ -102,8 +94,8 @@
 /* portYIELD() uses SW interrupt */
 #define portYIELD()					__asm( " INT #40H " );
 
-/* portYIELDFromISR() uses delayed interrupt */
-#define portYIELDFromISR()			DICR_DLYI = 1;
+/* portYIELD_FROM_ISR() uses delayed interrupt */
+#define portYIELD_FROM_ISR()			DICR_DLYI = 1
 /*-----------------------------------------------------------*/
 
 /* Task function macros as described on the FreeRTOS.org WEB site. */