diff --git a/src/bluetooth/gatt_services/CMakeLists.txt b/src/bluetooth/gatt_services/CMakeLists.txt index dc64e7b9..2f799d59 100644 --- a/src/bluetooth/gatt_services/CMakeLists.txt +++ b/src/bluetooth/gatt_services/CMakeLists.txt @@ -5,4 +5,6 @@ target_sources(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/sensor_service.c ${CMAKE_CURRENT_SOURCE_DIR}/audio_config_service.c ${CMAKE_CURRENT_SOURCE_DIR}/led_service.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/device_error_service.c + ${CMAKE_CURRENT_SOURCE_DIR}/device_error_log_backend.c ) diff --git a/src/bluetooth/gatt_services/device_error_log_backend.c b/src/bluetooth/gatt_services/device_error_log_backend.c new file mode 100644 index 00000000..c6dd2e24 --- /dev/null +++ b/src/bluetooth/gatt_services/device_error_log_backend.c @@ -0,0 +1,230 @@ +#include "device_error_service.h" + +#include + +#include +#include +#include +#include +#include +#include +#include + +/* LOG_LEVEL_WRN forwards LOG_ERR and LOG_WRN. Use LOG_LEVEL_INF to include info logs. */ +#define DEVICE_ERROR_LOG_BACKEND_LEVEL LOG_LEVEL_WRN +#define DEVICE_ERROR_LOG_QUEUE_SIZE 8 +#define DEVICE_ERROR_LOG_FORMAT_BUFFER_SIZE 128 + +struct device_error_log_event { + enum device_error_level level; + uint16_t error_code; + char message[DEVICE_ERROR_MESSAGE_MAX_LENGTH]; +}; + +struct log_capture_context { + char *buffer; + size_t size; + size_t offset; +}; + +K_MSGQ_DEFINE(device_error_log_queue, sizeof(struct device_error_log_event), + DEVICE_ERROR_LOG_QUEUE_SIZE, 4); + + static atomic_t device_error_log_backend_enabled; + +static void device_error_log_work_handler(struct k_work *work); +K_WORK_DEFINE(device_error_log_work, device_error_log_work_handler); + +static uint8_t log_output_buffer[DEVICE_ERROR_LOG_FORMAT_BUFFER_SIZE]; +static int capture_log_output(uint8_t *data, size_t length, void *ctx); +LOG_OUTPUT_DEFINE(device_error_log_output, capture_log_output, log_output_buffer, + sizeof(log_output_buffer)); + +static bool log_level_should_forward(uint8_t level) +{ + return (level != LOG_LEVEL_NONE) && (level <= DEVICE_ERROR_LOG_BACKEND_LEVEL); +} + +static enum device_error_level device_error_level_from_log(uint8_t level) +{ + switch (level) { + case LOG_LEVEL_ERR: + return DEVICE_ERROR_LEVEL_ERROR; + case LOG_LEVEL_WRN: + return DEVICE_ERROR_LEVEL_WARNING; + case LOG_LEVEL_INF: + default: + return DEVICE_ERROR_LEVEL_INFO; + } +} + +static uint16_t device_error_code_from_log(uint8_t level) +{ + switch (level) { + case LOG_LEVEL_ERR: + return DEVICE_ERROR_CODE_FIRMWARE_LOG_ERROR; + case LOG_LEVEL_WRN: + return DEVICE_ERROR_CODE_FIRMWARE_LOG_WARNING; + case LOG_LEVEL_INF: + return DEVICE_ERROR_CODE_FIRMWARE_LOG_INFO; + case LOG_LEVEL_DBG: + default: + return DEVICE_ERROR_CODE_FIRMWARE_LOG_DEBUG; + } +} + +static bool is_device_error_service_log(struct log_msg *log_msg) +{ + int16_t source_id = log_msg_get_source_id(log_msg); + + if (source_id < 0) { + return false; + } + + const char *source_name = log_source_name_get(log_msg_get_domain(log_msg), source_id); + + return (source_name != NULL) && (strcmp(source_name, "device_error_service") == 0); +} + +static int capture_log_output(uint8_t *data, size_t length, void *ctx) +{ + struct log_capture_context *capture = ctx; + + if ((capture->size > 0) && (capture->offset < (capture->size - 1))) { + size_t remaining = capture->size - 1 - capture->offset; + size_t copy_len = MIN(length, remaining); + + memcpy(&capture->buffer[capture->offset], data, copy_len); + capture->offset += copy_len; + capture->buffer[capture->offset] = '\0'; + } + + return length; +} + +static void trim_trailing_line_endings(char *message) +{ + size_t len = strlen(message); + + while ((len > 0) && + ((message[len - 1] == '\r') || (message[len - 1] == '\n') || + (message[len - 1] == ' '))) { + message[len - 1] = '\0'; + len--; + } +} + +static void strip_default_log_tag(char *message) +{ +#if defined(CONFIG_LOG_TAG_MAX_LEN) && (CONFIG_LOG_TAG_MAX_LEN > 0) && defined(CONFIG_LOG_TAG_DEFAULT) + const char *tag = CONFIG_LOG_TAG_DEFAULT; + size_t tag_len = strlen(tag); + + if ((tag_len > 0) && (strncmp(message, tag, tag_len) == 0) && + (message[tag_len] == ' ')) { + memmove(message, &message[tag_len + 1], strlen(&message[tag_len + 1]) + 1); + } +#else + ARG_UNUSED(message); +#endif +} + +static void format_log_message(struct log_msg *log_msg, char *message, size_t message_len) +{ + struct log_capture_context capture = { + .buffer = message, + .size = message_len, + .offset = 0, + }; + + if (message_len == 0) { + return; + } + + message[0] = '\0'; + log_output_ctx_set(&device_error_log_output, &capture); + log_output_msg_process(&device_error_log_output, log_msg, LOG_OUTPUT_FLAG_CRLF_NONE); + strip_default_log_tag(message); + trim_trailing_line_endings(message); +} + +static void process(const struct log_backend *const backend, union log_msg_generic *msg) +{ + ARG_UNUSED(backend); + + if (!atomic_get(&device_error_log_backend_enabled)) { + return; + } + + if (!z_log_item_is_msg(msg)) { + return; + } + + uint8_t level = log_msg_get_level(&msg->log); + + if (!log_level_should_forward(level) || is_device_error_service_log(&msg->log)) { + return; + } + + struct device_error_log_event event = { + .level = device_error_level_from_log(level), + .error_code = device_error_code_from_log(level), + }; + + format_log_message(&msg->log, event.message, sizeof(event.message)); + + if (event.message[0] == '\0') { + strcpy(event.message, "Firmware log event"); + } + + if (k_msgq_put(&device_error_log_queue, &event, K_NO_WAIT) == 0) { + k_work_submit(&device_error_log_work); + } +} + +static void device_error_log_work_handler(struct k_work *work) +{ + struct device_error_log_event event; + + ARG_UNUSED(work); + + if (!atomic_get(&device_error_log_backend_enabled)) { + k_msgq_purge(&device_error_log_queue); + return; + } + + while (k_msgq_get(&device_error_log_queue, &event, K_NO_WAIT) == 0) { + (void)send_device_error(event.level, event.error_code, DEVICE_ERROR_SOURCE_SYSTEM, + event.message); + } +} + +static void panic(const struct log_backend *const backend) +{ + ARG_UNUSED(backend); + + atomic_clear(&device_error_log_backend_enabled); + k_msgq_purge(&device_error_log_queue); +} + +static const struct log_backend_api device_error_log_backend_api = { + .process = process, + .panic = panic, +}; + +LOG_BACKEND_DEFINE(device_error_log_backend, device_error_log_backend_api, false); + +void device_error_log_backend_enable(void) +{ + k_msgq_purge(&device_error_log_queue); + atomic_set(&device_error_log_backend_enabled, 1); + log_backend_enable(&device_error_log_backend, NULL, CONFIG_LOG_MAX_LEVEL); +} + +void device_error_log_backend_disable(void) +{ + atomic_clear(&device_error_log_backend_enabled); + log_backend_disable(&device_error_log_backend); + (void)k_work_cancel(&device_error_log_work); + k_msgq_purge(&device_error_log_queue); +} diff --git a/src/bluetooth/gatt_services/device_error_service.c b/src/bluetooth/gatt_services/device_error_service.c new file mode 100644 index 00000000..33bff091 --- /dev/null +++ b/src/bluetooth/gatt_services/device_error_service.c @@ -0,0 +1,105 @@ +#include "device_error_service.h" + +#include +#include + +#include +#include +#include + +#include "zbus_common.h" +#include "openearable_common.h" + +#include +LOG_MODULE_REGISTER(device_error_service, CONFIG_BLE_LOG_LEVEL); + +ZBUS_CHAN_DECLARE(bt_mgmt_chan); + +static bool device_error_notify_enabled; +static device_error_data_t device_error_data; + +static void device_error_bt_evt_handler(const struct zbus_channel *chan); +ZBUS_LISTENER_DEFINE(device_error_bt_evt_listen, device_error_bt_evt_handler); + +static void device_error_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value) +{ + ARG_UNUSED(attr); + + device_error_notify_enabled = (value == BT_GATT_CCC_NOTIFY); + if (device_error_notify_enabled) { + device_error_log_backend_enable(); + } else { + device_error_log_backend_disable(); + } + + LOG_INF("Device error notifications %s", device_error_notify_enabled ? "enabled" : "disabled"); +} + +BT_GATT_SERVICE_DEFINE(device_error_service, + BT_GATT_PRIMARY_SERVICE(BT_UUID_DEVICE_ERROR_SERVICE), + BT_GATT_CHARACTERISTIC(BT_UUID_DEVICE_ERROR_EVENT, + BT_GATT_CHRC_NOTIFY, + BT_GATT_PERM_NONE, + NULL, NULL, &device_error_data), + BT_GATT_CCC(device_error_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE), +); + +static void device_error_bt_evt_handler(const struct zbus_channel *chan) +{ + const struct bt_mgmt_msg *msg = zbus_chan_const_msg(chan); + + if (msg->event == BT_MGMT_DISCONNECTED) { + device_error_notify_enabled = false; + device_error_log_backend_disable(); + } +} + +static void copy_message(char *dest, size_t dest_len, const char *message) +{ + if (dest_len == 0) { + return; + } + + if (message == NULL) { + message = ""; + } + + strncpy(dest, message, dest_len - 1); + dest[dest_len - 1] = '\0'; +} + +int init_device_error_service(void) +{ + int ret = zbus_chan_add_obs(&bt_mgmt_chan, &device_error_bt_evt_listen, ZBUS_ADD_OBS_TIMEOUT_MS); + + if (ret) { + LOG_ERR("Failed to add device error bt_mgmt listener: %d", ret); + return ret; + } + + return 0; +} + +int send_device_error(enum device_error_level level, uint16_t error_code, uint8_t source_id, + const char *message) +{ + if (!device_error_notify_enabled) { + return -ENOENT; + } + + device_error_data.version = DEVICE_ERROR_PAYLOAD_VERSION; + device_error_data.level = (uint8_t)level; + device_error_data.error_code = error_code; + device_error_data.source_id = source_id; + device_error_data.timestamp_ms = k_uptime_get_32(); + copy_message(device_error_data.message, sizeof(device_error_data.message), message); + + int err = bt_gatt_notify(NULL, &device_error_service.attrs[2], &device_error_data, + sizeof(device_error_data)); + + if (err) { + LOG_WRN("Failed to send device error notification: %d", err); + } + + return err; +} diff --git a/src/bluetooth/gatt_services/device_error_service.h b/src/bluetooth/gatt_services/device_error_service.h new file mode 100644 index 00000000..b75eb233 --- /dev/null +++ b/src/bluetooth/gatt_services/device_error_service.h @@ -0,0 +1,58 @@ +#ifndef DEVICE_ERROR_SERVICE_H +#define DEVICE_ERROR_SERVICE_H + +#include + +#include + +#define BT_UUID_DEVICE_ERROR_SERVICE_VAL \ + BT_UUID_128_ENCODE(0x5f9c0001, 0x6f4a, 0x4c6b, 0x9f0d, 0x4f2f3b0a0001) +#define BT_UUID_DEVICE_ERROR_EVENT_VAL \ + BT_UUID_128_ENCODE(0x5f9c0002, 0x6f4a, 0x4c6b, 0x9f0d, 0x4f2f3b0a0001) + +#define BT_UUID_DEVICE_ERROR_SERVICE BT_UUID_DECLARE_128(BT_UUID_DEVICE_ERROR_SERVICE_VAL) +#define BT_UUID_DEVICE_ERROR_EVENT BT_UUID_DECLARE_128(BT_UUID_DEVICE_ERROR_EVENT_VAL) + +#define DEVICE_ERROR_PAYLOAD_VERSION 1 +#define DEVICE_ERROR_MESSAGE_MAX_LENGTH 48 + +#define DEVICE_ERROR_SOURCE_SYSTEM 0xFF + +#define DEVICE_ERROR_CODE_FIRMWARE_FATAL 0x0301 +#define DEVICE_ERROR_CODE_FIRMWARE_LOG_ERROR 0x0302 +#define DEVICE_ERROR_CODE_FIRMWARE_LOG_WARNING 0x0303 +#define DEVICE_ERROR_CODE_FIRMWARE_LOG_INFO 0x0304 +#define DEVICE_ERROR_CODE_FIRMWARE_LOG_DEBUG 0x0305 + +#ifdef __cplusplus +extern "C" { +#endif + +enum device_error_level { + DEVICE_ERROR_LEVEL_INFO = 0, + DEVICE_ERROR_LEVEL_WARNING = 1, + DEVICE_ERROR_LEVEL_ERROR = 2, + DEVICE_ERROR_LEVEL_FATAL = 3, +}; + +typedef struct __attribute__((packed)) { + uint8_t version; + uint8_t level; + uint16_t error_code; + uint8_t source_id; + uint32_t timestamp_ms; + char message[DEVICE_ERROR_MESSAGE_MAX_LENGTH]; +} device_error_data_t; + +int init_device_error_service(void); +int send_device_error(enum device_error_level level, uint16_t error_code, uint8_t source_id, + const char *message); + +void device_error_log_backend_enable(void); +void device_error_log_backend_disable(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/unicast_server/main.cpp b/unicast_server/main.cpp index 706cab4e..c3e6570d 100644 --- a/unicast_server/main.cpp +++ b/unicast_server/main.cpp @@ -55,6 +55,8 @@ LOG_MODULE_REGISTER(main, CONFIG_MAIN_LOG_LEVEL); /* STEP 5.4 - Include header for USB */ #include +//bluetooth logging +#include "device_error_service.h" int main(void) { int ret; @@ -107,6 +109,9 @@ int main(void) { ret = initParseInfoService(&defaultSensorIds, defaultSensors); ERR_CHK(ret); + ret = init_device_error_service(); + ERR_CHK(ret); + ret = init_sensor_service(); ERR_CHK(ret);