1 /* ST Microelectronics LSM6DSV16X 6-axis IMU sensor driver
2 *
3 * Copyright (c) 2023 Google LLC
4 * Copyright (c) 2024 STMicroelectronics
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/drivers/sensor.h>
10 #include "lsm6dsv16x.h"
11 #include "lsm6dsv16x_rtio.h"
12 #include "lsm6dsv16x_decoder.h"
13 #include <zephyr/rtio/work.h>
14
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_REGISTER(LSM6DSV16X_RTIO, CONFIG_SENSOR_LOG_LEVEL);
17
lsm6dsv16x_submit_sample(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)18 static void lsm6dsv16x_submit_sample(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
19 {
20 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
21 const struct sensor_chan_spec *const channels = cfg->channels;
22 const size_t num_channels = cfg->count;
23 uint32_t min_buf_len = sizeof(struct lsm6dsv16x_rtio_data);
24 int rc = 0;
25 uint8_t *buf;
26 uint32_t buf_len;
27 struct lsm6dsv16x_rtio_data *edata;
28 struct lsm6dsv16x_data *data = dev->data;
29
30 const struct lsm6dsv16x_config *config = dev->config;
31 stmdev_ctx_t *ctx = (stmdev_ctx_t *)&config->ctx;
32
33 /* Get the buffer for the frame, it may be allocated dynamically by the rtio context */
34 rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len);
35 if (rc != 0) {
36 LOG_ERR("Failed to get a read buffer of size %u bytes", min_buf_len);
37 goto err;
38 }
39
40 edata = (struct lsm6dsv16x_rtio_data *)buf;
41
42 edata->has_accel = 0;
43 edata->has_gyro = 0;
44 edata->has_temp = 0;
45
46 for (int i = 0; i < num_channels; i++) {
47 switch (channels[i].chan_type) {
48 case SENSOR_CHAN_ACCEL_X:
49 case SENSOR_CHAN_ACCEL_Y:
50 case SENSOR_CHAN_ACCEL_Z:
51 case SENSOR_CHAN_ACCEL_XYZ:
52 edata->has_accel = 1;
53
54 rc = lsm6dsv16x_acceleration_raw_get(ctx, edata->acc);
55 if (rc < 0) {
56 LOG_DBG("Failed to read accel sample");
57 goto err;
58 }
59 break;
60 case SENSOR_CHAN_GYRO_X:
61 case SENSOR_CHAN_GYRO_Y:
62 case SENSOR_CHAN_GYRO_Z:
63 case SENSOR_CHAN_GYRO_XYZ:
64 edata->has_gyro = 1;
65
66 rc = lsm6dsv16x_angular_rate_raw_get(ctx, edata->gyro);
67 if (rc < 0) {
68 LOG_DBG("Failed to read gyro sample");
69 goto err;
70 }
71 break;
72 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
73 case SENSOR_CHAN_DIE_TEMP:
74 edata->has_temp = 1;
75
76 rc = lsm6dsv16x_temperature_raw_get(ctx, &edata->temp);
77 if (rc < 0) {
78 LOG_DBG("Failed to read temp sample");
79 goto err;
80 }
81 break;
82 #endif
83 case SENSOR_CHAN_ALL:
84 edata->has_accel = 1;
85
86 rc = lsm6dsv16x_acceleration_raw_get(ctx, edata->acc);
87 if (rc < 0) {
88 LOG_DBG("Failed to read accel sample");
89 goto err;
90 }
91
92 edata->has_gyro = 1;
93
94 rc = lsm6dsv16x_angular_rate_raw_get(ctx, edata->gyro);
95 if (rc < 0) {
96 LOG_DBG("Failed to read gyro sample");
97 goto err;
98 }
99
100 #if defined(CONFIG_LSM6DSV16X_ENABLE_TEMP)
101 edata->has_temp = 1;
102
103 rc = lsm6dsv16x_temperature_raw_get(ctx, &edata->temp);
104 if (rc < 0) {
105 LOG_DBG("Failed to read temp sample");
106 goto err;
107 }
108 #endif
109 break;
110 default:
111 continue;
112 }
113 }
114
115 edata->header.is_fifo = false;
116 edata->header.accel_fs = data->accel_fs;
117 edata->header.gyro_fs = data->gyro_fs;
118 edata->header.timestamp = k_ticks_to_ns_floor64(k_uptime_ticks());
119
120 rtio_iodev_sqe_ok(iodev_sqe, 0);
121
122 err:
123 /* Check that the fetch succeeded */
124 if (rc != 0) {
125 LOG_ERR("Failed to fetch samples");
126 rtio_iodev_sqe_err(iodev_sqe, rc);
127 return;
128 }
129 }
130
lsm6dsv16x_submit_sync(struct rtio_iodev_sqe * iodev_sqe)131 void lsm6dsv16x_submit_sync(struct rtio_iodev_sqe *iodev_sqe)
132 {
133 const struct sensor_read_config *cfg = iodev_sqe->sqe.iodev->data;
134 const struct device *dev = cfg->sensor;
135
136 if (!cfg->is_streaming) {
137 lsm6dsv16x_submit_sample(dev, iodev_sqe);
138 } else if (IS_ENABLED(CONFIG_LSM6DSV16X_STREAM)) {
139 lsm6dsv16x_submit_stream(dev, iodev_sqe);
140 } else {
141 rtio_iodev_sqe_err(iodev_sqe, -ENOTSUP);
142 }
143 }
144
lsm6dsv16x_submit(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)145 void lsm6dsv16x_submit(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe)
146 {
147 struct rtio_work_req *req = rtio_work_req_alloc();
148
149 if (req == NULL) {
150 LOG_ERR("RTIO work item allocation failed. Consider to increase "
151 "CONFIG_RTIO_WORKQ_POOL_ITEMS.");
152 rtio_iodev_sqe_err(iodev_sqe, -ENOMEM);
153 return;
154 }
155
156 rtio_work_req_submit(req, iodev_sqe, lsm6dsv16x_submit_sync);
157 }
158
