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