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