1 /*
2 * Copyright (c) 2023, Bjarki Arge Andreasen
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/device.h>
8 #include <zephyr/kernel.h>
9 #include <zephyr/net/socket.h>
10 #include <zephyr/net/net_if.h>
11 #include <zephyr/net/dns_resolve.h>
12 #include <zephyr/pm/device.h>
13 #include <zephyr/pm/device_runtime.h>
14 #include <string.h>
15
16 #include <zephyr/drivers/cellular.h>
17
18 #define SAMPLE_TEST_ENDPOINT_HOSTNAME CONFIG_SAMPLE_CELLULAR_MODEM_ENDPOINT_HOSTNAME
19 #define SAMPLE_TEST_ENDPOINT_UDP_ECHO_PORT (7780)
20 #define SAMPLE_TEST_ENDPOINT_UDP_RECEIVE_PORT (7781)
21 #define SAMPLE_TEST_PACKET_SIZE (1024)
22 #define SAMPLE_TEST_ECHO_PACKETS (16)
23 #define SAMPLE_TEST_TRANSMIT_PACKETS (128)
24
25 const struct device *modem = DEVICE_DT_GET(DT_ALIAS(modem));
26
27 static uint8_t sample_test_packet[SAMPLE_TEST_PACKET_SIZE];
28 static uint8_t sample_recv_buffer[SAMPLE_TEST_PACKET_SIZE];
29 static bool sample_test_dns_in_progress;
30 static struct dns_addrinfo sample_test_dns_addrinfo;
31
32 K_SEM_DEFINE(dns_query_sem, 0, 1);
33
sample_prng_random(void)34 static uint8_t sample_prng_random(void)
35 {
36 static uint32_t prng_state = 1234;
37
38 prng_state = ((1103515245 * prng_state) + 12345) % (1U << 31);
39 return (uint8_t)(prng_state & 0xFF);
40 }
41
init_sample_test_packet(void)42 static void init_sample_test_packet(void)
43 {
44 for (size_t i = 0; i < sizeof(sample_test_packet); i++) {
45 sample_test_packet[i] = sample_prng_random();
46 }
47 }
48
print_cellular_info(void)49 static void print_cellular_info(void)
50 {
51 int rc;
52 int16_t rssi;
53 char buffer[64];
54
55 rc = cellular_get_signal(modem, CELLULAR_SIGNAL_RSSI, &rssi);
56 if (!rc) {
57 printk("RSSI %d\n", rssi);
58 }
59
60 rc = cellular_get_modem_info(modem, CELLULAR_MODEM_INFO_IMEI, &buffer[0], sizeof(buffer));
61 if (!rc) {
62 printk("IMEI: %s\n", buffer);
63 }
64 rc = cellular_get_modem_info(modem, CELLULAR_MODEM_INFO_MODEL_ID, &buffer[0],
65 sizeof(buffer));
66 if (!rc) {
67 printk("MODEL_ID: %s\n", buffer);
68 }
69 rc = cellular_get_modem_info(modem, CELLULAR_MODEM_INFO_MANUFACTURER, &buffer[0],
70 sizeof(buffer));
71 if (!rc) {
72 printk("MANUFACTURER: %s\n", buffer);
73 }
74 rc = cellular_get_modem_info(modem, CELLULAR_MODEM_INFO_SIM_IMSI, &buffer[0],
75 sizeof(buffer));
76 if (!rc) {
77 printk("SIM_IMSI: %s\n", buffer);
78 }
79 rc = cellular_get_modem_info(modem, CELLULAR_MODEM_INFO_SIM_ICCID, &buffer[0],
80 sizeof(buffer));
81 if (!rc) {
82 printk("SIM_ICCID: %s\n", buffer);
83 }
84 rc = cellular_get_modem_info(modem, CELLULAR_MODEM_INFO_FW_VERSION, &buffer[0],
85 sizeof(buffer));
86 if (!rc) {
87 printk("FW_VERSION: %s\n", buffer);
88 }
89 }
90
91 #ifdef CONFIG_SAMPLE_CELLULAR_MODEM_AUTO_APN
92
93 struct apn_profile {
94 const char *apn;
95 const char *imsi_list;
96 };
97
98 /* Build the static table */
99 static const struct apn_profile apn_profiles[] = {
100 { CONFIG_SAMPLE_CELLULAR_APN_0, CONFIG_SAMPLE_CELLULAR_IMSI_LIST_0 },
101 { CONFIG_SAMPLE_CELLULAR_APN_1, CONFIG_SAMPLE_CELLULAR_IMSI_LIST_1 },
102 { CONFIG_SAMPLE_CELLULAR_APN_2, CONFIG_SAMPLE_CELLULAR_IMSI_LIST_2 },
103 { CONFIG_SAMPLE_CELLULAR_APN_3, CONFIG_SAMPLE_CELLULAR_IMSI_LIST_3 },
104 };
105
106
107 /* Helper function to skip whitespace */
skip_whitespace(const char * ptr)108 static const char *skip_whitespace(const char *ptr)
109 {
110 while (*ptr == ' ' || *ptr == '\t') {
111 ++ptr;
112 }
113 return ptr;
114 }
115
116 /* Helper function to find the end of current profile entry */
list_matches_imsi(const char * list,const char * imsi)117 static bool list_matches_imsi(const char *list, const char *imsi)
118 {
119 for (const char *p = list; *p; ) {
120 p = skip_whitespace(p);
121 if (!*p) {
122 break;
123 }
124
125 /* copy one token from the list */
126 char tok[7];
127 size_t len = 0;
128
129 while (*p && *p != ' ' && *p != '\t' && *p != ',' && len < sizeof(tok) - 1) {
130 tok[len++] = *p++;
131 }
132 tok[len] = '\0';
133
134 if (len >= 5 && len <= 6 && !strncmp(imsi, tok, len)) {
135 return true; /* prefix matches */
136 }
137 }
138 return false;
139 }
140
modem_cellular_find_apn(char * dst,size_t dst_sz,const char * key)141 static int modem_cellular_find_apn(char *dst, size_t dst_sz, const char *key)
142 {
143 for (size_t i = 0; i < ARRAY_SIZE(apn_profiles); i++) {
144 const struct apn_profile *p = &apn_profiles[i];
145
146 if (p->apn[0] == '\0') {
147 continue;
148 }
149
150 if (p->apn[0] && list_matches_imsi(p->imsi_list, key)) {
151 strncpy(dst, p->apn, dst_sz - 1);
152 dst[dst_sz - 1] = '\0';
153 return 0;
154 }
155 }
156
157 return -ENOENT;
158 }
159
modem_event_cb(const struct device * dev,enum cellular_event evt,const void * payload,void * user_data)160 static void modem_event_cb(const struct device *dev, enum cellular_event evt, const void *payload,
161 void *user_data)
162 {
163 ARG_UNUSED(user_data);
164
165 if (evt != CELLULAR_EVENT_MODEM_INFO_CHANGED) {
166 return;
167 }
168
169 const struct cellular_evt_modem_info *mi = payload;
170
171 if (!mi || mi->field != CELLULAR_MODEM_INFO_SIM_IMSI) {
172 return; /* not the IMSI notification */
173 }
174
175 char imsi[32] = {0};
176
177 if (cellular_get_modem_info(dev, CELLULAR_MODEM_INFO_SIM_IMSI, imsi, sizeof(imsi)) != 0) {
178 return;
179 }
180
181 /* Buffer for the APN we may discover */
182 char apn[32] = {0};
183
184 /* Try MCC+MNC with 6 digits first, then 5 digits */
185 for (size_t len = 6; len >= 5; len--) {
186 if (strlen(imsi) < len) {
187 continue;
188 }
189
190 char key[7] = {0};
191
192 memcpy(key, imsi, len);
193
194 if (modem_cellular_find_apn(apn, sizeof(apn), key) == 0) {
195 int rc = cellular_set_apn(dev, apn);
196
197 switch (rc) {
198 case 0:
199 printk("Auto-selected APN: %s\n", apn);
200 break;
201 case -EALREADY:
202 printk("APN %s already active\n", apn);
203 break;
204 case -EBUSY:
205 printk("Driver busy, cannot change APN now\n");
206 break;
207 default:
208 printk("Driver rejected APN %s (err %d)\n", apn, rc);
209 break;
210 }
211 return;
212 }
213 }
214
215 printk("No APN profile matches IMSI %s - waiting for manual APN\n", imsi);
216 }
217
218 #endif
219
sample_dns_request_result(enum dns_resolve_status status,struct dns_addrinfo * info,void * user_data)220 static void sample_dns_request_result(enum dns_resolve_status status, struct dns_addrinfo *info,
221 void *user_data)
222 {
223 if (sample_test_dns_in_progress == false) {
224 return;
225 }
226
227 if (status != DNS_EAI_INPROGRESS) {
228 return;
229 }
230
231 sample_test_dns_in_progress = false;
232 sample_test_dns_addrinfo = *info;
233 k_sem_give(&dns_query_sem);
234 }
235
sample_dns_request(void)236 static int sample_dns_request(void)
237 {
238 static uint16_t dns_id;
239 int ret;
240
241 sample_test_dns_in_progress = true;
242 ret = dns_get_addr_info(SAMPLE_TEST_ENDPOINT_HOSTNAME,
243 DNS_QUERY_TYPE_A,
244 &dns_id,
245 sample_dns_request_result,
246 NULL,
247 19000);
248 if (ret < 0) {
249 return -EAGAIN;
250 }
251
252 if (k_sem_take(&dns_query_sem, K_SECONDS(20)) < 0) {
253 return -EAGAIN;
254 }
255
256 return 0;
257 }
258
sample_echo_packet(struct sockaddr * ai_addr,socklen_t ai_addrlen,uint16_t * port)259 int sample_echo_packet(struct sockaddr *ai_addr, socklen_t ai_addrlen, uint16_t *port)
260 {
261 int ret;
262 int socket_fd;
263 uint32_t packets_sent = 0;
264 uint32_t send_start_ms;
265 uint32_t echo_received_ms;
266 uint32_t accumulated_ms = 0;
267
268 printk("Opening UDP socket\n");
269
270 socket_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
271 if (socket_fd < 0) {
272 printk("Failed to open socket (%d)\n", errno);
273 return -1;
274 }
275
276 {
277 const struct timeval tv = { .tv_sec = 10 };
278
279 if (zsock_setsockopt(socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
280 printk("Failed to set socket receive timeout (%d)\n", errno);
281 return -1;
282 }
283 }
284
285 printk("Socket opened\n");
286
287 *port = htons(SAMPLE_TEST_ENDPOINT_UDP_ECHO_PORT);
288
289 for (uint32_t i = 0; i < SAMPLE_TEST_ECHO_PACKETS; i++) {
290 printk("Sending echo packet\n");
291 send_start_ms = k_uptime_get_32();
292
293 ret = sendto(socket_fd, sample_test_packet, sizeof(sample_test_packet), 0,
294 ai_addr, ai_addrlen);
295
296 if (ret < sizeof(sample_test_packet)) {
297 printk("Failed to send sample test packet\n");
298 continue;
299 }
300
301 printk("Receiving echoed packet\n");
302 ret = recv(socket_fd, sample_recv_buffer, sizeof(sample_recv_buffer), 0);
303 if (ret != sizeof(sample_test_packet)) {
304 if (ret == -1) {
305 printk("Failed to receive echoed sample test packet (%d)\n", errno);
306 } else {
307 printk("Echoed sample test packet has incorrect size (%d)\n", ret);
308 }
309 continue;
310 }
311
312 echo_received_ms = k_uptime_get_32();
313
314 if (memcmp(sample_test_packet, sample_recv_buffer,
315 sizeof(sample_recv_buffer)) != 0) {
316 printk("Echoed sample test packet data mismatch\n");
317 continue;
318 }
319
320 packets_sent++;
321 accumulated_ms += echo_received_ms - send_start_ms;
322
323 printk("Echo transmit time %ums\n", echo_received_ms - send_start_ms);
324 }
325
326 printk("Successfully sent and received %u of %u packets\n", packets_sent,
327 SAMPLE_TEST_ECHO_PACKETS);
328
329 if (packets_sent > 0) {
330 printk("Average time per successful echo: %u ms\n",
331 accumulated_ms / packets_sent);
332 }
333
334 printk("Close UDP socket\n");
335
336 ret = close(socket_fd);
337 if (ret < 0) {
338 printk("Failed to close socket\n");
339 return -1;
340 }
341
342 return 0;
343 }
344
345
sample_transmit_packets(struct sockaddr * ai_addr,socklen_t ai_addrlen,uint16_t * port)346 int sample_transmit_packets(struct sockaddr *ai_addr, socklen_t ai_addrlen, uint16_t *port)
347 {
348 int ret;
349 int socket_fd;
350 uint32_t packets_sent = 0;
351 uint32_t packets_received;
352 uint32_t packets_dropped;
353 uint32_t send_start_ms;
354 uint32_t send_end_ms;
355
356 printk("Opening UDP socket\n");
357
358 socket_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
359 if (socket_fd < 0) {
360 printk("Failed to open socket\n");
361 return -1;
362 }
363
364 printk("Socket opened\n");
365
366 *port = htons(SAMPLE_TEST_ENDPOINT_UDP_RECEIVE_PORT);
367
368 printk("Sending %u packets\n", SAMPLE_TEST_TRANSMIT_PACKETS);
369 send_start_ms = k_uptime_get_32();
370 for (uint32_t i = 0; i < SAMPLE_TEST_TRANSMIT_PACKETS; i++) {
371 ret = sendto(socket_fd, sample_test_packet, sizeof(sample_test_packet), 0,
372 ai_addr, ai_addrlen);
373
374 if (ret < sizeof(sample_test_packet)) {
375 printk("Failed to send sample test packet\n");
376 break;
377 }
378
379 packets_sent++;
380 }
381 send_end_ms = k_uptime_get_32();
382
383 printk("Awaiting response from server\n");
384 ret = recv(socket_fd, sample_recv_buffer, sizeof(sample_recv_buffer), 0);
385 if (ret != 2) {
386 printk("Invalid response\n");
387 return -1;
388 }
389
390 packets_received = sample_recv_buffer[0];
391 packets_dropped = sample_recv_buffer[1];
392 printk("Server received %u/%u packets\n", packets_received, packets_sent);
393 printk("Server dropped %u packets\n", packets_dropped);
394 printk("Time elapsed sending packets %ums\n", send_end_ms - send_start_ms);
395 printk("Throughput %u bytes/s\n",
396 ((SAMPLE_TEST_PACKET_SIZE * SAMPLE_TEST_TRANSMIT_PACKETS) * 1000) /
397 (send_end_ms - send_start_ms));
398
399 printk("Close UDP socket\n");
400 ret = close(socket_fd);
401 if (ret < 0) {
402 printk("Failed to close socket\n");
403 return -1;
404 }
405
406 return 0;
407 }
408
main(void)409 int main(void)
410 {
411 struct net_if *const iface = net_if_get_first_by_type(&NET_L2_GET_NAME(PPP));
412 uint16_t *port;
413 int ret;
414
415 #ifdef CONFIG_SAMPLE_CELLULAR_MODEM_AUTO_APN
416 /* subscribe before powering the modem so we catch the IMSI event */
417 cellular_set_callback(modem, CELLULAR_EVENT_MODEM_INFO_CHANGED, modem_event_cb, NULL);
418 #endif
419
420 init_sample_test_packet();
421
422 printk("Powering on modem\n");
423 pm_device_action_run(modem, PM_DEVICE_ACTION_RESUME);
424
425 printk("Bring up network interface\n");
426 ret = net_if_up(iface);
427 if (ret < 0) {
428 printk("Failed to bring up network interface\n");
429 return -1;
430 }
431
432 printk("Waiting for L4 connected & DNS server added\n");
433 ret = net_mgmt_event_wait_on_iface(iface, NET_EVENT_L4_CONNECTED | NET_EVENT_DNS_SERVER_ADD,
434 NULL, NULL, NULL, K_SECONDS(120));
435
436 if (ret != 0) {
437 printk("L4 was not connected in time\n");
438 return -1;
439 }
440
441 printk("Retrieving cellular info\n");
442 print_cellular_info();
443
444 printk("Performing DNS lookup of %s\n", SAMPLE_TEST_ENDPOINT_HOSTNAME);
445 ret = sample_dns_request();
446 if (ret < 0) {
447 printk("DNS query failed\n");
448 return -1;
449 }
450
451 {
452 char ip_str[INET6_ADDRSTRLEN];
453 const void *src;
454
455 switch (sample_test_dns_addrinfo.ai_addr.sa_family) {
456 case AF_INET:
457 src = &net_sin(&sample_test_dns_addrinfo.ai_addr)->sin_addr;
458 port = &net_sin(&sample_test_dns_addrinfo.ai_addr)->sin_port;
459 break;
460 case AF_INET6:
461 src = &net_sin6(&sample_test_dns_addrinfo.ai_addr)->sin6_addr;
462 port = &net_sin6(&sample_test_dns_addrinfo.ai_addr)->sin6_port;
463 break;
464 default:
465 printk("Unsupported address family\n");
466 return -1;
467 }
468 inet_ntop(sample_test_dns_addrinfo.ai_addr.sa_family, src, ip_str, sizeof(ip_str));
469 printk("Resolved to %s\n", ip_str);
470 }
471
472 ret = sample_echo_packet(&sample_test_dns_addrinfo.ai_addr,
473 sample_test_dns_addrinfo.ai_addrlen, port);
474
475 if (ret < 0) {
476 printk("Failed to send echos\n");
477 return -1;
478 }
479
480 ret = sample_transmit_packets(&sample_test_dns_addrinfo.ai_addr,
481 sample_test_dns_addrinfo.ai_addrlen, port);
482
483 if (ret < 0) {
484 printk("Failed to send packets\n");
485 return -1;
486 }
487
488 printk("Restart modem\n");
489 ret = pm_device_action_run(modem, PM_DEVICE_ACTION_SUSPEND);
490 if (ret != 0) {
491 printk("Failed to power down modem\n");
492 return -1;
493 }
494
495 pm_device_action_run(modem, PM_DEVICE_ACTION_RESUME);
496
497 printk("Waiting for L4 connected\n");
498 ret = net_mgmt_event_wait_on_iface(iface, NET_EVENT_L4_CONNECTED, NULL, NULL, NULL,
499 K_SECONDS(60));
500 if (ret != 0) {
501 printk("L4 was not connected in time\n");
502 return -1;
503 }
504 printk("L4 connected\n");
505
506 /* Wait a bit to avoid (unsuccessfully) trying to send the first echo packet too quickly. */
507 k_sleep(K_SECONDS(5));
508
509 ret = sample_echo_packet(&sample_test_dns_addrinfo.ai_addr,
510 sample_test_dns_addrinfo.ai_addrlen, port);
511
512 if (ret < 0) {
513 printk("Failed to send echos after restart\n");
514 return -1;
515 }
516
517 ret = net_if_down(iface);
518 if (ret < 0) {
519 printk("Failed to bring down network interface\n");
520 return -1;
521 }
522
523 printk("Powering down modem\n");
524 ret = pm_device_action_run(modem, PM_DEVICE_ACTION_SUSPEND);
525 if (ret != 0) {
526 printk("Failed to power down modem\n");
527 return -1;
528 }
529
530 printk("Sample complete\n");
531 return 0;
532 }
533