libcr: Fix stack alignment

1 files changed, 9 insertions, 4 deletions

diff --git a/libcr/coroutine.c b/libcr/coroutine.c
index fd44acc..9ddfa00 100644
--- a/libcr/coroutine.c
+++ b/libcr/coroutine.c
@@ -6,7 +6,7 @@
 
 #include <stdint.h> /* for uint8_t */
 #include <stdio.h>  /* for printf(), fprintf(), stderr */
-#include <stdlib.h> /* for malloc(), free() */
+#include <stdlib.h> /* for aligned_alloc(), free() */
 #include <assert.h>
 #include <setjmp.h>
 
@@ -237,6 +237,11 @@ cid_t coroutine_add(cr_fn_t fn, void *args) {
 	return coroutine_add_with_stack_size(CONFIG_COROUTINE_DEFAULT_STACK_SIZE, fn, args);
 }
 
+#define STACK_ALIGNMENT ({ __attribute__((aligned)) void fn(void) {}; __alignof__(fn); })
+
+/* Return `n` rounded up to the nearest multiple of `d` */
+#define round_up(n, d) ( ( ((n)+(d)-1) / (d) ) * (d) )
+
 cid_t coroutine_add_with_stack_size(size_t stack_size, cr_fn_t fn, void *args) {
 	static cid_t last_created = 0;
 	cid_t parent = coroutine_running;
@@ -263,7 +268,7 @@ cid_t coroutine_add_with_stack_size(size_t stack_size, cr_fn_t fn, void *args) {
 	last_created = child;
 
 	coroutine_table[child-1].stack_size = stack_size;
-	coroutine_table[child-1].stack = malloc(stack_size);
+	coroutine_table[child-1].stack = aligned_alloc(STACK_ALIGNMENT, stack_size);
 #if CONFIG_COROUTINE_MEASURE_STACK || CONFIG_COROUTINE_PROTECT_STACK
 	for (size_t i = 0; i < stack_size; i++)
 		((uint8_t*)coroutine_table[child-1].stack)[i] = stack_pattern[i%sizeof(stack_pattern)];
@@ -275,9 +280,9 @@ cid_t coroutine_add_with_stack_size(size_t stack_size, cr_fn_t fn, void *args) {
 		void *stack_base = coroutine_table[child-1].stack + (STACK_GROWS_DOWNWARD ? stack_size : 0);
 #if CONFIG_COROUTINE_PROTECT_STACK
 #  if STACK_GROWS_DOWNWARD
-		stack_base -= sizeof(stack_pattern);
+		stack_base -= round_up(sizeof(stack_pattern), STACK_ALIGNMENT);
 #  else
-		stack_base += sizeof(stack_pattern);
+		stack_base += round_up(sizeof(stack_pattern), STACK_ALIGNMENT);
 #  endif
 #endif
 		debugf("...stack     =%#p\n", coroutine_table[child-1].stack);