From f790a5f8744d2adf354fe4ca030c32df0904985b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E9=9B=B6=E4=B8=AD=E6=96=AD=E5=BB=B6=E8=BF=9F=E7=9A=84RTOS?=
 <cosyos@139.com>
Date: Sat, 20 Apr 2024 15:20:15 +0000
Subject: [PATCH] update System/os_handler.c.
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: 零中断延迟的RTOS <cosyos@139.com>
---
 System/os_handler.c | 87 +++++++++++++++++++++++++++++++++------------
 1 file changed, 65 insertions(+), 22 deletions(-)

diff --git a/System/os_handler.c b/System/os_handler.c
index ebf0e01..988a481 100644
--- a/System/os_handler.c
+++ b/System/os_handler.c
@@ -3,8 +3,8 @@
  * @file     os_handler.c
  * @brief    SysTick2_Handler and PendSV2_Handler
  * @author   迟凯峰
- * @version  V2.2.0
- * @date     2024.04.18
+ * @version  V2.2.1
+ * @date     2024.04.20
  ******************************************************************************/
 
 #include "os_link.h"
@@ -756,12 +756,13 @@ void PendSV2_Handler(void) MCUCFG_PENDSV_ATTRIBUTE
 			MOV DPH, s_task_current
 			MOV A, SP
 			SUBB A, #?STACK-1
-			MOV R1, A
 			MOVX @DPTR, A
+			SUBB A, #6
+			MOV R1, A
 			
 			CLR C
-			MOV A, m_bsp_add
-			ADD A, s_task_current+1
+			MOV A, s_task_current+1
+			ADD A, m_bsp_add
 			MOV DPL, A
 			JNC LABLE_A0
 			INC DPH
@@ -776,6 +777,30 @@ LABLE_A0:	MOVX A, @DPTR
 			
 			MOV R0, #?STACK
 			
+			MOV A, @R0
+			MOVX @DPTR, A
+			INC R0
+			INC DPTR
+			MOV A, @R0
+			MOVX @DPTR, A
+			INC R0
+			INC DPTR
+			MOV A, @R0
+			MOVX @DPTR, A
+			INC R0
+			INC DPTR
+			MOV A, @R0
+			MOVX @DPTR, A
+			INC R0
+			INC DPTR
+			MOV A, @R0
+			MOVX @DPTR, A
+			INC R0
+			INC DPTR
+			MOV A, @R0
+			MOVX @DPTR, A
+			INC R0
+			INC DPTR
 LABLE_A1:	MOV A, @R0
 			MOVX @DPTR, A
 			INC R0
@@ -783,13 +808,14 @@ LABLE_A1:	MOV A, @R0
 			DJNZ R1, LABLE_A1
 			
 			#if MCUCFG_SYSTICKREGBANK
-			MOV A, m_reg_add
-			ADD A, s_task_current+1
+			MOV A, s_task_current+1
+			ADD A, m_reg_add
 			MOV DPL, A
-			JNC LABLE_A2
-			INC DPH
+			CLR A
+			ADDC A, s_task_current
+			MOV DPH, A
 			
-LABLE_A2:	MOV A, 0x00
+			MOV A, 0x00
 			MOVX @DPTR, A
 			INC DPTR
 			MOV A, 0x01
@@ -832,13 +858,12 @@ LABLE_A2:	MOV A, 0x00
 			MOV DPL, s_task_current+1
 			MOV DPH, s_task_current
 			MOVX A, @DPTR
+			SUBB A, #6
 			MOV R1, A
-			ADD A, #?STACK-1
-			MOV SP, A
 			
 			CLR C
-			MOV A, m_bsp_add
-			ADD A, s_task_current+1
+			MOV A, s_task_current+1
+			ADD A, m_bsp_add
 			MOV DPL, A
 			JNC LABLE_B0
 			INC DPH
@@ -851,22 +876,40 @@ LABLE_B0:	MOVX A, @DPTR
 			MOV DPH, R0
 			MOV DPL, A
 			
-			MOV R0, #?STACK
+			MOV SP, #?STACK-1
 			
+			MOVX A, @DPTR
+			PUSH ACC
+			INC DPTR
+			MOVX A, @DPTR
+			PUSH ACC
+			INC DPTR
+			MOVX A, @DPTR
+			PUSH ACC
+			INC DPTR
+			MOVX A, @DPTR
+			PUSH ACC
+			INC DPTR
+			MOVX A, @DPTR
+			PUSH ACC
+			INC DPTR
+			MOVX A, @DPTR
+			PUSH ACC
+			INC DPTR
 LABLE_B1:	MOVX A, @DPTR
-			MOV @R0, A
-			INC R0
+			PUSH ACC
 			INC DPTR
 			DJNZ R1, LABLE_B1
 			
 			#if MCUCFG_SYSTICKREGBANK
-			MOV A, m_reg_add
-			ADD A, s_task_current+1
+			MOV A, s_task_current+1
+			ADD A, m_reg_add
 			MOV DPL, A
-			JNC LABLE_B2
-			INC DPH
+			CLR A
+			ADDC A, s_task_current
+			MOV DPH, A
 			
-LABLE_B2:	MOVX A, @DPTR
+			MOVX A, @DPTR
 			MOV 0x00, A
 			INC DPTR
 			MOVX A, @DPTR