Routine infrastructure

CMakeLists.txt

@@ -3,5 +3,16 @@ find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE})
 
 project(openrcore)
 
-target_sources(app PRIVATE core/main.c)
-target_sources(app PRIVATE core/ble.c)
+target_include_directories(app PRIVATE .)
+
+target_sources(app PRIVATE
+    core/main.c
+    core/ble.c
+    core/routine.c
+)
+
+# User routines to compile
+target_sources(app PRIVATE
+    routines/routine_list.c
+    routines/input_echo.c
+)

core/ble.c

@@ -9,7 +9,8 @@
 #include <zephyr/sys/util_macro.h>
 #include <zephyr/types.h>
 
-#include "ble.h"
+#include "core/ble.h"
+#include "core/routine.h"
 
 /**
  * @brief Begin BLE advertising with the default parameters
@@ -35,7 +36,7 @@ static struct bt_conn_cb connection_callbacks = {
 
 /** Advertising data **/
 /** Our name to advertise */
-#define BLE_NAME_DEFAULT ("FM25 Kit 1")
+#define BLE_NAME_DEFAULT ("FM25 Kit x")
 
 /** Advertising/Scanning parameters */
 static struct bt_le_adv_param advertise_params = {
@@ -107,6 +108,11 @@ static struct bt_uuid_16 characteristic_description_uuid = {
     .val = BT_UUID_GATT_CUD_VAL,
 };
 
+static struct bt_uuid_16 characteristic_format_uuid = {
+    .uuid = { .type = BT_UUID_TYPE_16 },
+    .val = BT_UUID_GATT_CPF_VAL,
+};
+
 static struct bt_uuid_128 service_uuid = {
     .uuid = { .type = BT_UUID_TYPE_128 },
     .val = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
@@ -122,7 +128,25 @@ static struct bt_uuid_128 telemetry_characteristic_uuid = {
     .val = { 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
 };
 
+static struct bt_uuid_128 routine_list_characteristic_uuid = {
+    .uuid = { .type = BT_UUID_TYPE_128 },
+    .val = { 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4 },
+};
+
+static struct bt_uuid_128 current_routine_characteristic_uuid = {
+    .uuid = { .type = BT_UUID_TYPE_128 },
+    .val = { 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5 },
+};
+
 /*** Characteristic Data ***/
+static struct bt_gatt_cpf string_format = {
+    .format = 0x19, /* UTF8 string */
+    .exponent = 0,
+    .unit = 0,
+    .name_space = 0x01, /* Bluetooth SIG */
+    .description = 0,
+};
+
 typedef struct ble_attribute_userdata_struct {
     /** Buffer allocated for this attribute's data */
     void *buffer;
@@ -155,7 +179,7 @@ char input_characteristic_description[] = "Input Data";
 
 /**** Telemetry Characteristic ****/
 struct bt_gatt_chrc telemetry_characteristic = {
-    .uuid = (void *)&telemetry_characteristic_uuid,
+    .uuid = (struct bt_uuid *)&telemetry_characteristic_uuid,
     .properties = BT_GATT_CHRC_READ,
 };
 /** Buffer for the contents of the Telemetry characteristic */
@@ -171,6 +195,45 @@ ble_attribute_userdata_t telemetry_characteristic_userdata = {
 char telemetry_characteristic_description[] = "Telemetry Data";
 
 
+/**** Routine List Characteristic ****/
+extern const routine_listing_t *routine_list;
+struct bt_gatt_chrc routine_list_characteristic = {
+    .uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
+    .properties = BT_GATT_CHRC_READ,
+};
+char routine_list_characteristic_buffer[512];
+ble_attribute_userdata_t routine_list_characterist_userdata = {
+    .buffer = routine_list_characteristic_buffer,
+    .buffer_size = sizeof(routine_list_characteristic_buffer),
+    .value_size = 0,
+};
+char routine_list_characteristic_description[] = "Routine List";
+
+
+
+/**** Current Routine Characteristic ****/
+struct bt_gatt_chrc current_routine_characteristic = {
+    .uuid = (struct bt_uuid *)&current_routine_characteristic_uuid,
+    .properties = BT_GATT_CHRC_READ | BT_GATT_CHRC_WRITE,
+};
+static ssize_t _read_current_routine(
+    struct bt_conn *connection,
+    const struct bt_gatt_attr *attribute,
+    void *buffer,
+    uint16_t buffer_len,
+    uint16_t offset
+);
+static ssize_t _write_current_routine(
+    struct bt_conn *connection,
+    const struct bt_gatt_attr *attribute,
+    const void *buffer,
+    uint16_t buffer_len,
+    uint16_t offset,
+    uint8_t flags
+);
+char current_routine_characteristic_description[] = "Current Routine";
+
+
 /*** Service Layout ***/
 static struct bt_gatt_attr primary_service_attributes[] = {
     /* Service declaration */
@@ -200,6 +263,12 @@ static struct bt_gatt_attr primary_service_attributes[] = {
         .user_data = (void *)input_characteristic_description,
         .perm = BT_GATT_PERM_READ,
     },
+    {
+        .uuid = (struct bt_uuid *)&characteristic_format_uuid,
+        .read = bt_gatt_attr_read_cpf,
+        .user_data = (void *)&string_format,
+        .perm = BT_GATT_PERM_READ,
+    },
     /** Telemetry Data Characteristic **/
     {
         .uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
@@ -218,7 +287,57 @@ static struct bt_gatt_attr primary_service_attributes[] = {
         .read = bt_gatt_attr_read_cud,
         .user_data = (void *)telemetry_characteristic_description,
         .perm = BT_GATT_PERM_READ,
-    }
+    },
+    {
+        .uuid = (struct bt_uuid *)&characteristic_format_uuid,
+        .read = bt_gatt_attr_read_cpf,
+        .user_data = (void *)&string_format,
+        .perm = BT_GATT_PERM_READ,
+    },
+    /** Routine List Characteristic **/
+    {
+        .uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
+        .read = bt_gatt_attr_read_chrc,
+        .user_data = (void *)&routine_list_characteristic,
+        .perm = BT_GATT_PERM_READ,
+    },
+    {
+        .uuid = (struct bt_uuid *)&routine_list_characteristic_uuid,
+        .read = _generic_attribute_read,
+        .user_data = (void *)&routine_list_characterist_userdata,
+        .perm = BT_GATT_PERM_READ,
+    },
+    {
+        .uuid = (struct bt_uuid *)&characteristic_description_uuid,
+        .read = bt_gatt_attr_read_cud,
+        .user_data = (void *)routine_list_characteristic_description,
+        .perm = BT_GATT_PERM_READ,
+    },
+    {
+        .uuid = (struct bt_uuid *)&characteristic_format_uuid,
+        .read = bt_gatt_attr_read_cpf,
+        .user_data = (void *)&string_format,
+        .perm = BT_GATT_PERM_READ,
+    },
+    /** Current Routine Characteristic */
+    {
+        .uuid = (struct bt_uuid *)&characteristic_declaration_uuid,
+        .read = bt_gatt_attr_read_chrc,
+        .user_data = (void *)&current_routine_characteristic,
+        .perm = BT_GATT_PERM_READ,
+    },
+    {
+        .uuid = (struct bt_uuid *)&current_routine_characteristic_uuid,
+        .read = _read_current_routine,
+        .write = _write_current_routine,
+        .perm = BT_GATT_PERM_READ | BT_GATT_PERM_WRITE
+    },
+    {
+        .uuid = (struct bt_uuid *)&characteristic_description_uuid,
+        .read = bt_gatt_attr_read_cud,
+        .user_data = (void *)current_routine_characteristic_description,
+        .perm = BT_GATT_PERM_READ,
+    },
 };
 
 static struct bt_gatt_service primary_service = {
@@ -231,6 +350,8 @@ static struct bt_gatt_service primary_service = {
 typedef struct ble_state_struct {
     /** Current callback for a complete input write */
     void (*oninput_callback)(const void *buffer, uint16_t buffer_len);
+#define BLE_FLAG_DISCONNECTED (0x01)
+    uint32_t flags;
     /** Lock for data in the BLE state */
     struct k_mutex lock;
 } ble_state_t;
@@ -257,6 +378,8 @@ ble_init()
     /* Initialize the service data */
     k_mutex_init(&input_characteristic_userdata.lock);
     k_mutex_init(&telemetry_characteristic_userdata.lock);
+    routine_get_listing(routine_list_characteristic_buffer, sizeof(routine_list_characteristic_buffer));
+    routine_list_characterist_userdata.value_size = strlen(routine_list_characteristic_buffer);
 
     ret = bt_gatt_service_register(&primary_service);
     if (ret) {
@@ -268,6 +391,17 @@ ble_init()
     return 0;
 }
 
+void
+ble_tick()
+{
+    k_mutex_lock(&ble.lock, K_FOREVER);
+    if (ble.flags & BLE_FLAG_DISCONNECTED) {
+        _start_advertising();
+        ble.flags &= ~BLE_FLAG_DISCONNECTED;
+    }
+    k_mutex_unlock(&ble.lock);
+}
+
 void
 ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buffer_len))
 {
@@ -276,6 +410,14 @@ ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buff
     k_mutex_unlock(&ble.lock);
 }
 
+void
+ble_clear_oninput_callback()
+{
+    k_mutex_lock(&ble.lock, K_FOREVER);
+    ble.oninput_callback = NULL;
+    k_mutex_unlock(&ble.lock);
+}
+
 void
 ble_set_telemetry_data(const void *buffer, uint16_t buffer_len)
 {
@@ -314,7 +456,9 @@ static void
 _disconnected_callback(struct bt_conn *connection, uint8_t reason)
 {
     /* Resume advertising so we can be found again */
-    _start_advertising();
+    k_mutex_lock(&ble.lock, K_FOREVER);
+    ble.flags |= BLE_FLAG_DISCONNECTED;
+    k_mutex_unlock(&ble.lock);
 }
 
 static ssize_t
@@ -346,7 +490,7 @@ _generic_attribute_read(
     return read_len;
 }
 
-static int _generic_attribute_write(
+static ssize_t _generic_attribute_write(
     struct bt_conn *connection,
     const struct bt_gatt_attr *attribute,
     const void *buffer,
@@ -381,3 +525,42 @@ static int _generic_attribute_write(
 
     return write_len;
 }
+
+static ssize_t _read_current_routine(
+    struct bt_conn *connection,
+    const struct bt_gatt_attr *attribute,
+    void *buffer,
+    uint16_t buffer_len,
+    uint16_t offset
+) {
+    if ((offset != 0) || (buffer_len < sizeof(int8_t))) {
+        return (0);
+    }
+
+    ((int8_t *)buffer)[0] = routine_get_current();
+
+    return (sizeof(int8_t));
+}
+
+static ssize_t _write_current_routine(
+    struct bt_conn *connection,
+    const struct bt_gatt_attr *attribute,
+    const void *buffer,
+    uint16_t buffer_len,
+    uint16_t offset,
+    uint8_t flags
+) {
+    uint8_t index;
+
+    if ((offset != 0) || (buffer_len < sizeof(uint8_t))) {
+        return (0);
+    }
+
+    index = ((uint8_t *)buffer)[0];
+
+    if (routine_select(index)) {
+        return (0);
+    } else {
+        return (sizeof(uint8_t));
+    }
+}

core/ble.h

@@ -1,19 +1,15 @@
 #ifndef _BLE_H_
 #define _BLE_H_
 
+#include <stdint.h>
+
+#include "includes/ble.h"
+
 /**
  * @brief Enable Bluetooth, setup default services, and begin advertising
  */
 int ble_init(void);
 
-/**
- * @brief Register a callback to occur when a new complete input is sent
- */
-void ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buffer_len));
-
-/**
- * @brief Set a new value for telemetry data
- */
-void ble_set_telemetry_data(const void *buffer, uint16_t buffer_len);
+void ble_tick(void);
 
 #endif /* _BLE_H_ */

core/main.c

@@ -3,23 +3,14 @@
 
 #include <zephyr/kernel.h>
 
-#include "ble.h"
+#include "core/ble.h"
+#include "core/routine.h"
 
-static void oninput(const void *buffer, uint16_t buffer_len);
 
 int main(void)
 {
+    routine_init();
     ble_init();
-    ble_register_oninput_callback(oninput);
 
-    while (1) {
-        k_sleep(K_MSEC(1000));
-        k_yield();
-    }
-}
-
-static void
-oninput(const void *buffer, uint16_t buffer_len)
-{
-    ble_set_telemetry_data(buffer, buffer_len);
+    routine_exec();
 }

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();
+}

core/routine.h

@@ -0,0 +1,36 @@
+#ifndef _ROUTINE_H_
+#define _ROUTINE_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "includes/routine.h"
+
+#define ROUTINE_INDEX_NONE (-1)
+
+/**
+ * @brief Initialize the routine state
+ */
+void routine_init(void);
+
+/**
+ * @brief Execute the routine, intended to be called from main. Does not return.
+ */
+void routine_exec(void);
+
+/**
+ * @brief Select a new routine to be active
+ */
+int routine_select(uint8_t index);
+
+/**
+ * @brief Fill out a buffer with a human readable list of routines
+ */
+void routine_get_listing(char *buf, uint16_t buf_len);
+
+/**
+ * @brief Get the index of the current running routine
+ */
+int8_t routine_get_current(void);
+
+#endif /* _ROUTINE_H_ */

includes/ble.h

@@ -0,0 +1,22 @@
+#ifndef _INCLUDES_BLE_H_
+#define _INCLUDES_BLE_H_
+
+#include <stdint.h>
+
+/**
+ * @brief Register a callback to occur when a new complete input is sent
+ */
+void ble_register_oninput_callback(void (*callback)(const void *buffer, uint16_t buffer_len));
+
+/**
+ * @brief Clear the on-input callback registered with
+ * ble_register_oninput_callback
+ */
+void ble_clear_oninput_callback(void);
+
+/**
+ * @brief Set a new value for telemetry data
+ */
+void ble_set_telemetry_data(const void *buffer, uint16_t buffer_len);
+
+#endif /* _INCLUDES_BLE_H_ */

includes/routine.h

@@ -0,0 +1,47 @@
+#ifndef _INCLUDES_ROUTINE_H_
+#define _INCLUDES_ROUTINE_H_
+
+#include <stdint.h>
+
+typedef enum routine_return_enum {
+    /** Routine executed successfully */
+    ROUTINE_RETURN_SUCCESS = 0,
+    /** Generic error occured during routine */
+    ROUTINE_RETURN_FAILURE,
+} routine_return_e;
+
+typedef routine_return_e (*routine_t)(void);
+
+typedef struct routine_listing_struct {
+    /** Method that will be called to enter the routine */
+    routine_t entry;
+    /** Name that will be listed for the routine */
+    const char *name;
+} routine_listing_t;
+
+typedef enum routine_status_enum {
+    /** no routine is currently running */
+    ROUTINE_STATUS_STOPPED = 0,
+    /** a routine is running */
+    ROUTINE_STATUS_RUNNING,
+    /** a routine is running but has been requested to stop*/
+    ROUTINE_STATUS_STOPPING,
+} routine_status_e;
+
+/**
+ * @brief Get the current routine status
+ */
+routine_status_e routine_status(void);
+
+/**
+ * @brief Delay the routine for the specified number of milliseconds
+ */
+void routine_delay(uint32_t milliseconds);
+
+/**
+ * @brief Function that a routine should call while running in a loop to allow
+ * for the rest of the system to properly execute
+ */
+void routine_yield(void);
+
+#endif /* _INCLUDE_ROUTINE_H_ */

prj.conf

@@ -5,4 +5,6 @@ CONFIG_BT_PERIPHERAL=y
 CONFIG_BT_DEVICE_NAME="FM Sample 1"
 CONFIG_BT_GATT_DYNAMIC_DB=y
 
+CONFIG_HEAP_MEM_POOL_SIZE=1024
+
 CONFIG_DEBUG=y

routines/input_echo.c

@@ -0,0 +1,44 @@
+#include <stdint.h>
+#include <string.h>
+
+#include <zephyr/kernel.h>
+
+#include "includes/routine.h"
+#include "includes/ble.h"
+
+static void ble_input_callback(const void *buffer, uint16_t buffer_len);
+
+routine_return_e
+input_echo()
+{
+    ble_register_oninput_callback(ble_input_callback);
+
+    while (routine_status() == ROUTINE_STATUS_RUNNING) {
+        routine_yield();
+    }
+
+    ble_clear_oninput_callback();
+    return (ROUTINE_RETURN_SUCCESS);
+}
+
+static void
+ble_input_callback(const void *buffer, uint16_t buffer_len)
+{
+    size_t echo_length = snprintk(
+        NULL, 0, "Echo: %.*s",
+        buffer_len, (const char *)buffer
+    );
+    char *echo_buf = k_malloc(echo_length + 1);
+
+    if (echo_buf == NULL) {
+        ble_set_telemetry_data("Alloc failure", 13);
+        return;
+    }
+
+    snprintk(
+        echo_buf, echo_length + 1, "Echo: %.*s",
+        buffer_len, (const char *)buffer
+    );
+    ble_set_telemetry_data(echo_buf, echo_length);
+    k_free(echo_buf);
+}

routines/routine_list.c

@@ -0,0 +1,11 @@
+#include <stddef.h>
+
+#include "core/routine.h"
+
+/** Extern all of your routines here so they can be included in the list **/
+extern routine_return_e input_echo();
+
+const routine_listing_t routine_list[] = {
+    { .entry = input_echo, .name = "Input Echo" },
+    { NULL, NULL },
+};