Merge branch "preempt" in "trunk".

[bertos.git] / bertos / cpu / arm / hw / switch_ctx_arm.S

diff --git a/bertos/cpu/arm/hw/switch_ctx_arm.S b/bertos/cpu/arm/hw/switch_ctx_arm.S
index 39e9f46a2dabb58b88475203acf095dacd9a1155..db4f297ff71e93c7d3157e2591546131bbecb877 100644 (file)
--- a/bertos/cpu/arm/hw/switch_ctx_arm.S
+++ b/bertos/cpu/arm/hw/switch_ctx_arm.S
@@ -32,30 +32,68 @@
  *
  * \brief ARM context switch
  *
- * \version $Id: proc.c 18271 2007-10-11 14:51:31Z batt $
  *
  * \author Stefano Fedrigo <aleph@develer.com>
  * \author Francesco Sacchi <batt@develer.com>
+ * \author Andrea Righi <arighi@develer.com>
  */
 
+#include "cfg/cfg_proc.h"
+
 /* void asm_switch_context(void **new_sp [r0], void **save_sp [r1]) */
 .globl asm_switch_context
 asm_switch_context:
-       mrs     r2, cpsr               /* Save status. */
+       /* Save registers */
+       stmfd   sp!, {r4 - r11, lr}
+       /* Save old stack pointer */
+       str     sp, [r1]
+       /* Load new stack pointer */
+       ldr     sp, [r0]
+       /* Load new registers */
+       ldmfd   sp!, {r4 - r11, pc}
+
+#if CONFIG_KERN_PREEMPT
+
+/* ARM interrupt mode with IRQ and FIQ disabled */
+#define ARM_IRQ_MODE 0xD2
+/* ARM supervisor mode with IRQ and FIQ disabled */
+#define ARM_SVC_MODE 0xD3
+
+.globl asm_irq_switch_context
+asm_irq_switch_context:
+       /* Return if preemption is not needed */
+       bl      proc_needPreempt
+       cmp     r0, #0
+       ldmeqfd sp!, {r0 - r3, ip, pc}^
+
+       /* Otherwise restore regs used by the ISR */
+       ldmfd   sp!, {r0 - r3, ip, lr}
+
+       /* Save current process context */
+       msr     cpsr_c, #ARM_SVC_MODE
+       stmfd   sp!, {r0 - r3, ip, lr}
 
-       /*
-        * To work correctly the lr should point to proc_exit, so when process finish
-        * could be return
-        */
-       push {lr}                  /* Add one element on stack, for next switching */
+       /* Save lr_irq and spsr_irq in process stack */
+       msr     cpsr_c, #ARM_IRQ_MODE
+       mov     r0, lr
+       mrs     r1, spsr
+       msr     cpsr_c, #ARM_SVC_MODE
+       stmfd   sp!, {r0, r1}
 
-       stmfd   sp!, {r2, r4-r11, lr}  /* Save registers. */
+       /* Perform the context switch */
+       bl      proc_preempt
 
-       str     sp, [r1]               /* Save old stack pointer. */
-       ldr     sp, [r0]               /* Load new stack pointer */
+       /* Restore lr_irq and spsr_irq from process stack */
+       ldmfd   sp!, {r0, r1}
+       msr     cpsr_c, #ARM_IRQ_MODE
+       mov     lr, r0
+       msr     spsr_cxsf, r1
 
-       ldmfd   sp!, {r2, r4-r11, lr}  /* Load new registers. */
-       msr     cpsr, r2               /* restore flags reg. */
+       /* Restore process regs */
+       msr     cpsr_c, #ARM_SVC_MODE
+       ldmfd   sp!, {r0 - r3, ip, lr}
 
-       pop {r0}                  /* pop on stack address of the process that we want to jump */
-       mov     pc, r0
+       /* Exit from ISR */
+       msr     cpsr_c, #ARM_IRQ_MODE
+       movs    pc, lr
+#endif /* CONFIG_KERN_PREEMPT */