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 #if defined(CONFIG_XOSHIRO_RANDOM_GENERATOR) || defined(CONFIG_TIMER_RANDOM_GENERATOR)
41 #define USE_TEST_RANDOM 1
42 #endif
43
44 static const struct busy_sim_config sim_config = {
45 .entropy = COND_CODE_1(USE_TEST_RANDOM,
46 (NULL),
47 (DEVICE_DT_GET(DT_CHOSEN(zephyr_entropy)))),
48 .counter = DEVICE_DT_GET(DT_PHANDLE(DT_BUSY_SIM, counter)),
49 .pin_spec = GPIO_DT_SPEC_GET_OR(DT_BUSY_SIM, active_gpios, {0}),
50 };
51
52 static struct busy_sim_data sim_data;
53 static const struct device *const busy_sim_dev = DEVICE_DT_GET_ONE(vnd_busy_sim);
54
rng_pool_work_handler(struct k_work * work)55 static void rng_pool_work_handler(struct k_work *work)
56 {
57 uint8_t *buf;
58 uint32_t len;
59 const struct busy_sim_config *config = busy_sim_dev->config;
60
61 len = ring_buf_put_claim(&rnd_rbuf, &buf, BUFFER_SIZE - 1);
62 if (len) {
63 int err = entropy_get_entropy(config->entropy, buf, len);
64
65 if (err == 0) {
66 ring_buf_put_finish(&rnd_rbuf, len);
67 return;
68 }
69 ring_buf_put_finish(&rnd_rbuf, 0);
70 }
71
72 k_work_submit(work);
73 }
74
75
get_timeout(bool idle,bool use_rand)76 static uint32_t get_timeout(bool idle, bool use_rand)
77 {
78 struct busy_sim_data *data = busy_sim_dev->data;
79 uint32_t avg = idle ? data->idle_avg : data->active_avg;
80 uint32_t delta = idle ? data->idle_delta : data->active_delta;
81 uint16_t rand_val;
82 uint32_t len;
83
84 if (use_rand) {
85 sys_rand_get(&rand_val, sizeof(rand_val));
86 } else {
87 len = ring_buf_get(&rnd_rbuf,
88 (uint8_t *)&rand_val,
89 sizeof(rand_val));
90 if (len < sizeof(rand_val)) {
91 k_work_submit(&sim_work);
92 rand_val = 0;
93 }
94 }
95
96 avg *= data->us_tick;
97 delta *= data->us_tick;
98
99 return avg - delta + 2 * (rand_val % delta);
100 }
101
counter_alarm_callback(const struct device * dev,uint8_t chan_id,uint32_t ticks,void * user_data)102 static void counter_alarm_callback(const struct device *dev,
103 uint8_t chan_id, uint32_t ticks,
104 void *user_data)
105 {
106 int err;
107 const struct busy_sim_config *config = busy_sim_dev->config;
108 struct busy_sim_data *data = busy_sim_dev->data;
109
110 data->alarm_cfg.ticks = get_timeout(true, !config->entropy);
111
112 if (config->pin_spec.port) {
113 err = gpio_pin_set_dt(&config->pin_spec, 1);
114 __ASSERT_NO_MSG(err >= 0);
115 }
116
117 /* Busy loop */
118 if (data->cb) {
119 data->cb();
120 }
121
122 k_busy_wait(get_timeout(false, !config->entropy) / data->us_tick);
123
124 if (config->pin_spec.port) {
125 err = gpio_pin_set_dt(&config->pin_spec, 0);
126 __ASSERT_NO_MSG(err >= 0);
127 }
128
129 err = counter_set_channel_alarm(config->counter, 0, &data->alarm_cfg);
130 __ASSERT_NO_MSG(err == 0);
131
132 }
133
busy_sim_start(uint32_t active_avg,uint32_t active_delta,uint32_t idle_avg,uint32_t idle_delta,busy_sim_cb_t cb)134 void busy_sim_start(uint32_t active_avg, uint32_t active_delta,
135 uint32_t idle_avg, uint32_t idle_delta, busy_sim_cb_t cb)
136 {
137 int err;
138 const struct busy_sim_config *config = busy_sim_dev->config;
139 struct busy_sim_data *data = busy_sim_dev->data;
140
141 data->cb = cb;
142 data->active_avg = active_avg;
143 data->active_delta = active_delta;
144 data->idle_avg = idle_avg;
145 data->idle_delta = idle_delta;
146
147 if (config->entropy) {
148 err = k_work_submit(&sim_work);
149 __ASSERT_NO_MSG(err >= 0);
150 }
151
152 data->alarm_cfg.ticks = counter_us_to_ticks(config->counter, 100);
153 err = counter_set_channel_alarm(config->counter, 0, &data->alarm_cfg);
154 __ASSERT_NO_MSG(err == 0);
155
156 err = counter_start(config->counter);
157 __ASSERT_NO_MSG(err == 0);
158 }
159
busy_sim_stop(void)160 void busy_sim_stop(void)
161 {
162 int err;
163 const struct busy_sim_config *config = busy_sim_dev->config;
164
165 if (config->entropy) {
166 k_work_cancel(&sim_work);
167 }
168
169 err = counter_cancel_channel_alarm(config->counter, 0);
170 __ASSERT_NO_MSG(err == 0);
171
172 err = counter_stop(config->counter);
173 __ASSERT_NO_MSG(err == 0);
174 }
175
busy_sim_init(const struct device * dev)176 static int busy_sim_init(const struct device *dev)
177 {
178 uint32_t freq;
179 const struct busy_sim_config *config = dev->config;
180 struct busy_sim_data *data = dev->data;
181
182 if ((config->pin_spec.port && !gpio_is_ready_dt(&config->pin_spec)) ||
183 !device_is_ready(config->counter) ||
184 (config->entropy && !device_is_ready(config->entropy))) {
185 __ASSERT(0, "Devices needed by busy simulator not ready.");
186 return -EIO;
187 }
188
189 if (config->pin_spec.port) {
190 gpio_pin_configure_dt(&config->pin_spec, GPIO_OUTPUT | GPIO_OUTPUT_INIT_LOW);
191 }
192
193 freq = counter_get_frequency(config->counter);
194 if (freq < 1000000) {
195 __ASSERT(0, "Counter device has too low frequency for busy simulator.");
196 return -EINVAL;
197 }
198
199 if (config->entropy) {
200 k_work_init(&sim_work, rng_pool_work_handler);
201 ring_buf_init(&rnd_rbuf, BUFFER_SIZE, rnd_buf);
202 }
203
204 data->us_tick = freq / 1000000;
205 data->alarm_cfg.callback = counter_alarm_callback;
206 data->alarm_cfg.flags = COUNTER_ALARM_CFG_EXPIRE_WHEN_LATE;
207
208 return 0;
209 }
210
211 DEVICE_DT_DEFINE(DT_BUSY_SIM, busy_sim_init, NULL,
212 &sim_data, &sim_config,
213 POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY,
214 NULL);
215
