| 1 | /* |
| 2 | libco.aarch64 (2017-06-26) |
| 3 | author: webgeek1234 |
| 4 | license: public domain |
| 5 | */ |
| 6 | |
| 7 | #define LIBCO_C |
| 8 | #include "libco.h" |
| 9 | #include <assert.h> |
| 10 | #include <stdlib.h> |
| 11 | #include <string.h> |
| 12 | #include <stdint.h> |
| 13 | |
| 14 | #ifndef __APPLE__ |
| 15 | #include <malloc.h> |
| 16 | #endif |
| 17 | |
| 18 | #ifdef __cplusplus |
| 19 | extern "C" { |
| 20 | #endif |
| 21 | |
| 22 | static thread_local uint64_t co_active_buffer[64]; |
| 23 | static thread_local cothread_t co_active_handle; |
| 24 | |
| 25 | asm ( |
| 26 | ".globl co_switch_aarch64\n" |
| 27 | ".globl _co_switch_aarch64\n" |
| 28 | "co_switch_aarch64:\n" |
| 29 | "_co_switch_aarch64:\n" |
| 30 | " stp x8, x9, [x1]\n" |
| 31 | " stp x10, x11, [x1, #16]\n" |
| 32 | " stp x12, x13, [x1, #32]\n" |
| 33 | " stp x14, x15, [x1, #48]\n" |
| 34 | " str x19, [x1, #72]\n" |
| 35 | " stp x20, x21, [x1, #80]\n" |
| 36 | " stp x22, x23, [x1, #96]\n" |
| 37 | " stp x24, x25, [x1, #112]\n" |
| 38 | " stp x26, x27, [x1, #128]\n" |
| 39 | " stp x28, x29, [x1, #144]\n" |
| 40 | " mov x16, sp\n" |
| 41 | " stp x16, x30, [x1, #160]\n" |
| 42 | |
| 43 | " ldp x8, x9, [x0]\n" |
| 44 | " ldp x10, x11, [x0, #16]\n" |
| 45 | " ldp x12, x13, [x0, #32]\n" |
| 46 | " ldp x14, x15, [x0, #48]\n" |
| 47 | " ldr x19, [x0, #72]\n" |
| 48 | " ldp x20, x21, [x0, #80]\n" |
| 49 | " ldp x22, x23, [x0, #96]\n" |
| 50 | " ldp x24, x25, [x0, #112]\n" |
| 51 | " ldp x26, x27, [x0, #128]\n" |
| 52 | " ldp x28, x29, [x0, #144]\n" |
| 53 | " ldp x16, x17, [x0, #160]\n" |
| 54 | " mov sp, x16\n" |
| 55 | " br x17\n" |
| 56 | ); |
| 57 | |
| 58 | /* ASM */ |
| 59 | void co_switch_aarch64(cothread_t handle, cothread_t current); |
| 60 | |
| 61 | static void crash(void) |
| 62 | { |
| 63 | /* Called only if cothread_t entrypoint returns. */ |
| 64 | assert(0); |
| 65 | } |
| 66 | |
| 67 | cothread_t co_create(unsigned int size, void (*entrypoint)(void)) |
| 68 | { |
| 69 | uint64_t *ptr = NULL; |
| 70 | cothread_t handle = 0; |
| 71 | size = (size + 1023) & ~1023; |
| 72 | #if HAVE_POSIX_MEMALIGN >= 1 |
| 73 | if (posix_memalign(&handle, 1024, size + 512) < 0) |
| 74 | return 0; |
| 75 | #else |
| 76 | handle = memalign(1024, size + 512); |
| 77 | #endif |
| 78 | |
| 79 | if (!handle) |
| 80 | return handle; |
| 81 | |
| 82 | ptr = (uint64_t*)handle; |
| 83 | /* Non-volatiles. */ |
| 84 | ptr[0] = 0; /* x8 */ |
| 85 | ptr[1] = 0; /* x9 */ |
| 86 | ptr[2] = 0; /* x10 */ |
| 87 | ptr[3] = 0; /* x11 */ |
| 88 | ptr[4] = 0; /* x12 */ |
| 89 | ptr[5] = 0; /* x13 */ |
| 90 | ptr[6] = 0; /* x14 */ |
| 91 | ptr[7] = 0; /* x15 */ |
| 92 | ptr[8] = 0; /* padding */ |
| 93 | ptr[9] = 0; /* x19 */ |
| 94 | ptr[10] = 0; /* x20 */ |
| 95 | ptr[11] = 0; /* x21 */ |
| 96 | ptr[12] = 0; /* x22 */ |
| 97 | ptr[13] = 0; /* x23 */ |
| 98 | ptr[14] = 0; /* x24 */ |
| 99 | ptr[15] = 0; /* x25 */ |
| 100 | ptr[16] = 0; /* x26 */ |
| 101 | ptr[17] = 0; /* x27 */ |
| 102 | ptr[18] = 0; /* x28 */ |
| 103 | ptr[20] = (uintptr_t)ptr + size + 512 - 16; /* x30, stack pointer */ |
| 104 | ptr[19] = ptr[20]; /* x29, frame pointer */ |
| 105 | ptr[21] = (uintptr_t)entrypoint; /* PC (link register x31 gets saved here). */ |
| 106 | return handle; |
| 107 | } |
| 108 | |
| 109 | cothread_t co_active(void) |
| 110 | { |
| 111 | if (!co_active_handle) |
| 112 | co_active_handle = co_active_buffer; |
| 113 | return co_active_handle; |
| 114 | } |
| 115 | |
| 116 | void co_delete(cothread_t handle) |
| 117 | { |
| 118 | free(handle); |
| 119 | } |
| 120 | |
| 121 | void co_switch(cothread_t handle) |
| 122 | { |
| 123 | cothread_t co_previous_handle = co_active(); |
| 124 | co_switch_aarch64(co_active_handle = handle, co_previous_handle); |
| 125 | } |
| 126 | |
| 127 | #ifdef __cplusplus |
| 128 | } |
| 129 | #endif |