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 "sfcp_psa_protocol_common.h"
12 #include "sfcp_psa_protocol_embed.h"
13
14 psa_status_t
sfcp_protocol_embed_serialize_msg(psa_handle_t handle,int16_t type,const psa_invec * in_vec,uint8_t in_len,const psa_outvec * out_vec,uint8_t out_len,struct sfcp_embed_msg_t * msg,size_t * msg_len)15 sfcp_protocol_embed_serialize_msg(psa_handle_t handle, int16_t type,
16 const psa_invec *in_vec, uint8_t in_len,
17 const psa_outvec *out_vec, uint8_t out_len,
18 struct sfcp_embed_msg_t *msg, size_t *msg_len)
19 {
20 uint32_t payload_size = 0;
21 uint32_t i;
22
23 assert(msg != NULL);
24 assert(msg_len != NULL);
25 assert(in_vec != NULL);
26
27 msg->ctrl_param = PARAM_PACK(type, in_len, out_len);
28 msg->handle = handle;
29
30 /* Fill msg iovec lengths */
31 for (i = 0U; i < in_len; ++i) {
32 msg->io_size[i] = in_vec[i].len;
33 }
34 for (i = 0U; i < out_len; ++i) {
35 msg->io_size[in_len + i] = out_vec[i].len;
36 }
37
38 for (i = 0U; i < in_len; ++i) {
39 if (in_vec[i].len > sizeof(msg->payload) - payload_size) {
40 return PSA_ERROR_INVALID_ARGUMENT;
41 }
42 memcpy(msg->payload + payload_size, in_vec[i].base,
43 in_vec[i].len);
44 payload_size += in_vec[i].len;
45 }
46
47 /* Output the actual size of the message, to optimize sending */
48 *msg_len = sizeof(*msg) - sizeof(msg->payload) + payload_size;
49
50 return PSA_SUCCESS;
51 }
52
sfcp_protocol_embed_deserialize_reply(psa_outvec * out_vec,uint8_t out_len,psa_status_t * return_val,const struct sfcp_embed_reply_t * reply,size_t reply_size)53 psa_status_t sfcp_protocol_embed_deserialize_reply(
54 psa_outvec *out_vec, uint8_t out_len, psa_status_t *return_val,
55 const struct sfcp_embed_reply_t *reply, size_t reply_size)
56 {
57 uint32_t payload_offset = 0;
58 uint32_t i;
59
60 assert(reply != NULL);
61 assert(return_val != NULL);
62
63 for (i = 0U; i < out_len; ++i) {
64 if ((sizeof(*reply) - sizeof(reply->payload) + payload_offset) >
65 reply_size) {
66 return PSA_ERROR_INVALID_ARGUMENT;
67 }
68
69 memcpy(out_vec[i].base, reply->payload + payload_offset,
70 reply->out_size[i]);
71 out_vec[i].len = reply->out_size[i];
72 payload_offset += reply->out_size[i];
73 }
74
75 *return_val = reply->return_val;
76
77 return PSA_SUCCESS;
78 }
79