314 lines
7.3 KiB
C
314 lines
7.3 KiB
C
#include <rt/assert.h>
|
|
#include <rt/context.h>
|
|
#include <rt/cycle.h>
|
|
#include <rt/idle.h>
|
|
#include <rt/interrupt.h>
|
|
#include <rt/log.h>
|
|
#include <rt/start.h>
|
|
#include <rt/syscall.h>
|
|
#include <rt/trap.h>
|
|
|
|
#include <rt/stack.h>
|
|
#include <rt/task.h>
|
|
#include <rt/tick.h>
|
|
|
|
#include <pthread.h>
|
|
#include <signal.h>
|
|
#include <sys/time.h>
|
|
|
|
#include <assert.h>
|
|
#include <stdarg.h>
|
|
#include <stdatomic.h>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
#define SIGTICK SIGALRM
|
|
#define SIGSYSCALL SIGUSR1
|
|
#define SIGRESUME SIGUSR2
|
|
|
|
struct pthread_arg
|
|
{
|
|
union task_fn
|
|
{
|
|
void (*fn)(void);
|
|
void (*fn_with_arg)(uintptr_t);
|
|
} task_fn;
|
|
uintptr_t arg;
|
|
bool has_arg;
|
|
};
|
|
|
|
static void block_all_signals(sigset_t *old_sigset)
|
|
{
|
|
sigset_t blocked_sigset;
|
|
sigaddset(&blocked_sigset, SIGTICK);
|
|
sigaddset(&blocked_sigset, SIGSYSCALL);
|
|
sigaddset(&blocked_sigset, SIGRESUME);
|
|
pthread_sigmask(SIG_BLOCK, &blocked_sigset, old_sigset);
|
|
}
|
|
|
|
static void unblock_all_signals(void)
|
|
{
|
|
sigset_t full_sigset;
|
|
sigfillset(&full_sigset);
|
|
pthread_sigmask(SIG_UNBLOCK, &full_sigset, NULL);
|
|
}
|
|
|
|
#if RT_LOG_ENABLE
|
|
void rt_logf(const char *format, ...)
|
|
{
|
|
va_list vlist;
|
|
va_start(vlist, format);
|
|
sigset_t old_sigset;
|
|
block_all_signals(&old_sigset);
|
|
vprintf(format, vlist);
|
|
fflush(stdout);
|
|
pthread_sigmask(SIG_SETMASK, &old_sigset, NULL);
|
|
}
|
|
#endif
|
|
|
|
void rt_assert(bool condition, const char *msg)
|
|
{
|
|
if (!condition)
|
|
{
|
|
fprintf(stderr, "%s\n", msg);
|
|
fflush(stderr);
|
|
abort();
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) static void *pthread_fn(void *arg)
|
|
{
|
|
struct pthread_arg *parg = arg;
|
|
union task_fn task_fn = parg->task_fn;
|
|
uintptr_t task_arg = parg->arg;
|
|
bool has_arg = parg->has_arg;
|
|
free(parg);
|
|
int sig;
|
|
sigset_t resume_sigset;
|
|
sigemptyset(&resume_sigset);
|
|
sigaddset(&resume_sigset, SIGRESUME);
|
|
sigwait(&resume_sigset, &sig);
|
|
unblock_all_signals();
|
|
if (has_arg)
|
|
{
|
|
task_fn.fn_with_arg(task_arg);
|
|
}
|
|
else
|
|
{
|
|
task_fn.fn();
|
|
}
|
|
rt_task_exit();
|
|
}
|
|
|
|
static void *context_init(struct pthread_arg *parg, void *stack,
|
|
size_t stack_size)
|
|
{
|
|
pthread_attr_t attr;
|
|
pthread_attr_init(&attr);
|
|
pthread_attr_setstack(&attr, stack, stack_size);
|
|
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
|
|
|
|
/*
|
|
* Launch each thread with signals blocked so only the active thread will
|
|
* receive signals.
|
|
*/
|
|
sigset_t old_sigset;
|
|
block_all_signals(&old_sigset);
|
|
|
|
pthread_t thread;
|
|
pthread_create(&thread, &attr, pthread_fn, parg);
|
|
|
|
pthread_sigmask(SIG_SETMASK, &old_sigset, NULL);
|
|
|
|
pthread_attr_destroy(&attr);
|
|
|
|
return (void *)thread;
|
|
}
|
|
|
|
void *rt_context_init(void (*fn)(void), void *stack, size_t stack_size)
|
|
{
|
|
struct pthread_arg *parg = malloc(sizeof *parg);
|
|
parg->task_fn.fn = fn;
|
|
parg->has_arg = false;
|
|
return context_init(parg, stack, stack_size);
|
|
}
|
|
|
|
void *rt_context_init_arg(void (*fn)(uintptr_t), uintptr_t arg, void *stack,
|
|
size_t stack_size)
|
|
{
|
|
struct pthread_arg *parg = malloc(sizeof *parg);
|
|
parg->task_fn.fn_with_arg = fn;
|
|
parg->arg = arg;
|
|
parg->has_arg = true;
|
|
return context_init(parg, stack, stack_size);
|
|
}
|
|
|
|
static void syscall(void)
|
|
{
|
|
pthread_kill(pthread_self(), SIGSYSCALL);
|
|
}
|
|
|
|
void rt_syscall(void)
|
|
{
|
|
syscall();
|
|
}
|
|
|
|
void rt_syscall_pend(void)
|
|
{
|
|
syscall();
|
|
}
|
|
|
|
bool rt_interrupt_is_active(void)
|
|
{
|
|
sigset_t emptyset, sigset;
|
|
sigemptyset(&emptyset);
|
|
pthread_sigmask(SIG_BLOCK, &emptyset, &sigset);
|
|
return sigismember(&sigset, SIGSYSCALL) == 1;
|
|
}
|
|
|
|
__attribute__((noreturn)) static void resume_handler(int sig)
|
|
{
|
|
(void)sig;
|
|
// This is always an error, so don't use rt_logf.
|
|
fprintf(stderr, "thread %lx received a resume but was not suspended\n",
|
|
(unsigned long)pthread_self());
|
|
fflush(stderr);
|
|
exit(1);
|
|
}
|
|
|
|
static void syscall_handler(int sig)
|
|
{
|
|
(void)sig;
|
|
rt_syscall_handler();
|
|
}
|
|
|
|
void rt_syscall_handler(void)
|
|
{
|
|
void *newctx = rt_syscall_run();
|
|
|
|
if (newctx)
|
|
{
|
|
// Block signals on the suspending thread.
|
|
block_all_signals(NULL);
|
|
*rt_context_prev = (void *)pthread_self();
|
|
|
|
atomic_thread_fence(memory_order_release);
|
|
pthread_kill((pthread_t)newctx, SIGRESUME);
|
|
|
|
sigset_t resume_sigset;
|
|
sigemptyset(&resume_sigset);
|
|
sigaddset(&resume_sigset, SIGRESUME);
|
|
int sig;
|
|
sigwait(&resume_sigset, &sig);
|
|
unblock_all_signals();
|
|
}
|
|
}
|
|
|
|
static void tick_handler(int sig)
|
|
{
|
|
(void)sig;
|
|
rt_tick_advance();
|
|
}
|
|
|
|
__attribute__((noreturn)) void rt_idle(void)
|
|
{
|
|
rt_task_yield();
|
|
sigset_t sigset;
|
|
sigfillset(&sigset);
|
|
sigdelset(&sigset, SIGINT);
|
|
for (;;)
|
|
{
|
|
/* Block signals and wait for one to occur. */
|
|
block_all_signals(NULL);
|
|
rt_logf("%s waiting for signal\n", rt_task_name());
|
|
int sig;
|
|
sigwait(&sigset, &sig);
|
|
|
|
/* After receiving a signal, re-trigger it and unblock signals. */
|
|
pthread_kill(pthread_self(), sig);
|
|
unblock_all_signals();
|
|
}
|
|
}
|
|
|
|
__attribute__((noreturn)) void rt_trap(void)
|
|
{
|
|
exit(0);
|
|
}
|
|
|
|
void rt_task_drop_privilege(void)
|
|
{
|
|
}
|
|
|
|
uint32_t rt_cycle(void)
|
|
{
|
|
#if defined(__aarch64__)
|
|
uint64_t cycles;
|
|
__asm__ __volatile__("mrs %0, cntvct_el0" : "=r"(cycles));
|
|
return (uint32_t)cycles;
|
|
#elif defined(__x86_64__)
|
|
uint32_t cycles;
|
|
__asm__ __volatile__("rdtsc" : "=a"(cycles) : : "edx");
|
|
return cycles;
|
|
#else
|
|
return 0;
|
|
#endif
|
|
}
|
|
|
|
__attribute__((noreturn)) void rt_start(void)
|
|
{
|
|
#ifdef __APPLE__
|
|
fprintf(stderr, "running rt on macOS is not supported due to a bug in "
|
|
"pthread_kill\n");
|
|
#endif
|
|
block_all_signals(NULL);
|
|
|
|
/* The tick handler must block SIGSYSCALL. */
|
|
struct sigaction tick_action = {
|
|
.sa_handler = tick_handler,
|
|
};
|
|
sigemptyset(&tick_action.sa_mask);
|
|
sigaddset(&tick_action.sa_mask, SIGSYSCALL);
|
|
sigaction(SIGTICK, &tick_action, NULL);
|
|
|
|
/* The syscall handler doesn't need to block any signals. Add SIGSYSCALL to
|
|
* its own handler mask so that checking the signal mask can be used to
|
|
* detect that we're in a signal handler. */
|
|
struct sigaction syscall_action = {
|
|
.sa_handler = syscall_handler,
|
|
};
|
|
sigemptyset(&syscall_action.sa_mask);
|
|
sigaddset(&syscall_action.sa_mask, SIGSYSCALL);
|
|
sigaction(SIGSYSCALL, &syscall_action, NULL);
|
|
|
|
/* The handler for SIGRESUME is just to catch spurious resumes and error
|
|
* out. Threads expecting a SIGRESUME must block it and sigwait on it. This
|
|
* should be treated as fatal, so block all other normal signals. */
|
|
struct sigaction resume_action = {
|
|
.sa_handler = resume_handler,
|
|
};
|
|
sigemptyset(&resume_action.sa_mask);
|
|
sigaddset(&resume_action.sa_mask, SIGTICK);
|
|
sigaddset(&resume_action.sa_mask, SIGSYSCALL);
|
|
sigaction(SIGRESUME, &resume_action, NULL);
|
|
|
|
static const struct timeval milli = {
|
|
.tv_sec = 0,
|
|
.tv_usec = 1000,
|
|
};
|
|
struct itimerval timer = {
|
|
.it_interval = milli,
|
|
.it_value = milli,
|
|
};
|
|
setitimer(ITIMER_REAL, &timer, NULL);
|
|
|
|
pthread_kill((pthread_t)rt_start_context(), SIGRESUME);
|
|
pthread_exit(NULL);
|
|
}
|
|
|
|
__attribute__((weak)) int main(void)
|
|
{
|
|
rt_start();
|
|
}
|