1 /* 2 * Copyright (c) 2026, Arm Limited. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 * 6 */ 7 8 #include <assert.h> 9 #include <string.h> 10 11 #include <drivers/arm/sfcp.h> 12 #include "sfcp_defs.h" 13 #include "sfcp_encryption.h" 14 #include "sfcp_handler_buffer.h" 15 #include "sfcp_helpers.h" 16 #include "sfcp_link_hal.h" 17 #include "sfcp_protocol_error.h" 18 #include "sfcp_random.h" 19 20 #include <platform_def.h> 21 22 #ifndef SFCP_MAX_NUMBER_MESSAGE_HANDLERS 23 #define SFCP_MAX_NUMBER_MESSAGE_HANDLERS (2) 24 #endif 25 26 #ifndef SFCP_MAX_NUMBER_REPLY_HANDLERS 27 #define SFCP_MAX_NUMBER_REPLY_HANDLERS (2) 28 #endif 29 30 #define BUS_ARBITRATION_RANDOM_DELAY_MAX_CYCLES (10000) 31 32 struct sfcp_handler_table_entry_t { 33 sfcp_handler_t handler; 34 union { 35 uint16_t application_id; 36 uint16_t client_id; 37 }; 38 bool in_use; 39 }; 40 41 static struct sfcp_handler_table_entry_t 42 sfcp_msg_handlers[SFCP_MAX_NUMBER_MESSAGE_HANDLERS]; 43 static struct sfcp_handler_table_entry_t 44 sfcp_reply_handlers[SFCP_MAX_NUMBER_REPLY_HANDLERS]; 45 46 struct sfcp_requires_handshake_t { 47 bool requires_handshake; 48 uint8_t trusted_subnet_id; 49 }; 50 51 static struct sfcp_requires_handshake_t sfcp_requires_handshake; 52 53 static inline enum sfcp_error_t 54 sfcp_protocol_error_to_sfcp_error(enum sfcp_protocol_error_t protocol_error) 55 { 56 switch (protocol_error) { 57 case SFCP_PROTOCOL_ERROR_TRY_AGAIN_LATER: 58 return SFCP_ERROR_SEND_MSG_AGAIN; 59 default: 60 return SFCP_ERROR_PROTOCOL_ERROR; 61 } 62 } 63 64 static inline void 65 populate_reply_metadata(struct sfcp_reply_metadata_t *metadata, 66 sfcp_node_id_t receiver, bool uses_cryptography, 67 uint16_t client_id, uint16_t application_id, 68 uint8_t message_id, uint8_t trusted_subnet_id) 69 { 70 metadata->receiver = receiver; 71 metadata->uses_cryptography = uses_cryptography; 72 metadata->trusted_subnet_id = trusted_subnet_id; 73 metadata->client_id = client_id; 74 metadata->application_id = application_id; 75 metadata->message_id = message_id; 76 } 77 78 static inline void 79 populate_msg_metadata(struct sfcp_msg_metadata_t *metadata, 80 sfcp_node_id_t sender, bool uses_cryptography, 81 uint16_t client_id, uint16_t application_id, 82 uint8_t message_id, uint8_t trusted_subnet_id) 83 { 84 metadata->sender = sender; 85 metadata->uses_cryptography = uses_cryptography; 86 metadata->trusted_subnet_id = trusted_subnet_id; 87 metadata->client_id = client_id; 88 metadata->application_id = application_id; 89 metadata->message_id = message_id; 90 } 91 92 enum sfcp_error_t sfcp_init(void) 93 { 94 enum sfcp_hal_error_t hal_error; 95 enum sfcp_error_t sfcp_err; 96 97 hal_error = sfcp_hal_init(); 98 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 99 return sfcp_hal_error_to_sfcp_error(hal_error); 100 } 101 102 sfcp_err = sfcp_trusted_subnet_state_init(); 103 if (sfcp_err != SFCP_ERROR_SUCCESS) { 104 return sfcp_err; 105 } 106 107 return SFCP_ERROR_SUCCESS; 108 } 109 110 static uint8_t get_new_message_id(sfcp_node_id_t node) 111 { 112 static uint8_t message_ids_per_node[SFCP_NUMBER_NODES]; 113 114 return message_ids_per_node[node]++; 115 } 116 117 enum sfcp_error_t sfcp_init_msg(uint8_t *buf, size_t buf_size, 118 sfcp_node_id_t receiver, 119 uint16_t application_id, uint16_t client_id, 120 bool needs_reply, bool manually_specify_ts_id, 121 uint8_t trusted_subnet_id, uint8_t **payload, 122 size_t *payload_len, struct sfcp_packet_t **msg, 123 size_t *msg_size, 124 struct sfcp_reply_metadata_t *metadata) 125 { 126 enum sfcp_error_t sfcp_err; 127 bool uses_id_extension; 128 struct sfcp_packet_t *msg_ptr; 129 sfcp_node_id_t my_node_id; 130 enum sfcp_hal_error_t hal_error; 131 uint8_t message_id; 132 struct sfcp_trusted_subnet_config_t *trusted_subnet; 133 bool found_trusted_subnet = false; 134 bool uses_cryptography; 135 136 if ((buf == NULL) || (payload == NULL) || (payload_len == NULL) || 137 (msg == NULL) || (msg_size == NULL) || (metadata == NULL)) { 138 return SFCP_ERROR_INVALID_POINTER; 139 } 140 141 uses_id_extension = (application_id != 0) || (client_id != 0); 142 143 if (manually_specify_ts_id) { 144 sfcp_err = sfcp_get_trusted_subnet_by_id(trusted_subnet_id, 145 &trusted_subnet); 146 if (sfcp_err == SFCP_ERROR_SUCCESS) { 147 found_trusted_subnet = true; 148 } else { 149 return sfcp_err; 150 } 151 } else { 152 if (trusted_subnet_id != 0) { 153 return SFCP_ERROR_INVALID_TRUSTED_SUBNET_ID; 154 } 155 156 sfcp_err = sfcp_get_trusted_subnet_for_node(receiver, 157 &trusted_subnet); 158 if (sfcp_err == SFCP_ERROR_SUCCESS) { 159 found_trusted_subnet = true; 160 } 161 } 162 163 if (found_trusted_subnet) { 164 sfcp_err = 165 sfcp_trusted_subnet_state_requires_handshake_encryption( 166 trusted_subnet->id, 167 &sfcp_requires_handshake.requires_handshake, 168 &uses_cryptography); 169 if (sfcp_err != SFCP_ERROR_SUCCESS) { 170 return sfcp_err; 171 } 172 173 if (sfcp_requires_handshake.requires_handshake) { 174 sfcp_requires_handshake.trusted_subnet_id = 175 trusted_subnet->id; 176 } 177 } else { 178 uses_cryptography = false; 179 } 180 181 if (buf_size < SFCP_PACKET_SIZE_WITHOUT_PAYLOAD(uses_cryptography, 182 uses_id_extension)) { 183 return SFCP_ERROR_BUFFER_TOO_SMALL; 184 } 185 186 hal_error = sfcp_hal_get_my_node_id(&my_node_id); 187 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 188 return sfcp_hal_error_to_sfcp_error(hal_error); 189 } 190 191 if ((receiver >= SFCP_NUMBER_NODES) || (receiver == my_node_id)) { 192 return SFCP_ERROR_INVALID_NODE; 193 } 194 195 msg_ptr = (struct sfcp_packet_t *)buf; 196 message_id = get_new_message_id(receiver); 197 198 msg_ptr->header.metadata = SET_ALL_METADATA_FIELDS( 199 needs_reply ? SFCP_PACKET_TYPE_MSG_NEEDS_REPLY : 200 SFCP_PACKET_TYPE_MSG_NO_REPLY, 201 uses_cryptography, uses_id_extension, SFCP_PROTOCOL_VERSION); 202 msg_ptr->header.receiver_id = receiver; 203 msg_ptr->header.sender_id = my_node_id; 204 msg_ptr->header.message_id = message_id; 205 206 if (uses_id_extension) { 207 GET_SFCP_CLIENT_ID(msg_ptr, uses_cryptography) = client_id; 208 GET_SFCP_APPLICATION_ID(msg_ptr, uses_cryptography) = 209 application_id; 210 } 211 212 if (uses_cryptography) { 213 struct sfcp_cryptography_metadata_t *crypto_metadata = 214 &msg_ptr->cryptography_used.cryptography_metadata; 215 216 crypto_metadata->config.trusted_subnet_id = trusted_subnet->id; 217 } 218 219 *payload = (uint8_t *)GET_SFCP_PAYLOAD_PTR(msg_ptr, uses_cryptography, 220 uses_id_extension); 221 *payload_len = buf_size - SFCP_PACKET_SIZE_WITHOUT_PAYLOAD( 222 uses_cryptography, uses_id_extension); 223 224 *msg = msg_ptr; 225 *msg_size = buf_size; 226 227 populate_reply_metadata(metadata, receiver, uses_cryptography, 228 client_id, application_id, message_id, 229 trusted_subnet_id); 230 231 return SFCP_ERROR_SUCCESS; 232 } 233 234 static enum sfcp_error_t __send_msg_reply(sfcp_node_id_t remote_node, 235 sfcp_link_id_t link_id, 236 struct sfcp_packet_t *packet, 237 size_t packet_size, bool is_msg) 238 { 239 enum sfcp_hal_error_t hal_error; 240 volatile uint64_t lfsr_random; 241 242 hal_error = sfcp_hal_send_message(link_id, (const uint8_t *)packet, 243 packet_size); 244 if (hal_error != SFCP_HAL_ERROR_SEND_MESSAGE_BUS_BUSY) { 245 /* Success or error we cannot handle 246 * here 247 */ 248 return sfcp_hal_error_to_sfcp_error(hal_error); 249 } 250 251 /* Wait arbitrary amount of time 252 * before returning to ensure bus arbitration. 253 * Seed the LFSR with the packet 254 */ 255 lfsr_random = sfcp_random_generate_random_lfsr((uint8_t *)packet, 256 packet_size) % 257 BUS_ARBITRATION_RANDOM_DELAY_MAX_CYCLES; 258 259 while (lfsr_random > 0) { 260 lfsr_random--; 261 } 262 263 /* Indicate to the caller that a message is 264 * waiting, we expect the caller to receive the 265 * pending message or wait before trying to 266 * send their message again 267 */ 268 return SFCP_ERROR_SEND_MSG_BUS_BUSY; 269 } 270 271 static enum sfcp_error_t send_msg_reply(struct sfcp_packet_t *packet, 272 size_t packet_size, size_t payload_size, 273 bool is_msg) 274 { 275 enum sfcp_error_t sfcp_err; 276 bool uses_cryptography, uses_id_extension; 277 sfcp_link_id_t link_id; 278 size_t packet_transfer_size; 279 sfcp_node_id_t remote_node; 280 281 if (packet == NULL) { 282 return SFCP_ERROR_INVALID_PACKET; 283 } 284 285 if (packet_size < sizeof(struct sfcp_header_t)) { 286 return SFCP_ERROR_MESSAGE_TOO_SMALL; 287 } 288 289 uses_cryptography = 290 GET_METADATA_FIELD(USES_CRYPTOGRAPHY, packet->header.metadata); 291 uses_id_extension = 292 GET_METADATA_FIELD(USES_ID_EXTENSION, packet->header.metadata); 293 remote_node = is_msg ? packet->header.receiver_id : 294 packet->header.sender_id; 295 296 if (packet_size < SFCP_PACKET_SIZE_WITHOUT_PAYLOAD(uses_cryptography, 297 uses_id_extension)) { 298 return SFCP_ERROR_MESSAGE_TOO_SMALL; 299 } 300 301 if (payload_size > 302 (packet_size - SFCP_PACKET_SIZE_WITHOUT_PAYLOAD( 303 uses_cryptography, uses_id_extension))) { 304 return SFCP_ERROR_PAYLOAD_TOO_LARGE; 305 } 306 307 packet_transfer_size = SFCP_PACKET_SIZE_WITHOUT_PAYLOAD( 308 uses_cryptography, uses_id_extension) + 309 payload_size; 310 311 if (uses_cryptography) { 312 if (is_msg) { 313 sfcp_err = sfcp_encrypt_msg( 314 packet, packet_transfer_size, 315 packet->cryptography_used.cryptography_metadata 316 .config.trusted_subnet_id, 317 remote_node); 318 if (sfcp_err != SFCP_ERROR_SUCCESS) { 319 return sfcp_err; 320 } 321 } else { 322 sfcp_err = sfcp_encrypt_reply( 323 packet, packet_transfer_size, 324 packet->cryptography_used.cryptography_metadata 325 .config.trusted_subnet_id, 326 remote_node); 327 if (sfcp_err != SFCP_ERROR_SUCCESS) { 328 return sfcp_err; 329 } 330 } 331 } 332 333 link_id = sfcp_hal_get_route(remote_node); 334 if (link_id == 0) { 335 return SFCP_ERROR_INVALID_NODE; 336 } 337 338 #ifdef SFCP_SUPPORT_LEGACY_MSG_PROTOCOL 339 if (is_msg) { 340 sfcp_err = sfcp_convert_to_legacy_msg( 341 (uint8_t *)packet, packet_transfer_size, 342 sfcp_legacy_conversion_buffer, 343 sizeof(sfcp_legacy_conversion_buffer), 344 &packet_transfer_size); 345 if (sfcp_err != SFCP_ERROR_SUCCESS) { 346 return sfcp_err; 347 } 348 } else { 349 sfcp_err = sfcp_convert_to_legacy_reply( 350 (uint8_t *)packet, packet_transfer_size, 351 sfcp_legacy_conversion_buffer, 352 sizeof(sfcp_legacy_conversion_buffer), 353 &packet_transfer_size); 354 if (sfcp_err != SFCP_ERROR_SUCCESS) { 355 return sfcp_err; 356 } 357 } 358 359 packet = (struct sfcp_packet_t *)sfcp_legacy_conversion_buffer; 360 #endif 361 362 return __send_msg_reply(remote_node, link_id, packet, 363 packet_transfer_size, is_msg); 364 } 365 366 enum sfcp_error_t sfcp_send_msg(struct sfcp_packet_t *msg, size_t msg_size, 367 size_t payload_size) 368 { 369 enum sfcp_error_t sfcp_err; 370 371 if (sfcp_requires_handshake.requires_handshake) { 372 sfcp_err = sfcp_encryption_handshake_initiator( 373 sfcp_requires_handshake.trusted_subnet_id, true); 374 if (sfcp_err != SFCP_ERROR_SUCCESS) { 375 return sfcp_err; 376 } 377 } 378 379 return send_msg_reply(msg, msg_size, payload_size, true); 380 } 381 382 enum sfcp_error_t sfcp_init_reply(uint8_t *buf, size_t buf_size, 383 struct sfcp_msg_metadata_t metadata, 384 uint8_t **payload, size_t *payload_len, 385 struct sfcp_packet_t **reply, 386 size_t *reply_size) 387 { 388 enum sfcp_error_t sfcp_err; 389 struct sfcp_packet_t *reply_ptr; 390 bool uses_id_extension, uses_cryptography; 391 enum sfcp_hal_error_t hal_error; 392 sfcp_node_id_t my_node_id; 393 struct sfcp_trusted_subnet_config_t *trusted_subnet; 394 395 if ((buf == NULL) || (payload == NULL) || (payload_len == NULL) || 396 (reply == NULL) || (reply_size == NULL)) { 397 return SFCP_ERROR_INVALID_POINTER; 398 } 399 400 uses_id_extension = (metadata.client_id != 0) || 401 (metadata.application_id != 0); 402 uses_cryptography = metadata.uses_cryptography; 403 404 if (buf_size < SFCP_PACKET_SIZE_WITHOUT_PAYLOAD(uses_cryptography, 405 uses_id_extension)) { 406 return SFCP_ERROR_BUFFER_TOO_SMALL; 407 } 408 409 if (uses_cryptography) { 410 sfcp_err = sfcp_get_trusted_subnet_by_id( 411 metadata.trusted_subnet_id, &trusted_subnet); 412 if (sfcp_err != SFCP_ERROR_SUCCESS) { 413 return sfcp_err; 414 } 415 } 416 417 hal_error = sfcp_hal_get_my_node_id(&my_node_id); 418 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 419 return sfcp_hal_error_to_sfcp_error(hal_error); 420 } 421 422 reply_ptr = (struct sfcp_packet_t *)buf; 423 424 reply_ptr->header.metadata = SET_ALL_METADATA_FIELDS( 425 SFCP_PACKET_TYPE_REPLY, uses_cryptography, uses_id_extension, 426 SFCP_PROTOCOL_VERSION); 427 reply_ptr->header.receiver_id = my_node_id; 428 reply_ptr->header.sender_id = metadata.sender; 429 reply_ptr->header.message_id = metadata.message_id; 430 431 if (uses_id_extension) { 432 GET_SFCP_CLIENT_ID(reply_ptr, uses_cryptography) = 433 metadata.client_id; 434 GET_SFCP_APPLICATION_ID(reply_ptr, uses_cryptography) = 435 metadata.application_id; 436 } 437 438 if (uses_cryptography) { 439 struct sfcp_cryptography_metadata_t *crypto_metadata = 440 &reply_ptr->cryptography_used.cryptography_metadata; 441 442 crypto_metadata->config.trusted_subnet_id = trusted_subnet->id; 443 } 444 445 *payload = (uint8_t *)GET_SFCP_PAYLOAD_PTR(reply_ptr, uses_cryptography, 446 uses_id_extension); 447 *payload_len = buf_size - SFCP_PACKET_SIZE_WITHOUT_PAYLOAD( 448 uses_cryptography, uses_id_extension); 449 450 *reply = reply_ptr; 451 *reply_size = buf_size; 452 453 return SFCP_ERROR_SUCCESS; 454 } 455 456 enum sfcp_error_t sfcp_send_reply(struct sfcp_packet_t *reply, 457 size_t reply_size, size_t payload_size) 458 { 459 return send_msg_reply(reply, reply_size, payload_size, false); 460 } 461 462 static enum sfcp_error_t 463 send_protocol_error(sfcp_node_id_t sender_id, sfcp_node_id_t receiver_id, 464 sfcp_link_id_t link_id, uint16_t client_id, 465 uint8_t message_id, enum sfcp_protocol_error_t error) 466 { 467 struct sfcp_packet_t packet; 468 enum sfcp_hal_error_t hal_error; 469 470 sfcp_helpers_generate_protocol_error_packet(&packet, sender_id, 471 receiver_id, link_id, 472 client_id, message_id, 473 error); 474 475 hal_error = sfcp_hal_send_message(link_id, (const uint8_t *)&packet, 476 SFCP_PACKET_SIZE_ERROR_REPLY); 477 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 478 return sfcp_hal_error_to_sfcp_error(hal_error); 479 } 480 481 return SFCP_ERROR_SUCCESS; 482 } 483 484 static inline enum sfcp_error_t sfcp_not_available_error(bool is_msg) 485 { 486 return is_msg ? SFCP_ERROR_NO_MSG_AVAILABLE : 487 SFCP_ERROR_NO_REPLY_AVAILABLE; 488 } 489 490 static enum sfcp_error_t 491 receive_msg_reply_from_node(uint8_t *buf, size_t buf_size, 492 sfcp_node_id_t remote_id, sfcp_link_id_t *link_id, 493 bool is_msg, size_t *received_size) 494 { 495 enum sfcp_hal_error_t hal_error; 496 bool is_available; 497 498 *link_id = sfcp_hal_get_route(remote_id); 499 if (*link_id == 0) { 500 return SFCP_ERROR_INVALID_NODE; 501 } 502 503 hal_error = sfcp_hal_is_message_available(*link_id, &is_available); 504 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 505 return sfcp_hal_error_to_sfcp_error(hal_error); 506 } 507 508 if (!is_available) { 509 return sfcp_not_available_error(is_msg); 510 } 511 512 hal_error = sfcp_hal_get_receive_message_size(*link_id, received_size); 513 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 514 return sfcp_hal_error_to_sfcp_error(hal_error); 515 } 516 517 if (*received_size > buf_size) { 518 return SFCP_ERROR_BUFFER_TOO_SMALL; 519 } 520 521 hal_error = sfcp_hal_receive_message(*link_id, buf, *received_size, 0, 522 *received_size); 523 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 524 return sfcp_hal_error_to_sfcp_error(hal_error); 525 } 526 527 return SFCP_ERROR_SUCCESS; 528 } 529 530 static enum sfcp_error_t 531 receive_msg_reply(uint8_t *buf, size_t buf_size, bool any_remote_id, 532 sfcp_node_id_t remote_id, sfcp_node_id_t *received_remote_id, 533 sfcp_link_id_t *link_id, sfcp_link_id_t *my_node_id, 534 bool is_msg, size_t *received_size) 535 { 536 enum sfcp_hal_error_t hal_error; 537 enum sfcp_error_t sfcp_err; 538 sfcp_node_id_t start_node, end_node; 539 540 /* We do not know if the remote will use the ID extension so ensure the 541 * buffer is large enough with it enabled 542 */ 543 if (buf_size < SFCP_PACKET_SIZE_WITHOUT_PAYLOAD(true, true)) { 544 return SFCP_ERROR_BUFFER_TOO_SMALL; 545 } 546 547 hal_error = sfcp_hal_get_my_node_id(my_node_id); 548 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 549 return sfcp_hal_error_to_sfcp_error(hal_error); 550 } 551 552 if (!any_remote_id && (remote_id == *my_node_id)) { 553 return SFCP_ERROR_INVALID_NODE; 554 } 555 556 if (any_remote_id) { 557 start_node = 0; 558 end_node = SFCP_NUMBER_NODES - 1; 559 } else { 560 start_node = remote_id; 561 end_node = remote_id; 562 } 563 564 for (sfcp_node_id_t node = start_node; node <= end_node; node++) { 565 if (node == *my_node_id) { 566 continue; 567 } 568 569 sfcp_err = receive_msg_reply_from_node( 570 buf, buf_size, node, link_id, is_msg, received_size); 571 if (sfcp_err == SFCP_ERROR_SUCCESS) { 572 *received_remote_id = node; 573 return SFCP_ERROR_SUCCESS; 574 } else if (sfcp_err != sfcp_not_available_error(is_msg)) { 575 return sfcp_err; 576 } 577 } 578 579 return sfcp_not_available_error(is_msg); 580 } 581 582 enum sfcp_error_t sfcp_receive_msg(uint8_t *buf, size_t buf_size, 583 bool any_sender, sfcp_node_id_t sender, 584 uint16_t application_id, uint16_t *client_id, 585 uint8_t **payload, size_t *payload_len, 586 struct sfcp_msg_metadata_t *metadata) 587 { 588 struct sfcp_packet_t *packet; 589 enum sfcp_error_t sfcp_err; 590 enum sfcp_protocol_error_t protocol_error; 591 enum sfcp_packet_type_t packet_type; 592 sfcp_link_id_t link_id; 593 sfcp_node_id_t my_node_id; 594 bool needs_reply; 595 bool uses_id_extension; 596 size_t received_size; 597 bool packet_uses_crypto; 598 uint16_t packet_application_id; 599 uint8_t message_id; 600 sfcp_node_id_t packet_sender; 601 sfcp_node_id_t packet_receiver; 602 sfcp_node_id_t forwarding_destination; 603 sfcp_node_id_t received_sender_id; 604 bool is_handshake_req; 605 606 if ((buf == NULL) || (client_id == NULL) || (payload == NULL) || 607 (payload_len == NULL) || (metadata == NULL)) { 608 return SFCP_ERROR_INVALID_POINTER; 609 } 610 611 if (any_sender && (sender != 0)) { 612 return SFCP_ERROR_INVALID_SENDER_ID; 613 } 614 615 sfcp_err = receive_msg_reply(buf, buf_size, any_sender, sender, 616 &received_sender_id, &link_id, &my_node_id, 617 true, &received_size); 618 if (sfcp_err != SFCP_ERROR_SUCCESS) { 619 return sfcp_err; 620 } 621 622 packet = (struct sfcp_packet_t *)buf; 623 624 sfcp_err = sfcp_helpers_parse_packet( 625 packet, received_size, &packet_sender, &packet_receiver, 626 &message_id, &packet_uses_crypto, &uses_id_extension, 627 &packet_application_id, client_id, payload, payload_len, 628 &needs_reply, &packet_type); 629 if (sfcp_err != SFCP_ERROR_SUCCESS) { 630 /* Do not know enough about this packet to reply */ 631 return sfcp_err; 632 } 633 634 if (sfcp_helpers_packet_requires_forwarding_get_destination( 635 packet_sender, packet_receiver, packet_type, my_node_id, 636 &forwarding_destination)) { 637 protocol_error = SFCP_PROTOCOL_FORWARDING_UNSUPPORTED; 638 sfcp_err = SFCP_ERROR_NO_MSG_AVAILABLE; 639 goto error_reply; 640 } 641 642 sfcp_err = sfcp_encryption_handshake_responder( 643 packet, received_size, 644 packet_type == SFCP_PACKET_TYPE_REPLY ? packet_receiver : 645 packet_sender, 646 message_id, packet_uses_crypto, *payload, *payload_len, 647 &is_handshake_req); 648 if (sfcp_err != SFCP_ERROR_SUCCESS) { 649 protocol_error = SFCP_PROTOCOL_ERROR_HANDSHAKE_FAILED; 650 goto error_reply; 651 } 652 653 if (is_handshake_req) { 654 /* Handshake message has been successfully handled, nothing else to do */ 655 return SFCP_ERROR_NO_MSG_AVAILABLE; 656 } 657 658 if ((packet_type != SFCP_PACKET_TYPE_MSG_NEEDS_REPLY) && 659 (packet_type != SFCP_PACKET_TYPE_MSG_NO_REPLY)) { 660 protocol_error = SFCP_PROTOCOL_ERROR_INVALID_CONTEXT; 661 sfcp_err = SFCP_ERROR_NO_MSG_AVAILABLE; 662 goto error_reply; 663 } 664 665 if ((packet_sender != received_sender_id) || 666 (packet_receiver != my_node_id)) { 667 protocol_error = SFCP_PROTOCOL_ERROR_UNSUPPORTED; 668 sfcp_err = SFCP_ERROR_INVALID_NODE; 669 goto error_reply; 670 } 671 672 if (!uses_id_extension && (application_id != 0)) { 673 protocol_error = SFCP_PROTOCOL_ERROR_UNSUPPORTED; 674 sfcp_err = SFCP_ERROR_INVALID_APPLICATION_ID; 675 goto error_reply; 676 } 677 678 if (uses_id_extension && (packet_application_id != application_id)) { 679 /* This message is not for us so we have to drop and get the remote to 680 * send it again later 681 */ 682 protocol_error = SFCP_PROTOCOL_ERROR_TRY_AGAIN_LATER; 683 sfcp_err = SFCP_ERROR_NO_MSG_AVAILABLE; 684 goto error_reply; 685 } 686 687 if (packet_uses_crypto) { 688 sfcp_err = sfcp_decrypt_msg(packet, received_size, 689 received_sender_id); 690 if (sfcp_err != SFCP_ERROR_SUCCESS) { 691 protocol_error = SFCP_PROTOCOL_ERROR_DECRYPTION_FAILED; 692 goto error_reply; 693 } 694 } 695 696 populate_msg_metadata( 697 metadata, received_sender_id, packet_uses_crypto, *client_id, 698 application_id, message_id, 699 packet_uses_crypto ? 700 packet->cryptography_used.cryptography_metadata.config 701 .trusted_subnet_id : 702 0); 703 704 return SFCP_ERROR_SUCCESS; 705 706 error_reply: 707 if (needs_reply) { 708 enum sfcp_error_t send_reply_error = send_protocol_error( 709 packet_sender, packet_receiver, link_id, *client_id, 710 message_id, protocol_error); 711 if (send_reply_error != SFCP_ERROR_SUCCESS) { 712 return send_reply_error; 713 } 714 } 715 716 return sfcp_err; 717 } 718 719 enum sfcp_error_t sfcp_receive_reply(uint8_t *buf, size_t buf_size, 720 struct sfcp_reply_metadata_t metadata, 721 uint8_t **payload, size_t *payload_len) 722 { 723 struct sfcp_packet_t *packet; 724 enum sfcp_error_t sfcp_err; 725 enum sfcp_protocol_error_t protocol_error; 726 enum sfcp_packet_type_t packet_type; 727 sfcp_link_id_t link_id; 728 sfcp_link_id_t my_node_id; 729 bool needs_reply; 730 bool uses_id_extension; 731 size_t received_size = 0; 732 bool packet_uses_crypto; 733 uint16_t packet_application_id; 734 uint16_t packet_client_id; 735 sfcp_node_id_t packet_sender; 736 sfcp_node_id_t packet_receiver; 737 sfcp_node_id_t forwarding_destination; 738 sfcp_node_id_t received_receiver_id; 739 uint8_t message_id; 740 741 if ((buf == NULL) || (payload == NULL) || (payload_len == NULL)) { 742 return SFCP_ERROR_INVALID_POINTER; 743 } 744 745 sfcp_err = receive_msg_reply(buf, buf_size, false, metadata.receiver, 746 &received_receiver_id, &link_id, 747 &my_node_id, false, &received_size); 748 if (sfcp_err != SFCP_ERROR_SUCCESS) { 749 return sfcp_err; 750 } 751 752 packet = (struct sfcp_packet_t *)buf; 753 754 sfcp_err = sfcp_helpers_parse_packet( 755 packet, received_size, &packet_sender, &packet_receiver, 756 &message_id, &packet_uses_crypto, &uses_id_extension, 757 &packet_application_id, &packet_client_id, payload, payload_len, 758 &needs_reply, &packet_type); 759 if (sfcp_err != SFCP_ERROR_SUCCESS) { 760 /* Do not know enough about this packet to reply */ 761 return sfcp_err; 762 } 763 764 if (sfcp_helpers_packet_requires_forwarding_get_destination( 765 packet_sender, packet_receiver, packet_type, my_node_id, 766 &forwarding_destination)) { 767 protocol_error = SFCP_PROTOCOL_FORWARDING_UNSUPPORTED; 768 sfcp_err = SFCP_ERROR_NO_REPLY_AVAILABLE; 769 goto error_reply; 770 } 771 772 if ((packet_type != SFCP_PACKET_TYPE_PROTOCOL_ERROR_REPLY) && 773 (packet_type != SFCP_PACKET_TYPE_REPLY)) { 774 protocol_error = SFCP_PROTOCOL_ERROR_TRY_AGAIN_LATER; 775 sfcp_err = SFCP_ERROR_NO_REPLY_AVAILABLE; 776 goto error_reply; 777 } 778 779 /* Message is definitely not something we need to reply 780 * to, so we can just return an error to the caller 781 */ 782 783 if ((packet_sender != my_node_id) || 784 (packet_receiver != received_receiver_id)) { 785 return SFCP_ERROR_INVALID_NODE; 786 } 787 788 if (!uses_id_extension && (metadata.client_id != 0)) { 789 return SFCP_ERROR_INVALID_CLIENT_ID; 790 } 791 792 if (packet_type == SFCP_PACKET_TYPE_PROTOCOL_ERROR_REPLY) { 793 if (packet_client_id == metadata.client_id) { 794 /* Error message for us */ 795 return sfcp_protocol_error_to_sfcp_error( 796 packet->error_reply.protocol_error); 797 } else { 798 /* Error message for a different client ID, drop */ 799 return SFCP_ERROR_NO_REPLY_AVAILABLE; 800 } 801 } 802 803 if (uses_id_extension && (packet_client_id != metadata.client_id)) { 804 /* This reply is not for us so we have to drop it */ 805 return SFCP_ERROR_NO_REPLY_AVAILABLE; 806 } 807 808 if (uses_id_extension && 809 (packet_application_id != metadata.application_id)) { 810 /* Message has the client ID so it is for us, but the sender has not correctly 811 * set the application ID, drop 812 */ 813 return SFCP_ERROR_INVALID_APPLICATION_ID; 814 } 815 816 if (packet_uses_crypto != metadata.uses_cryptography) { 817 return SFCP_ERROR_INVALID_CRYPTO_MODE; 818 } 819 820 if (message_id != metadata.message_id) { 821 return SFCP_ERROR_INVALID_SEQUENCE_NUMBER; 822 } 823 824 if (packet_uses_crypto) { 825 sfcp_err = sfcp_decrypt_reply(packet, received_size, 826 received_receiver_id); 827 if (sfcp_err != SFCP_ERROR_SUCCESS) { 828 return sfcp_err; 829 } 830 } 831 832 return SFCP_ERROR_SUCCESS; 833 834 error_reply: 835 if (needs_reply) { 836 enum sfcp_error_t send_reply_error = send_protocol_error( 837 packet_sender, packet_receiver, link_id, 838 packet_client_id, message_id, protocol_error); 839 if (send_reply_error != SFCP_ERROR_SUCCESS) { 840 return send_reply_error; 841 } 842 } 843 844 return sfcp_err; 845 } 846 847 enum sfcp_error_t sfcp_get_msg_handler(uint16_t application_id, 848 sfcp_handler_t *handler) 849 { 850 if (handler == NULL) { 851 return SFCP_ERROR_INVALID_POINTER; 852 } 853 854 for (uint8_t i = 0; i < SFCP_MAX_NUMBER_MESSAGE_HANDLERS; i++) { 855 if (sfcp_msg_handlers[i].in_use && 856 (sfcp_msg_handlers[i].application_id == application_id)) { 857 *handler = sfcp_msg_handlers[i].handler; 858 return SFCP_ERROR_SUCCESS; 859 } 860 } 861 862 return SFCP_ERROR_INVALID_APPLICATION_ID; 863 } 864 865 enum sfcp_error_t sfcp_register_msg_handler(uint16_t application_id, 866 sfcp_handler_t handler) 867 { 868 if (handler == NULL) { 869 return SFCP_ERROR_INVALID_POINTER; 870 } 871 872 for (uint8_t i = 0; i < SFCP_MAX_NUMBER_MESSAGE_HANDLERS; i++) { 873 if (!sfcp_msg_handlers[i].in_use) { 874 sfcp_msg_handlers[i].handler = handler; 875 sfcp_msg_handlers[i].application_id = application_id; 876 sfcp_msg_handlers[i].in_use = true; 877 return SFCP_ERROR_SUCCESS; 878 } 879 } 880 881 return SFCP_ERROR_HANDLER_TABLE_FULL; 882 } 883 884 enum sfcp_error_t sfcp_pop_msg_from_buffer(sfcp_buffer_handle_t buffer_handle, 885 sfcp_node_id_t *sender, 886 uint16_t *client_id, 887 bool *needs_reply, uint8_t *payload, 888 size_t payload_len, 889 size_t *payload_size, 890 struct sfcp_msg_metadata_t *metadata) 891 { 892 enum sfcp_error_t sfcp_err; 893 enum sfcp_hal_error_t hal_error; 894 enum sfcp_protocol_error_t protocol_error; 895 struct sfcp_packet_t *packet; 896 enum sfcp_packet_type_t packet_type; 897 sfcp_node_id_t my_node_id; 898 size_t packet_size; 899 bool packet_uses_id_extension, packet_uses_crypto; 900 sfcp_node_id_t packet_receiver; 901 uint8_t *packet_payload; 902 size_t packet_payload_size; 903 uint8_t message_id; 904 uint16_t packet_application_id; 905 906 if ((sender == NULL) || (client_id == NULL) || (needs_reply == NULL) || 907 (payload_size == NULL) || (metadata == NULL)) { 908 return SFCP_ERROR_INVALID_POINTER; 909 } 910 911 if ((payload_len != 0) && (payload == NULL)) { 912 return SFCP_ERROR_INVALID_POINTER; 913 } 914 915 sfcp_err = sfcp_get_handler_buffer(buffer_handle, (uint8_t **)&packet, 916 &packet_size); 917 if (sfcp_err != SFCP_ERROR_SUCCESS) { 918 return sfcp_err; 919 } 920 921 sfcp_err = sfcp_helpers_parse_packet( 922 packet, packet_size, sender, &packet_receiver, &message_id, 923 &packet_uses_crypto, &packet_uses_id_extension, 924 &packet_application_id, client_id, &packet_payload, 925 &packet_payload_size, needs_reply, &packet_type); 926 if (sfcp_err != SFCP_ERROR_SUCCESS) { 927 /* Do not know enough about this packet to reply */ 928 return sfcp_err; 929 } 930 931 hal_error = sfcp_hal_get_my_node_id(&my_node_id); 932 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 933 protocol_error = SFCP_PROTOCOL_INTERNAL_ERROR; 934 sfcp_err = sfcp_hal_error_to_sfcp_error(hal_error); 935 goto error_reply; 936 } 937 938 if ((packet_type != SFCP_PACKET_TYPE_MSG_NEEDS_REPLY) && 939 (packet_type != SFCP_PACKET_TYPE_MSG_NO_REPLY)) { 940 protocol_error = SFCP_PROTOCOL_ERROR_INVALID_CONTEXT; 941 sfcp_err = SFCP_ERROR_NO_MSG_AVAILABLE; 942 goto error_reply; 943 } 944 945 if (packet_receiver != my_node_id) { 946 protocol_error = SFCP_PROTOCOL_ERROR_UNSUPPORTED; 947 sfcp_err = SFCP_ERROR_INVALID_NODE; 948 goto error_reply; 949 } 950 951 if (packet_payload_size > payload_len) { 952 protocol_error = SFCP_PROTOCOL_ERROR_MSG_TOO_LARGE_TO_RECIEVE; 953 sfcp_err = SFCP_ERROR_PAYLOAD_TOO_LARGE; 954 goto error_reply; 955 } 956 957 if (packet_uses_crypto) { 958 sfcp_err = sfcp_decrypt_msg(packet, packet_size, *sender); 959 if (sfcp_err != SFCP_ERROR_SUCCESS) { 960 protocol_error = SFCP_PROTOCOL_ERROR_DECRYPTION_FAILED; 961 goto error_reply; 962 } 963 } 964 965 memcpy(payload, packet_payload, packet_payload_size); 966 *payload_size = packet_payload_size; 967 968 populate_msg_metadata( 969 metadata, *sender, packet_uses_crypto, *client_id, 970 packet_application_id, message_id, 971 packet_uses_crypto ? 972 packet->cryptography_used.cryptography_metadata.config 973 .trusted_subnet_id : 974 0); 975 976 sfcp_err = SFCP_ERROR_SUCCESS; 977 goto pop_message; 978 979 error_reply: 980 if (*needs_reply) { 981 enum sfcp_error_t send_reply_error; 982 sfcp_link_id_t link_id; 983 984 link_id = sfcp_hal_get_route(*sender); 985 if (link_id == 0) { 986 return SFCP_ERROR_INVALID_NODE; 987 } 988 989 send_reply_error = send_protocol_error(*sender, packet_receiver, 990 link_id, *client_id, 991 message_id, 992 protocol_error); 993 if (send_reply_error != SFCP_ERROR_SUCCESS) { 994 return send_reply_error; 995 } 996 } 997 998 pop_message: { 999 enum sfcp_error_t pop_buffer_sfcp_error = 1000 sfcp_pop_handler_buffer(buffer_handle); 1001 if (pop_buffer_sfcp_error != SFCP_ERROR_SUCCESS) { 1002 return pop_buffer_sfcp_error; 1003 } 1004 } 1005 1006 return sfcp_err; 1007 } 1008 1009 enum sfcp_error_t sfcp_get_reply_handler(uint16_t client_id, 1010 sfcp_handler_t *handler) 1011 { 1012 for (uint8_t i = 0; i < SFCP_MAX_NUMBER_REPLY_HANDLERS; i++) { 1013 if (sfcp_reply_handlers[i].in_use && 1014 (sfcp_reply_handlers[i].client_id == client_id)) { 1015 *handler = sfcp_reply_handlers[i].handler; 1016 return SFCP_ERROR_SUCCESS; 1017 } 1018 } 1019 1020 return SFCP_ERROR_INVALID_CLIENT_ID; 1021 } 1022 1023 enum sfcp_error_t sfcp_register_reply_handler(uint16_t client_id, 1024 sfcp_handler_t handler) 1025 { 1026 for (uint8_t i = 0; i < SFCP_MAX_NUMBER_REPLY_HANDLERS; i++) { 1027 if (!sfcp_reply_handlers[i].in_use) { 1028 sfcp_reply_handlers[i].handler = handler; 1029 sfcp_reply_handlers[i].client_id = client_id; 1030 sfcp_reply_handlers[i].in_use = true; 1031 return SFCP_ERROR_SUCCESS; 1032 } 1033 } 1034 1035 return SFCP_ERROR_HANDLER_TABLE_FULL; 1036 } 1037 1038 enum sfcp_error_t 1039 sfcp_pop_reply_from_buffer(sfcp_buffer_handle_t buffer_handle, uint8_t *payload, 1040 size_t payload_len, size_t *payload_size, 1041 struct sfcp_reply_metadata_t *metadata) 1042 { 1043 enum sfcp_error_t sfcp_err; 1044 enum sfcp_hal_error_t hal_error; 1045 enum sfcp_protocol_error_t protocol_error; 1046 struct sfcp_packet_t *packet; 1047 enum sfcp_packet_type_t packet_type; 1048 sfcp_node_id_t my_node_id; 1049 size_t packet_size; 1050 bool packet_uses_id_extension, packet_uses_crypto; 1051 bool needs_reply; 1052 sfcp_node_id_t packet_sender, packet_receiver; 1053 uint8_t *packet_payload; 1054 size_t packet_payload_size; 1055 uint8_t message_id; 1056 uint16_t packet_application_id, packet_client_id; 1057 1058 if ((payload_size == NULL) || (metadata == NULL)) { 1059 return SFCP_ERROR_INVALID_POINTER; 1060 } 1061 1062 if ((payload_len != 0) && (payload == NULL)) { 1063 return SFCP_ERROR_INVALID_POINTER; 1064 } 1065 1066 sfcp_err = sfcp_get_handler_buffer(buffer_handle, (uint8_t **)&packet, 1067 &packet_size); 1068 if (sfcp_err != SFCP_ERROR_SUCCESS) { 1069 return sfcp_err; 1070 } 1071 1072 sfcp_err = sfcp_helpers_parse_packet( 1073 packet, packet_size, &packet_sender, &packet_receiver, 1074 &message_id, &packet_uses_crypto, &packet_uses_id_extension, 1075 &packet_application_id, &packet_client_id, &packet_payload, 1076 &packet_payload_size, &needs_reply, &packet_type); 1077 if (sfcp_err != SFCP_ERROR_SUCCESS) { 1078 /* Do not know enough about this packet to reply */ 1079 return sfcp_err; 1080 } 1081 1082 hal_error = sfcp_hal_get_my_node_id(&my_node_id); 1083 if (hal_error != SFCP_HAL_ERROR_SUCCESS) { 1084 protocol_error = SFCP_PROTOCOL_INTERNAL_ERROR; 1085 sfcp_err = sfcp_hal_error_to_sfcp_error(hal_error); 1086 goto error_reply; 1087 } 1088 1089 if ((packet_type != SFCP_PACKET_TYPE_PROTOCOL_ERROR_REPLY) && 1090 (packet_type != SFCP_PACKET_TYPE_REPLY)) { 1091 protocol_error = SFCP_PROTOCOL_ERROR_TRY_AGAIN_LATER; 1092 sfcp_err = SFCP_ERROR_NO_REPLY_AVAILABLE; 1093 goto error_reply; 1094 } 1095 1096 /* Message is definitely not something we need to reply 1097 * to, so we can just return an error to the caller 1098 */ 1099 1100 if (packet_sender != my_node_id) { 1101 return SFCP_ERROR_INVALID_NODE; 1102 } 1103 1104 if (packet_type == SFCP_PACKET_TYPE_PROTOCOL_ERROR_REPLY) { 1105 return sfcp_protocol_error_to_sfcp_error( 1106 packet->error_reply.protocol_error); 1107 } 1108 1109 if (packet_payload_size > payload_len) { 1110 return SFCP_ERROR_PAYLOAD_TOO_LARGE; 1111 } 1112 1113 if (packet_uses_crypto) { 1114 sfcp_err = sfcp_decrypt_reply(packet, packet_size, 1115 packet_receiver); 1116 if (sfcp_err != SFCP_ERROR_SUCCESS) { 1117 return sfcp_err; 1118 } 1119 } 1120 1121 memcpy(payload, packet_payload, packet_payload_size); 1122 *payload_size = packet_payload_size; 1123 1124 populate_reply_metadata( 1125 metadata, packet_receiver, packet_uses_crypto, packet_client_id, 1126 packet_application_id, message_id, 1127 packet_uses_crypto ? 1128 packet->cryptography_used.cryptography_metadata.config 1129 .trusted_subnet_id : 1130 0); 1131 1132 sfcp_err = SFCP_ERROR_SUCCESS; 1133 goto pop_message; 1134 1135 error_reply: 1136 if (needs_reply) { 1137 enum sfcp_error_t send_reply_error; 1138 sfcp_link_id_t link_id; 1139 1140 link_id = sfcp_hal_get_route(packet_sender); 1141 if (link_id == 0) { 1142 return SFCP_ERROR_INVALID_NODE; 1143 } 1144 1145 send_reply_error = send_protocol_error( 1146 packet_sender, packet_receiver, link_id, 1147 packet_client_id, message_id, protocol_error); 1148 if (send_reply_error != SFCP_ERROR_SUCCESS) { 1149 return send_reply_error; 1150 } 1151 } 1152 1153 pop_message: { 1154 enum sfcp_error_t pop_buffer_sfcp_error = 1155 sfcp_pop_handler_buffer(buffer_handle); 1156 if (pop_buffer_sfcp_error != SFCP_ERROR_SUCCESS) { 1157 return pop_buffer_sfcp_error; 1158 } 1159 } 1160 1161 return sfcp_err; 1162 } 1163