debug Cortex-M3 PendSV

[hos/hos-v4a.git] / kernel / source / arch / proc / arm / arm_v7m / gcc / kpnd_hdr.S

diff --git a/kernel/source/arch/proc/arm/arm_v7m/gcc/kpnd_hdr.S b/kernel/source/arch/proc/arm/arm_v7m/gcc/kpnd_hdr.S
index 6bb4af9..fc0089d 100644 (file)
--- a/kernel/source/arch/proc/arm/arm_v7m/gcc/kpnd_hdr.S
+++ b/kernel/source/arch/proc/arm/arm_v7m/gcc/kpnd_hdr.S
@@ -24,31 +24,54 @@ _kernel_pendsv_handler:
                                /* 割り込み禁止 */
                                cpsid   i
 
-                               /* スレッドモードへ */
-                               mrs             r0, control
-                               bic             r0, r0, #1
-                               msr             control, r0
-                               isb
+                               /* リターンアドレス変更 */
+                               mrs             r0, psp
+                               ldr             r1, [r0, #24]
+                               movw    r2, #:lower16:pendsv_ret_addr
+                               movt    r2, #:upper16:pendsv_ret_addr
+                               str             r1, [r2]
+
+                               movw    r1, #:lower16:_kernel_pendsv_proc
+                               movt    r1, #:upper16:_kernel_pendsv_proc
+                               str             r1, [r0, #24]
+
+                               /* スレッドモードへの復帰 */
+                               bx              lr
+
+                               .size   _kernel_pendsv_handler, .-_kernel_pendsv_handler
+
+
+                               .thumb_func
+                               .type   _kernel_pendsv_proc, %function
+_kernel_pendsv_proc:
+                               push    {r0}
+                               push    {r0, r1, r2, r3, r12, lr}
+
+                               mrs             r0, apsr
+                               push    {r0}
+
+                               /* リターンアドレス格納 */
+                               movw    r0, #:lower16:pendsv_ret_addr
+                               movt    r0, #:upper16:pendsv_ret_addr
+                               ldr             r1, [r0]
+                               orr             r1, r1, #1
+                               str             r1, [sp, #28]
 
                                /* 遅延ディスパッチ */
                                blx             _kernel_dsp_tsk
 
-                               /* ハンドラモードへ */
-                               mrs             r0, control
-                               orr             r0, r0, #1
-                               msr             control, r0
-                               isb
+                               pop             {r0}
+                               msr             apsr, r0
 
-                               /* 割り込み許可 */
                                cpsie   i
 
-                               /* スレッドモードへの復帰 */
-                               movw    lr, #0xfffd
-                               movt    lr, #0xffff
-                               bx              lr
+                               pop             {r0, r1, r2, r3, r12, lr}
+                               pop             {pc}
+
+                               .size   _kernel_pendsv_proc, .-_kernel_pendsv_proc
 
-                               .size   _kernel_pendsv_handler, .-_kernel_pendsv_handler
 
+                               .comm   pendsv_ret_addr, 4, 4
 
                                .end