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