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
sfcp_protocol_error_to_sfcp_error(enum sfcp_protocol_error_t protocol_error)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
populate_reply_metadata(struct sfcp_reply_metadata_t * metadata,sfcp_node_id_t receiver,bool uses_cryptography,uint16_t client_id,uint16_t application_id,uint8_t message_id,uint8_t trusted_subnet_id)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
populate_msg_metadata(struct sfcp_msg_metadata_t * metadata,sfcp_node_id_t sender,bool uses_cryptography,uint16_t client_id,uint16_t application_id,uint8_t message_id,uint8_t trusted_subnet_id)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
sfcp_init(void)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
get_new_message_id(sfcp_node_id_t node)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
sfcp_init_msg(uint8_t * buf,size_t buf_size,sfcp_node_id_t receiver,uint16_t application_id,uint16_t client_id,bool needs_reply,bool manually_specify_ts_id,uint8_t trusted_subnet_id,uint8_t ** payload,size_t * payload_len,struct sfcp_packet_t ** msg,size_t * msg_size,struct sfcp_reply_metadata_t * metadata)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
__send_msg_reply(sfcp_node_id_t remote_node,sfcp_link_id_t link_id,struct sfcp_packet_t * packet,size_t packet_size,bool is_msg)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
send_msg_reply(struct sfcp_packet_t * packet,size_t packet_size,size_t payload_size,bool is_msg)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
sfcp_send_msg(struct sfcp_packet_t * msg,size_t msg_size,size_t payload_size)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
sfcp_init_reply(uint8_t * buf,size_t buf_size,struct sfcp_msg_metadata_t metadata,uint8_t ** payload,size_t * payload_len,struct sfcp_packet_t ** reply,size_t * reply_size)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
sfcp_send_reply(struct sfcp_packet_t * reply,size_t reply_size,size_t payload_size)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
send_protocol_error(sfcp_node_id_t sender_id,sfcp_node_id_t receiver_id,sfcp_link_id_t link_id,uint16_t client_id,uint8_t message_id,enum sfcp_protocol_error_t error)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
sfcp_not_available_error(bool is_msg)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
receive_msg_reply_from_node(uint8_t * buf,size_t buf_size,sfcp_node_id_t remote_id,sfcp_link_id_t * link_id,bool is_msg,size_t * received_size)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
receive_msg_reply(uint8_t * buf,size_t buf_size,bool any_remote_id,sfcp_node_id_t remote_id,sfcp_node_id_t * received_remote_id,sfcp_link_id_t * link_id,sfcp_link_id_t * my_node_id,bool is_msg,size_t * received_size)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
sfcp_receive_msg(uint8_t * buf,size_t buf_size,bool any_sender,sfcp_node_id_t sender,uint16_t application_id,uint16_t * client_id,uint8_t ** payload,size_t * payload_len,struct sfcp_msg_metadata_t * metadata)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
sfcp_receive_reply(uint8_t * buf,size_t buf_size,struct sfcp_reply_metadata_t metadata,uint8_t ** payload,size_t * payload_len)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
sfcp_get_msg_handler(uint16_t application_id,sfcp_handler_t * handler)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
sfcp_register_msg_handler(uint16_t application_id,sfcp_handler_t handler)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
sfcp_pop_msg_from_buffer(sfcp_buffer_handle_t buffer_handle,sfcp_node_id_t * sender,uint16_t * client_id,bool * needs_reply,uint8_t * payload,size_t payload_len,size_t * payload_size,struct sfcp_msg_metadata_t * metadata)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
sfcp_get_reply_handler(uint16_t client_id,sfcp_handler_t * handler)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
sfcp_register_reply_handler(uint16_t client_id,sfcp_handler_t handler)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
sfcp_pop_reply_from_buffer(sfcp_buffer_handle_t buffer_handle,uint8_t * payload,size_t payload_len,size_t * payload_size,struct sfcp_reply_metadata_t * metadata)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