diff --git a/drivers/sensor/bosch/bmm350/CMakeLists.txt b/drivers/sensor/bosch/bmm350/CMakeLists.txt index 52f637ae8f7..e91f08ab51e 100644 --- a/drivers/sensor/bosch/bmm350/CMakeLists.txt +++ b/drivers/sensor/bosch/bmm350/CMakeLists.txt @@ -9,3 +9,4 @@ zephyr_library_sources( bmm350_decoder.c ) zephyr_library_sources_ifdef(CONFIG_BMM350_TRIGGER bmm350_trigger.c) +zephyr_library_sources_ifdef(CONFIG_BMM350_STREAM bmm350_stream.c) diff --git a/drivers/sensor/bosch/bmm350/Kconfig b/drivers/sensor/bosch/bmm350/Kconfig index 5a666c64ac8..0b4b8e44e08 100644 --- a/drivers/sensor/bosch/bmm350/Kconfig +++ b/drivers/sensor/bosch/bmm350/Kconfig @@ -20,4 +20,12 @@ thread_priority = 10 thread_stack_size = 1024 source "drivers/sensor/Kconfig.trigger_template" +config BMM350_STREAM + bool "Streaming mode" + default y + depends on SENSOR_ASYNC_API + depends on GPIO + depends on $(dt_compat_any_has_prop,$(DT_COMPAT_BOSCH_BMM350),drdy-gpios) + depends on !BMM350_TRIGGER + endif # BMM350 diff --git a/drivers/sensor/bosch/bmm350/bmm350.c b/drivers/sensor/bosch/bmm350/bmm350.c index 821857a552a..337f4f4e2cc 100644 --- a/drivers/sensor/bosch/bmm350/bmm350.c +++ b/drivers/sensor/bosch/bmm350/bmm350.c @@ -17,6 +17,7 @@ #include "bmm350.h" #include "bmm350_decoder.h" +#include "bmm350_stream.h" LOG_MODULE_REGISTER(BMM350, CONFIG_SENSOR_LOG_LEVEL); @@ -823,6 +824,8 @@ static void bmm350_submit(const struct device *dev, struct rtio_iodev_sqe *iodev if (!cfg->is_streaming) { bmm350_submit_one_shot(dev, iodev_sqe); + } else if (IS_ENABLED(CONFIG_BMM350_STREAM)) { + bmm350_stream_submit(dev, iodev_sqe); } else { LOG_ERR("Streaming mode not supported"); rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP); @@ -976,7 +979,12 @@ static int bmm350_init(const struct device *dev) return -EINVAL; } #endif - +#ifdef CONFIG_BMM350_STREAM + if (bmm350_stream_init(dev) < 0) { + LOG_ERR("Cannot set up streaming mode."); + return -EINVAL; + } +#endif /* Initialize to odr and osr */ if (set_mag_odr_osr(dev, &odr, &osr) < 0) { LOG_ERR("failed to set default odr and osr"); @@ -1009,7 +1017,9 @@ static int bmm350_init(const struct device *dev) .default_odr = DT_INST_ENUM_IDX(inst, odr) + BMM350_DATA_RATE_400HZ, \ .default_osr = DT_INST_PROP(inst, osr), \ .drive_strength = DT_INST_PROP(inst, drive_strength), \ - IF_ENABLED(CONFIG_BMM350_TRIGGER, (BMM350_INT_CFG(inst)))}; \ + IF_ENABLED(CONFIG_BMM350_TRIGGER, (BMM350_INT_CFG(inst))) \ + IF_ENABLED(CONFIG_BMM350_STREAM, (BMM350_INT_CFG(inst))) \ + }; \ \ PM_DEVICE_DT_INST_DEFINE(inst, pm_action); \ \ diff --git a/drivers/sensor/bosch/bmm350/bmm350.h b/drivers/sensor/bosch/bmm350/bmm350.h index 8abfb1552a0..1ac6f682fa5 100644 --- a/drivers/sensor/bosch/bmm350/bmm350.h +++ b/drivers/sensor/bosch/bmm350/bmm350.h @@ -480,15 +480,20 @@ struct bmm350_encoded_data { struct bmm350_config { struct bmm350_bus bus; const struct bmm350_bus_io *bus_io; -#ifdef CONFIG_BMM350_TRIGGER struct gpio_dt_spec drdy_int; uint8_t int_flags; -#endif uint8_t default_odr; uint8_t default_osr; uint8_t drive_strength; }; +struct bmm350_stream { + atomic_t state; + const struct device *dev; + struct gpio_callback cb; + struct rtio_iodev_sqe *iodev_sqe; +}; + struct bmm350_data { struct mag_compensate mag_comp; @@ -500,6 +505,10 @@ struct bmm350_data { uint8_t enable_auto_br; struct bmm350_mag_temp_data mag_temp_data; +#ifdef CONFIG_BMM350_STREAM + struct bmm350_stream stream; +#endif + #ifdef CONFIG_BMM350_TRIGGER struct gpio_callback gpio_cb; #endif @@ -519,7 +528,7 @@ struct bmm350_data { #ifdef CONFIG_BMM350_TRIGGER const struct sensor_trigger *drdy_trigger; sensor_trigger_handler_t drdy_handler; -#endif /* CONFIG_BMM350_TRIGGER */ +#endif }; int bmm350_trigger_mode_init(const struct device *dev); diff --git a/drivers/sensor/bosch/bmm350/bmm350_stream.c b/drivers/sensor/bosch/bmm350/bmm350_stream.c new file mode 100644 index 00000000000..c23b4d9e724 --- /dev/null +++ b/drivers/sensor/bosch/bmm350/bmm350_stream.c @@ -0,0 +1,220 @@ +/* + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ +#include +#include +#include +#include "bmm350.h" +#include "bmm350_stream.h" +#include "bmm350_decoder.h" + +#include +LOG_MODULE_REGISTER(BMM350_STREAM, CONFIG_SENSOR_LOG_LEVEL); + +enum bmm350_stream_state { + BMM350_STREAM_OFF = 0, + BMM350_STREAM_ON = 1, + BMM350_STREAM_BUSY = 2, +}; + +static void bmm350_stream_event_complete(struct rtio *ctx, const struct rtio_sqe *sqe, void *arg0) +{ + struct rtio_iodev_sqe *iodev_sqe = (struct rtio_iodev_sqe *)arg0; + struct sensor_read_config *cfg = (struct sensor_read_config *)iodev_sqe->sqe.iodev->data; + const struct device *dev = (const struct device *)sqe->userdata; + struct bmm350_data *data = dev->data; + uint8_t *buf; + uint32_t buf_len; + struct rtio_cqe *cqe; + int err = 0; + + do { + cqe = rtio_cqe_consume(ctx); + if (cqe == NULL) { + continue; + } + + /** Keep looping through results until we get the first error. + * Usually this causes the remaining CQEs to result in -ECANCELED. + */ + if (err == 0) { + err = cqe->result; + } + rtio_cqe_release(ctx, cqe); + } while (cqe != NULL); + + if (err != 0) { + goto bmm350_stream_evt_finish; + } + + /* We've allocated the data already, just grab the pointer to fill comp-data + * now that the bus transfer is complete. + */ + err = rtio_sqe_rx_buf(iodev_sqe, 0, 0, &buf, &buf_len); + + CHECKIF(err != 0 || !buf || buf_len < sizeof(struct bmm350_encoded_data)) { + LOG_ERR("Couldn't get encoded buffer on completion"); + err = -EIO; + goto bmm350_stream_evt_finish; + } + + err = bmm350_encode(dev, cfg, true, buf); + if (err != 0) { + LOG_ERR("Failed to encode frame: %d", err); + goto bmm350_stream_evt_finish; + } + +bmm350_stream_evt_finish: + atomic_set(&data->stream.state, BMM350_STREAM_ON); + + if (err < 0) { + rtio_iodev_sqe_err(iodev_sqe, err); + } else { + rtio_iodev_sqe_ok(iodev_sqe, 0); + } + +} + +static void bmm350_event_handler(const struct device *dev) +{ + struct bmm350_data *data = dev->data; + const struct bmm350_config *cfg = dev->config; + struct rtio_iodev_sqe *iodev_sqe = data->stream.iodev_sqe; + uint8_t *buf = NULL; + uint32_t buf_len = 0; + int err; + + CHECKIF(!data->stream.iodev_sqe || + FIELD_GET(RTIO_SQE_CANCELED, iodev_sqe->sqe.flags)) { + + LOG_WRN("Callback triggered with no streaming submission - Disabling interrupts"); + + (void)gpio_pin_interrupt_configure_dt(&cfg->drdy_int, GPIO_INT_DISABLE); + + err = bmm350_prep_reg_write_async(dev, BMM350_REG_INT_CTRL, 0, NULL); + if (err >= 0) { + rtio_submit(cfg->bus.rtio.ctx, 0); + } + + (void)atomic_set(&data->stream.state, BMM350_STREAM_OFF); + + return; + } + + CHECKIF(atomic_cas(&data->stream.state, BMM350_STREAM_ON, BMM350_STREAM_BUSY) == false) { + LOG_WRN("Callback triggered while stream is busy. Ignoring request"); + return; + } + + err = rtio_sqe_rx_buf(iodev_sqe, + sizeof(struct bmm350_encoded_data), + sizeof(struct bmm350_encoded_data), + &buf, &buf_len); + CHECKIF(err != 0 || buf_len < sizeof(struct bmm350_encoded_data)) { + LOG_ERR("Failed to allocate BMM350 encoded buffer: %d", err); + rtio_iodev_sqe_err(iodev_sqe, -ENOMEM); + return; + } + + struct bmm350_encoded_data *edata = (struct bmm350_encoded_data *)buf; + struct rtio_sqe *read_sqe = NULL; + struct rtio_sqe *cb_sqe; + + err = bmm350_prep_reg_read_async(dev, BMM350_REG_MAG_X_XLSB, + edata->payload.buf, sizeof(edata->payload.buf), + &read_sqe); + CHECKIF(err < 0 || !read_sqe) { + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + read_sqe->flags |= RTIO_SQE_CHAINED; + + cb_sqe = rtio_sqe_acquire(cfg->bus.rtio.ctx); + + rtio_sqe_prep_callback_no_cqe(cb_sqe, bmm350_stream_event_complete, + iodev_sqe, (void *)dev); + + rtio_submit(cfg->bus.rtio.ctx, 0); +} + +static void bmm350_gpio_callback(const struct device *port, struct gpio_callback *cb, uint32_t pin) +{ + struct bmm350_stream *stream = CONTAINER_OF(cb, struct bmm350_stream, cb); + const struct device *dev = stream->dev; + + bmm350_event_handler(dev); +} + +void bmm350_stream_submit(const struct device *dev, + struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *read_config = iodev_sqe->sqe.iodev->data; + struct bmm350_data *data = dev->data; + const struct bmm350_config *cfg = dev->config; + int err; + + if ((read_config->count != 1) || + (read_config->triggers[0].trigger != SENSOR_TRIG_DATA_READY)) { + LOG_ERR("Only SENSOR_TRIG_DATA_READY is supported"); + rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP); + return; + } + + data->stream.iodev_sqe = iodev_sqe; + + if (atomic_cas(&data->stream.state, BMM350_STREAM_OFF, BMM350_STREAM_ON)) { + /* Set PMU command configuration */ + err = bmm350_prep_reg_write_async(dev, BMM350_REG_INT_CTRL, cfg->int_flags, NULL); + if (err < 0) { + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + rtio_submit(cfg->bus.rtio.ctx, 0); + + err = gpio_pin_interrupt_configure_dt(&cfg->drdy_int, GPIO_INT_EDGE_TO_ACTIVE); + if (err < 0) { + rtio_iodev_sqe_err(iodev_sqe, err); + return; + } + } +} + +int bmm350_stream_init(const struct device *dev) +{ + struct bmm350_data *data = dev->data; + const struct bmm350_config *cfg = dev->config; + int err; + + /** Needed to get back the device handle from the callback context */ + data->stream.dev = dev; + + (void)atomic_set(&data->stream.state, BMM350_STREAM_OFF); + + if (!device_is_ready(cfg->drdy_int.port)) { + LOG_ERR("INT device is not ready"); + return -ENODEV; + } + + err = gpio_pin_configure_dt(&cfg->drdy_int, GPIO_INPUT); + if (err < 0) { + return err; + } + + gpio_init_callback(&data->stream.cb, bmm350_gpio_callback, BIT(cfg->drdy_int.pin)); + + err = gpio_add_callback(cfg->drdy_int.port, &data->stream.cb); + if (err < 0) { + return err; + } + + err = gpio_pin_interrupt_configure_dt(&cfg->drdy_int, GPIO_INT_DISABLE); + if (err < 0) { + return err; + } + + + return 0; +} diff --git a/drivers/sensor/bosch/bmm350/bmm350_stream.h b/drivers/sensor/bosch/bmm350/bmm350_stream.h new file mode 100644 index 00000000000..ccd3345515d --- /dev/null +++ b/drivers/sensor/bosch/bmm350/bmm350_stream.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_BMM350_STREAM_H_ +#define ZEPHYR_DRIVERS_SENSOR_BMM350_STREAM_H_ + +int bmm350_stream_init(const struct device *dev); + +void bmm350_stream_submit(const struct device *dev, + struct rtio_iodev_sqe *iodev_sqe); + +#endif /* ZEPHYR_DRIVERS_SENSOR_BMM350_STREAM_H_ */