1 /*
2 * Copyright (c) 2022 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT vnd_sensor
8
9 #include <zephyr/kernel.h>
10 #include <zephyr/rtio/rtio.h>
11 #include <zephyr/logging/log.h>
12 #include <zephyr/sys/util.h>
13
14 LOG_MODULE_REGISTER(vnd_sensor, LOG_LEVEL_DBG);
15
16 struct vnd_sensor_config {
17 uint32_t sample_period;
18 size_t sample_size;
19 };
20
21 struct vnd_sensor_data {
22 struct rtio_iodev iodev;
23 struct mpsc io_q;
24 struct k_timer timer;
25 const struct device *dev;
26 uint32_t sample_number;
27 };
28
vnd_sensor_iodev_read(const struct device * dev,uint8_t * buf,uint32_t buf_len)29 static int vnd_sensor_iodev_read(const struct device *dev, uint8_t *buf,
30 uint32_t buf_len)
31 {
32 const struct vnd_sensor_config *config = dev->config;
33 struct vnd_sensor_data *data = dev->data;
34 uint32_t sample_number;
35 uint32_t key;
36
37 LOG_DBG("%s: buf_len = %d, buf = %p", dev->name, buf_len, buf);
38
39 key = irq_lock();
40 sample_number = data->sample_number++;
41 irq_unlock(key);
42
43 if (buf_len < config->sample_size) {
44 LOG_ERR("%s: Buffer is too small", dev->name);
45 return -ENOMEM;
46 }
47
48 for (int i = 0; i < MIN(config->sample_size, buf_len); i++) {
49 buf[i] = sample_number * config->sample_size + i;
50 }
51
52 return 0;
53 }
54
vnd_sensor_iodev_execute(const struct device * dev,struct rtio_iodev_sqe * iodev_sqe)55 static void vnd_sensor_iodev_execute(const struct device *dev,
56 struct rtio_iodev_sqe *iodev_sqe)
57 {
58 const struct vnd_sensor_config *config = dev->config;
59 uint8_t *buf = NULL;
60 uint32_t buf_len;
61 int result;
62
63 if (iodev_sqe->sqe.op == RTIO_OP_RX) {
64 result = rtio_sqe_rx_buf(iodev_sqe, config->sample_size, config->sample_size, &buf,
65 &buf_len);
66 if (result != 0) {
67 LOG_ERR("Failed to get RX buffer");
68 } else {
69 result = vnd_sensor_iodev_read(dev, buf, buf_len);
70 }
71 } else {
72 LOG_ERR("%s: Invalid op", dev->name);
73 result = -EINVAL;
74 }
75
76 if (result < 0) {
77 rtio_iodev_sqe_err(iodev_sqe, result);
78 } else {
79 rtio_iodev_sqe_ok(iodev_sqe, result);
80 }
81 }
82
vnd_sensor_iodev_submit(struct rtio_iodev_sqe * iodev_sqe)83 static void vnd_sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe)
84 {
85 struct vnd_sensor_data *data = (struct vnd_sensor_data *) iodev_sqe->sqe.iodev;
86
87 mpsc_push(&data->io_q, &iodev_sqe->q);
88 }
89
vnd_sensor_handle_int(const struct device * dev)90 static void vnd_sensor_handle_int(const struct device *dev)
91 {
92 struct vnd_sensor_data *data = dev->data;
93 struct mpsc_node *node = mpsc_pop(&data->io_q);
94
95 if (node != NULL) {
96 struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q);
97
98 vnd_sensor_iodev_execute(dev, iodev_sqe);
99 } else {
100 LOG_ERR("%s: Could not get a msg", dev->name);
101 }
102 }
103
vnd_sensor_timer_expiry(struct k_timer * timer)104 static void vnd_sensor_timer_expiry(struct k_timer *timer)
105 {
106 struct vnd_sensor_data *data =
107 CONTAINER_OF(timer, struct vnd_sensor_data, timer);
108
109 vnd_sensor_handle_int(data->dev);
110 }
111
vnd_sensor_init(const struct device * dev)112 static int vnd_sensor_init(const struct device *dev)
113 {
114 const struct vnd_sensor_config *config = dev->config;
115 struct vnd_sensor_data *data = dev->data;
116 uint32_t sample_period = config->sample_period;
117
118 data->dev = dev;
119
120 mpsc_init(&data->io_q);
121
122 k_timer_init(&data->timer, vnd_sensor_timer_expiry, NULL);
123
124 k_timer_start(&data->timer, K_MSEC(sample_period),
125 K_MSEC(sample_period));
126
127 return 0;
128 }
129
130 static const struct rtio_iodev_api vnd_sensor_iodev_api = {
131 .submit = vnd_sensor_iodev_submit,
132 };
133
134 #define VND_SENSOR_INIT(n) \
135 \
136 static const struct vnd_sensor_config vnd_sensor_config_##n = { \
137 .sample_period = DT_INST_PROP(n, sample_period), \
138 .sample_size = DT_INST_PROP(n, sample_size), \
139 }; \
140 \
141 static struct vnd_sensor_data vnd_sensor_data_##n = { \
142 .iodev = \
143 { \
144 .api = &vnd_sensor_iodev_api, \
145 }, \
146 }; \
147 \
148 DEVICE_DT_INST_DEFINE(n, vnd_sensor_init, NULL, &vnd_sensor_data_##n, \
149 &vnd_sensor_config_##n, POST_KERNEL, \
150 CONFIG_KERNEL_INIT_PRIORITY_DEVICE, NULL);
151
152 DT_INST_FOREACH_STATUS_OKAY(VND_SENSOR_INIT)
153
