1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
|
#include "kern/priv.h"
#include "arch.h"
#include "kern/log.h"
void handle_svc_call(uint32_t svc_number)
{
klogf("Handle SVC: %p\n", svc_number);
for (;;)
;
}
asm(" .align 2\n"
" .global on_svc\n"
" .syntax unified\n"
" .fpu softvfp\n"
" .type on_svc, %function\n"
"on_svc:\n"
" tst lr,#4\n"
" ite eq\n"
" moveq r12,sp\n"
" mrsne r12,psp\n"
" ldr r12, [r12, #24]\n"
" ldrh r12, [r12, #-2]\n"
" bics r12, r12, #0xff00\n"
" push {r4, lr}\n"
" mov r0 , r12\n"
" bl handle_svc_call\n"
" pop {r4, lr}\n"
" tst lr, #4\n"
" ite eq\n"
" moveq r12,sp\n"
" mrsne r12,psp\n"
" stm r12,{r0-r3}\n"
" bx lr\n");
// void on_svc() // ISR handler. Override from weak symbol.
// {
// asm volatile(
// "push {r0-r12, lr}\n\t"
// "ldr r0, [lr, #-4]\n\t"
// "bic r0, r0, #0xff000000\n\t"
// );
// TST LR,#4 ; Called from Handler Mode?
// MRSNE R12,PSP ; Yes, use PSP
// MOVEQ R12,SP ; No, use MSP
// LDR R12,[R12,#24] ; Read Saved PC from Stack
// LDRH R12,[R12,#-2] ; Load Halfword
// BICS R12,R12,#0xFF00 ; Extract SVC Number
// uint32_t reg;
// asm volatile(
// "mov r12, sp\n\t"
// "ldr r12, [r12,#24]\n\t"
// // "ldrh r12,[r12, #-2]\n\t"
// // "bics r12, r12, #0xff00\n\t"
// "mov %0, r12\n\t": "=r"(reg));
// uint32_t base[0];
// uint32_t reg;
//
//
// asm volatile("mov %0, sp\n\t" : "=r"(reg));
// uint32_t* at = (uint32_t*) (reg + 44); /* Does GCC set up 5 stack frame
// words? */
//
// klogf("Stack pointer: %p\n", reg);
// klogf("Stack pointer +44: %p\n", (reg + 44));
// klogf("At_: %p: %p\n\n", 0x80009f6, *(uint32_t*)0x80009f6);
// klogf("At: %p: %p\n\n", at, *at);
//
//
// int i = 0;
// for (; i < 20 && &base[i] != (void*)STACK_TOP; ++ i) {
// kerr_logf(" (%p) %p\n", &base[i], base[i]);
// }
//
// // klogf("TEST %p\n", reg);
//
// for(;;);
// register int svc_number asm ("r0");
// klogf("SVC #: %p\n", svc_number);
// }
void set_control_register(uint32_t reg)
{
asm volatile("msr control, %0" : "=r"(reg) :);
}
uint32_t get_control_register()
{
uint32_t control;
asm volatile("mrs %0, control" : "=r"(control) :);
return control;
}
void enter_user_mode()
{
asm volatile(
"mov r0, #1\n\t"
"msr control, r0\n\t");
}
|
