|
| 1 | +#if defined(__arm64__) || defined(__aarch64__) |
| 2 | +#include "Coro.h" |
| 3 | +#include <stdio.h> |
| 4 | +#include <stdlib.h> |
| 5 | +#include <string.h> |
| 6 | + |
| 7 | +// External functions implemented in asm.S |
| 8 | +extern int coro_arm64_getcontext(void *context) __asm("coro_arm64_getcontext"); |
| 9 | +extern int coro_arm64_setcontext(void *context) __asm("coro_arm64_setcontext"); |
| 10 | + |
| 11 | +// Custom context implementation for ARM64 |
| 12 | +typedef struct { |
| 13 | + unsigned long x19_x20[2]; // x19, x20 |
| 14 | + unsigned long x21_x22[2]; // x21, x22 |
| 15 | + unsigned long x23_x24[2]; // x23, x24 |
| 16 | + unsigned long x25_x26[2]; // x25, x26 |
| 17 | + unsigned long x27_x28[2]; // x27, x28 |
| 18 | + unsigned long fp_lr[2]; // x29 (fp), x30 (lr) |
| 19 | + unsigned long sp; // stack pointer |
| 20 | +} arm64_context_t; |
| 21 | + |
| 22 | +typedef struct CallbackBlock { |
| 23 | + void *context; |
| 24 | + CoroStartCallback *func; |
| 25 | +} CallbackBlock; |
| 26 | + |
| 27 | +static CallbackBlock globalCallbackBlock; |
| 28 | + |
| 29 | +static void Coro_StartWithArg(void) { |
| 30 | + //fprintf(stderr, "Coro_StartWithArg called\n"); |
| 31 | + CallbackBlock *block = &globalCallbackBlock; |
| 32 | + //fprintf(stderr, "Function pointer: %p\n", block->func); |
| 33 | + block->func(block->context); |
| 34 | + fprintf(stderr, "Scheduler error: returned from coro start function\n"); |
| 35 | + exit(-1); |
| 36 | +} |
| 37 | + |
| 38 | +void Coro_free(Coro *self) { |
| 39 | + if (self->stack) { |
| 40 | + free(self->stack); |
| 41 | + } |
| 42 | + free(self); |
| 43 | +} |
| 44 | + |
| 45 | +void Coro_initializeMainCoro(Coro *self) { |
| 46 | + self->isMain = 1; |
| 47 | +} |
| 48 | + |
| 49 | +Coro *Coro_new(void) { |
| 50 | + Coro *c = (Coro *)calloc(1, sizeof(Coro)); |
| 51 | + if (c) { |
| 52 | + c->requestedStackSize = CORO_DEFAULT_STACK_SIZE; |
| 53 | + c->allocatedStackSize = 0; |
| 54 | + c->stack = NULL; |
| 55 | + } |
| 56 | + return c; |
| 57 | +} |
| 58 | + |
| 59 | +void Coro_setStackSize_(Coro *self, size_t size) { |
| 60 | + self->requestedStackSize = size; |
| 61 | +} |
| 62 | + |
| 63 | +static void Coro_allocStackIfNeeded(Coro *self) { |
| 64 | + if (self->stack && self->requestedStackSize < self->allocatedStackSize) { |
| 65 | + free(self->stack); |
| 66 | + self->stack = NULL; |
| 67 | + self->allocatedStackSize = 0; |
| 68 | + } |
| 69 | + if (!self->stack) { |
| 70 | + // Make sure stack is 16-byte aligned for ARM64 |
| 71 | + self->stack = calloc(1, self->requestedStackSize + 16); |
| 72 | + self->allocatedStackSize = self->requestedStackSize; |
| 73 | + } |
| 74 | +} |
| 75 | + |
| 76 | +// This function initializes the context with a new stack and entry point |
| 77 | +void Coro_setup(Coro *self, void *arg) { |
| 78 | + arm64_context_t *context = (arm64_context_t *)&self->env; |
| 79 | + memset(context, 0, sizeof(*context)); |
| 80 | + |
| 81 | + Coro_allocStackIfNeeded(self); |
| 82 | + |
| 83 | + // Initialize stack pointer to top of stack (ARM64 full descending stack) |
| 84 | + unsigned long sp = (unsigned long)self->stack + self->allocatedStackSize - 16; |
| 85 | + // Ensure 16-byte alignment |
| 86 | + sp &= ~15UL; |
| 87 | + |
| 88 | + // Store stack pointer in context |
| 89 | + context->sp = sp; |
| 90 | + |
| 91 | + // Store entry point in link register (x30) |
| 92 | + context->fp_lr[1] = (unsigned long)Coro_StartWithArg; |
| 93 | +} |
| 94 | + |
| 95 | +int Coro_stackSpaceAlmostGone(Coro *self) { |
| 96 | + arm64_context_t *context = (arm64_context_t *)&self->env; |
| 97 | + unsigned long sp = context->sp; |
| 98 | + unsigned long stack_base = (unsigned long)self->stack; |
| 99 | + |
| 100 | + // Check if we have less than 1KB of stack space left |
| 101 | + return (sp - stack_base) < 1024; |
| 102 | +} |
| 103 | + |
| 104 | +size_t Coro_bytesLeftOnStack(Coro *self) { |
| 105 | + arm64_context_t *context = (arm64_context_t *)&self->env; |
| 106 | + unsigned long sp = context->sp; |
| 107 | + unsigned long stack_base = (unsigned long)self->stack; |
| 108 | + |
| 109 | + // Return number of bytes between stack pointer and stack base |
| 110 | + if (sp > stack_base) { |
| 111 | + return sp - stack_base; |
| 112 | + } |
| 113 | + |
| 114 | + // Fallback if stack info not available |
| 115 | + return 1024 * 1024; |
| 116 | +} |
| 117 | + |
| 118 | +void Coro_startCoro_(Coro *self, Coro *other, void *context, CoroStartCallback *callback) { |
| 119 | + globalCallbackBlock.context = context; |
| 120 | + globalCallbackBlock.func = callback; |
| 121 | + Coro_setup(other, &globalCallbackBlock); |
| 122 | + Coro_switchTo_(self, other); |
| 123 | +} |
| 124 | + |
| 125 | +void Coro_switchTo_(Coro *self, Coro *next) { |
| 126 | + // Get the context pointers |
| 127 | + arm64_context_t *from_context = (arm64_context_t *)&self->env; |
| 128 | + arm64_context_t *to_context = (arm64_context_t *)&next->env; |
| 129 | + |
| 130 | + // Save current context, if successful (returns 0), then restore the next context |
| 131 | + if (coro_arm64_getcontext((void*)from_context) == 0) { |
| 132 | + coro_arm64_setcontext((void*)to_context); |
| 133 | + } |
| 134 | + |
| 135 | +} |
| 136 | +#endif |
0 commit comments