diff --git a/omi/firmware/omi/src/ffi/ble_manifest.h b/omi/firmware/omi/src/ffi/ble_manifest.h new file mode 100644 index 0000000000..c92df29a69 --- /dev/null +++ b/omi/firmware/omi/src/ffi/ble_manifest.h @@ -0,0 +1,33 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +const char *omi_ble_audio_service_uuid(void); +const char *omi_ble_audio_data_uuid(void); +const char *omi_ble_audio_codec_uuid(void); +const char *omi_ble_audio_speaker_uuid(void); + +const char *omi_ble_settings_service_uuid(void); +const char *omi_ble_settings_dim_ratio_uuid(void); + +const char *omi_ble_features_service_uuid(void); +const char *omi_ble_features_flags_uuid(void); + +const char *omi_ble_storage_service_uuid(void); +const char *omi_ble_storage_command_uuid(void); +const char *omi_ble_storage_status_uuid(void); + +const char *omi_ble_button_service_uuid(void); +const char *omi_ble_button_event_uuid(void); + +const char *omi_ble_accel_service_uuid(void); +const char *omi_ble_accel_sample_uuid(void); + +const char *omi_ble_haptic_service_uuid(void); +const char *omi_ble_haptic_command_uuid(void); + +#ifdef __cplusplus +} +#endif diff --git a/omi/firmware/omi/src/ffi/shims.c b/omi/firmware/omi/src/ffi/shims.c new file mode 100644 index 0000000000..b9d25477f2 --- /dev/null +++ b/omi/firmware/omi/src/ffi/shims.c @@ -0,0 +1,721 @@ +#include "shims.h" +#include "ble_manifest.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define LOG_MODULE_NAME omi_shims +#include +LOG_MODULE_REGISTER(LOG_MODULE_NAME); + +#include + +// LED PWM specs +static const struct pwm_dt_spec led_red = PWM_DT_SPEC_GET(DT_NODELABEL(led_red)); +static const struct pwm_dt_spec led_green = PWM_DT_SPEC_GET(DT_NODELABEL(led_green)); +static const struct pwm_dt_spec led_blue = PWM_DT_SPEC_GET(DT_NODELABEL(led_blue)); + +static const struct device *const battery_adc = DEVICE_DT_GET(DT_NODELABEL(adc)); +static const struct gpio_dt_spec battery_read_pin = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(bat_read_pin), gpios, {0}); +static const struct gpio_dt_spec battery_chg_pin = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(bat_chg_pin), gpios, {0}); + +static const struct device *const dmic_dev = DEVICE_DT_GET(DT_ALIAS(dmic0)); + +static const struct device *const sd_dev = DEVICE_DT_GET(DT_NODELABEL(sdhc0)); +static const struct gpio_dt_spec sd_en_pin = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(sdcard_en_pin), gpios, {0}); + +#define DISK_DRIVE_NAME "SDMMC" +#define DISK_MOUNT_PT "/ext" + +static struct fs_mount_t sd_mount = { + .type = FS_EXT2, + .flags = FS_MOUNT_FLAG_NO_FORMAT, + .storage_dev = (void *)DISK_DRIVE_NAME, + .mnt_point = DISK_MOUNT_PT, +}; + +#define MAX_SAMPLE_RATE 16000 +#define SAMPLE_BIT_WIDTH 16 +#define BYTES_PER_SAMPLE sizeof(int16_t) +#define MIC_BLOCK_SIZE(sample_rate, channels) (BYTES_PER_SAMPLE * ((sample_rate) / 10) * (channels)) +#define MIC_MAX_BLOCK_SIZE MIC_BLOCK_SIZE(MAX_SAMPLE_RATE, 2) +#define MIC_BLOCK_COUNT 4 + +K_MEM_SLAB_DEFINE_STATIC(mic_mem_slab, MIC_MAX_BLOCK_SIZE, MIC_BLOCK_COUNT, sizeof(void *)); + +static struct gpio_callback battery_chg_cb; +static omi_gpio_edge_cb_t battery_chg_handler; +static void *battery_chg_user_data; + +#define BATTERY_ADC_CHANNEL_ID 0 +#define BATTERY_ADC_RESOLUTION 12 +#define BATTERY_ADC_ACQ_TIME ADC_ACQ_TIME(ADC_ACQ_TIME_MICROSECONDS, 10) + +static const struct adc_channel_cfg battery_channel_cfg = { + .gain = ADC_GAIN_1_3, + .reference = ADC_REF_INTERNAL, + .acquisition_time = BATTERY_ADC_ACQ_TIME, + .channel_id = BATTERY_ADC_CHANNEL_ID, +#if defined(CONFIG_ADC_CONFIGURABLE_INPUTS) + .input_positive = NRF_SAADC_INPUT_AIN0, +#endif +}; + +static struct adc_sequence_options battery_sequence_opts = { + .extra_samplings = 0, + .interval_us = 0, + .callback = NULL, + .user_data = NULL, +}; + +bool omi_led_ready_red(void) +{ + return pwm_is_ready_dt(&led_red); +} + +bool omi_led_ready_green(void) +{ + return pwm_is_ready_dt(&led_green); +} + +bool omi_led_ready_blue(void) +{ + return pwm_is_ready_dt(&led_blue); +} + +uint32_t omi_led_period_red(void) +{ + return led_red.period; +} + +uint32_t omi_led_period_green(void) +{ + return led_green.period; +} + +uint32_t omi_led_period_blue(void) +{ + return led_blue.period; +} + +int omi_led_set_red(uint32_t pulse_width_ns) +{ + return pwm_set_pulse_dt(&led_red, pulse_width_ns); +} + +int omi_led_set_green(uint32_t pulse_width_ns) +{ + return pwm_set_pulse_dt(&led_green, pulse_width_ns); +} + +int omi_led_set_blue(uint32_t pulse_width_ns) +{ + return pwm_set_pulse_dt(&led_blue, pulse_width_ns); +} + +// ----------------------------------------------------------------------------- +// Device helpers + +const struct device *omi_device_get(enum omi_device_id id) +{ + switch (id) { + case OMI_DEVICE_SPI_FLASH: + return DEVICE_DT_GET(DT_NODELABEL(spi_flash)); + case OMI_DEVICE_ADC: + return DEVICE_DT_GET(DT_NODELABEL(adc)); + case OMI_DEVICE_SDHC0: + return DEVICE_DT_GET(DT_NODELABEL(sdhc0)); + case OMI_DEVICE_DMIC0: + return DEVICE_DT_GET(DT_ALIAS(dmic0)); + default: + return NULL; + } +} + +bool omi_device_is_ready(const struct device *dev) +{ + return device_is_ready(dev); +} + +int omi_pm_device_action(const struct device *dev, enum pm_device_action action) +{ + return pm_device_action_run(dev, action); +} + +const char *omi_device_name(const struct device *dev) +{ + if (!dev) { + return ""; + } + return dev->name; +} + +// ----------------------------------------------------------------------------- +// GPIO helpers + +static const struct gpio_dt_spec gpio_motor = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(motor_pin), gpios, {0}); +static const struct gpio_dt_spec gpio_bat_power = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(power_pin), gpios, {0}); +static const struct gpio_dt_spec gpio_bat_read = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(bat_read_pin), gpios, {0}); +static const struct gpio_dt_spec gpio_bat_chg = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(bat_chg_pin), gpios, {0}); +static const struct gpio_dt_spec gpio_sd_en = GPIO_DT_SPEC_GET_OR(DT_NODELABEL(sdcard_en_pin), gpios, {0}); + +const struct gpio_dt_spec *omi_gpio_pin(enum omi_gpio_pin_id id) +{ + switch (id) { + case OMI_PIN_MOTOR: + return &gpio_motor; + case OMI_PIN_BAT_POWER: + return &gpio_bat_power; + case OMI_PIN_BAT_READ: + return &gpio_bat_read; + case OMI_PIN_BAT_CHG: + return &gpio_bat_chg; + case OMI_PIN_SD_EN: + return &gpio_sd_en; + default: + return NULL; + } +} + +bool omi_gpio_is_ready(const struct gpio_dt_spec *pin) +{ + return gpio_is_ready_dt(pin); +} + +int omi_gpio_configure(const struct gpio_dt_spec *pin, gpio_flags_t flags) +{ + return gpio_pin_configure_dt(pin, flags); +} + +int omi_gpio_set(const struct gpio_dt_spec *pin, int value) +{ + return gpio_pin_set_dt(pin, value); +} + +int omi_gpio_get(const struct gpio_dt_spec *pin) +{ + return gpio_pin_get_dt(pin); +} + +uint32_t omi_gpio_flag_output(void) +{ + return GPIO_OUTPUT; +} + +uint32_t omi_gpio_flag_input(void) +{ + return GPIO_INPUT; +} + +// ----------------------------------------------------------------------------- +// ADC helpers + +static struct adc_sequence_options adc_seq_opts = { + .interval_us = 0, + .callback = NULL, + .user_data = NULL, +}; + +void omi_adc_sequence_init(struct adc_sequence *sequence, uint32_t channel_mask, void *buffer, size_t buffer_size, uint8_t resolution) +{ + sequence->options = &adc_seq_opts; + sequence->channels = channel_mask; + sequence->buffer = buffer; + sequence->buffer_size = buffer_size; + sequence->resolution = resolution; +} + +int omi_adc_channel_setup(const struct device *adc_dev, const struct adc_channel_cfg *cfg) +{ + return adc_channel_setup(adc_dev, cfg); +} + +int omi_adc_read(const struct device *adc_dev, const struct adc_sequence *sequence) +{ + return adc_read(adc_dev, sequence); +} + +uint16_t omi_adc_ref_internal_mv(const struct device *adc_dev) +{ + return adc_ref_internal(adc_dev); +} + +int omi_adc_raw_to_millivolts(uint16_t vref, enum adc_gain gain, uint8_t resolution, int32_t *val) +{ + return adc_raw_to_millivolts(vref, gain, resolution, val); +} + +// ----------------------------------------------------------------------------- +// Delayable work helper + +struct omi_delayable_work { + struct k_work_delayable work; + omi_work_callback_t callback; + void *user_data; +}; + +static void omi_work_trampoline(struct k_work *work) +{ + struct k_work_delayable *dwork = CONTAINER_OF(work, struct k_work_delayable, work); + struct omi_delayable_work *wrapper = CONTAINER_OF(dwork, struct omi_delayable_work, work); + if (wrapper->callback) { + wrapper->callback(wrapper->user_data); + } +} + +struct omi_delayable_work *omi_delayable_work_create(omi_work_callback_t cb, void *user_data) +{ + struct omi_delayable_work *wrapper = k_calloc(1, sizeof(*wrapper)); + if (!wrapper) { + return NULL; + } + wrapper->callback = cb; + wrapper->user_data = user_data; + k_work_init_delayable(&wrapper->work, omi_work_trampoline); + return wrapper; +} + +void omi_delayable_work_destroy(struct omi_delayable_work *wrapper) +{ + if (!wrapper) { + return; + } + k_work_cancel_delayable(&wrapper->work); + k_free(wrapper); +} + +void omi_delayable_work_set_user_data(struct omi_delayable_work *wrapper, void *user_data) +{ + if (wrapper) { + wrapper->user_data = user_data; + } +} + +int omi_delayable_work_schedule(struct omi_delayable_work *wrapper, uint32_t delay_ms) +{ + return k_work_schedule(&wrapper->work, K_MSEC(delay_ms)); +} + +int omi_delayable_work_cancel(struct omi_delayable_work *wrapper) +{ + return k_work_cancel_delayable(&wrapper->work); +} + +// ----------------------------------------------------------------------------- +// PM counter helpers for SD card + +int omi_disk_access_ioctl(const char *disk_pdrv, uint8_t cmd, void *buffer) +{ + return disk_access_ioctl(disk_pdrv, cmd, buffer); +} + +int omi_fs_mount(struct fs_mount_t *mount) +{ + return fs_mount(mount); +} + +int omi_fs_unmount(struct fs_mount_t *mount) +{ + return fs_unmount(mount); +} + +int omi_fs_mkfs(fs_system_t type, uintptr_t storage_dev, void *scratch, uint32_t scratch_size) +{ + return fs_mkfs(type, storage_dev, scratch, scratch_size); +} + +// ----------------------------------------------------------------------------- +// DMIC helpers + +int omi_dmic_configure(const struct device *dev, const struct dmic_cfg *cfg) +{ + return dmic_configure(dev, cfg); +} + +int omi_dmic_trigger(const struct device *dev, enum dmic_trigger trigger) +{ + return dmic_trigger(dev, trigger); +} + +int omi_dmic_read(const struct device *dev, uint8_t stream, void **buffer, uint32_t *size, int32_t timeout) +{ + return dmic_read(dev, stream, buffer, size, K_MSEC(timeout)); +} + +int omi_mic_configure(uint32_t sample_rate, uint8_t channels) +{ + if (!device_is_ready(dmic_dev)) { + return -ENODEV; + } + + struct pcm_stream_cfg stream = { + .pcm_width = SAMPLE_BIT_WIDTH, + .mem_slab = &mic_mem_slab, + .pcm_rate = sample_rate, + .block_size = MIC_BLOCK_SIZE(sample_rate, channels), + }; + + struct dmic_cfg cfg = { + .io = { + .min_pdm_clk_freq = 1000000, + .max_pdm_clk_freq = 3500000, + .min_pdm_clk_dc = 40, + .max_pdm_clk_dc = 60, + }, + .streams = &stream, + .channel = { + .req_num_streams = 1, + .req_num_chan = channels, + .req_chan_map_lo = dmic_build_channel_map(0, 0, PDM_CHAN_LEFT), + }, + }; + + int ret = dmic_configure(dmic_dev, &cfg); + if (ret < 0) { + return ret; + } + + return 0; +} + +// ----------------------------------------------------------------------------- +// Logging helpers + +void omi_log_inf(const char *msg) +{ + LOG_INF("%s", msg); +} + +void omi_log_err(const char *msg) +{ + LOG_ERR("%s", msg); +} + +// ----------------------------------------------------------------------------- +// Battery helpers + +static void battery_chg_trampoline(const struct device *dev, struct gpio_callback *cb, uint32_t pins) +{ + ARG_UNUSED(dev); + ARG_UNUSED(cb); + ARG_UNUSED(pins); + if (battery_chg_handler) { + battery_chg_handler(battery_chg_user_data); + } +} + +int omi_battery_prepare_measurement_pin(void) +{ + int ret = gpio_pin_configure_dt(&battery_read_pin, GPIO_OUTPUT | NRF_GPIO_DRIVE_S0H1); + if (ret < 0) { + return ret; + } + ret = gpio_pin_set_dt(&battery_read_pin, 0); + return ret; +} + +int omi_battery_restore_measurement_pin(void) +{ + return gpio_pin_configure_dt(&battery_read_pin, GPIO_INPUT); +} + +int omi_battery_channel_setup(void) +{ + return adc_channel_setup(battery_adc, &battery_channel_cfg); +} + +int omi_battery_perform_read(int16_t *buffer, size_t sample_count, uint32_t extra_samplings) +{ + if (!buffer || sample_count == 0) { + return -EINVAL; + } + + struct adc_sequence sequence = { + .options = &battery_sequence_opts, + .channels = BIT(BATTERY_ADC_CHANNEL_ID), + .buffer = buffer, + .buffer_size = sample_count * sizeof(int16_t), + .resolution = BATTERY_ADC_RESOLUTION, + }; + + battery_sequence_opts.extra_samplings = extra_samplings; + + return adc_read(battery_adc, &sequence); +} + +int omi_battery_configure_pins(void) +{ + int err = gpio_pin_configure_dt(&battery_read_pin, GPIO_INPUT); + if (err < 0) { + return err; + } + + err = gpio_pin_configure_dt(&battery_chg_pin, GPIO_INPUT | GPIO_PULL_UP); + if (err < 0) { + return err; + } + + gpio_init_callback(&battery_chg_cb, battery_chg_trampoline, BIT(battery_chg_pin.pin)); + err = gpio_add_callback(battery_chg_pin.port, &battery_chg_cb); + if (err < 0) { + return err; + } + + return 0; +} + +int omi_battery_set_chg_handler(omi_gpio_edge_cb_t handler, void *user_data) +{ + battery_chg_handler = handler; + battery_chg_user_data = user_data; + return 0; +} + +int omi_battery_enable_chg_interrupt(void) +{ + return gpio_pin_interrupt_configure_dt(&battery_chg_pin, GPIO_INT_EDGE_BOTH); +} + +int omi_battery_disable_chg_interrupt(void) +{ + return gpio_pin_interrupt_configure_dt(&battery_chg_pin, GPIO_INT_DISABLE); +} + +int omi_battery_read_chg_pin(void) +{ + return gpio_pin_get_dt(&battery_chg_pin); +} + +void omi_sleep_ms(uint32_t ms) +{ + k_msleep(ms); +} + +void omi_busy_wait_us(uint32_t us) +{ + k_busy_wait(us); +} + +// ----------------------------------------------------------------------------- +// Threads and memory slabs + +struct k_mem_slab *omi_mic_mem_slab(void) +{ + return &mic_mem_slab; +} + +int omi_mem_slab_alloc(struct k_mem_slab *slab, void **mem, uint32_t timeout_ms) +{ + return k_mem_slab_alloc(slab, mem, K_MSEC(timeout_ms)); +} + +int omi_mem_slab_free(struct k_mem_slab *slab, void *mem) +{ + return k_mem_slab_free(slab, mem); +} + +#define MIC_THREAD_STACK_SIZE 2048 +K_THREAD_STACK_DEFINE(mic_thread_stack, MIC_THREAD_STACK_SIZE); +static struct k_thread mic_thread; + +struct k_thread *omi_thread_create(void (*entry)(void *, void *, void *), void *p1, void *p2, void *p3, int priority) +{ + k_tid_t tid = k_thread_create(&mic_thread, + mic_thread_stack, + K_THREAD_STACK_SIZEOF(mic_thread_stack), + entry, + p1, + p2, + p3, + priority, + 0, + K_NO_WAIT); + ARG_UNUSED(tid); + return &mic_thread; +} + +void omi_thread_start(struct k_thread *thread) +{ + if (thread) { + k_thread_start(thread); + } +} + +void omi_thread_abort(struct k_thread *thread) +{ + if (thread) { + k_thread_abort(thread); + } +} + +// ----------------------------------------------------------------------------- +// Haptic helpers + +static omi_haptic_write_cb_t haptic_write_cb; + +static ssize_t haptic_write_handler(struct bt_conn *conn, + const struct bt_gatt_attr *attr, + const void *buf, + uint16_t len, + uint16_t offset, + uint8_t flags) +{ + ARG_UNUSED(conn); + ARG_UNUSED(attr); + ARG_UNUSED(offset); + ARG_UNUSED(flags); + + if (len < 1) { + return BT_GATT_ERR(BT_ATT_ERR_INVALID_ATTRIBUTE_LEN); + } + + if (haptic_write_cb) { + haptic_write_cb(((const uint8_t *)buf)[0]); + } + + return len; +} + +static struct bt_uuid_128 haptic_service_uuid; +static struct bt_uuid_128 haptic_char_uuid; +static bool haptic_uuid_initialized; + +static int ensure_haptic_uuids(void) +{ + if (haptic_uuid_initialized) { + return 0; + } + + int err = bt_uuid_from_str(omi_ble_haptic_service_uuid(), &haptic_service_uuid.uuid); + if (err) { + return err; + } + + err = bt_uuid_from_str(omi_ble_haptic_command_uuid(), &haptic_char_uuid.uuid); + if (err) { + return err; + } + + haptic_uuid_initialized = true; + return 0; +} + +static struct bt_gatt_attr haptic_attrs[] = { + BT_GATT_PRIMARY_SERVICE(&haptic_service_uuid), + BT_GATT_CHARACTERISTIC(&haptic_char_uuid.uuid, + BT_GATT_CHRC_WRITE, + BT_GATT_PERM_WRITE, + NULL, + haptic_write_handler, + NULL), +}; + +static struct bt_gatt_service haptic_service = BT_GATT_SERVICE(haptic_attrs); + +int omi_haptic_register_service(omi_haptic_write_cb_t cb) +{ + int err = ensure_haptic_uuids(); + if (err) { + return err; + } + + haptic_write_cb = cb; + return bt_gatt_service_register(&haptic_service); +} + +// ----------------------------------------------------------------------------- +// SD card helpers + +const char *omi_sd_drive_name(void) +{ + return DISK_DRIVE_NAME; +} + +const char *omi_sd_mount_point(void) +{ + return DISK_MOUNT_PT; +} + +struct fs_mount_t *omi_sd_mount_struct(void) +{ + return &sd_mount; +} + +const struct device *omi_sd_device(void) +{ + return sd_dev; +} + +const struct gpio_dt_spec *omi_sd_enable_pin(void) +{ + return &sd_en_pin; +} + +// ----------------------------------------------------------------------------- +// Settings helpers + +int omi_settings_subsys_init(void) +{ + return settings_subsys_init(); +} + +int omi_settings_load(void) +{ + return settings_load(); +} + +int omi_settings_save_one(const char *name, const void *value, size_t len) +{ + return settings_save_one(name, value, len); +} + +bool omi_settings_name_steq(const char *name, const char *key, const char **next) +{ + return settings_name_steq(name, key, next); +} + +static omi_settings_set_cb g_settings_cb; +static void *g_settings_user_data; + +static int omi_settings_set_trampoline(const char *name, size_t len, settings_read_cb read_cb, void *cb_arg) +{ + if (!g_settings_cb) { + return -ENOENT; + } + return g_settings_cb(name, len, read_cb, cb_arg, g_settings_user_data); +} + +int omi_settings_register_handler(const char *subtree, omi_settings_set_cb set_cb, void *user_data) +{ + static struct settings_handler handler = { + .name = NULL, + .h_set = omi_settings_set_trampoline, + .h_get = NULL, + .h_commit = NULL, + .h_export = NULL, + }; + + handler.name = subtree; + g_settings_cb = set_cb; + g_settings_user_data = user_data; + return settings_register(&handler); +} + +// ----------------------------------------------------------------------------- +// SAADC helpers + +void omi_saadc_trigger_offset_calibration(void) +{ + NRF_SAADC_S->TASKS_CALIBRATEOFFSET = 1; +} diff --git a/omi/firmware/omi/src/ffi/shims.h b/omi/firmware/omi/src/ffi/shims.h new file mode 100644 index 0000000000..3a752886a2 --- /dev/null +++ b/omi/firmware/omi/src/ffi/shims.h @@ -0,0 +1,204 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ----------------------------------------------------------------------------- +// LED helpers +// ----------------------------------------------------------------------------- + +bool omi_led_ready_red(void); +bool omi_led_ready_green(void); +bool omi_led_ready_blue(void); + +uint32_t omi_led_period_red(void); +uint32_t omi_led_period_green(void); +uint32_t omi_led_period_blue(void); + +int omi_led_set_red(uint32_t pulse_width_ns); +int omi_led_set_green(uint32_t pulse_width_ns); +int omi_led_set_blue(uint32_t pulse_width_ns); + +// ----------------------------------------------------------------------------- +// Device helpers +// ----------------------------------------------------------------------------- + +enum omi_device_id { + OMI_DEVICE_SPI_FLASH, + OMI_DEVICE_ADC, + OMI_DEVICE_SDHC0, + OMI_DEVICE_DMIC0, +}; + +const struct device *omi_device_get(enum omi_device_id id); +bool omi_device_is_ready(const struct device *dev); +int omi_pm_device_action(const struct device *dev, enum pm_device_action action); +const char *omi_device_name(const struct device *dev); + +// ----------------------------------------------------------------------------- +// GPIO helpers +// ----------------------------------------------------------------------------- + +enum omi_gpio_pin_id { + OMI_PIN_MOTOR, + OMI_PIN_BAT_POWER, + OMI_PIN_BAT_READ, + OMI_PIN_BAT_CHG, + OMI_PIN_SD_EN, +}; + +const struct gpio_dt_spec *omi_gpio_pin(enum omi_gpio_pin_id id); +bool omi_gpio_is_ready(const struct gpio_dt_spec *pin); +int omi_gpio_configure(const struct gpio_dt_spec *pin, gpio_flags_t flags); +int omi_gpio_set(const struct gpio_dt_spec *pin, int value); +int omi_gpio_get(const struct gpio_dt_spec *pin); + +// Commonly used GPIO flags re-exported for FFI consumers +enum { + OMI_GPIO_OUTPUT = GPIO_OUTPUT, + OMI_GPIO_INPUT = GPIO_INPUT, + OMI_GPIO_PULL_UP = GPIO_PULL_UP, + OMI_GPIO_PULL_DOWN = GPIO_PULL_DOWN, +}; + +uint32_t omi_gpio_flag_output(void); +uint32_t omi_gpio_flag_input(void); + +// nRF drive strengths often needed by battery code +#ifdef NRF_GPIO_DRIVE_S0H1 +enum { + OMI_GPIO_DRIVE_S0H1 = NRF_GPIO_DRIVE_S0H1, +}; +#endif + +// ----------------------------------------------------------------------------- +// ADC helpers +// ----------------------------------------------------------------------------- + +void omi_adc_sequence_init(struct adc_sequence *sequence, uint32_t channel_mask, void *buffer, size_t buffer_size, uint8_t resolution); +int omi_adc_channel_setup(const struct device *adc_dev, const struct adc_channel_cfg *cfg); +int omi_adc_read(const struct device *adc_dev, const struct adc_sequence *sequence); +uint16_t omi_adc_ref_internal_mv(const struct device *adc_dev); +int omi_adc_raw_to_millivolts(uint16_t vref, enum adc_gain gain, uint8_t resolution, int32_t *val); + +// ----------------------------------------------------------------------------- +// Delayable work helpers +// ----------------------------------------------------------------------------- + +typedef void (*omi_work_callback_t)(void *user_data); + +struct omi_delayable_work; + +struct omi_delayable_work *omi_delayable_work_create(omi_work_callback_t cb, void *user_data); +void omi_delayable_work_destroy(struct omi_delayable_work *wrapper); +void omi_delayable_work_set_user_data(struct omi_delayable_work *wrapper, void *user_data); +int omi_delayable_work_schedule(struct omi_delayable_work *wrapper, uint32_t delay_ms); +int omi_delayable_work_cancel(struct omi_delayable_work *wrapper); + +// ----------------------------------------------------------------------------- +// File system helpers +// ----------------------------------------------------------------------------- + +int omi_disk_access_ioctl(const char *disk_pdrv, uint8_t cmd, void *buffer); +int omi_fs_mount(struct fs_mount_t *mount); +int omi_fs_unmount(struct fs_mount_t *mount); +int omi_fs_mkfs(fs_system_t type, uintptr_t storage_dev, void *scratch, uint32_t scratch_size); + +// ----------------------------------------------------------------------------- +// DMIC helpers +// ----------------------------------------------------------------------------- + +int omi_dmic_configure(const struct device *dev, const struct dmic_cfg *cfg); +int omi_dmic_trigger(const struct device *dev, enum dmic_trigger trigger); +int omi_dmic_read(const struct device *dev, uint8_t stream, void **buffer, uint32_t *size, int32_t timeout_ms); +int omi_mic_configure(uint32_t sample_rate, uint8_t channels); + +// ----------------------------------------------------------------------------- +// Logging helpers +// ----------------------------------------------------------------------------- + +void omi_log_inf(const char *msg); +void omi_log_err(const char *msg); + +// ----------------------------------------------------------------------------- +// Settings helpers +// ----------------------------------------------------------------------------- + +int omi_settings_subsys_init(void); +int omi_settings_load(void); +int omi_settings_save_one(const char *name, const void *value, size_t len); +bool omi_settings_name_steq(const char *name, const char *key, const char **next); + +typedef int (*omi_settings_set_cb)(const char *name, size_t len, settings_read_cb read_cb, void *cb_arg, void *user_data); + +int omi_settings_register_handler(const char *subtree, omi_settings_set_cb set_cb, void *user_data); + +// ----------------------------------------------------------------------------- +// SAADC helpers +// ----------------------------------------------------------------------------- + +void omi_saadc_trigger_offset_calibration(void); + +// ----------------------------------------------------------------------------- +// Battery helpers +// ----------------------------------------------------------------------------- + +typedef void (*omi_gpio_edge_cb_t)(void *user_data); + +int omi_battery_prepare_measurement_pin(void); +int omi_battery_restore_measurement_pin(void); +int omi_battery_channel_setup(void); +int omi_battery_perform_read(int16_t *buffer, size_t sample_count, uint32_t extra_samplings); +int omi_battery_configure_pins(void); +int omi_battery_set_chg_handler(omi_gpio_edge_cb_t handler, void *user_data); +int omi_battery_enable_chg_interrupt(void); +int omi_battery_disable_chg_interrupt(void); +int omi_battery_read_chg_pin(void); + +void omi_sleep_ms(uint32_t ms); +void omi_busy_wait_us(uint32_t us); +// ----------------------------------------------------------------------------- +// Thread/memory helpers +// ----------------------------------------------------------------------------- + +#include + +struct k_thread; +struct k_mem_slab; + +struct k_thread *omi_thread_create(void (*entry)(void *, void *, void *), void *p1, void *p2, void *p3, int priority); +void omi_thread_start(struct k_thread *thread); +void omi_thread_abort(struct k_thread *thread); + +struct k_mem_slab *omi_mic_mem_slab(void); +int omi_mem_slab_alloc(struct k_mem_slab *slab, void **mem, uint32_t timeout_ms); +int omi_mem_slab_free(struct k_mem_slab *slab, void *mem); + +// ----------------------------------------------------------------------------- +// Haptic helpers +// ----------------------------------------------------------------------------- + +typedef void (*omi_haptic_write_cb_t)(uint8_t value); + +int omi_haptic_register_service(omi_haptic_write_cb_t cb); + +// ----------------------------------------------------------------------------- +// SD card helpers +// ----------------------------------------------------------------------------- + +const char *omi_sd_drive_name(void); +const char *omi_sd_mount_point(void); +struct fs_mount_t *omi_sd_mount_struct(void); +const struct device *omi_sd_device(void); +const struct gpio_dt_spec *omi_sd_enable_pin(void); diff --git a/omi/firmware/omi/src/rust/Cargo.toml b/omi/firmware/omi/src/rust/Cargo.toml new file mode 100644 index 0000000000..6272ed272f --- /dev/null +++ b/omi/firmware/omi/src/rust/Cargo.toml @@ -0,0 +1,21 @@ +[package] +name = "omi_rust" +version = "0.1.0" +edition = "2021" + +[lib] +path = "lib.rs" +crate-type = ["staticlib"] + +[features] +default = [] +std = [] + +[dependencies] +spin = { version = "0.9", default-features = false, features = ["mutex", "spin_mutex"] } + +[profile.dev] +panic = "abort" + +[profile.release] +panic = "abort" diff --git a/omi/firmware/omi/src/rust/app_main.rs b/omi/firmware/omi/src/rust/app_main.rs new file mode 100644 index 0000000000..e712e5ab2c --- /dev/null +++ b/omi/firmware/omi/src/rust/app_main.rs @@ -0,0 +1,223 @@ +use core::sync::atomic::{AtomicU32, Ordering}; + +use crate::battery; +use crate::button; +use crate::codec; +use crate::ffi; +use crate::hal; +use crate::haptic; +use crate::led; +use crate::mic; +use crate::sd_card; +use crate::settings; +use crate::spi_flash; +use crate::transport; +use crate::util; + +const MIC_BUFFER_SAMPLES: usize = 1600; +const ENABLE_BATTERY: bool = option_env!("CONFIG_OMI_ENABLE_BATTERY").is_some(); +const ENABLE_BUTTON: bool = option_env!("CONFIG_OMI_ENABLE_BUTTON").is_some(); +const ENABLE_HAPTIC: bool = option_env!("CONFIG_OMI_ENABLE_HAPTIC").is_some(); +const ENABLE_OFFLINE_STORAGE: bool = option_env!("CONFIG_OMI_ENABLE_OFFLINE_STORAGE").is_some(); + +#[no_mangle] +pub static mut is_connected: bool = false; +#[no_mangle] +pub static mut is_charging: bool = false; +#[no_mangle] +pub static mut is_off: bool = false; + +#[no_mangle] +pub static mut gatt_notify_count: u32 = 0; +#[no_mangle] +pub static mut total_mic_buffer_bytes: u32 = 0; +#[no_mangle] +pub static mut broadcast_audio_count: u32 = 0; +#[no_mangle] +pub static mut write_to_tx_queue_count: u32 = 0; + +static MIC_BUFFER_TOTAL: AtomicU32 = AtomicU32::new(0); + +fn log_model_info() { + if let Some(model) = option_env!("CONFIG_BT_DIS_MODEL") { + util::log_info_fmt(format_args!("Model: {model}")); + } + if let Some(fw) = option_env!("CONFIG_BT_DIS_FW_REV_STR") { + util::log_info_fmt(format_args!("Firmware revision: {fw}")); + } + if let Some(hw) = option_env!("CONFIG_BT_DIS_HW_REV_STR") { + util::log_info_fmt(format_args!("Hardware revision: {hw}")); + } +} + +fn boot_led_sequence() { + led::red(true); + hal::sleep_ms(600); + led::red(false); + hal::sleep_ms(200); + + led::green(true); + hal::sleep_ms(600); + led::green(false); + hal::sleep_ms(200); + + led::blue(true); + hal::sleep_ms(600); + led::blue(false); + hal::sleep_ms(200); + + led::red(true); + led::green(true); + led::blue(true); + hal::sleep_ms(600); + led::red(false); + led::green(false); + led::blue(false); +} + +fn set_led_state() { + let charging = unsafe { is_charging }; + let off = unsafe { is_off }; + let connected = unsafe { is_connected }; + + led::green(charging); + led::red(!(off || connected)); + led::blue(connected); +} + +fn suspend_unused_modules() { + if let Err(err) = spi_flash::suspend() { + util::log_error_fmt(format_args!("Cannot suspend SPI flash: {err}")); + } + + if let Err(err) = sd_card::power_off() { + util::log_error_fmt(format_args!("Cannot suspend SD card: {err}")); + } +} + +extern "C" fn codec_handler(data: *mut u8, len: usize) { + unsafe { + broadcast_audio_count = broadcast_audio_count.wrapping_add(1); + } + + let _ = codec::broadcast_packets(data, len); +} + +unsafe extern "C" fn mic_handler(buffer: *mut i16) { + MIC_BUFFER_TOTAL.fetch_add(1, Ordering::Relaxed); + let _ = codec::receive_pcm(buffer, MIC_BUFFER_SAMPLES); +} + +fn loop_body() { + let total_bytes = MIC_BUFFER_TOTAL.load(Ordering::Relaxed); + unsafe { + total_mic_buffer_bytes = total_bytes; + } + + unsafe { + util::log_info_fmt(format_args!( + "Total mic buffer bytes: {total_bytes}, GATT notify count: {gatt_notify_count}, \ +Broadcast count: {broadcast_audio_count}, TX queue writes: {write_to_tx_queue_count}" + )); + } + + set_led_state(); + hal::sleep_ms(1000); +} + +#[no_mangle] +pub extern "C" fn main() -> i32 { + unsafe { + ffi::printk(b"Starting omi ...\n\0".as_ptr()); + } + + util::log_info("Suspending unused modules...\n"); + suspend_unused_modules(); + + util::log_info("Initializing settings...\n"); + if let Err(ret) = settings::init() { + util::log_error_fmt(format_args!("Failed to initialize settings ({ret})")); + let _ = settings::save_dim_ratio(5); + led::red(true); + hal::sleep_ms(500); + led::red(false); + hal::sleep_ms(200); + let _ = settings::save_dim_ratio(100); + led::red(true); + hal::sleep_ms(500); + led::red(false); + hal::sleep_ms(200); + let _ = settings::save_dim_ratio(30); + led::red(true); + hal::sleep_ms(500); + led::red(false); + } + + util::log_info("Initializing LEDs...\n"); + if let Err(ret) = led::init() { + util::log_error_fmt(format_args!("Failed to initialize LEDs ({ret})")); + return ret; + } + + boot_led_sequence(); + + if ENABLE_BATTERY { + if let Err(err) = battery::init() { + util::log_error_fmt(format_args!("Battery init failed ({err})")); + return err; + } + + if let Err(err) = battery::start_charging() { + util::log_error_fmt(format_args!("Battery failed to start ({err})")); + return err; + } + util::log_info("Battery initialized\n"); + } + + if ENABLE_BUTTON { + if let Err(err) = button::init() { + return err; + } + button::activate_work(); + } + + if ENABLE_HAPTIC { + if let Err(err) = haptic::init() { + util::log_error_fmt(format_args!("Failed to initialize Haptic ({err})")); + } else { + haptic::play_milliseconds(100); + } + } + + log_model_info(); + + if ENABLE_OFFLINE_STORAGE { + util::log_info("Initializing transport...\n"); + } + + if let Err(err) = transport::start() { + return err; + } + + util::log_info("Initializing codec...\n"); + codec::set_callback(codec_handler); + if let Err(ret) = codec::start() { + return ret; + } + + util::log_info("Initializing microphone...\n"); + mic::set_callback(mic_handler); + if let Err(ret) = mic::start() { + return ret; + } + + if ENABLE_HAPTIC { + haptic::register_service(); + } + + util::log_info("Device initialized successfully\n"); + + loop { + loop_body(); + } +} diff --git a/omi/firmware/omi/src/rust/battery.rs b/omi/firmware/omi/src/rust/battery.rs new file mode 100644 index 0000000000..d60defa97e --- /dev/null +++ b/omi/firmware/omi/src/rust/battery.rs @@ -0,0 +1,374 @@ +use core::ffi::c_void; + +use crate::hal::{self, AdcDevice, BatteryHardware, Error as HalError}; +use crate::util; +use spin::Mutex; + +const ADC_TOTAL_SAMPLES: usize = 50; +const HISTORY_SIZE: usize = 5; +const R1: u16 = 1091; +const R2: u16 = 499; +const ADC_RESOLUTION: u8 = 12; + +static SAMPLE_BUFFER: Mutex<[i16; ADC_TOTAL_SAMPLES + 1]> = Mutex::new([0; ADC_TOTAL_SAMPLES + 1]); + +struct History { + values: [u16; HISTORY_SIZE], + index: usize, + initialized: bool, +} + +static HISTORY: Mutex = Mutex::new(History { + values: [0; HISTORY_SIZE], + index: 0, + initialized: false, +}); + +#[derive(Clone, Copy)] +struct BatteryState { + millivolts: u16, + percentage: u8, +} + +const BATTERY_STATES: [BatteryState; 12] = [ + BatteryState { + millivolts: 4200, + percentage: 100, + }, + BatteryState { + millivolts: 4160, + percentage: 99, + }, + BatteryState { + millivolts: 4090, + percentage: 91, + }, + BatteryState { + millivolts: 4030, + percentage: 78, + }, + BatteryState { + millivolts: 3890, + percentage: 63, + }, + BatteryState { + millivolts: 3830, + percentage: 53, + }, + BatteryState { + millivolts: 3680, + percentage: 36, + }, + BatteryState { + millivolts: 3660, + percentage: 35, + }, + BatteryState { + millivolts: 3480, + percentage: 14, + }, + BatteryState { + millivolts: 3420, + percentage: 11, + }, + BatteryState { + millivolts: 3400, + percentage: 1, + }, + BatteryState { + millivolts: 0, + percentage: 0, + }, +]; + +extern "C" { + static mut is_charging: bool; +} + +fn error_code(err: HalError) -> i32 { + err.as_errno() +} + +struct MeasureGuard<'a> { + hw: &'a BatteryHardware, + active: bool, +} + +impl<'a> MeasureGuard<'a> { + fn new(hw: &'a BatteryHardware) -> Self { + Self { hw, active: false } + } + + fn arm(&mut self) { + self.active = true; + } +} + +impl<'a> Drop for MeasureGuard<'a> { + fn drop(&mut self) { + if self.active { + let _ = self.hw.restore_measurement(); + } + } +} + +fn update_history(value: u16) -> u16 { + let mut history = HISTORY.lock(); + + if !history.initialized { + history.values.fill(value); + history.initialized = true; + } else { + let idx = history.index; + history.values[idx] = value; + } + + history.index = (history.index + 1) % HISTORY_SIZE; + + let sum: u32 = history.values.iter().map(|&v| v as u32).sum(); + (sum / HISTORY_SIZE as u32) as u16 +} + +fn median_from_buffer(buffer: &[i16]) -> i32 { + let mut sorted = [0i16; ADC_TOTAL_SAMPLES]; + sorted.copy_from_slice(&buffer[1..=ADC_TOTAL_SAMPLES]); + sorted.sort_unstable(); + + if ADC_TOTAL_SAMPLES % 2 == 0 { + let a = sorted[ADC_TOTAL_SAMPLES / 2 - 1] as i32; + let b = sorted[ADC_TOTAL_SAMPLES / 2] as i32; + (a + b) / 2 + } else { + sorted[ADC_TOTAL_SAMPLES / 2] as i32 + } +} + +fn convert_to_millivolts(adc: &AdcDevice, raw: i32) -> Result { + let mut value = raw; + if let Err(err) = adc.raw_to_millivolts(hal::ADC_GAIN_1_3, ADC_RESOLUTION, &mut value) { + Err(err.as_errno()) + } else { + Ok(value) + } +} + +fn scale_to_battery_voltage(adc_mv: i32, charging: bool) -> u16 { + let corrected = if charging { adc_mv - 16 } else { adc_mv }; + if corrected <= 0 { + return 0; + } + + let numerator = (corrected as i64) * (R1 as i64 + R2 as i64); + let voltage = numerator / R2 as i64; + voltage.max(0).min(u16::MAX as i64) as u16 +} + +fn hardware() -> BatteryHardware { + BatteryHardware::new() +} + +#[no_mangle] +pub extern "C" fn battery_get_millivolt(out: *mut u16) -> i32 { + if out.is_null() { + return HalError::NullPointer.as_errno(); + } + + let adc = match AdcDevice::new() { + Ok(adc) => adc, + Err(err) => return err.as_errno(), + }; + + let hw = hardware(); + let mut buffer = SAMPLE_BUFFER.lock(); + let mut guard = MeasureGuard::new(&hw); + + if let Err(err) = hw.prepare_measurement() { + return error_code(err); + } + guard.arm(); + + if !adc.is_ready() { + return HalError::DeviceNotReady.as_errno(); + } + + if let Err(err) = hw.channel_setup() { + return error_code(err); + } + + hw.calibrate_offset(); + hal::busy_wait_us(100); + + if let Err(err) = hw.read_samples(&mut buffer[..], ADC_TOTAL_SAMPLES as u32) { + return error_code(err); + } + + let median = median_from_buffer(&buffer[..]); + let adc_mv = match convert_to_millivolts(&adc, median) { + Ok(value) => value, + Err(err) => return err, + }; + + let charging = unsafe { is_charging }; + let raw_mv = scale_to_battery_voltage(adc_mv, charging); + let filtered = update_history(raw_mv); + + unsafe { + *out = filtered; + } + + 0 +} + +#[no_mangle] +pub extern "C" fn battery_get_percentage(out: *mut u8, battery_millivolt: u16) -> i32 { + if out.is_null() { + return HalError::NullPointer.as_errno(); + } + + let percentage = if battery_millivolt >= BATTERY_STATES[0].millivolts { + BATTERY_STATES[0].percentage + } else if battery_millivolt <= BATTERY_STATES[BATTERY_STATES.len() - 1].millivolts { + BATTERY_STATES[BATTERY_STATES.len() - 1].percentage + } else { + let mut value = BATTERY_STATES[BATTERY_STATES.len() - 1].percentage; + for window in BATTERY_STATES.windows(2) { + let upper = window[0]; + let lower = window[1]; + if battery_millivolt <= upper.millivolts && battery_millivolt > lower.millivolts { + let voltage_range = (upper.millivolts - lower.millivolts) as u32; + let percentage_range = (upper.percentage - lower.percentage) as u32; + let voltage_diff = (upper.millivolts - battery_millivolt) as u32; + value = + upper.percentage - ((voltage_diff * percentage_range) / voltage_range) as u8; + break; + } + } + value + }; + + unsafe { + *out = percentage; + } + + 0 +} + +#[no_mangle] +pub extern "C" fn battery_charge_start() -> i32 { + match start_charging_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +#[no_mangle] +pub extern "C" fn battery_charge_stop() -> i32 { + 0 +} + +#[no_mangle] +pub extern "C" fn battery_set_fast_charge() -> i32 { + 0 +} + +#[no_mangle] +pub extern "C" fn battery_set_slow_charge() -> i32 { + 0 +} + +#[no_mangle] +pub extern "C" fn battery_charging_state_read() -> i32 { + let hw = hardware(); + let state = match hw.read_charge_pin() { + Ok(val) => val, + Err(err) => return error_code(err), + }; + + unsafe { + is_charging = state == 0; + } + 0 +} + +#[no_mangle] +pub extern "C" fn battery_enable_read() -> i32 { + let adc = match AdcDevice::new() { + Ok(adc) => adc, + Err(err) => return err.as_errno(), + }; + + let hw = hardware(); + let mut buffer = SAMPLE_BUFFER.lock(); + let mut guard = MeasureGuard::new(&hw); + + if let Err(err) = hw.prepare_measurement() { + return error_code(err); + } + guard.arm(); + + hal::sleep_ms(10); + + if !adc.is_ready() { + return HalError::DeviceNotReady.as_errno(); + } + + if let Err(err) = hw.channel_setup() { + return error_code(err); + } + + hw.calibrate_offset(); + hal::sleep_ms(5); + + match hw.read_samples(&mut buffer[..], ADC_TOTAL_SAMPLES as u32) { + Ok(()) => 0, + Err(err) => error_code(err), + } +} + +unsafe extern "C" fn battery_chg_trampoline(_user_data: *mut c_void) { + let res = battery_charging_state_read(); + if res < 0 { + util::log_error_fmt(format_args!("Failed to read charging state ({res})\n")); + } +} + +#[no_mangle] +pub extern "C" fn battery_init() -> i32 { + match init_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +fn start_charging_impl() -> Result<(), i32> { + Ok(()) +} + +fn init_impl() -> Result<(), i32> { + let hw = hardware(); + + if let Err(err) = hw.configure_pins() { + return Err(error_code(err)); + } + + if let Err(err) = hw.set_charging_handler(Some(battery_chg_trampoline), core::ptr::null_mut()) { + return Err(error_code(err)); + } + + if let Err(err) = hw.enable_charging_interrupt() { + return Err(error_code(err)); + } + + let _ = battery_enable_read(); + let _ = battery_charging_state_read(); + + Ok(()) +} + +pub fn init() -> Result<(), i32> { + init_impl() +} + +pub fn start_charging() -> Result<(), i32> { + start_charging_impl() +} diff --git a/omi/firmware/omi/src/rust/ble.rs b/omi/firmware/omi/src/rust/ble.rs new file mode 100644 index 0000000000..ebf03bf37d --- /dev/null +++ b/omi/firmware/omi/src/rust/ble.rs @@ -0,0 +1,283 @@ +use core::ffi::c_char; + +use crate::macros::omi_macro_support::{ + BleAccess, BleCharacteristicSpec, BleServiceSpec, CodecSpec, +}; +use crate::omi_ble_service; + +omi_ble_service!( + name: audio_service, + uuid: "19B10000-E8F2-537E-4F6C-D104768A1214", + advertise: true, + characteristics: [ + { + name: data, + uuid: "19B10001-E8F2-537E-4F6C-D104768A1214", + access: [Read, Notify], + codec: Binary + }, + { + name: codec, + uuid: "19B10002-E8F2-537E-4F6C-D104768A1214", + access: [Read], + codec: Json(u8) + }, + { + name: speaker, + uuid: "19B10003-E8F2-537E-4F6C-D104768A1214", + access: [Write, Notify], + codec: Binary + } + ] +); + +omi_ble_service!( + name: settings_service, + uuid: "19B10010-E8F2-537E-4F6C-D104768A1214", + advertise: false, + characteristics: [ + { + name: dim_ratio, + uuid: "19B10011-E8F2-537E-4F6C-D104768A1214", + access: [Read, Write], + codec: Json(u8) + } + ] +); + +omi_ble_service!( + name: features_service, + uuid: "19B10020-E8F2-537E-4F6C-D104768A1214", + advertise: false, + characteristics: [ + { + name: flags, + uuid: "19B10021-E8F2-537E-4F6C-D104768A1214", + access: [Read], + codec: Json(u32) + } + ] +); + +omi_ble_service!( + name: storage_service, + uuid: "30295780-4301-EABD-2904-2849ADFEAE43", + advertise: false, + characteristics: [ + { + name: command, + uuid: "30295781-4301-EABD-2904-2849ADFEAE43", + access: [Write, Notify], + codec: Binary + }, + { + name: status, + uuid: "30295782-4301-EABD-2904-2849ADFEAE43", + access: [Read, Notify], + codec: Binary + } + ] +); + +omi_ble_service!( + name: button_service, + uuid: "23BA7924-0000-1000-7450-346EAC492E92", + advertise: false, + characteristics: [ + { + name: event, + uuid: "23BA7925-0000-1000-7450-346EAC492E92", + access: [Read, Notify], + codec: Binary + } + ] +); + +omi_ble_service!( + name: accel_service, + uuid: "32403790-0000-1000-7450-BF445E5829A2", + advertise: false, + characteristics: [ + { + name: sample, + uuid: "32403791-0000-1000-7450-BF445E5829A2", + access: [Read, Notify], + codec: Binary + } + ] +); + +omi_ble_service!( + name: haptic_service, + uuid: "CAB1AB95-2EA5-4F4D-BB56-874B72CFC984", + advertise: false, + characteristics: [ + { + name: command, + uuid: "CAB1AB96-2EA5-4F4D-BB56-874B72CFC984", + access: [Write], + codec: Binary + } + ] +); + +pub use accel_service::Characteristic as AccelCharacteristic; +pub use audio_service::Characteristic as AudioCharacteristic; +pub use button_service::Characteristic as ButtonCharacteristic; +pub use features_service::Characteristic as FeaturesCharacteristic; +pub use haptic_service::Characteristic as HapticCharacteristic; +pub use settings_service::Characteristic as SettingsCharacteristic; +pub use storage_service::Characteristic as StorageCharacteristic; + +pub const AUDIO_SERVICE: &BleServiceSpec = &audio_service::SPEC; +pub const SETTINGS_SERVICE: &BleServiceSpec = &settings_service::SPEC; +pub const FEATURES_SERVICE: &BleServiceSpec = &features_service::SPEC; +pub const STORAGE_SERVICE: &BleServiceSpec = &storage_service::SPEC; +pub const BUTTON_SERVICE: &BleServiceSpec = &button_service::SPEC; +pub const ACCEL_SERVICE: &BleServiceSpec = &accel_service::SPEC; +pub const HAPTIC_SERVICE: &BleServiceSpec = &haptic_service::SPEC; + +pub fn audio_service_spec() -> &'static BleServiceSpec { + AUDIO_SERVICE +} + +pub fn settings_service_spec() -> &'static BleServiceSpec { + SETTINGS_SERVICE +} + +pub fn features_service_spec() -> &'static BleServiceSpec { + FEATURES_SERVICE +} + +pub fn storage_service_spec() -> &'static BleServiceSpec { + STORAGE_SERVICE +} + +pub fn button_service_spec() -> &'static BleServiceSpec { + BUTTON_SERVICE +} + +pub fn accel_service_spec() -> &'static BleServiceSpec { + ACCEL_SERVICE +} + +pub fn haptic_service_spec() -> &'static BleServiceSpec { + HAPTIC_SERVICE +} + +pub fn haptic_command() -> &'static BleCharacteristicSpec { + &haptic_service::command +} + +pub fn haptic_command_access() -> &'static [BleAccess] { + haptic_command().access() +} + +pub fn haptic_command_codec() -> CodecSpec { + haptic_command().codec() +} + +pub fn services() -> &'static [&'static BleServiceSpec] { + &[ + AUDIO_SERVICE, + SETTINGS_SERVICE, + FEATURES_SERVICE, + STORAGE_SERVICE, + BUTTON_SERVICE, + ACCEL_SERVICE, + HAPTIC_SERVICE, + ] +} + +pub fn advertised_services() -> &'static [&'static BleServiceSpec] { + &[AUDIO_SERVICE] +} + +fn c_str_ptr(s: &'static str) -> *const c_char { + s.as_ptr() as *const c_char +} + +#[no_mangle] +pub extern "C" fn omi_ble_audio_service_uuid() -> *const c_char { + c_str_ptr(audio_service::uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_audio_data_uuid() -> *const c_char { + c_str_ptr(AudioCharacteristic::data.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_audio_codec_uuid() -> *const c_char { + c_str_ptr(AudioCharacteristic::codec.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_audio_speaker_uuid() -> *const c_char { + c_str_ptr(AudioCharacteristic::speaker.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_settings_service_uuid() -> *const c_char { + c_str_ptr(settings_service::uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_settings_dim_ratio_uuid() -> *const c_char { + c_str_ptr(SettingsCharacteristic::dim_ratio.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_features_service_uuid() -> *const c_char { + c_str_ptr(features_service::uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_features_flags_uuid() -> *const c_char { + c_str_ptr(FeaturesCharacteristic::flags.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_storage_service_uuid() -> *const c_char { + c_str_ptr(storage_service::uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_storage_command_uuid() -> *const c_char { + c_str_ptr(StorageCharacteristic::command.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_storage_status_uuid() -> *const c_char { + c_str_ptr(StorageCharacteristic::status.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_button_service_uuid() -> *const c_char { + c_str_ptr(button_service::uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_button_event_uuid() -> *const c_char { + c_str_ptr(ButtonCharacteristic::event.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_accel_service_uuid() -> *const c_char { + c_str_ptr(accel_service::uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_accel_sample_uuid() -> *const c_char { + c_str_ptr(AccelCharacteristic::sample.uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_haptic_service_uuid() -> *const c_char { + c_str_ptr(haptic_service::uuid_cstr()) +} + +#[no_mangle] +pub extern "C" fn omi_ble_haptic_command_uuid() -> *const c_char { + c_str_ptr(HapticCharacteristic::command.uuid_cstr()) +} diff --git a/omi/firmware/omi/src/rust/button.rs b/omi/firmware/omi/src/rust/button.rs new file mode 100644 index 0000000000..196633376d --- /dev/null +++ b/omi/firmware/omi/src/rust/button.rs @@ -0,0 +1,23 @@ +use crate::util; + +mod ffi { + extern "C" { + pub fn button_init() -> i32; + pub fn activate_button_work(); + } +} + +pub fn init() -> Result<(), i32> { + let rc = unsafe { ffi::button_init() }; + if rc < 0 { + util::log_error_fmt(format_args!("Failed to initialize Button ({rc})")); + Err(rc) + } else { + util::log_info("Button initialized\n"); + Ok(()) + } +} + +pub fn activate_work() { + unsafe { ffi::activate_button_work() }; +} diff --git a/omi/firmware/omi/src/rust/codec.rs b/omi/firmware/omi/src/rust/codec.rs new file mode 100644 index 0000000000..43a2604c85 --- /dev/null +++ b/omi/firmware/omi/src/rust/codec.rs @@ -0,0 +1,44 @@ +use crate::util; + +mod ffi { + extern "C" { + pub fn set_codec_callback(cb: extern "C" fn(*mut u8, usize)); + pub fn codec_start() -> i32; + pub fn broadcast_audio_packets(data: *mut u8, len: usize) -> i32; + pub fn codec_receive_pcm(buffer: *mut i16, samples: usize) -> i32; + } +} + +pub fn set_callback(cb: extern "C" fn(*mut u8, usize)) { + unsafe { ffi::set_codec_callback(cb) }; +} + +pub fn start() -> Result<(), i32> { + let rc = unsafe { ffi::codec_start() }; + if rc < 0 { + util::log_error_fmt(format_args!("Failed to start codec ({rc})")); + Err(rc) + } else { + Ok(()) + } +} + +pub fn broadcast_packets(data: *mut u8, len: usize) -> Result<(), i32> { + let rc = unsafe { ffi::broadcast_audio_packets(data, len) }; + if rc < 0 { + util::log_error_fmt(format_args!("Failed to broadcast audio packets ({rc})")); + Err(rc) + } else { + Ok(()) + } +} + +pub fn receive_pcm(buffer: *mut i16, samples: usize) -> Result<(), i32> { + let rc = unsafe { ffi::codec_receive_pcm(buffer, samples) }; + if rc < 0 { + util::log_error_fmt(format_args!("Failed to process PCM data ({rc})")); + Err(rc) + } else { + Ok(()) + } +} diff --git a/omi/firmware/omi/src/rust/ffi.rs b/omi/firmware/omi/src/rust/ffi.rs new file mode 100644 index 0000000000..e7db506b47 --- /dev/null +++ b/omi/firmware/omi/src/rust/ffi.rs @@ -0,0 +1,195 @@ +#![allow(non_camel_case_types)] +#![allow(dead_code)] + +use core::ffi::{c_char, c_int, c_uint, c_void}; + +pub const OMI_DEVICE_SPI_FLASH: c_int = 0; +pub const OMI_DEVICE_ADC: c_int = 1; +pub const OMI_DEVICE_SDHC0: c_int = 2; +pub const OMI_DEVICE_DMIC0: c_int = 3; + +pub const OMI_PIN_MOTOR: c_int = 0; +pub const OMI_PIN_BAT_POWER: c_int = 1; +pub const OMI_PIN_BAT_READ: c_int = 2; +pub const OMI_PIN_BAT_CHG: c_int = 3; +pub const OMI_PIN_SD_EN: c_int = 4; + +pub const PM_DEVICE_ACTION_RESUME: c_int = 0; +pub const PM_DEVICE_ACTION_SUSPEND: c_int = 1; +pub const PM_DEVICE_ACTION_TURN_ON: c_int = 2; +pub const PM_DEVICE_ACTION_TURN_OFF: c_int = 3; +pub const PM_DEVICE_ACTION_LOW_POWER: c_int = 4; + +pub const ADC_GAIN_1_3: c_int = 3; + +pub const DMIC_TRIGGER_START: c_int = 0; +pub const DMIC_TRIGGER_STOP: c_int = 1; + +pub const DISK_IOCTL_CTRL_INIT: u8 = 0x10; +pub const DISK_IOCTL_GET_SECTOR_COUNT: u8 = 0x01; +pub const DISK_IOCTL_GET_SECTOR_SIZE: u8 = 0x02; +pub const DISK_IOCTL_CTRL_DEINIT: u8 = 0x11; +pub const FS_EXT2: c_int = 3; + +pub type omi_gpio_edge_cb_t = Option; +pub type omi_haptic_write_cb_t = Option; + +pub type settings_read_cb = + unsafe extern "C" fn(cb_arg: *mut c_void, data: *mut c_void, len: usize) -> c_int; +pub type omi_settings_set_cb = unsafe extern "C" fn( + name: *const c_char, + len: usize, + read_cb: settings_read_cb, + cb_arg: *mut c_void, + user_data: *mut c_void, +) -> c_int; + +pub type omi_work_callback_t = Option; + +extern "C" { + // LED helpers + pub fn omi_led_ready_red() -> bool; + pub fn omi_led_ready_green() -> bool; + pub fn omi_led_ready_blue() -> bool; + + pub fn omi_led_period_red() -> c_uint; + pub fn omi_led_period_green() -> c_uint; + pub fn omi_led_period_blue() -> c_uint; + + pub fn omi_led_set_red(pulse_width_ns: c_uint) -> c_int; + pub fn omi_led_set_green(pulse_width_ns: c_uint) -> c_int; + pub fn omi_led_set_blue(pulse_width_ns: c_uint) -> c_int; + + // Device helpers + pub fn omi_device_get(id: c_int) -> *const c_void; + pub fn omi_device_is_ready(dev: *const c_void) -> bool; + pub fn omi_pm_device_action(dev: *const c_void, action: c_int) -> c_int; + pub fn omi_device_name(dev: *const c_void) -> *const c_char; + + // GPIO helpers + pub fn omi_gpio_pin(id: c_int) -> *const c_void; + pub fn omi_gpio_is_ready(pin: *const c_void) -> bool; + pub fn omi_gpio_configure(pin: *const c_void, flags: u32) -> c_int; + pub fn omi_gpio_set(pin: *const c_void, value: c_int) -> c_int; + pub fn omi_gpio_get(pin: *const c_void) -> c_int; + pub fn omi_gpio_flag_output() -> u32; + pub fn omi_gpio_flag_input() -> u32; + + // ADC helpers + pub fn omi_adc_sequence_init( + sequence: *mut c_void, + channel_mask: u32, + buffer: *mut c_void, + buffer_size: usize, + resolution: u8, + ); + pub fn omi_adc_channel_setup(adc_dev: *const c_void, cfg: *const c_void) -> c_int; + pub fn omi_adc_read(adc_dev: *const c_void, sequence: *const c_void) -> c_int; + pub fn omi_adc_ref_internal_mv(adc_dev: *const c_void) -> u16; + pub fn omi_adc_raw_to_millivolts( + vref_mv: u16, + gain: c_int, + resolution: u8, + value: *mut i32, + ) -> c_int; + + // Delayable work helpers + pub fn omi_delayable_work_create( + cb: omi_work_callback_t, + user_data: *mut c_void, + ) -> *mut c_void; + pub fn omi_delayable_work_destroy(wrapper: *mut c_void); + pub fn omi_delayable_work_set_user_data(wrapper: *mut c_void, user_data: *mut c_void); + pub fn omi_delayable_work_schedule(wrapper: *mut c_void, delay_ms: u32) -> c_int; + pub fn omi_delayable_work_cancel(wrapper: *mut c_void) -> c_int; + + // File system helpers + pub fn omi_disk_access_ioctl(disk: *const c_char, cmd: u8, buffer: *mut c_void) -> c_int; + pub fn omi_fs_mount(mount: *mut c_void) -> c_int; + pub fn omi_fs_unmount(mount: *mut c_void) -> c_int; + pub fn omi_fs_mkfs( + fs_type: c_int, + storage_dev: usize, + scratch: *mut c_void, + scratch_size: u32, + ) -> c_int; + + // DMIC helpers + pub fn omi_dmic_configure(dev: *const c_void, cfg: *const c_void) -> c_int; + pub fn omi_dmic_trigger(dev: *const c_void, trigger: c_int) -> c_int; + pub fn omi_dmic_read( + dev: *const c_void, + stream: u8, + buffer: *mut *mut c_void, + size: *mut u32, + timeout_ms: i32, + ) -> c_int; + pub fn omi_mic_configure(sample_rate: u32, channels: u8) -> c_int; + + // Logging helpers + pub fn omi_log_inf(msg: *const c_char); + pub fn omi_log_err(msg: *const c_char); + + // Settings helpers + pub fn omi_settings_subsys_init() -> c_int; + pub fn omi_settings_load() -> c_int; + pub fn omi_settings_save_one(name: *const c_char, value: *const c_void, len: usize) -> c_int; + pub fn omi_settings_name_steq( + name: *const c_char, + key: *const c_char, + next: *mut *const c_char, + ) -> bool; + pub fn omi_settings_register_handler( + subtree: *const c_char, + set_cb: omi_settings_set_cb, + user_data: *mut c_void, + ) -> c_int; + + // SAADC helpers + pub fn omi_saadc_trigger_offset_calibration(); + + // Battery helpers + pub fn omi_battery_prepare_measurement_pin() -> c_int; + pub fn omi_battery_restore_measurement_pin() -> c_int; + pub fn omi_battery_channel_setup() -> c_int; + pub fn omi_battery_perform_read( + buffer: *mut i16, + sample_count: usize, + extra_samplings: u32, + ) -> c_int; + pub fn omi_battery_configure_pins() -> c_int; + pub fn omi_battery_set_chg_handler(cb: omi_gpio_edge_cb_t, user_data: *mut c_void) -> c_int; + pub fn omi_battery_enable_chg_interrupt() -> c_int; + pub fn omi_battery_disable_chg_interrupt() -> c_int; + pub fn omi_battery_read_chg_pin() -> c_int; + + pub fn omi_sleep_ms(ms: u32); + pub fn omi_busy_wait_us(us: u32); + + // Thread & memory helpers + pub fn omi_mic_mem_slab() -> *mut c_void; + pub fn omi_mem_slab_alloc(slab: *mut c_void, mem: *mut *mut c_void, timeout_ms: u32) -> c_int; + pub fn omi_mem_slab_free(slab: *mut c_void, mem: *mut c_void) -> c_int; + pub fn omi_thread_create( + entry: unsafe extern "C" fn(*mut c_void, *mut c_void, *mut c_void), + p1: *mut c_void, + p2: *mut c_void, + p3: *mut c_void, + priority: c_int, + ) -> *mut c_void; + pub fn omi_thread_start(thread: *mut c_void); + pub fn omi_thread_abort(thread: *mut c_void); + + // Haptic helpers + pub fn omi_haptic_register_service(cb: omi_haptic_write_cb_t) -> c_int; + + // SD helpers + pub fn omi_sd_drive_name() -> *const c_char; + pub fn omi_sd_mount_point() -> *const c_char; + pub fn omi_sd_mount_struct() -> *mut c_void; + pub fn omi_sd_device() -> *const c_void; + pub fn omi_sd_enable_pin() -> *const c_void; + + // Misc + pub fn printk(fmt: *const u8, ...) -> c_int; +} diff --git a/omi/firmware/omi/src/rust/hal.rs b/omi/firmware/omi/src/rust/hal.rs new file mode 100644 index 0000000000..fe82fdc663 --- /dev/null +++ b/omi/firmware/omi/src/rust/hal.rs @@ -0,0 +1,771 @@ +use core::ffi::{c_char, c_void, CStr}; +use core::ptr::{self, NonNull}; + +use crate::ffi; + +const ENODEV: i32 = -19; +const EINVAL: i32 = -22; +const EALREADY: i32 = -114; + +pub const ADC_GAIN_1_3: i32 = ffi::ADC_GAIN_1_3; +pub const DMIC_TRIGGER_START: i32 = ffi::DMIC_TRIGGER_START; +pub const DMIC_TRIGGER_STOP: i32 = ffi::DMIC_TRIGGER_STOP; +pub const DISK_IOCTL_CTRL_INIT: u8 = ffi::DISK_IOCTL_CTRL_INIT; +pub const DISK_IOCTL_CTRL_DEINIT: u8 = ffi::DISK_IOCTL_CTRL_DEINIT; +pub const DISK_IOCTL_GET_SECTOR_COUNT: u8 = ffi::DISK_IOCTL_GET_SECTOR_COUNT; +pub const DISK_IOCTL_GET_SECTOR_SIZE: u8 = ffi::DISK_IOCTL_GET_SECTOR_SIZE; + +#[derive(Debug, Clone, Copy)] +pub enum Error { + NullPointer, + DeviceNotReady, + C(i32), +} + +impl Error { + pub fn as_errno(self) -> i32 { + match self { + Error::NullPointer => EINVAL, + Error::DeviceNotReady => ENODEV, + Error::C(code) => code, + } + } +} + +pub type Result = core::result::Result; + +impl From for Error { + fn from(code: i32) -> Self { + if code < 0 { + Error::C(code) + } else { + Error::C(code) + } + } +} + +#[derive(Clone, Copy)] +pub struct GpioPin { + raw: NonNull, +} + +impl GpioPin { + pub fn new(raw: *const c_void) -> Result { + NonNull::new(raw as *mut c_void) + .map(|raw| Self { raw }) + .ok_or(Error::NullPointer) + } + + pub fn raw(&self) -> *const c_void { + self.raw.as_ptr() + } + + pub fn is_ready(&self) -> bool { + unsafe { ffi::omi_gpio_is_ready(self.raw.as_ptr() as *const c_void) } + } + + pub fn configure_output(&self) -> Result<()> { + let err = unsafe { + ffi::omi_gpio_configure( + self.raw.as_ptr() as *const c_void, + ffi::omi_gpio_flag_output(), + ) + }; + if err < 0 { + Err(Error::from(err)) + } else { + Ok(()) + } + } + + pub fn configure_input(&self) -> Result<()> { + let err = unsafe { + ffi::omi_gpio_configure( + self.raw.as_ptr() as *const c_void, + ffi::omi_gpio_flag_input(), + ) + }; + if err < 0 { + Err(Error::from(err)) + } else { + Ok(()) + } + } + + pub fn set(&self, value: bool) -> Result<()> { + let err = unsafe { ffi::omi_gpio_set(self.raw.as_ptr() as *const c_void, value as i32) }; + if err < 0 { + Err(Error::from(err)) + } else { + Ok(()) + } + } +} + +#[derive(Clone, Copy)] +pub enum LedColor { + Red, + Green, + Blue, +} + +#[derive(Clone, Copy)] +pub struct LedPwm { + color: LedColor, +} + +impl LedPwm { + pub fn new(color: LedColor) -> Self { + Self { color } + } + + pub fn red() -> Self { + Self::new(LedColor::Red) + } + + pub fn green() -> Self { + Self::new(LedColor::Green) + } + + pub fn blue() -> Self { + Self::new(LedColor::Blue) + } + + fn is_ready_fn(&self) -> unsafe extern "C" fn() -> bool { + match self.color { + LedColor::Red => ffi::omi_led_ready_red, + LedColor::Green => ffi::omi_led_ready_green, + LedColor::Blue => ffi::omi_led_ready_blue, + } + } + + fn period_fn(&self) -> unsafe extern "C" fn() -> u32 { + match self.color { + LedColor::Red => ffi::omi_led_period_red, + LedColor::Green => ffi::omi_led_period_green, + LedColor::Blue => ffi::omi_led_period_blue, + } + } + + fn set_fn(&self) -> unsafe extern "C" fn(u32) -> i32 { + match self.color { + LedColor::Red => ffi::omi_led_set_red, + LedColor::Green => ffi::omi_led_set_green, + LedColor::Blue => ffi::omi_led_set_blue, + } + } + + pub fn is_ready(&self) -> bool { + unsafe { (self.is_ready_fn())() } + } + + pub fn period(&self) -> u32 { + unsafe { (self.period_fn())() } + } + + pub fn set_pulse(&self, pulse: u32) -> Result<()> { + let err = unsafe { (self.set_fn())(pulse) }; + if err < 0 { + Err(Error::from(err)) + } else { + Ok(()) + } + } + + pub fn set_duty(&self, percent: u32) -> Result<()> { + let ratio = percent.min(100); + let pulse = (self.period() * ratio) / 100; + self.set_pulse(pulse) + } + + pub fn off(&self) -> Result<()> { + self.set_pulse(0) + } +} + +pub fn led_start() -> Result<()> { + let leds = [LedPwm::red(), LedPwm::green(), LedPwm::blue()]; + for led in leds.iter() { + if !led.is_ready() { + return Err(Error::DeviceNotReady); + } + } + Ok(()) +} + +#[derive(Clone, Copy)] +struct DeviceHandle(NonNull); + +impl DeviceHandle { + fn new(raw: *const c_void) -> Result { + NonNull::new(raw as *mut c_void) + .map(DeviceHandle) + .ok_or(Error::NullPointer) + } + + fn as_ptr(&self) -> *const c_void { + self.0.as_ptr() + } + + fn is_ready(&self) -> bool { + unsafe { ffi::omi_device_is_ready(self.as_ptr()) } + } + + fn pm_action(&self, action: i32) -> i32 { + unsafe { ffi::omi_pm_device_action(self.as_ptr(), action) } + } + + fn name(&self) -> Option<&'static str> { + let ptr = unsafe { ffi::omi_device_name(self.as_ptr()) }; + NonNull::new(ptr as *mut c_char) + .and_then(|nn| unsafe { CStr::from_ptr(nn.as_ptr()) }.to_str().ok()) + } +} + +#[derive(Clone, Copy)] +struct MountHandle(NonNull); + +impl MountHandle { + fn new(raw: *mut c_void) -> Result { + NonNull::new(raw).map(MountHandle).ok_or(Error::NullPointer) + } + + fn as_ptr(&self) -> *mut c_void { + self.0.as_ptr() + } +} + +#[derive(Clone, Copy)] +struct CStrHandle(NonNull); + +impl CStrHandle { + fn new(raw: *const c_char) -> Result { + NonNull::new(raw as *mut c_char) + .map(CStrHandle) + .ok_or(Error::NullPointer) + } + + fn as_ptr(&self) -> *const c_char { + self.0.as_ptr() + } + + fn as_c_str(&self) -> &'static CStr { + unsafe { CStr::from_ptr(self.as_ptr()) } + } + + fn to_str(&self) -> Option<&'static str> { + self.as_c_str().to_str().ok() + } +} + +pub struct SdCard { + device: DeviceHandle, + enable_pin: GpioPin, + mount: MountHandle, + drive: CStrHandle, + mount_point: CStrHandle, +} + +impl SdCard { + pub fn new() -> Result { + let device = DeviceHandle::new(unsafe { ffi::omi_sd_device() })?; + let enable_pin = GpioPin::new(unsafe { ffi::omi_sd_enable_pin() })?; + let mount = MountHandle::new(unsafe { ffi::omi_sd_mount_struct() })?; + let drive = CStrHandle::new(unsafe { ffi::omi_sd_drive_name() })?; + let mount_point = CStrHandle::new(unsafe { ffi::omi_sd_mount_point() })?; + + Ok(Self { + device, + enable_pin, + mount, + drive, + mount_point, + }) + } + + pub fn device_name(&self) -> Option<&'static str> { + self.device.name() + } + + pub fn drive_name(&self) -> &'static str { + self.drive.to_str().unwrap_or("SDMMC") + } + + pub fn mount_point(&self) -> &'static str { + self.mount_point.to_str().unwrap_or("/ext") + } + + pub fn power_on(&self) -> Result<()> { + self.enable_pin.configure_output()?; + self.enable_pin.set(true)?; + let ret = self.device.pm_action(ffi::PM_DEVICE_ACTION_RESUME); + if ret < 0 && ret != EALREADY { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn power_off(&self) -> Result<()> { + let ret = self.device.pm_action(ffi::PM_DEVICE_ACTION_SUSPEND); + if ret < 0 && ret != EALREADY { + return Err(Error::from(ret)); + } + let _ = self.enable_pin.set(false); + Ok(()) + } + + pub fn disk_ioctl_raw(&self, cmd: u8, buffer: *mut c_void) -> Result<()> { + let ret = unsafe { ffi::omi_disk_access_ioctl(self.drive.as_ptr(), cmd, buffer) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn disk_init(&self) -> Result<()> { + self.disk_ioctl_raw(DISK_IOCTL_CTRL_INIT, ptr::null_mut()) + } + + pub fn disk_deinit(&self) -> Result<()> { + self.disk_ioctl_raw(DISK_IOCTL_CTRL_DEINIT, ptr::null_mut()) + } + + pub fn disk_ioctl(&self, cmd: u8, buffer: Option<&mut T>) -> Result<()> { + let ptr = buffer + .map(|b| b as *mut T as *mut c_void) + .unwrap_or(ptr::null_mut()); + self.disk_ioctl_raw(cmd, ptr) + } + + pub fn stats(&self) -> Result<(u32, u32)> { + let mut block_count: u32 = 0; + self.disk_ioctl(ffi::DISK_IOCTL_GET_SECTOR_COUNT, Some(&mut block_count))?; + let mut block_size: u32 = 0; + self.disk_ioctl(ffi::DISK_IOCTL_GET_SECTOR_SIZE, Some(&mut block_size))?; + Ok((block_count, block_size)) + } + + pub fn mount(&self) -> Result<()> { + let ret = unsafe { ffi::omi_fs_mount(self.mount.as_ptr()) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn unmount(&self) -> Result<()> { + let ret = unsafe { ffi::omi_fs_unmount(self.mount.as_ptr()) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn format_ext2(&self) -> Result<()> { + let ret = unsafe { + ffi::omi_fs_mkfs( + ffi::FS_EXT2, + self.mount.as_ptr() as usize, + ptr::null_mut(), + 0, + ) + }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } +} + +pub struct DelayableWork { + raw: NonNull, +} + +impl DelayableWork { + pub fn new(callback: ffi::omi_work_callback_t, user_data: *mut c_void) -> Result { + NonNull::new(unsafe { ffi::omi_delayable_work_create(callback, user_data) }) + .map(|raw| Self { raw }) + .ok_or(Error::NullPointer) + } + + pub fn cancel(&self) -> Result<()> { + let ret = unsafe { ffi::omi_delayable_work_cancel(self.raw.as_ptr()) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn schedule(&self, delay_ms: u32) -> Result<()> { + let ret = unsafe { ffi::omi_delayable_work_schedule(self.raw.as_ptr(), delay_ms) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn raw(&self) -> *mut c_void { + self.raw.as_ptr() + } +} + +impl Drop for DelayableWork { + fn drop(&mut self) { + unsafe { ffi::omi_delayable_work_destroy(self.raw.as_ptr()) } + } +} + +unsafe impl Send for DelayableWork {} + +pub struct BatteryHardware; + +impl BatteryHardware { + pub fn new() -> Self { + Self + } + + pub fn prepare_measurement(&self) -> Result<()> { + let ret = unsafe { ffi::omi_battery_prepare_measurement_pin() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn restore_measurement(&self) -> Result<()> { + let ret = unsafe { ffi::omi_battery_restore_measurement_pin() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn channel_setup(&self) -> Result<()> { + let ret = unsafe { ffi::omi_battery_channel_setup() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn read_samples(&self, buffer: &mut [i16], extra: u32) -> Result<()> { + let ret = + unsafe { ffi::omi_battery_perform_read(buffer.as_mut_ptr(), buffer.len(), extra) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn calibrate_offset(&self) { + unsafe { ffi::omi_saadc_trigger_offset_calibration() }; + } + + pub fn configure_pins(&self) -> Result<()> { + let ret = unsafe { ffi::omi_battery_configure_pins() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn set_charging_handler( + &self, + cb: ffi::omi_gpio_edge_cb_t, + user_data: *mut c_void, + ) -> Result<()> { + let ret = unsafe { ffi::omi_battery_set_chg_handler(cb, user_data) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn enable_charging_interrupt(&self) -> Result<()> { + let ret = unsafe { ffi::omi_battery_enable_chg_interrupt() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn disable_charging_interrupt(&self) -> Result<()> { + let ret = unsafe { ffi::omi_battery_disable_chg_interrupt() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn read_charge_pin(&self) -> Result { + let ret = unsafe { ffi::omi_battery_read_chg_pin() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(ret) + } + } +} + +pub struct Dmic { + device: DeviceHandle, +} + +impl Dmic { + pub fn new() -> Result { + let device = DeviceHandle::new(unsafe { ffi::omi_device_get(ffi::OMI_DEVICE_DMIC0) })?; + if !device.is_ready() { + return Err(Error::DeviceNotReady); + } + Ok(Self { device }) + } + + pub fn configure(&self, sample_rate: u32, channels: u8) -> Result<()> { + let ret = unsafe { ffi::omi_mic_configure(sample_rate, channels) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn trigger(&self, trigger: i32) -> Result<()> { + let ret = unsafe { ffi::omi_dmic_trigger(self.device.as_ptr(), trigger) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn read(&self, buffer: &mut *mut c_void, size: &mut u32, timeout_ms: i32) -> Result<()> { + let ret = unsafe { ffi::omi_dmic_read(self.device.as_ptr(), 0, buffer, size, timeout_ms) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } +} + +pub struct MemSlab { + raw: NonNull, +} + +impl MemSlab { + pub fn global() -> Result { + NonNull::new(unsafe { ffi::omi_mic_mem_slab() }) + .map(|raw| Self { raw }) + .ok_or(Error::NullPointer) + } + + pub fn alloc(&self, timeout_ms: u32) -> Result<*mut c_void> { + let mut block: *mut c_void = ptr::null_mut(); + let ret = unsafe { ffi::omi_mem_slab_alloc(self.raw.as_ptr(), &mut block, timeout_ms) }; + if ret < 0 { + Err(Error::from(ret)) + } else if block.is_null() { + Err(Error::NullPointer) + } else { + Ok(block) + } + } + + pub fn free(&self, block: *mut c_void) -> Result<()> { + let ret = unsafe { ffi::omi_mem_slab_free(self.raw.as_ptr(), block) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } +} + +pub struct ThreadHandle { + raw: NonNull, +} + +impl ThreadHandle { + pub fn create( + entry: unsafe extern "C" fn(*mut c_void, *mut c_void, *mut c_void), + priority: i32, + ) -> Result { + NonNull::new(unsafe { + ffi::omi_thread_create( + entry, + ptr::null_mut(), + ptr::null_mut(), + ptr::null_mut(), + priority, + ) + }) + .map(|raw| Self { raw }) + .ok_or(Error::NullPointer) + } + + pub fn start(&self) { + unsafe { ffi::omi_thread_start(self.raw.as_ptr()) }; + } + + pub fn abort(&self) { + unsafe { ffi::omi_thread_abort(self.raw.as_ptr()) }; + } +} + +impl Drop for ThreadHandle { + fn drop(&mut self) { + self.abort(); + } +} + +unsafe impl Send for ThreadHandle {} + +pub fn motor_pin() -> Result { + GpioPin::new(unsafe { ffi::omi_gpio_pin(ffi::OMI_PIN_MOTOR) }) +} + +pub fn sleep_ms(ms: u32) { + unsafe { ffi::omi_sleep_ms(ms) } +} + +pub fn busy_wait_us(us: u32) { + unsafe { ffi::omi_busy_wait_us(us) } +} + +pub struct Settings; + +impl Settings { + pub fn new() -> Self { + Settings + } + + pub fn init(&self) -> Result<()> { + let ret = unsafe { ffi::omi_settings_subsys_init() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn load(&self) -> Result<()> { + let ret = unsafe { ffi::omi_settings_load() }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn save_one(&self, name: &CStr, value: &[u8]) -> Result<()> { + let ret = unsafe { + ffi::omi_settings_save_one(name.as_ptr(), value.as_ptr() as *const c_void, value.len()) + }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn register_handler( + &self, + subtree: &CStr, + set_cb: ffi::omi_settings_set_cb, + user_data: *mut c_void, + ) -> Result<()> { + let ret = + unsafe { ffi::omi_settings_register_handler(subtree.as_ptr(), set_cb, user_data) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } + + pub fn name_steq(name: *const c_char, key: &CStr, next: &mut *const c_char) -> bool { + unsafe { ffi::omi_settings_name_steq(name, key.as_ptr(), next) } + } +} + +pub fn register_haptic_service(cb: ffi::omi_haptic_write_cb_t) -> Result<()> { + let ret = unsafe { ffi::omi_haptic_register_service(cb) }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } +} + +pub struct SpiFlash { + device: DeviceHandle, +} + +impl SpiFlash { + pub fn new() -> Result { + let device = DeviceHandle::new(unsafe { ffi::omi_device_get(ffi::OMI_DEVICE_SPI_FLASH) })?; + Ok(Self { device }) + } + + pub fn is_ready(&self) -> bool { + self.device.is_ready() + } + + pub fn suspend(&self) -> Result<()> { + let ret = self.device.pm_action(ffi::PM_DEVICE_ACTION_SUSPEND); + if ret < 0 && ret != EALREADY { + Err(Error::from(ret)) + } else { + Ok(()) + } + } +} + +pub struct AdcDevice { + device: DeviceHandle, +} + +impl AdcDevice { + pub fn new() -> Result { + let device = DeviceHandle::new(unsafe { ffi::omi_device_get(ffi::OMI_DEVICE_ADC) })?; + Ok(Self { device }) + } + + pub fn as_ptr(&self) -> *const c_void { + self.device.as_ptr() + } + + pub fn is_ready(&self) -> bool { + self.device.is_ready() + } + + pub fn ref_internal_mv(&self) -> u16 { + unsafe { ffi::omi_adc_ref_internal_mv(self.device.as_ptr()) } + } + + pub fn raw_to_millivolts(&self, gain: i32, resolution: u8, value: &mut i32) -> Result<()> { + let ret = unsafe { + ffi::omi_adc_raw_to_millivolts(self.ref_internal_mv(), gain as _, resolution, value) + }; + if ret < 0 { + Err(Error::from(ret)) + } else { + Ok(()) + } + } +} diff --git a/omi/firmware/omi/src/rust/haptic.rs b/omi/firmware/omi/src/rust/haptic.rs new file mode 100644 index 0000000000..11320b514b --- /dev/null +++ b/omi/firmware/omi/src/rust/haptic.rs @@ -0,0 +1,205 @@ +use core::ffi::c_void; +use core::ptr; +use core::sync::atomic::{AtomicBool, Ordering}; + +use spin::Mutex; + +use crate::ble::{self, HapticCharacteristic}; +use crate::hal::{self, DelayableWork, Error as HalError}; +use crate::macros::omi_macro_support::{BleAccess, CodecSpec}; +use crate::util; + +const MAX_HAPTIC_DURATION_MS: u32 = 5_000; + +static WORK_HANDLE: Mutex> = Mutex::new(None); +static SERVICE_REGISTERED: AtomicBool = AtomicBool::new(false); + +fn haptic_pin() -> Result { + hal::motor_pin() +} + +fn log_service_metadata() { + let spec = ble::haptic_service_spec(); + util::log_info_fmt(format_args!( + "Haptic BLE service '{}' (uuid: {}) advertise={}\n", + spec.name, spec.uuid, spec.advertise + )); + + for characteristic in spec.characteristics() { + util::log_info_fmt(format_args!( + " characteristic '{}' uuid={} access={:?} codec={:?}\n", + characteristic.name(), + characteristic.uuid(), + characteristic.access(), + characteristic.codec() + )); + } +} + +fn ensure_work_locked(slot: &mut Option) -> Result<(), i32> { + if slot.is_some() { + return Ok(()); + } + + unsafe extern "C" fn off_trampoline(_user_data: *mut c_void) { + haptic_off(); + util::log_info("Haptic turned off by work handler\n"); + } + + let work = + DelayableWork::new(Some(off_trampoline), ptr::null_mut()).map_err(|err| err.as_errno())?; + *slot = Some(work); + Ok(()) +} + +fn with_work(f: F) -> Result<(), i32> +where + F: FnOnce(&DelayableWork) -> Result<(), i32>, +{ + let mut guard = WORK_HANDLE.lock(); + ensure_work_locked(&mut *guard)?; + let work = guard.as_ref().expect("work initialized"); + f(work) +} + +#[no_mangle] +pub extern "C" fn haptic_init() -> i32 { + match init_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +fn configure_output(pin: &hal::GpioPin) -> Result<(), i32> { + pin.configure_output().map_err(|err| { + util::log_error("Failed to configure haptic pin\n"); + err.as_errno() + }) +} + +#[no_mangle] +pub extern "C" fn play_haptic_milli(duration_ms: u32) { + if let Err(code) = play_impl(duration_ms) { + util::log_error_fmt(format_args!("Failed to control haptic ({code})\n")); + } +} + +#[no_mangle] +pub extern "C" fn haptic_off() { + if let Ok(pin) = haptic_pin() { + if let Err(err) = pin.set(false) { + util::log_error_fmt(format_args!("Failed to disable haptic pin ({:?})\n", err)); + } + } +} + +unsafe extern "C" fn haptic_ble_callback(value: u8) { + debug_assert!(matches!(ble::haptic_command_codec(), CodecSpec::Binary)); + match value { + 1 => play_milliseconds(100), + 2 => play_milliseconds(300), + 3 => play_milliseconds(500), + _ => util::log_error_fmt(format_args!( + "Haptic write: invalid value {} on {}\n", + value, + HapticCharacteristic::command.uuid() + )), + } +} + +#[no_mangle] +pub extern "C" fn register_haptic_service() { + register_service_impl(); +} + +fn init_impl() -> Result<(), i32> { + let pin = match haptic_pin() { + Ok(pin) => pin, + Err(err) => { + util::log_error("Haptic GPIO pin not found\n"); + return Err(err.as_errno()); + } + }; + + if !pin.is_ready() { + util::log_error("Haptic GPIO device not ready\n"); + return Err(HalError::DeviceNotReady.as_errno()); + } + + with_work(|_| Ok(()))?; + util::log_info("Haptic system initialized\n"); + Ok(()) +} + +fn play_impl(duration_ms: u32) -> Result<(), i32> { + let pin = match haptic_pin() { + Ok(pin) => pin, + Err(err) => { + util::log_error_fmt(format_args!("Haptic GPIO pin not found ({:?})\n", err)); + return Err(err.as_errno()); + } + }; + + if !pin.is_ready() { + util::log_error("Haptic GPIO not ready\n"); + return Err(HalError::DeviceNotReady.as_errno()); + } + + with_work(|work| { + let _ = work.cancel(); + + if duration_ms == 0 { + pin.set(false).map_err(|err| err.as_errno())?; + util::log_info("Haptic explicitly stopped\n"); + return Ok(()); + } + + let bounded = duration_ms.min(MAX_HAPTIC_DURATION_MS); + if bounded != duration_ms { + util::log_error("Requested haptic duration exceeded max; capping\n"); + } + + configure_output(&pin)?; + pin.set(true).map_err(|err| err.as_errno())?; + work.schedule(bounded).map_err(|err| err.as_errno())?; + util::log_info("Playing haptic\n"); + Ok(()) + }) +} + +fn register_service_impl() { + if SERVICE_REGISTERED.load(Ordering::Relaxed) { + return; + } + + debug_assert!( + ble::haptic_command_access().contains(&BleAccess::Write), + "Haptic command characteristic must allow writes" + ); + + log_service_metadata(); + + if let Err(err) = hal::register_haptic_service(Some(haptic_ble_callback)) { + util::log_error_fmt(format_args!( + "Failed to register Haptic GATT service ({:?})\n", + err + )); + } else { + SERVICE_REGISTERED.store(true, Ordering::Relaxed); + util::log_info("Haptic GATT service registered\n"); + } +} + +pub fn init() -> Result<(), i32> { + init_impl() +} + +pub fn play_milliseconds(duration_ms: u32) { + if let Err(code) = play_impl(duration_ms) { + util::log_error_fmt(format_args!("Failed to control haptic ({code})\n")); + } +} + +pub fn register_service() { + register_service_impl(); +} diff --git a/omi/firmware/omi/src/rust/led.rs b/omi/firmware/omi/src/rust/led.rs new file mode 100644 index 0000000000..850a206e02 --- /dev/null +++ b/omi/firmware/omi/src/rust/led.rs @@ -0,0 +1,70 @@ +use crate::hal::{self, LedPwm}; +use crate::util; + +fn dim_ratio() -> u32 { + extern "C" { + fn app_settings_get_dim_ratio() -> u8; + } + unsafe { app_settings_get_dim_ratio() as u32 } +} + +fn apply_channel(channel: LedPwm, on: bool) { + if !channel.is_ready() { + util::log_error("LED device not ready\n"); + return; + } + + let duty = if on { dim_ratio().min(100) } else { 0 }; + if let Err(err) = channel.set_duty(duty) { + util::log_error_fmt(format_args!("Failed to set LED pulse ({:?})\n", err)); + } +} + +fn init_impl() -> Result<(), i32> { + match hal::led_start() { + Ok(()) => Ok(()), + Err(err) => { + util::log_error_fmt(format_args!("LED PWM device not ready ({:?})\n", err)); + Err(err.as_errno()) + } + } +} + +pub fn init() -> Result<(), i32> { + init_impl() +} + +pub fn red(on: bool) { + apply_channel(LedPwm::red(), on); +} + +pub fn green(on: bool) { + apply_channel(LedPwm::green(), on); +} + +pub fn blue(on: bool) { + apply_channel(LedPwm::blue(), on); +} + +#[no_mangle] +pub extern "C" fn led_start() -> i32 { + match init_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +#[no_mangle] +pub extern "C" fn set_led_red(on: bool) { + red(on); +} + +#[no_mangle] +pub extern "C" fn set_led_green(on: bool) { + green(on); +} + +#[no_mangle] +pub extern "C" fn set_led_blue(on: bool) { + blue(on); +} diff --git a/omi/firmware/omi/src/rust/led_rust_native.rs b/omi/firmware/omi/src/rust/led_rust_native.rs new file mode 100644 index 0000000000..7837f56d34 --- /dev/null +++ b/omi/firmware/omi/src/rust/led_rust_native.rs @@ -0,0 +1,127 @@ +#![allow(dead_code)] +#![allow(unused_imports)] + +//! Sketch of what the LED driver might look like if we relied on +//! Zephyr's native Rust bindings (no C shim). +//! +//! This assumes the existence of high-level safe wrappers around +//! PWM dt-specs and the settings subsystem, which the upstream +//! `embassy-zephyr` and `zephyr-rust` experiments are converging on. +//! The code below is illustrative only. + +use zephyr::drivers::pwm::{Pwm, PwmChannel, PulseWidth, Period}; +use zephyr::dt::{self, DevicePath}; +use zephyr::kernel::error::Result; +use zephyr::logging::Log; +use zephyr::settings::{Settings, SettingsHandle}; + +/// Strongly typed wrapper around the three LED channels the board exposes. +struct StatusLeds { + red: PwmChannel, + green: PwmChannel, + blue: PwmChannel, + settings: SettingsHandle, +} + +impl StatusLeds { + /// Instantiate channels straight from device tree handles. + pub fn new(settings: SettingsHandle) -> Result { + // DevicePath::label resolves to <DT_NODELABEL> entries at build time. + let red = Pwm::from_dt(DevicePath::label("led_red"))?.channel(0)?; + let green = Pwm::from_dt(DevicePath::label("led_green"))?.channel(0)?; + let blue = Pwm::from_dt(DevicePath::label("led_blue"))?.channel(0)?; + + Ok(Self { red, green, blue, settings }) + } + + /// Equivalent to `led_start` in C: ensure each PWM device is ready. + pub fn init(&self) -> Result<()> { + self.red.device().ensure_ready()?; + self.green.device().ensure_ready()?; + self.blue.device().ensure_ready()?; + Log::info!("LED PWM devices ready"); + Ok(()) + } + + fn dim_ratio(&self) -> u32 { + self.settings + .get::("omi/dim_ratio") + .ok() + .map(u32::from) + .unwrap_or(50) + .min(100) + } + + fn apply(&self, channel: &PwmChannel, on: bool) -> Result<()> { + let period = channel.period()?; + let pulse = if on { + Period::ticks(period.ticks() * self.dim_ratio() / 100) + } else { + Period::ticks(0) + }; + channel.set_pulse(PulseWidth::from_period(pulse)) + } + + pub fn set_red(&self, on: bool) { + if let Err(err) = self.apply(&self.red, on) { + Log::error!("Failed to set red LED: {:?}", err); + } + } + + pub fn set_green(&self, on: bool) { + if let Err(err) = self.apply(&self.green, on) { + Log::error!("Failed to set green LED: {:?}", err); + } + } + + pub fn set_blue(&self, on: bool) { + if let Err(err) = self.apply(&self.blue, on) { + Log::error!("Failed to set blue LED: {:?}", err); + } + } +} + +/// Example of how an async task might drive the boot animation using Embassy. +#[embassy::task] +pub async fn boot_chaser(mut leds: StatusLeds) { + use embassy_time::Timer; + + if leds.init().is_err() { + return; + } + + let delay = embassy_time::Duration::from_millis(600); + let pause = embassy_time::Duration::from_millis(200); + + leds.set_red(true); + Timer::after(delay).await; + leds.set_red(false); + Timer::after(pause).await; + + leds.set_green(true); + Timer::after(delay).await; + leds.set_green(false); + Timer::after(pause).await; + + leds.set_blue(true); + Timer::after(delay).await; + leds.set_blue(false); + Timer::after(pause).await; + + leds.set_red(true); + leds.set_green(true); + leds.set_blue(true); + Timer::after(delay).await; + + leds.set_red(false); + leds.set_green(false); + leds.set_blue(false); +} + +/// Entry point (called from your Zephyr Rust `main`). +pub fn start(settings: SettingsHandle) { + if let Ok(leds) = StatusLeds::new(settings) { + embassy::spawn!(boot_chaser(leds)).unwrap(); + } +} + diff --git a/omi/firmware/omi/src/rust/lib.rs b/omi/firmware/omi/src/rust/lib.rs new file mode 100644 index 0000000000..becbb2e574 --- /dev/null +++ b/omi/firmware/omi/src/rust/lib.rs @@ -0,0 +1,26 @@ +#![no_std] +#![allow(macro_expanded_macro_exports_accessed_by_absolute_paths)] + +pub mod app_main; +pub mod battery; +pub mod ble; +pub mod button; +pub mod codec; +pub mod ffi; +pub mod hal; +pub mod haptic; +pub mod led; +pub mod macros; +pub mod mic; +pub mod sd_card; +pub mod settings; +pub mod spi_flash; +pub mod transport; +pub mod util; + +use core::panic::PanicInfo; + +#[panic_handler] +fn panic(_info: &PanicInfo) -> ! { + loop {} +} diff --git a/omi/firmware/omi/src/rust/macros.rs b/omi/firmware/omi/src/rust/macros.rs new file mode 100644 index 0000000000..2299895571 --- /dev/null +++ b/omi/firmware/omi/src/rust/macros.rs @@ -0,0 +1,749 @@ +// Prototypical ergonomics layer for migrating legacy Zephyr firmware into Rust. +// The macros below are intentionally declarative so firmware code feels familiar to +// Python/Node engineers without compromising on safety or determinism. + +/// Support types shared by the macros below. +pub mod omi_macro_support { + use core::fmt; + use core::future::Future; + use core::marker::PhantomData; + + /// Specification produced by `omi_task!` describing an async Zephyr/Embassy task. + /// Keeping this in a dedicated type lets the macro stay const-friendly while the + /// runtime decides how to actually spawn the future. + pub struct TaskSpec + where + F: Fn() -> Fut, + Fut: Future, + { + pub name: &'static str, + pub stack_bytes: usize, + pub priority: u8, + pub future_factory: F, + phantom: PhantomData, + } + + impl TaskSpec + where + F: Fn() -> Fut, + Fut: Future, + { + pub fn new( + name: &'static str, + stack_bytes: usize, + priority: u8, + future_factory: F, + ) -> Self { + Self { + name, + stack_bytes, + priority, + future_factory, + phantom: PhantomData, + } + } + } + + /// High-level peripheral category emitted by `omi_peripheral!`. + #[derive(Debug, Clone, Copy)] + pub enum PeripheralKind { + Gpio, + Adc, + Spi, + Custom(&'static str), + } + + /// Individual pin description for declarative peripheral setup. + #[derive(Debug, Clone, Copy)] + pub struct PinSpec { + pub name: &'static str, + pub number: u8, + pub active_high: bool, + pub capabilities: &'static [&'static str], + } + + /// Aggregated peripheral metadata plus init/teardown hooks produced at compile time. + pub struct PeripheralSpec { + pub name: &'static str, + pub kind: PeripheralKind, + pub power_domain: &'static str, + pub pins: &'static [PinSpec], + pub capabilities: &'static [&'static str], + pub init: fn() -> Result<(), &'static str>, + pub teardown: fn(), + } + + /// GATT access directives used by `omi_ble_service!`. + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum BleAccess { + Read, + Write, + Notify, + Indicate, + } + + /// Serialization options for BLE characteristics so host bindings pick the right codec. + #[derive(Debug, Clone, Copy)] + pub enum CodecSpec { + Cbor { rust_type: &'static str }, + Json { rust_type: &'static str }, + Binary, + } + + /// Individual characteristic metadata. + #[derive(Debug, Clone, Copy)] + pub struct BleCharacteristicSpec { + pub name: &'static str, + pub uuid: &'static str, + pub access: &'static [BleAccess], + pub codec: CodecSpec, + } + + impl BleCharacteristicSpec { + pub fn name(&self) -> &'static str { + self.name + } + + pub fn uuid(&self) -> &'static str { + self.uuid + } + + pub fn access(&self) -> &'static [BleAccess] { + self.access + } + + pub fn codec(&self) -> CodecSpec { + self.codec + } + } + + /// Minimal registry hook to keep host bindings in sync regardless of transport backend. + pub trait BleRegistry { + fn register_service(&mut self, spec: &BleServiceSpec); + } + + /// Service description returned by `omi_ble_service!`. + #[derive(Debug)] + pub struct BleServiceSpec { + pub name: &'static str, + pub uuid: &'static str, + pub characteristics: &'static [BleCharacteristicSpec], + pub advertise: bool, + } + + impl BleServiceSpec { + pub fn register(&self, registry: &mut dyn BleRegistry) { + registry.register_service(self); + } + + pub fn characteristics(&self) -> &'static [BleCharacteristicSpec] { + self.characteristics + } + + pub fn find_characteristic(&self, name: &str) -> Option<&'static BleCharacteristicSpec> { + self.characteristics.iter().find(|spec| spec.name == name) + } + } + + /// Metadata emitted by `omi_config!` for bridging into JS/Python tooling. + #[derive(Debug, Clone, Copy)] + pub struct ConfigFieldSpec { + pub field: &'static str, + pub symbol: &'static str, + pub rust_type: &'static str, + pub default: Option<&'static str>, + } + + #[derive(Debug, Clone, Copy)] + pub struct ConfigSpec { + pub name: &'static str, + pub fields: &'static [ConfigFieldSpec], + } + + /// Helper trait for converting Kconfig symbols into strongly typed values. + pub trait KconfigValue: Sized { + fn parse(symbol: &'static str, raw: Option<&'static str>, default: Option) -> Self; + } + + impl KconfigValue for bool { + fn parse(symbol: &'static str, raw: Option<&'static str>, default: Option) -> Self { + match raw { + Some("y") | Some("Y") | Some("1") | Some("true") | Some("TRUE") => true, + Some("n") | Some("N") | Some("0") | Some("false") | Some("FALSE") => false, + Some(value) => panic!("Unexpected boolean value `{value}` for {symbol}"), + None => default.unwrap_or(false), + } + } + } + + macro_rules! impl_kconfig_fromstr { + ($($ty:ty),+ $(,)?) => { + $( + impl KconfigValue for $ty { + fn parse(symbol: &'static str, raw: Option<&'static str>, default: Option) -> Self { + match raw { + Some(value) => value.parse::<$ty>().unwrap_or_else(|err| { + panic!("Failed to parse `{value}` for {symbol}: {err:?}") + }), + None => default.unwrap_or_else(|| { + panic!("Missing Kconfig symbol {symbol} and no default provided") + }), + } + } + } + )+ + }; + } + + impl_kconfig_fromstr!(u8, u16, u32, u64, usize, i8, i16, i32, i64, isize, f32, f64); + + impl KconfigValue for &'static str { + fn parse(symbol: &'static str, raw: Option<&'static str>, default: Option) -> Self { + match raw { + Some(value) => value, + None => default.unwrap_or_else(|| { + panic!("Missing Kconfig symbol {symbol} and no default provided") + }), + } + } + } + + /// Runtime parser used by `omi_config!` so the macro can stay tiny. + pub fn kconfig_value( + symbol: &'static str, + raw: Option<&'static str>, + default: Option, + ) -> T + where + T: KconfigValue, + { + T::parse(symbol, raw, default) + } + + /// Guard utility shared by `omi_guard!` and `omi_ffi_export!`. + pub struct Guard; + + impl Guard { + #[allow(unused_variables)] + pub fn fail( + condition: &'static str, + file: &'static str, + line: u32, + breadcrumbs: &[&'static str], + error: E, + ) -> E { + #[cfg(feature = "std")] + { + eprintln!( + "omi_guard assertion failed: `{condition}` at {file}:{line} breadcrumbs={:?}", + breadcrumbs + ); + } + error + } + + #[allow(unused_variables)] + pub fn report_ffi_error(name: &'static str, file: &'static str, line: u32, detail: &str) { + #[cfg(feature = "std")] + { + eprintln!("omi_ffi_export error in {name} ({file}:{line}): {detail}"); + } + } + } + + /// Dispatch helper for `omi_ffi_export!` that converts a `Result` into an ABI-safe value. + pub fn ffi_export( + name: &'static str, + file: &'static str, + line: u32, + f: F, + error_value: R, + ) -> R + where + F: FnOnce() -> Result, + E: fmt::Debug, + { + match f() { + Ok(value) => value, + Err(err) => { + Guard::report_ffi_error(name, file, line, core::any::type_name::()); + let _ = err; // keep error in scope for post-mortem prints behind feature flags + error_value + } + } + } +} + +#[doc(hidden)] +#[macro_export] +macro_rules! __omi_slice { + // No-arg form creates a canonical empty slice so generated specs do not allocate. + () => { + &[] + }; + // Any number of expressions becomes a static slice, which works in const contexts. + ($($item:expr),+ $(,)?) => { + &[$($item),+] + }; +} + +#[macro_export] +macro_rules! omi_task { + // Primary entry point that accepts explicit field-style arguments. This mirrors how + // async tasks are configured inside Zephyr Kconfig blocks and keeps call sites readable. + ( + name: $name:ident, + stack: $stack:expr, + priority: $priority:expr, + future: $future:expr $(,)? + ) => {{ + $crate::macros::omi_macro_support::TaskSpec::new( + stringify!($name), + $stack, + $priority, + $future, + ) + }}; + // Secondary syntax sugar so `omi_task!(my_task, stack = ...)` works without repeating + // keywords. It simply rewrites the invocation into the canonical form above. + ( + $name:ident, + stack = $stack:expr, + priority = $priority:expr, + future = $future:expr $(,)? + ) => {{ + $crate::omi_task!( + name: $name, + stack: $stack, + priority: $priority, + future: $future + ) + }}; +} + +#[macro_export] +macro_rules! omi_peripheral { + // Internal helpers to normalise optional `active_high` parameters. + (@active_high $value:expr) => { $value }; + (@active_high) => { true }; + + // Declarative description of a peripheral. The macro emits a module containing + // the init/teardown shims plus a `PeripheralSpec` that higher-level code can index. + ( + name: $name:ident, + kind: $kind:ident, + power: $power:expr, + pins: [ + $( + { + name: $pin_name:ident, + number: $pin_number:expr + $(, active_high: $active_high:expr)? + $(, capabilities: [ $( $pin_cap:expr ),* $(,)? ])? + } + ),* $(,)? + ], + capabilities: [ $( $cap:expr ),* $(,)? ], + init: $init:block, + teardown: $teardown:block + ) => { + pub mod $name { + use $crate::macros::omi_macro_support::{PeripheralKind, PeripheralSpec, PinSpec}; + + pub fn init() -> Result<(), &'static str> $init + pub fn teardown() $teardown + + pub const SPEC: PeripheralSpec = PeripheralSpec { + name: stringify!($name), + kind: PeripheralKind::$kind, + power_domain: $power, + pins: &[ + $( + PinSpec { + name: stringify!($pin_name), + number: $pin_number, + active_high: $crate::omi_peripheral!(@active_high $( $active_high )?), + capabilities: $crate::__omi_slice!( $( $( $pin_cap ),* )? ), + } + ),* + ], + capabilities: $crate::__omi_slice!( $( $cap ),* ), + init: init, + teardown: teardown, + }; + } + + pub use $name::SPEC as $name; + }; +} + +#[macro_export] +macro_rules! omi_ble_service { + // Access/codec helpers keep macro bodies readable and guarantee exhaustive mapping. + (@access Read) => { $crate::macros::omi_macro_support::BleAccess::Read }; + (@access Write) => { $crate::macros::omi_macro_support::BleAccess::Write }; + (@access Notify) => { $crate::macros::omi_macro_support::BleAccess::Notify }; + (@access Indicate) => { $crate::macros::omi_macro_support::BleAccess::Indicate }; + + (@codec_kind Cbor($ty:ty)) => { + $crate::macros::omi_macro_support::CodecSpec::Cbor { rust_type: stringify!($ty) } + }; + (@codec_kind Json($ty:ty)) => { + $crate::macros::omi_macro_support::CodecSpec::Json { rust_type: stringify!($ty) } + }; + (@codec_kind Binary) => { $crate::macros::omi_macro_support::CodecSpec::Binary }; + (@codec_kind Binary()) => { $crate::macros::omi_macro_support::CodecSpec::Binary }; + (@codec_kind Raw) => { $crate::macros::omi_macro_support::CodecSpec::Binary }; + (@codec_kind Raw()) => { $crate::macros::omi_macro_support::CodecSpec::Binary }; + (@codec_kind $other:ident($($rest:tt)*)) => { + compile_error!(concat!("Unsupported codec `", stringify!($other), "`")) + }; + + ( + name: $name:ident, + uuid: $uuid:expr, + advertise: $advertise:expr, + characteristics: [] + ) => { + compile_error!("omi_ble_service! requires at least one characteristic"); + }; + + ( + name: $name:ident, + uuid: $uuid:expr, + advertise: $advertise:expr, + characteristics: [ + $( + { + name: $char_name:ident, + uuid: $char_uuid:expr, + access: [ $( $access:ident ),* $(,)? ], + codec: $codec_kind:ident $( ( $codec_ty:ty ) )? + } + ),+ $(,)? + ] + ) => { + #[allow(non_snake_case)] + pub mod $name { + use $crate::macros::omi_macro_support::{BleAccess, BleCharacteristicSpec, BleRegistry, BleServiceSpec, CodecSpec}; + + $( + #[allow(non_upper_case_globals)] + pub const $char_name: BleCharacteristicSpec = BleCharacteristicSpec { + name: stringify!($char_name), + uuid: $char_uuid, + access: $crate::__omi_slice!( $( $crate::omi_ble_service!(@access $access) ),* ), + codec: $crate::omi_ble_service!(@codec_kind $codec_kind( $( $codec_ty )? )), + }; + )* + + pub const CHARACTERISTICS: &[BleCharacteristicSpec] = $crate::__omi_slice!( $( $char_name ),* ); + + pub const SPEC: BleServiceSpec = BleServiceSpec { + name: stringify!($name), + uuid: $uuid, + characteristics: CHARACTERISTICS, + advertise: $advertise, + }; + + pub fn uuid_str() -> &'static str { + $uuid + } + + pub fn uuid_cstr() -> &'static str { + concat!($uuid, "\0") + } + + #[allow(non_camel_case_types)] + #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] + pub enum Characteristic { + $( $char_name ),* + } + + impl Characteristic { + pub fn spec(self) -> &'static BleCharacteristicSpec { + match self { + $( Self::$char_name => &$char_name, )* + } + } + + pub fn uuid(self) -> &'static str { + self.spec().uuid + } + + pub fn access(self) -> &'static [BleAccess] { + self.spec().access + } + + pub fn codec(self) -> CodecSpec { + self.spec().codec + } + + pub fn uuid_cstr(self) -> &'static str { + match self { + $( Self::$char_name => concat!($char_uuid, "\0"), )* + } + } + } + + pub fn spec() -> &'static BleServiceSpec { + &SPEC + } + + pub fn register(registry: &mut dyn BleRegistry) { + registry.register_service(&SPEC); + } + } + }; + + // Allow codec helper reuse inside generated modules without qualifying. + (@access $role:ident) => { compile_error!(concat!("Unsupported access role `", stringify!($role), "`")) }; +} + +#[macro_export] +macro_rules! omi_config { + // Struct definition macro that wires Kconfig symbols into a typed configuration bucket. + ( + $(#[$meta:meta])* struct $name:ident { + $( + $(#[$field_meta:meta])* $field:ident : $ty:ty $(= default($default:expr))? => $symbol:literal $(,)? + )* + } + ) => { + $(#[$meta])* pub struct $name { + $( + $(#[$field_meta])* pub $field: $ty, + )* + } + + impl $name { + pub fn load() -> Self { + Self { + $( + // Each field pulls from `option_env!` so the same code works in tests. + $field: $crate::omi_macro_support::kconfig_value( + $symbol, + option_env!($symbol), + $crate::omi_config!(@default $( $default )?) + ), + )* + } + } + + pub const FIELD_SPECS: &'static [$crate::omi_macro_support::ConfigFieldSpec] = &[ + $( + $crate::omi_macro_support::ConfigFieldSpec { + field: stringify!($field), + symbol: $symbol, + rust_type: stringify!($ty), + default: $crate::omi_config!(@default_str $( $default )?), + } + ),* + ]; + + pub fn spec() -> $crate::omi_macro_support::ConfigSpec { + $crate::omi_macro_support::ConfigSpec { + name: stringify!($name), + fields: Self::FIELD_SPECS, + } + } + } + + impl Default for $name { + fn default() -> Self { + Self::load() + } + } + }; + + (@default $default:expr) => { Some($default) }; + (@default) => { None }; + + (@default_str $default:expr) => { Some(stringify!($default)) }; + (@default_str) => { None }; +} + +#[macro_export] +macro_rules! omi_guard { + // Runtime guard patterned after Python's assert, but returning early with a caller-supplied error. + ($cond:expr, $err:expr $(, $breadcrumb:expr )* $(,)?) => {{ + if !$cond { + return Err($crate::omi_macro_support::Guard::fail( + stringify!($cond), + file!(), + line!(), + &[$( $breadcrumb ),*], + $err, + )); + } + }}; +} + +#[macro_export] +macro_rules! omi_ffi_export { + // Wrap extern "C" functions so they return ABI-safe values while logging rich errors. + ( + $(#[$meta:meta])* fn $name:ident ( $( $arg:ident : $ty:ty ),* $(,)? ) -> $ret:ty { + $($body:tt)* + } catch $error_value:expr + ) => { + $(#[$meta])* #[no_mangle] + pub extern "C" fn $name( $( $arg : $ty ),* ) -> $ret { + $crate::omi_macro_support::ffi_export( + stringify!($name), + file!(), + line!(), + || -> Result<$ret, _> { $($body)* }, + $error_value, + ) + } + }; +} + +#[cfg(test)] +mod tests { + use super::omi_macro_support::{BleRegistry, BleServiceSpec, PeripheralSpec}; + use super::*; + + struct FakeRegistry; + impl BleRegistry for FakeRegistry { + fn register_service(&mut self, spec: &BleServiceSpec) { + assert_eq!(spec.name, "demo_audio"); + } + } + + #[test] + fn task_macro_builds_spec() { + let spec = omi_task!( + name: blink_task, + stack: 1024, + priority: 3, + future: || async { Ok::<(), &'static str>(()) } + ); + + assert_eq!(spec.name, "blink_task"); + assert_eq!(spec.stack_bytes, 1024); + assert_eq!(spec.priority, 3); + + // ensure future factory is callable without pinning infrastructure + let _future = (spec.future_factory)(); + } + + #[test] + fn peripheral_macro_registers_spec() { + omi_peripheral!( + name: demo_led, + kind: Gpio, + power: "vdd_main", + pins: [ + { name: red, number: 17, active_high: true, capabilities: ["pwm"] }, + { name: green, number: 18, capabilities: ["pwm"] }, + { name: blue, number: 19, capabilities: ["pwm"] } + ], + capabilities: ["status-led"], + init: { Ok(()) }, + teardown: { {} } + ); + + fn assert_spec(spec: &PeripheralSpec) { + assert_eq!(spec.name, "demo_led"); + assert_eq!(spec.power_domain, "vdd_main"); + assert_eq!(spec.pins.len(), 3); + assert!((spec.init)().is_ok()); + (spec.teardown)(); + } + + assert_spec(&demo_led); + } + + #[test] + fn ble_service_macro_creates_spec() { + omi_ble_service!( + name: demo_audio, + uuid: "814b9b7c-25fd-4acd-8604-d28877beee6d", + advertise: true, + characteristics: [ + { + name: audio_data, + uuid: "814b9b7c-25fd-4acd-8604-d28877beee6e", + access: [Read, Notify], + codec: Cbor(u8) + }, + { + name: codec_format, + uuid: "814b9b7c-25fd-4acd-8604-d28877beee6f", + access: [Read], + codec: Json(u16) + } + ] + ); + + assert_eq!(demo_audio::SPEC.characteristics().len(), 2); + assert!(demo_audio::SPEC.advertise); + + let audio_char = demo_audio::Characteristic::audio_data.spec(); + assert_eq!(audio_char.name, "audio_data"); + assert_eq!(audio_char.uuid, "814b9b7c-25fd-4acd-8604-d28877beee6e"); + + assert!(demo_audio::SPEC + .find_characteristic("codec_format") + .is_some()); + + let mut registry = FakeRegistry; + demo_audio::register(&mut registry); + } + + omi_config!( + #[derive(Debug)] + struct DemoConfig { + sample_rate_hz: u32 = default(44100) => "CONFIG_OMI_SAMPLE_RATE", + enable_leds: bool = default(true) => "CONFIG_OMI_ENABLE_LEDS", + } + ); + + #[test] + fn config_macro_loads_defaults() { + let cfg = DemoConfig::load(); + assert_eq!(cfg.sample_rate_hz, 44100); + assert!(cfg.enable_leds); + + let spec = DemoConfig::spec(); + assert_eq!(spec.name, "DemoConfig"); + assert_eq!(spec.fields.len(), 2); + let enable_field = &spec.fields[1]; + assert_eq!(enable_field.field, "enable_leds"); + assert_eq!(enable_field.symbol, "CONFIG_OMI_ENABLE_LEDS"); + assert_eq!(enable_field.default, Some("true")); + + let defaults = DemoConfig::default(); + assert_eq!(defaults.sample_rate_hz, 44100); + } + + #[test] + fn guard_macro_short_circuits() { + fn ensure_under_limit(value: u8) -> Result<(), &'static str> { + omi_guard!(value < 10, "too-large", "ensure_under_limit", "value"); + Ok(()) + } + + assert!(ensure_under_limit(5).is_ok()); + assert!(matches!(ensure_under_limit(42), Err("too-large"))); + } + + omi_ffi_export!( + fn ffi_checked_add(a: i32, b: i32) -> i32 { + if let Some(sum) = a.checked_add(b) { + Ok(sum) + } else { + Err("overflow") + } + }, + catch - 1 + ); + + #[test] + fn ffi_macro_wraps_result() { + assert_eq!(ffi_checked_add(2, 3), 5); + assert_eq!(ffi_checked_add(i32::MAX, 1), -1); + } +} diff --git a/omi/firmware/omi/src/rust/mic.rs b/omi/firmware/omi/src/rust/mic.rs new file mode 100644 index 0000000000..2283bdf09b --- /dev/null +++ b/omi/firmware/omi/src/rust/mic.rs @@ -0,0 +1,184 @@ +use core::ffi::c_void; +use core::sync::atomic::{AtomicBool, Ordering}; + +use spin::Mutex; + +use crate::hal::{self, Dmic, MemSlab, ThreadHandle}; +use crate::util; + +const MAX_SAMPLE_RATE: u32 = 16_000; +const READ_TIMEOUT_MS: i32 = 1_000; +const MIC_THREAD_PRIORITY: i32 = 5; + +static MIC_RUNNING: AtomicBool = AtomicBool::new(false); +static THREAD_HANDLE: Mutex> = Mutex::new(None); +static CALLBACK: Mutex> = Mutex::new(None); + +fn get_dmic() -> Result { + Dmic::new().map_err(|err| err.as_errno()) +} + +fn spawn_thread() -> Result<(), i32> { + let handle = + ThreadHandle::create(mic_thread, MIC_THREAD_PRIORITY).map_err(|err| err.as_errno())?; + handle.start(); + *THREAD_HANDLE.lock() = Some(handle); + Ok(()) +} + +unsafe extern "C" fn mic_thread(_p1: *mut c_void, _p2: *mut c_void, _p3: *mut c_void) { + let dmic = match get_dmic() { + Ok(d) => d, + Err(err) => { + util::log_error_fmt(format_args!("DMIC not ready ({err})\n")); + MIC_RUNNING.store(false, Ordering::Relaxed); + return; + } + }; + + let slab = match MemSlab::global() { + Ok(s) => s, + Err(err) => { + util::log_error_fmt(format_args!("Failed to access mem slab ({:?})\n", err)); + MIC_RUNNING.store(false, Ordering::Relaxed); + return; + } + }; + + while MIC_RUNNING.load(Ordering::Relaxed) { + let mut buffer: *mut c_void = core::ptr::null_mut(); + let mut size: u32 = 0; + if let Err(err) = dmic.read(&mut buffer, &mut size, READ_TIMEOUT_MS) { + util::log_error_fmt(format_args!("DMIC read failed ({:?})\n", err)); + continue; + } + + if buffer.is_null() { + continue; + } + + if let Some(cb) = *CALLBACK.lock() { + cb(buffer as *mut i16); + } + + if let Err(err) = slab.free(buffer) { + util::log_error_fmt(format_args!("Failed to free DMIC buffer ({:?})\n", err)); + } + } +} + +#[no_mangle] +pub extern "C" fn mic_start() -> i32 { + match start_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +fn start_impl() -> Result<(), i32> { + let dmic = match get_dmic() { + Ok(d) => d, + Err(err) => return Err(err), + }; + + if let Err(err) = dmic.configure(MAX_SAMPLE_RATE, 1) { + util::log_error_fmt(format_args!("Failed to configure DMIC ({:?})\n", err)); + return Err(err.as_errno()); + } + + if let Err(err) = dmic.trigger(hal::DMIC_TRIGGER_START) { + util::log_error_fmt(format_args!("START trigger failed ({:?})\n", err)); + return Err(err.as_errno()); + } + + MIC_RUNNING.store(true, Ordering::Relaxed); + if let Err(err) = spawn_thread() { + MIC_RUNNING.store(false, Ordering::Relaxed); + let _ = dmic.trigger(hal::DMIC_TRIGGER_STOP); + return Err(err); + } + + util::log_info("Microphone started\n"); + Ok(()) +} + +#[no_mangle] +pub extern "C" fn set_mic_callback(callback: unsafe extern "C" fn(*mut i16)) { + set_callback_impl(callback); +} + +fn set_callback_impl(callback: unsafe extern "C" fn(*mut i16)) { + if (callback as usize) == 0 { + *CALLBACK.lock() = None; + } else { + *CALLBACK.lock() = Some(callback); + } +} + +#[no_mangle] +pub extern "C" fn mic_off() { + off_impl(); +} + +fn off_impl() { + if MIC_RUNNING.swap(false, Ordering::Relaxed) { + if let Ok(dmic) = get_dmic() { + let _ = dmic.trigger(hal::DMIC_TRIGGER_STOP); + } + + if let Some(handle) = THREAD_HANDLE.lock().take() { + drop(handle); + } + + util::log_info("Microphone stopped\n"); + } +} + +#[no_mangle] +pub extern "C" fn mic_on() { + on_impl(); +} + +fn on_impl() { + if MIC_RUNNING.load(Ordering::Relaxed) { + return; + } + + let dmic = match get_dmic() { + Ok(d) => d, + Err(err) => { + util::log_error_fmt(format_args!("DMIC not ready ({err})\n")); + return; + } + }; + + if let Err(err) = dmic.trigger(hal::DMIC_TRIGGER_START) { + util::log_error_fmt(format_args!("START trigger failed ({:?})\n", err)); + return; + } + + MIC_RUNNING.store(true, Ordering::Relaxed); + if let Err(err) = spawn_thread() { + MIC_RUNNING.store(false, Ordering::Relaxed); + let _ = dmic.trigger(hal::DMIC_TRIGGER_STOP); + util::log_error_fmt(format_args!("Failed to restart microphone ({err})\n")); + } else { + util::log_info("Microphone restarted\n"); + } +} + +pub fn start() -> Result<(), i32> { + start_impl() +} + +pub fn set_callback(callback: unsafe extern "C" fn(*mut i16)) { + set_callback_impl(callback); +} + +pub fn off() { + off_impl(); +} + +pub fn on() { + on_impl(); +} diff --git a/omi/firmware/omi/src/rust/sd_card.rs b/omi/firmware/omi/src/rust/sd_card.rs new file mode 100644 index 0000000000..4b3d810aab --- /dev/null +++ b/omi/firmware/omi/src/rust/sd_card.rs @@ -0,0 +1,95 @@ +use core::sync::atomic::{AtomicBool, Ordering}; + +use crate::hal::{self, Error as HalError}; +use crate::util; + +static IS_MOUNTED: AtomicBool = AtomicBool::new(false); + +fn sd_card() -> Result { + hal::SdCard::new().map_err(|err| err.as_errno()) +} + +fn log_stats(sd: &hal::SdCard) { + if let Ok((count, size)) = sd.stats() { + util::log_info_fmt(format_args!("Block count {count}")); + util::log_info_fmt(format_args!("Sector size {size}")); + let memory_mb = ((count as u64) * (size as u64)) >> 20; + util::log_info_fmt(format_args!("Memory Size(MB) {memory_mb}")); + } +} + +fn mount_sd_card(sd: &hal::SdCard) -> Result<(), HalError> { + sd.power_on()?; + sd.disk_init()?; + log_stats(sd); + let _ = sd.disk_deinit(); + + match sd.mount() { + Ok(()) => { + IS_MOUNTED.store(true, Ordering::Relaxed); + util::log_info("Disk mounted.\n"); + Ok(()) + } + Err(_) => { + util::log_info("File system not found, creating file system...\n"); + sd.format_ext2()?; + sd.mount()?; + IS_MOUNTED.store(true, Ordering::Relaxed); + util::log_info("Disk mounted.\n"); + Ok(()) + } + } +} + +fn unmount_sd_card(sd: &hal::SdCard) -> Result<(), HalError> { + if IS_MOUNTED.load(Ordering::Relaxed) { + sd.unmount()?; + IS_MOUNTED.store(false, Ordering::Relaxed); + util::log_info("Disk unmounted.\n"); + } + sd.power_off() +} + +#[no_mangle] +pub extern "C" fn app_sd_init() -> i32 { + match init_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +#[no_mangle] +pub extern "C" fn app_sd_off() -> i32 { + match power_off_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +fn init_impl() -> Result<(), i32> { + match sd_card() { + Ok(sd) => { + if let Some(name) = sd.device_name() { + util::log_info_fmt(format_args!("SD card module initialized (Device: {name})")); + } else { + util::log_info("SD card module initialized\n"); + } + Ok(()) + } + Err(err) => Err(err), + } +} + +fn power_off_impl() -> Result<(), i32> { + let sd = sd_card()?; + let _ = mount_sd_card(&sd); + unmount_sd_card(&sd).map_err(|err| err.as_errno()) +} + +pub fn init() -> Result<(), i32> { + init_impl() +} + +pub fn power_off() -> Result<(), i32> { + power_off_impl() +} diff --git a/omi/firmware/omi/src/rust/settings.rs b/omi/firmware/omi/src/rust/settings.rs new file mode 100644 index 0000000000..2e2a295667 --- /dev/null +++ b/omi/firmware/omi/src/rust/settings.rs @@ -0,0 +1,159 @@ +use core::ffi::{c_char, c_int, c_void, CStr}; +use core::ptr; +use core::sync::atomic::{AtomicBool, AtomicU8, Ordering}; + +use crate::ffi; +use crate::hal::Settings; +use crate::util; + +const DEFAULT_DIM_RATIO: u8 = 50; +const EINVAL: i32 = -22; +const ENOENT: i32 = -2; + +static DIM_RATIO: AtomicU8 = AtomicU8::new(DEFAULT_DIM_RATIO); +static HANDLER_REGISTERED: AtomicBool = AtomicBool::new(false); + +const SUBTREE_BYTES: &[u8] = b"omi\0"; +const DIM_KEY_BYTES: &[u8] = b"dim_ratio\0"; +const DIM_PATH_BYTES: &[u8] = b"omi/dim_ratio\0"; + +fn subtree() -> &'static CStr { + unsafe { CStr::from_bytes_with_nul_unchecked(SUBTREE_BYTES) } +} + +fn dim_key() -> &'static CStr { + unsafe { CStr::from_bytes_with_nul_unchecked(DIM_KEY_BYTES) } +} + +fn dim_path() -> &'static CStr { + unsafe { CStr::from_bytes_with_nul_unchecked(DIM_PATH_BYTES) } +} + +fn settings() -> Settings { + Settings::new() +} + +fn ensure_handler_registered() -> i32 { + if HANDLER_REGISTERED.load(Ordering::Relaxed) { + return 0; + } + + let err = settings() + .register_handler(subtree(), settings_set_cb, ptr::null_mut()) + .map_err(|err| err.as_errno()); + + match err { + Ok(()) => { + HANDLER_REGISTERED.store(true, Ordering::Relaxed); + 0 + } + Err(code) => { + util::log_error_fmt(format_args!( + "Failed to register settings handler ({code})\n" + )); + code + } + } +} + +unsafe extern "C" fn settings_set_cb( + name: *const c_char, + len: usize, + read_cb: ffi::settings_read_cb, + cb_arg: *mut c_void, + _user_data: *mut c_void, +) -> c_int { + if name.is_null() { + return EINVAL; + } + + let mut next: *const c_char = ptr::null(); + let matches = Settings::name_steq(name, dim_key(), &mut next); + + if matches && next.is_null() { + if len != core::mem::size_of::() { + return EINVAL; + } + + let mut value: u8 = DIM_RATIO.load(Ordering::Relaxed); + let rc = read_cb(cb_arg, &mut value as *mut _ as *mut c_void, len); + if rc >= 0 { + DIM_RATIO.store(value, Ordering::Relaxed); + util::log_info("Loaded dim_ratio\n"); + return 0; + } + return rc; + } + + ENOENT +} + +#[no_mangle] +pub extern "C" fn app_settings_init() -> i32 { + match init_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +#[no_mangle] +pub extern "C" fn app_settings_save_dim_ratio(new_ratio: u8) -> i32 { + match save_dim_ratio_impl(new_ratio) { + Ok(()) => 0, + Err(err) => err, + } +} + +#[no_mangle] +pub extern "C" fn app_settings_get_dim_ratio() -> u8 { + DIM_RATIO.load(Ordering::Relaxed) +} + +fn init_impl() -> Result<(), i32> { + if let Err(err) = settings().init() { + util::log_error_fmt(format_args!( + "Failed to initialize settings subsystem ({:?})\n", + err + )); + return Err(err.as_errno()); + } + + let err = ensure_handler_registered(); + if err != 0 { + return Err(err); + } + + match settings().load() { + Ok(()) => { + util::log_info("Settings initialized\n"); + Ok(()) + } + Err(err) => { + util::log_error_fmt(format_args!("Failed to load settings ({:?})\n", err)); + Err(err.as_errno()) + } + } +} + +fn save_dim_ratio_impl(new_ratio: u8) -> Result<(), i32> { + DIM_RATIO.store(new_ratio, Ordering::Relaxed); + let bytes = [new_ratio]; + match settings().save_one(dim_path(), &bytes) { + Ok(()) => { + util::log_info("Saved dim_ratio\n"); + Ok(()) + } + Err(err) => { + util::log_error_fmt(format_args!("Failed to save dim_ratio ({:?})\n", err)); + Err(err.as_errno()) + } + } +} + +pub fn init() -> Result<(), i32> { + init_impl() +} + +pub fn save_dim_ratio(new_ratio: u8) -> Result<(), i32> { + save_dim_ratio_impl(new_ratio) +} diff --git a/omi/firmware/omi/src/rust/spi_flash.rs b/omi/firmware/omi/src/rust/spi_flash.rs new file mode 100644 index 0000000000..ba6ff13c76 --- /dev/null +++ b/omi/firmware/omi/src/rust/spi_flash.rs @@ -0,0 +1,73 @@ +use crate::hal::{Error as HalError, SpiFlash}; +use crate::util; + +#[no_mangle] +pub extern "C" fn flash_init() -> i32 { + match init_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +#[no_mangle] +pub extern "C" fn flash_off() -> i32 { + match suspend_impl() { + Ok(()) => 0, + Err(err) => err, + } +} + +fn init_impl() -> Result<(), i32> { + match SpiFlash::new() { + Ok(flash) => { + if !flash.is_ready() { + util::log_error("SPI Flash device not ready\n"); + return Err(HalError::DeviceNotReady.as_errno()); + } + util::log_info("SPI Flash control module initialized\n"); + Ok(()) + } + Err(err) => { + util::log_error_fmt(format_args!("SPI Flash device handle error ({:?})\n", err)); + Err(err.as_errno()) + } + } +} + +fn suspend_impl() -> Result<(), i32> { + match SpiFlash::new() { + Ok(flash) => match flash.suspend() { + Ok(()) => { + util::log_info("SPI Flash device suspended successfully\n"); + Ok(()) + } + Err(err) => { + if err.as_errno() == -114 { + util::log_info("SPI Flash device already suspended\n"); + Ok(()) + } else { + util::log_error_fmt(format_args!( + "Failed to suspend SPI Flash device ({:?})\n", + err + )); + Err(err.as_errno()) + } + } + }, + Err(err) => { + util::log_error_fmt(format_args!( + "SPI Flash device handle is null ({:?})\n", + err + )); + Err(err.as_errno()) + } + } +} + +pub fn init() -> Result<(), i32> { + init_impl() +} + +pub fn suspend() -> Result<(), i32> { + suspend_impl() +} diff --git a/omi/firmware/omi/src/rust/transport.rs b/omi/firmware/omi/src/rust/transport.rs new file mode 100644 index 0000000000..2bf1157b9d --- /dev/null +++ b/omi/firmware/omi/src/rust/transport.rs @@ -0,0 +1,17 @@ +use crate::util; + +mod ffi { + extern "C" { + pub fn transport_start() -> i32; + } +} + +pub fn start() -> Result<(), i32> { + let rc = unsafe { ffi::transport_start() }; + if rc < 0 { + util::log_error_fmt(format_args!("Failed to start transport ({rc})")); + Err(rc) + } else { + Ok(()) + } +} diff --git a/omi/firmware/omi/src/rust/util.rs b/omi/firmware/omi/src/rust/util.rs new file mode 100644 index 0000000000..675be64b9b --- /dev/null +++ b/omi/firmware/omi/src/rust/util.rs @@ -0,0 +1,79 @@ +use core::ffi::c_char; +use core::fmt::{self, Write}; + +use crate::ffi; + +pub fn log_info(message: &str) { + log(message, true); +} + +pub fn log_error(message: &str) { + log(message, false); +} + +fn log(message: &str, info: bool) { + let mut buffer = [0u8; 128]; + let bytes = message.as_bytes(); + let len = bytes.len().min(buffer.len() - 1); + buffer[..len].copy_from_slice(&bytes[..len]); + buffer[len] = 0; + unsafe { + let ptr = buffer.as_ptr() as *const c_char; + if info { + ffi::omi_log_inf(ptr); + } else { + ffi::omi_log_err(ptr); + } + } +} + +struct LogBuffer { + buf: [u8; 160], + len: usize, +} + +impl LogBuffer { + fn new() -> Self { + Self { + buf: [0; 160], + len: 0, + } + } + + fn as_c_str(&mut self) -> *const c_char { + self.buf[self.len.min(self.buf.len() - 1)] = 0; + self.buf.as_ptr() as *const c_char + } +} + +impl Write for LogBuffer { + fn write_str(&mut self, s: &str) -> fmt::Result { + let bytes = s.as_bytes(); + let space = self.buf.len().saturating_sub(self.len + 1); + let to_copy = bytes.len().min(space); + self.buf[self.len..self.len + to_copy].copy_from_slice(&bytes[..to_copy]); + self.len += to_copy; + Ok(()) + } +} + +fn log_fmt(args: fmt::Arguments, info: bool) { + let mut buf = LogBuffer::new(); + let _ = buf.write_fmt(args); + unsafe { + let ptr = buf.as_c_str(); + if info { + ffi::omi_log_inf(ptr); + } else { + ffi::omi_log_err(ptr); + } + } +} + +pub fn log_info_fmt(args: fmt::Arguments) { + log_fmt(args, true); +} + +pub fn log_error_fmt(args: fmt::Arguments) { + log_fmt(args, false); +}