diff --git a/drivers/sensor/bosch/bmi08x/CMakeLists.txt b/drivers/sensor/bosch/bmi08x/CMakeLists.txt index 0282cc73d2051..5899fedf05971 100644 --- a/drivers/sensor/bosch/bmi08x/CMakeLists.txt +++ b/drivers/sensor/bosch/bmi08x/CMakeLists.txt @@ -7,3 +7,12 @@ zephyr_library_sources(bmi08x_gyro.c) zephyr_library_sources(bmi08x.c) zephyr_library_sources_ifdef(CONFIG_BMI08X_ACCEL_TRIGGER bmi08x_accel_trigger.c) zephyr_library_sources_ifdef(CONFIG_BMI08X_GYRO_TRIGGER bmi08x_gyro_trigger.c) +zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API + bmi08x_bus.c + bmi08x_gyro_async.c + bmi08x_gyro_decoder.c + bmi08x_accel_async.c + bmi08x_accel_decoder.c +) +zephyr_library_sources_ifdef(CONFIG_BMI08X_GYRO_STREAM bmi08x_gyro_stream.c) +zephyr_library_sources_ifdef(CONFIG_BMI08X_ACCEL_STREAM bmi08x_accel_stream.c) diff --git a/drivers/sensor/bosch/bmi08x/Kconfig b/drivers/sensor/bosch/bmi08x/Kconfig index cbe40bf5c26e5..fbc5b042d614b 100644 --- a/drivers/sensor/bosch/bmi08x/Kconfig +++ b/drivers/sensor/bosch/bmi08x/Kconfig @@ -11,6 +11,8 @@ menuconfig BMI08X || $(dt_compat_on_bus,$(DT_COMPAT_BOSCH_BMI08X_GYRO),i2c) select SPI if $(dt_compat_on_bus,$(DT_COMPAT_BOSCH_BMI08X_ACCEL),spi) \ || $(dt_compat_on_bus,$(DT_COMPAT_BOSCH_BMI08X_GYRO),spi) + select SPI_RTIO if SPI + select I2C_RTIO if I2C help Enable Bosch BMI08X inertial measurement unit that provides acceleration and angular rate measurements. @@ -19,6 +21,7 @@ if BMI08X choice BMI08X_ACCEL_TRIGGER_MODE prompt "Accelerometer trigger mode" + default BMI08X_ACCEL_TRIGGER_NONE default BMI08X_ACCEL_TRIGGER_GLOBAL_THREAD help Specify the type of triggering to be used by the driver. @@ -39,6 +42,13 @@ config BMI08X_ACCEL_TRIGGER_OWN_THREAD select BMI08X_ACCEL_TRIGGER endchoice +config BMI08X_ACCEL_STREAM + bool "Accelerometer Streaming Mode (FIFO)" + depends on !BMI08X_ACCEL_TRIGGER + depends on GPIO + depends on $(dt_compat_any_has_prop,$(DT_COMPAT_BOSCH_BMI08X_ACCEL),int-gpios) + + config BMI08X_ACCEL_TRIGGER bool @@ -75,6 +85,12 @@ config BMI08X_GYRO_TRIGGER_OWN_THREAD select BMI08X_GYRO_TRIGGER endchoice +config BMI08X_GYRO_STREAM + bool "Gyroscope Streaming Mode (FIFO)" + depends on !BMI08X_GYRO_TRIGGER + depends on GPIO + depends on $(dt_compat_any_has_prop,$(DT_COMPAT_BOSCH_BMI08X_GYRO),int-gpios) + config BMI08X_GYRO_TRIGGER bool diff --git a/drivers/sensor/bosch/bmi08x/bmi08x.h b/drivers/sensor/bosch/bmi08x/bmi08x.h index 83926126e4844..5aa4dd7a5779c 100644 --- a/drivers/sensor/bosch/bmi08x/bmi08x.h +++ b/drivers/sensor/bosch/bmi08x/bmi08x.h @@ -14,6 +14,8 @@ #include #include +#include "bmi08x_bus.h" + /* Accel Chip Id register */ #define BMI08X_REG_ACCEL_CHIP_ID 0x00 @@ -65,6 +67,15 @@ /* Sensor temperature LSB data register */ #define BMI08X_REG_TEMP_LSB 0x23 +/* Accel FIFO Length (low byte) */ +#define BMI08X_REG_ACCEL_FIFO_LEN_0 0x24 + +/* Accel FIFO Length (high byte) */ +#define BMI08X_REG_ACCEL_FIFO_LEN_1 0x25 + +/* Accel FIFO Data register */ +#define BMI08X_REG_ACCEL_FIFO_DATA 0x26 + /* Accel general purpose register 4*/ #define BMI08X_REG_ACCEL_GP_4 0x27 @@ -77,6 +88,18 @@ /* Accel range setting register */ #define BMI08X_REG_ACCEL_RANGE 0x41 +/* Accel FIFO Watermark (low byte) */ +#define BMI08X_REG_ACCEL_FIFO_WTM_0 0x46 + +/* Accel FIFO Watermark (high byte) */ +#define BMI08X_REG_ACCEL_FIFO_WTM_1 0x47 + +/* Accel FIFO Config (FIFO mode) */ +#define BMI08X_REG_ACCEL_FIFO_CONFIG_0 0x48 + +/* Accel FIFO Config (Interrupt enabling) */ +#define BMI08X_REG_ACCEL_FIFO_CONFIG_1 0x49 + /* Accel Interrupt pin 1 configuration register */ #define BMI08X_REG_ACCEL_INT1_IO_CONF 0x53 @@ -262,6 +285,9 @@ /* Gyro Interrupt status register */ #define BMI08X_REG_GYRO_INT_STAT_1 0x0A +/* FIFO Status register (Overrun and Frame counter) */ +#define BMI08X_REG_FIFO_STATUS 0x0E + /* Gyro Range register */ #define BMI08X_REG_GYRO_RANGE 0x0F @@ -283,9 +309,21 @@ /* Gyro Interrupt Map register */ #define BMI08X_REG_GYRO_INT3_INT4_IO_MAP 0x18 +/* FIFO Watermark enable */ +#define BMI08X_REG_GYRO_FIFO_WM_EN 0x1E + /* Gyro Self test register */ #define BMI08X_REG_GYRO_SELF_TEST 0x3C +/* FIFO Config register (FIFO Watermark) */ +#define BMI08X_REG_GYRO_FIFO_CONFIG_0 0x3D + +/* FIFO Config register (FIFO Mode) */ +#define BMI08X_REG_GYRO_FIFO_CONFIG_1 0x3E + +/* FIFO Data register */ +#define BMI08X_REG_GYRO_FIFO_DATA 0x3F + /* Gyro unique chip identifier */ #define BMI08X_GYRO_CHIP_ID 0x0F @@ -469,8 +507,9 @@ struct bmi08x_gyro_bus_io { struct bmi08x_accel_config { union bmi08x_bus bus; const struct bmi08x_accel_bus_io *api; -#if defined(CONFIG_BMI08X_ACCEL_TRIGGER) struct gpio_dt_spec int_gpio; +#if defined(CONFIG_SENSOR_ASYNC_API) + struct bmi08x_rtio_bus rtio_bus; #endif #if defined(CONFIG_BMI08X_ACCEL_TRIGGER) || BMI08X_ACCEL_ANY_INST_HAS_DATA_SYNC uint8_t int1_map; @@ -488,8 +527,9 @@ struct bmi08x_accel_config { struct bmi08x_gyro_config { union bmi08x_bus bus; const struct bmi08x_gyro_bus_io *api; -#if defined(CONFIG_BMI08X_GYRO_TRIGGER) struct gpio_dt_spec int_gpio; +#if defined(CONFIG_SENSOR_ASYNC_API) + struct bmi08x_rtio_bus rtio_bus; #endif #if defined(CONFIG_BMI08X_GYRO_TRIGGER) || BMI08X_GYRO_ANY_INST_HAS_DATA_SYNC uint8_t int3_4_map; @@ -500,11 +540,20 @@ struct bmi08x_gyro_config { }; struct bmi08x_accel_data { -#if defined(CONFIG_BMI08X_ACCEL_TRIGGER) + const struct device *dev; +#if defined(CONFIG_BMI08X_ACCEL_TRIGGER) || defined(CONFIG_SENSOR_ASYNC_API) struct gpio_callback gpio_cb; #endif uint16_t acc_sample[3]; uint16_t scale; /* micro m/s^2/lsb */ + uint16_t range; +#if defined(CONFIG_SENSOR_ASYNC_API) + struct { + struct rtio_iodev_sqe *iodev_sqe; + atomic_t state; + uint8_t fifo_wm; + } stream; +#endif #if defined(CONFIG_BMI08X_ACCEL_TRIGGER_OWN_THREAD) K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_BMI08X_ACCEL_THREAD_STACK_SIZE); @@ -512,7 +561,6 @@ struct bmi08x_accel_data { struct k_sem sem; #elif defined(CONFIG_BMI08X_ACCEL_TRIGGER_GLOBAL_THREAD) struct k_work work; - const struct device *dev; #endif #ifdef CONFIG_BMI08X_ACCEL_TRIGGER @@ -523,11 +571,20 @@ struct bmi08x_accel_data { }; struct bmi08x_gyro_data { -#if defined(CONFIG_BMI08X_GYRO_TRIGGER) + const struct device *dev; +#if defined(CONFIG_BMI08X_GYRO_TRIGGER) || defined(CONFIG_SENSOR_ASYNC_API) struct gpio_callback gpio_cb; #endif uint16_t gyr_sample[3]; uint16_t scale; /* micro radians/s/lsb */ + uint16_t range; +#if defined(CONFIG_SENSOR_ASYNC_API) + struct { + struct rtio_iodev_sqe *iodev_sqe; + atomic_t state; + uint8_t fifo_wm; + } stream; +#endif #if defined(CONFIG_BMI08X_GYRO_TRIGGER_OWN_THREAD) K_KERNEL_STACK_MEMBER(thread_stack, CONFIG_BMI08X_GYRO_THREAD_STACK_SIZE); @@ -535,7 +592,6 @@ struct bmi08x_gyro_data { struct k_sem sem; #elif defined(CONFIG_BMI08X_GYRO_TRIGGER_GLOBAL_THREAD) struct k_work work; - const struct device *dev; #endif #ifdef CONFIG_BMI08X_GYRO_TRIGGER @@ -544,6 +600,62 @@ struct bmi08x_gyro_data { #endif /* CONFIG_BMI08X_GYRO_TRIGGER */ }; +struct bmi08x_accel_frame { + uint8_t header; + union { + struct { + uint16_t payload[3]; + } accel; + struct { + uint8_t skipped_frames; + } skip; + struct { + uint8_t time[3]; + } sensortime; + struct { + uint8_t change; + } fifo_config; + }; +} __packed; + +struct bmi08x_gyro_frame { + uint16_t payload[3]; +} __packed; + +struct bmi08x_accel_encoded_data { + struct { + uint64_t timestamp; + uint16_t range; + uint8_t chip_id; + bool has_accel; + bool is_streaming; + uint16_t fifo_len; + uint8_t sample_count; + uint16_t buf_len; + } header; + union { + uint16_t payload[3]; + uint8_t fifo[0]; /* Left as bytes since it can contain multiple frames */ + }; +}; + +struct bmi08x_gyro_encoded_data { + struct { + uint64_t timestamp; + uint16_t range; + bool has_gyro; + bool is_streaming; + uint8_t int_status; + uint8_t fifo_status; + uint8_t sample_count; + } header; + union { + struct bmi08x_gyro_frame frame; + struct bmi08x_gyro_frame fifo[0]; + } __packed; +}; + + /* common functions for accel and gyro */ int bmi08x_freq_to_odr_val(uint16_t freq_int, uint16_t freq_milli); int32_t bmi08x_range_to_reg_val(uint16_t range, const struct bmi08x_range *range_map, diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_accel.c b/drivers/sensor/bosch/bmi08x/bmi08x_accel.c index 6053638b0d4c8..e8f0eafa7141e 100644 --- a/drivers/sensor/bosch/bmi08x/bmi08x_accel.c +++ b/drivers/sensor/bosch/bmi08x/bmi08x_accel.c @@ -6,16 +6,21 @@ */ #include +#include #include #include #include #include #include #include +#include #define DT_DRV_COMPAT bosch_bmi08x_accel #include "bmi08x.h" #include "bmi08x_config_file.h" +#include "bmi08x_accel_async.h" +#include "bmi08x_accel_stream.h" +#include "bmi08x_accel_decoder.h" LOG_MODULE_REGISTER(BMI08X_ACCEL, CONFIG_SENSOR_LOG_LEVEL); @@ -320,6 +325,7 @@ static int bmi08x_acc_range_set(const struct device *dev, int32_t range) } data->scale = BMI08X_ACC_SCALE(range); + data->range = reg_val; return ret; } @@ -538,6 +544,10 @@ static DEVICE_API(sensor, bmi08x_api) = { .attr_set = bmi08x_attr_set, #ifdef CONFIG_BMI08X_ACCEL_TRIGGER .trigger_set = bmi08x_trigger_set_acc, +#endif +#ifdef CONFIG_SENSOR_ASYNC_API + .submit = bmi08x_accel_async_submit, + .get_decoder = bmi08x_accel_decoder_get, #endif .sample_fetch = bmi08x_sample_fetch, .channel_get = bmi08x_channel_get, @@ -719,6 +729,14 @@ int bmi08x_accel_init(const struct device *dev) } #endif +#if defined(CONFIG_BMI08X_ACCEL_STREAM) + ret = bmi08x_accel_stream_init(dev); + if (ret < 0) { + LOG_ERR("Failed to init stream: %d", ret); + return ret; + } +#endif + return ret; } @@ -784,22 +802,44 @@ int bmi08x_accel_init(const struct device *dev) #define BMI08X_CREATE_INST(inst) \ \ + RTIO_DEFINE(bmi08x_accel_rtio_ctx_##inst, 16, 16); \ + \ + COND_CODE_1(DT_INST_ON_BUS(inst, i2c), \ + (I2C_DT_IODEV_DEFINE(bmi08x_accel_rtio_bus_##inst, \ + DT_DRV_INST(inst))), \ + (COND_CODE_1(DT_INST_ON_BUS(inst, spi), \ + (SPI_DT_IODEV_DEFINE(bmi08x_accel_rtio_bus_##inst, \ + DT_DRV_INST(inst), \ + SPI_OP_MODE_MASTER | SPI_WORD_SET(8) | SPI_TRANSFER_MSB)),\ + ()))); \ + \ IF_ENABLED(BMI08X_ACCEL_DATA_SYNC_EN(inst), (BMI08X_VERIFY_DATA_SYNC(inst);)) \ IF_ENABLED(BMI08X_ACCEL_DATA_SYNC_EN(inst), (BMI08X_VERIFY_DATA_SYNC_ODR(inst);)) \ IF_ENABLED(BMI08X_ACCEL_DATA_SYNC_EN(inst), (BMI08X_VERIFY_GYRO_DATA_SYNC_EN(inst);)) \ \ - static struct bmi08x_accel_data bmi08x_drv_##inst; \ + static struct bmi08x_accel_data bmi08x_drv_##inst = { \ + IF_ENABLED(CONFIG_BMI08X_ACCEL_STREAM, \ + (.stream.fifo_wm = DT_INST_PROP_OR(inst, fifo_watermark, 0),)) \ + }; \ + \ \ static const struct bmi08x_accel_config bmi08x_config_##inst = { \ COND_CODE_1(DT_INST_ON_BUS(inst, spi), (BMI08X_CONFIG_SPI(inst)), \ (BMI08X_CONFIG_I2C(inst))) \ .api = COND_CODE_1(DT_INST_ON_BUS(inst, spi), (&bmi08x_spi_api), \ (&bmi08x_i2c_api)), \ - IF_ENABLED(CONFIG_BMI08X_ACCEL_TRIGGER, \ - (.int_gpio = GPIO_DT_SPEC_INST_GET(inst, int_gpios),)) \ + .int_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ BMI08X_ACCEL_TRIGGER_PINS(inst) \ .accel_hz = DT_INST_ENUM_IDX(inst, accel_hz) + 5, \ - .accel_fs = DT_INST_PROP(inst, accel_fs), BMI08X_DATA_SYNC_REG(inst)}; \ + .rtio_bus = { \ + .ctx = &bmi08x_accel_rtio_ctx_##inst, \ + .iodev = &bmi08x_accel_rtio_bus_##inst, \ + .type = COND_CODE_1(DT_INST_ON_BUS(inst, i2c), \ + (BMI08X_RTIO_BUS_TYPE_I2C), \ + (BMI08X_RTIO_BUS_TYPE_SPI)), \ + }, \ + .accel_fs = DT_INST_PROP(inst, accel_fs), BMI08X_DATA_SYNC_REG(inst) \ + }; \ \ PM_DEVICE_DT_INST_DEFINE(inst, bmi08x_accel_pm_action); \ SENSOR_DEVICE_DT_INST_DEFINE(inst, bmi08x_accel_init, PM_DEVICE_DT_INST_GET(inst), \ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_accel_async.c b/drivers/sensor/bosch/bmi08x/bmi08x_accel_async.c new file mode 100644 index 0000000000000..0d085f8d3c4c2 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_accel_async.c @@ -0,0 +1,99 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include + +#define DT_DRV_COMPAT bosch_bmi08x_accel +#include "bmi08x.h" +#include "bmi08x_bus.h" +#include "bmi08x_accel_stream.h" +#include "bmi08x_accel_decoder.h" + +#include +LOG_MODULE_REGISTER(BMI08X_ACCEL_ASYNC, CONFIG_SENSOR_LOG_LEVEL); + +static void bmi08x_complete_result(struct rtio *ctx, const struct rtio_sqe *sqe, int result, + void *arg) +{ + struct rtio_iodev_sqe *iodev_sqe = (struct rtio_iodev_sqe *)arg; + + (void)rtio_flush_completion_queue(ctx); + if (result >= 0) { + rtio_iodev_sqe_ok(iodev_sqe, 0); + } else { + rtio_iodev_sqe_err(iodev_sqe, result); + } +} + +static void bmi08x_submit_one_shot(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + uint32_t buf_len_req = sizeof(struct bmi08x_accel_encoded_data); + struct bmi08x_accel_encoded_data *edata; + uint32_t buf_len; + uint64_t cycles; + int err; + + err = rtio_sqe_rx_buf(iodev_sqe, buf_len_req, buf_len_req, (uint8_t **)&edata, &buf_len); + CHECKIF(err < 0 || buf_len < buf_len_req || !edata) { + LOG_ERR("Failed to get a read-buffer of size %u bytes", buf_len_req); + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + + err = sensor_clock_get_cycles(&cycles); + CHECKIF (err != 0) { + LOG_ERR("Failed to get sensor clock cycles: %d", err); + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + bmi08x_accel_encode_header(dev, edata, false, 0); + + struct rtio_sqe *out_sqe; + struct rtio_sqe *complete_sqe; + const struct bmi08x_accel_config *config = dev->config; + + err = bmi08x_prep_reg_read_rtio_async(&config->rtio_bus, BMI08X_REG_ACCEL_X_LSB, + (uint8_t *)edata->payload, sizeof(edata->payload), + &out_sqe, true); + if (err < 0) { + LOG_ERR("Failed to perpare async read operation"); + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + out_sqe->flags |= RTIO_SQE_CHAINED; + + complete_sqe = rtio_sqe_acquire(config->rtio_bus.ctx); + if (!complete_sqe) { + LOG_ERR("Failed to perpare async read operation"); + rtio_sqe_drop_all(config->rtio_bus.ctx); + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + + rtio_sqe_prep_callback_no_cqe(complete_sqe, bmi08x_complete_result, iodev_sqe, (void *)dev); + rtio_submit(config->rtio_bus.ctx, 0); +} + +void bmi08x_accel_async_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; + + if (!cfg->is_streaming) { + bmi08x_submit_one_shot(dev, iodev_sqe); + } else if (IS_ENABLED(CONFIG_BMI08X_ACCEL_STREAM)){ + bmi08x_accel_stream_submit(dev, iodev_sqe); + } else { + LOG_ERR("Streaming not supported"); + rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP); + } +} diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_accel_async.h b/drivers/sensor/bosch/bmi08x/bmi08x_accel_async.h new file mode 100644 index 0000000000000..241814fa39d2a --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_accel_async.h @@ -0,0 +1,16 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_ASYNC_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_ASYNC_H_ + +#include +#include + +void bmi08x_accel_async_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_ASYNC_H_ */ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_accel_decoder.c b/drivers/sensor/bosch/bmi08x/bmi08x_accel_decoder.c new file mode 100644 index 0000000000000..38e84b8ddb9ab --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_accel_decoder.c @@ -0,0 +1,227 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include + +#define DT_DRV_COMPAT bosch_bmi08x_accel +#include "bmi08x.h" +#include "bmi08x_bus.h" +#include "bmi08x_accel_decoder.h" + +#include +LOG_MODULE_REGISTER(BMI08X_ACCEL_DECODER, CONFIG_SENSOR_LOG_LEVEL); + +enum bmi08x_accel_fifo_header { + BMI08X_ACCEL_FIFO_FRAME_ACCEL = 0x84, + BMI08X_ACCEL_FIFO_FRAME_SKIP = 0x40, + BMI08X_ACCEL_FIFO_FRAME_TIME = 0x44, + BMI08X_ACCEL_FIFO_FRAME_CONFIG = 0x48, + BMI08X_ACCEL_FIFO_FRAME_DROP = 0x50, + BMI08X_ACCEL_FIFO_FRAME_EMPTY = 0x80, +}; + +struct frame_len { + enum bmi08x_accel_fifo_header header; + uint8_t len; +} fifo_frame_len[] = { + {.header = BMI08X_ACCEL_FIFO_FRAME_ACCEL, .len = 7}, + {.header = BMI08X_ACCEL_FIFO_FRAME_SKIP, .len = 2}, + {.header = BMI08X_ACCEL_FIFO_FRAME_TIME, .len = 4}, + {.header = BMI08X_ACCEL_FIFO_FRAME_CONFIG, .len = 2}, + {.header = BMI08X_ACCEL_FIFO_FRAME_DROP, .len = 2}, + {.header = BMI08X_ACCEL_FIFO_FRAME_EMPTY, .len = 2}, +}; + +void bmi08x_accel_encode_header(const struct device *dev, struct bmi08x_accel_encoded_data *edata, + bool is_streaming, uint16_t buf_len) +{ + struct bmi08x_accel_data *data = dev->data; + + edata->header.has_accel = true; + edata->header.timestamp = sensor_clock_get_ns(); + edata->header.range = data->range; + edata->header.chip_id = data->accel_chip_id; + edata->header.is_streaming = is_streaming; + edata->header.sample_count = is_streaming ? data->stream.fifo_wm : 1; + edata->header.buf_len = buf_len; +} + +static int bmi08x_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec chan_spec, + uint16_t *frame_count) +{ + const struct bmi08x_accel_encoded_data *edata = + (const struct bmi08x_accel_encoded_data *)buffer; + + if (!edata->header.has_accel || chan_spec.chan_idx != 0) { + return -ENODATA; + } + + if (chan_spec.chan_type != SENSOR_CHAN_ACCEL_XYZ) { + return -EINVAL; + } + + *frame_count = edata->header.sample_count; + + return 0; +} + +static int bmi08x_decoder_get_size_info(struct sensor_chan_spec chan_spec, size_t *base_size, + size_t *frame_size) +{ + if (chan_spec.chan_idx != 0) { + return -EINVAL; + } + + switch (chan_spec.chan_type) { + case SENSOR_CHAN_ACCEL_XYZ: + *base_size = sizeof(struct sensor_three_axis_data); + *frame_size = sizeof(struct sensor_three_axis_sample_data); + return 0; + default: + return -ENOTSUP; + } +} + +static inline void fixed_point_from_encoded_data(const uint16_t encoded_payload[3], uint8_t shift, + uint32_t fsr_value_g, q31_t output[3]) +{ + for (size_t i = 0 ; i < 3 ; i++) { + int64_t raw_value; + + raw_value = sign_extend_64(encoded_payload[i], 15); + raw_value = (raw_value * fsr_value_g << (31 - 5 - 15)) * SENSOR_G / 1000000; + + output[i] = raw_value; + } +} + +static inline int bmi08x_decode_one_shot(const struct bmi08x_accel_encoded_data *edata, + uint32_t *fit, struct sensor_three_axis_data *data_output) +{ + uint32_t fsr_value_g = (edata->header.chip_id == BMI085_ACCEL_CHIP_ID) ? 2 : 3; + + if (*fit != 0) { + return -ENODATA; + } + + /** Bits we need to represent the integer part of FSR in m/s2: + * - 2 - 3 G (19.6 - 29.4 m/s2) = 5 bits. + * - 4 - 6 G (39.2 - 58.8 m/s2) = 6 bits. + * - 8 - 12 G (78.4 - 117.6 m/s2) = 7 bits. + * - 16 - 24 G (156.8 235.2 m/s2) = 8 bits. + */ + data_output->shift = 5 + edata->header.range; + data_output->header.reading_count = 1; + data_output->header.base_timestamp_ns = edata->header.timestamp; + fixed_point_from_encoded_data(edata->payload, data_output->shift, fsr_value_g, + data_output->readings[0].values); + + return ++(*fit); +} + +static inline int fifo_get_frame_len(enum bmi08x_accel_fifo_header header) +{ + for (size_t i = 0 ; i < ARRAY_SIZE(fifo_frame_len) ; i++) { + if (header == fifo_frame_len[i].header) { + return fifo_frame_len[i].len; + } + } + return -EINVAL; +} + +static inline int bmi08x_decode_fifo(const struct bmi08x_accel_encoded_data *edata, uint32_t *fit, + uint16_t max_count, struct sensor_three_axis_data *data_output) +{ + uint8_t reading_count = 0; + uint32_t fsr_value_g = edata->header.chip_id == BMI085_ACCEL_CHIP_ID ? 2 : 3; + + if (*fit >= edata->header.buf_len) { + return -ENODATA; + } + + /** Bits we need to represent the integer part of FSR in m/s2: + * - 2 - 3 G (19.6 - 29.4 m/s2) = 5 bits. + * - 4 - 6 G (39.2 - 58.8 m/s2) = 6 bits. + * - 8 - 12 G (78.4 - 117.6 m/s2) = 7 bits. + * - 16 - 24 G (156.8 235.2 m/s2) = 8 bits. + */ + data_output->shift = 5 + edata->header.range; + data_output->header.reading_count = 0; + data_output->header.base_timestamp_ns = edata->header.timestamp; + + do { + uint8_t header_byte = edata->fifo[*fit] & 0xFC; + int frame_len = fifo_get_frame_len(header_byte); + + if (frame_len < 0) { + LOG_WRN("Invalid frame header: 0x%02X", header_byte); + return frame_len; + } + + if (header_byte == BMI08X_ACCEL_FIFO_FRAME_ACCEL && + *fit + frame_len <= edata->header.buf_len) { + const uint16_t *values = (const uint16_t *)&edata->fifo[*fit + 1]; + + fixed_point_from_encoded_data( + values, data_output->shift, fsr_value_g, + data_output->readings[reading_count].values); + reading_count++; + } + *fit += frame_len; + } while (*fit < edata->header.buf_len && reading_count < max_count); + + data_output->header.reading_count = reading_count; + return reading_count; +} + +static int bmi08x_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec, + uint32_t *fit, uint16_t max_count, void *data_out) +{ + struct sensor_three_axis_data *data_output = (struct sensor_three_axis_data *)data_out; + const struct bmi08x_accel_encoded_data *edata = + (const struct bmi08x_accel_encoded_data *)buffer; + + if (chan_spec.chan_type != SENSOR_CHAN_ACCEL_XYZ || chan_spec.chan_idx != 0 || + max_count == 0 || !edata->header.has_accel) { + return -EINVAL; + } + + if (edata->header.is_streaming) { + return bmi08x_decode_fifo(edata, fit, max_count, data_output); + } else { + return bmi08x_decode_one_shot(edata, fit, data_output); + } +} + +static bool bmi08x_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger) +{ + const struct bmi08x_accel_encoded_data *edata = + (const struct bmi08x_accel_encoded_data *)buffer; + + return edata->header.is_streaming && edata->header.fifo_len > 0 && + trigger == SENSOR_TRIG_FIFO_WATERMARK; +} + +SENSOR_DECODER_API_DT_DEFINE() = { + .get_frame_count = bmi08x_decoder_get_frame_count, + .get_size_info = bmi08x_decoder_get_size_info, + .decode = bmi08x_decoder_decode, + .has_trigger = bmi08x_decoder_has_trigger, +}; + +int bmi08x_accel_decoder_get(const struct device *dev, const struct sensor_decoder_api **decoder) +{ + ARG_UNUSED(dev); + *decoder = &SENSOR_DECODER_NAME(); + return 0; +} diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_accel_decoder.h b/drivers/sensor/bosch/bmi08x/bmi08x_accel_decoder.h new file mode 100644 index 0000000000000..7bda48059cbd4 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_accel_decoder.h @@ -0,0 +1,19 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_DECODER_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_DECODER_H_ + +#include +#include +#include "bmi08x.h" + +void bmi08x_accel_encode_header(const struct device *dev, struct bmi08x_accel_encoded_data *edata, + bool is_streaming, uint16_t buf_len); +int bmi08x_accel_decoder_get(const struct device *dev, const struct sensor_decoder_api **decoder); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_DECODER_H_ */ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_accel_stream.c b/drivers/sensor/bosch/bmi08x/bmi08x_accel_stream.c new file mode 100644 index 0000000000000..3141ad2a0d330 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_accel_stream.c @@ -0,0 +1,229 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include + +#define DT_DRV_COMPAT bosch_bmi08x_accel +#include "bmi08x.h" +#include "bmi08x_bus.h" +#include "bmi08x_accel_stream.h" +#include "bmi08x_accel_decoder.h" + +#include +LOG_MODULE_REGISTER(BMI08X_ACCEL_STREAM, CONFIG_SENSOR_LOG_LEVEL); + +enum bmi08x_stream_state { + BMI08X_STREAM_OFF, + BMI08X_STREAM_ON, + BMI08X_STREAM_BUSY, +}; + +static inline void bmi08x_stream_result(const struct device *dev, int result) +{ + struct bmi08x_accel_data *data = dev->data; + const struct bmi08x_accel_config *config = dev->config; + struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe; + + data->stream.iodev_sqe = NULL; + (void)rtio_flush_completion_queue(config->rtio_bus.ctx); + if (result >= 0) { + (void)atomic_set(&data->stream.state, BMI08X_STREAM_ON); + if (iodev_sqe) { + rtio_iodev_sqe_ok(iodev_sqe, 0); + } + } else { + (void)atomic_set(&data->stream.state, BMI08X_STREAM_OFF); + if (iodev_sqe) { + rtio_iodev_sqe_err(iodev_sqe, result); + } + } +} + +static void bmi08x_stream_complete_handler(struct rtio *ctx, const struct rtio_sqe *sqe, int err, + void *arg) +{ + const struct device *dev = (const struct device *)arg; + struct bmi08x_accel_data *data = dev->data; + struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe; + struct bmi08x_accel_encoded_data *edata; + uint32_t buf_len; + int ret; + + ret = rtio_sqe_rx_buf(iodev_sqe, 0, 0, (uint8_t **)&edata, &buf_len); + if (ret < 0 || buf_len == 0 || edata->header.fifo_len == 0) { + err = -EIO; + } + + bmi08x_stream_result(dev, err); +} + +static inline void bmi08x_accel_stream_evt_handler(const struct device *dev) +{ + struct bmi08x_accel_data *data = dev->data; + const struct bmi08x_accel_config *config = dev->config; + struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe; + int ret; + struct bmi08x_accel_encoded_data *edata; + uint32_t buf_len; + size_t readout_len = sizeof(struct bmi08x_accel_frame) * (data->stream.fifo_wm + 1); + size_t required_len = sizeof(struct bmi08x_accel_encoded_data) + readout_len; + struct rtio_sqe *out_sqe; + + CHECKIF(!iodev_sqe || atomic_get(&data->stream.state) == BMI08X_STREAM_OFF) { + LOG_WRN("Event with Stream is Off. Disabling stream"); + bmi08x_stream_result(dev, -EIO); + return; + } + CHECKIF(atomic_cas(&data->stream.state, BMI08X_STREAM_ON, BMI08X_STREAM_BUSY) == false) { + LOG_DBG("Event while Stream is busy. Ignoring"); + return; + } + + ret = rtio_sqe_rx_buf(iodev_sqe, required_len, required_len, (uint8_t **)&edata, &buf_len); + CHECKIF(ret < 0 || buf_len < required_len) { + LOG_ERR("Failed to obtain buffer. Err: %d, Req-len: %d", ret, required_len); + bmi08x_stream_result(dev, -ENOMEM); + return; + } + bmi08x_accel_encode_header(dev, edata, true, readout_len); + + struct reg_val_read { + uint8_t reg; + void *buf; + size_t len; + } streaming_readout[] = { + {.reg = BMI08X_REG_ACCEL_FIFO_LEN_0, .buf = &edata->header.fifo_len, .len = 2}, + {.reg = BMI08X_REG_ACCEL_FIFO_DATA, .buf = edata->fifo, .len = readout_len}, + }; + + for (size_t i = 0 ; i < ARRAY_SIZE(streaming_readout) ; i++) { + ret = bmi08x_prep_reg_read_rtio_async(&config->rtio_bus, streaming_readout[i].reg, + (uint8_t *)streaming_readout[i].buf, + streaming_readout[i].len, &out_sqe, true); + CHECKIF(ret < 0 || !out_sqe) { + bmi08x_stream_result(dev, -EIO); + return; + } + out_sqe->flags |= RTIO_SQE_CHAINED; + } + out_sqe = rtio_sqe_acquire(config->rtio_bus.ctx); + CHECKIF(!out_sqe) { + bmi08x_stream_result(dev, -EIO); + return; + } + rtio_sqe_prep_callback_no_cqe(out_sqe, bmi08x_stream_complete_handler, (void *)dev, NULL); + (void)rtio_submit(config->rtio_bus.ctx, 0); +} + +static void bmi08x_accel_gpio_callback(const struct device *port, struct gpio_callback *cb, + uint32_t pin) +{ + struct bmi08x_accel_data *data = CONTAINER_OF(cb, struct bmi08x_accel_data, gpio_cb); + const struct device *dev = data->dev; + const struct bmi08x_accel_config *cfg = dev->config; + + (void)gpio_pin_interrupt_configure_dt(&cfg->int_gpio, GPIO_INT_DISABLE); + (void)gpio_remove_callback(cfg->int_gpio.port, &data->gpio_cb); + bmi08x_accel_stream_evt_handler(dev); +} + +static inline int start_stream(const struct device *dev) +{ + struct bmi08x_accel_data *data = dev->data; + const struct bmi08x_accel_config *cfg = dev->config; + struct rtio_sqe *out_sqe; + int ret; + uint16_t fifo_wm_bytes = data->stream.fifo_wm * sizeof(struct bmi08x_accel_frame); + struct reg_vals { + uint8_t reg; + uint8_t val; + } stream_cfg_reg_writes[] = { + {.reg = BMI08X_REG_ACCEL_FIFO_WTM_0, .val = fifo_wm_bytes & 0xFF}, + {.reg = BMI08X_REG_ACCEL_FIFO_WTM_1, .val = (fifo_wm_bytes >> 8) & 0x1F}, + {.reg = BMI08X_REG_ACCEL_FIFO_CONFIG_0, .val = BIT(1) | BIT(0)}, /* FIFO Mode */ + {.reg = BMI08X_REG_ACCEL_FIFO_CONFIG_1, .val = BIT(6) | BIT(4)}, + {.reg = BMI08X_REG_ACCEL_INT1_INT2_MAP_DATA, .val = BIT(0) | BIT(1)}, /* INT1 */ + {.reg = BMI08X_REG_ACCEL_INT1_IO_CONF, .val = BIT(1) | BIT(3)}, /* Push-pull */ + }; + + for (size_t i = 0 ; i < ARRAY_SIZE(stream_cfg_reg_writes) ; i++) { + ret = bmi08x_prep_reg_write_rtio_async(&cfg->rtio_bus, stream_cfg_reg_writes[i].reg, + &stream_cfg_reg_writes[i].val, 1, &out_sqe); + CHECKIF(ret < 0 || !out_sqe) { + return ret; + } + out_sqe->flags |= RTIO_SQE_CHAINED; + } + out_sqe->flags &= ~RTIO_SQE_CHAINED; + + /** We Synchronously write the stream since we want to do be done before enabling the + * interrupts. In the event that we're recovering from a failure, the interrupt line + * will be de-asserted. + */ + (void)rtio_submit(cfg->rtio_bus.ctx, ret); + return 0; +} + +void bmi08x_accel_stream_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + struct bmi08x_accel_data *data = dev->data; + const struct bmi08x_accel_config *cfg = dev->config; + const struct sensor_read_config *stream_cfg = iodev_sqe->sqe.iodev->data; + int ret = 0; + + if (stream_cfg->count != 1 || + stream_cfg->triggers->trigger != SENSOR_TRIG_FIFO_WATERMARK || + stream_cfg->triggers->opt != SENSOR_STREAM_DATA_INCLUDE) { + LOG_ERR("Invalid stream configuration"); + rtio_iodev_sqe_err(iodev_sqe, -EINVAL); + return; + } + data->stream.iodev_sqe = iodev_sqe; + + if (atomic_cas(&data->stream.state, BMI08X_STREAM_OFF, BMI08X_STREAM_ON) == true) { + ret = start_stream(dev); + if (ret != 0) { + LOG_ERR("Failed to configure stream"); + bmi08x_stream_result(dev, ret); + return; + } + } + (void)gpio_add_callback(cfg->int_gpio.port, &data->gpio_cb); + (void)gpio_pin_interrupt_configure_dt(&cfg->int_gpio, GPIO_INT_LEVEL_HIGH); +} + +int bmi08x_accel_stream_init(const struct device *dev) +{ + struct bmi08x_accel_data *data = dev->data; + const struct bmi08x_accel_config *cfg = dev->config; + int ret; + + if (!gpio_is_ready_dt(&cfg->int_gpio)) { + LOG_ERR("GPIO device not ready: %p - dev: %p", &cfg->int_gpio, dev); + return -ENODEV; + } + ret = gpio_pin_configure_dt(&cfg->int_gpio, GPIO_INPUT); + if (ret < 0) { + LOG_ERR("Failed to configure GPIO: %d", ret); + return ret; + } + ret = gpio_pin_interrupt_configure_dt(&cfg->int_gpio, GPIO_INT_DISABLE); + if (ret < 0) { + LOG_ERR("Failed to configure GPIO: %d", ret); + return ret; + } + gpio_init_callback(&data->gpio_cb, bmi08x_accel_gpio_callback, BIT(cfg->int_gpio.pin)); + data->dev = dev; + + return 0; +} diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_accel_stream.h b/drivers/sensor/bosch/bmi08x/bmi08x_accel_stream.h new file mode 100644 index 0000000000000..8caaa1547dc80 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_accel_stream.h @@ -0,0 +1,17 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_STREAM_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_STREAM_H_ + +#include +#include + +int bmi08x_accel_stream_init(const struct device *dev); +void bmi08x_accel_stream_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_ACCEL_STREAM_H_ */ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_bus.c b/drivers/sensor/bosch/bmi08x/bmi08x_bus.c new file mode 100644 index 0000000000000..92f96f4cd499b --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_bus.c @@ -0,0 +1,148 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "bmi08x_bus.h" + +static uint8_t dummy_byte_val; + +int bmi08x_prep_reg_read_rtio_async(const struct bmi08x_rtio_bus *bus, + uint8_t reg, uint8_t *buf, size_t size, + struct rtio_sqe **out, bool dummy_byte) +{ + struct rtio *ctx = bus->ctx; + struct rtio_iodev *iodev = bus->iodev; + struct rtio_sqe *write_reg_sqe = rtio_sqe_acquire(ctx); + size_t sqe_ct = 2; + + if (!write_reg_sqe) { + rtio_sqe_drop_all(ctx); + return -ENOMEM; + } + reg |= BIT(7); + rtio_sqe_prep_tiny_write(write_reg_sqe, iodev, RTIO_PRIO_NORM, ®, 1, NULL); + write_reg_sqe->flags |= RTIO_SQE_TRANSACTION; + + if (dummy_byte) { + struct rtio_sqe *dummy_byte_sqe = rtio_sqe_acquire(ctx); + + if (!dummy_byte_sqe) { + rtio_sqe_drop_all(ctx); + return -ENOMEM; + } + rtio_sqe_prep_read(dummy_byte_sqe, iodev, RTIO_PRIO_NORM, &dummy_byte_val, 1, + NULL); + dummy_byte_sqe->flags |= RTIO_SQE_TRANSACTION; + sqe_ct++; + } + struct rtio_sqe *read_buf_sqe = rtio_sqe_acquire(ctx); + + if (!read_buf_sqe) { + rtio_sqe_drop_all(ctx); + return -ENOMEM; + } + rtio_sqe_prep_read(read_buf_sqe, iodev, RTIO_PRIO_NORM, buf, size, NULL); + if (bus->type == BMI08X_RTIO_BUS_TYPE_I2C) { + read_buf_sqe->iodev_flags |= RTIO_IODEV_I2C_STOP | RTIO_IODEV_I2C_RESTART; + } + + /** Send back last SQE so it can be concatenated later. */ + if (out) { + *out = read_buf_sqe; + } + + return sqe_ct; +} + +int bmi08x_prep_reg_write_rtio_async(const struct bmi08x_rtio_bus *bus, + uint8_t reg, const uint8_t *buf, size_t size, + struct rtio_sqe **out) +{ + struct rtio *ctx = bus->ctx; + struct rtio_iodev *iodev = bus->iodev; + struct rtio_sqe *write_reg_sqe = rtio_sqe_acquire(ctx); + struct rtio_sqe *write_buf_sqe = rtio_sqe_acquire(ctx); + + if (!write_reg_sqe || !write_buf_sqe) { + rtio_sqe_drop_all(ctx); + return -ENOMEM; + } + + /** More than 7 won't work with tiny-write */ + if (size > 7) { + return -EINVAL; + } + + rtio_sqe_prep_tiny_write(write_reg_sqe, iodev, RTIO_PRIO_NORM, ®, 1, NULL); + write_reg_sqe->flags |= RTIO_SQE_TRANSACTION; + rtio_sqe_prep_tiny_write(write_buf_sqe, iodev, RTIO_PRIO_NORM, buf, size, NULL); + if (bus->type == BMI08X_RTIO_BUS_TYPE_I2C) { + write_buf_sqe->iodev_flags |= RTIO_IODEV_I2C_STOP; + } + + /** Send back last SQE so it can be concatenated later. */ + if (out) { + *out = write_buf_sqe; + } + + return 2; +} + +int bmi08x_reg_read_rtio(const struct bmi08x_rtio_bus *bus, uint8_t start, uint8_t *buf, int size, + bool dummy_byte) +{ + struct rtio *ctx = bus->ctx; + struct rtio_cqe *cqe; + int ret; + + ret = bmi08x_prep_reg_read_rtio_async(bus, start, buf, size, NULL, dummy_byte); + if (ret < 0) { + return ret; + } + + ret = rtio_submit(ctx, ret); + if (ret) { + return ret; + } + + do { + cqe = rtio_cqe_consume(ctx); + if (cqe != NULL) { + ret = cqe->result; + rtio_cqe_release(ctx, cqe); + } + } while (cqe != NULL); + + return ret; +} + +int bmi08x_reg_write_rtio(const struct bmi08x_rtio_bus *bus, uint8_t reg, const uint8_t *buf, + int size) +{ + struct rtio *ctx = bus->ctx; + struct rtio_cqe *cqe; + int ret; + + ret = bmi08x_prep_reg_write_rtio_async(bus, reg, buf, size, NULL); + if (ret < 0) { + return ret; + } + + ret = rtio_submit(ctx, ret); + if (ret) { + return ret; + } + + do { + cqe = rtio_cqe_consume(ctx); + if (cqe != NULL) { + ret = cqe->result; + rtio_cqe_release(ctx, cqe); + } + } while (cqe != NULL); + + return ret; +} diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_bus.h b/drivers/sensor/bosch/bmi08x/bmi08x_bus.h new file mode 100644 index 0000000000000..f2ef90c0986ff --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_bus.h @@ -0,0 +1,39 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_RTIO_BUS_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_RTIO_BUS_H_ + +#include +#include + +enum bmi08x_rtio_bus_type { + BMI08X_RTIO_BUS_TYPE_I2C, + BMI08X_RTIO_BUS_TYPE_SPI, +}; + +struct bmi08x_rtio_bus { + struct rtio *ctx; + struct rtio_iodev *iodev; + enum bmi08x_rtio_bus_type type; +}; + +int bmi08x_prep_reg_read_rtio_async(const struct bmi08x_rtio_bus *bus, + uint8_t reg, uint8_t *buf, size_t size, + struct rtio_sqe **out, bool dummy_byte); + +int bmi08x_prep_reg_write_rtio_async(const struct bmi08x_rtio_bus *bus, + uint8_t reg, const uint8_t *buf, size_t size, + struct rtio_sqe **out); + +int bmi08x_reg_read_rtio(const struct bmi08x_rtio_bus *bus, uint8_t start, uint8_t *buf, int size, + bool dummy_byte); + +int bmi08x_reg_write_rtio(const struct bmi08x_rtio_bus *bus, uint8_t reg, const uint8_t *buf, + int size); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_RTIO_BUS_H_ */ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_gyro.c b/drivers/sensor/bosch/bmi08x/bmi08x_gyro.c index 9fec8f71222e9..31548662e94fb 100644 --- a/drivers/sensor/bosch/bmi08x/bmi08x_gyro.c +++ b/drivers/sensor/bosch/bmi08x/bmi08x_gyro.c @@ -6,15 +6,23 @@ */ #include +#include #include #include #include #include #include #include +#include +#include +#include #define DT_DRV_COMPAT bosch_bmi08x_gyro #include "bmi08x.h" +#include "bmi08x_bus.h" +#include "bmi08x_gyro_async.h" +#include "bmi08x_gyro_stream.h" +#include "bmi08x_gyro_decoder.h" LOG_MODULE_REGISTER(BMI08X_GYRO, CONFIG_SENSOR_LOG_LEVEL); @@ -178,6 +186,7 @@ static int bmi08x_gyr_range_set(const struct device *dev, uint16_t range) } bmi08x->scale = BMI08X_GYR_SCALE(range); + bmi08x->range = reg_val; return ret; } @@ -334,6 +343,10 @@ static DEVICE_API(sensor, bmi08x_api) = { .attr_set = bmi08x_attr_set, #ifdef CONFIG_BMI08X_GYRO_TRIGGER .trigger_set = bmi08x_trigger_set_gyr, +#endif +#ifdef CONFIG_SENSOR_ASYNC_API + .submit = bmi08x_gyro_async_submit, + .get_decoder = bmi08x_gyro_decoder_get, #endif .sample_fetch = bmi08x_sample_fetch, .channel_get = bmi08x_channel_get, @@ -416,6 +429,14 @@ int bmi08x_gyro_init(const struct device *dev) } #endif +#if defined(CONFIG_BMI08X_GYRO_STREAM) + ret = bmi08x_gyro_stream_init(dev); + if (ret < 0) { + LOG_ERR("Failed to init stream: %d", ret); + return ret; + } +#endif + return ret; } @@ -444,17 +465,45 @@ BUILD_ASSERT(CONFIG_BMI08X_GYRO_TRIGGER_NONE, #define BMI08X_CREATE_INST(inst) \ \ - static struct bmi08x_gyro_data bmi08x_drv_##inst; \ + IF_ENABLED(CONFIG_BMI08X_GYRO_STREAM, \ + (BUILD_ASSERT(DT_INST_PROP_OR(inst, fifo_watermark, 0) > 0 && \ + DT_INST_PROP_OR(inst, fifo_watermark, 0) < 100, \ + "FIFO Watermark must be defined for streaming mode, and be " \ + "within 1 and 99. Please define fifo-watermark accordingly or " \ + "disable CONFIG_BMI08X_STREAM"))); \ + \ + RTIO_DEFINE(bmi08x_gyro_rtio_ctx_##inst, 16, 16); \ + \ + COND_CODE_1(DT_INST_ON_BUS(inst, i2c), \ + (I2C_DT_IODEV_DEFINE(bmi08x_gyro_rtio_bus_##inst, \ + DT_DRV_INST(inst))), \ + (COND_CODE_1(DT_INST_ON_BUS(inst, spi), \ + (SPI_DT_IODEV_DEFINE(bmi08x_gyro_rtio_bus_##inst, \ + DT_DRV_INST(inst), \ + SPI_OP_MODE_MASTER | SPI_WORD_SET(8) | SPI_TRANSFER_MSB)),\ + ()))); \ + \ + static struct bmi08x_gyro_data bmi08x_drv_##inst = { \ + IF_ENABLED(CONFIG_BMI08X_GYRO_STREAM, \ + (.stream.fifo_wm = DT_INST_PROP_OR(inst, fifo_watermark, 0),)) \ + }; \ \ static const struct bmi08x_gyro_config bmi08x_config_##inst = { \ COND_CODE_1(DT_INST_ON_BUS(inst, spi), (BMI08X_CONFIG_SPI(inst)), \ (BMI08X_CONFIG_I2C(inst))) \ .api = COND_CODE_1(DT_INST_ON_BUS(inst, spi), (&bmi08x_spi_api), \ (&bmi08x_i2c_api)), \ - IF_ENABLED(CONFIG_BMI08X_GYRO_TRIGGER, \ - (.int_gpio = GPIO_DT_SPEC_INST_GET(inst, int_gpios),)) \ + .int_gpio = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ .gyro_hz = DT_INST_ENUM_IDX(inst, gyro_hz), \ - BMI08X_GYRO_TRIGGER_PINS(inst).gyro_fs = DT_INST_PROP(inst, gyro_fs), \ + BMI08X_GYRO_TRIGGER_PINS(inst) \ + .gyro_fs = DT_INST_PROP(inst, gyro_fs), \ + .rtio_bus = { \ + .ctx = &bmi08x_gyro_rtio_ctx_##inst, \ + .iodev = &bmi08x_gyro_rtio_bus_##inst, \ + .type = COND_CODE_1(DT_INST_ON_BUS(inst, i2c), \ + (BMI08X_RTIO_BUS_TYPE_I2C), \ + (BMI08X_RTIO_BUS_TYPE_SPI)), \ + }, \ }; \ \ PM_DEVICE_DT_INST_DEFINE(inst, bmi08x_gyro_pm_action); \ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_gyro_async.c b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_async.c new file mode 100644 index 0000000000000..0eae888a36edd --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_async.c @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include + +#define DT_DRV_COMPAT bosch_bmi08x_gyro +#include "bmi08x.h" +#include "bmi08x_bus.h" +#include "bmi08x_gyro_stream.h" +#include "bmi08x_gyro_decoder.h" + +#include +LOG_MODULE_REGISTER(BMI08X_GYRO_ASYNC, CONFIG_SENSOR_LOG_LEVEL); + +static void bmi08x_complete_result(struct rtio *ctx, const struct rtio_sqe *sqe, int result, + void *arg) +{ + struct rtio_iodev_sqe *iodev_sqe = (struct rtio_iodev_sqe *)arg; + + (void)rtio_flush_completion_queue(ctx); + if (result >= 0) { + rtio_iodev_sqe_ok(iodev_sqe, 0); + } else { + rtio_iodev_sqe_err(iodev_sqe, result); + } +} + +static void bmi08x_submit_one_shot(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + uint32_t buf_len_req = sizeof(struct bmi08x_gyro_encoded_data); + struct bmi08x_gyro_encoded_data *edata; + uint32_t buf_len; + int err; + + err = rtio_sqe_rx_buf(iodev_sqe, buf_len_req, buf_len_req, (uint8_t **)&edata, &buf_len); + CHECKIF(err < 0 || buf_len < buf_len_req || !edata) { + LOG_ERR("Failed to get a read-buffer of size %u bytes", buf_len_req); + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + bmi08x_gyro_encode_header(dev, edata, false); + + struct rtio_sqe *out_sqe; + struct rtio_sqe *complete_sqe; + const struct bmi08x_gyro_config *config = dev->config; + + err = bmi08x_prep_reg_read_rtio_async(&config->rtio_bus, BMI08X_REG_GYRO_X_LSB, + (uint8_t *)edata->frame.payload, + sizeof(edata->frame.payload), &out_sqe, false); + if (err < 0) { + LOG_ERR("Failed to perpare async read operation"); + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + out_sqe->flags |= RTIO_SQE_CHAINED; + + complete_sqe = rtio_sqe_acquire(config->rtio_bus.ctx); + if (!complete_sqe) { + LOG_ERR("Failed to perpare async read operation"); + rtio_sqe_drop_all(config->rtio_bus.ctx); + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + + rtio_sqe_prep_callback_no_cqe(complete_sqe, bmi08x_complete_result, iodev_sqe, (void *)dev); + rtio_submit(config->rtio_bus.ctx, 0); +} + +void bmi08x_gyro_async_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; + + if (!cfg->is_streaming) { + bmi08x_submit_one_shot(dev, iodev_sqe); + } else if (IS_ENABLED(CONFIG_BMI08X_GYRO_STREAM)) { + bmi08x_gyro_stream_submit(dev, iodev_sqe); + } else { + LOG_ERR("Streaming not enabled"); + rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP); + } +} diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_gyro_async.h b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_async.h new file mode 100644 index 0000000000000..9b2b15f8f16d8 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_async.h @@ -0,0 +1,17 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_ASYNC_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_ASYNC_H_ + +#include +#include +#include + +void bmi08x_gyro_async_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_ASYNC_H_ */ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_gyro_decoder.c b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_decoder.c new file mode 100644 index 0000000000000..807ed769c4ea2 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_decoder.c @@ -0,0 +1,143 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include + +#define DT_DRV_COMPAT bosch_bmi08x_gyro +#include "bmi08x.h" +#include "bmi08x_bus.h" +#include "bmi08x_gyro_stream.h" +#include "bmi08x_gyro_decoder.h" + +#include +LOG_MODULE_REGISTER(BMI08X_GYRO_DECODER, CONFIG_SENSOR_LOG_LEVEL); + +void bmi08x_gyro_encode_header(const struct device *dev, struct bmi08x_gyro_encoded_data *edata, + bool is_streaming) +{ + struct bmi08x_gyro_data *data = dev->data; + + edata->header.has_gyro = true; + edata->header.timestamp = sensor_clock_get_ns(); + edata->header.range = data->range; + edata->header.is_streaming = is_streaming; + edata->header.sample_count = is_streaming ? data->stream.fifo_wm : 1; +} + +static int bmi08x_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec chan_spec, + uint16_t *frame_count) +{ + const struct bmi08x_gyro_encoded_data *edata = + (const struct bmi08x_gyro_encoded_data *)buffer; + + if (!edata->header.has_gyro || chan_spec.chan_idx != 0) { + return -ENODATA; + } + if (chan_spec.chan_type != SENSOR_CHAN_GYRO_XYZ) { + return -EINVAL; + } + + *frame_count = edata->header.sample_count; + + return 0; +} + +static int bmi08x_decoder_get_size_info(struct sensor_chan_spec chan_spec, size_t *base_size, + size_t *frame_size) +{ + if (chan_spec.chan_idx != 0) { + return -EINVAL; + } + + switch (chan_spec.chan_type) { + case SENSOR_CHAN_GYRO_XYZ: + *base_size = sizeof(struct sensor_three_axis_data); + *frame_size = sizeof(struct sensor_three_axis_sample_data); + return 0; + default: + return -ENOTSUP; + } +} + +static int bmi08x_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec, + uint32_t *fit, uint16_t max_count, void *data_out) +{ + const struct bmi08x_gyro_encoded_data *edata = + (const struct bmi08x_gyro_encoded_data *)buffer; + struct sensor_three_axis_data *data_output = (struct sensor_three_axis_data *)data_out; + uint32_t fit0 = *fit; + + if (chan_spec.chan_type != SENSOR_CHAN_GYRO_XYZ || chan_spec.chan_idx != 0 || + max_count == 0) { + return -EINVAL; + } + if (!edata->header.has_gyro || *fit >= edata->header.sample_count) { + return -ENODATA; + } + if (edata->header.is_streaming && + ((edata->header.fifo_status & 0x7F) == 0)) { + return -ENODATA; + } + uint32_t max_samples = MIN(edata->header.sample_count, edata->header.fifo_status & 0x7F); + + /** Bits we need to represent the integer part of FSR in rad/s: + * - 2000 dps (34.91 rad/s) = 6 bits. + * - 1000 dps (17.45 rad/s) = 5 bits. + * - 500 dps (8.73 rad/s) = 4 bits. + * - 250 dps (4.36 rad/s) = 3 bits. + * - 125 dps (2.18 rad/s) = 2 bits. + */ + data_output->shift = 6 - edata->header.range; + data_output->header.base_timestamp_ns = edata->header.timestamp; + + do { + for (size_t i = 0 ; i < 3 ; i++) { + int64_t raw_value; + + raw_value = sign_extend_64(edata->fifo[*fit].payload[i], 15); + raw_value = (raw_value * 2000 << (31 - 6 - 15)) * SENSOR_PI / 1000000 / 180; + + data_output->readings[*fit - fit0].values[i] = raw_value; + } + } while (++(*fit) < MIN(max_samples, fit0 + max_count)); + + data_output->header.reading_count = *fit - fit0; + + return data_output->header.reading_count; +} + +static bool bmi08x_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger) +{ + const struct bmi08x_gyro_encoded_data *edata = + (const struct bmi08x_gyro_encoded_data *)buffer; + + return trigger == SENSOR_TRIG_FIFO_WATERMARK && + edata->header.has_gyro && + edata->header.is_streaming && + edata->header.int_status & BIT(4) && + (edata->header.fifo_status & BIT_MASK(7)) > 0; +} + +SENSOR_DECODER_API_DT_DEFINE() = { + .get_frame_count = bmi08x_decoder_get_frame_count, + .get_size_info = bmi08x_decoder_get_size_info, + .decode = bmi08x_decoder_decode, + .has_trigger = bmi08x_decoder_has_trigger, +}; + +int bmi08x_gyro_decoder_get(const struct device *dev, const struct sensor_decoder_api **decoder) +{ + ARG_UNUSED(dev); + *decoder = &SENSOR_DECODER_NAME(); + return 0; +} diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_gyro_decoder.h b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_decoder.h new file mode 100644 index 0000000000000..3baa7f6ab5c8a --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_decoder.h @@ -0,0 +1,19 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_DECODER_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_DECODER_H_ + +#include +#include +#include "bmi08x.h" + +void bmi08x_gyro_encode_header(const struct device *dev, struct bmi08x_gyro_encoded_data *edata, + bool is_streaming); +int bmi08x_gyro_decoder_get(const struct device *dev, const struct sensor_decoder_api **decoder); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_DECODER_H_ */ diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_gyro_stream.c b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_stream.c new file mode 100644 index 0000000000000..dd13e0170d742 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_stream.c @@ -0,0 +1,231 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include + +#define DT_DRV_COMPAT bosch_bmi08x_gyro +#include "bmi08x.h" +#include "bmi08x_bus.h" +#include "bmi08x_gyro_stream.h" +#include "bmi08x_gyro_decoder.h" + +#include +LOG_MODULE_REGISTER(BMI08X_GYRO_STREAM, CONFIG_SENSOR_LOG_LEVEL); + +enum bmi08x_stream_state { + BMI08X_STREAM_OFF, + BMI08X_STREAM_ON, + BMI08X_STREAM_BUSY, +}; + +static inline void bmi08x_stream_result(const struct device *dev, int result) +{ + struct bmi08x_gyro_data *data = dev->data; + const struct bmi08x_gyro_config *config = dev->config; + struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe; + + data->stream.iodev_sqe = NULL; + (void)rtio_flush_completion_queue(config->rtio_bus.ctx); + if (result >= 0) { + (void)atomic_set(&data->stream.state, BMI08X_STREAM_ON); + if (iodev_sqe) { + rtio_iodev_sqe_ok(iodev_sqe, 0); + } + } else { + (void)atomic_set(&data->stream.state, BMI08X_STREAM_OFF); + if (iodev_sqe) { + rtio_iodev_sqe_err(iodev_sqe, result); + } + } +} + +static void bmi08x_stream_complete_handler(struct rtio *ctx, const struct rtio_sqe *sqe, int err, + void *arg) +{ + const struct device *dev = (const struct device *)arg; + struct bmi08x_gyro_data *data = dev->data; + struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe; + struct bmi08x_gyro_encoded_data *edata; + uint32_t buf_len; + int ret; + + ret = rtio_sqe_rx_buf(iodev_sqe, 0, 0, (uint8_t **)&edata, &buf_len); + if (ret < 0 || buf_len == 0 || edata->header.int_status == 0 || + edata->header.fifo_status == 0) { + err = -EIO; + } + + bmi08x_stream_result(dev, err); +} + +static inline void bmi08x_gyro_stream_evt_handler(const struct device *dev) +{ + struct bmi08x_gyro_data *data = dev->data; + const struct bmi08x_gyro_config *config = dev->config; + struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe; + int ret; + struct bmi08x_gyro_encoded_data *edata; + uint32_t buf_len; + size_t readout_len = sizeof(struct bmi08x_gyro_frame) * data->stream.fifo_wm; + size_t required_len = sizeof(struct bmi08x_gyro_encoded_data) + readout_len; + struct rtio_sqe *out_sqe; + + CHECKIF(!iodev_sqe || atomic_get(&data->stream.state) == BMI08X_STREAM_OFF) { + LOG_WRN("Event with Stream is Off. Disabling stream"); + bmi08x_stream_result(dev, -EIO); + return; + } + CHECKIF(atomic_cas(&data->stream.state, BMI08X_STREAM_ON, BMI08X_STREAM_BUSY) == false) { + LOG_DBG("Event while Stream is busy. Ignoring"); + return; + } + + ret = rtio_sqe_rx_buf(iodev_sqe, required_len, required_len, (uint8_t **)&edata, &buf_len); + CHECKIF(ret < 0 || buf_len < required_len) { + LOG_ERR("Failed to obtain buffer. Err: %d, Req-len: %d", ret, required_len); + bmi08x_stream_result(dev, -ENOMEM); + return; + } + bmi08x_gyro_encode_header(dev, edata, true); + + struct reg_val_read { + uint8_t reg; + void *buf; + size_t len; + } streaming_readout[] = { + {.reg = BMI08X_REG_GYRO_INT_STAT_1, .buf = &edata->header.int_status, .len = 1,}, + {.reg = BMI08X_REG_FIFO_STATUS, .buf = &edata->header.fifo_status, .len = 1}, + {.reg = BMI08X_REG_GYRO_FIFO_DATA, .buf = edata->fifo, .len = readout_len}, + }; + + for (size_t i = 0 ; i < ARRAY_SIZE(streaming_readout) ; i++) { + ret = bmi08x_prep_reg_read_rtio_async(&config->rtio_bus, streaming_readout[i].reg, + (uint8_t *)streaming_readout[i].buf, + streaming_readout[i].len, &out_sqe, false); + CHECKIF (ret < 0 || !out_sqe) { + bmi08x_stream_result(dev, -EIO); + return; + } + out_sqe->flags |= RTIO_SQE_CHAINED; + } + out_sqe = rtio_sqe_acquire(config->rtio_bus.ctx); + CHECKIF(!out_sqe) { + rtio_sqe_drop_all(config->rtio_bus.ctx); + bmi08x_stream_result(dev, -EIO); + return; + } + rtio_sqe_prep_callback_no_cqe(out_sqe, bmi08x_stream_complete_handler, (void *)dev, NULL); + (void)rtio_submit(config->rtio_bus.ctx, 0); +} + +static void bmi08x_gyro_gpio_callback(const struct device *port, struct gpio_callback *cb, + uint32_t pin) +{ + struct bmi08x_gyro_data *data = CONTAINER_OF(cb, struct bmi08x_gyro_data, gpio_cb); + const struct device *dev = data->dev; + const struct bmi08x_accel_config *cfg = dev->config; + + (void)gpio_pin_interrupt_configure_dt(&cfg->int_gpio, GPIO_INT_DISABLE); + (void)gpio_remove_callback(cfg->int_gpio.port, &data->gpio_cb); + bmi08x_gyro_stream_evt_handler(dev); +} + +static inline int start_stream(const struct device *dev) +{ + struct bmi08x_gyro_data *data = dev->data; + const struct bmi08x_gyro_config *cfg = dev->config; + struct rtio_sqe *out_sqe; + int ret; + struct reg_vals { + uint8_t reg; + uint8_t val; + } stream_cfg_reg_writes[] = { + {.reg = BMI08X_REG_GYRO_FIFO_CONFIG_1, .val = 0x80}, /* FIFO Stream mode */ + {.reg = BMI08X_REG_GYRO_FIFO_CONFIG_0, .val = data->stream.fifo_wm}, /* WM Level */ + {.reg = BMI08X_REG_GYRO_FIFO_WM_EN, .val = 0x88}, /* Enable FIFO WM */ + {.reg = BMI08X_REG_GYRO_INT3_INT4_IO_MAP, .val = BIT(2)}, /* FIFO INT to INT3 */ + {.reg = BMI08X_REG_GYRO_INT3_INT4_IO_CONF, .val = BIT(0)}, /* Push-pull act-high */ + {.reg = BMI08X_REG_GYRO_INT_CTRL, .val = BIT(6)}, /* Enable FIFO Interrupts */ + }; + + for (size_t i = 0 ; i < ARRAY_SIZE(stream_cfg_reg_writes) ; i++) { + ret = bmi08x_prep_reg_write_rtio_async(&cfg->rtio_bus, stream_cfg_reg_writes[i].reg, + &stream_cfg_reg_writes[i].val, 1, &out_sqe); + CHECKIF (ret < 0 || !out_sqe) { + return ret; + } + out_sqe->flags |= RTIO_SQE_CHAINED; + } + out_sqe->flags &= ~RTIO_SQE_CHAINED; + + /** We Synchronously write the stream since we want to do be done before enabling the + * interrupts. In the event that we're recovering from a failure, the interrupt line + * will be de-asserted. + */ + (void)rtio_submit(cfg->rtio_bus.ctx, ret); + return 0; +} + +void bmi08x_gyro_stream_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + struct bmi08x_gyro_data *data = dev->data; + const struct bmi08x_gyro_config *cfg = dev->config; + const struct sensor_read_config *stream_cfg = iodev_sqe->sqe.iodev->data; + int ret = 0; + + if (stream_cfg->count != 1 || + stream_cfg->triggers->trigger != SENSOR_TRIG_FIFO_WATERMARK || + stream_cfg->triggers->opt != SENSOR_STREAM_DATA_INCLUDE) { + LOG_ERR("Invalid stream configuration"); + rtio_iodev_sqe_err(iodev_sqe, -EINVAL); + return; + } + data->stream.iodev_sqe = iodev_sqe; + + if (atomic_cas(&data->stream.state, BMI08X_STREAM_OFF, BMI08X_STREAM_ON) == true) { + ret = start_stream(dev); + if (ret != 0) { + LOG_ERR("Failed to configure stream"); + bmi08x_stream_result(dev, ret); + return; + } + } + (void)gpio_add_callback(cfg->int_gpio.port, &data->gpio_cb); + (void)gpio_pin_interrupt_configure_dt(&cfg->int_gpio, GPIO_INT_LEVEL_HIGH); +} + +int bmi08x_gyro_stream_init(const struct device *dev) +{ + struct bmi08x_gyro_data *data = dev->data; + const struct bmi08x_gyro_config *cfg = dev->config; + int ret; + + if (!gpio_is_ready_dt(&cfg->int_gpio)) { + LOG_ERR("GPIO device not ready: %p - dev: %p", &cfg->int_gpio, dev); + return -ENODEV; + } + ret = gpio_pin_configure_dt(&cfg->int_gpio, GPIO_INPUT); + if (ret < 0) { + LOG_ERR("Failed to configure GPIO: %d", ret); + return ret; + } + ret = gpio_pin_interrupt_configure_dt(&cfg->int_gpio, GPIO_INT_DISABLE); + if (ret < 0) { + LOG_ERR("Failed to configure GPIO: %d", ret); + return ret; + } + gpio_init_callback(&data->gpio_cb, bmi08x_gyro_gpio_callback, BIT(cfg->int_gpio.pin)); + data->dev = dev; + + return 0; +} diff --git a/drivers/sensor/bosch/bmi08x/bmi08x_gyro_stream.h b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_stream.h new file mode 100644 index 0000000000000..676e008544966 --- /dev/null +++ b/drivers/sensor/bosch/bmi08x/bmi08x_gyro_stream.h @@ -0,0 +1,17 @@ +/* + * Copyright (c) 2025 Croxel, Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_STREAM_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_STREAM_H_ + +#include +#include + +int bmi08x_gyro_stream_init(const struct device *dev); +void bmi08x_gyro_stream_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMI08X_BMI08X_GYRO_STREAM_H_ */ diff --git a/dts/bindings/sensor/bosch,bmi08x-accel.yaml b/dts/bindings/sensor/bosch,bmi08x-accel.yaml index 2bd311df0f532..3aa9c85014268 100644 --- a/dts/bindings/sensor/bosch,bmi08x-accel.yaml +++ b/dts/bindings/sensor/bosch,bmi08x-accel.yaml @@ -82,3 +82,8 @@ properties: description: | Enables data sync if defined. This is to point to the bmi08x-gyro definition that is within the same IC as the bmi08x-accel. + + fifo-watermark: + type: int + description: | + FIFO Watermark in number of samples. Ranges between 1 and 99 diff --git a/dts/bindings/sensor/bosch,bmi08x-gyro.yaml b/dts/bindings/sensor/bosch,bmi08x-gyro.yaml index 0a3086d8417c7..7ffb1ecbe23a3 100644 --- a/dts/bindings/sensor/bosch,bmi08x-gyro.yaml +++ b/dts/bindings/sensor/bosch,bmi08x-gyro.yaml @@ -60,3 +60,8 @@ properties: type: boolean description: | Enables data sync if defined. Must be set if bmi08x-accel data-sync is set as well. + + fifo-watermark: + type: int + description: | + FIFO Watermark in number of frames. Ranges between 1 and 99 diff --git a/include/zephyr/drivers/sensor_clock.h b/include/zephyr/drivers/sensor_clock.h index 8a67ce0a3c964..b3db59bf2610a 100644 --- a/include/zephyr/drivers/sensor_clock.h +++ b/include/zephyr/drivers/sensor_clock.h @@ -38,6 +38,25 @@ int sensor_clock_get_cycles(uint64_t *cycles); */ uint64_t sensor_clock_cycles_to_ns(uint64_t cycles); +/** + * @brief Get Current sensor clock in nanoseconds. + * + * This helper function performs the acquisition of sensor time in cycles + * and converts it directly in nanoseconds, instead of having to do it in a + * two-step process. + * + * @return nanoseconds on succes, zero on failure. +*/ +static inline uint64_t sensor_clock_get_ns(void) +{ + uint64_t cycles; + + if (sensor_clock_get_cycles(&cycles) != 0) { + return 0; + } + return sensor_clock_cycles_to_ns(cycles); +} + #ifdef __cplusplus } #endif