static psa_status_t storage_info(psa_msg_t *msg)
{
struct psa_storage_info_t info = { 0 };
psa_storage_uid_t key = 0;
if ((msg->in_size[0] != sizeof(key)) || (msg->out_size[0] != sizeof(info))) {
return PSA_DROP_CONNECTION;
}
if (psa_read(msg->handle, 0, &key, sizeof(key)) != sizeof(key)) {
return PSA_DROP_CONNECTION;
}
#if defined(TARGET_MBED_SPM)
psa_status_t status = psa_its_get_info_impl(psa_identity(msg->handle), key, &info);
#else
psa_status_t status = psa_its_get_info_impl(msg->client_id, key, &info);
#endif
if (status == PSA_SUCCESS) {
psa_write(msg->handle, 0, &info, msg->out_size[0]);
}
return status;
}
static psa_status_t storage_set(psa_msg_t *msg)
{
psa_storage_uid_t key = 0;
void *data = NULL;
uint32_t alloc_size = msg->in_size[1];
psa_storage_create_flags_t flags = 0;
if ((msg->in_size[0] != sizeof(key)) || (msg->in_size[2] != sizeof(flags))) {
return PSA_DROP_CONNECTION;
}
if (psa_read(msg->handle, 0, &key, sizeof(key)) != sizeof(key)) {
return PSA_DROP_CONNECTION;
}
if (psa_read(msg->handle, 2, &flags, sizeof(flags)) != sizeof(flags)) {
return PSA_DROP_CONNECTION;
}
data = malloc(alloc_size);
if (data == NULL) {
return PSA_ERROR_STORAGE_FAILURE;
}
if (psa_read(msg->handle, 1, data, msg->in_size[1]) != msg->in_size[1]) {
free(data);
return PSA_ERROR_STORAGE_FAILURE;
}
#if defined(TARGET_MBED_SPM)
psa_status_t status = psa_its_set_impl(psa_identity(msg->handle), key, alloc_size, data, flags);
#else
psa_status_t status = psa_its_set_impl(msg->client_id, key, alloc_size, data, flags);
#endif
memset(data, 0, alloc_size);
free(data);
return status;
}
static psa_status_t storage_get(psa_msg_t *msg)
{
psa_storage_uid_t key = 0;
uint32_t offset = 0;
if ((msg->in_size[0] != sizeof(key)) || (msg->in_size[1] != sizeof(offset))) {
return PSA_DROP_CONNECTION;
}
if (psa_read(msg->handle, 0, &key, sizeof(key)) != sizeof(key)) {
return PSA_DROP_CONNECTION;
}
if (psa_read(msg->handle, 1, &offset, sizeof(offset)) != sizeof(offset)) {
return PSA_DROP_CONNECTION;
}
uint8_t *data = (uint8_t *)malloc(msg->out_size[0]);
if (data == NULL) {
return PSA_ERROR_STORAGE_FAILURE;
}
#if defined(TARGET_MBED_SPM)
psa_status_t status = psa_its_get_impl(psa_identity(msg->handle), key, offset, msg->out_size[0], data);
#else
psa_status_t status = psa_its_get_impl(msg->client_id, key, offset, msg->out_size[0], data);
#endif
if (status == PSA_SUCCESS) {
psa_write(msg->handle, 0, data, msg->out_size[0]);
}
memset(data, 0, msg->out_size[0]);
free(data);
return status;
}
static psa_status_t storage_remove(psa_msg_t *msg)
{
psa_storage_uid_t key = 0;
if (msg->in_size[0] != sizeof(key)) {
return PSA_DROP_CONNECTION;
}
if (psa_read(msg->handle, 0, &key, sizeof(key)) != sizeof(key)) {
return PSA_DROP_CONNECTION;
}
#if defined(TARGET_MBED_SPM)
return psa_its_remove_impl(psa_identity(msg->handle), key);
#else
return psa_its_remove_impl(msg->client_id, key);
#endif
}
void smoke_part_main(void *ptr)
{
uint32_t signals = 0;
int32_t client_id = 0;
psa_msg_t msg = {0};
while (1) {
signals = psa_wait(ROT_SRV1_MSK, PSA_BLOCK);
if ((signals & ROT_SRV1_MSK) != ROT_SRV1_MSK) {
SPM_PANIC("Received unknown signal (0x%08lx)\n", signals);
}
if (PSA_SUCCESS != psa_get(ROT_SRV1_MSK, &msg)) {
continue;
}
if (msg.handle != PSA_NULL_HANDLE) {
client_id = msg.client_id;
if (client_id != -1) {
SPM_PANIC("Received message from unexpected source (0x%08lx)\n", client_id);
}
}
switch (msg.type) {
case PSA_IPC_CALL: {
if (
((msg.in_size[0] + msg.in_size[1] + msg.in_size[2] + msg.in_size[3]) != ACTUAL_MSG_SIZE) ||
(msg.out_size[0] != SERVER_RSP_BUF_SIZE)
) {
SPM_PANIC("Received message does not comply with message convention");
}
char *read_msg_buf = malloc(sizeof(char) * SERVER_READ_MSG_BUF_SIZE);
if (NULL == read_msg_buf) {
SPM_PANIC("Failed to allocate Memory");
}
memset(read_msg_buf, 0, SERVER_READ_MSG_BUF_SIZE);
char *read_ptr = read_msg_buf;
for (size_t i = 0; i < PSA_MAX_IOVEC - 1; i++) {
uint32_t bytes_read = psa_read(msg.handle, i, read_ptr, msg.in_size[i]);
if (bytes_read != msg.in_size[i]) {
SPM_PANIC("Expected to read %zu, got %lu", msg.in_size[i], bytes_read);
}
read_ptr += bytes_read;
}
int cmp_res = strcmp(SERVER_EXPECTED_READ_MSG, read_msg_buf);
if (cmp_res != 0) {
SPM_PANIC("psa_read() - Bad reading!!");
}
psa_write(msg.handle, 0, WRITE_MSG_BUF, strlen(WRITE_MSG_BUF) + 1);
free(read_msg_buf);
read_msg_buf = NULL;
read_ptr = NULL;
break;
}
case PSA_IPC_DISCONNECT:
// Fallthrough
case PSA_IPC_CONNECT: {
if (
(msg.out_size[0] != 0) || (msg.out_size[1] != 0) ||
(msg.out_size[2] != 0) || (msg.out_size[3] != 0) ||
(msg.in_size[0] != 0) || (msg.in_size[1] != 0) ||
(msg.in_size[2] != 0) || (msg.in_size[3] != 0)
) {
SPM_PANIC("Should not receive iovecs in PSA_IPC_CONNECT or PSA_IPC_DISCONNECT");
}
break;
}
default:
SPM_PANIC("Unexpected message type %lu!", msg.type);
break;
}
psa_reply(msg.handle, PSA_SUCCESS);
}
}
void part2_main(void *ptr)
{
uint32_t signals = 0;
size_t len = 0;
char *str = NULL;
while (1) {
signals = psa_wait_any(PSA_BLOCK);
if (0 == (signals & (ROT_SRV_REVERSE_MSK | ROT_SRV_DB_TST_MSK))) {
SPM_PANIC("returned from psa_wait_any without ROT_SRV_REVERSE_MSK or ROT_SRV_DB_TST_MSK bit on\n");
}
if (signals & ROT_SRV_REVERSE_MSK) {
psa_get(ROT_SRV_REVERSE_MSK, &msg);
switch (msg.type) {
case PSA_IPC_CALL: {
if ((msg.in_size[0] + msg.in_size[1] + msg.in_size[2]) == 0) {
SPM_PANIC("got a zero message size to REVERSE ROT_SRV\n");
}
len = msg.in_size[0];
str = (char *)malloc(sizeof(char) * len);
if (NULL == str) {
SPM_PANIC("memory allocation failure\n");
}
psa_read(msg.handle, 0, str, len);
for (size_t i = 0; i < len / 2; i ++) {
char a = str[i];
str[i] = str[len - i - 1];
str[len - i - 1] = a;
}
psa_write(msg.handle, 0, str, len);
free(str);
str = NULL;
break;
}
case PSA_IPC_CONNECT:
case PSA_IPC_DISCONNECT:
break;
default:
SPM_PANIC("Unexpected message type %d!", (int)(msg.type));
break;
}
psa_reply(msg.handle, PSA_SUCCESS);
} else { // -- Doorbell test
psa_get(ROT_SRV_DB_TST_MSK, &msg);
switch (msg.type) {
case PSA_IPC_CALL: {
int32_t caller_part_id = psa_identity(msg.handle);
// Doorbell contract is valid only between secure partitions
if (PSA_NSPE_IDENTIFIER == caller_part_id) {
SPM_PANIC("Caller partition is non secure\n");
}
// In doorbell scenario the server first calls psa_reply()
psa_reply(msg.handle, PSA_SUCCESS);
// Then the servers waits to some driver making long calculations - imitate using osDelay()
osDelay(20);
// After work is done, ring the doorbell
psa_notify(caller_part_id);
break;
}
case PSA_IPC_CONNECT:
case PSA_IPC_DISCONNECT:
psa_reply(msg.handle, PSA_SUCCESS);
break;
default:
SPM_PANIC("Unexpected message type %d!", (int)(msg.type));
break;
}
}
}
}
