1 /*
2 * Copyright (c) 2026, Arm Limited. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include "sfcp_helpers.h"
9
10 #include "sfcp_link_hal.h"
11
12 enum sfcp_error_t
sfcp_helpers_parse_packet(struct sfcp_packet_t * packet,size_t packet_size,sfcp_node_id_t * sender,sfcp_node_id_t * receiver,uint8_t * message_id,bool * uses_cryptography,bool * uses_id_extension,uint16_t * application_id,uint16_t * client_id,uint8_t ** payload,size_t * payload_len,bool * needs_reply,enum sfcp_packet_type_t * packet_type)13 sfcp_helpers_parse_packet(struct sfcp_packet_t *packet, size_t packet_size,
14 sfcp_node_id_t *sender, sfcp_node_id_t *receiver,
15 uint8_t *message_id, bool *uses_cryptography,
16 bool *uses_id_extension, uint16_t *application_id,
17 uint16_t *client_id, uint8_t **payload,
18 size_t *payload_len, bool *needs_reply,
19 enum sfcp_packet_type_t *packet_type)
20 {
21 if (GET_METADATA_FIELD(PROTOCOL_VERSION, packet->header.metadata) !=
22 SFCP_PROTOCOL_VERSION) {
23 return SFCP_ERROR_INVALID_PROTOCOL_VERSION;
24 }
25
26 if (packet_size < sizeof(packet->header)) {
27 return SFCP_ERROR_INVALID_PACKET_SIZE;
28 }
29
30 /* Parse header */
31 *sender = packet->header.sender_id;
32 *receiver = packet->header.receiver_id;
33 *message_id = packet->header.message_id;
34 *packet_type = GET_METADATA_FIELD(PACKET_TYPE, packet->header.metadata);
35 *uses_cryptography =
36 GET_METADATA_FIELD(USES_CRYPTOGRAPHY, packet->header.metadata);
37 *uses_id_extension =
38 GET_METADATA_FIELD(USES_ID_EXTENSION, packet->header.metadata);
39
40 switch (*packet_type) {
41 case SFCP_PACKET_TYPE_PROTOCOL_ERROR_REPLY:
42 if (packet_size != SFCP_PACKET_SIZE_ERROR_REPLY) {
43 return SFCP_ERROR_INVALID_PACKET_SIZE;
44 }
45
46 *client_id = packet->error_reply.client_id;
47 *needs_reply = false;
48 *application_id = 0;
49 *payload = NULL;
50 *payload_len = 0;
51
52 return SFCP_ERROR_SUCCESS;
53
54 case SFCP_PACKET_TYPE_MSG_NO_REPLY:
55 case SFCP_PACKET_TYPE_MSG_NEEDS_REPLY:
56 case SFCP_PACKET_TYPE_REPLY:
57 if (packet_size <
58 SFCP_PACKET_SIZE_WITHOUT_PAYLOAD(*uses_cryptography,
59 *uses_id_extension)) {
60 return SFCP_ERROR_INVALID_PACKET_SIZE;
61 }
62
63 *needs_reply =
64 (*packet_type == SFCP_PACKET_TYPE_MSG_NEEDS_REPLY);
65
66 if (*uses_id_extension) {
67 *client_id =
68 GET_SFCP_CLIENT_ID(packet, *uses_cryptography);
69 *application_id = GET_SFCP_APPLICATION_ID(
70 packet, *uses_cryptography);
71 } else {
72 *client_id = 0;
73 *application_id = 0;
74 }
75
76 *payload_len = packet_size -
77 SFCP_PACKET_SIZE_WITHOUT_PAYLOAD(
78 *uses_cryptography, *uses_id_extension);
79 if (*payload_len > 0) {
80 *payload = (uint8_t *)GET_SFCP_PAYLOAD_PTR(
81 packet, *uses_cryptography, *uses_id_extension);
82 } else {
83 *payload = NULL;
84 }
85
86 return SFCP_ERROR_SUCCESS;
87
88 default:
89 return SFCP_ERROR_INVALID_MSG_TYPE;
90 }
91 }
92
sfcp_helpers_packet_requires_forwarding_get_destination(sfcp_node_id_t sender,sfcp_node_id_t receiver,enum sfcp_packet_type_t packet_type,sfcp_node_id_t my_node_id,sfcp_node_id_t * destination)93 bool sfcp_helpers_packet_requires_forwarding_get_destination(
94 sfcp_node_id_t sender, sfcp_node_id_t receiver,
95 enum sfcp_packet_type_t packet_type, sfcp_node_id_t my_node_id,
96 sfcp_node_id_t *destination)
97 {
98 bool msg_requires_forwarding, reply_requires_forwarding;
99
100 msg_requires_forwarding =
101 ((packet_type == SFCP_PACKET_TYPE_MSG_NO_REPLY) ||
102 (packet_type == SFCP_PACKET_TYPE_MSG_NEEDS_REPLY)) &&
103 (receiver != my_node_id);
104 if (msg_requires_forwarding) {
105 *destination = receiver;
106 return true;
107 }
108
109 reply_requires_forwarding = (packet_type == SFCP_PACKET_TYPE_REPLY) &&
110 (sender != my_node_id);
111 if (reply_requires_forwarding) {
112 *destination = sender;
113 return true;
114 }
115
116 return false;
117 }
118
sfcp_helpers_generate_protocol_error_packet(struct sfcp_packet_t * packet,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)119 void sfcp_helpers_generate_protocol_error_packet(
120 struct sfcp_packet_t *packet, sfcp_node_id_t sender_id,
121 sfcp_node_id_t receiver_id, sfcp_link_id_t link_id, uint16_t client_id,
122 uint8_t message_id, enum sfcp_protocol_error_t error)
123 {
124 packet->header.metadata =
125 SET_ALL_METADATA_FIELDS(SFCP_PACKET_TYPE_PROTOCOL_ERROR_REPLY,
126 false, false, SFCP_PROTOCOL_VERSION);
127 packet->header.sender_id = sender_id;
128 packet->header.receiver_id = receiver_id;
129 packet->header.message_id = message_id;
130
131 packet->error_reply.client_id = client_id;
132 packet->error_reply.protocol_error = error;
133 }
134
sfcp_helpers_drop_receive_message(sfcp_link_id_t link_id,size_t message_size,size_t already_received)135 enum sfcp_error_t sfcp_helpers_drop_receive_message(sfcp_link_id_t link_id,
136 size_t message_size,
137 size_t already_received)
138 {
139 enum sfcp_hal_error_t hal_err;
140 size_t remaining = message_size - already_received;
141 /* 32-byte buffer used to read out message */
142 __aligned(4) uint8_t drop_message_buf[32];
143
144 while (remaining > 0) {
145 size_t chunk = remaining;
146
147 if (chunk > sizeof(drop_message_buf)) {
148 chunk = sizeof(drop_message_buf);
149 }
150
151 hal_err = sfcp_hal_receive_message(link_id, drop_message_buf,
152 message_size,
153 already_received, chunk);
154 if (hal_err != SFCP_HAL_ERROR_SUCCESS) {
155 /* Drop if we cannot drain the sender */
156 return sfcp_hal_error_to_sfcp_error(hal_err);
157 }
158
159 already_received += chunk;
160 remaining -= chunk;
161 }
162
163 return SFCP_ERROR_SUCCESS;
164 }
165