Skip to content

feat(storage): improve SD logging and USB mass storage #211

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
DennisMoschina wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

feat(storage): improve SD logging and USB mass storage #211

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 1 addition & 9 deletions include/openearable_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ uint64_t oe_micros();

#define load_switch_sd_id DT_NODELABEL(load_switch_sd)
#define load_switch_1_8_id DT_NODELABEL(load_switch)
#define load_switch_3_3_id DT_NODELABEL(bq25120a)
#define load_switch_3_3_id DT_CHILD(DT_NODELABEL(bq25120a), load_switch)

extern const struct device *const cons;
extern const struct device *const ls_1_8;
Expand Down Expand Up @@ -85,7 +85,6 @@ enum sensor_id {
ID_TEMP_BARO=1,
ID_MICRO=2,
ID_PPG=4,
ID_PULSOX=5,
ID_OPTTEMP=6,
ID_BONE_CONDUCTION=7,
};
Expand Down Expand Up @@ -138,11 +137,4 @@ struct sd_msg {
bool removed;
};

#include "audio_i2s.h"

struct audio_rx_data {
char data[FRAME_SIZE_BYTES];
size_t size;
};

#endif
1 change: 1 addition & 0 deletions src/SD_Card/SDLogger/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
target_sources(app PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/SDLogger.cpp
${CMAKE_CURRENT_SOURCE_DIR}/sd_bench.c
)
120 changes: 65 additions & 55 deletions src/SD_Card/SDLogger/SDLogger.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include "PowerManager.h"
#include <errno.h>
#include "audio_datapath.h"
#include "channel_assignment.h"

#include "StateIndicator.h"

Expand All @@ -16,16 +17,8 @@ LOG_MODULE_REGISTER(sd_logger, CONFIG_LOG_DEFAULT_LEVEL);

ZBUS_CHAN_DECLARE(sd_card_chan);

void sensor_listener_cb(const struct zbus_channel *chan);

K_MSGQ_DEFINE(sd_sensor_queue, sizeof(sensor_data), CONFIG_SENSOR_SD_SUB_QUEUE_SIZE, 4);
ZBUS_LISTENER_DEFINE(sensor_data_listener, sensor_listener_cb);

// Define thread stack
K_THREAD_STACK_DEFINE(thread_stack, CONFIG_SENSOR_SD_STACK_SIZE);

ZBUS_CHAN_DECLARE(sensor_chan);

void sd_listener_callback(const struct zbus_channel *chan);

ZBUS_LISTENER_DEFINE(sd_card_event_listener, sd_listener_callback);
Expand All @@ -36,14 +29,24 @@ static k_tid_t thread_id;
struct ring_buf ring_buffer;
struct k_mutex ring_mutex; // Protects ring_buffer operations
struct k_mutex file_mutex; // Protects sd_card open/write/close
uint8_t buffer[BUFFER_SIZE]; // Ring Buffer Speicher
uint8_t buffer[BUFFER_SIZE];

// Coordination flags (atomic because they are accessed from multiple threads)
static atomic_t g_stop_writing; // 1 while end()/flush/close is in progress
static atomic_t g_sd_removed; // 1 if SD was removed while recording

uint32_t count_max_buffer_fill = 0;

static struct {
uint32_t ring_full;
uint32_t ring_mutex_fail;
uint32_t sd_writes;
uint32_t sd_write_max_us;
uint64_t sd_write_total_us;
uint32_t bytes_written;
uint32_t bytes_dropped;
} sd_stats;

struct k_poll_signal logger_sig;
static struct k_poll_event logger_evt =
K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY, &logger_sig);
Expand All @@ -60,20 +63,6 @@ SDLogger::~SDLogger() {

}

//static bool _prio_boost = false;

void sensor_listener_cb(const struct zbus_channel *chan) {
const sensor_msg* msg = (sensor_msg*)zbus_chan_const_msg(chan);

if (msg->sd) {
int ret = sdlogger.write_sensor_data(msg->data);
if (ret < 0) {
LOG_WRN("Failed to enqueue sensor data for SD: %d", ret);
}
}
}


