|
|
|
@ -25,318 +25,55 @@ void start_first_task(void) |
|
|
|
__ISB(); |
|
|
|
} |
|
|
|
|
|
|
|
#define STACK_SIZE 512 // 姣忎釜浠诲姟鐨勬爤澶у皬锛堟寜闇€璋冩暣锛�
|
|
|
|
tcb_t *task[2]; |
|
|
|
|
|
|
|
void init_task(tcb_t *tcb, void (*task_entry)(void *), void *arg) |
|
|
|
uint8_t tcb_cunt = 0; |
|
|
|
void tcb_scheduler(void) |
|
|
|
{ |
|
|
|
if(tcb_cunt >= 1) |
|
|
|
{ |
|
|
|
tcb_cunt = -1; |
|
|
|
} |
|
|
|
tcb_cunt++; |
|
|
|
next_task = task[tcb_cunt]; |
|
|
|
SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; |
|
|
|
__DSB(); |
|
|
|
__ISB(); |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void init_task(tcb_t *tcb, void (*task_entry)(void *), uint8_t *start_stack, void *arg) |
|
|
|
{ |
|
|
|
// 栈顶对齐到 8 字节（硬件要求）
|
|
|
|
static uint8_t tcb_id = 0; |
|
|
|
task[tcb_id] = tcb; |
|
|
|
tcb_id++; |
|
|
|
tcb->task_id = tcb_id; |
|
|
|
tcb->stack_ptr = (uint32_t *)start_stack; |
|
|
|
uint32_t *stack_top = (uint32_t*)((uint8_t*)tcb->stack_ptr + STACK_SIZE); |
|
|
|
stack_top = (uint32_t*)((uint32_t)stack_top & ~0x07); |
|
|
|
|
|
|
|
// 鏋勯€犲垵濮嬫爤甯э紙妯℃嫙寮傚父閫€鍑烘椂鐨勪笂涓嬫枃锛�
|
|
|
|
*(--stack_top) = 0x01000000; // xPSR (Thumb 模式)
|
|
|
|
*(--stack_top) = (uint32_t)task_entry; // PC (浠诲姟鍏ュ彛)
|
|
|
|
*(--stack_top) = 0xDEADBEEF; // LR (閫氬父鏃犳剰涔夛紝浣嗛渶鍗犱綅)
|
|
|
|
*(--stack_top) = (uint32_t)task_entry; // PC
|
|
|
|
*(--stack_top) = 0xDEADBEEF; // LR
|
|
|
|
*(--stack_top) = 0x0000000C; // R12
|
|
|
|
*(--stack_top) = 0x00000003; // R3
|
|
|
|
*(--stack_top) = 0x00000002; // R2
|
|
|
|
*(--stack_top) = (uint32_t)arg;// R1 (鍙傛暟)
|
|
|
|
*(--stack_top) = 0x00000000; // R0
|
|
|
|
*(--stack_top) = 0x00000000; // R1 (参数)
|
|
|
|
*(--stack_top) = (uint32_t)arg; // R0
|
|
|
|
|
|
|
|
// 鎵嬪姩淇濆瓨鐨勫瘎瀛樺櫒锛圧4-R11锛夊垵濮嬪€硷紙閫氬父涓�0锛�
|
|
|
|
for (int i = 0; i < 8; i++) |
|
|
|
{ |
|
|
|
*(--stack_top) = 0x00000000; // R4-R11
|
|
|
|
*(--stack_top) = 0x12345678; // R4-R11
|
|
|
|
} |
|
|
|
|
|
|
|
// 淇濆瓨鏍堥《鍒� TCB
|
|
|
|
tcb->stack_ptr = stack_top; |
|
|
|
} |
|
|
|
|
|
|
|
#if 0 |
|
|
|
void trigger_context_switch(void) |
|
|
|
printf("init_tch:%d\r\n", tcb_id); |
|
|
|
if(tcb_id == 1) |
|
|
|
{ |
|
|
|
SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; // ?? PendSV ???
|
|
|
|
__DSB(); // ??????
|
|
|
|
__ISB(); // ??????
|
|
|
|
printf("start_tch:%d\r\n", tcb_id); |
|
|
|
next_task = tcb; |
|
|
|
} |
|
|
|
|
|
|
|
#define STACK_SIZE 32 |
|
|
|
|
|
|
|
|
|
|
|
struct exception_stack_frame |
|
|
|
{ |
|
|
|
uint32_t r0; |
|
|
|
uint32_t r1; |
|
|
|
uint32_t r2; |
|
|
|
uint32_t r3; |
|
|
|
uint32_t r12; |
|
|
|
uint32_t lr; |
|
|
|
uint32_t pc; |
|
|
|
uint32_t psr; |
|
|
|
}; |
|
|
|
|
|
|
|
typedef struct |
|
|
|
{ |
|
|
|
/* r4 ~ r11 register */ |
|
|
|
uint32_t r4; |
|
|
|
uint32_t r5; |
|
|
|
uint32_t r6; |
|
|
|
uint32_t r7; |
|
|
|
uint32_t r8; |
|
|
|
uint32_t r9; |
|
|
|
uint32_t r10; |
|
|
|
uint32_t r11; |
|
|
|
struct exception_stack_frame exception_stack_frame; |
|
|
|
}stack_frame_t; |
|
|
|
|
|
|
|
|
|
|
|
struct Thread { |
|
|
|
unsigned long ip; |
|
|
|
unsigned int *sp; |
|
|
|
}; |
|
|
|
|
|
|
|
typedef struct PCB { |
|
|
|
struct Thread thread; |
|
|
|
int pid; |
|
|
|
volatile long state; /* -1 idle, 0 runnable */ |
|
|
|
uint32_t stack[STACK_SIZE]; |
|
|
|
struct PCB *next; |
|
|
|
} tPCB; |
|
|
|
|
|
|
|
static tPCB task[MAX_TASK_NUM]; |
|
|
|
static tPCB *current_task = NULL; |
|
|
|
static uint32_t *current_task_sp = NULL; |
|
|
|
static uint32_t *next_task_sp = NULL; |
|
|
|
|
|
|
|
|
|
|
|
void tTaskRunFirst(void); |
|
|
|
void tTaskSwitch(void); |
|
|
|
void tTask_schedule(void); |
|
|
|
|
|
|
|
void my_process(void) |
|
|
|
{ |
|
|
|
uint32_t i = 0; |
|
|
|
while (1) { |
|
|
|
i++; |
|
|
|
if (i % 1000000 == 0) { |
|
|
|
printf("this is process %d - \r\n", current_task->pid); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#define RT_ALIGN_DOWN(size, align) ((size) & ~((align) - 1)) |
|
|
|
void StackInit(tPCB * task, void (*entry)(void), unsigned int * stack_addr) |
|
|
|
{ |
|
|
|
stack_frame_t * stack_frame; |
|
|
|
uint32_t *stk; |
|
|
|
unsigned long i; |
|
|
|
|
|
|
|
stk = stack_addr + sizeof(uint32_t); |
|
|
|
stk = (uint32_t *)RT_ALIGN_DOWN((uint32_t)stk, 8); |
|
|
|
stk -= sizeof(stack_frame_t); |
|
|
|
stack_frame = (stack_frame_t *)stk; |
|
|
|
|
|
|
|
/* init all register */ |
|
|
|
for (i = 0; i < sizeof(stack_frame_t) / sizeof(uint32_t); i ++) |
|
|
|
{ |
|
|
|
((uint32_t *)stack_frame)[i] = 0xdeadbeef; |
|
|
|
} |
|
|
|
|
|
|
|
stack_frame->exception_stack_frame.r0 = 0; /* r0 : argument */ |
|
|
|
stack_frame->exception_stack_frame.r1 = 0; /* r1 */ |
|
|
|
stack_frame->exception_stack_frame.r2 = 0; /* r2 */ |
|
|
|
stack_frame->exception_stack_frame.r3 = 0; /* r3 */ |
|
|
|
stack_frame->exception_stack_frame.r12 = 0; /* r12 */ |
|
|
|
stack_frame->exception_stack_frame.lr = 0; /* lr */ |
|
|
|
stack_frame->exception_stack_frame.pc = (unsigned long)entry; /* entry point, pc */ |
|
|
|
stack_frame->exception_stack_frame.psr = 0x01000000L; /* PSR */ |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
void tTaskInit(int task_num) |
|
|
|
{ |
|
|
|
int i = 0; |
|
|
|
|
|
|
|
task[i].pid = i; |
|
|
|
task[i].state = 0; |
|
|
|
task[i].thread.ip = (unsigned long)my_process; |
|
|
|
task[i].thread.sp = &task[i].stack[STACK_SIZE - 1]; |
|
|
|
task[i].next = &task[i]; |
|
|
|
for (i = 1; i < task_num; i++) { |
|
|
|
task[i].pid = i; |
|
|
|
task[i].state = 0; |
|
|
|
task[i].thread.ip = (unsigned long)my_process; |
|
|
|
task[i].thread.sp = &task[i].stack[STACK_SIZE - 1]; |
|
|
|
task[i].next = task[i - 1].next; |
|
|
|
task[i - 1].next = &task[i]; |
|
|
|
StackInit(&task[i], my_process, task[i].stack); |
|
|
|
} |
|
|
|
current_task = &task[0]; |
|
|
|
current_task_sp = (uint32_t *)__get_MSP(); |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void tTask_schedule(void) |
|
|
|
{ |
|
|
|
current_task_sp = current_task->thread.sp; |
|
|
|
current_task = current_task->next; |
|
|
|
next_task_sp = current_task->thread.sp; |
|
|
|
SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk; |
|
|
|
__DSB(); |
|
|
|
__ISB(); |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
void tTaskRunFirst(void) |
|
|
|
{ |
|
|
|
tTask_schedule(); |
|
|
|
__enable_irq(); |
|
|
|
while(1); |
|
|
|
} |
|
|
|
|
|
|
|
// PendSV 异常处理函数
|
|
|
|
void PendSV_Handler(void) |
|
|
|
{ |
|
|
|
|
|
|
|
extern unsigned int *next_task_sp; |
|
|
|
extern unsigned int *current_task_sp; |
|
|
|
__asm volatile ( |
|
|
|
// 禁用中断（确保原子操作）
|
|
|
|
"CPSID I\n" |
|
|
|
|
|
|
|
// 加载 current_task 的地址到 R
|
|
|
|
"LDR R0, = current_task_sp\n" |
|
|
|
"STR R0, [R0]\n" |
|
|
|
|
|
|
|
// 保存当前任务的上下文
|
|
|
|
"MRS R1, MSP\n" // 将进程栈指针（MSP）保存到 R0
|
|
|
|
"STMDB R1!, {R4-R11}\n" // 手动保存 R4-R11
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 切换到下一个任务
|
|
|
|
"LDR R1, = next_task_sp\n" |
|
|
|
"LDR R0, [R1]\n" |
|
|
|
|
|
|
|
// 恢复下一个任务的上下文
|
|
|
|
"LDMIA R0!, {R4-R11}\n" // 手动恢复 R4-R11
|
|
|
|
"MSR MSP, R0\n" // 将恢复后的栈指针写入 PSP
|
|
|
|
|
|
|
|
// 启用中断
|
|
|
|
"CPSIE I\n" |
|
|
|
|
|
|
|
// 从异常返回
|
|
|
|
"BX LR\n" |
|
|
|
); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
void tTaskSwitch(void) |
|
|
|
{ |
|
|
|
extern tPCB *current_task; |
|
|
|
extern tPCB *next_task; |
|
|
|
|
|
|
|
__asm volatile ( |
|
|
|
"CPSID I\n" |
|
|
|
// R0 = current_task->thread.ip
|
|
|
|
"LDR r0, =current_task\n" // 加载 current_task 的地址到 r0
|
|
|
|
"LDR r0, [r0]\n" // 加载 current_task 的值到 r0
|
|
|
|
"LDR r0, [r0, #4]\n" // 加载 current_task->thread.ip 到 r0
|
|
|
|
|
|
|
|
// PUSH {LR}
|
|
|
|
"STMDB r0!, {lr}\n" // 将 LR（R14）压栈，并更新 r0
|
|
|
|
|
|
|
|
// 刷新 current_task->thread.ip
|
|
|
|
"LDR r1, =current_task\n" // 加载 current_task 的地址到 r1
|
|
|
|
"LDR r1, [r1]\n" // 加载 current_task 的值到 r1
|
|
|
|
"STR r0, [r1, #4]\n" // 将更新后的 r0 存储到 current_task->thread.ip
|
|
|
|
|
|
|
|
// current_task = next_task
|
|
|
|
"LDR r0, =current_task\n" // 加载 current_task 的地址到 r0
|
|
|
|
"LDR r1, =next_task\n" // 加载 next_task 的地址到 r1
|
|
|
|
"LDR r1, [r1]\n" // 加载 next_task 的值到 r1
|
|
|
|
"STR r1, [r0]\n" // 将 next_task 的值存储到 current_task
|
|
|
|
|
|
|
|
// R0 = next_task->thread.ip
|
|
|
|
"LDR r0, =next_task\n" // 加载 next_task 的地址到 r0
|
|
|
|
"LDR r0, [r0]\n" // 加载 next_task 的值到 r0
|
|
|
|
"LDR r0, [r0, #4]\n" // 加载 next_task->thread.ip 到 r0
|
|
|
|
|
|
|
|
// POP {LR}
|
|
|
|
"LDMIA r0!, {lr}\n" // 从 r0 指向的地址加载 LR（R14），并更新 r0
|
|
|
|
|
|
|
|
// 刷新 next_task->thread.ip
|
|
|
|
"LDR r1, =next_task\n" // 加载 next_task 的地址到 r1
|
|
|
|
"LDR r1, [r1]\n" // 加载 next_task 的值到 r1
|
|
|
|
"STR r0, [r1, #4]\n" // 将更新后的 r0 存储到 next_task->thread.ip
|
|
|
|
|
|
|
|
// 跳转到 LR 指向的地址
|
|
|
|
"BX lr\n" // 跳转到 LR 指向的地址
|
|
|
|
); |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
__asm void tTaskRunFirst(void) |
|
|
|
{ |
|
|
|
IMPORT current_task |
|
|
|
|
|
|
|
// R0 = current_task->thread.ip
|
|
|
|
LDR R0, =current_task |
|
|
|
LDR R0, [R0] |
|
|
|
LDR R0, [R0, #4] |
|
|
|
|
|
|
|
// POP
|
|
|
|
LDMIA R0!,{R14}; |
|
|
|
|
|
|
|
// refresh current_task->thread.ip
|
|
|
|
LDR R1, =current_task |
|
|
|
LDR R1, [R1] |
|
|
|
STR R0, [R1, #4] |
|
|
|
|
|
|
|
BX R14 |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
__asm void tTaskSwitch(void) |
|
|
|
{ |
|
|
|
IMPORT current_task |
|
|
|
IMPORT next_task |
|
|
|
|
|
|
|
// R0 = current_task->thread.ip
|
|
|
|
LDR R0, =current_task |
|
|
|
LDR R0, [R0] |
|
|
|
LDR R0, [R0, #4] |
|
|
|
|
|
|
|
// PUSH
|
|
|
|
STMDB R0!, {R14}; |
|
|
|
|
|
|
|
// refresh current_task->thread.ip
|
|
|
|
LDR R1, =current_task |
|
|
|
LDR R1, [R1] |
|
|
|
STR R0, [R1, #4] |
|
|
|
|
|
|
|
// current_task = next_task;
|
|
|
|
LDR R0, =current_task |
|
|
|
LDR R1, =next_task |
|
|
|
LDR R1, [R1] |
|
|
|
STR R1, [R0] |
|
|
|
|
|
|
|
// R0 = next_task->thread.ip
|
|
|
|
LDR R0, =next_task |
|
|
|
LDR R0, [R0] |
|
|
|
LDR R0, [R0, #4] |
|
|
|
|
|
|
|
// POP
|
|
|
|
LDMIA R0!,{R14}; |
|
|
|
|
|
|
|
// refresh next_task->thread.ip
|
|
|
|
LDR R1, =next_task |
|
|
|
LDR R1, [R1] |
|
|
|
STR R0, [R1, #4] |
|
|
|
|
|
|
|
BX R14 |
|
|
|
} |
|
|
|
*/ |
|
|
|
#endif |
|
|
|
|
zhibiao.xu
4 months ago