之前看過一篇盧曉銘寫的簡易RTOS設計，自己也實踐了一下，感覺多任務運行起來毫無壓力，在此做份筆記以備他日之需也爲他人提供一份參考
要想完成一個RTOS，我們核心任務是需要編寫任務調度。
所以，我們需要知道，任務到底什麼地方會被調度。
1. 我們開始OSStart();時，肯定需要調度一次任務。這樣才能進入第一個最高優先級就緒任務中。
2. 在任務中的OSTimeDly();延時函數中，我們需要進行任務調度。當前任務延時了，肯定就要換一個別的任務來運行呀。
3. 在中斷退出時，需要進行任務調度(該處主要指定時器中斷)，可以理解爲每個時鐘週期都在進行任務最高優先級別的檢驗。
任務狀態的標誌，我想我們可以用1bit來代表，0:代表任務掛起或不存在，1:代表任務就緒。
U32 OSRdyTbl; 這是一個32bit的任務就緒表，每一位代表任務的狀態.
/*在就緒表中登記任務*/
__inline void OSSetPrioRdy(u8 prio) //__inline是內聯函數，用在該處是爲了提高效率。關於內聯函數的詳情和使用，可以百度。
{
OSRdyTbl|= 1 << prio;
}
/*在就緒表中刪除任務*/
__inline void OSDelPrioRdy(u8 prio)
{
OSRdyTbl&= ~(1<<prio);
}
在每個任務調度前，肯定需要知道當前最高優先級的就緒任務是什麼，所以我們需要一個查找函數
/*在就緒表中查找更高級的就緒任務*/
__inline void OSGetHighRdy(void)
{
u8OSNextTaskPrio = 0; /*任務優先級*/
for(OSNextTaskPrio = 0; (OSNextTaskPrio < OS_TASKS) &&(!(OSRdyTbl&(0x01<<OSNextTaskPrio))); OSNextTaskPrio++ ); //注意for最後有個分號
OSPrioHighRdy= OSNextTaskPrio; //獲得最高優先級,OSPrioHighRdy是一個全局變量
}
根據分析，我們知道我們的簡易RTOS有3個地方會出現任務調度
首先是任務剛開始時.
我們先定義幾個全局變量:
1. OSRunning指示OS是否開始運行
2. OSPrioCur指示當前任務優先級
3. OSPrioHighRdy指示當前已就緒的最高優先級任務，由OSGetHighRdy函數更新
void OSStart(void)
{
if(OSRunning== 0) //OSRuning是一個全局變量
{
OSRunning= 1;
//先不忙講任務創建 OSTaskCreate(IdleTask, (void *)0,(u32 *)&IDELTASK_STK[31], IdelTask_Prio); //創建空閒任
OSGetHighRdy(); /*獲得最高級的就緒任務*/
OSPrioCur= OSPrioHighRdy; /*獲得最高優先級就緒任務ID*/
p_OSTCBCur= &TCB[OSPrioCur];
p_OSTCBHighRdy= &TCB[OSPrioHighRdy];
OSStartHighRdy(); //在彙編語句中
}
}
其次出現任務調度的地方是延時函數OSTimeDly()
在這個函數前，要先介紹一個結構體TCB,這是任務控制怪(Task Control Block)
每一個任務都有一個任務控制塊，這個控制塊記錄着任務的重要信息，由於該處是簡易OS設計，所以僅僅有兩種
typedef struct TaskCtrBlockHead /*任務控制塊數據結構*/
{
u32OSTCBStkPtr; /*保存任務棧頂地址*/
u32OSTCBDly; /*任務延時時鐘*/
}TaskCtrBlock;
#define OS_TASKS 32 //最多任務數
TaskCtrBlock TCB[OS_TASKS]; //定義任務TCB，由於最多有32個任務，所以該處定義32個TCB
void OSTimeDly(u32 ticks)
{
if(ticks> 0)
{
OS_ENTER_CRITICAL(); //進入臨界區
OSDelPrioRdy(OSPrioCur); //將任務掛起
TCB[OSPrioCur].OSTCBDly= ticks; //設置TCB中任務延時節拍數
OS_EXIT_CRITICAL(); //退出臨界區
OSSched(); //任務調度
}
}
/*任務切換*/
void OSSched(void)
{
OSGetHighRdy(); //找出任務就緒表中優先級最高的任務
if(OSPrioHighRdy!=OSPrioCur) //如果不是當前運行任務，進行任務調度
{
p_OSTCBCur= &TCB[OSPrioCur]; //以便在彙編中引用,可以理解爲用於將現在的任務環境保存在該指針指向的堆棧中
//直接把當前任務的堆棧指針保存到當前任務的TCB（TCB的OSTCBStkPtr）
p_OSTCBHighRdy= &TCB[OSPrioHighRdy]; //以便在彙編中引用,可以理解爲用於將該指針中的數據釋放出來，恢復指定任務環境
//直接把最高優先級就緒任務的TCB（TCB的OSTCBStkPtr）設爲當前任務的堆棧指針
OSPrioCur= OSPrioHighRdy; //更新OSPrio
OSCtxSw(); //調度任務,在彙編中引用
}
}
最後是在時鐘中斷中出現
由於每次時鐘中斷我們都需要解決任務時鐘延時問題，所以我們需要一個函數
/*定時器中斷對任務延時處理函數*/
void TicksInterrupt(void)
{
static u8i;
OSTime++;
for(i=0;i<OS_TASKS;i++)
{
if(TCB[i].OSTCBDly)
{
TCB[i].OSTCBDly--;
if(TCB[i].OSTCBDly==0) //延時時鐘到達
{
OSSetPrioRdy(i); //任務重新就緒
}
}
}
}
//系統時鐘中斷服務函數
void SysTick_Handler(void)
{
OS_ENTER_CRITICAL(); //進入臨界區
OSIntNesting++; //任務前套數
OS_EXIT_CRITICAL(); //退出臨界區
TicksInterrupt(); //
OSIntExit(); //在中斷中處理任務調度
}
void OSIntExit(void)
{
OS_ENTER_CRITICAL(); //進入臨界區
if(OSIntNesting> 0)
OSIntNesting--;
if(OSIntNesting== 0) //沒有中斷嵌套時，纔可以進行任務調度
{
OSGetHighRdy(); /*找出任務優先級最高的就緒任務*/
if(OSPrioHighRdy!=OSPrioCur) /*當前任務並非優先級最高的就緒任務*/
{
p_OSTCBCur= &TCB[OSPrioCur];
p_OSTCBHighRdy= &TCB[OSPrioHighRdy];
OSPrioCur= OSPrioHighRdy;
OSIntCtxSw(); /*中斷級任務調度,注意這裏和OSCtxSw不一樣，但是作用是一樣的*/
}
}
OS_EXIT_CRITICAL(); //退出臨界區
}
現在任務調度已經寫完了，那麼應該要創建任務了吧，這裏使用創建任務函數
/*任務創建*/
void OSTaskCreate(void (*Task)(void *parg), void *parg,u32 *p_Stack, u8 TaskID)
{
if(TaskID<= OS_TASKS)
{
*(p_Stack) = (u32)0x01000000L; /* xPSR */
*(--p_Stack) = (u32)Task; /* Entry Point of the task 任務入口地址 */
*(--p_Stack) = (u32)0xFFFFFFFEL; /* R14 (LR) (init value will */
*(--p_Stack) = (u32)0x12121212L; /* R12 */
*(--p_Stack) = (u32)0x03030303L; /* R3 */
*(--p_Stack) = (u32)0x02020202L; /* R2 */
*(--p_Stack) = (u32)0x01010101L; /* R1 */
*(--p_Stack) = (u32)parg; /* R0 : argument 輸入參數 */
*(--p_Stack) = (u32)0x11111111L; /* R11 */
*(--p_Stack) = (u32)0x10101010L; /* R10 */
*(--p_Stack) = (u32)0x09090909L; /* R9 */
*(--p_Stack) = (u32)0x08080808L; /* R8 */
*(--p_Stack) = (u32)0x07070707L; /* R7 */
*(--p_Stack) = (u32)0x06060606L; /* R6 */
*(--p_Stack) = (u32)0x05050505L; /* R5 */
*(--p_Stack) = (u32)0x04040404L; /* R4 */
TCB[TaskID].OSTCBStkPtr= (u32)p_Stack; /*保存堆棧地址*/
TCB[TaskID].OSTCBDly= 0; /*初始化任務延時*/
OSSetPrioRdy(TaskID); /*在任務就緒表中登記*/
}
}
這裏主要是入棧寄存器地址
爲了保證系統的正常運行，我們需要一個空閒任務，空閒任務可以什麼事情都不做,也可以隨便做點什麼簡單的事。
/*系統空閒任務*/
void IdleTask(void *pdata)
{
u32IdleCount = 0;
while(1)
{
IdleCount++;
}
}
既然如此，那麼系統開始前應該申請一個空閒任務，所以OSStart()函數改爲
void OSStart(void)
{
if(OSRunning== 0)
{
OSRunning= 1;
OSTaskCreate(IdleTask,(void *)0, (u32 *)&IDELTASK_STK[31], IdelTask_Prio); //創建空閒任務
OSGetHighRdy(); /*獲得最高級的就緒任務*/
OSPrioCur= OSPrioHighRdy; /*獲得最高優先級就緒任務ID*/
p_OSTCBCur= &TCB[OSPrioCur];
p_OSTCBHighRdy= &TCB[OSPrioHighRdy];
OSStartHighRdy();
}
}
以上就是任務調度器的核心.
至於彙編代碼，可以直接參考ucosii在STM32上的彙編代碼。
文件RTOS.c
/*********************** (C) COPYRIGHT 2013 Libraworks *************************
* File Name : RTOS.c
* Author : 盧曉銘
* Version : V1.0
* Date : 01/26/2013
* Description : LXM-RTOS 任務管理
*******************************************************************************/
#include"RTOS.h"
TaskCtrBlock TCB[OS_TASKS - 1]; /*任務控制塊定義*/
TaskCtrBlock *p_OSTCBCur; /*指向當前任務控制塊的指針*/
TaskCtrBlock *p_OSTCBHighRdy; /*指向最高優先級就緒任務控制塊的指針*/
u8 OSPrioCur; /*當前執行任務*/
u8 OSPrioHighRdy; /*最高優先級*/
u8 OSRunning; /*多任務運行標誌0:爲運行，1:運行*/
u32 OSInterruptSum; /*進入中斷次數*/
u32 OSTime; /*系統時間(進入時鐘中斷次數)*/
u32 OSRdyTbl; /*任務就緒表,0表示掛起,1表示就緒*/
u32 OSIntNesting; /*任務嵌套數*/
/*在就緒表中登記任務*/
void OSSetPrioRdy(u8 prio)
{
OSRdyTbl |= 1 << prio;
}
/*在就緒表中刪除任務*/
void OSDelPrioRdy(u8 prio)
{
OSRdyTbl &= ~(1<<prio);
}
/*在就緒表中查找更高級的就緒任務*/
void OSGetHighRdy(void)
{
u8 OSNextTaskPrio = 0; /*任務優先級*/
for (OSNextTaskPrio = 0; (OSNextTaskPrio < OS_TASKS) && (!(OSRdyTbl&(0x01<<OSNextTaskPrio))); OSNextTaskPrio++ );
OSPrioHighRdy = OSNextTaskPrio;
}
/*設置任務延時時間*/
void OSTimeDly(u32 ticks)
{
if(ticks > 0)
{
OS_ENTER_CRITICAL(); //進入臨界區
OSDelPrioRdy(OSPrioCur); //將任務掛起
TCB[OSPrioCur].OSTCBDly = ticks; //設置TCB中任務延時節拍數
OS_EXIT_CRITICAL(); //退出臨界區
OSSched();
}
}
/*定時器中斷對任務延時處理函數*/
void TicksInterrupt(void)
{
static u8 i;
OSTime++;
for(i=0;i<OS_TASKS;i++)
{
if(TCB[i].OSTCBDly)
{
TCB[i].OSTCBDly--;
if(TCB[i].OSTCBDly==0) //延時時鐘到達
{
OSSetPrioRdy(i); //任務重新就緒
}
}
}
}
/*任務切換*/
void OSSched(void)
{
OSGetHighRdy(); //找出任務就緒表中優先級最高的任務
if(OSPrioHighRdy!=OSPrioCur) //如果不是當前運行任務，進行任務調度
{
p_OSTCBCur = &TCB[OSPrioCur]; //彙編中引用
p_OSTCBHighRdy = &TCB[OSPrioHighRdy]; //彙編中引用
OSPrioCur = OSPrioHighRdy; //更新OSPrio
OSCtxSw(); //調度任務
}
}
/*任務創建*/
void OSTaskCreate(void (*Task)(void *parg), void *parg, u32 *p_Stack, u8 TaskID)
{
if(TaskID <= OS_TASKS)
{
*(p_Stack) = (u32)0x01000000L; /* xPSR */
*(--p_Stack) = (u32)Task; /* Entry Point of the task 任務入口地址 */
*(--p_Stack) = (u32)0xFFFFFFFEL; /* R14 (LR) (init value will */
*(--p_Stack) = (u32)0x12121212L; /* R12 */
*(--p_Stack) = (u32)0x03030303L; /* R3 */
*(--p_Stack) = (u32)0x02020202L; /* R2 */
*(--p_Stack) = (u32)0x01010101L; /* R1 */
*(--p_Stack) = (u32)parg; /* R0 : argument 輸入參數 */
*(--p_Stack) = (u32)0x11111111L; /* R11 */
*(--p_Stack) = (u32)0x10101010L; /* R10 */
*(--p_Stack) = (u32)0x09090909L; /* R9 */
*(--p_Stack) = (u32)0x08080808L; /* R8 */
*(--p_Stack) = (u32)0x07070707L; /* R7 */
*(--p_Stack) = (u32)0x06060606L; /* R6 */
*(--p_Stack) = (u32)0x05050505L; /* R5 */
*(--p_Stack) = (u32)0x04040404L; /* R4 */
TCB[TaskID].OSTCBStkPtr = (u32)p_Stack; /*保存堆棧地址*/
TCB[TaskID].OSTCBDly = 0; /*初始化任務延時*/
OSSetPrioRdy(TaskID); /*在任務就緒表中登記*/
}
}
void OSTaskSuspend(u8 prio)
{
OS_ENTER_CRITICAL(); /*進入臨界區*/
TCB[prio].OSTCBDly = 0;
OSDelPrioRdy(prio); /*掛起任務*/
OS_EXIT_CRITICAL(); /*退出臨界區*/
if(OSPrioCur == prio) /*掛起的任務爲當前運行的任務*/
{
OSSched(); /*重新調度*/
}
}
void OSTaskResume(u8 prio)
{
OS_ENTER_CRITICAL();
TCB[prio].OSTCBDly = 0; /*設置任務延時時間爲0*/
OSSetPrioRdy(prio); /*就緒任務*/
OS_EXIT_CRITICAL();
if(OSPrioCur > prio) /*當前任務優先級小於恢復的任務優先級*/
{
OSSched();
}
}
u32 IDELTASK_STK[32];
void OSStart(void)
{
if(OSRunning == 0)
{
OSRunning = 1;
OSTaskCreate(IdleTask, (void *)0, (u32 *)&IDELTASK_STK[31], IdelTask_Prio); //創建空閒任務
OSGetHighRdy(); /*獲得最高級的就緒任務*/
OSPrioCur = OSPrioHighRdy; /*獲得最高優先級就緒任務ID*/
p_OSTCBCur = &TCB[OSPrioCur];
p_OSTCBHighRdy = &TCB[OSPrioHighRdy];
OSStartHighRdy();
}
}
void OSIntExit(void)
{
OS_ENTER_CRITICAL();
if(OSIntNesting > 0)
OSIntNesting--;
if(OSIntNesting == 0)
{
OSGetHighRdy(); /*找出任務優先級最高的就緒任務*/
if(OSPrioHighRdy!=OSPrioCur) /*當前任務並非優先級最高的就緒任務*/
{
p_OSTCBCur = &TCB[OSPrioCur];
p_OSTCBHighRdy = &TCB[OSPrioHighRdy];
OSPrioCur = OSPrioHighRdy;
OSIntCtxSw(); /*中斷級任務調度*/
}
}
OS_EXIT_CRITICAL();
}
/*系統空閒任務*/
void IdleTask(void *pdata)
{
u32 IdleCount = 0;
while(1)
{
IdleCount++;
}
}
void OSTaskSwHook(void)
{
}
;/*********************** (C) COPYRIGHT 2013 Libraworks *************************
;* File Name : RTOS.h
;* Author : 盧曉銘
;* Version : V1.0
;* Date : 01/26/2013
;* Description : LXM-RTOS asm port
;*******************************************************************************/
#ifndef __RTOS_H
#define __RTOS_H
#include "stm32f10x.h"//加入頭文件
typedef struct TaskCtrBlockHead /*任務控制塊數據結構*/
{
u32 OSTCBStkPtr; /*保存任務棧頂*/
u32 OSTCBDly; /*任務延時時鐘*/
}TaskCtrBlock;
#define OS_TASKS 32 /*總任務數*/
#define IdelTask_Prio 31 /*空閒任務優先級*/
extern TaskCtrBlock TCB[OS_TASKS - 1]; /*任務控制塊定義*/
extern TaskCtrBlock *p_OSTCBCur; /*指向當前任務控制塊的指針*/
extern TaskCtrBlock *p_OSTCBHighRdy; /*指向最高優先級就緒任務控制塊的指針*/
extern u8 OSPrioCur; /*當前執行任務*/
extern u8 OSPrioHighRdy; /*最高優先級*/
extern u8 OSRunning; /*多任務運行標誌0:爲運行，1:運行*/
extern u32 OSInterruptSum; /*進入中斷次數*/
extern u32 OSTime; /*系統時間(進入時鐘中斷次數)*/
extern u32 OSRdyTbl; /*任務就緒表,0表示掛起,1表示就緒*/
extern u32 OSIntNesting; /*任務嵌套數*/
void OSTimeDly(u32 ticks); /*設置任務延時時間*/
void TicksInterrupt(void); /*定時器中斷對任務延時處理函數*/
void IdleTask(void *pdata); /*系統空閒任務*/
void OSSched(void); /*任務切換*/
void OSStart(void); /*多任務系統開始*/
void OSIntExit(void); /*中斷退出函數*/
void OSTaskCreate(void (*Task)(void *parg), void *parg, u32 *p_Stack, u8 TaskID); /*創建任務函數*/
void OSTaskSuspend(u8 prio); /*掛起指定任務*/
void OSTaskResume(u8 prio); /*回覆指定的掛起任務*/
void OSTaskSwHook(void); /*空函數*/
/*in asm function*/
void OS_EXIT_CRITICAL(void); /*退出臨界區*/
void OS_ENTER_CRITICAL(void); /*進入臨界區*/
void OSStartHighRdy(void); /*調度第一個任務*/
void OSCtxSw(void); /*函數級任務切換*/
void OSIntCtxSw(void); /*中斷級任務切換*/
#endif
文件RTOS_ASM.s
;/*********************** (C) COPYRIGHT 2013 Libraworks *************************
;* File Name : RTOS_ASM.s
;* Author : 盧曉銘
;* Version : V1.0
;* Date : 01/26/2013
;* Description : LXM-RTOS asm port
;*******************************************************************************/
IMPORT OSInterruptSum
IMPORT OSRunning
IMPORT p_OSTCBCur
IMPORT p_OSTCBHighRdy
IMPORT OSTaskSwHook
IMPORT OSPrioCur
IMPORT OSPrioHighRdy
EXPORT OS_ENTER_CRITICAL
EXPORT OS_EXIT_CRITICAL
EXPORT OSStartHighRdy
EXPORT OSCtxSw
EXPORT OSIntCtxSw
EXPORT PendSV_Handler
NVIC_INT_CTRL EQU 0xE000ED04 ; 中斷控制寄存器
NVIC_SYSPRI2 EQU 0xE000ED20 ; 系統優先級寄存器(2)
NVIC_PENDSV_PRI EQU 0xFFFF0000 ; 軟件中斷和系統節拍中斷
; (都爲最低，0xff).
NVIC_PENDSVSET EQU 0x10000000 ; 觸發軟件中斷的值.
PRESERVE8
SECTION .text:CODE:NOROOT(2)
THUMB
;/***************************************************************************************
;* 函數名稱: OS_ENTER_CRITICAL
;*
;* 功能描述: 進入臨界區
;*
;* 參 數: None
;*
;* 返 回 值: None
;*****************************************************************************************/
OS_ENTER_CRITICAL
CPSID I ; Disable all the interrupts
PUSH {R1,R2}
LDR R1, =OSInterruptSum ; OSInterrputSum++
LDRB R2, [R1]
ADD R2, R2, #1
STRB R2, [R1]
POP {R1,R2}
BX LR
;/***************************************************************************************
;* 函數名稱: OS_EXIT_CRITICAL
;*
;* 功能描述: 退出臨界區
;*
;* 參 數: None
;*
;* 返 回 值: None
;*****************************************************************************************/
OS_EXIT_CRITICAL
PUSH {R1, R2}
LDR R1, =OSInterruptSum ; OSInterrputSum--
LDRB R2, [R1]
SUB R2, R2, #1
STRB R2, [R1]
MOV R1, #0
CMP R2, #0 ; if OSInterrputSum=0,enable
; interrupts如果OSInterrputSum=0，
;MSREQ PRIMASK, R1
CPSIE I
POP {R1, R2}
BX LR
;/**************************************************************************************
;* 函數名稱: OSStartHighRdy
;*
;* 功能描述: 使用調度器運行第一個任務
;*
;* 參 數: None
;*
;* 返 回 值: None
;**************************************************************************************/
OSStartHighRdy
LDR R4, =NVIC_SYSPRI2 ; set the PendSV exception priority
LDR R5, =NVIC_PENDSV_PRI
STR R5, [R4]
MOV R4, #0 ; set the PSP to 0 for initial context switch call
MSR PSP, R4
LDR R4, =OSRunning ; OSRunning = TRUE
MOV R5, #1
STRB R5, [R4]
;切換到最高優先級的任務
LDR R4, =NVIC_INT_CTRL ;rigger the PendSV exception (causes context switch)
LDR R5, =NVIC_PENDSVSET
STR R5, [R4]
CPSIE I ;enable interrupts at processor level
OSStartHang
B OSStartHang ;should never get here
;/**************************************************************************************
;* 函數名稱: OSCtxSw
;*
;* 功能描述: 函數級任務切換
;*
;* 參 數: None
;*
;* 返 回 值: None
;***************************************************************************************/
OSCtxSw
PUSH {R4, R5}
LDR R4, =NVIC_INT_CTRL ;觸發PendSV異常 (causes context switch)
LDR R5, =NVIC_PENDSVSET
STR R5, [R4]
POP {R4, R5}
BX LR
;/**************************************************************************************
;* 函數名稱: OSIntCtxSw
;*
;* 功能描述: 中斷級任務切換
;*
;* 參 數: None
;*
;* 返 回 值: None
;***************************************************************************************/
OSIntCtxSw
PUSH {R4, R5}
LDR R4, =NVIC_INT_CTRL ;觸發PendSV異常 (causes context switch)
LDR R5, =NVIC_PENDSVSET
STR R5, [R4]
POP {R4, R5}
BX LR
NOP
;/**************************************************************************************
;* 函數名稱: OSPendSV
;*
;* 功能描述: OSPendSV is used to cause a context switch.
;*
;* 參 數: None
;*
;* 返 回 值: None
;***************************************************************************************/
PendSV_Handler
CPSID I ; Prevent interruption during context switch
MRS R0, PSP ; PSP is process stack pointer 如果在用PSP堆棧,則可以忽略保存寄存器,參考CM3權威中的雙堆棧-白菜注
CBZ R0, PendSV_Handler_Nosave ; Skip register save the first time
SUBS R0, R0, #0x20 ; Save remaining regs r4-11 on process stack
STM R0, {R4-R11}
LDR R1, =p_OSTCBCur ; OSTCBCur->OSTCBStkPtr = SP;
LDR R1, [R1]
STR R0, [R1] ; R0 is SP of process being switched out
; At this point, entire context of process has been saved
PendSV_Handler_Nosave
PUSH {R14} ; Save LR exc_return value
LDR R0, =OSTaskSwHook ; OSTaskSwHook();
BLX R0
POP {R14}
LDR R0, =OSPrioCur ; OSPrioCur = OSPrioHighRdy;
LDR R1, =OSPrioHighRdy
LDRB R2, [R1]
STRB R2, [R0]
LDR R0, =p_OSTCBCur ; OSTCBCur = OSTCBHighRdy;
LDR R1, =p_OSTCBHighRdy
LDR R2, [R1]
STR R2, [R0]
LDR R0, [R2] ; R0 is new process SP; SP = OSTCBHighRdy->OSTCBStkPtr;
LDM R0, {R4-R11} ; Restore r4-11 from new process stack
ADDS R0, R0, #0x20
MSR PSP, R0 ; Load PSP with new process SP
ORR LR, LR, #0x04 ; Ensure exception return uses process stack
CPSIE I
BX LR ; Exception return will restore remaining context
;************************************************************
END