Routine infrastructure

core/routine.c

@@ -0,0 +1,154 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <zephyr/kernel.h>
+
+#include "core/ble.h"
+#include "core/routine.h"
+
+extern const routine_listing_t *routine_list;
+
+typedef struct routine_state_struct {
+    uint8_t routine_count;
+    /** Index of the current routine */
+    int8_t current_routine;
+    /** Index of the next routine */
+    int8_t next_routine;
+    /** Return status of the last routine to complete execution */
+    routine_return_e last_routine_ret;
+    /** Routine status */
+    routine_status_e status;
+    /** Lock for the */
+    struct k_mutex lock;
+} routine_state_t;
+
+static routine_state_t routine_state = {
+    .routine_count = 0,
+    .current_routine = ROUTINE_INDEX_NONE,
+    .next_routine = ROUTINE_INDEX_NONE,
+};
+
+void
+routine_init()
+{
+    k_mutex_init(&routine_state.lock);
+
+    while (1) {
+        const routine_listing_t *listing = &routine_list[routine_state.routine_count];
+
+        if (listing->entry == NULL) {
+            break;
+        }
+
+        routine_state.routine_count++;
+    }
+}
+
+void
+routine_exec()
+{
+    while (1) {
+        routine_return_e ret;
+
+        ble_tick();
+
+        k_mutex_lock(&routine_state.lock, K_FOREVER);
+
+        if (routine_state.next_routine == ROUTINE_INDEX_NONE) {
+            /* nothing to do */
+            k_mutex_unlock(&routine_state.lock);
+            k_yield();
+            continue;
+        }
+
+        /* something to do! */
+        routine_state.current_routine = routine_state.next_routine;
+        routine_state.next_routine = ROUTINE_INDEX_NONE;
+        routine_state.status = ROUTINE_STATUS_RUNNING;
+
+        k_mutex_unlock(&routine_state.lock);
+
+
+        ret = routine_list[routine_state.current_routine].entry();
+
+        k_mutex_lock(&routine_state.lock, K_FOREVER);
+        routine_state.last_routine_ret = ret;
+        routine_state.current_routine = ROUTINE_INDEX_NONE;
+        routine_state.status = ROUTINE_STATUS_STOPPED;
+        k_mutex_unlock(&routine_state.lock);
+    }
+}
+
+int
+routine_select(uint8_t index)
+{
+    if (index >= routine_state.routine_count) {
+        return (-1);
+    }
+
+    k_mutex_lock(&routine_state.lock, K_FOREVER);
+    routine_state.next_routine = index;
+    routine_state.status = ROUTINE_STATUS_STOPPING;
+    k_mutex_unlock(&routine_state.lock);
+
+    return (0);
+}
+
+void
+routine_get_listing(char *buf, uint16_t buf_len)
+{
+    int written;
+    uint16_t pos = 0;
+
+    written = snprintf(&buf[pos], buf_len - pos, "Routines (%d):\r\n", routine_state.routine_count);
+    pos += written;
+
+    for (int i = 0; i < routine_state.routine_count; i++) {
+        written = snprintf(
+            &buf[pos], buf_len - pos,
+            "%d: %s\r\n",
+            i, routine_list[i].name
+        );
+        pos += written;
+    }
+
+    /* walk back trailing \r\n */
+    buf[pos - 2] = '\0';
+}
+
+int8_t
+routine_get_current()
+{
+    int8_t ret;
+
+    k_mutex_lock(&routine_state.lock, K_FOREVER);
+    ret = routine_state.current_routine;
+    k_mutex_unlock(&routine_state.lock);
+
+    return (ret);
+}
+
+routine_status_e
+routine_status()
+{
+    routine_status_e ret;
+
+    k_mutex_lock(&routine_state.lock, K_FOREVER);
+    ret = routine_state.status;
+    k_mutex_unlock(&routine_state.lock);
+
+    return (ret);
+}
+
+void
+routine_delay(uint32_t milliseconds)
+{
+    k_sleep(K_MSEC(milliseconds));
+}
+
+void
+routine_yield()
+{
+    ble_tick();
+    k_yield();
+}