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 "mov lr, xzr\n"
26 "br x1\n"
27 ".size context_entry, . - context_entry\n"
28 ".global co2_save_context; .type co2_save_context, %function; co2_save_context:\n"
29 "stp x19, x20, [x0,#0]\n"
30 "stp x21, x22, [x0,#16]\n"
31 "stp x23, x24, [x0,#32]\n"
32 "stp x25, x26, [x0,#48]\n"
33 "stp x27, x28, [x0,#64]\n"
34 "stp x29, x30, [x0,#80]\n"
35 "mov x2, sp\n"
36 "str x2, [x0,#104]\n"
37 "stp d8, d9, [x0,#112]\n"
38 "stp d10, d11, [x0,#128]\n"
39 "stp d12, d13, [x0,#144]\n"
40 "stp d14, d15, [x0,#160]\n"
41 "mov x0, #0\n"
42 "ret\n"
43 ".size co2_save_context, . - co2_save_context\n"
44 ".global co2_restore_context; .type co2_restore_context, %function; co2_restore_context:\n"
45 "ldp x19, x20, [x0,#0]\n"
46 "ldp x21, x22, [x0,#16]\n"
47 "ldp x23, x24, [x0,#32]\n"
48 "ldp x25, x26, [x0,#48]\n"
49 "ldp x27, x28, [x0,#64]\n"
50 "ldp x29, x30, [x0,#80]\n"
51 "ldr x2, [x0,#104]\n"
52 "mov sp, x2\n"
53 "ldp d8, d9, [x0,#112]\n"
54 "ldp d10, d11, [x0,#128]\n"
55 "ldp d12, d13, [x0,#144]\n"
56 "ldp d14, d15, [x0,#160]\n"
57 "mov x0, #1\n"
58 "ret\n"
59 ".size co2_restore_context, . - co2_restore_context\n");
60 #elif defined(__arm__)
61 asm(".global context_entry; .type context_entry, %function; context_entry:\n"
62 "pop {r0, r1}\n"
63 "mov lr, #0\n"
64 "bx r1\n"
65 ".size context_entry, . - context_entry\n"
66 ".global co2_save_context; .type co2_save_context, %function; co2_save_context:\n"
67 "mov ip, r0\n"
68 "str sp, [ip], #4\n"
69 "str lr, [ip], #4\n"
70 "stmia ip!, {v1-v6, sl, fp}\n"
71 ".fpu vfp\n"
72 "vstmia ip!, {d8-d15}\n"
73 "mov r0, #0\n"
74 "bx lr\n"
75 ".size co2_save_context, . - co2_save_context\n"
76 ".global co2_restore_context; .type co2_restore_context, %function; co2_restore_context:\n"
77 "mov ip, r0\n"
78 "ldr a4, [ip], #4\n"
79 "ldr r4, [ip], #4\n"
80 "mov sp, a4\n"
81 "mov lr, r4\n"
82 "ldmia ip!, {v1-v6, sl, fp}\n"
83 ".fpu vfp\n"
84 "vldmia ip!, {d8-d15}\n"
85 "mov r0, #1\n"
86 "bx lr\n"
87 ".size co2_restore_context, . - co2_restore_context\n");
88 #elif (defined(__x86_64__) && !defined _MSC_VER)
89 asm(".global context_entry; .type context_entry, %function; context_entry:\n"
90 "pop %rdi\n"
91 "jmp *(%rsp)\n"
92 ".size context_entry, . - context_entry\n"
93 ".global co2_save_context; .type co2_save_context, %function; co2_save_context:\n"
94 "mov %rbx, (%rdi)\n"
95 "mov %rbp, 8(%rdi)\n"
96 "mov %r12, 16(%rdi)\n"
97 "mov %r13, 24(%rdi)\n"
98 "mov %r14, 32(%rdi)\n"
99 "mov %r15, 40(%rdi)\n"
100 "lea 8(%rsp), %rdx\n"
101 "mov %rdx, 48(%rdi)\n"
102 "mov (%rsp), %rdx\n"
103 "mov %rdx, 56(%rdi)\n"
104 "xor %rax, %rax\n"
105 "ret\n"
106 ".size co2_save_context, . - co2_save_context\n"
107 ".global co2_restore_context; .type co2_restore_context, %function; co2_restore_context:\n"
108 "xor %rax, %rax\n"
109 "inc %rax\n"
110 "mov (%rdi), %rbx\n"
111 "mov 8(%rdi), %rbp\n"
112 "mov 16(%rdi), %r12\n"
113 "mov 24(%rdi), %r13\n"
114 "mov 32(%rdi), %r14\n"
115 "mov 40(%rdi), %r15\n"
116 "mov 48(%rdi), %rdx\n"
117 "mov %rdx, %rsp\n"
118 "jmp *56(%rdi)\n"
119 ".size co2_restore_context, . - co2_restore_context\n");
120 #endif
121
co2_init_context(struct co2_context * ctx,void (* func)(void *),void * arg,void * stack,size_t stack_size)122 int co2_init_context(struct co2_context* ctx, void (*func)(void*), void* arg, void* stack, size_t stack_size)
123 {
124 if (stack_size < 0x4 * sizeof(uintptr_t)) {
125 return EINVAL;
126 }
127
128 uintptr_t stack_top = (uintptr_t)stack + stack_size - 0x2 * sizeof(uintptr_t);
129 stack_top -= stack_top % (0x2 * sizeof(uintptr_t));
130 uintptr_t* data = (uintptr_t*)stack_top;
131
132 ctx->regs[REG_LR] = (uintptr_t)context_entry;
133 ctx->regs[REG_SP] = stack_top;
134
135 data[0] = (uintptr_t)arg;
136 data[1] = (uintptr_t)func;
137
138 return 0;
139 }
140