1 /*
2 * Copyright (c) 2021 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #include <zephyr/drivers/entropy.h>
7 #include <zephyr/drivers/counter.h>
8 #include <zephyr/drivers/gpio.h>
9 #include <zephyr/busy_sim.h>
10 #include <zephyr/sys/ring_buffer.h>
11 #include <zephyr/random/random.h>
12
13 #define BUFFER_SIZE 32
14
15 struct busy_sim_data {
16 uint32_t idle_avg;
17 uint32_t active_avg;
18 uint16_t idle_delta;
19 uint16_t active_delta;
20 uint32_t us_tick;
21 struct counter_alarm_cfg alarm_cfg;
22 busy_sim_cb_t cb;
23 };
24
25 static struct k_work sim_work;
26 static struct ring_buf rnd_rbuf;
27 static uint8_t rnd_buf[BUFFER_SIZE];
28
29 struct busy_sim_config {
30 const struct device *entropy;
31 const struct device *counter;
32 struct gpio_dt_spec pin_spec;
33 };
34
35 #define DT_BUSY_SIM DT_COMPAT_GET_ANY_STATUS_OKAY(vnd_busy_sim)
36
37 BUILD_ASSERT(DT_NUM_INST_STATUS_OKAY(vnd_busy_sim) == 1,
38 "add exactly one vnd,busy-sim node to the devicetree");
39
40 static const struct busy_sim_config sim_config = {
41 .entropy = COND_CODE_1(CONFIG_XOSHIRO_RANDOM_GENERATOR,
42 (NULL),
43 (DEVICE_DT_GET(DT_CHOSEN(zephyr_entropy)))),
44 .counter = DEVICE_DT_GET(DT_PHANDLE(DT_BUSY_SIM, counter)),
45 .pin_spec = GPIO_DT_SPEC_GET_OR(DT_BUSY_SIM, active_gpios, {0}),
46 };
47
48 static struct busy_sim_data sim_data;
49 static const struct device *const busy_sim_dev = DEVICE_DT_GET_ONE(vnd_busy_sim);
50
rng_pool_work_handler(struct k_work * work)51 static void rng_pool_work_handler(struct k_work *work)
52 {
53 uint8_t *buf;
54 uint32_t len;
55 const struct busy_sim_config *config = busy_sim_dev->config;
56
57 len = ring_buf_put_claim(&rnd_rbuf, &buf, BUFFER_SIZE - 1);
58 if (len) {
59 int err = entropy_get_entropy(config->entropy, buf, len);
60
61 if (err == 0) {
62 ring_buf_put_finish(&rnd_rbuf, len);
63 return;
64 }
65 ring_buf_put_finish(&rnd_rbuf, 0);
66 }
67
68 k_work_submit(work);
69 }
70
71
get_timeout(bool idle)72 static uint32_t get_timeout(bool idle)
73 {
74 struct busy_sim_data *data = busy_sim_dev->data;
75 uint32_t avg = idle ? data->idle_avg : data->active_avg;
76 uint32_t delta = idle ? data->idle_delta : data->active_delta;
77 uint16_t rand_val;
78 uint32_t len;
79
80 if (IS_ENABLED(CONFIG_XOSHIRO_RANDOM_GENERATOR)) {
81 sys_rand_get(&rand_val, sizeof(rand_val));
82 } else {
83 len = ring_buf_get(&rnd_rbuf,
84 (uint8_t *)&rand_val,
85 sizeof(rand_val));
86 if (len < sizeof(rand_val)) {
87 k_work_submit(&sim_work);
88 rand_val = 0;
89 }
90 }
91
92 avg *= data->us_tick;
93 delta *= data->us_tick;
94
95 return avg - delta + 2 * (rand_val % delta);
96 }
97
counter_alarm_callback(const struct device * dev,uint8_t chan_id,uint32_t ticks,void * user_data)98 static void counter_alarm_callback(const struct device *dev,
99 uint8_t chan_id, uint32_t ticks,
100 void *user_data)
101 {
102 int err;
103 const struct busy_sim_config *config = busy_sim_dev->config;
104 struct busy_sim_data *data = busy_sim_dev->data;
105
106 data->alarm_cfg.ticks = get_timeout(true);
107
108 if (config->pin_spec.port) {
109 err = gpio_pin_set_dt(&config->pin_spec, 1);
110 __ASSERT_NO_MSG(err >= 0);
111 }
112
113 /* Busy loop */
114 if (data->cb) {
115 data->cb();
116 }
117
118 k_busy_wait(get_timeout(false) / data->us_tick);
119
120 if (config->pin_spec.port) {
121 err = gpio_pin_set_dt(&config->pin_spec, 0);
122 __ASSERT_NO_MSG(err >= 0);
123 }
124
125 err = counter_set_channel_alarm(config->counter, 0, &data->alarm_cfg);
126 __ASSERT_NO_MSG(err == 0);
127
128 }
129
busy_sim_start(uint32_t active_avg,uint32_t active_delta,uint32_t idle_avg,uint32_t idle_delta,busy_sim_cb_t cb)130 void busy_sim_start(uint32_t active_avg, uint32_t active_delta,
131 uint32_t idle_avg, uint32_t idle_delta, busy_sim_cb_t cb)
132 {
133 int err;
134 const struct busy_sim_config *config = busy_sim_dev->config;
135 struct busy_sim_data *data = busy_sim_dev->data;
136
137 data->cb = cb;
138 data->active_avg = active_avg;
139 data->active_delta = active_delta;
140 data->idle_avg = idle_avg;
141 data->idle_delta = idle_delta;
142
143 if (!IS_ENABLED(CONFIG_XOSHIRO_RANDOM_GENERATOR)) {
144 err = k_work_submit(&sim_work);
145 __ASSERT_NO_MSG(err >= 0);
146 }
147
148 data->alarm_cfg.ticks = counter_us_to_ticks(config->counter, 100);
149 err = counter_set_channel_alarm(config->counter, 0, &data->alarm_cfg);
150 __ASSERT_NO_MSG(err == 0);
151
152 err = counter_start(config->counter);
153 __ASSERT_NO_MSG(err == 0);
154 }
155
busy_sim_stop(void)156 void busy_sim_stop(void)
157 {
158 int err;
159 const struct busy_sim_config *config = busy_sim_dev->config;
160
161 if (!IS_ENABLED(CONFIG_XOSHIRO_RANDOM_GENERATOR)) {
162 k_work_cancel(&sim_work);
163 }
164
165 err = counter_stop(config->counter);
166 __ASSERT_NO_MSG(err == 0);
167 }
168
busy_sim_init(const struct device * dev)169 static int busy_sim_init(const struct device *dev)
170 {
171 uint32_t freq;
172 const struct busy_sim_config *config = dev->config;
173 struct busy_sim_data *data = dev->data;
174
175 if ((config->pin_spec.port && !gpio_is_ready_dt(&config->pin_spec)) ||
176 !device_is_ready(config->counter) ||
177 (!IS_ENABLED(CONFIG_XOSHIRO_RANDOM_GENERATOR) &&
178 !device_is_ready(config->entropy))) {
179 __ASSERT(0, "Devices needed by busy simulator not ready.");
180 return -EIO;
181 }
182
183 if (config->pin_spec.port) {
184 gpio_pin_configure_dt(&config->pin_spec, GPIO_OUTPUT | GPIO_OUTPUT_INIT_LOW);
185 }
186
187 freq = counter_get_frequency(config->counter);
188 if (freq < 1000000) {
189 __ASSERT(0, "Counter device has too low frequency for busy simulator.");
190 return -EINVAL;
191 }
192
193 if (!IS_ENABLED(CONFIG_XOSHIRO_RANDOM_GENERATOR)) {
194 k_work_init(&sim_work, rng_pool_work_handler);
195 ring_buf_init(&rnd_rbuf, BUFFER_SIZE, rnd_buf);
196 }
197
198 data->us_tick = freq / 1000000;
199 data->alarm_cfg.callback = counter_alarm_callback;
200 data->alarm_cfg.flags = COUNTER_ALARM_CFG_EXPIRE_WHEN_LATE;
201
202 return 0;
203 }
204
205 DEVICE_DT_DEFINE(DT_BUSY_SIM, busy_sim_init, NULL,
206 &sim_data, &sim_config,
207 POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY,
208 NULL);
209
