drivers: sensor: adxl372: Updated driver with RTIO stream functionality
Updated ADXL372 driver with RTIO stream functionality. RTIO stream is using both FIFO threshold and FIFO full triggers. Together with RTIO stream, RTIO async read is also implemented. Signed-off-by: Vladislav Pejic <vladislav.pejic@orioninc.com>
This commit is contained in:
parent
9356999229
commit
cfe64f7f1c
8 changed files with 957 additions and 21 deletions
|
@ -9,3 +9,5 @@ zephyr_library_sources(adxl372.c)
|
|||
zephyr_library_sources(adxl372_spi.c)
|
||||
zephyr_library_sources(adxl372_i2c.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_ADXL372_TRIGGER adxl372_trigger.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_SENSOR_ASYNC_API adxl372_rtio.c adxl372_decoder.c)
|
||||
zephyr_library_sources_ifdef(CONFIG_ADXL372_STREAM adxl372_stream.c adxl372_decoder.c)
|
||||
|
|
|
@ -9,6 +9,7 @@ menuconfig ADXL372
|
|||
depends on DT_HAS_ADI_ADXL372_ENABLED
|
||||
select I2C if $(dt_compat_on_bus,$(DT_COMPAT_ADI_ADXL372),i2c)
|
||||
select SPI if $(dt_compat_on_bus,$(DT_COMPAT_ADI_ADXL372),spi)
|
||||
select RTIO_WORKQ if SENSOR_ASYNC_API
|
||||
help
|
||||
Enable driver for ADXL372 Three-Axis Digital Accelerometers.
|
||||
|
||||
|
@ -106,6 +107,15 @@ config ADXL372_TRIGGER_OWN_THREAD
|
|||
|
||||
endchoice
|
||||
|
||||
config ADXL372_STREAM
|
||||
bool "Use FIFO to stream data"
|
||||
select ADXL372_TRIGGER
|
||||
default y
|
||||
depends on SPI_RTIO
|
||||
depends on SENSOR_ASYNC_API
|
||||
help
|
||||
Use this configuration option to enable streaming sensor data via RTIO.
|
||||
|
||||
config ADXL372_TRIGGER
|
||||
bool
|
||||
|
||||
|
|
|
@ -87,14 +87,22 @@ static int adxl372_set_activity_threshold_xyz(const struct device *dev,
|
|||
* ADXL372_FULL_BW_MEASUREMENT
|
||||
* @return 0 in case of success, negative error code otherwise.
|
||||
*/
|
||||
static int adxl372_set_op_mode(const struct device *dev,
|
||||
enum adxl372_op_mode op_mode)
|
||||
int adxl372_set_op_mode(const struct device *dev, enum adxl372_op_mode op_mode)
|
||||
{
|
||||
struct adxl372_data *data = dev->data;
|
||||
|
||||
return data->hw_tf->write_reg_mask(dev, ADXL372_POWER_CTL,
|
||||
int ret = data->hw_tf->write_reg_mask(dev, ADXL372_POWER_CTL,
|
||||
ADXL372_POWER_CTL_MODE_MSK,
|
||||
ADXL372_POWER_CTL_MODE(op_mode));
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
if (ret == 0) {
|
||||
data->pwr_reg &= ~ADXL372_POWER_CTL_MODE_MSK;
|
||||
data->pwr_reg |= ADXL372_POWER_CTL_MODE(op_mode);
|
||||
}
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -145,6 +153,11 @@ static int adxl372_set_bandwidth(const struct device *dev,
|
|||
return ret;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
data->pwr_reg &= ~ADXL372_POWER_CTL_LPF_DIS_MSK;
|
||||
data->pwr_reg |= mask;
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
return data->hw_tf->write_reg_mask(dev, ADXL372_MEASURE,
|
||||
ADXL372_MEASURE_BANDWIDTH_MSK,
|
||||
ADXL372_MEASURE_BANDWIDTH_MODE(bw));
|
||||
|
@ -181,6 +194,11 @@ static int adxl372_set_hpf_corner(const struct device *dev,
|
|||
return ret;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
data->pwr_reg &= ~ADXL372_POWER_CTL_HPF_DIS_MSK;
|
||||
data->pwr_reg |= mask;
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
return data->hw_tf->write_reg(dev, ADXL372_HPF, ADXL372_HPF_CORNER(c));
|
||||
}
|
||||
|
||||
|
@ -237,9 +255,18 @@ static int adxl372_set_instant_on_th(const struct device *dev,
|
|||
{
|
||||
struct adxl372_data *data = dev->data;
|
||||
|
||||
return data->hw_tf->write_reg_mask(dev, ADXL372_POWER_CTL,
|
||||
int ret = data->hw_tf->write_reg_mask(dev, ADXL372_POWER_CTL,
|
||||
ADXL372_POWER_CTL_INSTANT_ON_TH_MSK,
|
||||
ADXL372_POWER_CTL_INSTANT_ON_TH_MODE(mode));
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
if (ret == 0) {
|
||||
data->pwr_reg &= ~ADXL372_POWER_CTL_INSTANT_ON_TH_MSK;
|
||||
data->pwr_reg |= ADXL372_POWER_CTL_INSTANT_ON_TH_MODE(mode);
|
||||
}
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -313,9 +340,18 @@ static int adxl372_set_filter_settle(const struct device *dev,
|
|||
{
|
||||
struct adxl372_data *data = dev->data;
|
||||
|
||||
return data->hw_tf->write_reg_mask(dev, ADXL372_POWER_CTL,
|
||||
int ret = data->hw_tf->write_reg_mask(dev, ADXL372_POWER_CTL,
|
||||
ADXL372_POWER_CTL_FIL_SETTLE_MSK,
|
||||
ADXL372_POWER_CTL_FIL_SETTLE_MODE(mode));
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
if (ret == 0) {
|
||||
data->pwr_reg &= ~ADXL372_POWER_CTL_FIL_SETTLE_MSK;
|
||||
data->pwr_reg |= ADXL372_POWER_CTL_FIL_SETTLE_MODE(mode);
|
||||
}
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -432,13 +468,10 @@ static int adxl372_reset(const struct device *dev)
|
|||
* @param fifo_samples - FIFO Samples. Watermark number of FIFO samples that
|
||||
* triggers a FIFO_FULL condition when reached.
|
||||
* Values range from 0 to 512.
|
||||
|
||||
* @return 0 in case of success, negative error code otherwise.
|
||||
*/
|
||||
static int adxl372_configure_fifo(const struct device *dev,
|
||||
enum adxl372_fifo_mode mode,
|
||||
enum adxl372_fifo_format format,
|
||||
uint16_t fifo_samples)
|
||||
int adxl372_configure_fifo(const struct device *dev, enum adxl372_fifo_mode mode,
|
||||
enum adxl372_fifo_format format, uint16_t fifo_samples)
|
||||
{
|
||||
struct adxl372_data *data = dev->data;
|
||||
uint8_t fifo_config;
|
||||
|
@ -485,8 +518,8 @@ static int adxl372_configure_fifo(const struct device *dev,
|
|||
* where (x, y, z) acceleration data will be stored.
|
||||
* @return 0 in case of success, negative error code otherwise.
|
||||
*/
|
||||
static int adxl372_get_accel_data(const struct device *dev, bool maxpeak,
|
||||
struct adxl372_xyz_accel_data *accel_data)
|
||||
int adxl372_get_accel_data(const struct device *dev, bool maxpeak,
|
||||
struct adxl372_xyz_accel_data *accel_data)
|
||||
{
|
||||
struct adxl372_data *data = dev->data;
|
||||
uint8_t buf[6];
|
||||
|
@ -501,7 +534,9 @@ static int adxl372_get_accel_data(const struct device *dev, bool maxpeak,
|
|||
|
||||
ret = data->hw_tf->read_reg_multiple(dev, maxpeak ? ADXL372_X_MAXPEAK_H :
|
||||
ADXL372_X_DATA_H, buf, 6);
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
accel_data->is_fifo = 0;
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
accel_data->x = (buf[0] << 8) | (buf[1] & 0xF0);
|
||||
accel_data->y = (buf[2] << 8) | (buf[3] & 0xF0);
|
||||
accel_data->z = (buf[4] << 8) | (buf[5] & 0xF0);
|
||||
|
@ -515,6 +550,7 @@ static int adxl372_attr_set_odr(const struct device *dev,
|
|||
const struct sensor_value *val)
|
||||
{
|
||||
enum adxl372_odr odr;
|
||||
struct adxl372_dev_config *cfg = (struct adxl372_dev_config *)dev->config;
|
||||
|
||||
switch (val->val1) {
|
||||
case 400:
|
||||
|
@ -536,7 +572,13 @@ static int adxl372_attr_set_odr(const struct device *dev,
|
|||
return -EINVAL;
|
||||
}
|
||||
|
||||
return adxl372_set_odr(dev, odr);
|
||||
int ret = adxl372_set_odr(dev, odr);
|
||||
|
||||
if (ret == 0) {
|
||||
cfg->odr = odr;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int adxl372_attr_set_thresh(const struct device *dev,
|
||||
|
@ -610,7 +652,7 @@ static int adxl372_sample_fetch(const struct device *dev,
|
|||
&data->sample);
|
||||
}
|
||||
|
||||
static void adxl372_accel_convert(struct sensor_value *val, int16_t value)
|
||||
void adxl372_accel_convert(struct sensor_value *val, int16_t value)
|
||||
{
|
||||
/*
|
||||
* Sensor resolution is 100mg/LSB, 12-bit value needs to be right
|
||||
|
@ -651,12 +693,16 @@ static int adxl372_channel_get(const struct device *dev,
|
|||
}
|
||||
|
||||
static const struct sensor_driver_api adxl372_api_funcs = {
|
||||
.attr_set = adxl372_attr_set,
|
||||
.attr_set = adxl372_attr_set,
|
||||
.sample_fetch = adxl372_sample_fetch,
|
||||
.channel_get = adxl372_channel_get,
|
||||
.channel_get = adxl372_channel_get,
|
||||
#ifdef CONFIG_ADXL372_TRIGGER
|
||||
.trigger_set = adxl372_trigger_set,
|
||||
#endif
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
.submit = adxl372_submit,
|
||||
.get_decoder = adxl372_get_decoder,
|
||||
#endif /* CONFIG_SENSOR_ASYNC_API */
|
||||
|
||||
};
|
||||
|
||||
|
@ -822,6 +868,11 @@ static int adxl372_init(const struct device *dev)
|
|||
/*
|
||||
* Instantiation macros used when a device is on a SPI bus.
|
||||
*/
|
||||
#define ADXL372_SPI_CFG SPI_WORD_SET(8) | SPI_TRANSFER_MSB
|
||||
|
||||
#define ADXL372_RTIO_DEFINE(inst) \
|
||||
SPI_DT_IODEV_DEFINE(adxl372_iodev_##inst, DT_DRV_INST(inst), ADXL372_SPI_CFG, 0U); \
|
||||
RTIO_DEFINE(adxl372_rtio_ctx_##inst, 16, 16);
|
||||
|
||||
#ifdef CONFIG_ADXL372_TRIGGER
|
||||
#define ADXL372_CFG_IRQ(inst) \
|
||||
|
@ -857,15 +908,18 @@ static int adxl372_init(const struct device *dev)
|
|||
#define ADXL372_CONFIG_SPI(inst) \
|
||||
{ \
|
||||
.bus_init = adxl372_spi_init, \
|
||||
.spi = SPI_DT_SPEC_INST_GET(inst, SPI_WORD_SET(8) | \
|
||||
SPI_TRANSFER_MSB, 0), \
|
||||
.spi = SPI_DT_SPEC_INST_GET(inst, ADXL372_SPI_CFG, 0), \
|
||||
ADXL372_CONFIG(inst) \
|
||||
COND_CODE_1(DT_INST_NODE_HAS_PROP(inst, int1_gpios), \
|
||||
(ADXL372_CFG_IRQ(inst)), ()) \
|
||||
}
|
||||
|
||||
#define ADXL372_DEFINE_SPI(inst) \
|
||||
static struct adxl372_data adxl372_data_##inst; \
|
||||
IF_ENABLED(CONFIG_ADXL372_STREAM, (ADXL372_RTIO_DEFINE(inst))); \
|
||||
static struct adxl372_data adxl372_data_##inst = { \
|
||||
IF_ENABLED(CONFIG_ADXL372_STREAM, (.rtio_ctx = &adxl372_rtio_ctx_##inst, \
|
||||
.iodev = &adxl372_iodev_##inst,)) \
|
||||
}; \
|
||||
static const struct adxl372_dev_config adxl372_config_##inst = \
|
||||
ADXL372_CONFIG_SPI(inst); \
|
||||
ADXL372_DEVICE_INIT(inst)
|
||||
|
|
|
@ -14,6 +14,12 @@
|
|||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/sys/util.h>
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
#include <zephyr/rtio/rtio.h>
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
#define DT_DRV_COMPAT adi_adxl372
|
||||
|
||||
#if DT_ANY_INST_ON_BUS_STATUS_OKAY(spi)
|
||||
#include <zephyr/drivers/spi.h>
|
||||
#endif /* DT_ANY_INST_ON_BUS_STATUS_OKAY(spi) */
|
||||
|
@ -281,6 +287,10 @@ struct adxl372_activity_threshold {
|
|||
};
|
||||
|
||||
struct adxl372_xyz_accel_data {
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
uint8_t is_fifo: 1;
|
||||
uint8_t res: 7;
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
|
@ -319,6 +329,16 @@ struct adxl372_data {
|
|||
struct k_work work;
|
||||
#endif
|
||||
#endif /* CONFIG_ADXL372_TRIGGER */
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
struct rtio_iodev_sqe *sqe;
|
||||
struct rtio *rtio_ctx;
|
||||
struct rtio_iodev *iodev;
|
||||
uint8_t status1;
|
||||
uint8_t fifo_ent[2];
|
||||
uint64_t timestamp;
|
||||
uint8_t fifo_full_irq;
|
||||
uint8_t pwr_reg;
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
};
|
||||
|
||||
struct adxl372_dev_config {
|
||||
|
@ -358,9 +378,27 @@ struct adxl372_dev_config {
|
|||
uint8_t int2_config;
|
||||
};
|
||||
|
||||
struct adxl372_fifo_data {
|
||||
uint8_t is_fifo: 1;
|
||||
uint8_t sample_set_size: 4;
|
||||
uint8_t has_x: 1;
|
||||
uint8_t has_y: 1;
|
||||
uint8_t has_z: 1;
|
||||
uint8_t int_status;
|
||||
uint16_t accel_odr: 4;
|
||||
uint16_t fifo_byte_count: 12;
|
||||
uint64_t timestamp;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
BUILD_ASSERT(sizeof(struct adxl372_fifo_data) % 4 == 0,
|
||||
"adxl372_fifo_data struct should be word aligned");
|
||||
|
||||
int adxl372_spi_init(const struct device *dev);
|
||||
int adxl372_i2c_init(const struct device *dev);
|
||||
|
||||
void adxl372_submit_stream(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe);
|
||||
void adxl372_stream_irq_handler(const struct device *dev);
|
||||
|
||||
#ifdef CONFIG_ADXL372_TRIGGER
|
||||
int adxl372_get_status(const struct device *dev,
|
||||
uint8_t *status1, uint8_t *status2, uint16_t *fifo_entries);
|
||||
|
@ -372,4 +410,19 @@ int adxl372_trigger_set(const struct device *dev,
|
|||
int adxl372_init_interrupt(const struct device *dev);
|
||||
#endif /* CONFIG_ADXL372_TRIGGER */
|
||||
|
||||
#ifdef CONFIG_SENSOR_ASYNC_API
|
||||
int adxl372_get_accel_data(const struct device *dev, bool maxpeak,
|
||||
struct adxl372_xyz_accel_data *accel_data);
|
||||
void adxl372_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe);
|
||||
int adxl372_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder);
|
||||
void adxl372_accel_convert(struct sensor_value *val, int16_t sample);
|
||||
#endif /* CONFIG_SENSOR_ASYNC_API */
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
int adxl372_configure_fifo(const struct device *dev, enum adxl372_fifo_mode mode,
|
||||
enum adxl372_fifo_format format, uint16_t fifo_samples);
|
||||
size_t adxl372_get_packet_size(const struct adxl372_dev_config *cfg);
|
||||
int adxl372_set_op_mode(const struct device *dev, enum adxl372_op_mode op_mode);
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
#endif /* ZEPHYR_DRIVERS_SENSOR_ADXL372_ADXL372_H_ */
|
||||
|
|
303
drivers/sensor/adi/adxl372/adxl372_decoder.c
Normal file
303
drivers/sensor/adi/adxl372/adxl372_decoder.c
Normal file
|
@ -0,0 +1,303 @@
|
|||
/*
|
||||
* Copyright (c) 2024 Analog Devices Inc.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include "adxl372.h"
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
|
||||
/*
|
||||
* Sensor resolution is 100mg/LSB, 12-bit value needs to be right
|
||||
* shifted by 4 or divided by 16. Overall this results in a scale of 160
|
||||
*/
|
||||
#define SENSOR_SCALING_FACTOR (SENSOR_G / (16 * 1000 / 100))
|
||||
#define ADXL372_COMPLEMENT 0xf000
|
||||
|
||||
static const uint32_t accel_period_ns[] = {
|
||||
[ADXL372_ODR_400HZ] = UINT32_C(1000000000) / 400,
|
||||
[ADXL372_ODR_800HZ] = UINT32_C(1000000000) / 800,
|
||||
[ADXL372_ODR_1600HZ] = UINT32_C(1000000000) / 1600,
|
||||
[ADXL372_ODR_3200HZ] = UINT32_C(1000000000) / 3200,
|
||||
[ADXL372_ODR_6400HZ] = UINT32_C(1000000000) / 6400,
|
||||
};
|
||||
|
||||
static inline void adxl372_accel_convert_q31(q31_t *out, const uint8_t *buff)
|
||||
{
|
||||
int16_t data_in = ((int16_t)*buff << 4) | (((int16_t)*(buff + 1) & 0xF0) >> 4);
|
||||
|
||||
if (data_in & BIT(11)) {
|
||||
data_in |= ADXL372_COMPLEMENT;
|
||||
}
|
||||
|
||||
int64_t micro_ms2 = data_in * SENSOR_SCALING_FACTOR;
|
||||
|
||||
*out = CLAMP((micro_ms2 + (micro_ms2 % 1000000)), INT32_MIN, INT32_MAX);
|
||||
}
|
||||
|
||||
static int adxl372_decode_stream(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
|
||||
uint32_t *fit, uint16_t max_count, void *data_out)
|
||||
{
|
||||
const struct adxl372_fifo_data *enc_data = (const struct adxl372_fifo_data *)buffer;
|
||||
const uint8_t *buffer_end =
|
||||
buffer + sizeof(struct adxl372_fifo_data) + enc_data->fifo_byte_count;
|
||||
int count = 0;
|
||||
uint8_t sample_num = 0;
|
||||
|
||||
if ((uintptr_t)buffer_end <= *fit || chan_spec.chan_idx != 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
struct sensor_three_axis_data *data = (struct sensor_three_axis_data *)data_out;
|
||||
|
||||
memset(data, 0, sizeof(struct sensor_three_axis_data));
|
||||
data->header.base_timestamp_ns = enc_data->timestamp;
|
||||
data->header.reading_count = 1;
|
||||
|
||||
buffer += sizeof(struct adxl372_fifo_data);
|
||||
|
||||
uint8_t sample_set_size = enc_data->sample_set_size;
|
||||
uint64_t period_ns = accel_period_ns[enc_data->accel_odr];
|
||||
|
||||
/* Calculate which sample is decoded. */
|
||||
if ((uint8_t *)*fit >= buffer) {
|
||||
sample_num = ((uint8_t *)*fit - buffer) / sample_set_size;
|
||||
}
|
||||
|
||||
while (count < max_count && buffer < buffer_end) {
|
||||
const uint8_t *sample_end = buffer;
|
||||
|
||||
sample_end += sample_set_size;
|
||||
|
||||
if ((uintptr_t)buffer < *fit) {
|
||||
/* This frame was already decoded, move on to the next frame */
|
||||
buffer = sample_end;
|
||||
continue;
|
||||
}
|
||||
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
if (enc_data->has_x) {
|
||||
data->readings[count].timestamp_delta = sample_num * period_ns;
|
||||
adxl372_accel_convert_q31(&data->readings[count].x, buffer);
|
||||
}
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
if (enc_data->has_y) {
|
||||
uint8_t buff_offset = 0;
|
||||
|
||||
/* If packet has X channel, then Y channel has offset. */
|
||||
if (enc_data->has_x) {
|
||||
buff_offset = 2;
|
||||
}
|
||||
data->readings[count].timestamp_delta = sample_num * period_ns;
|
||||
adxl372_accel_convert_q31(&data->readings[count].y,
|
||||
(buffer + buff_offset));
|
||||
}
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
if (enc_data->has_z) {
|
||||
uint8_t buff_offset = 0;
|
||||
|
||||
/* If packet has X channel and/or Y channel,
|
||||
* then Z channel has offset.
|
||||
*/
|
||||
if (enc_data->has_x) {
|
||||
buff_offset = 2;
|
||||
}
|
||||
|
||||
if (enc_data->has_y) {
|
||||
buff_offset += 2;
|
||||
}
|
||||
data->readings[count].timestamp_delta = sample_num * period_ns;
|
||||
adxl372_accel_convert_q31(&data->readings[count].z,
|
||||
(buffer + buff_offset));
|
||||
}
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
data->readings[count].timestamp_delta = sample_num * period_ns;
|
||||
uint8_t buff_offset = 0;
|
||||
|
||||
if (enc_data->has_x) {
|
||||
adxl372_accel_convert_q31(&data->readings[count].x, buffer);
|
||||
buff_offset = 2;
|
||||
}
|
||||
|
||||
if (enc_data->has_y) {
|
||||
adxl372_accel_convert_q31(&data->readings[count].y,
|
||||
(buffer + buff_offset));
|
||||
|
||||
buff_offset += 2;
|
||||
}
|
||||
|
||||
if (enc_data->has_z) {
|
||||
adxl372_accel_convert_q31(&data->readings[count].z,
|
||||
(buffer + buff_offset));
|
||||
}
|
||||
break;
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
buffer = sample_end;
|
||||
*fit = (uintptr_t)sample_end;
|
||||
count++;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
static int adxl372_decoder_get_frame_count(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
|
||||
uint16_t *frame_count)
|
||||
{
|
||||
int32_t ret = -ENOTSUP;
|
||||
|
||||
if (chan_spec.chan_idx != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
const struct adxl372_fifo_data *data = (const struct adxl372_fifo_data *)buffer;
|
||||
|
||||
if (!data->is_fifo) {
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
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 = 1;
|
||||
ret = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
} else {
|
||||
if (data->fifo_byte_count == 0) {
|
||||
*frame_count = 0;
|
||||
ret = 0;
|
||||
} else {
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
if (data->has_x) {
|
||||
*frame_count =
|
||||
data->fifo_byte_count / data->sample_set_size;
|
||||
ret = 0;
|
||||
}
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
if (data->has_y) {
|
||||
*frame_count =
|
||||
data->fifo_byte_count / data->sample_set_size;
|
||||
ret = 0;
|
||||
}
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
if (data->has_z) {
|
||||
*frame_count =
|
||||
data->fifo_byte_count / data->sample_set_size;
|
||||
ret = 0;
|
||||
}
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
if (data->has_x || data->has_y || data->has_z) {
|
||||
*frame_count =
|
||||
data->fifo_byte_count / data->sample_set_size;
|
||||
ret = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int adxl372_decode_sample(const struct adxl372_xyz_accel_data *data,
|
||||
struct sensor_chan_spec chan_spec, uint32_t *fit,
|
||||
uint16_t max_count, void *data_out)
|
||||
{
|
||||
struct sensor_value *out = (struct sensor_value *)data_out;
|
||||
|
||||
if (*fit > 0) {
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
switch (chan_spec.chan_type) {
|
||||
case SENSOR_CHAN_ACCEL_X:
|
||||
adxl372_accel_convert(out, data->x);
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_Y:
|
||||
adxl372_accel_convert(out, data->y);
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_Z:
|
||||
adxl372_accel_convert(out, data->z);
|
||||
break;
|
||||
case SENSOR_CHAN_ACCEL_XYZ:
|
||||
adxl372_accel_convert(out++, data->x);
|
||||
adxl372_accel_convert(out++, data->y);
|
||||
adxl372_accel_convert(out, data->z);
|
||||
break;
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
|
||||
*fit = 1;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int adxl372_decoder_decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
|
||||
uint32_t *fit, uint16_t max_count, void *data_out)
|
||||
{
|
||||
const struct adxl372_xyz_accel_data *data = (const struct adxl372_xyz_accel_data *)buffer;
|
||||
|
||||
#ifdef CONFIG_ADXL372_STREAM
|
||||
if (data->is_fifo) {
|
||||
return adxl372_decode_stream(buffer, chan_spec, fit, max_count, data_out);
|
||||
}
|
||||
#endif /* CONFIG_ADXL372_STREAM */
|
||||
|
||||
return adxl372_decode_sample(data, chan_spec, fit, max_count, data_out);
|
||||
}
|
||||
|
||||
static bool adxl372_decoder_has_trigger(const uint8_t *buffer, enum sensor_trigger_type trigger)
|
||||
{
|
||||
const struct adxl372_fifo_data *data = (const struct adxl372_fifo_data *)buffer;
|
||||
|
||||
if (!data->is_fifo) {
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (trigger) {
|
||||
case SENSOR_TRIG_DATA_READY:
|
||||
return FIELD_GET(ADXL372_INT1_MAP_DATA_RDY_MSK, data->int_status);
|
||||
case SENSOR_TRIG_FIFO_WATERMARK:
|
||||
case SENSOR_TRIG_FIFO_FULL:
|
||||
return FIELD_GET(ADXL372_INT1_MAP_FIFO_FULL_MSK, data->int_status);
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
SENSOR_DECODER_API_DT_DEFINE() = {
|
||||
.get_frame_count = adxl372_decoder_get_frame_count,
|
||||
.decode = adxl372_decoder_decode,
|
||||
.has_trigger = adxl372_decoder_has_trigger,
|
||||
};
|
||||
|
||||
int adxl372_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder)
|
||||
{
|
||||
ARG_UNUSED(dev);
|
||||
*decoder = &SENSOR_DECODER_NAME();
|
||||
|
||||
return 0;
|
||||
}
|
62
drivers/sensor/adi/adxl372/adxl372_rtio.c
Normal file
62
drivers/sensor/adi/adxl372/adxl372_rtio.c
Normal file
|
@ -0,0 +1,62 @@
|
|||
/*
|
||||
* Copyright (c) 2024 Analog Devices Inc.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/rtio/work.h>
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
|
||||
#include "adxl372.h"
|
||||
|
||||
LOG_MODULE_DECLARE(ADXL372, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
static void adxl372_submit_fetch(struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg =
|
||||
(const struct sensor_read_config *)iodev_sqe->sqe.iodev->data;
|
||||
const struct device *dev = cfg->sensor;
|
||||
int rc;
|
||||
uint32_t min_buffer_len = sizeof(struct adxl372_xyz_accel_data);
|
||||
uint8_t *buffer;
|
||||
uint32_t buffer_len;
|
||||
|
||||
const struct adxl372_dev_config *cfg_372 = (const struct adxl372_dev_config *)dev->config;
|
||||
|
||||
rc = rtio_sqe_rx_buf(iodev_sqe, min_buffer_len, min_buffer_len, &buffer, &buffer_len);
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to get a read buffer of size %u bytes", min_buffer_len);
|
||||
rtio_iodev_sqe_err(iodev_sqe, rc);
|
||||
return;
|
||||
}
|
||||
|
||||
struct adxl372_xyz_accel_data *data = (struct adxl372_xyz_accel_data *)buffer;
|
||||
|
||||
rc = adxl372_get_accel_data(dev, cfg_372->max_peak_detect_mode, data);
|
||||
if (rc != 0) {
|
||||
LOG_ERR("Failed to fetch samples");
|
||||
rtio_iodev_sqe_err(iodev_sqe, rc);
|
||||
return;
|
||||
}
|
||||
|
||||
rtio_iodev_sqe_ok(iodev_sqe, 0);
|
||||
}
|
||||
|
||||
void adxl372_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg =
|
||||
(const struct sensor_read_config *)iodev_sqe->sqe.iodev->data;
|
||||
|
||||
if (!cfg->is_streaming) {
|
||||
struct rtio_work_req *req = rtio_work_req_alloc();
|
||||
|
||||
__ASSERT_NO_MSG(req);
|
||||
|
||||
rtio_work_req_submit(req, iodev_sqe, adxl372_submit_fetch);
|
||||
} else if (IS_ENABLED(CONFIG_ADXL372_STREAM)) {
|
||||
adxl372_submit_stream(dev, iodev_sqe);
|
||||
} else {
|
||||
rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
|
||||
}
|
||||
}
|
445
drivers/sensor/adi/adxl372/adxl372_stream.c
Normal file
445
drivers/sensor/adi/adxl372/adxl372_stream.c
Normal file
|
@ -0,0 +1,445 @@
|
|||
/*
|
||||
* Copyright (c) 2024 Analog Devices Inc.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
|
||||
#include "adxl372.h"
|
||||
|
||||
LOG_MODULE_DECLARE(ADXL372, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
static void adxl372_irq_en_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg)
|
||||
{
|
||||
const struct device *dev = (const struct device *)arg;
|
||||
const struct adxl372_dev_config *cfg = dev->config;
|
||||
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
}
|
||||
|
||||
static void adxl372_fifo_flush_rtio(const struct device *dev)
|
||||
{
|
||||
struct adxl372_data *data = dev->data;
|
||||
uint8_t pow_reg = data->pwr_reg;
|
||||
const struct adxl372_dev_config *cfg = dev->config;
|
||||
uint8_t fifo_config;
|
||||
|
||||
pow_reg &= ~ADXL372_POWER_CTL_MODE_MSK;
|
||||
pow_reg |= ADXL372_POWER_CTL_MODE(ADXL372_STANDBY);
|
||||
|
||||
struct rtio_sqe *sqe = rtio_sqe_acquire(data->rtio_ctx);
|
||||
const uint8_t reg_addr_w[2] = {ADXL372_REG_WRITE(ADXL372_POWER_CTL), pow_reg};
|
||||
|
||||
rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w, 2, NULL);
|
||||
|
||||
fifo_config = (ADXL372_FIFO_CTL_FORMAT_MODE(data->fifo_config.fifo_format) |
|
||||
ADXL372_FIFO_CTL_MODE_MODE(ADXL372_FIFO_BYPASSED) |
|
||||
ADXL372_FIFO_CTL_SAMPLES_MODE(data->fifo_config.fifo_samples));
|
||||
|
||||
sqe = rtio_sqe_acquire(data->rtio_ctx);
|
||||
const uint8_t reg_addr_w2[2] = {ADXL372_REG_WRITE(ADXL372_FIFO_CTL), fifo_config};
|
||||
|
||||
rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w2, 2, NULL);
|
||||
|
||||
fifo_config = (ADXL372_FIFO_CTL_FORMAT_MODE(data->fifo_config.fifo_format) |
|
||||
ADXL372_FIFO_CTL_MODE_MODE(data->fifo_config.fifo_mode) |
|
||||
ADXL372_FIFO_CTL_SAMPLES_MODE(data->fifo_config.fifo_samples));
|
||||
|
||||
sqe = rtio_sqe_acquire(data->rtio_ctx);
|
||||
const uint8_t reg_addr_w3[2] = {ADXL372_REG_WRITE(ADXL372_FIFO_CTL), fifo_config};
|
||||
|
||||
rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w3, 2, NULL);
|
||||
|
||||
pow_reg = data->pwr_reg;
|
||||
|
||||
pow_reg &= ~ADXL372_POWER_CTL_MODE_MSK;
|
||||
pow_reg |= ADXL372_POWER_CTL_MODE(cfg->op_mode);
|
||||
|
||||
sqe = rtio_sqe_acquire(data->rtio_ctx);
|
||||
struct rtio_sqe *complete_op = rtio_sqe_acquire(data->rtio_ctx);
|
||||
const uint8_t reg_addr_w4[2] = {ADXL372_REG_WRITE(ADXL372_POWER_CTL), pow_reg};
|
||||
|
||||
rtio_sqe_prep_tiny_write(sqe, data->iodev, RTIO_PRIO_NORM, reg_addr_w4, 2, NULL);
|
||||
sqe->flags |= RTIO_SQE_CHAINED;
|
||||
rtio_sqe_prep_callback(complete_op, adxl372_irq_en_cb, (void *)dev, NULL);
|
||||
rtio_submit(data->rtio_ctx, 0);
|
||||
}
|
||||
|
||||
void adxl372_submit_stream(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
|
||||
{
|
||||
const struct sensor_read_config *cfg =
|
||||
(const struct sensor_read_config *)iodev_sqe->sqe.iodev->data;
|
||||
struct adxl372_data *data = (struct adxl372_data *)dev->data;
|
||||
const struct adxl372_dev_config *cfg_372 = dev->config;
|
||||
uint8_t int_value = (uint8_t)~ADXL372_INT1_MAP_FIFO_FULL_MSK;
|
||||
uint8_t fifo_full_irq = 0;
|
||||
|
||||
int rc = gpio_pin_interrupt_configure_dt(&cfg_372->interrupt, GPIO_INT_DISABLE);
|
||||
|
||||
if (rc < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < cfg->count; i++) {
|
||||
if ((cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_WATERMARK) ||
|
||||
(cfg->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL)) {
|
||||
int_value = ADXL372_INT1_MAP_FIFO_FULL_MSK;
|
||||
fifo_full_irq = 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (fifo_full_irq != data->fifo_full_irq) {
|
||||
data->fifo_full_irq = fifo_full_irq;
|
||||
|
||||
rc = data->hw_tf->write_reg_mask(dev, ADXL372_INT1_MAP,
|
||||
ADXL372_INT1_MAP_FIFO_FULL_MSK, int_value);
|
||||
if (rc < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Flush the FIFO by disabling it. Save current mode for after the reset. */
|
||||
enum adxl372_fifo_mode current_fifo_mode = data->fifo_config.fifo_mode;
|
||||
|
||||
adxl372_configure_fifo(dev, ADXL372_FIFO_BYPASSED, data->fifo_config.fifo_format,
|
||||
data->fifo_config.fifo_samples);
|
||||
|
||||
if (current_fifo_mode == ADXL372_FIFO_BYPASSED) {
|
||||
current_fifo_mode = ADXL372_FIFO_STREAMED;
|
||||
}
|
||||
|
||||
adxl372_configure_fifo(dev, current_fifo_mode, data->fifo_config.fifo_format,
|
||||
data->fifo_config.fifo_samples);
|
||||
|
||||
adxl372_set_op_mode(dev, cfg_372->op_mode);
|
||||
}
|
||||
|
||||
rc = gpio_pin_interrupt_configure_dt(&cfg_372->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
if (rc < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
data->sqe = iodev_sqe;
|
||||
}
|
||||
|
||||
static void adxl372_fifo_read_cb(struct rtio *rtio_ctx, const struct rtio_sqe *sqe, void *arg)
|
||||
{
|
||||
const struct device *dev = (const struct device *)arg;
|
||||
const struct adxl372_dev_config *cfg = (const struct adxl372_dev_config *)dev->config;
|
||||
struct rtio_iodev_sqe *iodev_sqe = sqe->userdata;
|
||||
|
||||
rtio_iodev_sqe_ok(iodev_sqe, 0);
|
||||
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
}
|
||||
|
||||
size_t adxl372_get_packet_size(const struct adxl372_dev_config *cfg)
|
||||
{
|
||||
/* If one sample contains XYZ values. */
|
||||
size_t packet_size;
|
||||
|
||||
switch (cfg->fifo_config.fifo_format) {
|
||||
case ADXL372_X_FIFO:
|
||||
case ADXL372_Y_FIFO:
|
||||
case ADXL372_Z_FIFO:
|
||||
packet_size = 2;
|
||||
break;
|
||||
|
||||
case ADXL372_XY_FIFO:
|
||||
case ADXL372_XZ_FIFO:
|
||||
case ADXL372_YZ_FIFO:
|
||||
packet_size = 4;
|
||||
break;
|
||||
|
||||
default:
|
||||
packet_size = 6;
|
||||
break;
|
||||
}
|
||||
|
||||
return packet_size;
|
||||
}
|
||||
|
||||
static void adxl372_process_fifo_samples_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg)
|
||||
{
|
||||
const struct device *dev = (const struct device *)arg;
|
||||
struct adxl372_data *data = (struct adxl372_data *)dev->data;
|
||||
const struct adxl372_dev_config *cfg = (const struct adxl372_dev_config *)dev->config;
|
||||
struct rtio_iodev_sqe *current_sqe = data->sqe;
|
||||
uint16_t fifo_samples = (((data->fifo_ent[0] & 0x3) << 8) | data->fifo_ent[1]);
|
||||
size_t sample_set_size = adxl372_get_packet_size(cfg);
|
||||
|
||||
/* At least one sample set must remain in FIFO to encure that data
|
||||
* is not overwritten and stored out of order.
|
||||
*/
|
||||
if (fifo_samples > sample_set_size / 2) {
|
||||
fifo_samples -= sample_set_size / 2;
|
||||
} else {
|
||||
LOG_ERR("fifo sample count error %d\n", fifo_samples);
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
uint16_t fifo_bytes = fifo_samples * 2 /*sample size*/;
|
||||
|
||||
data->sqe = NULL;
|
||||
|
||||
/* Not inherently an underrun/overrun as we may have a buffer to fill next time */
|
||||
if (current_sqe == NULL) {
|
||||
LOG_ERR("No pending SQE");
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
const size_t min_read_size = sizeof(struct adxl372_fifo_data) + sample_set_size;
|
||||
const size_t ideal_read_size = sizeof(struct adxl372_fifo_data) + fifo_bytes;
|
||||
|
||||
uint8_t *buf;
|
||||
uint32_t buf_len;
|
||||
|
||||
if (rtio_sqe_rx_buf(current_sqe, min_read_size, ideal_read_size, &buf, &buf_len) != 0) {
|
||||
LOG_ERR("Failed to get buffer");
|
||||
rtio_iodev_sqe_err(current_sqe, -ENOMEM);
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
LOG_DBG("Requesting buffer [%u, %u] got %u", (unsigned int)min_read_size,
|
||||
(unsigned int)ideal_read_size, buf_len);
|
||||
|
||||
/* Read FIFO and call back to rtio with rtio_sqe completion */
|
||||
struct adxl372_fifo_data *hdr = (struct adxl372_fifo_data *)buf;
|
||||
|
||||
hdr->is_fifo = 1;
|
||||
hdr->timestamp = data->timestamp;
|
||||
hdr->int_status = data->status1;
|
||||
hdr->accel_odr = cfg->odr;
|
||||
hdr->sample_set_size = sample_set_size;
|
||||
|
||||
if ((cfg->fifo_config.fifo_format == ADXL372_X_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_XY_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_XZ_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_XYZ_FIFO)) {
|
||||
hdr->has_x = 1;
|
||||
}
|
||||
|
||||
if ((cfg->fifo_config.fifo_format == ADXL372_Y_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_XY_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_YZ_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_XYZ_FIFO)) {
|
||||
hdr->has_y = 1;
|
||||
}
|
||||
|
||||
if ((cfg->fifo_config.fifo_format == ADXL372_Z_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_XZ_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_YZ_FIFO) ||
|
||||
(cfg->fifo_config.fifo_format == ADXL372_XYZ_FIFO)) {
|
||||
hdr->has_z = 1;
|
||||
}
|
||||
|
||||
uint32_t buf_avail = buf_len;
|
||||
|
||||
buf_avail -= sizeof(*hdr);
|
||||
|
||||
uint32_t read_len = MIN(fifo_bytes, buf_avail);
|
||||
uint32_t pkts = read_len / sample_set_size;
|
||||
|
||||
read_len = pkts * sample_set_size;
|
||||
|
||||
((struct adxl372_fifo_data *)buf)->fifo_byte_count = read_len;
|
||||
|
||||
__ASSERT_NO_MSG(read_len % pkt_size == 0);
|
||||
|
||||
uint8_t *read_buf = buf + sizeof(*hdr);
|
||||
|
||||
/* Flush completions */
|
||||
struct rtio_cqe *cqe;
|
||||
int res = 0;
|
||||
|
||||
do {
|
||||
cqe = rtio_cqe_consume(data->rtio_ctx);
|
||||
if (cqe != NULL) {
|
||||
if ((cqe->result < 0 && res == 0)) {
|
||||
LOG_ERR("Bus error: %d", cqe->result);
|
||||
res = cqe->result;
|
||||
}
|
||||
rtio_cqe_release(data->rtio_ctx, cqe);
|
||||
}
|
||||
} while (cqe != NULL);
|
||||
|
||||
/* Bail/cancel attempt to read sensor on any error */
|
||||
if (res != 0) {
|
||||
rtio_iodev_sqe_err(current_sqe, res);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Setup new rtio chain to read the fifo data and report then check the
|
||||
* result
|
||||
*/
|
||||
struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(data->rtio_ctx);
|
||||
struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(data->rtio_ctx);
|
||||
struct rtio_sqe *complete_op = rtio_sqe_acquire(data->rtio_ctx);
|
||||
const uint8_t reg_addr = ADXL372_REG_READ(ADXL372_FIFO_DATA);
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_fifo_addr, data->iodev, RTIO_PRIO_NORM, ®_addr, 1, NULL);
|
||||
write_fifo_addr->flags = RTIO_SQE_TRANSACTION;
|
||||
rtio_sqe_prep_read(read_fifo_data, data->iodev, RTIO_PRIO_NORM, read_buf, read_len,
|
||||
current_sqe);
|
||||
read_fifo_data->flags = RTIO_SQE_CHAINED;
|
||||
rtio_sqe_prep_callback(complete_op, adxl372_fifo_read_cb, (void *)dev, current_sqe);
|
||||
|
||||
rtio_submit(data->rtio_ctx, 0);
|
||||
}
|
||||
|
||||
static void adxl372_process_status1_cb(struct rtio *r, const struct rtio_sqe *sqr, void *arg)
|
||||
{
|
||||
const struct device *dev = (const struct device *)arg;
|
||||
struct adxl372_data *data = (struct adxl372_data *)dev->data;
|
||||
const struct adxl372_dev_config *cfg = (const struct adxl372_dev_config *)dev->config;
|
||||
struct rtio_iodev_sqe *current_sqe = data->sqe;
|
||||
struct sensor_read_config *read_config;
|
||||
uint8_t status1 = data->status1;
|
||||
|
||||
if (data->sqe == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
read_config = (struct sensor_read_config *)data->sqe->sqe.iodev->data;
|
||||
|
||||
if (read_config == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (read_config->is_streaming == false) {
|
||||
return;
|
||||
}
|
||||
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_DISABLE);
|
||||
|
||||
struct sensor_stream_trigger *fifo_wmark_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_wmark_cfg = &read_config->triggers[i];
|
||||
continue;
|
||||
}
|
||||
|
||||
if (read_config->triggers[i].trigger == SENSOR_TRIG_FIFO_FULL) {
|
||||
fifo_full_cfg = &read_config->triggers[i];
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
bool fifo_full_irq = false;
|
||||
|
||||
if ((fifo_wmark_cfg != NULL) && (fifo_full_cfg != NULL) &&
|
||||
FIELD_GET(ADXL372_INT1_MAP_FIFO_FULL_MSK, status1)) {
|
||||
fifo_full_irq = true;
|
||||
}
|
||||
|
||||
if (!fifo_full_irq) {
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
struct rtio_cqe *cqe;
|
||||
int res = 0;
|
||||
|
||||
do {
|
||||
cqe = rtio_cqe_consume(data->rtio_ctx);
|
||||
if (cqe != NULL) {
|
||||
if ((cqe->result < 0) && (res == 0)) {
|
||||
LOG_ERR("Bus error: %d", cqe->result);
|
||||
res = cqe->result;
|
||||
}
|
||||
rtio_cqe_release(data->rtio_ctx, cqe);
|
||||
}
|
||||
} while (cqe != NULL);
|
||||
|
||||
/* Bail/cancel attempt to read sensor on any error */
|
||||
if (res != 0) {
|
||||
rtio_iodev_sqe_err(current_sqe, res);
|
||||
return;
|
||||
}
|
||||
|
||||
enum sensor_stream_data_opt data_opt;
|
||||
|
||||
if ((fifo_wmark_cfg != NULL) && (fifo_full_cfg == NULL)) {
|
||||
data_opt = fifo_wmark_cfg->opt;
|
||||
} else if ((fifo_wmark_cfg == NULL) && (fifo_full_cfg != NULL)) {
|
||||
data_opt = fifo_full_cfg->opt;
|
||||
} else {
|
||||
data_opt = MIN(fifo_wmark_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 */
|
||||
data->sqe = NULL;
|
||||
if (rtio_sqe_rx_buf(current_sqe, sizeof(struct adxl372_fifo_data),
|
||||
sizeof(struct adxl372_fifo_data), &buf, &buf_len) != 0) {
|
||||
rtio_iodev_sqe_err(current_sqe, -ENOMEM);
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
struct adxl372_fifo_data *rx_data = (struct adxl372_fifo_data *)buf;
|
||||
|
||||
memset(buf, 0, buf_len);
|
||||
rx_data->is_fifo = 1;
|
||||
rx_data->timestamp = data->timestamp;
|
||||
rx_data->int_status = status1;
|
||||
rx_data->fifo_byte_count = 0;
|
||||
rtio_iodev_sqe_ok(current_sqe, 0);
|
||||
|
||||
if (data_opt == SENSOR_STREAM_DATA_DROP) {
|
||||
/* Flush the FIFO by disabling it. Save current mode for after the reset. */
|
||||
adxl372_fifo_flush_rtio(dev);
|
||||
return;
|
||||
}
|
||||
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_EDGE_TO_ACTIVE);
|
||||
return;
|
||||
}
|
||||
|
||||
struct rtio_sqe *write_fifo_addr = rtio_sqe_acquire(data->rtio_ctx);
|
||||
struct rtio_sqe *read_fifo_data = rtio_sqe_acquire(data->rtio_ctx);
|
||||
struct rtio_sqe *complete_op = rtio_sqe_acquire(data->rtio_ctx);
|
||||
const uint8_t reg_addr = ADXL372_REG_READ(ADXL372_FIFO_ENTRIES_2);
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_fifo_addr, data->iodev, RTIO_PRIO_NORM, ®_addr, 1, NULL);
|
||||
write_fifo_addr->flags = RTIO_SQE_TRANSACTION;
|
||||
rtio_sqe_prep_read(read_fifo_data, data->iodev, RTIO_PRIO_NORM, data->fifo_ent, 2,
|
||||
current_sqe);
|
||||
read_fifo_data->flags = RTIO_SQE_CHAINED;
|
||||
rtio_sqe_prep_callback(complete_op, adxl372_process_fifo_samples_cb, (void *)dev,
|
||||
current_sqe);
|
||||
|
||||
rtio_submit(data->rtio_ctx, 0);
|
||||
}
|
||||
|
||||
void adxl372_stream_irq_handler(const struct device *dev)
|
||||
{
|
||||
struct adxl372_data *data = (struct adxl372_data *)dev->data;
|
||||
|
||||
if (data->sqe == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
data->timestamp = k_ticks_to_ns_floor64(k_uptime_ticks());
|
||||
|
||||
struct rtio_sqe *write_status_addr = rtio_sqe_acquire(data->rtio_ctx);
|
||||
struct rtio_sqe *read_status_reg = rtio_sqe_acquire(data->rtio_ctx);
|
||||
struct rtio_sqe *check_status_reg = rtio_sqe_acquire(data->rtio_ctx);
|
||||
uint8_t reg = ADXL372_REG_READ(ADXL372_STATUS_1);
|
||||
|
||||
rtio_sqe_prep_tiny_write(write_status_addr, data->iodev, RTIO_PRIO_NORM, ®, 1, NULL);
|
||||
write_status_addr->flags = RTIO_SQE_TRANSACTION;
|
||||
rtio_sqe_prep_read(read_status_reg, data->iodev, RTIO_PRIO_NORM, &data->status1, 1, NULL);
|
||||
read_status_reg->flags = RTIO_SQE_CHAINED;
|
||||
rtio_sqe_prep_callback(check_status_reg, adxl372_process_status1_cb, (void *)dev, NULL);
|
||||
rtio_submit(data->rtio_ctx, 0);
|
||||
}
|
|
@ -16,6 +16,7 @@
|
|||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_DECLARE(ADXL372, CONFIG_SENSOR_LOG_LEVEL);
|
||||
|
||||
#if defined(CONFIG_ADXL372_TRIGGER_OWN_THREAD) || defined(CONFIG_ADXL372_TRIGGER_GLOBAL_THREAD)
|
||||
static void adxl372_thread_cb(const struct device *dev)
|
||||
{
|
||||
const struct adxl372_dev_config *cfg = dev->config;
|
||||
|
@ -49,8 +50,10 @@ static void adxl372_thread_cb(const struct device *dev)
|
|||
|
||||
ret = gpio_pin_interrupt_configure_dt(&cfg->interrupt,
|
||||
GPIO_INT_EDGE_TO_ACTIVE);
|
||||
|
||||
__ASSERT(ret == 0, "Interrupt configuration failed");
|
||||
}
|
||||
#endif /* CONFIG_ADXL372_TRIGGER_OWN_THREAD || CONFIG_ADXL372_TRIGGER_GLOBAL_THREAD */
|
||||
|
||||
static void adxl372_gpio_callback(const struct device *dev,
|
||||
struct gpio_callback *cb, uint32_t pins)
|
||||
|
@ -61,6 +64,10 @@ static void adxl372_gpio_callback(const struct device *dev,
|
|||
|
||||
gpio_pin_interrupt_configure_dt(&cfg->interrupt, GPIO_INT_DISABLE);
|
||||
|
||||
if (IS_ENABLED(CONFIG_ADXL372_STREAM)) {
|
||||
adxl372_stream_irq_handler(drv_data->dev);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_ADXL372_TRIGGER_OWN_THREAD)
|
||||
k_sem_give(&drv_data->gpio_sem);
|
||||
#elif defined(CONFIG_ADXL372_TRIGGER_GLOBAL_THREAD)
|
||||
|
@ -160,7 +167,7 @@ int adxl372_init_interrupt(const struct device *dev)
|
|||
return -EINVAL;
|
||||
}
|
||||
|
||||
ret = gpio_pin_configure_dt(&cfg->interrupt, GPIO_INPUT);
|
||||
ret = gpio_pin_configure_dt(&cfg->interrupt, GPIO_INPUT | GPIO_PUSH_PULL);
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue