drivers/lis2dux12: add read_and_decode APIs support
Add RTIO async and RTIO stream functionalities that enables, among all the other things, the sensor data streaming from FIFO. RTIO stream supports following triggers: - SENSOR_TRIG_FIFO_WATERMARK - SENSOR_TRIG_FIFO_FULL - SENSOR_TRIG_DATA_READY Following FIFO parameters has to be defined in device tree to correctly stream sensor data: - fifo-watermark - accel-fifo-batch-rate Currently the driver can decode FIFO content with Accelerometer 16-bit samples. Signed-off-by: Armando Visconti <armando.visconti@st.com>
This commit is contained in:
parent
a2ff40e112
commit
a10f807994
14 changed files with 1641 additions and 7 deletions
|
@ -6,5 +6,7 @@ zephyr_library_sources(lis2dux12.c)
|
|||
zephyr_library_sources_ifdef(CONFIG_LIS2DUX12_TRIGGER lis2dux12_trigger.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_DT_HAS_ST_LIS2DUX12_ENABLED lis2dux12_api.c )
|
||||
zephyr_library_sources_ifdef(CONFIG_DT_HAS_ST_LIS2DUXS12_ENABLED lis2duxs12_api.c )
|
||||
zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API lis2dux12_rtio.c lis2dux12_decoder.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_LIS2DUX12_STREAM lis2dux12_rtio_stream.c)
|
||||
|
||||
zephyr_library_include_directories(../stmemsc)
|
||||
|
|
|
@ -15,11 +15,21 @@ menuconfig LIS2DUX12
|
|||
select HAS_STMEMSC
|
||||
select USE_STDC_LIS2DUX12 if DT_HAS_ST_LIS2DUX12_ENABLED
|
||||
select USE_STDC_LIS2DUXS12 if DT_HAS_ST_LIS2DUXS12_ENABLED
|
||||
select RTIO_WORKQ if SENSOR_ASYNC_API
|
||||
help
|
||||
Enable driver for LIS2DUX12 accelerometer sensor driver
|
||||
|
||||
if LIS2DUX12
|
||||
|
||||
config LIS2DUX12_STREAM
|
||||
bool "Use hardware FIFO to stream data"
|
||||
select LIS2DUX12_TRIGGER
|
||||
default y
|
||||
depends on I2C_RTIO || SPI_RTIO
|
||||
depends on SENSOR_ASYNC_API
|
||||
help
|
||||
Use this config option to enable streaming sensor data via RTIO subsystem.
|
||||
|
||||
module = LIS2DUX12
|
||||
thread_priority = 10
|
||||
thread_stack_size = 1024
|
||||
|
|
|
@ -18,6 +18,8 @@
|
|||
#include <zephyr/logging/log.h>
|
||||
|
||||
#include "lis2dux12.h"
|
||||
#include "lis2dux12_decoder.h"
|
||||
#include "lis2dux12_rtio.h"
|
||||
|
||||
#if DT_HAS_COMPAT_STATUS_OKAY(st_lis2dux12)
|
||||
#include "lis2dux12_api.h"
|
||||
|
@ -243,6 +245,10 @@ static DEVICE_API(sensor, lis2dux12_driver_api) = {
|
|||
#endif
|
||||
.sample_fetch = lis2dux12_sample_fetch,
|
||||
.channel_get = lis2dux12_channel_get,
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
.get_decoder = lis2dux12_get_decoder,
|
||||
.submit = lis2dux12_submit,
|
||||
#endif
|
||||
};
|
||||
|
||||
/*
|
||||
|
@ -269,16 +275,25 @@ static DEVICE_API(sensor, lis2dux12_driver_api) = {
|
|||
.range = DT_INST_PROP(inst, range), \
|
||||
.pm = DT_INST_PROP(inst, power_mode), \
|
||||
.odr = DT_INST_PROP(inst, odr), \
|
||||
IF_ENABLED(CONFIG_LIS2DUX12_STREAM, \
|
||||
(.fifo_wtm = DT_INST_PROP(inst, fifo_watermark), \
|
||||
.accel_batch = DT_INST_PROP(inst, accel_fifo_batch_rate), \
|
||||
.ts_batch = DT_INST_PROP(inst, timestamp_fifo_batch_rate),)) \
|
||||
IF_ENABLED(UTIL_OR(DT_INST_NODE_HAS_PROP(inst, int1_gpios), \
|
||||
DT_INST_NODE_HAS_PROP(inst, int2_gpios)), \
|
||||
(LIS2DUX12_CFG_IRQ(inst))) \
|
||||
|
||||
#define LIS2DUX12_SPI_OPERATION \
|
||||
(SPI_WORD_SET(8) | SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA)
|
||||
|
||||
/*
|
||||
* Instantiation macros used when a device is on a SPI bus.
|
||||
*/
|
||||
#define LIS2DUX12_SPI_OPERATION \
|
||||
(SPI_WORD_SET(8) | SPI_OP_MODE_MASTER | SPI_MODE_CPOL | SPI_MODE_CPHA)
|
||||
|
||||
#define LIS2DUX12_SPI_RTIO_DEFINE(inst, name) \
|
||||
SPI_DT_IODEV_DEFINE(lis2dux12_iodev_##name##_##inst, \
|
||||
DT_DRV_INST(inst), LIS2DUX12_SPI_OPERATION, 0U); \
|
||||
RTIO_DEFINE(lis2dux12_rtio_ctx_##name##_##inst, 4, 4);
|
||||
|
||||
#define LIS2DUX12_CONFIG_SPI(inst, name) \
|
||||
{ \
|
||||
STMEMSC_CTX_SPI(&lis2dux12_config_##name##_##inst.stmemsc_cfg), \
|
||||
|
@ -288,9 +303,27 @@ static DEVICE_API(sensor, lis2dux12_driver_api) = {
|
|||
LIS2DUX12_CONFIG_COMMON(inst, name) \
|
||||
}
|
||||
|
||||
#define LIS2DUX12_DEFINE_SPI(inst, name) \
|
||||
IF_ENABLED(UTIL_AND(CONFIG_LIS2DUX12_STREAM, \
|
||||
CONFIG_SPI_RTIO), \
|
||||
(LIS2DUX12_SPI_RTIO_DEFINE(inst, name))); \
|
||||
static struct lis2dux12_data lis2dux12_data_##name##_##inst = { \
|
||||
IF_ENABLED(UTIL_AND(CONFIG_LIS2DUX12_STREAM, \
|
||||
CONFIG_SPI_RTIO), \
|
||||
(.rtio_ctx = &lis2dux12_rtio_ctx_##name##_##inst, \
|
||||
.iodev = &lis2dux12_iodev_##name##_##inst, \
|
||||
.bus_type = BUS_SPI,)) \
|
||||
}; \
|
||||
static const struct lis2dux12_config lis2dux12_config_##name##_##inst = \
|
||||
LIS2DUX12_CONFIG_SPI(inst, name);
|
||||
|
||||
/*
|
||||
* Instantiation macros used when a device is on an I2C bus.
|
||||
*/
|
||||
#define LIS2DUX12_I2C_RTIO_DEFINE(inst, name) \
|
||||
I2C_DT_IODEV_DEFINE(lis2dux12_iodev_##name##_##inst, DT_DRV_INST(inst)); \
|
||||
RTIO_DEFINE(lis2dux12_rtio_ctx_##name##_##inst, 4, 4);
|
||||
|
||||
#define LIS2DUX12_CONFIG_I2C(inst, name) \
|
||||
{ \
|
||||
STMEMSC_CTX_I2C(&lis2dux12_config_##name##_##inst.stmemsc_cfg), \
|
||||
|
@ -300,17 +333,29 @@ static DEVICE_API(sensor, lis2dux12_driver_api) = {
|
|||
LIS2DUX12_CONFIG_COMMON(inst, name) \
|
||||
}
|
||||
|
||||
#define LIS2DUX12_DEFINE_I2C(inst, name) \
|
||||
IF_ENABLED(UTIL_AND(CONFIG_LIS2DUX12_STREAM, \
|
||||
CONFIG_I2C_RTIO), \
|
||||
(LIS2DUX12_I2C_RTIO_DEFINE(inst, name))); \
|
||||
static struct lis2dux12_data lis2dux12_data_##name##_##inst = { \
|
||||
IF_ENABLED(UTIL_AND(CONFIG_LIS2DUX12_STREAM, \
|
||||
CONFIG_I2C_RTIO), \
|
||||
(.rtio_ctx = &lis2dux12_rtio_ctx_##name##_##inst, \
|
||||
.iodev = &lis2dux12_iodev_##name##_##inst, \
|
||||
.bus_type = BUS_I2C,)) \
|
||||
}; \
|
||||
static const struct lis2dux12_config lis2dux12_config_##name##_##inst = \
|
||||
LIS2DUX12_CONFIG_I2C(inst, name);
|
||||
|
||||
/*
|
||||
* Main instantiation macro. Use of COND_CODE_1() selects the right
|
||||
* bus-specific macro at preprocessor time.
|
||||
*/
|
||||
|
||||
#define LIS2DUX12_DEFINE(inst, name) \
|
||||
static struct lis2dux12_data lis2dux12_data_##name##_##inst; \
|
||||
static const struct lis2dux12_config lis2dux12_config_##name##_##inst = \
|
||||
COND_CODE_1(DT_INST_ON_BUS(inst, spi), \
|
||||
(LIS2DUX12_CONFIG_SPI(inst, name)), \
|
||||
(LIS2DUX12_CONFIG_I2C(inst, name))); \
|
||||
(LIS2DUX12_DEFINE_SPI(inst, name)), \
|
||||
(LIS2DUX12_DEFINE_I2C(inst, name))); \
|
||||
\
|
||||
SENSOR_DEVICE_DT_INST_DEFINE(inst, name##_init, NULL, \
|
||||
&lis2dux12_data_##name##_##inst, \
|
||||
|
|
|
@ -34,12 +34,33 @@
|
|||
#include <zephyr/drivers/i2c.h>
|
||||
#endif
|
||||
|
||||
/* Accel sensor sensitivity grain is 61 ug/LSB */
|
||||
#define GAIN_UNIT (61LL)
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
struct trigger_config {
|
||||
uint8_t int_fifo_th : 1;
|
||||
uint8_t int_fifo_full : 1;
|
||||
uint8_t int_drdy : 1;
|
||||
};
|
||||
#endif
|
||||
|
||||
typedef int32_t (*api_lis2dux12_set_odr_raw)(const struct device *dev, uint8_t odr);
|
||||
typedef int32_t (*api_lis2dux12_set_range)(const struct device *dev, uint8_t range);
|
||||
typedef int32_t (*api_lis2dux12_sample_fetch_accel)(const struct device *dev);
|
||||
#ifdef CONFIG_LIS2DUX12_ENABLE_TEMP
|
||||
typedef int32_t (*api_lis2dux12_sample_fetch_temp)(const struct device *dev);
|
||||
#endif
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
typedef int32_t (*api_lis2dux12_rtio_read_accel)(const struct device *dev, int16_t *acc);
|
||||
typedef int32_t (*api_lis2dux12_rtio_read_temp)(const struct device *dev, int16_t *temp);
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
typedef void (*api_lis2dux12_stream_config_fifo)(const struct device *dev,
|
||||
struct trigger_config trig_cfg);
|
||||
typedef void (*api_lis2dux12_stream_config_drdy)(const struct device *dev,
|
||||
struct trigger_config trig_cfg);
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
typedef void (*api_lis2dux12_handle_interrupt)(const struct device *dev);
|
||||
typedef int32_t (*api_lis2dux12_init_interrupt)(const struct device *dev);
|
||||
|
@ -52,6 +73,14 @@ struct lis2dux12_chip_api {
|
|||
#ifdef CONFIG_LIS2DUX12_ENABLE_TEMP
|
||||
api_lis2dux12_sample_fetch_temp sample_fetch_temp;
|
||||
#endif
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
api_lis2dux12_rtio_read_accel rtio_read_accel;
|
||||
api_lis2dux12_rtio_read_temp rtio_read_temp;
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
api_lis2dux12_stream_config_fifo stream_config_fifo;
|
||||
api_lis2dux12_stream_config_drdy stream_config_drdy;
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
api_lis2dux12_handle_interrupt handle_interrupt;
|
||||
api_lis2dux12_init_interrupt init_interrupt;
|
||||
|
@ -73,6 +102,12 @@ struct lis2dux12_config {
|
|||
uint8_t range;
|
||||
uint8_t pm;
|
||||
uint8_t odr;
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
uint8_t fifo_wtm;
|
||||
uint8_t accel_batch : 3;
|
||||
uint8_t ts_batch : 2;
|
||||
uint8_t reserved : 3;
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
const struct gpio_dt_spec int1_gpio;
|
||||
const struct gpio_dt_spec int2_gpio;
|
||||
|
@ -95,6 +130,21 @@ struct lis2dux12_data {
|
|||
float sample_temp;
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
struct rtio_iodev_sqe *streaming_sqe;
|
||||
struct rtio *rtio_ctx;
|
||||
struct rtio_iodev *iodev;
|
||||
uint64_t timestamp;
|
||||
uint8_t status;
|
||||
uint8_t fifo_status[2];
|
||||
uint16_t fifo_count;
|
||||
struct trigger_config trig_cfg;
|
||||
uint8_t accel_batch_odr : 3;
|
||||
uint8_t ts_batch_odr : 2;
|
||||
uint8_t bus_type : 1; /* I2C is 0, SPI is 1 */
|
||||
uint8_t reserved : 2;
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
struct gpio_dt_spec *drdy_gpio;
|
||||
struct gpio_callback gpio_cb;
|
||||
|
@ -114,6 +164,19 @@ struct lis2dux12_data {
|
|||
#endif /* CONFIG_LIS2DUX12_TRIGGER */
|
||||
};
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
#define BUS_I2C 0
|
||||
#define BUS_SPI 1
|
||||
|
||||
static inline uint8_t lis2dux12_bus_reg(struct lis2dux12_data *data, uint8_t x)
|
||||
{
|
||||
return (data->bus_type == BUS_SPI) ? x | 0x80 : x;
|
||||
}
|
||||
|
||||
#define LIS2DUX12_FIFO_ITEM_LEN 7
|
||||
#define LIS2DUX12_FIFO_SIZE(x) (x * LIS2DUX12_FIFO_ITEM_LEN)
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
int lis2dux12_trigger_set(const struct device *dev,
|
||||
const struct sensor_trigger *trig,
|
||||
|
|
|
@ -101,6 +101,121 @@ static int32_t st_lis2dux12_sample_fetch_temp(const struct device *dev)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
static int32_t st_lis2dux12_rtio_read_accel(const struct device *dev, int16_t *acc)
|
||||
{
|
||||
struct lis2dux12_data *data = dev->data;
|
||||
const struct lis2dux12_config *cfg = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
|
||||
|
||||
/* fetch raw data sample */
|
||||
lis2dux12_md_t mode = {.fs = data->range};
|
||||
lis2dux12_xl_data_t xzy_data = {0};
|
||||
|
||||
if (lis2dux12_xl_data_get(ctx, &mode, &xzy_data) < 0) {
|
||||
LOG_ERR("Failed to fetch raw data sample");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
acc[0] = xzy_data.raw[0];
|
||||
acc[1] = xzy_data.raw[1];
|
||||
acc[2] = xzy_data.raw[2];
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int32_t st_lis2dux12_rtio_read_temp(const struct device *dev, int16_t *temp)
|
||||
{
|
||||
const struct lis2dux12_config *cfg = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
|
||||
|
||||
/* fetch raw data sample */
|
||||
lis2dux12_outt_data_t temp_data = {0};
|
||||
|
||||
if (lis2dux12_outt_data_get(ctx, &temp_data) < 0) {
|
||||
LOG_ERR("Failed to fetch raw temperature data sample");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
*temp = temp_data.heat.raw;
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
static void st_lis2dux12_stream_config_fifo(const struct device *dev,
|
||||
struct trigger_config trig_cfg)
|
||||
{
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
const struct lis2dux12_config *config = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&config->ctx;
|
||||
lis2dux12_pin_int_route_t pin_int = { 0 };
|
||||
lis2dux12_fifo_mode_t fifo_mode;
|
||||
|
||||
/* disable FIFO as first thing */
|
||||
fifo_mode.store = LIS2DUX12_FIFO_1X;
|
||||
fifo_mode.xl_only = 1;
|
||||
fifo_mode.watermark = 0;
|
||||
fifo_mode.operation = LIS2DUX12_BYPASS_MODE;
|
||||
fifo_mode.batch.dec_ts = LIS2DUX12_DEC_TS_OFF;
|
||||
fifo_mode.batch.bdr_xl = LIS2DUX12_BDR_XL_ODR_OFF;
|
||||
|
||||
pin_int.fifo_th = PROPERTY_DISABLE;
|
||||
pin_int.fifo_full = PROPERTY_DISABLE;
|
||||
|
||||
if (trig_cfg.int_fifo_th || trig_cfg.int_fifo_full) {
|
||||
pin_int.fifo_th = (trig_cfg.int_fifo_th) ? PROPERTY_ENABLE : PROPERTY_DISABLE;
|
||||
pin_int.fifo_full = (trig_cfg.int_fifo_full) ? PROPERTY_ENABLE : PROPERTY_DISABLE;
|
||||
|
||||
fifo_mode.operation = LIS2DUX12_STREAM_MODE;
|
||||
fifo_mode.batch.dec_ts = config->ts_batch;
|
||||
fifo_mode.batch.bdr_xl = config->accel_batch;
|
||||
fifo_mode.watermark = config->fifo_wtm;
|
||||
}
|
||||
|
||||
/*
|
||||
* Set FIFO watermark (number of unread sensor data TAG + 6 bytes
|
||||
* stored in FIFO) to FIFO_WATERMARK samples
|
||||
*/
|
||||
lis2dux12_fifo_mode_set(ctx, fifo_mode);
|
||||
|
||||
/* Set FIFO batch rates */
|
||||
lis2dux12->accel_batch_odr = fifo_mode.batch.bdr_xl;
|
||||
lis2dux12->ts_batch_odr = fifo_mode.batch.dec_ts;
|
||||
|
||||
/* Set pin interrupt (fifo_th could be on or off) */
|
||||
if (config->drdy_pin == 1) {
|
||||
lis2dux12_pin_int1_route_set(ctx, &pin_int);
|
||||
} else {
|
||||
lis2dux12_pin_int2_route_set(ctx, &pin_int);
|
||||
}
|
||||
}
|
||||
|
||||
static void st_lis2dux12_stream_config_drdy(const struct device *dev,
|
||||
struct trigger_config trig_cfg)
|
||||
{
|
||||
const struct lis2dux12_config *config = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&config->ctx;
|
||||
lis2dux12_pin_int_route_t pin_int = { 0 };
|
||||
|
||||
/* dummy read: re-trigger interrupt */
|
||||
lis2dux12_md_t md = {.fs = LIS2DUX12_2g};
|
||||
lis2dux12_xl_data_t buf;
|
||||
|
||||
lis2dux12_xl_data_get(ctx, &md, &buf);
|
||||
|
||||
pin_int.drdy = PROPERTY_ENABLE;
|
||||
|
||||
/* Set pin interrupt */
|
||||
if (config->drdy_pin == 1) {
|
||||
lis2dux12_pin_int1_route_set(ctx, &pin_int);
|
||||
} else {
|
||||
lis2dux12_pin_int2_route_set(ctx, &pin_int);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
static void st_lis2dux12_handle_interrupt(const struct device *dev)
|
||||
{
|
||||
|
@ -164,6 +279,14 @@ const struct lis2dux12_chip_api st_lis2dux12_chip_api = {
|
|||
#ifdef CONFIG_LIS2DUX12_ENABLE_TEMP
|
||||
.sample_fetch_temp = st_lis2dux12_sample_fetch_temp,
|
||||
#endif
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
.rtio_read_accel = st_lis2dux12_rtio_read_accel,
|
||||
.rtio_read_temp = st_lis2dux12_rtio_read_temp,
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
.stream_config_fifo = st_lis2dux12_stream_config_fifo,
|
||||
.stream_config_drdy = st_lis2dux12_stream_config_drdy,
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
.handle_interrupt = st_lis2dux12_handle_interrupt,
|
||||
.init_interrupt = st_lis2dux12_init_interrupt,
|
||||
|
|
449
drivers/sensor/st/lis2dux12/lis2dux12_decoder.c
Normal file
449
drivers/sensor/st/lis2dux12/lis2dux12_decoder.c
Normal file
|
@ -0,0 +1,449 @@
|
|||
/* ST Microelectronics LIS2DUX12 6-axis IMU sensor driver
|
||||
*
|
||||
* Copyright (c) 2023 Google LLC
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include "lis2dux12.h"
|
||||
#include "lis2dux12_decoder.h"
|
||||
#include <zephyr/dt-bindings/sensor/lis2dux12.h>
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(LIS2DUX12_DECODER, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
static const uint8_t accel_divisor[] = {
|
||||
[LIS2DUX12_DT_BDR_XL_ODR] = 1,
|
||||
[LIS2DUX12_DT_BDR_XL_ODR_DIV_2] = 2,
|
||||
[LIS2DUX12_DT_BDR_XL_ODR_DIV_4] = 4,
|
||||
[LIS2DUX12_DT_BDR_XL_ODR_DIV_8] = 8,
|
||||
[LIS2DUX12_DT_BDR_XL_ODR_DIV_16] = 16,
|
||||
[LIS2DUX12_DT_BDR_XL_ODR_DIV_32] = 32,
|
||||
[LIS2DUX12_DT_BDR_XL_ODR_DIV_64] = 64,
|
||||
[LIS2DUX12_DT_BDR_XL_ODR_OFF] = 0,
|
||||
};
|
||||
|
||||
static uint32_t accel_period_ns(uint8_t odr, uint8_t scaler)
|
||||
{
|
||||
switch (odr) {
|
||||
default:
|
||||
case LIS2DUX12_DT_ODR_OFF:
|
||||
return UINT32_C(0);
|
||||
|
||||
case LIS2DUX12_DT_ODR_1Hz_ULP:
|
||||
return ((UINT32_C(1000000000000) / 1000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_3Hz_ULP:
|
||||
return ((UINT32_C(1000000000000) / 3000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_25Hz_ULP:
|
||||
return ((UINT32_C(1000000000000) / 25000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_6Hz:
|
||||
return ((UINT32_C(1000000000000) / 6000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_12Hz5:
|
||||
return ((UINT32_C(1000000000000) / 12500) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_25Hz:
|
||||
return ((UINT32_C(1000000000000) / 25000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_50Hz:
|
||||
return ((UINT32_C(1000000000000) / 50000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_100Hz:
|
||||
return ((UINT32_C(1000000000000) / 100000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_200Hz:
|
||||
return ((UINT32_C(1000000000000) / 200000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_400Hz:
|
||||
return ((UINT32_C(1000000000000) / 400000) * accel_divisor[scaler]);
|
||||
|
||||
case LIS2DUX12_DT_ODR_800Hz:
|
||||
return ((UINT32_C(1000000000000) / 800000) * accel_divisor[scaler]);
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif /* CONFIG_LIS2DUX12_STREAM */
|
||||
|
||||
/*
|
||||
* Expand val to q31_t according to its range; this is achieved multiplying by 2^31/2^range.
|
||||
*/
|
||||
#define Q31_SHIFT_VAL(val, range) \
|
||||
(q31_t) (roundf((val) * ((int64_t)1 << (31 - (range)))))
|
||||
|
||||
/*
|
||||
* Expand micro_val (a generic micro unit) to q31_t according to its range; this is achieved
|
||||
* multiplying by 2^31/2^range. Then transform it to val.
|
||||
*/
|
||||
#define Q31_SHIFT_MICROVAL(micro_val, range) \
|
||||
(q31_t) ((int64_t)(micro_val) * ((int64_t)1 << (31 - (range))) / 1000000LL)
|
||||
|
||||
/* bit range for Accelerometer for a given range value */
|
||||
static const int8_t accel_range[] = {
|
||||
[LIS2DUX12_DT_FS_2G] = 5,
|
||||
[LIS2DUX12_DT_FS_4G] = 6,
|
||||
[LIS2DUX12_DT_FS_8G] = 7,
|
||||
[LIS2DUX12_DT_FS_16G] = 8,
|
||||
};
|
||||
|
||||
#if defined(CONFIG_LIS2DUX12_ENABLE_TEMP)
|
||||
/* bit range for Temperature sensor */
|
||||
static const int8_t temp_range = 9;
|
||||
|
||||
/* transform temperature LSB into micro-Celsius */
|
||||
#define SENSOR_TEMP_UCELSIUS(t_lsb) \
|
||||
(int64_t) (25000000LL + (((int64_t)(t_lsb) * 1000000LL) / 355LL))
|
||||
|
||||
#endif
|
||||
|
||||
/* Calculate scaling factor to transform micro-g/LSB unit into micro-ms2/LSB */
|
||||
#define SENSOR_SCALE_UG_TO_UMS2(ug_lsb) \
|
||||
(int32_t)((ug_lsb) * SENSOR_G / 1000000LL)
|
||||
|
||||
/*
|
||||
* Accelerometer scaling factors table for a given range value
|
||||
* GAIN_UNIT_XL is expressed in ug/LSB.
|
||||
*/
|
||||
static const int32_t accel_scaler[] = {
|
||||
/* LIS2DUX12_DT_FS_2G */
|
||||
SENSOR_SCALE_UG_TO_UMS2(GAIN_UNIT),
|
||||
/* LIS2DUX12_DT_FS_4G / LSM6DSV32X_DT_FS_4G */
|
||||
SENSOR_SCALE_UG_TO_UMS2(2 * GAIN_UNIT),
|
||||
/* LIS2DUX12_DT_FS_8G / LSM6DSV32X_DT_FS_8G*/
|
||||
SENSOR_SCALE_UG_TO_UMS2(4 * GAIN_UNIT),
|
||||
/* LIS2DUX12_DT_FS_16G / LSM6DSV32X_DT_FS_16G */
|
||||
SENSOR_SCALE_UG_TO_UMS2(8 * GAIN_UNIT),
|
||||
/* LSM6DSV32X_DT_FS_32G */
|
||||
SENSOR_SCALE_UG_TO_UMS2(16 * GAIN_UNIT),
|
||||
};
|
||||
|
||||
/* Calculate scaling factor to transform micro-dps/LSB unit into micro-rads/LSB */
|
||||
#define SENSOR_SCALE_UDPS_TO_URADS(udps_lsb) \
|
||||
(int32_t)(((udps_lsb) * SENSOR_PI / 180LL) / 1000000LL)
|
||||
|
||||
static int lis2dux12_decoder_get_frame_count(const uint8_t *buffer,
|
||||
struct sensor_chan_spec chan_spec,
|
||||
uint16_t *frame_count)
|
||||
{
|
||||
struct lis2dux12_fifo_data *data = (struct lis2dux12_fifo_data *)buffer;
|
||||
struct lis2dux12_rtio_data *rdata = (struct lis2dux12_rtio_data *)buffer;
|
||||
const struct lis2dux12_decoder_header *header = &data->header;
|
||||
|
||||
if (chan_spec.chan_idx != 0) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
if (!header->is_fifo) {
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
*frame_count = rdata->has_accel ? 1 : 0;
|
||||
return 0;
|
||||
|
||||
case SENSOR_CHAN_DIE_TEMP:
|
||||
*frame_count = rdata->has_temp ? 1 : 0;
|
||||
return 0;
|
||||
|
||||
default:
|
||||
*frame_count = 0;
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
const struct lis2dux12_fifo_data *edata = (const struct lis2dux12_fifo_data *)buffer;
|
||||
const uint8_t *buffer_end;
|
||||
uint8_t fifo_tag;
|
||||
uint8_t tot_accel_fifo_words = 0;
|
||||
uint8_t tot_ts_fifo_words = 0;
|
||||
|
||||
#if defined(CONFIG_LIS2DUX12_ENABLE_TEMP)
|
||||
uint8_t tot_temp_fifo_words = 0;
|
||||
#endif
|
||||
|
||||
buffer += sizeof(struct lis2dux12_fifo_data);
|
||||
buffer_end = buffer + LIS2DUX12_FIFO_SIZE(edata->fifo_count);
|
||||
|
||||
/* count total FIFO word for each tag */
|
||||
while (buffer < buffer_end) {
|
||||
fifo_tag = (buffer[0] >> 3);
|
||||
|
||||
switch (fifo_tag) {
|
||||
case LIS2DUXXX_XL_ONLY_2X_TAG:
|
||||
tot_accel_fifo_words += 2;
|
||||
break;
|
||||
case LIS2DUXXX_XL_TEMP_TAG:
|
||||
tot_accel_fifo_words++;
|
||||
break;
|
||||
case LIS2DUXXX_TIMESTAMP_TAG:
|
||||
tot_ts_fifo_words++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
buffer += LIS2DUX12_FIFO_ITEM_LEN;
|
||||
}
|
||||
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
*frame_count = tot_accel_fifo_words;
|
||||
break;
|
||||
|
||||
#if defined(CONFIG_LIS2DUX12_ENABLE_TEMP)
|
||||
case SENSOR_CHAN_DIE_TEMP:
|
||||
*frame_count = tot_temp_fifo_words;
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
*frame_count = 0;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
static int lis2dux12_decode_fifo(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
|
||||
uint32_t *fit, uint16_t max_count, void *data_out)
|
||||
{
|
||||
const struct lis2dux12_fifo_data *edata = (const struct lis2dux12_fifo_data *)buffer;
|
||||
const uint8_t *buffer_end;
|
||||
const struct lis2dux12_decoder_header *header = &edata->header;
|
||||
int count = 0;
|
||||
uint8_t fifo_tag;
|
||||
uint16_t xl_count = 0;
|
||||
uint16_t tot_chan_fifo_words = 0;
|
||||
int ret;
|
||||
|
||||
/* count total FIFO word for each tag */
|
||||
ret = lis2dux12_decoder_get_frame_count(buffer, chan_spec, &tot_chan_fifo_words);
|
||||
if (ret < 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
buffer += sizeof(struct lis2dux12_fifo_data);
|
||||
buffer_end = buffer + LIS2DUX12_FIFO_SIZE(edata->fifo_count);
|
||||
|
||||
/*
|
||||
* Timestamp in header is set when FIFO threshold is reached, so
|
||||
* set time baseline going back in past according to total number
|
||||
* of FIFO word for each type.
|
||||
*/
|
||||
if (SENSOR_CHANNEL_IS_ACCEL(chan_spec.chan_type)) {
|
||||
((struct sensor_data_header *)data_out)->base_timestamp_ns =
|
||||
edata->header.timestamp -
|
||||
(tot_chan_fifo_words - 1) *
|
||||
accel_period_ns(edata->accel_odr, edata->accel_batch_odr);
|
||||
}
|
||||
|
||||
while (count < max_count && buffer < buffer_end) {
|
||||
const uint8_t *frame_end = buffer;
|
||||
uint8_t skip_frame;
|
||||
|
||||
skip_frame = 0;
|
||||
frame_end += LIS2DUX12_FIFO_ITEM_LEN;
|
||||
|
||||
fifo_tag = (buffer[0] >> 3);
|
||||
|
||||
switch (fifo_tag) {
|
||||
case LIS2DUXXX_XL_TEMP_TAG: {
|
||||
struct sensor_three_axis_data *out = data_out;
|
||||
int16_t x, y, z;
|
||||
const int32_t scale = accel_scaler[header->range];
|
||||
|
||||
xl_count++;
|
||||
if ((uintptr_t)buffer < *fit) {
|
||||
/* This frame was already decoded, move on to the next frame */
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!SENSOR_CHANNEL_IS_ACCEL(chan_spec.chan_type)) {
|
||||
buffer = frame_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
out->readings[count].timestamp_delta =
|
||||
(xl_count - 1) * accel_period_ns(edata->accel_odr,
|
||||
edata->accel_batch_odr);
|
||||
|
||||
x = (int16_t)buffer[1] + (int16_t)buffer[2] * 256;
|
||||
y = (int16_t)buffer[3] + (int16_t)buffer[4] * 256;
|
||||
z = (int16_t)buffer[5] + (int16_t)buffer[6] * 256;
|
||||
|
||||
out->shift = accel_range[header->range];
|
||||
|
||||
out->readings[count].x = Q31_SHIFT_MICROVAL(scale * x, out->shift);
|
||||
out->readings[count].y = Q31_SHIFT_MICROVAL(scale * y, out->shift);
|
||||
out->readings[count].z = Q31_SHIFT_MICROVAL(scale * z, out->shift);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
/* skip unhandled FIFO tag */
|
||||
buffer = frame_end;
|
||||
LOG_DBG("unknown FIFO tag %02x", fifo_tag);
|
||||
continue;
|
||||
}
|
||||
|
||||
buffer = frame_end;
|
||||
*fit = (uintptr_t)frame_end;
|
||||
count++;
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
#endif /* CONFIG_LIS2DUX12_STREAM */
|
||||
|
||||
static int lis2dux12_decode_sample(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
|
||||
uint32_t *fit, uint16_t max_count, void *data_out)
|
||||
{
|
||||
const struct lis2dux12_rtio_data *edata = (const struct lis2dux12_rtio_data *)buffer;
|
||||
const struct lis2dux12_decoder_header *header = &edata->header;
|
||||
|
||||
if (*fit != 0) {
|
||||
return 0;
|
||||
}
|
||||
if (max_count == 0 || chan_spec.chan_idx != 0) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
case SENSOR_CHAN_ACCEL_XYZ: {
|
||||
const int32_t scale = accel_scaler[header->range];
|
||||
|
||||
if (edata->has_accel == 0) {
|
||||
return -ENODATA;
|
||||
}
|
||||
|
||||
struct sensor_three_axis_data *out = data_out;
|
||||
|
||||
out->header.base_timestamp_ns = edata->header.timestamp;
|
||||
out->header.reading_count = 1;
|
||||
|
||||
out->shift = accel_range[header->range];
|
||||
|
||||
out->readings[0].x = Q31_SHIFT_MICROVAL(scale * edata->acc[0], out->shift);
|
||||
out->readings[0].y = Q31_SHIFT_MICROVAL(scale * edata->acc[1], out->shift);
|
||||
out->readings[0].z = Q31_SHIFT_MICROVAL(scale * edata->acc[2], out->shift);
|
||||
*fit = 1;
|
||||
return 1;
|
||||
}
|
||||
#if defined(CONFIG_LIS2DUX12_ENABLE_TEMP)
|
||||
case SENSOR_CHAN_DIE_TEMP: {
|
||||
int64_t t_uC;
|
||||
|
||||
if (edata->has_temp == 0) {
|
||||
return -ENODATA;
|
||||
}
|
||||
|
||||
struct sensor_q31_data *out = data_out;
|
||||
|
||||
out->header.base_timestamp_ns = edata->header.timestamp;
|
||||
out->header.reading_count = 1;
|
||||
|
||||
out->shift = temp_range;
|
||||
|
||||
/* transform temperature LSB into micro-Celsius */
|
||||
t_uC = SENSOR_TEMP_UCELSIUS(edata->temp);
|
||||
|
||||
out->readings[0].temperature = Q31_SHIFT_MICROVAL(t_uC, out->shift);
|
||||
*fit = 1;
|
||||
return 1;
|
||||
}
|
||||
#endif
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
}
|
||||
|
||||
static int lis2dux12_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
|
||||
uint32_t *fit, uint16_t max_count, void *data_out)
|
||||
{
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
const struct lis2dux12_decoder_header *header =
|
||||
(const struct lis2dux12_decoder_header *)buffer;
|
||||
|
||||
if (header->is_fifo) {
|
||||
return lis2dux12_decode_fifo(buffer, chan_spec, fit, max_count, data_out);
|
||||
}
|
||||
#endif
|
||||
|
||||
return lis2dux12_decode_sample(buffer, chan_spec, fit, max_count, data_out);
|
||||
}
|
||||
|
||||
static int lis2dux12_decoder_get_size_info(struct sensor_chan_spec chan_spec, size_t *base_size,
|
||||
size_t *frame_size)
|
||||
{
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
*base_size = sizeof(struct sensor_three_axis_data);
|
||||
*frame_size = sizeof(struct sensor_three_axis_sample_data);
|
||||
return 0;
|
||||
case SENSOR_CHAN_DIE_TEMP:
|
||||
*base_size = sizeof(struct sensor_q31_data);
|
||||
*frame_size = sizeof(struct sensor_q31_sample_data);
|
||||
return 0;
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
}
|
||||
|
||||
static bool lis2dux12_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
|
||||
{
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
const struct lis2dux12_decoder_header *header =
|
||||
(const struct lis2dux12_decoder_header *)buffer;
|
||||
|
||||
switch (trigger) {
|
||||
case SENSOR_TRIG_DATA_READY:
|
||||
return header->int_status & 0x01;
|
||||
case SENSOR_TRIG_FIFO_WATERMARK:
|
||||
return header->int_status & 0x80;
|
||||
case SENSOR_TRIG_FIFO_FULL:
|
||||
return header->int_status & 0x40;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
#define DT_DRV_COMPAT st_lis2dux12
|
||||
|
||||
SENSOR_DECODER_API_DT_DEFINE() = {
|
||||
.get_frame_count = lis2dux12_decoder_get_frame_count,
|
||||
.get_size_info = lis2dux12_decoder_get_size_info,
|
||||
.decode = lis2dux12_decoder_decode,
|
||||
.has_trigger = lis2dux12_decoder_has_trigger,
|
||||
};
|
||||
|
||||
int lis2dux12_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
*decoder = &SENSOR_DECODER_NAME();
|
||||
|
||||
return 0;
|
||||
}
|
55
drivers/sensor/st/lis2dux12/lis2dux12_decoder.h
Normal file
55
drivers/sensor/st/lis2dux12/lis2dux12_decoder.h
Normal file
|
@ -0,0 +1,55 @@
|
|||
/* ST Microelectronics LIS2DUX12 6-axis IMU sensor driver
|
||||
*
|
||||
* Copyright (c) 2023 Google LLC
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef ZEPHYR_DRIVERS_SENSOR_LIS2DUX12_DECODER_H_
|
||||
#define ZEPHYR_DRIVERS_SENSOR_LIS2DUX12_DECODER_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
|
||||
/*
|
||||
* This macro converts the Accelerometer full-scale range value (which should be a power of 2) to
|
||||
* an index value used by the decoder. Note: this index is not the same as the RAW register value.
|
||||
*/
|
||||
#define LIS2DUX12_ACCEL_FS_VAL_TO_FS_IDX(x) (__builtin_clz(x) - 1)
|
||||
|
||||
struct lis2dux12_decoder_header {
|
||||
uint64_t timestamp;
|
||||
uint8_t is_fifo: 1;
|
||||
uint8_t range: 2;
|
||||
uint8_t reserved: 5;
|
||||
uint8_t int_status;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct lis2dux12_fifo_data {
|
||||
struct lis2dux12_decoder_header header;
|
||||
uint32_t accel_odr: 4;
|
||||
uint32_t fifo_count: 7;
|
||||
uint32_t reserved_1: 5;
|
||||
uint32_t accel_batch_odr: 3;
|
||||
uint32_t ts_batch_odr: 2;
|
||||
uint32_t reserved: 11;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct lis2dux12_rtio_data {
|
||||
struct lis2dux12_decoder_header header;
|
||||
struct {
|
||||
uint8_t has_accel: 1; /* set if accel channel has data */
|
||||
uint8_t has_temp: 1; /* set if temp channel has data */
|
||||
uint8_t reserved: 6;
|
||||
} __attribute__((__packed__));
|
||||
int16_t acc[3];
|
||||
int16_t temp;
|
||||
};
|
||||
|
||||
int lis2dux12_encode(const struct device *dev, const struct sensor_chan_spec *const channels,
|
||||
const size_t num_channels, uint8_t *buf);
|
||||
|
||||
int lis2dux12_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder);
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_LIS2DUX12_DECODER_H_ */
|
144
drivers/sensor/st/lis2dux12/lis2dux12_rtio.c
Normal file
144
drivers/sensor/st/lis2dux12/lis2dux12_rtio.c
Normal file
|
@ -0,0 +1,144 @@
|
|||
/* ST Microelectronics LIS2DUX12 6-axis IMU sensor driver
|
||||
*
|
||||
* Copyright (c) 2023 Google LLC
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include "lis2dux12.h"
|
||||
#include "lis2dux12_rtio.h"
|
||||
#include "lis2dux12_decoder.h"
|
||||
#include <zephyr/rtio/work.h>
|
||||
#include <zephyr/drivers/sensor_clock.h>
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(LIS2DUX12_RTIO, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
static void lis2dux12_submit_sample(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
const struct sensor_chan_spec *const channels = cfg->channels;
|
||||
const size_t num_channels = cfg->count;
|
||||
uint32_t min_buf_len = sizeof(struct lis2dux12_rtio_data);
|
||||
uint64_t cycles;
|
||||
int rc = 0;
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
struct lis2dux12_rtio_data *edata;
|
||||
struct lis2dux12_data *data = dev->data;
|
||||
|
||||
const struct lis2dux12_config *config = dev->config;
|
||||
const struct lis2dux12_chip_api *chip_api = config->chip_api;
|
||||
|
||||
/* 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);
|
||||
goto err;
|
||||
}
|
||||
|
||||
edata = (struct lis2dux12_rtio_data *)buf;
|
||||
|
||||
edata->has_accel = 0;
|
||||
edata->has_temp = 0;
|
||||
|
||||
for (int i = 0; i < num_channels; i++) {
|
||||
switch (channels[i].chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
edata->has_accel = 1;
|
||||
|
||||
rc = chip_api->rtio_read_accel(dev, edata->acc);
|
||||
if (rc < 0) {
|
||||
LOG_DBG("Failed to read accel sample");
|
||||
goto err;
|
||||
}
|
||||
break;
|
||||
#if defined(CONFIG_LIS2DUX12_ENABLE_TEMP)
|
||||
case SENSOR_CHAN_DIE_TEMP:
|
||||
edata->has_temp = 1;
|
||||
|
||||
rc = chip_api->rtio_read_temp(dev, &edata->temp);
|
||||
if (rc < 0) {
|
||||
LOG_DBG("Failed to read temp sample");
|
||||
goto err;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
case SENSOR_CHAN_ALL:
|
||||
edata->has_accel = 1;
|
||||
|
||||
rc = chip_api->rtio_read_accel(dev, edata->acc);
|
||||
if (rc < 0) {
|
||||
LOG_DBG("Failed to read accel sample");
|
||||
goto err;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_LIS2DUX12_ENABLE_TEMP)
|
||||
edata->has_temp = 1;
|
||||
|
||||
rc = chip_api->rtio_read_temp(dev, &edata->temp);
|
||||
if (rc < 0) {
|
||||
LOG_DBG("Failed to read temp sample");
|
||||
goto err;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
rc = sensor_clock_get_cycles(&cycles);
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to get sensor clock cycles");
|
||||
rtio_iodev_sqe_err(iodev_sqe, rc);
|
||||
goto err;
|
||||
}
|
||||
|
||||
edata->header.is_fifo = false;
|
||||
edata->header.range = data->range;
|
||||
edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
|
||||
|
||||
rtio_iodev_sqe_ok(iodev_sqe, 0);
|
||||
|
||||
err:
|
||||
/* Check that the fetch succeeded */
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to fetch samples");
|
||||
rtio_iodev_sqe_err(iodev_sqe, rc);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void lis2dux12_submit_sync(struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
const struct device *dev = cfg->sensor;
|
||||
|
||||
if (!cfg->is_streaming) {
|
||||
lis2dux12_submit_sample(dev, iodev_sqe);
|
||||
} else if (IS_ENABLED(CONFIG_LIS2DUX12_STREAM)) {
|
||||
lis2dux12_submit_stream(dev, iodev_sqe);
|
||||
} else {
|
||||
rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
|
||||
}
|
||||
}
|
||||
|
||||
void lis2dux12_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
struct rtio_work_req *req = rtio_work_req_alloc();
|
||||
|
||||
if (req == NULL) {
|
||||
LOG_ERR("RTIO work item allocation failed. Consider to increase "
|
||||
"CONFIG_RTIO_WORKQ_POOL_ITEMS.");
|
||||
rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
|
||||
return;
|
||||
}
|
||||
|
||||
rtio_work_req_submit(req, iodev_sqe, lis2dux12_submit_sync);
|
||||
}
|
19
drivers/sensor/st/lis2dux12/lis2dux12_rtio.h
Normal file
19
drivers/sensor/st/lis2dux12/lis2dux12_rtio.h
Normal file
|
@ -0,0 +1,19 @@
|
|||
/* ST Microelectronics LIS2DUX12 6-axis IMU sensor driver
|
||||
*
|
||||
* Copyright (c) 2023 Google LLC
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#ifndef ZEPHYR_DRIVERS_SENSOR_LIS2DUX12_RTIO_H_
|
||||
#define ZEPHYR_DRIVERS_SENSOR_LIS2DUX12_RTIO_H_
|
||||
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
|
||||
void lis2dux12_submit(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe);
|
||||
void lis2dux12_submit_stream(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe);
|
||||
void lis2dux12_stream_irq_handler(const struct device *dev);
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_LIS2DUX12_RTIO_H_ */
|
510
drivers/sensor/st/lis2dux12/lis2dux12_rtio_stream.c
Normal file
510
drivers/sensor/st/lis2dux12/lis2dux12_rtio_stream.c
Normal file
|
@ -0,0 +1,510 @@
|
|||
/* ST Microelectronics LIS2DUX12 6-axis IMU sensor driver
|
||||
*
|
||||
* Copyright (c) 2023 Google LLC
|
||||
* Copyright (c) 2024 STMicroelectronics
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/dt-bindings/sensor/lis2dux12.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
#include "lis2dux12.h"
|
||||
#include "lis2dux12_decoder.h"
|
||||
#include <zephyr/rtio/work.h>
|
||||
#include <zephyr/drivers/sensor_clock.h>
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_DECLARE(LIS2DUX12_RTIO);
|
||||
|
||||
/*
|
||||
* Create a chain of SQEs representing a bus transaction to read a reg.
|
||||
* The RTIO-enabled bus driver will:
|
||||
*
|
||||
* - write "reg" address
|
||||
* - read "len" data bytes into "buf".
|
||||
* - call complete_op callback
|
||||
*
|
||||
* If drdy_xl is active it reads XL data (6 bytes) from LIS2DUX12_OUTX_L_A reg.
|
||||
*/
|
||||
static void lis2dux12_rtio_rw_transaction(const struct device *dev, uint8_t reg,
|
||||
uint8_t *buf, uint32_t len,
|
||||
rtio_callback_t complete_op_cb)
|
||||
{
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
struct rtio *rtio = lis2dux12->rtio_ctx;
|
||||
struct rtio_iodev *iodev = lis2dux12->iodev;
|
||||
struct rtio_sqe *write_addr = rtio_sqe_acquire(rtio);
|
||||
struct rtio_sqe *read_reg = rtio_sqe_acquire(rtio);
|
||||
struct rtio_sqe *complete_op = rtio_sqe_acquire(rtio);
|
||||
struct rtio_iodev_sqe *sqe = lis2dux12->streaming_sqe;
|
||||
uint8_t reg_bus = lis2dux12_bus_reg(lis2dux12, reg);
|
||||
|
||||
/* check we have been able to acquire sqe */
|
||||
if (write_addr == NULL || read_reg == NULL || complete_op == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_addr, iodev, RTIO_PRIO_NORM, ®_bus, 1, NULL);
|
||||
write_addr->flags = RTIO_SQE_TRANSACTION;
|
||||
rtio_sqe_prep_read(read_reg, iodev, RTIO_PRIO_NORM, buf, len, NULL);
|
||||
read_reg->flags = RTIO_SQE_CHAINED;
|
||||
if (lis2dux12->bus_type == BUS_I2C) {
|
||||
read_reg->iodev_flags |= RTIO_IODEV_I2C_STOP | RTIO_IODEV_I2C_RESTART;
|
||||
}
|
||||
|
||||
rtio_sqe_prep_callback_no_cqe(complete_op, complete_op_cb, (void *)dev, sqe);
|
||||
rtio_submit(rtio, 0);
|
||||
}
|
||||
|
||||
void lis2dux12_submit_stream(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
const struct lis2dux12_config *config = dev->config;
|
||||
const struct lis2dux12_chip_api *chip_api = config->chip_api;
|
||||
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
|
||||
struct trigger_config trig_cfg = { 0 };
|
||||
|
||||
gpio_pin_interrupt_configure_dt(lis2dux12->drdy_gpio, GPIO_INT_DISABLE);
|
||||
|
||||
for (size_t i = 0; i < cfg->count; i++) {
|
||||
if (cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_WATERMARK) {
|
||||
trig_cfg.int_fifo_th = 1;
|
||||
} else if (cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL) {
|
||||
trig_cfg.int_fifo_full = 1;
|
||||
} else if (cfg->triggers[i].trigger == SENSOR_TRIG_DATA_READY) {
|
||||
trig_cfg.int_drdy = 1;
|
||||
}
|
||||
}
|
||||
|
||||
/* if any change in trig_cfg for FIFO triggers */
|
||||
if (trig_cfg.int_fifo_th != lis2dux12->trig_cfg.int_fifo_th ||
|
||||
trig_cfg.int_fifo_full != lis2dux12->trig_cfg.int_fifo_full) {
|
||||
lis2dux12->trig_cfg.int_fifo_th = trig_cfg.int_fifo_th;
|
||||
lis2dux12->trig_cfg.int_fifo_full = trig_cfg.int_fifo_full;
|
||||
|
||||
/* enable/disable the FIFO */
|
||||
chip_api->stream_config_fifo(dev, trig_cfg);
|
||||
}
|
||||
|
||||
/* if any change in trig_cfg for DRDY triggers */
|
||||
if (trig_cfg.int_drdy != lis2dux12->trig_cfg.int_drdy) {
|
||||
lis2dux12->trig_cfg.int_drdy = trig_cfg.int_drdy;
|
||||
|
||||
/* enable/disable drdy events */
|
||||
chip_api->stream_config_drdy(dev, trig_cfg);
|
||||
}
|
||||
|
||||
lis2dux12->streaming_sqe = iodev_sqe;
|
||||
|
||||
gpio_pin_interrupt_configure_dt(lis2dux12->drdy_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
}
|
||||
|
||||
/*
|
||||
* Called by bus driver to complete the sqe.
|
||||
*/
|
||||
static void lis2dux12_complete_op_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg)
|
||||
{
|
||||
const struct device *dev = arg;
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
|
||||
/*
|
||||
* Mark operation completed
|
||||
*/
|
||||
rtio_iodev_sqe_ok(sqe->userdata, 0);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(lis2dux12->drdy_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
}
|
||||
|
||||
/*
|
||||
* Called by bus driver to complete the LIS2DUX12_FIFO_STATUS read op (2 bytes).
|
||||
* If FIFO threshold or FIFO full events are active it reads all FIFO entries.
|
||||
*/
|
||||
static void lis2dux12_read_fifo_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg)
|
||||
{
|
||||
const struct device *dev = arg;
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
struct rtio *rtio = lis2dux12->rtio_ctx;
|
||||
struct gpio_dt_spec *irq_gpio = lis2dux12->drdy_gpio;
|
||||
struct rtio_iodev *iodev = lis2dux12->iodev;
|
||||
struct sensor_read_config *read_config;
|
||||
uint8_t fifo_th = 0, fifo_full = 0;
|
||||
uint16_t fifo_count;
|
||||
|
||||
/* At this point, no sqe request is queued should be considered as a bug */
|
||||
__ASSERT_NO_MSG(lis2dux12->streaming_sqe != NULL);
|
||||
|
||||
read_config = (struct sensor_read_config *)lis2dux12->streaming_sqe->sqe.iodev->data;
|
||||
__ASSERT_NO_MSG(read_config != NULL);
|
||||
__ASSERT_NO_MSG(read_config->is_streaming == true);
|
||||
|
||||
/* parse the configuration in search for any configured trigger */
|
||||
struct sensor_stream_trigger *fifo_ths_cfg = NULL;
|
||||
struct sensor_stream_trigger *fifo_full_cfg = NULL;
|
||||
|
||||
for (int i = 0; i < read_config->count; ++i) {
|
||||
if (read_config->triggers[i].trigger == SENSOR_TRIG_FIFO_WATERMARK) {
|
||||
fifo_ths_cfg = &read_config->triggers[i];
|
||||
continue;
|
||||
}
|
||||
|
||||
if (read_config->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL) {
|
||||
fifo_full_cfg = &read_config->triggers[i];
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
/* fill fifo h/w status */
|
||||
fifo_th = (lis2dux12->fifo_status[0] & 0x80) ? 1 : 0;
|
||||
fifo_full = (lis2dux12->fifo_status[0] & 0x40) ? 1 : 0;
|
||||
fifo_count = (uint16_t)lis2dux12->fifo_status[1];
|
||||
lis2dux12->fifo_count = fifo_count;
|
||||
|
||||
bool has_fifo_ths_trig = fifo_ths_cfg != NULL && fifo_th == 1;
|
||||
bool has_fifo_full_trig = fifo_full_cfg != NULL && fifo_full == 1;
|
||||
|
||||
/* check if no theshold/full fifo interrupt or spurious interrupts */
|
||||
if (!has_fifo_ths_trig && !has_fifo_full_trig) {
|
||||
/* complete operation with no error */
|
||||
rtio_iodev_sqe_ok(sqe->userdata, 0);
|
||||
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
/* flush completion */
|
||||
struct rtio_cqe *cqe;
|
||||
int res = 0;
|
||||
|
||||
do {
|
||||
cqe = rtio_cqe_consume(rtio);
|
||||
if (cqe != NULL) {
|
||||
if ((cqe->result < 0) && (res == 0)) {
|
||||
LOG_ERR("Bus error: %d", cqe->result);
|
||||
res = cqe->result;
|
||||
}
|
||||
rtio_cqe_release(rtio, cqe);
|
||||
}
|
||||
} while (cqe != NULL);
|
||||
|
||||
/* Bail/cancel attempt to read sensor on any error */
|
||||
if (res != 0) {
|
||||
rtio_iodev_sqe_err(lis2dux12->streaming_sqe, res);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
enum sensor_stream_data_opt data_opt;
|
||||
|
||||
if (has_fifo_ths_trig && !has_fifo_full_trig) {
|
||||
/* Only care about fifo threshold */
|
||||
data_opt = fifo_ths_cfg->opt;
|
||||
} else if (!has_fifo_ths_trig && has_fifo_full_trig) {
|
||||
/* Only care about fifo full */
|
||||
data_opt = fifo_full_cfg->opt;
|
||||
} else {
|
||||
/* Both fifo threshold and full */
|
||||
data_opt = MIN(fifo_ths_cfg->opt, fifo_full_cfg->opt);
|
||||
}
|
||||
|
||||
if (data_opt == SENSOR_STREAM_DATA_NOP || data_opt == SENSOR_STREAM_DATA_DROP) {
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
|
||||
/* Clear streaming_sqe since we're done with the call */
|
||||
if (rtio_sqe_rx_buf(lis2dux12->streaming_sqe, sizeof(struct lis2dux12_fifo_data),
|
||||
sizeof(struct lis2dux12_fifo_data), &buf, &buf_len) != 0) {
|
||||
rtio_iodev_sqe_err(lis2dux12->streaming_sqe, -ENOMEM);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
struct lis2dux12_fifo_data *rx_data = (struct lis2dux12_fifo_data *)buf;
|
||||
|
||||
memset(buf, 0, buf_len);
|
||||
rx_data->header.is_fifo = 1;
|
||||
rx_data->header.timestamp = lis2dux12->timestamp;
|
||||
rx_data->header.int_status = lis2dux12->fifo_status[0];
|
||||
rx_data->fifo_count = 0;
|
||||
|
||||
/* complete request with ok */
|
||||
rtio_iodev_sqe_ok(lis2dux12->streaming_sqe, 0);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
|
||||
if (data_opt == SENSOR_STREAM_DATA_DROP) {
|
||||
|
||||
/*
|
||||
* Flush the FIFO by setting the mode to LIS2DUX12_BYPASS_MODE
|
||||
*
|
||||
* STMEMSC API equivalent code:
|
||||
*
|
||||
* lis2dux12_fifo_mode_set(ctx, LIS2DUX12_BYPASS_MODE);
|
||||
*/
|
||||
struct rtio_sqe *write_fifo_mode = rtio_sqe_acquire(rtio);
|
||||
uint8_t lis2dux12_fifo_mode_set[] = {
|
||||
LIS2DUXXX_DT_FIFO_CTRL,
|
||||
LIS2DUXXX_DT_BYPASS_MODE,
|
||||
};
|
||||
|
||||
write_fifo_mode->flags |= RTIO_SQE_NO_RESPONSE;
|
||||
rtio_sqe_prep_tiny_write(write_fifo_mode, iodev,
|
||||
RTIO_PRIO_NORM, lis2dux12_fifo_mode_set,
|
||||
ARRAY_SIZE(lis2dux12_fifo_mode_set), NULL);
|
||||
|
||||
rtio_submit(rtio, 0);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t *buf, *read_buf;
|
||||
uint32_t buf_len, buf_avail;
|
||||
uint32_t req_len = LIS2DUX12_FIFO_SIZE(fifo_count) + sizeof(struct lis2dux12_fifo_data);
|
||||
|
||||
if (rtio_sqe_rx_buf(lis2dux12->streaming_sqe, req_len, req_len, &buf, &buf_len) != 0) {
|
||||
LOG_ERR("Failed to get buffer");
|
||||
rtio_iodev_sqe_err(lis2dux12->streaming_sqe, -ENOMEM);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
/* clang-format off */
|
||||
struct lis2dux12_fifo_data hdr = {
|
||||
.header = {
|
||||
.is_fifo = 1,
|
||||
.range = lis2dux12->range,
|
||||
.timestamp = lis2dux12->timestamp,
|
||||
.int_status = lis2dux12->fifo_status[0],
|
||||
},
|
||||
.fifo_count = fifo_count,
|
||||
.accel_batch_odr = lis2dux12->accel_batch_odr,
|
||||
.accel_odr = lis2dux12->odr,
|
||||
};
|
||||
/* clang-format on */
|
||||
|
||||
memcpy(buf, &hdr, sizeof(hdr));
|
||||
read_buf = buf + sizeof(hdr);
|
||||
buf_avail = buf_len - sizeof(hdr);
|
||||
|
||||
/*
|
||||
* Prepare rtio enabled bus to read all fifo_count entries from
|
||||
* LIS2DUX12_FIFO_DATA_OUT_TAG. Then lis2dux12_complete_op_cb
|
||||
* callback will be invoked.
|
||||
*
|
||||
* STMEMSC API equivalent code:
|
||||
*
|
||||
* num = fifo_status.fifo_level;
|
||||
*
|
||||
* while (num--) {
|
||||
* lis2dux12_fifo_out_raw_t f_data;
|
||||
*
|
||||
* lis2dux12_fifo_out_raw_get(&dev_ctx, &f_data);
|
||||
* }
|
||||
*/
|
||||
lis2dux12_rtio_rw_transaction(dev, LIS2DUXXX_DT_FIFO_DATA_OUT_TAG,
|
||||
read_buf, buf_avail, lis2dux12_complete_op_cb);
|
||||
}
|
||||
|
||||
/*
|
||||
* Called by bus driver to complete the LIS2DUX12_STATUS_REG read op.
|
||||
* If drdy_xl is active it reads XL data (6 bytes) from LIS2DUX12_OUTX_L_A reg.
|
||||
*/
|
||||
static void lis2dux12_read_status_cb(struct rtio *r, const struct rtio_sqe *sqe, void *arg)
|
||||
{
|
||||
const struct device *dev = arg;
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
struct rtio *rtio = lis2dux12->rtio_ctx;
|
||||
struct gpio_dt_spec *irq_gpio = lis2dux12->drdy_gpio;
|
||||
struct sensor_read_config *read_config;
|
||||
|
||||
/* At this point, no sqe request is queued should be considered as a bug */
|
||||
__ASSERT_NO_MSG(lis2dux12->streaming_sqe != NULL);
|
||||
|
||||
read_config = (struct sensor_read_config *)lis2dux12->streaming_sqe->sqe.iodev->data;
|
||||
__ASSERT_NO_MSG(read_config != NULL);
|
||||
__ASSERT_NO_MSG(read_config->is_streaming == true);
|
||||
|
||||
/* parse the configuration in search for any configured trigger */
|
||||
struct sensor_stream_trigger *data_ready = NULL;
|
||||
|
||||
for (int i = 0; i < read_config->count; ++i) {
|
||||
if (read_config->triggers[i].trigger == SENSOR_TRIG_DATA_READY) {
|
||||
data_ready = &read_config->triggers[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* flush completion */
|
||||
struct rtio_cqe *cqe;
|
||||
int res = 0;
|
||||
|
||||
do {
|
||||
cqe = rtio_cqe_consume(rtio);
|
||||
if (cqe != NULL) {
|
||||
if ((cqe->result < 0) && (res == 0)) {
|
||||
LOG_ERR("Bus error: %d", cqe->result);
|
||||
res = cqe->result;
|
||||
}
|
||||
rtio_cqe_release(rtio, cqe);
|
||||
}
|
||||
} while (cqe != NULL);
|
||||
|
||||
/* Bail/cancel attempt to read sensor on any error */
|
||||
if (res != 0) {
|
||||
rtio_iodev_sqe_err(lis2dux12->streaming_sqe, res);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
return;
|
||||
}
|
||||
|
||||
if (data_ready->opt == SENSOR_STREAM_DATA_NOP ||
|
||||
data_ready->opt == SENSOR_STREAM_DATA_DROP) {
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
|
||||
/* Clear streaming_sqe since we're done with the call */
|
||||
if (rtio_sqe_rx_buf(lis2dux12->streaming_sqe, sizeof(struct lis2dux12_rtio_data),
|
||||
sizeof(struct lis2dux12_rtio_data), &buf, &buf_len) != 0) {
|
||||
rtio_iodev_sqe_err(lis2dux12->streaming_sqe, -ENOMEM);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
struct lis2dux12_rtio_data *rx_data = (struct lis2dux12_rtio_data *)buf;
|
||||
|
||||
memset(buf, 0, buf_len);
|
||||
rx_data->header.is_fifo = 0;
|
||||
rx_data->header.timestamp = lis2dux12->timestamp;
|
||||
rx_data->has_accel = 0;
|
||||
rx_data->has_temp = 0;
|
||||
|
||||
/* complete request with ok */
|
||||
rtio_iodev_sqe_ok(lis2dux12->streaming_sqe, 0);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
}
|
||||
|
||||
/*
|
||||
* Read XL data
|
||||
*
|
||||
* lis2dux12_data_ready_t drdy;
|
||||
* if (drdy.drdy_xl) {
|
||||
*/
|
||||
if (lis2dux12->status & 0x1) {
|
||||
uint8_t *buf, *read_buf;
|
||||
uint32_t buf_len;
|
||||
uint32_t req_len = 6 + sizeof(struct lis2dux12_rtio_data);
|
||||
|
||||
if (rtio_sqe_rx_buf(lis2dux12->streaming_sqe,
|
||||
req_len, req_len, &buf, &buf_len) != 0) {
|
||||
LOG_ERR("Failed to get buffer");
|
||||
rtio_iodev_sqe_err(lis2dux12->streaming_sqe, -ENOMEM);
|
||||
lis2dux12->streaming_sqe = NULL;
|
||||
gpio_pin_interrupt_configure_dt(irq_gpio, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
/* clang-format off */
|
||||
struct lis2dux12_rtio_data hdr = {
|
||||
.header = {
|
||||
.is_fifo = 0,
|
||||
.range = lis2dux12->range,
|
||||
.timestamp = lis2dux12->timestamp,
|
||||
.int_status = lis2dux12->status,
|
||||
},
|
||||
.has_accel = 1,
|
||||
.has_temp = 0,
|
||||
};
|
||||
/* clang-format on */
|
||||
|
||||
memcpy(buf, &hdr, sizeof(hdr));
|
||||
read_buf = (uint8_t *)&((struct lis2dux12_rtio_data *)buf)->acc[0];
|
||||
|
||||
/*
|
||||
* Prepare rtio enabled bus to read LIS2DUX12_OUTX_L_A register
|
||||
* where accelerometer data is available.
|
||||
* Then lis2dux12_complete_op_cb callback will be invoked.
|
||||
*
|
||||
* STMEMSC API equivalent code:
|
||||
*
|
||||
* uint8_t accel_raw[6];
|
||||
*
|
||||
* lis2dux12_acceleration_raw_get(&dev_ctx, accel_raw);
|
||||
*/
|
||||
lis2dux12_rtio_rw_transaction(dev, LIS2DUXXX_DT_OUTX_L,
|
||||
read_buf, 6, lis2dux12_complete_op_cb);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Called when one of the following trigger is active:
|
||||
*
|
||||
* - int_fifo_th (SENSOR_TRIG_FIFO_WATERMARK)
|
||||
* - int_fifo_full (SENSOR_TRIG_FIFO_FULL)
|
||||
* - int_drdy (SENSOR_TRIG_DATA_READY)
|
||||
*/
|
||||
void lis2dux12_stream_irq_handler(const struct device *dev)
|
||||
{
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
uint64_t cycles;
|
||||
int rc;
|
||||
|
||||
if (lis2dux12->streaming_sqe == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
rc = sensor_clock_get_cycles(&cycles);
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to get sensor clock cycles");
|
||||
rtio_iodev_sqe_err(lis2dux12->streaming_sqe, rc);
|
||||
return;
|
||||
}
|
||||
|
||||
/* get timestamp as soon as the irq is served */
|
||||
lis2dux12->timestamp = sensor_clock_cycles_to_ns(cycles);
|
||||
|
||||
/* handle FIFO triggers */
|
||||
if (lis2dux12->trig_cfg.int_fifo_th || lis2dux12->trig_cfg.int_fifo_full) {
|
||||
lis2dux12->fifo_status[0] = lis2dux12->fifo_status[1] = 0;
|
||||
|
||||
/*
|
||||
* Prepare rtio enabled bus to read LIS2DUX12_FIFO_STATUS1 and
|
||||
* LIS2DUX12_FIFO_STATUS2 registers where FIFO threshold condition and
|
||||
* count are reported. Then lis2dux12_read_fifo_cb callback will be
|
||||
* invoked.
|
||||
*
|
||||
* STMEMSC API equivalent code:
|
||||
*
|
||||
* uint8_t wmflag = 0;
|
||||
* lis2duxs12_fifo_wtm_flag_get(&dev_ctx, &wmflag);
|
||||
*
|
||||
* uint16_t num;
|
||||
* lis2duxs12_fifo_data_level_get(&dev_ctx, &num);
|
||||
*/
|
||||
lis2dux12_rtio_rw_transaction(dev, LIS2DUXXX_DT_FIFO_STATUS1,
|
||||
lis2dux12->fifo_status, 2, lis2dux12_read_fifo_cb);
|
||||
}
|
||||
|
||||
/* handle drdy trigger */
|
||||
if (lis2dux12->trig_cfg.int_drdy) {
|
||||
lis2dux12->status = 0;
|
||||
|
||||
/*
|
||||
* Prepare rtio enabled bus to read LIS2DUX12_STATUS_REG register
|
||||
* where accelerometer and gyroscope data ready status is available.
|
||||
* Then lis2dux12_read_status_cb callback will be invoked.
|
||||
*
|
||||
* STMEMSC API equivalent code:
|
||||
*
|
||||
* lis2dux12_data_ready_t drdy;
|
||||
*
|
||||
* lis2dux12_flag_data_ready_get(&dev_ctx, &drdy);
|
||||
*/
|
||||
lis2dux12_rtio_rw_transaction(dev, LIS2DUXXX_DT_STATUS,
|
||||
&lis2dux12->status, 1, lis2dux12_read_status_cb);
|
||||
}
|
||||
}
|
|
@ -11,6 +11,9 @@
|
|||
#include <zephyr/logging/log.h>
|
||||
#include "lis2dux12.h"
|
||||
|
||||
#include "lis2dux12.h"
|
||||
#include "lis2dux12_rtio.h"
|
||||
|
||||
LOG_MODULE_DECLARE(LIS2DUX12, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
static void lis2dux12_gpio_callback(const struct device *dev, struct gpio_callback *cb,
|
||||
|
@ -31,6 +34,9 @@ static void lis2dux12_gpio_callback(const struct device *dev, struct gpio_callba
|
|||
#elif defined(CONFIG_LIS2DUX12_TRIGGER_GLOBAL_THREAD)
|
||||
k_work_submit(&data->work);
|
||||
#endif
|
||||
if (IS_ENABLED(CONFIG_LIS2DUX12_STREAM)) {
|
||||
lis2dux12_stream_irq_handler(data->dev);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER_OWN_THREAD
|
||||
|
|
|
@ -101,6 +101,121 @@ static int32_t st_lis2duxs12_sample_fetch_temp(const struct device *dev)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
static int32_t st_lis2duxs12_rtio_read_accel(const struct device *dev, int16_t *acc)
|
||||
{
|
||||
struct lis2dux12_data *data = dev->data;
|
||||
const struct lis2dux12_config *cfg = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
|
||||
|
||||
/* fetch raw data sample */
|
||||
lis2duxs12_md_t mode = {.fs = data->range};
|
||||
lis2duxs12_xl_data_t xzy_data = {0};
|
||||
|
||||
if (lis2duxs12_xl_data_get(ctx, &mode, &xzy_data) < 0) {
|
||||
LOG_ERR("Failed to fetch raw data sample");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
acc[0] = xzy_data.raw[0];
|
||||
acc[1] = xzy_data.raw[1];
|
||||
acc[2] = xzy_data.raw[2];
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int32_t st_lis2duxs12_rtio_read_temp(const struct device *dev, int16_t *temp)
|
||||
{
|
||||
const struct lis2dux12_config *cfg = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&cfg->ctx;
|
||||
|
||||
/* fetch raw data sample */
|
||||
lis2duxs12_outt_data_t temp_data = {0};
|
||||
|
||||
if (lis2duxs12_outt_data_get(ctx, &temp_data) < 0) {
|
||||
LOG_ERR("Failed to fetch raw temperature data sample");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
*temp = temp_data.heat.raw;
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
static void st_lis2duxs12_stream_config_fifo(const struct device *dev,
|
||||
struct trigger_config trig_cfg)
|
||||
{
|
||||
struct lis2dux12_data *lis2dux12 = dev->data;
|
||||
const struct lis2dux12_config *config = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&config->ctx;
|
||||
lis2duxs12_pin_int_route_t pin_int = { 0 };
|
||||
lis2duxs12_fifo_mode_t fifo_mode;
|
||||
|
||||
/* disable FIFO as first thing */
|
||||
fifo_mode.store = LIS2DUXS12_FIFO_1X;
|
||||
fifo_mode.xl_only = 1;
|
||||
fifo_mode.watermark = 0;
|
||||
fifo_mode.operation = LIS2DUXS12_BYPASS_MODE;
|
||||
fifo_mode.batch.dec_ts = LIS2DUXS12_DEC_TS_OFF;
|
||||
fifo_mode.batch.bdr_xl = LIS2DUXS12_BDR_XL_ODR_OFF;
|
||||
|
||||
pin_int.fifo_th = PROPERTY_DISABLE;
|
||||
pin_int.fifo_full = PROPERTY_DISABLE;
|
||||
|
||||
if (trig_cfg.int_fifo_th || trig_cfg.int_fifo_full) {
|
||||
pin_int.fifo_th = (trig_cfg.int_fifo_th) ? PROPERTY_ENABLE : PROPERTY_DISABLE;
|
||||
pin_int.fifo_full = (trig_cfg.int_fifo_full) ? PROPERTY_ENABLE : PROPERTY_DISABLE;
|
||||
|
||||
fifo_mode.operation = LIS2DUXS12_STREAM_MODE;
|
||||
fifo_mode.batch.dec_ts = config->ts_batch;
|
||||
fifo_mode.batch.bdr_xl = config->accel_batch;
|
||||
fifo_mode.watermark = config->fifo_wtm;
|
||||
}
|
||||
|
||||
/*
|
||||
* Set FIFO watermark (number of unread sensor data TAG + 6 bytes
|
||||
* stored in FIFO) to FIFO_WATERMARK samples
|
||||
*/
|
||||
lis2duxs12_fifo_mode_set(ctx, fifo_mode);
|
||||
|
||||
/* Set FIFO batch rates */
|
||||
lis2dux12->accel_batch_odr = fifo_mode.batch.bdr_xl;
|
||||
lis2dux12->ts_batch_odr = fifo_mode.batch.dec_ts;
|
||||
|
||||
/* Set pin interrupt (fifo_th could be on or off) */
|
||||
if (config->drdy_pin == 1) {
|
||||
lis2duxs12_pin_int1_route_set(ctx, &pin_int);
|
||||
} else {
|
||||
lis2duxs12_pin_int2_route_set(ctx, &pin_int);
|
||||
}
|
||||
}
|
||||
|
||||
static void st_lis2duxs12_stream_config_drdy(const struct device *dev,
|
||||
struct trigger_config trig_cfg)
|
||||
{
|
||||
const struct lis2dux12_config *config = dev->config;
|
||||
stmdev_ctx_t *ctx = (stmdev_ctx_t *)&config->ctx;
|
||||
lis2duxs12_pin_int_route_t pin_int = { 0 };
|
||||
|
||||
/* dummy read: re-trigger interrupt */
|
||||
lis2duxs12_md_t md = {.fs = LIS2DUXS12_2g};
|
||||
lis2duxs12_xl_data_t buf;
|
||||
|
||||
lis2duxs12_xl_data_get(ctx, &md, &buf);
|
||||
|
||||
pin_int.drdy = PROPERTY_ENABLE;
|
||||
|
||||
/* Set pin interrupt */
|
||||
if (config->drdy_pin == 1) {
|
||||
lis2duxs12_pin_int1_route_set(ctx, &pin_int);
|
||||
} else {
|
||||
lis2duxs12_pin_int2_route_set(ctx, &pin_int);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
static void st_lis2duxs12_handle_interrupt(const struct device *dev)
|
||||
{
|
||||
|
@ -164,6 +279,14 @@ const struct lis2dux12_chip_api st_lis2duxs12_chip_api = {
|
|||
#ifdef CONFIG_LIS2DUX12_ENABLE_TEMP
|
||||
.sample_fetch_temp = st_lis2duxs12_sample_fetch_temp,
|
||||
#endif
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
.rtio_read_accel = st_lis2duxs12_rtio_read_accel,
|
||||
.rtio_read_temp = st_lis2duxs12_rtio_read_temp,
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_STREAM
|
||||
.stream_config_fifo = st_lis2duxs12_stream_config_fifo,
|
||||
.stream_config_drdy = st_lis2duxs12_stream_config_drdy,
|
||||
#endif
|
||||
#ifdef CONFIG_LIS2DUX12_TRIGGER
|
||||
.handle_interrupt = st_lis2duxs12_handle_interrupt,
|
||||
.init_interrupt = st_lis2duxs12_init_interrupt,
|
||||
|
|
|
@ -93,3 +93,45 @@ properties:
|
|||
- 11 # LIS2DUX12_DT_ODR_800Hz
|
||||
|
||||
enum: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
|
||||
|
||||
fifo-watermark:
|
||||
type: int
|
||||
default: 32
|
||||
description: |
|
||||
Specify the default FIFO watermark threshold. Every unit indicates a FIFO row (1 byte of TAG +
|
||||
6 bytes of data). (min 0; max 255)
|
||||
A typical threshold value is 32 which is then used as default.
|
||||
|
||||
accel-fifo-batch-rate:
|
||||
type: int
|
||||
default: 0x0
|
||||
description: |
|
||||
Specify the default accelerometer FIFO batch data rate expressed as a fraction of the odr.
|
||||
The values are taken in accordance to lis2dux12_bdr_xl_t enumerative in hal/st module.
|
||||
Default is power-up configuration.
|
||||
|
||||
- 0x0 # LIS2DUX12_DT_BDR_XL_ODR
|
||||
- 0x1 # LIS2DUX12_DT_BDR_XL_ODR_DIV_2
|
||||
- 0x2 # LIS2DUX12_DT_BDR_XL_ODR_DIV_4
|
||||
- 0x3 # LIS2DUX12_DT_BDR_XL_ODR_DIV_8
|
||||
- 0x4 # LIS2DUX12_DT_BDR_XL_ODR_DIV_16
|
||||
- 0x5 # LIS2DUX12_DT_BDR_XL_ODR_DIV_32
|
||||
- 0x6 # LIS2DUX12_DT_BDR_XL_ODR_DIV_64
|
||||
- 0x7 # LIS2DUX12_DT_BDR_XL_ODR_OFF
|
||||
|
||||
enum: [0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07]
|
||||
|
||||
timestamp-fifo-batch-rate:
|
||||
type: int
|
||||
default: 0x0
|
||||
description: |
|
||||
Specify the default timestamp FIFO batch data rate expressed as a fraction od the odr.
|
||||
The values are taken in accordance to lis2dux12_dec_ts_t enumerative in hal/st module.
|
||||
Default is power-up configuration.
|
||||
|
||||
- 0x0 # LIS2DUX12_DT_DEC_TS_OFF
|
||||
- 0x1 # LIS2DUX12_DT_DEC_TS_1
|
||||
- 0x2 # LIS2DUX12_DT_DEC_TS_8
|
||||
- 0x3 # LIS2DUX12_DT_DEC_TS_32
|
||||
|
||||
enum: [0x00, 0x01, 0x02, 0x03]
|
||||
|
|
|
@ -35,4 +35,47 @@
|
|||
#define LIS2DUX12_DT_FS_8G 2 /* 8g (0.244 mg/LSB) */
|
||||
#define LIS2DUX12_DT_FS_16G 3 /* 16g (0.488 mg/LSB) */
|
||||
|
||||
/* Accelerometer FIFO batching data rate */
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR 0x0
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR_DIV_2 0x1
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR_DIV_4 0x2
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR_DIV_8 0x3
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR_DIV_16 0x4
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR_DIV_32 0x5
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR_DIV_64 0x6
|
||||
#define LIS2DUX12_DT_BDR_XL_ODR_OFF 0x7
|
||||
|
||||
/* Accelerometer FIFO timestamp ratio */
|
||||
#define LIS2DUX12_DT_DEC_TS_OFF 0x0
|
||||
#define LIS2DUX12_DT_DEC_TS_1 0x1
|
||||
#define LIS2DUX12_DT_DEC_TS_8 0x2
|
||||
#define LIS2DUX12_DT_DEC_TS_32 0x3
|
||||
|
||||
/* Accelerometer FIFO tags (aligned with lis2dux12_fifo_sensor_tag_t) */
|
||||
#define LIS2DUXXX_FIFO_EMPTY 0x0
|
||||
#define LIS2DUXXX_XL_TEMP_TAG 0x2
|
||||
#define LIS2DUXXX_XL_ONLY_2X_TAG 0x3
|
||||
#define LIS2DUXXX_TIMESTAMP_TAG 0x4
|
||||
#define LIS2DUXXX_STEP_COUNTER_TAG 0x12
|
||||
#define LIS2DUXXX_MLC_RESULT_TAG 0x1A
|
||||
#define LIS2DUXXX_MLC_FILTER_TAG 0x1B
|
||||
#define LIS2DUXXX_MLC_FEATURE 0x1C
|
||||
#define LIS2DUXXX_FSM_RESULT_TAG 0x1D
|
||||
|
||||
/* Accelerometer FIFO modes (aligned with lis2dux12_operation_t) */
|
||||
#define LIS2DUXXX_DT_BYPASS_MODE 0x0
|
||||
#define LIS2DUXXX_DT_FIFO_MODE 0x1
|
||||
#define LIS2DUXXX_DT_STREAM_TO_FIFO_MODE 0x3
|
||||
#define LIS2DUXXX_DT_BYPASS_TO_STREAM_MODE 0x4
|
||||
#define LIS2DUXXX_DT_STREAM_MODE 0x6
|
||||
#define LIS2DUXXX_DT_BYPASS_TO_FIFO_MODE 0x7
|
||||
#define LIS2DUXXX_DT_FIFO_OFF 0x8
|
||||
|
||||
/* Accelerometer registers */
|
||||
#define LIS2DUXXX_DT_FIFO_CTRL 0x15U
|
||||
#define LIS2DUXXX_DT_STATUS 0x25U
|
||||
#define LIS2DUXXX_DT_FIFO_STATUS1 0x26U
|
||||
#define LIS2DUXXX_DT_OUTX_L 0x28U
|
||||
#define LIS2DUXXX_DT_FIFO_DATA_OUT_TAG 0x40U
|
||||
|
||||
#endif /* ZEPHYR_INCLUDE_DT_BINDINGS_LIS2DUX12_H_ */
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue