1 /*
2 Copyright (c) 2021 Fraunhofer AISEC. See the COPYRIGHT
3 file at the top-level directory of this distribution.
4
5 Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
6 http://www.apache.org/licenses/LICENSE-2.0> or the MIT license
7 <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
8 option. This file may not be copied, modified, or distributed
9 except according to those terms.
10 */
11 #include <logging/log.h>
12 #include <stdio.h>
13 LOG_MODULE_REGISTER(net_coap_server_sample, LOG_LEVEL_DBG);
14
15 #include <edhoc.h>
16 #include <errno.h>
17 #include <net/coap.h>
18 #include <net/coap_link_format.h>
19 #include <net/net_ip.h>
20 #include <net/net_mgmt.h>
21 #include <net/socket.h>
22 #include <net/udp.h>
23 #include <sys/byteorder.h>
24 #include <sys/printk.h>
25 #include <zephyr.h>
26
27 #include "credentials.h"
28 #include "net_private.h"
29
30 #define MAX_COAP_MSG_LEN 256
31
32 #define MY_COAP_PORT 5683
33
34 #define BLOCK_WISE_TRANSFER_SIZE_GET 2048
35
36 #if defined(CONFIG_NET_IPV6)
37 #define ALL_NODES_LOCAL_COAP_MCAST \
38 { \
39 { \
40 { \
41 0xff, 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, \
42 0, 0, 0xfd \
43 } \
44 } \
45 }
46
47 #define MY_IP6ADDR \
48 { \
49 { \
50 { \
51 0x20, 0x01, 0x0d, 0xb8, 0, 0, 0, 0, 0, 0, 0, \
52 0, 0, 0, 0, 0x1 \
53 } \
54 } \
55 }
56 #endif
57
58 #define NUM_OBSERVERS 3
59
60 #define NUM_PENDINGS 3
61
62 /* Uncomment the following line to enable printf output */
63 //#define ENABLE_PRINTK
64 #ifdef ENABLE_PRINTK
65 #define PRINTK(text, ...) printk(text, ##__VA_ARGS__)
66 #else
67 #define PRINTK(text, ...)
68 #endif
69
70 /* create queues for EDHOC */
71 #define MBOX_MSG_SIZE 300
72 #define MBOX_WAIT_TIME 20
73 K_MBOX_DEFINE(rx_queue);
74 K_MBOX_DEFINE(tx_queue);
75
76 /**
77 * @brief Entry function of EDHOC thread. Starts EDHOC responder.
78 * @param
79 * @retval none
80 */
edhoc_responder_init(void)81 void edhoc_responder_init(void)
82 {
83 err r;
84
85 /* Edhoc internal parameters */
86 uint8_t PRK_4x3m[PRK_DEFAULT_SIZE];
87 uint8_t th4[SHA_DEFAULT_SIZE];
88 uint8_t err_msg[ERR_MSG_DEFAULT_SIZE];
89 uint32_t err_msg_len = sizeof(err_msg);
90 struct other_party_cred cred_i = { .id_cred.ptr = ID_CRED_I,
91 .id_cred.len = ID_CRED_I_LEN,
92 .cred.ptr = CRED_I,
93 .cred.len = CRED_I_LEN,
94 .pk.ptr = PK_I,
95 .pk.len = PK_I_LEN,
96 .g.ptr = G_I,
97 .g.len = G_I_LEN,
98 .ca.ptr = CA,
99 .ca.len = CA_LEN,
100 .ca_pk.ptr = CA_PK,
101 .ca_pk.len = CA_PK_LEN };
102 struct edhoc_responder_context c_r = { .suites_r.ptr = SUITES_R,
103 .suites_r.len = SUITES_R_LEN,
104 .g_y.ptr = G_Y,
105 .g_y.len = G_Y_LEN,
106 .y.ptr = Y,
107 .y.len = Y_LEN,
108 .c_r.ptr = C_R,
109 .c_r.len = C_R_LEN,
110 .ad_2.ptr = AD_2,
111 .ad_2.len = AD_2_LEN,
112 .id_cred_r.ptr = ID_CRED_R,
113 .id_cred_r.len = ID_CRED_R_LEN,
114 .cred_r.ptr = CRED_R,
115 .cred_r.len = CRED_R_LEN,
116 .sk_r.ptr = SK_R,
117 .sk_r.len = SK_R_LEN,
118 .pk_r.ptr = PK_R,
119 .pk_r.len = PK_R_LEN,
120 .r.ptr = R,
121 .r.len = R_LEN,
122 .g_r.ptr = G_R,
123 .g_r.len = G_R_LEN };
124 uint8_t ad_1[AD_DEFAULT_SIZE];
125 uint64_t ad_1_len = sizeof(ad_1);
126 uint8_t ad_3[AD_DEFAULT_SIZE];
127 uint64_t ad_3_len = sizeof(ad_3);
128
129 while (1) {
130 r = edhoc_responder_run(&c_r, &cred_i, 1, err_msg, &err_msg_len,
131 (uint8_t *)&ad_1, &ad_1_len,
132 (uint8_t *)&ad_3, &ad_3_len, PRK_4x3m,
133 sizeof(PRK_4x3m), th4, sizeof(th4));
134 if (r != ok) {
135 PRINTK("error responder run (Error Code %d)\n", r);
136 }
137
138 PRINTK("PRK_4x3m: (size: %d)\n", sizeof(PRK_4x3m));
139 for (int i = 0; i < sizeof(PRK_4x3m); i++) {
140 if (i % 16 == 0)
141 PRINTK("\n");
142 else if (i % 8 == 0)
143 PRINTK(" ");
144 PRINTK("%02hhX ", PRK_4x3m[i]);
145 }
146 PRINTK("\n\n");
147 PRINTK("th4: (size: %d)\n", sizeof(th4));
148 for (int i = 0; i < sizeof(th4); i++) {
149 if (i % 16 == 0)
150 PRINTK("\n");
151 else if (i % 8 == 0)
152 PRINTK(" ");
153 PRINTK("%02hhX ", th4[i]);
154 }
155 PRINTK("\n\n");
156
157 /* run edhoc exporter here to extract OSCORE secrets */
158 }
159 }
160
161 /* create thread for EDHOC */
162 K_THREAD_DEFINE(edhoc_thread, //name
163 4608, //stack_size
164 edhoc_responder_init, //entry_function
165 NULL, NULL, NULL, //parameter1,parameter2,parameter3
166 0, //priority
167 0, //options
168 0); //delay
169
170 /* CoAP socket fd */
171 static int sock;
172
173 static struct coap_observer observers[NUM_OBSERVERS];
174
175 static struct coap_pending pendings[NUM_PENDINGS];
176
177 static struct k_delayed_work observer_work;
178
179 static int obs_counter;
180
181 static struct coap_resource *resource_to_notify;
182
183 static struct k_delayed_work retransmit_work;
184
185 #if defined(CONFIG_NET_IPV6)
join_coap_multicast_group(void)186 static bool join_coap_multicast_group(void)
187 {
188 static struct in6_addr my_addr = MY_IP6ADDR;
189 static struct sockaddr_in6 mcast_addr = {
190 .sin6_family = AF_INET6,
191 .sin6_addr = ALL_NODES_LOCAL_COAP_MCAST,
192 .sin6_port = htons(MY_COAP_PORT)
193 };
194 struct net_if_mcast_addr *mcast;
195 struct net_if_addr *ifaddr;
196 struct net_if *iface;
197
198 iface = net_if_get_default();
199 if (!iface) {
200 LOG_ERR("Could not get te default interface\n");
201 return false;
202 }
203
204 #if defined(CONFIG_NET_CONFIG_SETTINGS)
205 if (net_addr_pton(AF_INET6, CONFIG_NET_CONFIG_MY_IPV6_ADDR, &my_addr) <
206 0) {
207 LOG_ERR("Invalid IPv6 address %s",
208 CONFIG_NET_CONFIG_MY_IPV6_ADDR);
209 }
210 #endif
211
212 ifaddr = net_if_ipv6_addr_add(iface, &my_addr, NET_ADDR_MANUAL, 0);
213 if (!ifaddr) {
214 LOG_ERR("Could not add unicast address to interface");
215 return false;
216 }
217
218 ifaddr->addr_state = NET_ADDR_PREFERRED;
219
220 mcast = net_if_ipv6_maddr_add(iface, &mcast_addr.sin6_addr);
221 if (!mcast) {
222 LOG_ERR("Could not add multicast address to interface\n");
223 return false;
224 }
225
226 return true;
227 }
228 #endif
229
230 #if defined(CONFIG_NET_IPV6)
231 /**
232 * @brief Initializes sockets for CoAP server (IPv6)
233 * @param
234 * @retval error code
235 */
start_coap_server(void)236 static int start_coap_server(void)
237 {
238 struct sockaddr_in6 addr6;
239 int r;
240
241 memset(&addr6, 0, sizeof(addr6));
242 addr6.sin6_family = AF_INET6;
243 addr6.sin6_port = htons(MY_COAP_PORT);
244
245 sock = socket(addr6.sin6_family, SOCK_DGRAM, IPPROTO_UDP);
246 if (sock < 0) {
247 LOG_ERR("Failed to create UDP socket %d", errno);
248 return -errno;
249 }
250
251 r = bind(sock, (struct sockaddr *)&addr6, sizeof(addr6));
252 if (r < 0) {
253 LOG_ERR("Failed to bind UDP socket %d", errno);
254 return -errno;
255 }
256
257 return 0;
258 }
259 #endif
260
261 #if defined(CONFIG_NET_IPV4)
262 /**
263 * @brief Initializes sockets for CoAP server (IPv4)
264 * @param
265 * @retval error code
266 */
start_coap_server(void)267 static int start_coap_server(void)
268 {
269 struct sockaddr_in addr;
270 int r;
271
272 memset(&addr, 0, sizeof(addr));
273 addr.sin_family = AF_INET;
274 addr.sin_port = htons(MY_COAP_PORT);
275 addr.sin_addr.s_addr = htonl(INADDR_ANY); //inet_addr("127.0.0.1");
276
277 sock = socket(addr.sin_family, SOCK_DGRAM, IPPROTO_UDP);
278 if (sock < 0) {
279 LOG_ERR("Failed to create UDP socket %d", errno);
280 return -errno;
281 }
282
283 r = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
284 if (r < 0) {
285 LOG_ERR("Failed to bind UDP socket %d", errno);
286 return -errno;
287 }
288
289 return 0;
290 }
291 #endif
292
293 /**
294 * @brief Receives and replies a UDP message in order to check the connection
295 * This function is required since it may happen that the router
296 * is not set up at the moment when we want to send a message
297 * @param sock the socket's fd
298 * @retval error code
299 */
check_router_connection(int sock)300 static int check_router_connection(int sock)
301 {
302 uint8_t data[30];
303 struct sockaddr client_addr;
304 socklen_t client_addr_len;
305 client_addr_len = sizeof(client_addr);
306
307 int rcvd = recvfrom(sock, data, sizeof(data), 0, &client_addr,
308 &client_addr_len);
309 if (rcvd >= 0) {
310 PRINTK("%s\n", data);
311 PRINTK("sending connection confirmation message...\n");
312 sendto(sock, &data, rcvd, 0, &client_addr, client_addr_len);
313 } else {
314 PRINTK("error (%d)\n", errno);
315 return -errno;
316 }
317
318 return 0;
319 }
320
321 /**
322 * @brief Sends CoAP reply packet over network
323 * @param cpkt CoAP packet to be sent
324 * @param addr client address (destination)
325 * @param addr_len length of client address
326 * @retval error code
327 */
send_coap_reply(struct coap_packet * cpkt,const struct sockaddr * addr,socklen_t addr_len)328 static int send_coap_reply(struct coap_packet *cpkt,
329 const struct sockaddr *addr, socklen_t addr_len)
330 {
331 int r;
332
333 net_hexdump("Response", cpkt->data, cpkt->offset);
334
335 GPIO_HIGH(LED1);
336 r = sendto(sock, cpkt->data, cpkt->offset, 0, addr, addr_len);
337 if (r < 0) {
338 LOG_ERR("Failed to send %d", errno);
339 r = -errno;
340 }
341 GPIO_LOW(LED1);
342
343 return r;
344 }
345
346 /**
347 * @brief Reveals available resources to client (see RFC7252 for details)
348 * @param resource requested resource(s)
349 * @param request CoAP client request packet
350 * @param addr client address (destination)
351 * @param addr_len length of client address
352 * @retval error code
353 */
well_known_core_get(struct coap_resource * resource,struct coap_packet * request,struct sockaddr * addr,socklen_t addr_len)354 static int well_known_core_get(struct coap_resource *resource,
355 struct coap_packet *request,
356 struct sockaddr *addr, socklen_t addr_len)
357 {
358 struct coap_packet response;
359 uint8_t *data;
360 int r;
361
362 data = (uint8_t *)k_malloc(MAX_COAP_MSG_LEN);
363 if (!data) {
364 return -ENOMEM;
365 }
366
367 r = coap_well_known_core_get(resource, request, &response, data,
368 MAX_COAP_MSG_LEN);
369 if (r < 0) {
370 goto end;
371 }
372
373 r = send_coap_reply(&response, addr, addr_len);
374
375 end:
376 k_free(data);
377
378 return r;
379 }
380
381 /**
382 * @brief Callback for EDHOC Post request. Passes request payload to EDHOC thread
383 * and waits for EDHOC response message.
384 * @param resource requested resource
385 * @param request CoAP client request packet
386 * @param addr client address (destination)
387 * @param addr_len length of client address
388 * @retval error code
389 */
edhoc_post(struct coap_resource * resource,struct coap_packet * request,struct sockaddr * addr,socklen_t addr_len)390 static int edhoc_post(struct coap_resource *resource,
391 struct coap_packet *request, struct sockaddr *addr,
392 socklen_t addr_len)
393 {
394 struct coap_packet response;
395 uint8_t token[8];
396 const uint8_t *payload;
397 uint8_t *data;
398 uint16_t payload_len;
399 uint8_t code;
400 uint8_t type;
401 uint8_t tkl;
402 uint16_t id;
403 int r;
404 struct k_mbox_msg recv_msg; //edhoc post payload
405 struct k_mbox_msg send_msg; //edhoc response
406 char buffer[MBOX_MSG_SIZE];
407 int buffer_bytes_used;
408 uint8_t finished_indicator[] = { "EDHOC successful" };
409 static int msg_count = 1;
410
411 code = coap_header_get_code(request);
412 type = coap_header_get_type(request);
413 id = coap_header_get_id(request);
414 tkl = coap_header_get_token(request, token);
415
416 LOG_INF("*******");
417 LOG_INF("type: %u code %u id %u", type, code, id);
418 LOG_INF("*******");
419
420 payload = coap_packet_get_payload(request, &payload_len);
421 if (payload) {
422 net_hexdump("POST Payload", payload, payload_len);
423 }
424
425 /* prepare message box packet to send to edhoc thread */
426 buffer_bytes_used = payload_len;
427 memcpy(buffer, payload, buffer_bytes_used);
428 recv_msg.info = buffer_bytes_used;
429 recv_msg.size = buffer_bytes_used;
430 recv_msg.tx_data = buffer;
431 recv_msg.tx_block.data = NULL;
432 recv_msg.tx_target_thread = K_ANY;
433 k_mbox_put(&rx_queue, &recv_msg, K_FOREVER);
434 if (recv_msg.size < buffer_bytes_used) {
435 PRINTK("some message data dropped during transfer!");
436 PRINTK("receiver only had room for %d bytes", send_msg.info);
437 }
438
439 /* wait for data from edhoc_thread */
440 send_msg.info = MBOX_MSG_SIZE;
441 send_msg.size = MBOX_MSG_SIZE;
442 send_msg.rx_source_thread = K_ANY;
443 k_mbox_get(&tx_queue, &send_msg, buffer, K_SECONDS(MBOX_WAIT_TIME));
444 if (send_msg.info != send_msg.size) {
445 PRINTK("some message data dropped during transfer!");
446 PRINTK("sender tried to send %d bytes", send_msg.info);
447 }
448
449 if (type == COAP_TYPE_CON) {
450 type = COAP_TYPE_ACK;
451 } else {
452 type = COAP_TYPE_NON_CON;
453 }
454
455 data = (uint8_t *)k_malloc(MAX_COAP_MSG_LEN);
456 if (!data) {
457 return -ENOMEM;
458 }
459
460 r = coap_packet_init(&response, data, MAX_COAP_MSG_LEN, 1, type, tkl,
461 (uint8_t *)token, COAP_RESPONSE_CODE_CHANGED, id);
462 if (r < 0) {
463 goto end;
464 }
465
466 /* check if msg2 or edhoc finished */
467 if (memcmp(buffer, finished_indicator, sizeof(finished_indicator)) !=
468 0) {
469 r = coap_packet_append_payload_marker(&response);
470 if (r < 0) {
471 goto end;
472 }
473
474 r = coap_packet_append_payload(&response, buffer,
475 send_msg.size);
476 if (r < 0) {
477 goto end;
478 }
479 msg_count++;
480 } else {
481 msg_count = 1;
482 }
483
484 r = send_coap_reply(&response, addr, addr_len);
485
486 end:
487 k_free(data);
488
489 return r;
490 }
491
retransmit_request(struct k_work * work)492 static void retransmit_request(struct k_work *work)
493 {
494 struct coap_pending *pending;
495
496 pending = coap_pending_next_to_expire(pendings, NUM_PENDINGS);
497 if (!pending) {
498 return;
499 }
500
501 if (!coap_pending_cycle(pending)) {
502 k_free(pending->data);
503 coap_pending_clear(pending);
504 return;
505 }
506
507 k_delayed_work_submit(&retransmit_work, K_MSEC(pending->timeout));
508 }
509
update_counter(struct k_work * work)510 static void update_counter(struct k_work *work)
511 {
512 obs_counter++;
513
514 if (resource_to_notify) {
515 coap_resource_notify(resource_to_notify);
516 }
517
518 k_delayed_work_submit(&observer_work, K_SECONDS(5));
519 }
520
521 static const char *const edhoc_path[] = { ".well-known", "edhoc", NULL };
522
523 static struct coap_resource resources[] = {
524 {
525 .get = well_known_core_get,
526 .path = COAP_WELL_KNOWN_CORE_PATH,
527 },
528 {
529 .post = edhoc_post,
530 .path = edhoc_path,
531 },
532 {},
533 };
534
535 static struct coap_resource *
find_resource_by_observer(struct coap_resource * resources,struct coap_observer * o)536 find_resource_by_observer(struct coap_resource *resources,
537 struct coap_observer *o)
538 {
539 struct coap_resource *r;
540
541 for (r = resources; r && r->path; r++) {
542 sys_snode_t *node;
543
544 SYS_SLIST_FOR_EACH_NODE (&r->observers, node) {
545 if (&o->list == node) {
546 return r;
547 }
548 }
549 }
550
551 return NULL;
552 }
553
554 /**
555 * @brief Parses CoAP client request and calls handler of requested resource.
556 * @param data received CoAP client request
557 * @param data_len length of received CoAP client request
558 * @param addr client address (destination)
559 * @param addr_len length of client address
560 * @retval error code
561 */
process_coap_request(uint8_t * data,uint16_t data_len,struct sockaddr * client_addr,socklen_t client_addr_len)562 static void process_coap_request(uint8_t *data, uint16_t data_len,
563 struct sockaddr *client_addr,
564 socklen_t client_addr_len)
565 {
566 struct coap_packet request;
567 struct coap_pending *pending;
568 struct coap_option options[16] = { 0 };
569 uint8_t opt_num = 16U;
570 uint8_t type;
571 int r;
572
573 r = coap_packet_parse(&request, data, data_len, options, opt_num);
574 if (r < 0) {
575 LOG_ERR("Invalid data received (%d)\n", r);
576 }
577
578 type = coap_header_get_type(&request);
579
580 pending = coap_pending_received(&request, pendings, NUM_PENDINGS);
581 if (!pending) {
582 goto not_found;
583 }
584
585 /* Clear CoAP pending request */
586 if (type == COAP_TYPE_ACK) {
587 k_free(pending->data);
588 coap_pending_clear(pending);
589 }
590
591 return;
592
593 not_found:
594
595 if (type == COAP_TYPE_RESET) {
596 struct coap_resource *r;
597 struct coap_observer *o;
598
599 o = coap_find_observer_by_addr(observers, NUM_OBSERVERS,
600 client_addr);
601 if (!o) {
602 LOG_ERR("Observer not found\n");
603 goto end;
604 }
605
606 r = find_resource_by_observer(resources, o);
607 if (!r) {
608 LOG_ERR("Observer found but Resource not found\n");
609 goto end;
610 }
611
612 coap_remove_observer(r, o);
613
614 return;
615 }
616
617 end:
618
619 r = coap_handle_request(&request, resources, options, opt_num,
620 client_addr, client_addr_len);
621 if (r < 0) {
622 LOG_WRN("No handler for such request (%d)\n", r);
623 }
624 }
625
626 /**
627 * @brief Waits for CoAP client request and passes received data to process_coap_request
628 * @param
629 * @retval error code
630 */
process_client_request(void)631 static int process_client_request(void)
632 {
633 int received;
634 struct sockaddr client_addr;
635 socklen_t client_addr_len;
636 uint8_t request[MAX_COAP_MSG_LEN];
637
638 do {
639 client_addr_len = sizeof(client_addr);
640 GPIO_HIGH(LED1);
641 received = recvfrom(sock, request, sizeof(request), 0,
642 &client_addr, &client_addr_len);
643 GPIO_LOW(LED1);
644 if (received < 0) {
645 LOG_ERR("Connection error %d", errno);
646 return -errno;
647 }
648
649 process_coap_request(request, received, &client_addr,
650 client_addr_len);
651 } while (true);
652
653 return 0;
654 }
655
main(void)656 void main(void)
657 {
658 int r;
659 setbuf(stdout, NULL); //disable printf buffereing
660
661 LOG_DBG("Start CoAP-server sample");
662
663 #ifdef CONFIG_NET_IPV6
664 bool res;
665
666 res = join_coap_multicast_group();
667 if (!res) {
668 goto quit;
669 }
670 #endif
671
672 LED_INIT();
673 GPIO_LOW(LED0);
674 GPIO_LOW(LED1);
675
676 r = start_coap_server();
677 if (r < 0) {
678 goto quit;
679 }
680
681 r = check_router_connection(sock);
682 if (r < 0) {
683 goto quit;
684 }
685
686 k_delayed_work_init(&retransmit_work, retransmit_request);
687 k_delayed_work_init(&observer_work, update_counter);
688
689 while (1) {
690 r = process_client_request();
691 if (r < 0) {
692 goto quit;
693 }
694 }
695
696 LOG_DBG("Done");
697 return;
698
699 quit:
700 LOG_ERR("Quit");
701 }
702
