os.h

#ifndef OS_H_
    #define OS_H_
 
    #include <ucontext.h>
 
    typedef void (*Task)(void);
 
    #define STACKSIZE 4096
    typedef struct {
        ucontext_t context;
        unsigned char stack[STACKSIZE];
    } OS_Task;
 
    void os_init(void);
 
    void os_init_task(OS_Task* os_task, Task task);
 
    void os_start(OS_Task* tasks, int tasks_num);
 
#endif /*OS_H_*/

os.c

#include <stdio.h>
#include <sys/time.h>
 
#include "os.h"
 
#define INTERVAL 100        /* timer interval in nanoseconds */
 
static OS_Task foreground;
 
static OS_Task* os_tasks;
static int os_tasks_num;
 
static OS_Task* current_task;       /* a pointer to the current_context */
static int scheduled = 0;           /* whats the current context? */
 
/* The scheduling algorithm; selects the next context to run, then starts it. */
static void os_scheduler(void) {
    scheduled = (scheduled + 1) % os_tasks_num; /* round robin */
    current_task = &os_tasks[scheduled];
    setcontext(&current_task->context);          /* go */
}
 
/*
  Timer interrupt handler.
  Creates a new context to run the scheduler in, masks signals, then swaps
  contexts saving the previously executing thread and jumping to the scheduler.
*/
static void timer_interrupt(int j, siginfo_t *si, void *old_context) {
    /* Create new scheduler context */
    getcontext(&foreground.context);
    foreground.context.uc_stack.ss_sp = foreground.stack;
    foreground.context.uc_stack.ss_size = STACKSIZE;
    foreground.context.uc_stack.ss_flags = 0;
    sigemptyset(&foreground.context.uc_sigmask);
    makecontext(&foreground.context, os_scheduler, 0);
 
    /* save running thread, jump to scheduler */
    swapcontext(&current_task->context, &foreground.context);
}
 
void os_init_task(OS_Task* os_task, Task task) {
    /* we need to initialize the ucontext structure, give it a stack, flags, and a sigmask */
    getcontext(&os_task->context);
    os_task->context.uc_stack.ss_sp = os_task->stack;
    os_task->context.uc_stack.ss_size = STACKSIZE;
    os_task->context.uc_stack.ss_flags = 0;
    sigemptyset(&os_task->context.uc_sigmask);
 
    /* setup the function we're going to, and n-1 arguments. */
    makecontext(&os_task->context, task, 0);
}
 
void os_start(OS_Task* tasks, int tasks_num) {
    struct sigaction act;
    struct itimerval it;
 
    os_tasks     = tasks;
    os_tasks_num = tasks_num;
 
    /* Set up SIGALRM signal handler */
    act.sa_sigaction = timer_interrupt;
    sigemptyset(&act.sa_mask);
    act.sa_flags = SA_RESTART | SA_SIGINFO;
    if (sigaction(SIGALRM, &act, NULL) != 0) {
        perror("Signal handler");
    }
 
    it.it_interval.tv_sec = 0;
    it.it_interval.tv_usec = INTERVAL * 1000;
    it.it_value = it.it_interval;
    if (setitimer(ITIMER_REAL, &it, NULL) ) perror("setitiimer");
 
    /* force a swap to the first context */
    current_task = &os_tasks[0];
    setcontext(&os_tasks[0].context);
}

test.c

#include <stdio.h>
#include <pthread.h>
 
#include "os.h"
 
void task_a(void);
void task_b(void);
 
#define TASKS_NUM   2
 
OS_Task tasks[TASKS_NUM];
 
int main(int argc, char** argv) {
    os_init_task(&tasks[0], task_a);
    os_init_task(&tasks[1], task_b);
 
    os_start(tasks, TASKS_NUM);
 
    return 0;
}
 
void task_a() {
    int i;
    clock_t t1;
 
    while (1) {
        for (i=0; i < 10; i++) {
            printf("A[%d]\n", i);
 
            t1 = clock();
            while (((double)(clock()-t1) / CLOCKS_PER_SEC) < 1.0);
        }
    }
}
 
void task_b() {
    int i;
    clock_t t1;
 
    while (1) {
        for (i=0; i < 10; i++) {
            printf("B[%d]\n", i);
 
            t1 = clock();
            while (((double)(clock()-t1) / CLOCKS_PER_SEC) < 0.5);
        }
    }
}