1 /*
2 * Copyright (c) 2023 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16 #include "co2_context.h"
17
18 #include <errno.h>
19
20 void context_entry(void);
21
22 #if defined(__aarch64__)
23 asm(".global context_entry; .type context_entry, %function; context_entry:\n"
24 "ldp x0, x1, [sp], #0x10\n"
25 "br x1\n"
26 ".size context_entry, . - context_entry\n"
27 ".global co2_save_context; .type co2_save_context, %function; co2_save_context:\n"
28 "stp x19, x20, [x0,#0]\n"
29 "stp x21, x22, [x0,#16]\n"
30 "stp x23, x24, [x0,#32]\n"
31 "stp x25, x26, [x0,#48]\n"
32 "stp x27, x28, [x0,#64]\n"
33 "stp x29, x30, [x0,#80]\n"
34 "mov x2, sp\n"
35 "str x2, [x0,#104]\n"
36 "stp d8, d9, [x0,#112]\n"
37 "stp d10, d11, [x0,#128]\n"
38 "stp d12, d13, [x0,#144]\n"
39 "stp d14, d15, [x0,#160]\n"
40 "mov x0, #0\n"
41 "ret\n"
42 ".size co2_save_context, . - co2_save_context\n"
43 ".global co2_restore_context; .type co2_restore_context, %function; co2_restore_context:\n"
44 "ldp x19, x20, [x0,#0]\n"
45 "ldp x21, x22, [x0,#16]\n"
46 "ldp x23, x24, [x0,#32]\n"
47 "ldp x25, x26, [x0,#48]\n"
48 "ldp x27, x28, [x0,#64]\n"
49 "ldp x29, x30, [x0,#80]\n"
50 "ldr x2, [x0,#104]\n"
51 "mov sp, x2\n"
52 "ldp d8, d9, [x0,#112]\n"
53 "ldp d10, d11, [x0,#128]\n"
54 "ldp d12, d13, [x0,#144]\n"
55 "ldp d14, d15, [x0,#160]\n"
56 "mov x0, #1\n"
57 "ret\n"
58 ".size co2_restore_context, . - co2_restore_context\n");
59 #elif defined(__arm__)
60 asm(".global context_entry; .type context_entry, %function; context_entry:\n"
61 "pop {r0, r1}\n"
62 "bx r1\n"
63 ".size context_entry, . - context_entry\n"
64 ".global co2_save_context; .type co2_save_context, %function; co2_save_context:\n"
65 "mov ip, r0\n"
66 "str sp, [ip], #4\n"
67 "str lr, [ip], #4\n"
68 "stmia ip!, {v1-v6, sl, fp}\n"
69 ".fpu vfp\n"
70 "vstmia ip!, {d8-d15}\n"
71 "mov r0, #0\n"
72 "bx lr\n"
73 ".size co2_save_context, . - co2_save_context\n"
74 ".global co2_restore_context; .type co2_restore_context, %function; co2_restore_context:\n"
75 "mov ip, r0\n"
76 "ldr a4, [ip], #4\n"
77 "ldr r4, [ip], #4\n"
78 "mov sp, a4\n"
79 "mov lr, r4\n"
80 "ldmia ip!, {v1-v6, sl, fp}\n"
81 ".fpu vfp\n"
82 "vldmia ip!, {d8-d15}\n"
83 "mov r0, #1\n"
84 "bx lr\n"
85 ".size co2_restore_context, . - co2_restore_context\n");
86 #elif (defined(__x86_64__) && !defined _MSC_VER)
87 asm(".global context_entry; .type context_entry, %function; context_entry:\n"
88 "pop %rdi\n"
89 "jmp *(%rsp)\n"
90 ".size context_entry, . - context_entry\n"
91 ".global co2_save_context; .type co2_save_context, %function; co2_save_context:\n"
92 "mov %rbx, (%rdi)\n"
93 "mov %rbp, 8(%rdi)\n"
94 "mov %r12, 16(%rdi)\n"
95 "mov %r13, 24(%rdi)\n"
96 "mov %r14, 32(%rdi)\n"
97 "mov %r15, 40(%rdi)\n"
98 "lea 8(%rsp), %rdx\n"
99 "mov %rdx, 48(%rdi)\n"
100 "mov (%rsp), %rdx\n"
101 "mov %rdx, 56(%rdi)\n"
102 "xor %rax, %rax\n"
103 "ret\n"
104 ".size co2_save_context, . - co2_save_context\n"
105 ".global co2_restore_context; .type co2_restore_context, %function; co2_restore_context:\n"
106 "xor %rax, %rax\n"
107 "inc %rax\n"
108 "mov (%rdi), %rbx\n"
109 "mov 8(%rdi), %rbp\n"
110 "mov 16(%rdi), %r12\n"
111 "mov 24(%rdi), %r13\n"
112 "mov 32(%rdi), %r14\n"
113 "mov 40(%rdi), %r15\n"
114 "mov 48(%rdi), %rdx\n"
115 "mov %rdx, %rsp\n"
116 "jmp *56(%rdi)\n"
117 ".size co2_restore_context, . - co2_restore_context\n");
118 #endif
119
co2_init_context(struct co2_context * ctx,void (* func)(void *),void * arg,void * stack,size_t stack_size)120 int co2_init_context(struct co2_context* ctx, void (*func)(void*), void* arg, void* stack, size_t stack_size)
121 {
122 if (stack_size < 0x4 * sizeof(uintptr_t)) {
123 return EINVAL;
124 }
125
126 uintptr_t stack_top = (uintptr_t)stack + stack_size - 0x2 * sizeof(uintptr_t);
127 stack_top -= stack_top % (0x2 * sizeof(uintptr_t));
128 uintptr_t* data = (uintptr_t*)stack_top;
129
130 ctx->regs[REG_LR] = (uintptr_t)context_entry;
131 ctx->regs[REG_SP] = stack_top;
132
133 data[0] = (uintptr_t)arg;
134 data[1] = (uintptr_t)func;
135
136 return 0;
137 }
138