AGB thread attempt

thread.c

#include <stdlib.h>
#include <thread.h>

#include <sys/ucontext.h>

int __agbabi_getcontext( ucontext_t * );
int __agbabi_setcontext( const ucontext_t * ) ;
void __agbabi_makecontext( ucontext_t *, void ( * )( void ), int, ... );
int __agbabi_swapcontext( ucontext_t *, const ucontext_t * );

void __aeabi_memclr4( void * dest, size_t n );
void __aeabi_memcpy4( void * dest, const void * src, size_t n );

#define thread_ewram_attributes __attribute__((section(".ewram"))) __attribute__((target("thumb"))) __attribute__((optimize("O3")))
#define max_threads 8
#define thread_mask ( max_threads - 1 )

typedef struct {
    ucontext_t context;
    __agbabi_thread_t id;
} thread_t;

static __agbabi_thread_t current_thread = 0;

static ucontext_t thread_join_ctx; // Shared by all threads calling __agbabi_thread_join
static thread_t threads[max_threads];

static void * thread_ewram_attributes thread_trampoline( void * ( * start_routine )( void * ), void * arg ) {
    void * const result = start_routine( arg );
    if ( ( threads[current_thread >> 24].id & 0x80000000 ) == 0 ) {
        threads[current_thread >> 24].id &= 0xffffff; // Clear id to disable
        __agbabi_thread_yield();
        __builtin_unreachable();
    }
    return result;
}

int thread_ewram_attributes __agbabi_thread_create( __agbabi_thread_t * thread, const __agbabi_thread_attr_t * attr, void * ( * start_routine )( void * ), void * arg ) {
    thread_t * handle = NULL;
    for ( int i = 1; i < max_threads; ++i ) {
        if ( !( threads[i].id & 0x7f000000 ) ) {
            unsigned long counter = ( threads[i].id + 1 ) & 0xffffff;
            threads[i].id = ( i << 24 ) | counter;
            handle = &threads[i];
            break;
        }
    }

    if ( !handle ) {
        return -1;
    }

    handle->context.uc_link = &thread_join_ctx;
    handle->context.uc_stack.ss_sp = attr->stackaddr;
    handle->context.uc_stack.ss_size = attr->stacksize;
    __agbabi_makecontext( &handle->context, ( void( * )( void ) ) thread_trampoline, 2, start_routine, arg );

    *thread = handle->id;
    return 0;
}

int thread_ewram_attributes __agbabi_thread_join( __agbabi_thread_t thread, void ** retval ) {
    thread_t * const handle = &threads[thread >> 24];
    if ( handle->id != thread ) {
        return -1;
    }

    unsigned long int prevThread = current_thread;
    current_thread = handle->id;
    handle->id |= 0x80000000; // Set join bit
    __agbabi_swapcontext( handle->context.uc_link, &handle->context );
    current_thread = prevThread;

    *retval = ( void * ) handle->context.uc_mcontext.arm_r0;
    __aeabi_memclr4( &thread_join_ctx, sizeof( thread_join_ctx ) );
    __aeabi_memclr4( &handle->context, sizeof( handle->context ) );
    handle->id = handle->id & 0xffffff;
    return 0;
}

int thread_ewram_attributes __agbabi_thread_kill( __agbabi_thread_t thread, int exitCode ) {
    thread_t * const handle = &threads[thread >> 24];
    if ( handle->id != thread ) {
        return -1;
    }

    if ( current_thread == handle->id ) {
        exit( exitCode );
        __builtin_unreachable();
    }

    __aeabi_memclr4( &handle->context, sizeof( handle->context ) );
    handle->id = handle->id & 0xffffff;
    return 0;
}

int thread_ewram_attributes __agbabi_thread_yield() {
    thread_t * handle = NULL;

    const unsigned long current = current_thread >> 24;
    unsigned long next = current + 1;
    while ( next != current ) {
        if ( next == ( threads[next].id >> 24 ) ) {
            handle = &threads[next];
            break;
        }

        next = ( next + 1 ) & thread_mask;
    }

    if ( !handle ) {
        return 0; // Nothing to yield to
    }

    current_thread = handle->id;
    __agbabi_swapcontext( &threads[current].context, &handle->context );
    return 0;
}

struct ucontext_t * thread_ewram_attributes __agbabi_thread_next_context( struct ucontext_t * ctx ) {
    thread_t * handle = NULL;

    const unsigned long current = current_thread >> 24;
    unsigned long next = current + 1;
    while ( next != current ) {
        if ( next == ( threads[next].id >> 24 ) ) {
            handle = &threads[next];
            break;
        }

        next = ( next + 1 ) & thread_mask;
    }

    if ( !handle ) {
        return ctx; // Nothing to yield to
    }

    current_thread = handle->id;
    __aeabi_memcpy4( &threads[current].context.uc_mcontext, &ctx->uc_mcontext, sizeof( ctx->uc_mcontext ) );
    return &handle->context;
}

thread.h

#ifndef _AGBABI_THREAD_H_
#define _AGBABI_THREAD_H_

#if defined( __cplusplus )
extern "C" {
#endif

typedef unsigned long int __agbabi_thread_t;

typedef struct {
    void * stackaddr;
    unsigned long int stacksize;
} __agbabi_thread_attr_t;

int __agbabi_thread_create( __agbabi_thread_t * thread, const __agbabi_thread_attr_t * attr, void * ( * start_routine )( void * ), void * arg );
int __agbabi_thread_join( __agbabi_thread_t thread, void ** retval );
int __agbabi_thread_kill( __agbabi_thread_t thread, int exitCode );
int __agbabi_thread_yield();

struct ucontext_t * __agbabi_thread_next_context( struct ucontext_t * ctx );

#if defined( __cplusplus )
} // extern "C"
#endif

#endif // define _AGBABI_THREAD_H_