X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;ds=sidebyside;f=bertos%2Fcpu%2Farm%2Fhw%2Fswitch_ctx_arm.S;h=db4f297ff71e93c7d3157e2591546131bbecb877;hb=32d1445272120a254d77ce8d1af1f527da7a2c17;hp=b75a3c10eff9fee6838334054e249b72bee338cc;hpb=fe0a14d1434098bfd0780d06a2a7e55f27940d27;p=bertos.git

diff --git a/bertos/cpu/arm/hw/switch_ctx_arm.S b/bertos/cpu/arm/hw/switch_ctx_arm.S
index b75a3c10..db4f297f 100644
--- a/bertos/cpu/arm/hw/switch_ctx_arm.S
+++ b/bertos/cpu/arm/hw/switch_ctx_arm.S
@@ -35,30 +35,65 @@
  *
  * \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}^
 
-	stmfd	sp!, {r2, r4-r11, lr}  /* Save registers. */
+	/* Otherwise restore regs used by the ISR */
+	ldmfd   sp!, {r0 - r3, ip, lr}
 
-	str     sp, [r1]               /* Save old stack pointer. */
-	ldr	sp, [r0]               /* Load new stack pointer */
+	/* Save current process context */
+	msr     cpsr_c, #ARM_SVC_MODE
+	stmfd   sp!, {r0 - r3, ip, lr}
 
-	ldmfd	sp!, {r2, r4-r11, lr}  /* Load new registers. */
-	msr	cpsr, r2               /* restore flags reg. */
+	/* 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}
 
-	mov	pc, lr
+	/* Perform the context switch */
+	bl	proc_preempt
 
+	/* 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
 
-/* proc_entry trampoline needed because ARM does not pop return addresses
-from the stack, but uses lr instead.*/
-.globl asm_proc_entry
-asm_proc_entry:
-	mov	lr, pc
-	/* In r11 we have the real process entry point as set up by CPU_CREATE_NEW_STACK */
-	bx	r11
-	bl	proc_exit
+	/* Restore process regs */
+	msr     cpsr_c, #ARM_SVC_MODE
+	ldmfd   sp!, {r0 - r3, ip, lr}
 
+	/* Exit from ISR */
+	msr     cpsr_c, #ARM_IRQ_MODE
+	movs	pc, lr
+#endif /* CONFIG_KERN_PREEMPT */