Skip to content

Commit

Permalink
drivers: sensor: ak09918c use RTIO
Browse files Browse the repository at this point in the history 
first try wip

Signed-off-by: Florian Weber <[email protected]>
  • Loading branch information
@FlorianWeber1018
FlorianWeber1018 committed Dec 14, 2024
1 parent acf7174 commit 6b5345a
Show file tree
Hide file tree
Showing 5 changed files with 127 additions and 47 deletions.
1 change: 1 addition & 0 deletions drivers/sensor/asahi_kasei/akm09918c/Kconfig
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ config AKM09918C
default y
depends on DT_HAS_ASAHI_KASEI_AKM09918C_ENABLED
select I2C
select I2C_RTIO if SENSOR_ASYNC_API
select RTIO_WORKQ if SENSOR_ASYNC_API
help
Enable driver for AK8975 magnetometer.
Expand Down
28 changes: 18 additions & 10 deletions drivers/sensor/asahi_kasei/akm09918c/akm09918c.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ LOG_MODULE_REGISTER(AKM09918C, CONFIG_SENSOR_LOG_LEVEL);
* @param chan Channel ID for starting the measurement
* @return int 0 if successful or error code
*/
int akm09918c_start_measurement(const struct device *dev, enum sensor_channel chan)
int akm09918c_start_measurement_blocking(const struct device *dev, enum sensor_channel chan)
{
struct akm09918c_data *data = dev->data;
const struct akm09918c_config *cfg = dev->config;
Expand All @@ -41,12 +41,13 @@ int akm09918c_start_measurement(const struct device *dev, enum sensor_channel ch

if (data->mode == AKM09918C_CNTL2_PWR_DOWN) {
if (i2c_reg_write_byte_dt(&cfg->i2c, AKM09918C_REG_CNTL2,
AKM09918C_CNTL2_SINGLE_MEASURE) != 0) {
LOG_ERR("Failed to start measurement.");
return -EIO;
AKM09918C_CNTL2_SINGLE_MEASURE) == 0) {
return 0;
}
}
return 0;

LOG_ERR("Failed to start measurement.");
return -EIO;
}