void sd_listener_callback(const struct zbus_channel *chan)
{
const struct sd_msg * sd_msg_event = (sd_msg *)zbus_chan_const_msg(&sd_card_chan);
Expand Down Expand Up @@ -110,9 +99,13 @@ void SDLogger::sensor_sd_task() {
continue;
}

// Reset signal early so signals raised during the drain loop
// are preserved for the next k_poll wakeup.
k_poll_signal_reset(&logger_sig);
logger_evt.state = K_POLL_STATE_NOT_READY;

// If a close/flush is in progress, do not write concurrently.
if (atomic_get(&g_stop_writing)) {
k_poll_signal_reset(&logger_sig);
continue;
}

Expand All @@ -122,7 +115,6 @@ void SDLogger::sensor_sd_task() {
ring_buf_reset(&ring_buffer);
k_mutex_unlock(&ring_mutex);
sdlogger.is_open = false;
k_poll_signal_reset(&logger_sig);
continue;
}

Expand All @@ -132,69 +124,72 @@ void SDLogger::sensor_sd_task() {
return;
}

uint32_t fill = ring_buf_size_get(&ring_buffer);
// Drain all available full blocks from the ring buffer.
while (ring_buf_size_get(&ring_buffer) >= SD_BLOCK_SIZE) {
if (atomic_get(&g_stop_writing) || atomic_get(&g_sd_removed)) {
break;
}

if (fill >= SD_BLOCK_SIZE) {
uint32_t fill = ring_buf_size_get(&ring_buffer);
if (fill > count_max_buffer_fill) {
count_max_buffer_fill = fill;
}

uint8_t *data = nullptr;

// Claim up to one SD block from the ring buffer under lock.
// Claim up to 4 SD blocks from the ring buffer under lock.
k_mutex_lock(&ring_mutex, K_FOREVER);
uint32_t claimed = ring_buf_get_claim(&ring_buffer, &data, SD_BLOCK_SIZE);
uint32_t claimed = ring_buf_get_claim(&ring_buffer, &data, 4 * SD_BLOCK_SIZE);
k_mutex_unlock(&ring_mutex);

if (claimed == 0 || data == nullptr) {
// Nothing to write right now.
k_poll_signal_reset(&logger_sig);
continue;
break;
}

// Write the claimed bytes under file lock.
size_t write_size = claimed;
int written;
uint64_t t0 = micros();
k_mutex_lock(&file_mutex, K_FOREVER);
written = sdlogger.sd_card->write((char*)data, &write_size, false);
k_mutex_unlock(&file_mutex);
uint32_t elapsed = (uint32_t)(micros() - t0);

sd_stats.sd_writes++;
sd_stats.sd_write_total_us += elapsed;
if (elapsed > sd_stats.sd_write_max_us) {
sd_stats.sd_write_max_us = elapsed;
}

if (written < 0) {
state_indicator.set_sd_state(SD_FAULT);
LOG_ERR("SD write failed: %d", written);

// Do not advance the ring buffer on error.
// Wakeups will continue; user can call end().
k_poll_signal_reset(&logger_sig);
continue;
break;
}

sd_stats.bytes_written += (uint32_t)written;

// Advance ring buffer by the number of bytes actually written.
k_mutex_lock(&ring_mutex, K_FOREVER);
ring_buf_get_finish(&ring_buffer, (uint32_t)written);
k_mutex_unlock(&ring_mutex);
} else {

// Let lower-priority threads (sensor work queue) run between writes
k_yield();
}

k_poll_signal_reset(&logger_sig);

STACK_USAGE_PRINT("sensor_msg_thread", &sdlogger.thread_data);
}
}

