More work in progress (PPC).

[freertos] / Source / portable / GCC / PPC405 / portasm.s

diff --git a/Source/portable/GCC/PPC405/portasm.s b/Source/portable/GCC/PPC405/portasm.s
index 6849a8d9868e3e16138aaf10b8ca330941cbf8b1..3810a881159b8d8ab7d1f4786c1199aee67d1877 100644 (file)
--- a/Source/portable/GCC/PPC405/portasm.s
+++ b/Source/portable/GCC/PPC405/portasm.s
@@ -1,20 +1,40 @@
+#include "xreg405.h"\r
+\r
        .extern pxCurrentTCB\r
        .extern vTaskSwitchContext\r
+       .extern vTaskIncrementTick\r
 \r
-       .global vStartFirstTask\r
+       .global vPortStartFirstTask\r
        .global vPortYield\r
+       .global vPortTickISR\r
 \r
 .set portCONTEXT_SIZE, 156\r
 .set portR0_OFFSET, 152\r
 .set portGPR_OFFSET, 32\r
 .set portCR_OFFSET, 28\r
 .set portXER_OFFSET, 24\r
-.set portLR_OFFSET, 20\r
+.set portLR_OFFSET, 16\r
 .set portCTR_OFFSET, 16\r
 .set portUSPRG0_OFFSET, 12\r
 .set portSRR0_OFFSET, 8\r
 .set portSRR1_OFFSET, 4\r
 \r
+\r
+.set   BChainField, 0\r
+.set   NextLRField, BChainField + 4\r
+.set   MSRField,    NextLRField + 4\r
+.set   PCField,     MSRField    + 4\r
+.set   LRField,     PCField     + 4\r
+.set   CTRField,    LRField     + 4\r
+.set   XERField,    CTRField    + 4\r
+.set   CRField,     XERField    + 4\r
+.set   USPRG0Field, CRField     + 4\r
+.set   r0Field,     USPRG0Field + 4\r
+.set   r2Field,     r0Field     + 4\r
+.set   r3r31Field,  r2Field     + 4\r
+.set   IFrameSize,  r3r31Field  + ( ( 31 - 3 ) + 1 ) * 4\r
+\r
+\r
 .macro portRESTORE_CONTEXT\r
 \r
        # Get the address of the TCB.\r
@@ -88,17 +108,127 @@
 \r
        .endm\r
 \r
-vStartFirstTask:\r
-       portRESTORE_CONTEXT\r
+\r
+.macro int_epilogue\r
+\r
+       # Get the address of the TCB.\r
+       xor             R0, R0, R0\r
+    addis   SP, R0, pxCurrentTCB@ha\r
+    lwz                SP,     pxCurrentTCB@l( SP )\r
+\r
+       # Get the task stack pointer from the TCB.\r
+       lwz             SP, 0( SP )\r
+       \r
+       # Restore MSR register to SRR1.\r
+       lwz     R0,MSRField(R1)\r
+       mtsrr1  R0\r
+       \r
+       # Restore current PC location to SRR0.\r
+       lwz     R0,PCField(R1)\r
+       mtsrr0  R0\r
+\r
+       # Save USPRG0 register\r
+       lwz     R0,USPRG0Field(R1)\r
+       mtspr   0x100,R0\r
+       \r
+       # Restore Condition register\r
+       lwz     R0,CRField(R1)\r
+       mtcr    R0\r
+       \r
+       # Restore Fixed Point Exception register\r
+       lwz     R0,XERField(R1)\r
+       mtxer   R0\r
+       \r
+       # Restore Counter register\r
+       lwz     R0,CTRField(R1)\r
+       mtctr   R0\r
+       \r
+       # Restore Link register\r
+       lwz     R0,LRField(R1)\r
+       mtlr    R0\r
+       \r
+       # Restore remaining GPR registers.\r
+       lmw     R3,r3r31Field(R1)\r
+       \r
+       # Restore r0 and r2.\r
+       lwz     R0,r0Field(R1)\r
+       lwz     R2,r2Field(R1)\r
+       \r
+       # Remove frame from stack\r
+       addi    R1,R1,IFrameSize\r
+       \r
+.endm\r
+\r
+.macro portENTER_SWITCHING_ISR\r
+\r
+       # Get the address of the TCB.                           \r
+       xor             R0, R0, R0\r
+       addis   R2, R0, pxCurrentTCB@ha\r
+       lwz             R2,     pxCurrentTCB@l( R2 )\r
+\r
+       # Store the stack pointer into the TCB\r
+       stw             SP,     0( R2 )\r
+\r
+       # Save the link register\r
+       stwu    R1, -24( R1 )\r
+       mflr    R0\r
+       stw             R31, 20( R1 )\r
+       stw             R0, 28( R1 )\r
+       mr              R31, r1\r
+\r
+.endm\r
+\r
+.macro portEXIT_SWITCHING_ISR\r
+\r
+       # Restore the link register\r
+       lwz             R11, 0( R1 )\r
+       lwz             R0, 4( R11 )\r
+       mtlr    R0\r
+       lwz             R31, -4( R11 )\r
+       mr              R1, R11\r
+\r
+       # Get the address of the TCB.\r
+       xor             R0, R0, R0\r
+       addis   SP, R0, pxCurrentTCB@ha\r
+       lwz             SP,     pxCurrentTCB@l( R1 )\r
+\r
+       # Get the task stack pointer from the TCB.\r
+       lwz             SP, 0( SP )\r
+\r
+.endm\r
+\r
+\r
+vPortStartFirstTask:\r
+\r
+       int_epilogue\r
        rfi\r
 \r
+#vPortStartFirstTask:\r
+#      portRESTORE_CONTEXT\r
+#      rfi\r
+\r
 \r
 \r
 vPortYield:\r
 \r
-       portSAVE_CONTEXT\r
+       portENTER_SWITCHING_ISR\r
        bl vTaskSwitchContext\r
-       portRESTORE_CONTEXT\r
+       portEXIT_SWITCHING_ISR\r
+       blr\r
+\r
+vPortTickISR:\r
+\r
+       portENTER_SWITCHING_ISR\r
+       bl vTaskIncrementTick\r
+       #if configUSE_PREEMPTION == 1\r
+               bl vTaskSwitchContext\r
+       #endif\r
+\r
+       # Clear the interrupt\r
+       lis             R0, 2048\r
+       mttsr   R0\r
+\r
+       portEXIT_SWITCHING_ISR\r
+       blr\r
+\r
 \r
-       NOP\r
-       NOP\r