1 /*
2 *
3 * SPDX-License-Identifier: Apache-2.0
4 *
5 * Copyright (c) 2025 Jorge Ramirez-Ortiz <jorge.ramirez@oss.qualcomm.com>
6 */
7
8 #include <zephyr/logging/log.h>
9 LOG_MODULE_REGISTER(sample_latmon, LOG_LEVEL_DBG);
10
11 #include <zephyr/drivers/gpio.h>
12 #include <zephyr/net/latmon.h>
13 #include <zephyr/net/socket.h>
14 #include <zephyr/spinlock.h>
15 #include <zephyr/sys/atomic.h>
16 /*
17 * Blink Control
18 * DHCP: red
19 * waiting for connection: blue
20 * sampling: green
21 */
22 #define LED_WAIT_PERIOD 1000000
23 #define LED_DHCP_PERIOD 500000
24 #define LED_RUN_PERIOD 200000
25
26 #define BLINK_THREAD_PRIORITY K_IDLE_PRIO
27 #define BLINK_STACK_SIZE 4096
28 static K_THREAD_STACK_DEFINE(blink_stack, BLINK_STACK_SIZE);
29
30 static const struct gpio_dt_spec pulse =
31 GPIO_DT_SPEC_GET_OR(DT_PATH(zephyr_user), pulse_gpios, {0});
32 static const struct gpio_dt_spec ack =
33 GPIO_DT_SPEC_GET_OR(DT_PATH(zephyr_user), ack_gpios, {0});
34
35 static K_SEM_DEFINE(ack_event, 0, 1);
36
37 #define DHCP_DONE (atomic_test_bit(&dhcp_done, 0) == true)
38 #define SET_DHCP_DONE atomic_set_bit(&dhcp_done, 0)
39 static atomic_val_t dhcp_done;
40
41 static struct k_spinlock lock;
42
gpio_ack_handler(const struct device * port,struct gpio_callback * cb,gpio_port_pins_t pins)43 static void gpio_ack_handler(const struct device *port,
44 struct gpio_callback *cb,
45 gpio_port_pins_t pins)
46 {
47 k_sem_give(&ack_event);
48 }
49
configure_measurement_hardware(void)50 static int configure_measurement_hardware(void)
51 {
52 static struct gpio_callback gpio_cb = { };
53 int ret = 0;
54
55 if (!gpio_is_ready_dt(&pulse) || !gpio_is_ready_dt(&ack)) {
56 LOG_ERR("GPIO device not ready");
57 return -ENODEV;
58 }
59
60 ret = gpio_pin_configure_dt(&pulse, GPIO_OUTPUT_HIGH);
61 if (ret < 0) {
62 LOG_ERR("failed configuring pulse pin");
63 return ret;
64 }
65
66 ret = gpio_pin_configure_dt(&ack, GPIO_INPUT);
67 if (ret < 0) {
68 LOG_ERR("failed configuring ack pin");
69 return ret;
70 }
71
72 #if defined(CONFIG_LATMON_LOOPBACK_CALIBRATION)
73 /*
74 * Connect GPIO pins in loopback mode for validation (tx to ack)
75 * On FRDM_K64F, Latmus will show around 3.2 usec of latency.
76 *
77 * You can then use these values to adjust the reported latencies (ie,
78 * subtract the loopback latency from the measured latencies).
79 */
80 ret = gpio_pin_interrupt_configure_dt(&ack, GPIO_INT_EDGE_FALLING);
81 #else
82 ret = gpio_pin_interrupt_configure_dt(&ack, GPIO_INT_EDGE_RISING);
83 #endif
84 if (ret < 0) {
85 LOG_ERR("failed configuring ack pin interrupt");
86 return ret;
87 }
88
89 gpio_init_callback(&gpio_cb, gpio_ack_handler, BIT(ack.pin));
90
91 ret = gpio_add_callback_dt(&ack, &gpio_cb);
92 if (ret < 0) {
93 LOG_ERR("failed adding ack pin callback");
94 return ret;
95 }
96
97 return ret;
98 }
99
blink(void *,void *,void *)100 static void blink(void*, void*, void*)
101 {
102 const struct gpio_dt_spec led_run =
103 GPIO_DT_SPEC_GET_OR(DT_ALIAS(led0), gpios, {0});
104 const struct gpio_dt_spec led_wait =
105 GPIO_DT_SPEC_GET_OR(DT_ALIAS(led1), gpios, {0});
106 const struct gpio_dt_spec led_dhcp =
107 GPIO_DT_SPEC_GET_OR(DT_ALIAS(led2), gpios, {0});
108 const struct gpio_dt_spec *led = &led_dhcp, *tmp = NULL;
109 uint32_t period = LED_DHCP_PERIOD;
110
111 if (gpio_is_ready_dt(&led_run)) {
112 gpio_pin_configure_dt(&led_run, GPIO_OUTPUT_INACTIVE);
113 }
114
115 if (gpio_is_ready_dt(&led_wait)) {
116 gpio_pin_configure_dt(&led_wait, GPIO_OUTPUT_INACTIVE);
117 }
118
119 if (gpio_is_ready_dt(&led_dhcp)) {
120 gpio_pin_configure_dt(&led_dhcp, GPIO_OUTPUT_INACTIVE);
121 }
122
123 for (;;) {
124 k_usleep(period);
125 if (DHCP_DONE) {
126 led = net_latmon_running() ? &led_run : &led_wait;
127 }
128
129 if (tmp && led != tmp) {
130 gpio_pin_set_dt(tmp, 0);
131 }
132
133 if (!gpio_is_ready_dt(led)) {
134 continue;
135 }
136
137 if (led == &led_wait) {
138 period = LED_WAIT_PERIOD;
139 }
140
141 if (led == &led_run) {
142 period = LED_RUN_PERIOD;
143 }
144
145 gpio_pin_toggle_dt(led);
146 tmp = led;
147 }
148 gpio_pin_set_dt(led, 0);
149 }
150
start_led_blinking_thread(struct k_thread * blink_thread,k_thread_entry_t blink_thread_func)151 static k_tid_t start_led_blinking_thread(struct k_thread *blink_thread,
152 k_thread_entry_t blink_thread_func)
153 {
154 return k_thread_create(blink_thread, blink_stack, BLINK_STACK_SIZE,
155 (k_thread_entry_t)blink_thread_func,
156 NULL, NULL, NULL,
157 BLINK_THREAD_PRIORITY, 0, K_NO_WAIT);
158 }
159
160 /* Raw ticks */
161 #define CALCULATE_DELTA(ack, pulse) \
162 ((ack) < (pulse) ? \
163 (~(pulse) + 1 + (ack)) : ((ack) - (pulse)))
164
measure_latency_cycles(uint32_t * delta)165 static int measure_latency_cycles(uint32_t *delta)
166 {
167 k_spinlock_key_t key;
168 uint32_t tx = 0;
169 uint32_t rx = 0;
170 int ret = 0;
171
172 /* Remove spurious events */
173 k_sem_reset(&ack_event);
174
175 /* Generate a falling edge pulse to the DUT */
176 key = k_spin_lock(&lock);
177 if (gpio_pin_set_dt(&pulse, 0)) {
178 k_spin_unlock(&lock, key);
179 LOG_ERR("Failed to set pulse pin");
180 ret = -1;
181 goto out;
182 }
183 tx = k_cycle_get_32();
184 k_spin_unlock(&lock, key);
185
186 /* Wait for a rising edge from the Latmus controlled DUT */
187 if (k_sem_take(&ack_event, K_MSEC(1)) == 0) {
188 rx = k_cycle_get_32();
189 /* Measure the cycles */
190 *delta = CALCULATE_DELTA(rx, tx);
191 } else {
192 ret = -1;
193 }
194 out:
195 if (gpio_pin_set_dt(&pulse, 1)) {
196 LOG_ERR("Failed to clear pulse pin");
197 ret = -1;
198 }
199
200 return ret;
201 }
202
main(void)203 int main(void)
204 {
205 struct net_if *iface = net_if_get_default();
206 struct k_thread blink_thread;
207 static k_tid_t blink_tid;
208 int client, socket = 0;
209 int ret = 0;
210
211 /* Prepare the instrumentation */
212 if (configure_measurement_hardware() < 0) {
213 LOG_ERR("Failed to configure the measurement hardware");
214 return -1;
215 }
216
217 /* Start visual indicators - dhcp/blue, waiting/red, running/green */
218 blink_tid = start_led_blinking_thread(&blink_thread, blink);
219 if (!blink_tid) {
220 LOG_WRN("Failed to start led blinking thread");
221 }
222
223 /* Get a valid ip */
224 LOG_INF("DHCPv4: binding...");
225 net_dhcpv4_start(iface);
226 for (;;) {
227 ret = net_mgmt_event_wait(NET_EVENT_IPV4_DHCP_BOUND, NULL,
228 NULL, NULL, NULL, K_SECONDS(10));
229 if (ret == -ETIMEDOUT) {
230 LOG_WRN("DHCPv4: binding timed out, retrying...");
231 continue;
232 }
233 if (ret < 0) {
234 LOG_ERR("DHCPv4: binding failed, aborting...");
235 goto out;
236 }
237 break;
238 }
239
240 SET_DHCP_DONE;
241
242 /* Get a socket to the Latmus port */
243 socket = net_latmon_get_socket(NULL);
244 if (socket < 0) {
245 LOG_ERR("Failed to get a socket to latmon (errno %d)", socket);
246 ret = -1;
247 goto out;
248 }
249
250 for (;;) {
251 /* Wait for Latmus to connect */
252 client = net_latmon_connect(socket,
253 &iface->config.dhcpv4.requested_ip);
254 if (client < 0) {
255 if (client == -EAGAIN) {
256 continue;
257 }
258 LOG_ERR("Failed to connect to latmon");
259 ret = -1;
260 goto out;
261 }
262
263 /* Provide latency data until Latmus closes the connection */
264 net_latmon_start(client, measure_latency_cycles);
265 }
266 out:
267 k_thread_abort(blink_tid);
268 close(socket);
269
270 return ret;
271 }
272