/**
Expand All @@ -59,7 +60,8 @@ int akm09918c_start_measurement(const struct device *dev, enum sensor_channel ch
* @param z Location to write Z channel sample.
* @return int 0 if successful or error code
*/
int akm09918c_fetch_measurement(const struct device *dev, int16_t *x, int16_t *y, int16_t *z)
int akm09918c_fetch_measurement_blocking(const struct device *dev, int16_t *x, int16_t *y,
int16_t *z)
{
const struct akm09918c_config *cfg = dev->config;
uint8_t buf[9] = {0};
Expand All @@ -86,7 +88,7 @@ static int akm09918c_sample_fetch(const struct device *dev, enum sensor_channel
{
struct akm09918c_data *data = dev->data;

int ret = akm09918c_start_measurement(dev, chan);
int ret = akm09918c_start_measurement_blocking(dev, chan);

if (ret) {
return ret;
Expand All @@ -95,7 +97,8 @@ static int akm09918c_sample_fetch(const struct device *dev, enum sensor_channel
LOG_DBG("Waiting for sample...");
k_usleep(AKM09918C_MEASURE_TIME_US);

return akm09918c_fetch_measurement(dev, &data->x_sample, &data->y_sample, &data->z_sample);
return akm09918c_fetch_measurement_blocking(dev, &data->x_sample, &data->y_sample,
&data->z_sample);
}

static void akm09918c_convert(struct sensor_value *val, int16_t sample)
Expand Down Expand Up @@ -249,8 +252,13 @@ static DEVICE_API(sensor, akm09918c_driver_api) = {
};

#define AKM09918C_DEFINE(inst) \
static struct akm09918c_data akm09918c_data_##inst; \
\
IF_ENABLED(CONFIG_I2C_RTIO, \
(I2C_DT_IODEV_DEFINE(akm09918c_iodev_##inst, DT_DRV_INST(inst));)) \
IF_ENABLED(CONFIG_I2C_RTIO, (RTIO_DEFINE( \
akm09918c_rtio_ctx_##inst, CONFIG_I2C_RTIO_SQ_SIZE, CONFIG_I2C_RTIO_CQ_SIZE);)) \
static struct akm09918c_data akm09918c_data_##inst = { \
IF_ENABLED(CONFIG_I2C_RTIO, (.rtio_ctx = \
&akm09918c_rtio_ctx_##inst, .iodev = &akm09918c_iodev_##inst)) }; \
static const struct akm09918c_config akm09918c_config_##inst = { \
.i2c = I2C_DT_SPEC_INST_GET(inst), \
}; \
Expand Down
17 changes: 13 additions & 4 deletions drivers/sensor/asahi_kasei/akm09918c/akm09918c.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,9 @@ struct akm09918c_data {
uint64_t timestamp;
struct k_work_delayable async_fetch_work;
} work_ctx;
/* for communication to the bus controller */
struct rtio *rtio_ctx;
struct rtio_iodev *iodev;
#endif
};

Expand Down Expand Up @@ -82,9 +85,10 @@ static inline void akm09918c_reg_to_hz(uint8_t reg, struct sensor_value *val)
break;
}
}
int akm09918c_start_measurement(const struct device *dev, enum sensor_channel chan);
int akm09918c_start_measurement_blocking(const struct device *dev, enum sensor_channel chan);

int akm09918c_fetch_measurement(const struct device *dev, int16_t *x, int16_t *y, int16_t *z);
int akm09918c_fetch_measurement_blocking(const struct device *dev, int16_t *x, int16_t *y,
int16_t *z);
/*
* RTIO types
*/
Expand All @@ -95,13 +99,18 @@ struct akm09918c_decoder_header {

struct akm09918c_encoded_data {
struct akm09918c_decoder_header header;
int16_t readings[3];
};
struct {
int16_t data[3];
uint8_t tmp[9];
} reading;
} __packed __aligned(1);

void akm09918_async_fetch(struct k_work *work);

int akm09918c_get_decoder(const struct device *dev, const struct sensor_decoder_api **decoder);

void akm09918c_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe);
void akm09918_after_start_cb(struct rtio *rtio_ctx, const struct rtio_sqe *sqe, void *arg0);
void akm09918_complete_cb(struct rtio *rtio_ctx, const struct rtio_sqe *sqe, void *arg0);

#endif /* ZEPHYR_DRIVERS_SENSOR_AKM09918C_AKM09918C_H_ */
122 changes: 92 additions & 30 deletions drivers/sensor/asahi_kasei/akm09918c/akm09918c_async.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,20 +7,38 @@
*/

#include <zephyr/logging/log.h>
#include <zephyr/rtio/work.h>
#include <zephyr/rtio/rtio.h>
#include <zephyr/sys/byteorder.h>

#include "akm09918c.h"

LOG_MODULE_DECLARE(AKM09918C, CONFIG_SENSOR_LOG_LEVEL);

void akm09918c_submit_sync(struct rtio_iodev_sqe *iodev_sqe)
static int akm09918c_flush_cqes(struct rtio *rtio_ctx)
{
/* Flush completions */
struct rtio_cqe *cqe;
int res = 0;

do {
cqe = rtio_cqe_consume(rtio_ctx);
if (cqe != NULL) {
if ((cqe->result < 0 && res == 0)) {
LOG_ERR("Bus error: %d", cqe->result);
res = cqe->result;
}
rtio_cqe_release(rtio_ctx, cqe);
}
} while (cqe != NULL);
return res;
}

void akm09918c_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
{
const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
const struct device *dev = cfg->sensor;
struct akm09918c_data *data = dev->data;
const struct sensor_chan_spec *const channels = cfg->channels;
const size_t num_channels = cfg->count;
int rc;

/* Check if the requested channels are supported */
for (size_t i = 0; i < num_channels; i++) {
Expand All @@ -37,49 +55,55 @@ void akm09918c_submit_sync(struct rtio_iodev_sqe *iodev_sqe)
return;
}
}
struct rtio_sqe *writeByte_sqe = i2c_rtio_copy_reg_write_byte(
data->rtio_ctx, data->iodev, AKM09918C_REG_CNTL2, AKM09918C_CNTL2_SINGLE_MEASURE);
struct rtio_sqe *cb_sqe = rtio_sqe_acquire(data->rtio_ctx);

/* start the measurement in the sensor */
rc = akm09918c_start_measurement(dev, SENSOR_CHAN_MAGN_XYZ);
if (rc != 0) {
LOG_ERR("Failed to fetch samples.");
rtio_iodev_sqe_err(iodev_sqe, rc);
return;
writeByte_sqe->flags |= RTIO_SQE_CHAINED;
rtio_sqe_prep_callback_no_cqe(cb_sqe, akm09918_after_start_cb, (void *)iodev_sqe, NULL);

if (writeByte_sqe != NULL && cb_sqe != NULL) {
rtio_submit(data->rtio_ctx, 0);
} else {
rtio_sqe_drop_all(data->rtio_ctx);
rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
}
}

void akm09918_after_start_cb(struct rtio *rtio_ctx, const struct rtio_sqe *sqe, void *arg0)
{
const struct rtio_iodev_sqe *parent_iodev_sqe = (struct rtio_iodev_sqe *)arg0;
const struct sensor_read_config *cfg = parent_iodev_sqe->sqe.iodev->data;
const struct device *dev = cfg->sensor;
struct akm09918c_data *data = dev->data;
int rc;
/* save information for the work item */
data->work_ctx.timestamp = k_ticks_to_ns_floor64(k_uptime_ticks());
data->work_ctx.iodev_sqe = iodev_sqe;
data->work_ctx.iodev_sqe = (struct rtio_iodev_sqe *)arg0;

rc = akm09918c_flush_cqes(data->rtio_ctx);
if (rc != 0) {
rtio_iodev_sqe_err((struct rtio_iodev_sqe *)arg0, rc);
return;
}

rc = k_work_schedule(&data->work_ctx.async_fetch_work, K_USEC(AKM09918C_MEASURE_TIME_US));
if (rc == 0) {
LOG_ERR("The last fetch has not finished yet. "
"Try again later when the last sensor read operation has finished.");
rtio_iodev_sqe_err(iodev_sqe, -EBUSY);
rtio_iodev_sqe_err((struct rtio_iodev_sqe *)arg0, -EBUSY);
}
return;
}

void akm09918c_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, akm09918c_submit_sync);
}

void akm09918_async_fetch(struct k_work *work)
{
struct k_work_delayable *dwork = k_work_delayable_from_work(work);
struct akm09918c_async_fetch_ctx *ctx =
CONTAINER_OF(dwork, struct akm09918c_async_fetch_ctx, async_fetch_work);
const struct sensor_read_config *cfg = ctx->iodev_sqe->sqe.iodev->data;
const struct device *dev = cfg->sensor;
struct akm09918c_data *data = dev->data;
uint32_t req_buf_len = sizeof(struct akm09918c_encoded_data);
uint32_t buf_len;
uint8_t *buf;
Expand All @@ -94,11 +118,49 @@ void akm09918_async_fetch(struct k_work *work)
return;
}
edata = (struct akm09918c_encoded_data *)buf;
rc = akm09918c_fetch_measurement(dev, &edata->readings[0], &edata->readings[1],
&edata->readings[2]);

struct rtio_sqe *burstRead_sqe =
i2c_rtio_copy_reg_burst_read(data->rtio_ctx, data->iodev, AKM09918C_REG_ST1,
&(edata->reading.tmp), sizeof(edata->reading.tmp));

edata->header.timestamp = ctx->timestamp;

struct rtio_sqe *cb_sqe = rtio_sqe_acquire(data->rtio_ctx);

rtio_sqe_prep_callback_no_cqe(cb_sqe, akm09918_complete_cb, (void *)ctx->iodev_sqe, NULL);

if (burstRead_sqe != NULL && cb_sqe != NULL) {
burstRead_sqe->flags |= RTIO_SQE_CHAINED;
rtio_submit(data->rtio_ctx, 0);
} else {
rtio_sqe_drop_all(data->rtio_ctx);
rtio_iodev_sqe_err(ctx->iodev_sqe, -ENOMEM);
}
}

void akm09918_complete_cb(struct rtio *rtio_ctx, const struct rtio_sqe *sqe, void *arg0)
{
struct rtio_iodev_sqe *parent_iodev_sqe = (struct rtio_iodev_sqe *)arg0;
struct rtio_sqe *parent_sqe = &parent_iodev_sqe->sqe;
struct akm09918c_encoded_data *edata =
(struct akm09918c_encoded_data *)(parent_sqe->rx.buf);
int rc;

rc = akm09918c_flush_cqes(rtio_ctx);
if (rc != 0) {
rtio_iodev_sqe_err(ctx->iodev_sqe, rc);
rtio_iodev_sqe_err(parent_iodev_sqe, rc);
return;
}

if (FIELD_GET(AKM09918C_ST1_DRDY, edata->reading.tmp[0]) == 0) {
LOG_ERR("Data not ready, st1=0x%02x", edata->reading.tmp[0]);
rtio_iodev_sqe_err(parent_iodev_sqe, -EBUSY);
return;
}
rtio_iodev_sqe_ok(ctx->iodev_sqe, 0);

edata->reading.data[0] = edata->reading.tmp[1] | (edata->reading.tmp[2] << 8);
edata->reading.data[1] = edata->reading.tmp[3] | (edata->reading.tmp[4] << 8);
edata->reading.data[2] = edata->reading.tmp[5] | (edata->reading.tmp[6] << 8);

rtio_iodev_sqe_ok(parent_iodev_sqe, 0);
}
6 changes: 3 additions & 3 deletions drivers/sensor/asahi_kasei/akm09918c/akm09918c_decoder.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,9 @@ static int akm09918c_decoder_decode(const uint8_t *buffer, struct sensor_chan_sp
out->header.reading_count = 1;
out->shift = AKM09918C_SHIFT;

akm09918c_convert_raw_to_q31(edata->readings[0], &out->readings[0].x);
akm09918c_convert_raw_to_q31(edata->readings[1], &out->readings[0].y);
akm09918c_convert_raw_to_q31(edata->readings[2], &out->readings[0].z);
akm09918c_convert_raw_to_q31(edata->reading.data[0], &out->readings[0].x);
akm09918c_convert_raw_to_q31(edata->reading.data[1], &out->readings[0].y);
akm09918c_convert_raw_to_q31(edata->reading.data[2], &out->readings[0].z);
*fit = 1;

return 1;
Expand Down

0 comments on commit 6b5345a

Please sign in to comment.