post_coro_blink.ino

#include <assert.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>

/*
 * Includes map.h for macro magic by William Swanson in 2012.
 * start map.h
 */

/*
 * map.h by William Swanson in 2012.
 * I, William Swanson, dedicate this work to the public domain.
 * I waive all rights to the work worldwide under copyright law,
 * including all related and neighboring rights,
 * to the extent allowed by law.
 *
 * You can copy, modify, distribute and perform the work,
 * even for commercial purposes, all without asking permission.
 */

#ifndef MAP_H_INCLUDED
#define MAP_H_INCLUDED

#define EVAL0(...) __VA_ARGS__
#define EVAL1(...) EVAL0(EVAL0(EVAL0(__VA_ARGS__)))
#define EVAL2(...) EVAL1(EVAL1(EVAL1(__VA_ARGS__)))
#define EVAL3(...) EVAL2(EVAL2(EVAL2(__VA_ARGS__)))
#define EVAL4(...) EVAL3(EVAL3(EVAL3(__VA_ARGS__)))
#define EVAL(...) EVAL4(EVAL4(EVAL4(__VA_ARGS__)))

#define MAP_END(...)
#define MAP_OUT
#define MAP_COMMA ,

#define MAP_GET_END2() 0, MAP_END
#define MAP_GET_END1(...) MAP_GET_END2
#define MAP_GET_END(...) MAP_GET_END1
#define MAP_NEXT0(test, next, ...) next MAP_OUT
#define MAP_NEXT1(test, next) MAP_NEXT0(test, next, 0)
#define MAP_NEXT(test, next) MAP_NEXT1(MAP_GET_END test, next)

#define MAP0(f, x, peek, ...) f(x) MAP_NEXT(peek, MAP1)(f, peek, __VA_ARGS__)
#define MAP1(f, x, peek, ...) f(x) MAP_NEXT(peek, MAP0)(f, peek, __VA_ARGS__)

#define MAP_LIST_NEXT1(test, next) MAP_NEXT0(test, MAP_COMMA next, 0)
#define MAP_LIST_NEXT(test, next) MAP_LIST_NEXT1(MAP_GET_END test, next)

#define MAP_LIST0(f, x, peek, ...) f(x) MAP_LIST_NEXT(peek, MAP_LIST1)(f, peek, __VA_ARGS__)
#define MAP_LIST1(f, x, peek, ...) f(x) MAP_LIST_NEXT(peek, MAP_LIST0)(f, peek, __VA_ARGS__)

/**
  * Applies the function macro `f` to each of the remaining parameters.
  */
#define MAP(f, ...) EVAL(MAP1(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0))

/**
  * Applies the function macro `f` to each of the remaining parameters and
  * inserts commas between the results.
  */
#define MAP_LIST(f, ...) EVAL(MAP_LIST1(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0))

#define EMPTY()
#define DEFER(id) id EMPTY()
#define OBSTRUCT(...) __VA_ARGS__ DEFER(EMPTY)()
#define EXPAND(...) __VA_ARGS__

#endif
/* end map.h */


typedef uint8_t coro_res_t;
#define CORO_RES_PENDING 0x80
#define CORO_RES_PENDING_NON_PARKING 0xC0
#define CORO_RES_CANCELED 0x03
#define CORO_RES_DONE 0x01

#define CORO_NO_LOCALS ;
#define CORO_LOCALS(...) \
  ; \
  __VA_ARGS__

#define CORO_ANY_CALL(state_name, fnct_a, fnct_b) (state_name, { struct fnct_a##_fct_ctx a; struct fnct_b##_fct_ctx b; })
#define CORO_CALL(state_name, fnct) (state_name, fnct##_fct_ctx)

#define CORO_CALLS(...) __VA_ARGS__
#define CORO_CALL_UNION_INNER(state_name, ctx_struct) \
  struct ctx_struct state_name;

#define CORO_CALL_UNION(args) CORO_CALL_UNION_INNER args
#define CORO_STATE_JMP_INNER(state_name, fnct) \
  case coro_state_##state_name: \
    goto coro_lbl_##state_name;

#define CORO_STATE_JMP(args) CORO_STATE_JMP_INNER args
#define CORO_STATE_DEF_NAME_INNER(state_name, ctx_struct) coro_state_##state_name
#define CORO_STATE_DEF_NAME(args) CORO_STATE_DEF_NAME_INNER args

#define CORO_NO_ARGS *ctx
#define CORO_ARGS(...) *ctx, __VA_ARGS__

#define CORO(name, args, locals_inner, calls_inner, body) \
  enum name##_fct_state{ name##_state_initial = 0, \
                         MAP_LIST(CORO_STATE_DEF_NAME, calls_inner) }; \
  struct name##_fct_ctx { \
    enum name##_fct_state state locals_inner union { \
      uint8_t _placeholder_; \
      MAP(CORO_CALL_UNION, calls_inner) \
    } calls; \
  }; \
  coro_res_t name##_fct(struct name##_fct_ctx args) { \
    switch (ctx->state) { \
      case name##_state_initial: \
        body \
    } \
  }

#define LOCAL(name) (ctx->name)

#define CALL(res, state_name, fnct, ...) \
  do { \
    ctx->state = coro_state_##state_name; \
    ctx->calls.state_name.state = (enum fnct##_fct_state)0; \
    case coro_state_##state_name: \
      res = fnct##_fct(&ctx->calls.state_name, ##__VA_ARGS__); \
      if (res & CORO_RES_PENDING) \
        return res; \
  } while (0)

#define ANY_CALL(res_a, res_b, state_name, fnct_a, fnct_a_args, fnct_b, fnct_b_args) \
  do { \
    ctx->state = coro_state_##state_name; \
    ctx->calls.state_name.a.state = (enum fnct_a##_fct_state)0; \
    ctx->calls.state_name.b.state = (enum fnct_b##_fct_state)0; \
    case coro_state_##state_name: \
      res_a = fnct_a##_fct(&ctx->calls.state_name.a, EXPAND fnct_a_args); \
      res_b = fnct_b##_fct(&ctx->calls.state_name.b, EXPAND fnct_b_args); \
      if (res_a & CORO_RES_DONE && !(res_b & CORO_RES_DONE)) { \
        current_task->canceled++; \
        res_b = fnct_b##_fct(&ctx->calls.state_name.b, EXPAND fnct_b_args); \
        current_task->canceled--; \
      } else if (res_b & CORO_RES_DONE && !(res_a & CORO_RES_DONE)) { \
        current_task->canceled++; \
        res_a = fnct_a##_fct(&ctx->calls.state_name.a, EXPAND fnct_a_args); \
        current_task->canceled--; \
      } \
      if ((res_a | res_b) & CORO_RES_PENDING) \
        return (res_a | res_b); \
  } while (0)

#define DECLARE_TASK(name, fnct) \
  static struct fnct##_fct_ctx name##_ctx; \
  static struct coro_task name = { .state = coro_task_state_not_started, \
                                   .executor = NULL, \
                                   .next = NULL, \
                                   .root_fct = (coro_task_root_fct)&fnct##_fct, \
                                   .canceled = 0, \
                                   .context = &name##_ctx }

#define DECLARE_EXECUTOR(name) \
  static struct coro_executor name = { .task_queue_head = NULL, .task_queue_tail = NULL }

enum coro_task_state {
  coro_task_state_not_started = 0,
  coro_task_state_running = 1,
  coro_task_state_waiting_for_execution = 2,
  coro_task_state_parked = 3,
  coro_task_state_finished = 4,
  coro_task_state_failed = 5,
};

struct coro_task;

struct coro_task *current_task = NULL;

struct coro_executor {
  struct coro_task *task_queue_head;
  struct coro_task *task_queue_tail;
};

typedef coro_res_t (*coro_task_root_fct)(void *ctx);

struct coro_task {
  enum coro_task_state state;

  struct coro_executor *executor;
  struct coro_task *next;
  coro_task_root_fct root_fct;
  uint8_t canceled;
  void *context;
};

void coro_executor_enqueue_task(struct coro_executor *executor,
                                struct coro_task *task) {

  assert(task->state == coro_task_state_waiting_for_execution);
  assert(task->executor == executor);

  struct coro_task **task_queue_tail = &executor->task_queue_tail;
  if (*task_queue_tail)  // if queue not empty
    (*task_queue_tail)->next = task;
  else  // if(!executor->task_queue_head)
    executor->task_queue_head = task;
  *task_queue_tail = task;
}

void coro_executor_start_task(struct coro_executor *executor,
                              struct coro_task *task) {

  assert(task->state == coro_task_state_not_started);
  assert(task->executor == NULL);
  task->state = coro_task_state_waiting_for_execution;
  task->executor = executor;

  coro_executor_enqueue_task(executor, task);
}

void coro_executor_process(struct coro_executor *executor) {
  struct coro_task **task_queue_head = &executor->task_queue_head;

  while (*task_queue_head != NULL) {
    struct coro_task *task = *task_queue_head;
    if (task->state == coro_task_state_waiting_for_execution) {
      task->state = coro_task_state_running;

      current_task = task;
      coro_res_t res = task->root_fct(task->context);
      if (res == CORO_RES_DONE) {
        task->state = coro_task_state_finished;
      } else if (res == CORO_RES_CANCELED) {
        task->state = coro_task_state_failed;
      } else if (res == CORO_RES_PENDING) {
        task->state = coro_task_state_parked;
      } else if (res == CORO_RES_PENDING_NON_PARKING) {
        task->state = coro_task_state_waiting_for_execution;
        coro_executor_enqueue_task(executor, task);
      } else {
        assert(0);
      }
    }
    *task_queue_head = task->next;
    task->next = NULL;
  }
  executor->task_queue_tail = NULL;
  current_task = NULL;
}

enum coro_park_fct_state {
  park_state_init = 0,
  park_state_after_parked = 1,
};

struct coro_park_fct_ctx {
  enum coro_park_fct_state state;
};

coro_res_t coro_park_fct(struct coro_park_fct_ctx *ctx) {
  if (current_task->canceled)
    return CORO_RES_CANCELED;
  if (ctx->state == park_state_init) {
    ctx->state = park_state_after_parked;
    return CORO_RES_PENDING;
  } else {
    return CORO_RES_DONE;
  }
}


enum coro_yield_fct_state {
  yield_state_init = 0,
  yield_state_after_yield = 1,
};
struct coro_yield_fct_ctx {
  enum coro_yield_fct_state state;
};

coro_res_t coro_yield_fct(struct coro_yield_fct_ctx *ctx) {
  if (current_task->canceled)
    return CORO_RES_CANCELED;
  if (ctx->state == yield_state_init) {
    ctx->state = yield_state_after_yield;
    return CORO_RES_PENDING_NON_PARKING;
  } else {
    return CORO_RES_DONE;
  }
}

void coro_unpark_task(struct coro_task *task) {
  assert(task->state != coro_task_state_not_started);
  assert(task->executor != NULL);

  if (task->state == coro_task_state_parked) {
    task->state = coro_task_state_waiting_for_execution;

    coro_executor_enqueue_task(task->executor, task);
  }
}

enum coro_cond_var_waiter_state {
  coro_cond_var_waiter_idle = 0,
  coro_cond_var_waiter_waiting = 1,
  coro_cond_var_waiter_signaled = 2,
};

struct coro_cond_var_waiter {
  enum coro_cond_var_waiter_state state;
  struct coro_cond_var_waiter *next;
  struct coro_task *parked_task;
};

struct coro_cond_var {
  struct coro_cond_var_waiter *waiter_head;
};

#define DECLARE_COND_VAR(name) \
  struct coro_cond_var name = { .waiter_head = NULL };

void coro_cond_var_notify(struct coro_cond_var *cond_var) {
  struct coro_cond_var_waiter **waiter_head = &cond_var->waiter_head;
  while (*waiter_head != NULL) {
    if ((*waiter_head)->state == coro_cond_var_waiter_waiting) {
      (*waiter_head)->state = coro_cond_var_waiter_signaled;
      coro_unpark_task((*waiter_head)->parked_task);
    }
    *waiter_head = (*waiter_head)->next;
  }
}

static void coro_cond_var_add_waiter(struct coro_cond_var *cond_var,
                                     struct coro_cond_var_waiter *waiter) {
  struct coro_cond_var_waiter **waiter_head = &cond_var->waiter_head;
  waiter->next = *waiter_head;
  *waiter_head = waiter;
}

static void coro_cond_var_remove_waiter(struct coro_cond_var *cond_var,
                                        struct coro_cond_var_waiter *waiter) {

  for (struct coro_cond_var_waiter **waiter_head = &cond_var->waiter_head;
       *waiter_head != NULL; waiter_head = &(*waiter_head)->next) {
    if (*waiter_head == waiter) {
      *waiter_head = (*waiter_head)->next;
      break;
    }
  }
}

CORO(coro_cond_var_wait, CORO_ARGS(struct coro_cond_var *cond_var),
     CORO_LOCALS(struct coro_cond_var_waiter waiter;),
     CORO_CALLS(CORO_CALL(coro_cond_var_wait_park, coro_park)), {
       coro_res_t res;

       coro_cond_var_add_waiter(cond_var, &LOCAL(waiter));

       do {
         LOCAL(waiter).state = coro_cond_var_waiter_waiting;
         LOCAL(waiter).parked_task = current_task;
         CALL(res, coro_cond_var_wait_park, coro_park);
         if (res == CORO_RES_CANCELED && ctx->waiter.state != coro_cond_var_waiter_signaled) {
           printf("canceled\n");
           coro_cond_var_remove_waiter(cond_var, &LOCAL(waiter));
           return CORO_RES_CANCELED;
         }
       } while (ctx->waiter.state != coro_cond_var_waiter_signaled);
       return CORO_RES_DONE;
     })

#define BUTTON_PIN 2

uint64_t led_blink_duration_ms = 1000;
DECLARE_COND_VAR(reset_led_signal);



CORO(wait_ms, 
     CORO_ARGS(uint64_t delay), 
     CORO_LOCALS(uint64_t end_time;),
     CORO_CALLS(CORO_CALL(wait_ms_yield, coro_yield)), {
       coro_res_t res;
       LOCAL(end_time) = millis() + delay;
       while (LOCAL(end_time) >= millis()) {
         CALL(res, wait_ms_yield, coro_yield);
       }
       return CORO_RES_DONE;
     })

CORO(led_task_fn,
     CORO_NO_ARGS,
     CORO_NO_LOCALS,
     CORO_CALLS(
        CORO_ANY_CALL(wait_a, wait_ms, coro_cond_var_wait), 
        CORO_ANY_CALL(wait_b, wait_ms, coro_cond_var_wait)), {
       coro_res_t res_a;
       coro_res_t res_b;
       while (true) {
         digitalWrite(LED_BUILTIN, LOW);
         ANY_CALL(res_a, res_b, wait_a, wait_ms, (led_blink_duration_ms), coro_cond_var_wait, (&reset_led_signal));
         if (res_b == CORO_RES_DONE) continue;
         digitalWrite(LED_BUILTIN, HIGH);
         ANY_CALL(res_a, res_b, wait_b, wait_ms, (led_blink_duration_ms), coro_cond_var_wait, (&reset_led_signal));
         if (res_b == CORO_RES_DONE) continue;
       }
       return CORO_RES_DONE;
     })


CORO(wait_pin, 
     CORO_ARGS(int pin, int state),
     CORO_NO_LOCALS,
     CORO_CALLS(CORO_CALL(wait_pin_yield, coro_yield)), {
       coro_res_t res;
       while (digitalRead(pin) != state) {
         CALL(res, wait_pin_yield, coro_yield);
       }
       return CORO_RES_DONE;
     })

CORO(button_task_fn,
     CORO_NO_ARGS,
     CORO_LOCALS(uint64_t button_pressed_start_time;),
     CORO_CALLS(
        CORO_CALL(wait_pin_low, wait_pin),
        CORO_CALL(wait_pin_high, wait_pin)), {
       coro_res_t res;
       while (true) {
         CALL(res, wait_pin_low, wait_pin, BUTTON_PIN, LOW);
         LOCAL(button_pressed_start_time) = millis();
         CALL(res, wait_pin_high, wait_pin, BUTTON_PIN, HIGH);
         uint64_t button_pressed_end_time = millis();
         led_blink_duration_ms = button_pressed_end_time - LOCAL(button_pressed_start_time);
         coro_cond_var_notify(&reset_led_signal);
       }
       return CORO_RES_DONE;
     })

DECLARE_TASK(led_task, led_task_fn);
DECLARE_TASK(button_task, button_task_fn);
DECLARE_EXECUTOR(exe);

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  coro_executor_start_task(&exe, &led_task);
  coro_executor_start_task(&exe, &button_task);
}

void loop() {
  coro_executor_process(&exe);
}