blob: fc348d2b5d1ef6a5bbaaa0da1d53955316b712cb [file] [log] [blame]
Raef Coles734aaac2022-06-15 14:37:22 +01001/*
2 * Copyright (c) 2022, Arm Limited. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8
9#include <string.h>
10
11#include <common/debug.h>
12#include "rss_comms_protocol_embed.h"
13
14#define TYPE_OFFSET (16U)
15#define TYPE_MASK (0xFFFFUL << TYPE_OFFSET)
16#define IN_LEN_OFFSET (8U)
17#define IN_LEN_MASK (0xFFUL << IN_LEN_OFFSET)
18#define OUT_LEN_OFFSET (0U)
19#define OUT_LEN_MASK (0xFFUL << OUT_LEN_OFFSET)
20
21#define PARAM_PACK(type, in_len, out_len) \
22 (((((uint32_t)type) << TYPE_OFFSET) & TYPE_MASK) | \
23 ((((uint32_t)in_len) << IN_LEN_OFFSET) & IN_LEN_MASK) | \
24 ((((uint32_t)out_len) << OUT_LEN_OFFSET) & OUT_LEN_MASK))
25
26psa_status_t rss_protocol_embed_serialize_msg(psa_handle_t handle,
27 int16_t type,
28 const psa_invec *in_vec,
29 uint8_t in_len,
30 const psa_outvec *out_vec,
31 uint8_t out_len,
32 struct rss_embed_msg_t *msg,
33 size_t *msg_len)
34{
35 uint32_t payload_size = 0;
36 uint32_t i;
37
38 assert(msg != NULL);
39 assert(msg_len != NULL);
40 assert(in_vec != NULL);
41 assert(out_vec != NULL);
42
43 msg->ctrl_param = PARAM_PACK(type, in_len, out_len);
44 msg->handle = handle;
45
46 /* Fill msg iovec lengths */
47 for (i = 0U; i < in_len; ++i) {
48 msg->io_size[i] = in_vec[i].len;
49 }
50 for (i = 0U; i < out_len; ++i) {
51 msg->io_size[in_len + i] = out_vec[i].len;
52 }
53
54 for (i = 0U; i < in_len; ++i) {
55 if (in_vec[i].len > sizeof(msg->trailer) - payload_size) {
56 return PSA_ERROR_INVALID_ARGUMENT;
57 }
58 memcpy(msg->trailer + payload_size, in_vec[i].base, in_vec[i].len);
59 payload_size += in_vec[i].len;
60 }
61
62 /* Output the actual size of the message, to optimize sending */
63 *msg_len = sizeof(*msg) - sizeof(msg->trailer) + payload_size;
64
65 return PSA_SUCCESS;
66}
67
68psa_status_t rss_protocol_embed_deserialize_reply(psa_outvec *out_vec,
69 uint8_t out_len,
70 psa_status_t *return_val,
71 const struct rss_embed_reply_t *reply,
72 size_t reply_size)
73{
74 uint32_t payload_offset = 0;
75 uint32_t i;
76
77 assert(reply != NULL);
78 assert(out_vec != NULL);
79 assert(return_val != NULL);
80
81 for (i = 0U; i < out_len; ++i) {
82 if (sizeof(reply) - sizeof(reply->trailer) + payload_offset > reply_size) {
83 return PSA_ERROR_INVALID_ARGUMENT;
84 }
85
86 memcpy(out_vec[i].base, reply->trailer + payload_offset, out_vec[i].len);
87 payload_offset += out_vec[i].len;
88 }
89
90 *return_val = reply->return_val;
91
92 return PSA_SUCCESS;
93}