int SDLogger::init() {
int ret;

sd_card->init();

ring_buf_init(&ring_buffer, BUFFER_SIZE, buffer);

atomic_clear(&g_stop_writing);
atomic_clear(&g_sd_removed);

//set_ring_buffer(&ring_buffer);

k_poll_signal_init(&logger_sig);

thread_id = k_thread_create(
Expand All @@ -208,12 +203,6 @@ int SDLogger::init() {
return ret;
}

ret = zbus_chan_add_obs(&sensor_chan, &sensor_data_listener, ZBUS_ADD_OBS_TIMEOUT_MS);
if (ret) {
LOG_ERR("Failed to add sensor sub");
return ret;
}

ret = zbus_chan_add_obs(&sd_card_chan, &sd_card_event_listener, ZBUS_ADD_OBS_TIMEOUT_MS);
if (ret) {
LOG_ERR("Failed to add sd sub");
Expand Down Expand Up @@ -271,6 +260,9 @@ int SDLogger::begin(const std::string& filename) {
ring_buf_reset(&ring_buffer);
k_mutex_unlock(&ring_mutex);

memset(&sd_stats, 0, sizeof(sd_stats));
count_max_buffer_fill = 0;

ret = write_header();
if (ret < 0) {
state_indicator.set_sd_state(SD_FAULT);
Expand All @@ -290,6 +282,11 @@ int SDLogger::write_header() {

header->version = SENSOR_LOG_VERSION;
header->timestamp = micros();
header->device_id = oe_boot_state.device_id;

enum audio_channel ch;
channel_assignment_get(&ch);
header->channel = (uint8_t)ch;

int ret;
k_mutex_lock(&file_mutex, K_FOREVER);
Expand Down Expand Up @@ -320,17 +317,21 @@ int SDLogger::write_sensor_data(const void* const* data_blocks, const size_t* le
return -ENODEV;
}

// Do not block producers; if mutex is contended, drop quickly
if (k_mutex_lock(&ring_mutex, K_NO_WAIT) != 0) {
// Allow a short wait for mutex contention to clear.
// The writer holds ring_mutex only during fast ring_buf operations.
if (k_mutex_lock(&ring_mutex, K_USEC(200)) != 0) {
sd_stats.ring_mutex_fail++;
sd_stats.bytes_dropped += total_length;
return -EAGAIN;
}

// Ensure there is enough space; if not, free up room by discarding oldest bytes
// in SD_BLOCK_SIZE chunks to keep SD writer alignment and minimize partial writes.
uint32_t space = ring_buf_space_get(&ring_buffer);
if (space < total_length) {
LOG_ERR("Ring buffer low on space: have %u, need %zu. Skipping data",
space, total_length);
sd_stats.ring_full++;
sd_stats.bytes_dropped += total_length;
LOG_DBG("Ring buffer full: have %u, need %zu", space, total_length);
k_mutex_unlock(&ring_mutex);
return -ENOSPC;
}
Expand Down Expand Up @@ -439,7 +440,13 @@ int SDLogger::end() {

LOG_INF("Close File ....");

LOG_DBG("Max buffer fill: %d bytes", count_max_buffer_fill);
uint32_t avg_write_us = sd_stats.sd_writes ? (uint32_t)(sd_stats.sd_write_total_us / sd_stats.sd_writes) : 0;
LOG_INF("SD stats: ring_full=%u ring_mutex_fail=%u bytes_dropped=%u",
sd_stats.ring_full, sd_stats.ring_mutex_fail, sd_stats.bytes_dropped);
LOG_INF("SD stats: writes=%u bytes=%u max_fill=%u/%u",
sd_stats.sd_writes, sd_stats.bytes_written, count_max_buffer_fill, (unsigned)BUFFER_SIZE);
LOG_INF("SD stats: write_avg=%u us write_max=%u us",
avg_write_us, sd_stats.sd_write_max_us);

k_mutex_lock(&file_mutex, K_FOREVER);
ret = sd_card->close_file();
Expand All @@ -455,6 +462,9 @@ int SDLogger::end() {
atomic_clear(&g_stop_writing);
atomic_clear(&g_sd_removed);

/* Unmount the SD card so USB MSC can re-enable for host access. */
sd_card->unmount();

return 0;
}

Expand Down
19 changes: 4 additions & 15 deletions src/SD_Card/SDLogger/SDLogger.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,46 +10,35 @@


constexpr size_t SD_BLOCK_SIZE = 4096;
constexpr size_t BUFFER_BLOCK_COUNT = 8; // Number of blocks in the buffer
constexpr size_t BUFFER_BLOCK_COUNT = 16; // Number of blocks in the buffer
constexpr size_t BUFFER_SIZE = SD_BLOCK_SIZE * BUFFER_BLOCK_COUNT;

// BUFFER_SIZE must always be a multiple of SD_BLOCK_SIZE to ensure proper block alignment
// without requiring padding. The SensorLogger implementation assumes this relationship
// and will not work correctly otherwise.
static_assert(BUFFER_SIZE % SD_BLOCK_SIZE == 0, "BUFFER_SIZE must be a multiple of SD_BLOCK_SIZE");

// Forward declare the work handler
//static void sd_work_handler(struct k_work* work);

// Singleton pattern
class SDLogger {
protected:
// Add static instance pointer for work handler
//static SDLogger* instance_ptr;
//friend void sd_work_handler(struct k_work* work);

private:

SDCardManager* sd_card = nullptr;
bool is_open = false;

//uint8_t buffer[BUFFER_SIZE]; // Ring Buffer Speicher
//size_t buffer_pos = 0;
std::string current_file;

int write_header(); //Write file header with version and timestamp
int flush(); // Flush any buffered data to the SD card

static constexpr uint16_t SENSOR_LOG_VERSION = 0x0002;
static constexpr uint16_t SENSOR_LOG_VERSION = 0x0003;

struct __attribute__((packed)) FileHeader {
uint16_t version;
uint64_t timestamp;
uint64_t device_id;
uint8_t channel; // AUDIO_CH_L (0) = left, AUDIO_CH_R (1) = right
};

struct sensor_data msg;

//struct sensor_data* const data_buf = &(msg.data);
static void sensor_sd_task();

friend void sd_listener_callback(const struct zbus_channel *chan);
Expand Down
Loading
Loading