From 8d9313f942df5fe7d1364db0eb0aa8716ba61252 Mon Sep 17 00:00:00 2001 From: Al Semjonovs Date: Mon, 1 May 2023 11:43:43 -0600 Subject: [PATCH] sensors: icm42688 async API Implementation of the async API for the icm42688 Signed-off-by: Al Semjonovs topic#icm42688_async --- drivers/sensor/icm42688/CMakeLists.txt | 2 + drivers/sensor/icm42688/Kconfig | 8 + drivers/sensor/icm42688/icm42688.c | 119 ++++---- drivers/sensor/icm42688/icm42688.h | 12 +- drivers/sensor/icm42688/icm42688_decoder.c | 311 +++++++++++++++++++++ drivers/sensor/icm42688/icm42688_decoder.h | 35 +++ drivers/sensor/icm42688/icm42688_rtio.c | 81 ++++++ drivers/sensor/icm42688/icm42688_rtio.h | 12 + drivers/sensor/icm42688/icm42688_trigger.c | 4 +- 9 files changed, 521 insertions(+), 63 deletions(-) create mode 100644 drivers/sensor/icm42688/icm42688_decoder.c create mode 100644 drivers/sensor/icm42688/icm42688_decoder.h create mode 100644 drivers/sensor/icm42688/icm42688_rtio.c create mode 100644 drivers/sensor/icm42688/icm42688_rtio.h diff --git a/drivers/sensor/icm42688/CMakeLists.txt b/drivers/sensor/icm42688/CMakeLists.txt index 1e9acaa3731a546..cf9308bb95c4478 100644 --- a/drivers/sensor/icm42688/CMakeLists.txt +++ b/drivers/sensor/icm42688/CMakeLists.txt @@ -8,6 +8,8 @@ zephyr_library_sources( icm42688_spi.c ) +zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API icm42688_rtio.c) +zephyr_library_sources_ifdef(CONFIG_ICM42688_DECODER icm42688_decoder.c) zephyr_library_sources_ifdef(CONFIG_ICM42688_TRIGGER icm42688_trigger.c) zephyr_library_sources_ifdef(CONFIG_EMUL_ICM42688 icm42688_emul.c) zephyr_include_directories_ifdef(CONFIG_EMUL_ICM42688 .) diff --git a/drivers/sensor/icm42688/Kconfig b/drivers/sensor/icm42688/Kconfig index 5cdfb3cdc1eef87..23812c66d25dd59 100644 --- a/drivers/sensor/icm42688/Kconfig +++ b/drivers/sensor/icm42688/Kconfig @@ -21,10 +21,18 @@ config EMUL_ICM42688 Enable the hardware emulator for the ICM42688. Doing so allows exercising sensor APIs for this IMU in native_posix and qemu. +config ICM42688_DECODER + bool "ICM42688 decoder logic" + default y if ICM42688 + help + Compile the ICM42688 decoder API which allows decoding raw data returned + from the sensor. + if ICM42688 choice prompt "Trigger mode" + default ICM42688_TRIGGER_NONE if ICM42688_STREAM default ICM42688_TRIGGER_GLOBAL_THREAD help Specify the type of triggering to be used by the driver diff --git a/drivers/sensor/icm42688/icm42688.c b/drivers/sensor/icm42688/icm42688.c index 329907a010d9343..de7780698278777 100644 --- a/drivers/sensor/icm42688/icm42688.c +++ b/drivers/sensor/icm42688/icm42688.c @@ -13,23 +13,15 @@ #include #include "icm42688.h" +#include "icm42688_decoder.h" #include "icm42688_reg.h" +#include "icm42688_rtio.h" #include "icm42688_spi.h" #include "icm42688_trigger.h" #include LOG_MODULE_REGISTER(ICM42688, CONFIG_SENSOR_LOG_LEVEL); -struct icm42688_sensor_data { - struct icm42688_dev_data dev_data; - - int16_t readings[7]; -}; - -struct icm42688_sensor_config { - struct icm42688_dev_cfg dev_cfg; -}; - static void icm42688_convert_accel(struct sensor_value *val, int16_t raw_val, struct icm42688_cfg *cfg) { @@ -47,42 +39,40 @@ static inline void icm42688_convert_temp(struct sensor_value *val, int16_t raw_v icm42688_temp_c((int32_t)raw_val, &val->val1, &val->val2); } -static int icm42688_channel_get(const struct device *dev, enum sensor_channel chan, - struct sensor_value *val) +int icm42688_channel_parse_readings(enum sensor_channel chan, int16_t readings[7], + struct icm42688_cfg *cfg, struct sensor_value *val) { - struct icm42688_sensor_data *data = dev->data; - switch (chan) { case SENSOR_CHAN_ACCEL_XYZ: - icm42688_convert_accel(&val[0], data->readings[1], &data->dev_data.cfg); - icm42688_convert_accel(&val[1], data->readings[2], &data->dev_data.cfg); - icm42688_convert_accel(&val[2], data->readings[3], &data->dev_data.cfg); + icm42688_convert_accel(&val[0], readings[1], cfg); + icm42688_convert_accel(&val[1], readings[2], cfg); + icm42688_convert_accel(&val[2], readings[3], cfg); break; case SENSOR_CHAN_ACCEL_X: - icm42688_convert_accel(val, data->readings[1], &data->dev_data.cfg); + icm42688_convert_accel(val, readings[1], cfg); break; case SENSOR_CHAN_ACCEL_Y: - icm42688_convert_accel(val, data->readings[2], &data->dev_data.cfg); + icm42688_convert_accel(val, readings[2], cfg); break; case SENSOR_CHAN_ACCEL_Z: - icm42688_convert_accel(val, data->readings[3], &data->dev_data.cfg); + icm42688_convert_accel(val, readings[3], cfg); break; case SENSOR_CHAN_GYRO_XYZ: - icm42688_convert_gyro(&val[0], data->readings[4], &data->dev_data.cfg); - icm42688_convert_gyro(&val[1], data->readings[5], &data->dev_data.cfg); - icm42688_convert_gyro(&val[2], data->readings[6], &data->dev_data.cfg); + icm42688_convert_gyro(&val[0], readings[4], cfg); + icm42688_convert_gyro(&val[1], readings[5], cfg); + icm42688_convert_gyro(&val[2], readings[6], cfg); break; case SENSOR_CHAN_GYRO_X: - icm42688_convert_gyro(val, data->readings[4], &data->dev_data.cfg); + icm42688_convert_gyro(val, readings[4], cfg); break; case SENSOR_CHAN_GYRO_Y: - icm42688_convert_gyro(val, data->readings[5], &data->dev_data.cfg); + icm42688_convert_gyro(val, readings[5], cfg); break; case SENSOR_CHAN_GYRO_Z: - icm42688_convert_gyro(val, data->readings[6], &data->dev_data.cfg); + icm42688_convert_gyro(val, readings[6], cfg); break; case SENSOR_CHAN_DIE_TEMP: - icm42688_convert_temp(val, data->readings[0]); + icm42688_convert_temp(val, readings[0]); break; default: return -ENOTSUP; @@ -91,13 +81,21 @@ static int icm42688_channel_get(const struct device *dev, enum sensor_channel ch return 0; } +static int icm42688_channel_get(const struct device *dev, enum sensor_channel chan, + struct sensor_value *val) +{ + struct icm42688_dev_data *data = dev->data; + + return icm42688_channel_parse_readings(chan, data->readings, &data->cfg, val); +} + static int icm42688_sample_fetch(const struct device *dev, enum sensor_channel chan) { uint8_t status; - struct icm42688_sensor_data *data = dev->data; - const struct icm42688_sensor_config *cfg = dev->config; + struct icm42688_dev_data *data = dev->data; + const struct icm42688_dev_cfg *cfg = dev->config; - int res = icm42688_spi_read(&cfg->dev_cfg.spi, REG_INT_STATUS, &status, 1); + int res = icm42688_spi_read(&cfg->spi, REG_INT_STATUS, &status, 1); if (res) { return res; @@ -125,8 +123,8 @@ static int icm42688_sample_fetch(const struct device *dev, enum sensor_channel c static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr, const struct sensor_value *val) { - const struct icm42688_sensor_data *data = dev->data; - struct icm42688_cfg new_config = data->dev_data.cfg; + const struct icm42688_dev_data *data = dev->data; + struct icm42688_cfg new_config = data->cfg; int res = 0; __ASSERT_NO_MSG(val != NULL); @@ -173,8 +171,8 @@ static int icm42688_attr_set(const struct device *dev, enum sensor_channel chan, static int icm42688_attr_get(const struct device *dev, enum sensor_channel chan, enum sensor_attribute attr, struct sensor_value *val) { - const struct icm42688_sensor_data *data = dev->data; - const struct icm42688_cfg *cfg = &data->dev_data.cfg; + const struct icm42688_dev_data *data = dev->data; + const struct icm42688_cfg *cfg = &data->cfg; int res = 0; __ASSERT_NO_MSG(val != NULL); @@ -223,15 +221,17 @@ static const struct sensor_driver_api icm42688_driver_api = { #ifdef CONFIG_ICM42688_TRIGGER .trigger_set = icm42688_trigger_set, #endif + .get_decoder = icm42688_get_decoder, + .submit = icm42688_submit, }; int icm42688_init(const struct device *dev) { - struct icm42688_sensor_data *data = dev->data; - const struct icm42688_sensor_config *cfg = dev->config; + struct icm42688_dev_data *data = dev->data; + const struct icm42688_dev_cfg *cfg = dev->config; int res; - if (!spi_is_ready_dt(&cfg->dev_cfg.spi)) { + if (!spi_is_ready_dt(&cfg->spi)) { LOG_ERR("SPI bus is not ready"); return -ENODEV; } @@ -247,22 +247,18 @@ int icm42688_init(const struct device *dev) LOG_ERR("Failed to initialize triggers"); return res; } - - res = icm42688_trigger_enable_interrupt(dev); - if (res != 0) { - LOG_ERR("Failed to enable triggers"); - return res; - } #endif - data->dev_data.cfg.accel_mode = ICM42688_ACCEL_LN; - data->dev_data.cfg.gyro_mode = ICM42688_GYRO_LN; - data->dev_data.cfg.accel_fs = ICM42688_ACCEL_FS_2G; - data->dev_data.cfg.gyro_fs = ICM42688_GYRO_FS_125; - data->dev_data.cfg.accel_odr = ICM42688_ACCEL_ODR_1000; - data->dev_data.cfg.gyro_odr = ICM42688_GYRO_ODR_1000; + memset(&data->cfg, 0, sizeof(struct icm42688_cfg)); + data->cfg.accel_mode = ICM42688_ACCEL_LN; + data->cfg.gyro_mode = ICM42688_GYRO_LN; + data->cfg.accel_fs = ICM42688_ACCEL_FS_2G; + data->cfg.gyro_fs = ICM42688_GYRO_FS_125; + data->cfg.accel_odr = ICM42688_ACCEL_ODR_1000; + data->cfg.gyro_odr = ICM42688_GYRO_ODR_1000; + data->cfg.fifo_en = IS_ENABLED(CONFIG_ICM42688_STREAM); - res = icm42688_configure(dev, &data->dev_data.cfg); + res = icm42688_configure(dev, &data->cfg); if (res != 0) { LOG_ERR("Failed to configure"); return res; @@ -286,16 +282,25 @@ void icm42688_unlock(const struct device *dev) #define ICM42688_SPI_CFG \ SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_TRANSFER_MSB +#define ICM42688_RTIO_DEFINE(inst) \ + SPI_DT_IODEV_DEFINE(icm42688_spi_iodev_##inst, DT_DRV_INST(inst), ICM42688_SPI_CFG, 0U); \ + RTIO_DEFINE(icm42688_rtio_##inst, 8, 4); + +#define ICM42688_DEFINE_DATA(inst) \ + IF_ENABLED(CONFIG_ICM42688_STREAM, (ICM42688_RTIO_DEFINE(inst))); \ + static struct icm42688_dev_data icm42688_driver_##inst = { \ + IF_ENABLED(CONFIG_ICM42688_STREAM, ( \ + .r = &icm42688_rtio_##inst, \ + .spi_iodev = &icm42688_spi_iodev_##inst, \ + )) \ + }; #define ICM42688_INIT(inst) \ - static struct icm42688_sensor_data icm42688_driver_##inst = { 0 }; \ + ICM42688_DEFINE_DATA(inst); \ \ - static const struct icm42688_sensor_config icm42688_cfg_##inst = { \ - .dev_cfg = \ - { \ - .spi = SPI_DT_SPEC_INST_GET(inst, ICM42688_SPI_CFG, 0U), \ - .gpio_int1 = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ - }, \ + static const struct icm42688_dev_cfg icm42688_cfg_##inst = { \ + .spi = SPI_DT_SPEC_INST_GET(inst, ICM42688_SPI_CFG, 0U), \ + .gpio_int1 = GPIO_DT_SPEC_INST_GET_OR(inst, int_gpios, {0}), \ }; \ \ SENSOR_DEVICE_DT_INST_DEFINE(inst, icm42688_init, NULL, &icm42688_driver_##inst, \ diff --git a/drivers/sensor/icm42688/icm42688.h b/drivers/sensor/icm42688/icm42688.h index ac7c531c99d6846..18ae4606e72593a 100644 --- a/drivers/sensor/icm42688/icm42688.h +++ b/drivers/sensor/icm42688/icm42688.h @@ -411,6 +411,8 @@ struct icm42688_dev_data { const struct sensor_trigger *data_ready_trigger; struct k_mutex mutex; #endif /* CONFIG_ICM42688_TRIGGER */ + + int16_t readings[7]; }; /** @@ -515,7 +517,7 @@ static inline void icm42688_accel_g(struct icm42688_cfg *cfg, int32_t in, int32_ * @param out_dps whole deg/s output in int32_t * @param out_udps micro (1/1000000) deg/s as uint32_t */ -static inline void icm42688_gyro_dps(struct icm42688_cfg *cfg, int32_t in, int32_t *out_dps, +static inline void icm42688_gyro_dps(const struct icm42688_cfg *cfg, int32_t in, int32_t *out_dps, uint32_t *out_udps) { int64_t sensitivity = 0; /* value equivalent for 10x gyro reading deg/s */ @@ -565,8 +567,8 @@ static inline void icm42688_gyro_dps(struct icm42688_cfg *cfg, int32_t in, int32 * @param out_ms meters/s^2 (whole) output in int32_t * @param out_ums micrometers/s^2 output as uint32_t */ -static inline void icm42688_accel_ms(struct icm42688_cfg *cfg, int32_t in, int32_t *out_ms, - uint32_t *out_ums) +static inline void icm42688_accel_ms(const struct icm42688_cfg *cfg, int32_t in, int32_t *out_ms, + int32_t *out_ums) { int64_t sensitivity = 0; /* value equivalent for 1g */ @@ -603,8 +605,8 @@ static inline void icm42688_accel_ms(struct icm42688_cfg *cfg, int32_t in, int32 * @param out_rads whole rad/s output in int32_t * @param out_urads microrad/s as uint32_t */ -static inline void icm42688_gyro_rads(struct icm42688_cfg *cfg, int32_t in, int32_t *out_rads, - uint32_t *out_urads) +static inline void icm42688_gyro_rads(const struct icm42688_cfg *cfg, int32_t in, int32_t *out_rads, + int32_t *out_urads) { int64_t sensitivity = 0; /* value equivalent for 10x gyro reading deg/s */ diff --git a/drivers/sensor/icm42688/icm42688_decoder.c b/drivers/sensor/icm42688/icm42688_decoder.c new file mode 100644 index 000000000000000..582e7ac52b1e9dc --- /dev/null +++ b/drivers/sensor/icm42688/icm42688_decoder.c @@ -0,0 +1,311 @@ +/* + * Copyright (c) 2023 Google LLC + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "icm42688_decoder.h" +#include "icm42688_reg.h" +#include "icm42688.h" +#include + +#include +LOG_MODULE_REGISTER(ICM42688_DECODER, CONFIG_SENSOR_LOG_LEVEL); + +#define DT_DRV_COMPAT invensense_icm42688 + +static int icm42688_get_shift(enum sensor_channel channel, int accel_fs, int gyro_fs, int8_t *shift) +{ + switch (channel) { + case SENSOR_CHAN_ACCEL_XYZ: + case SENSOR_CHAN_ACCEL_X: + case SENSOR_CHAN_ACCEL_Y: + case SENSOR_CHAN_ACCEL_Z: + switch (accel_fs) { + case ICM42688_ACCEL_FS_2G: + *shift = 5; + return 0; + case ICM42688_ACCEL_FS_4G: + *shift = 6; + return 0; + case ICM42688_ACCEL_FS_8G: + *shift = 7; + return 0; + case ICM42688_ACCEL_FS_16G: + *shift = 8; + return 0; + default: + return -EINVAL; + } + case SENSOR_CHAN_GYRO_XYZ: + case SENSOR_CHAN_GYRO_X: + case SENSOR_CHAN_GYRO_Y: + case SENSOR_CHAN_GYRO_Z: + switch (gyro_fs) { + case ICM42688_GYRO_FS_15_625: + *shift = -1; + return 0; + case ICM42688_GYRO_FS_31_25: + *shift = 0; + return 0; + case ICM42688_GYRO_FS_62_5: + *shift = 1; + return 0; + case ICM42688_GYRO_FS_125: + *shift = 2; + return 0; + case ICM42688_GYRO_FS_250: + *shift = 3; + return 0; + case ICM42688_GYRO_FS_500: + *shift = 4; + return 0; + case ICM42688_GYRO_FS_1000: + *shift = 5; + return 0; + case ICM42688_GYRO_FS_2000: + *shift = 6; + return 0; + default: + return -EINVAL; + } + case SENSOR_CHAN_DIE_TEMP: + *shift = 9; + return 0; + default: + return -EINVAL; + } +} + +static enum sensor_channel icm42688_get_channel_from_position(int pos) +{ + switch (pos) { + case 0: + return SENSOR_CHAN_DIE_TEMP; + case 1: + return SENSOR_CHAN_ACCEL_X; + case 2: + return SENSOR_CHAN_ACCEL_Y; + case 3: + return SENSOR_CHAN_ACCEL_Z; + case 4: + return SENSOR_CHAN_GYRO_X; + case 5: + return SENSOR_CHAN_GYRO_Y; + case 6: + return SENSOR_CHAN_GYRO_Z; + default: + return SENSOR_CHAN_MAX; + } +} + +int icm42688_convert_raw_to_q31(struct icm42688_cfg *cfg, enum sensor_channel chan, + int32_t reading, q31_t *out) +{ + int32_t whole; + int32_t fraction; + int64_t intermediate; + int8_t shift; + int rc; + + rc = icm42688_get_shift(chan, cfg->accel_fs, cfg->gyro_fs, &shift); + if (rc != 0) { + return rc; + } + + switch (chan) { + case SENSOR_CHAN_ACCEL_XYZ: + case SENSOR_CHAN_ACCEL_X: + case SENSOR_CHAN_ACCEL_Y: + case SENSOR_CHAN_ACCEL_Z: + icm42688_accel_ms(cfg, reading, &whole, &fraction); + break; + case SENSOR_CHAN_GYRO_XYZ: + case SENSOR_CHAN_GYRO_X: + case SENSOR_CHAN_GYRO_Y: + case SENSOR_CHAN_GYRO_Z: + icm42688_gyro_rads(cfg, reading, &whole, &fraction); + break; + case SENSOR_CHAN_DIE_TEMP: + icm42688_temp_c(reading, &whole, &fraction); + break; + default: + return -ENOTSUP; + } + intermediate = ((int64_t)whole * INT64_C(1000000) + fraction); + if (shift < 0) { + intermediate = intermediate * INT32_MAX * (1 << -shift) / INT64_C(1000000); + } else if (shift > 0) { + intermediate = intermediate * INT32_MAX / (((1 << shift) - 1) * INT64_C(1000000)); + } + *out = CLAMP(intermediate, INT32_MIN, INT32_MAX); + + return 0; +} + +static int icm42688_get_channel_position(enum sensor_channel chan) +{ + switch (chan) { + case SENSOR_CHAN_DIE_TEMP: + return 0; + case SENSOR_CHAN_ACCEL_XYZ: + case SENSOR_CHAN_ACCEL_X: + return 1; + case SENSOR_CHAN_ACCEL_Y: + return 2; + case SENSOR_CHAN_ACCEL_Z: + return 3; + case SENSOR_CHAN_GYRO_XYZ: + case SENSOR_CHAN_GYRO_X: + return 4; + case SENSOR_CHAN_GYRO_Y: + return 5; + case SENSOR_CHAN_GYRO_Z: + return 6; + default: + return 0; + } +} + +static uint8_t icm42688_encode_channel(enum sensor_channel chan) +{ + uint8_t encode_bmask = 0; + + switch (chan) { + case SENSOR_CHAN_DIE_TEMP: + case SENSOR_CHAN_ACCEL_X: + case SENSOR_CHAN_ACCEL_Y: + case SENSOR_CHAN_ACCEL_Z: + case SENSOR_CHAN_GYRO_X: + case SENSOR_CHAN_GYRO_Y: + case SENSOR_CHAN_GYRO_Z: + encode_bmask = BIT(icm42688_get_channel_position(chan)); + break; + case SENSOR_CHAN_ACCEL_XYZ: + case SENSOR_CHAN_GYRO_XYZ: + encode_bmask = BIT(icm42688_get_channel_position(chan - 3)) | + BIT(icm42688_get_channel_position(chan - 2)) | + BIT(icm42688_get_channel_position(chan - 1)); + break; + default: + break; + } + + return encode_bmask; +} + +int icm42688_encode(const struct device *dev, const enum sensor_channel *const channels, + const size_t num_channels, uint8_t *buf) +{ + struct icm42688_dev_data *data = dev->data; + struct icm42688_encoded_data *edata = (struct icm42688_encoded_data *)buf; + + edata->channels = 0; + + for (int i = 0; i < num_channels; i++) { + edata->channels |= icm42688_encode_channel(channels[i]); + } + + edata->header.is_fifo = false; + edata->header.accel_fs = data->cfg.accel_fs; + edata->header.gyro_fs = data->cfg.gyro_fs; + edata->header.timestamp = k_ticks_to_ns_floor64(k_uptime_ticks()); + + return 0; +} + +static int icm42688_one_shot_decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, + q31_t *values, uint8_t max_count) +{ + const struct icm42688_encoded_data *edata = (const struct icm42688_encoded_data *)buffer; + uint8_t channels_read = edata->channels; + struct icm42688_cfg cfg = { + .accel_fs = edata->header.accel_fs, + .gyro_fs = edata->header.gyro_fs, + }; + int chan; + int pos; + int count = 0; + int num_samples = __builtin_popcount(channels_read); + + channels_read = edata->channels; + + if (*fit != 0) { + return 0; + } + + /* Skip channels already decoded */ + for (int i = 0; i < *cit && channels_read; i++) { + chan = __builtin_ctz(channels_read); + channels_read &= ~BIT(chan); + } + + /* Decode remaining channels */ + while (channels_read && *cit < num_samples && count < max_count) { + chan = icm42688_get_channel_from_position(__builtin_ctz(channels_read)); + + channels[count] = chan; + pos = icm42688_get_channel_position(chan); + + icm42688_convert_raw_to_q31(&cfg, chan, edata->readings[pos], &values[count]); + + count++; + channels_read &= ~BIT(chan); + *cit += 1; + } + + if (*cit >= __builtin_popcount(edata->channels)) { + *fit += 1; + *cit = 0; + } + + return count; +} + +static int icm42688_decoder_decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, + q31_t *values, uint8_t max_count) +{ + return icm42688_one_shot_decode(buffer, fit, cit, channels, values, max_count); +} + +static int icm42688_decoder_get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + ARG_UNUSED(buffer); + *frame_count = 1; + return 0; +} + +static int icm42688_decoder_get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + const struct icm42688_decoder_header *header = + (const struct icm42688_decoder_header *)buffer; + + *timestamp_ns = header->timestamp; + return 0; +} + +static int icm42688_decoder_get_shift(const uint8_t *buffer, enum sensor_channel channel_type, + int8_t *shift) +{ + const struct icm42688_decoder_header *header = + (const struct icm42688_decoder_header *)buffer; + + return icm42688_get_shift(channel_type, header->accel_fs, header->gyro_fs, shift); +} + +SENSOR_DECODER_API_DT_DEFINE() = { + .get_frame_count = icm42688_decoder_get_frame_count, + .get_timestamp = icm42688_decoder_get_timestamp, + .get_shift = icm42688_decoder_get_shift, + .decode = icm42688_decoder_decode, +}; + +int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder) +{ + ARG_UNUSED(dev); + *decoder = &SENSOR_DECODER_NAME(); + + return 0; +} diff --git a/drivers/sensor/icm42688/icm42688_decoder.h b/drivers/sensor/icm42688/icm42688_decoder.h new file mode 100644 index 000000000000000..402ba82d3d59fa4 --- /dev/null +++ b/drivers/sensor/icm42688/icm42688_decoder.h @@ -0,0 +1,35 @@ +/* + * Copyright (c) 2023 Google LLC + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_ICM42688_DECODER_H_ +#define ZEPHYR_DRIVERS_SENSOR_ICM42688_DECODER_H_ + +#include +#include + +struct icm42688_decoder_header { + uint8_t is_fifo: 1; + uint8_t gyro_fs: 3; + uint8_t accel_fs: 2; + uint8_t reserved: 2; + uint64_t timestamp; +} __attribute__((__packed__)); + +struct icm42688_encoded_data { + struct icm42688_decoder_header header; + struct { + uint16_t channels: 7; + uint16_t reserved: 1; + } __attribute__((__packed__)); + int16_t readings[7]; +}; + +int icm42688_encode(const struct device *dev, const enum sensor_channel *const channels, + const size_t num_channels, uint8_t *buf); + +int icm42688_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder); + +#endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_DECODER_H_ */ diff --git a/drivers/sensor/icm42688/icm42688_rtio.c b/drivers/sensor/icm42688/icm42688_rtio.c new file mode 100644 index 000000000000000..c1b20689393d6c0 --- /dev/null +++ b/drivers/sensor/icm42688/icm42688_rtio.c @@ -0,0 +1,81 @@ +/* + * Copyright (c) 2023 Google LLC + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include "icm42688.h" +#include "icm42688_decoder.h" +#include "icm42688_reg.h" +#include "icm42688_spi.h" + +#include +LOG_MODULE_REGISTER(ICM42688_RTIO, CONFIG_SENSOR_LOG_LEVEL); + +static int icm42688_rtio_sample_fetch(const struct device *dev, int16_t readings[7]) +{ + uint8_t status; + const struct icm42688_dev_cfg *cfg = dev->config; + uint8_t *buffer = (uint8_t *)readings; + + int res = icm42688_spi_read(&cfg->spi, REG_INT_STATUS, &status, 1); + + if (res) { + return res; + } + + if (!FIELD_GET(BIT_INT_STATUS_DATA_RDY, status)) { + return -EBUSY; + } + + res = icm42688_read_all(dev, buffer); + + if (res) { + return res; + } + + for (int i = 0; i < 7; i++) { + readings[i] = sys_le16_to_cpu((buffer[i * 2] << 8) | buffer[i * 2 + 1]); + } + + return 0; +} + +int icm42688_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data; + const enum sensor_channel *const channels = cfg->channels; + const size_t num_channels = cfg->count; + uint32_t min_buf_len = sizeof(struct icm42688_encoded_data); + int rc; + uint8_t *buf; + uint32_t buf_len; + struct icm42688_encoded_data *edata; + + /* Get the buffer for the frame, it may be allocated dynamically by the rtio context */ + rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len); + if (rc != 0) { + LOG_ERR("Failed to get a read buffer of size %u bytes", min_buf_len); + rtio_iodev_sqe_err(iodev_sqe, rc); + return rc; + } + + edata = (struct icm42688_encoded_data *)buf; + + icm42688_encode(dev, channels, num_channels, buf); + + rc = icm42688_rtio_sample_fetch(dev, edata->readings); + /* Check that the fetch succeeded */ + if (rc != 0) { + LOG_ERR("Failed to fetch samples"); + rtio_iodev_sqe_err(iodev_sqe, rc); + return rc; + } + + rtio_iodev_sqe_ok(iodev_sqe, 0); + + return 0; +} + +BUILD_ASSERT(sizeof(struct icm42688_decoder_header) == 9); diff --git a/drivers/sensor/icm42688/icm42688_rtio.h b/drivers/sensor/icm42688/icm42688_rtio.h new file mode 100644 index 000000000000000..748c8a022b09238 --- /dev/null +++ b/drivers/sensor/icm42688/icm42688_rtio.h @@ -0,0 +1,12 @@ +/* + * Copyright (c) 2023 Google LLC + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_ +#define ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_ + +int icm42688_submit(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe); + +#endif /* ZEPHYR_DRIVERS_SENSOR_ICM42688_RTIO_H_ */ diff --git a/drivers/sensor/icm42688/icm42688_trigger.c b/drivers/sensor/icm42688/icm42688_trigger.c index 4bf728b40545a87..adaecf84baa6566 100644 --- a/drivers/sensor/icm42688/icm42688_trigger.c +++ b/drivers/sensor/icm42688/icm42688_trigger.c @@ -32,6 +32,7 @@ static void icm42688_gpio_callback(const struct device *dev, struct gpio_callbac #endif } +#if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) || defined(CONFIG_ICM42688_TRIGGER_GLOBAL_THREAD) static void icm42688_thread_cb(const struct device *dev) { struct icm42688_dev_data *data = dev->data; @@ -44,8 +45,9 @@ static void icm42688_thread_cb(const struct device *dev) icm42688_unlock(dev); } +#endif -#if defined(CONFIG_ICM42688_TRIGGER_OWN_THREAD) +#ifdef CONFIG_ICM42688_TRIGGER_OWN_THREAD static void icm42688_thread(void *p1, void *p2, void *p3) {