Test: Add testcase for testing outvec write

Change-Id: I855e6a5ad9b073d89aaf404a03a96f895cd5642f
Signed-off-by: Mate Toth-Pal <mate.toth-pal@arm.com>
diff --git a/interface/include/tfm_veneers.h b/interface/include/tfm_veneers.h
index f771bb9..0e0f6db 100644
--- a/interface/include/tfm_veneers.h
+++ b/interface/include/tfm_veneers.h
@@ -67,6 +67,7 @@
 int32_t tfm_spm_core_test_2_slave_service_veneer(struct psa_invec *in_vec, size_t in_len, struct psa_outvec *out_vec, size_t out_len);
 int32_t tfm_spm_core_test_2_sfn_invert_veneer(struct psa_invec *in_vec, size_t in_len, struct psa_outvec *out_vec, size_t out_len);
 int32_t tfm_spm_core_test_2_check_caller_client_id_veneer(struct psa_invec *in_vec, size_t in_len, struct psa_outvec *out_vec, size_t out_len);
+int32_t tfm_spm_core_test_2_get_every_second_byte_veneer(struct psa_invec *in_vec, size_t in_len, struct psa_outvec *out_vec, size_t out_len);
 #endif /* TFM_PARTITION_TEST_CORE */
 
 #ifdef TFM_PARTITION_TEST_SST
diff --git a/secure_fw/ns_callable/tfm_veneers.c b/secure_fw/ns_callable/tfm_veneers.c
index aa77922..ee8b6c1 100644
--- a/secure_fw/ns_callable/tfm_veneers.c
+++ b/secure_fw/ns_callable/tfm_veneers.c
@@ -62,6 +62,7 @@
 int32_t spm_core_test_2_slave_service(struct psa_invec *, size_t, struct psa_outvec *, size_t);
 int32_t spm_core_test_2_sfn_invert(struct psa_invec *, size_t, struct psa_outvec *, size_t);
 int32_t spm_core_test_2_check_caller_client_id(struct psa_invec *, size_t, struct psa_outvec *, size_t);
+int32_t spm_core_test_2_get_every_second_byte(struct psa_invec *, size_t, struct psa_outvec *, size_t);
 #endif /* TFM_PARTITION_TEST_CORE */
 
 #ifdef TFM_PARTITION_TEST_SST
@@ -141,6 +142,7 @@
 TFM_VENEER_FUNCTION(TFM_SP_CORE_TEST_2, spm_core_test_2_slave_service)
 TFM_VENEER_FUNCTION(TFM_SP_CORE_TEST_2, spm_core_test_2_sfn_invert)
 TFM_VENEER_FUNCTION(TFM_SP_CORE_TEST_2, spm_core_test_2_check_caller_client_id)
+TFM_VENEER_FUNCTION(TFM_SP_CORE_TEST_2, spm_core_test_2_get_every_second_byte)
 #endif /* TFM_PARTITION_TEST_CORE */
 
 #ifdef TFM_PARTITION_TEST_SST
diff --git a/secure_fw/services/tfm_sfid_list.inc b/secure_fw/services/tfm_sfid_list.inc
index fb2255a..4a9073a 100644
--- a/secure_fw/services/tfm_sfid_list.inc
+++ b/secure_fw/services/tfm_sfid_list.inc
@@ -61,6 +61,7 @@
     {spm_core_test_2_slave_service, TFM_CORE_TEST_2_SFN_SLAVE_SERVICE_SFID},
     {spm_core_test_2_sfn_invert, TFM_CORE_TEST_2_SFN_INVERT_SFID},
     {spm_core_test_2_check_caller_client_id, TFM_CORE_TEST_2_SFN_CHECK_CALLER_CLIENT_ID_SFID},
+    {spm_core_test_2_get_every_second_byte, TFM_CORE_TEST_2_SFN_GET_EVERY_SECOND_BYTE},
 #endif /* TFM_PARTITION_TEST_CORE */
 
 #ifdef TFM_PARTITION_TEST_SST
diff --git a/test/suites/core/non_secure/core_ns_positive_testsuite.c b/test/suites/core/non_secure/core_ns_positive_testsuite.c
index 1fc0ee0..246bcb6 100644
--- a/test/suites/core/non_secure/core_ns_positive_testsuite.c
+++ b/test/suites/core/non_secure/core_ns_positive_testsuite.c
@@ -35,6 +35,7 @@
 static void tfm_core_test_get_caller_client_id(struct test_result_t *ret);
 static void tfm_core_test_spm_request(struct test_result_t *ret);
 static void tfm_core_test_iovec_sanitization(struct test_result_t *ret);
+static void tfm_core_test_outvec_write(struct test_result_t *ret);
 
 static struct test_t core_tests[] = {
 CORE_TEST_DESCRIPTION(CORE_TEST_ID_NS_THREAD, tfm_core_test_ns_thread,
@@ -70,6 +71,9 @@
 CORE_TEST_DESCRIPTION(CORE_TEST_ID_IOVEC_SANITIZATION,
     tfm_core_test_iovec_sanitization,
     "Test service parameter sanitization"),
+CORE_TEST_DESCRIPTION(CORE_TEST_ID_OUTVEC_WRITE,
+    tfm_core_test_outvec_write,
+    "Test outvec write"),
 };
 
 void register_testsuite_ns_core_positive(struct test_suite_t *p_test_suite)
@@ -261,6 +265,63 @@
     ret->val = TEST_PASSED;
 }
 
+static void tfm_core_test_outvec_write(struct test_result_t *ret)
+{
+    int32_t err;
+    int32_t test_case_id = CORE_TEST_ID_OUTVEC_WRITE;
+    int i;
+    uint8_t in_buf_0[] = {0, 1, 2, 3, 4};
+    uint8_t in_buf_1[] = {1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89};
+    uint8_t out_buf_0[sizeof(in_buf_0)];
+    uint8_t out_buf_1[sizeof(in_buf_1)];
+
+    psa_invec in_vec[PSA_MAX_IOVEC] = { {in_buf_0, sizeof(in_buf_0)},
+                                        {in_buf_1, sizeof(in_buf_1)} };
+    psa_outvec out_vec[PSA_MAX_IOVEC] = { {out_buf_0, sizeof(out_buf_0) },
+                                        {out_buf_1, sizeof(out_buf_1)} };
+    struct tfm_core_test_call_args_t args1 = {in_vec, 2, out_vec, 2};
+    struct tfm_core_test_call_args_t args2 = {in_vec, 1, NULL, 0};
+
+    err = tfm_core_test_call(tfm_spm_core_test_2_get_every_second_byte_veneer,
+                                                                        &args1);
+
+    if (err != TFM_SUCCESS) {
+        TEST_FAIL("call to secure function should be successful");
+        return;
+    }
+
+    if (out_vec[0].len != sizeof(in_buf_0)/2 ||
+        out_vec[1].len != sizeof(in_buf_1)/2) {
+        TEST_FAIL("Number of elements in outvec is not set properly");
+        return;
+    }
+    for (i = 1; i < sizeof(in_buf_0); i += 2) {
+        if (((uint8_t *)out_vec[0].base)[i/2] != in_buf_0[i]) {
+            TEST_FAIL("result is not correct");
+            return;
+        }
+    }
+    for (i = 1; i < sizeof(in_buf_1); i += 2) {
+        if (((uint8_t *)out_vec[1].base)[i/2] != in_buf_1[i]) {
+            TEST_FAIL("result is not correct");
+            return;
+        }
+    }
+
+    /* do the same test on the secure side */
+    in_vec[0].base = &test_case_id;
+    in_vec[0].len = sizeof(int32_t);
+    err = tfm_core_test_call(tfm_spm_core_test_sfn_veneer, &args2);
+
+    if (err != TFM_SUCCESS) {
+        TEST_FAIL("Failed to execute secure side test");
+        return;
+    }
+
+    ret->val = TEST_PASSED;
+}
+
+
 /*
  * \brief Tests whether the initialisation of the service was successful.
  *
diff --git a/test/test_services/tfm_core_test/core_test_defs.h b/test/test_services/tfm_core_test/core_test_defs.h
index a78cb34..cae4264 100644
--- a/test/test_services/tfm_core_test/core_test_defs.h
+++ b/test/test_services/tfm_core_test/core_test_defs.h
@@ -30,6 +30,7 @@
 #define CORE_TEST_ID_GET_CALLER_CLIENT_ID 1013
 #define CORE_TEST_ID_SPM_REQUEST          1014
 #define CORE_TEST_ID_IOVEC_SANITIZATION   1015
+#define CORE_TEST_ID_OUTVEC_WRITE         1016
 #define CORE_TEST_ID_BLOCK                2001
 
 /* Use lower 16 bits in return value for error code, upper 16 for line number
diff --git a/test/test_services/tfm_core_test/tfm_ss_core_test.c b/test/test_services/tfm_core_test/tfm_ss_core_test.c
index cbdb92c..18dc6ec 100644
--- a/test/test_services/tfm_core_test/tfm_ss_core_test.c
+++ b/test/test_services/tfm_core_test/tfm_ss_core_test.c
@@ -276,6 +276,79 @@
     return TFM_SUCCESS;
 }
 
+static int32_t test_outvec_write(void)
+{
+    int32_t err;
+    int i;
+    uint8_t *scratch_ptr = (uint8_t *)tfm_scratch_area;
+    psa_invec in_vec [2];
+    psa_outvec out_vec [2];
+    uint8_t *in_buf_0;
+    uint8_t *in_buf_1;
+    uint8_t *out_buf_0;
+    uint8_t *out_buf_1;
+
+    if (tfm_core_set_buffer_area(TFM_BUFFER_SHARE_SCRATCH) != TFM_SUCCESS) {
+        return CORE_TEST_ERRNO_UNEXPECTED_CORE_BEHAVIOUR;
+    }
+
+    in_buf_0 = scratch_ptr;
+    for (i = 0; i < 5; ++i, ++scratch_ptr)
+    {
+        *scratch_ptr = i;
+    }
+    in_vec[0].base = in_buf_0;
+    in_vec[0].len = scratch_ptr - in_buf_0;
+
+    in_buf_1 = scratch_ptr;
+    *(scratch_ptr++) = 1;
+    *(scratch_ptr++) = 1;
+    for (i = 2; i < 11; ++i, ++scratch_ptr)
+    {
+        *scratch_ptr = *(scratch_ptr-1) + *(scratch_ptr-2);
+    }
+    in_vec[1].base = in_buf_1;
+    in_vec[1].len = scratch_ptr - in_buf_1;
+
+    out_buf_0 = scratch_ptr;
+    scratch_ptr += in_vec[0].len;
+    out_vec[0].base = out_buf_0;
+    out_vec[0].len = scratch_ptr - out_buf_0;
+
+    out_buf_1 = scratch_ptr;
+    scratch_ptr += in_vec[1].len;
+    out_vec[1].base = out_buf_0;
+    out_vec[1].len = scratch_ptr - out_buf_0;
+
+    err = tfm_spm_core_test_2_get_every_second_byte_veneer(in_vec, 2,
+                                                           out_vec, 2);
+
+    if (err != TFM_SUCCESS) {
+        return CORE_TEST_ERRNO_TEST_FAULT;
+    }
+
+    if (out_vec[0].len != in_vec[0].len/2 ||
+        out_vec[1].len != in_vec[1].len/2) {
+        return CORE_TEST_ERRNO_TEST_FAULT;
+    }
+    for (i = 1; i < sizeof(in_buf_0); i += 2) {
+        if (((uint8_t *)out_vec[0].base)[i/2] != in_buf_0[i]) {
+            return CORE_TEST_ERRNO_TEST_FAULT;
+        }
+    }
+    for (i = 1; i < sizeof(in_buf_1); i += 2) {
+        if (((uint8_t *)out_vec[1].base)[i/2] != in_buf_1[i]) {
+            return CORE_TEST_ERRNO_TEST_FAULT;
+        }
+    }
+
+    if (tfm_core_set_buffer_area(TFM_BUFFER_SHARE_DEFAULT) != TFM_SUCCESS) {
+        return CORE_TEST_ERRNO_UNEXPECTED_CORE_BEHAVIOUR;
+    }
+
+    return TFM_SUCCESS;
+}
+
 static int32_t test_ss_to_ss(void)
 {
     /* Call to a different service, should be successful */
@@ -444,6 +517,8 @@
         arg1 = (int32_t)in_vec[1].base;
         arg2 = (int32_t)out_vec[0].base;
         return test_ss_to_ss_buffer((uint32_t *)arg1, (uint32_t *)arg2, arg3);
+    case CORE_TEST_ID_OUTVEC_WRITE:
+        return test_outvec_write();
     case CORE_TEST_ID_PERIPHERAL_ACCESS:
         return test_peripheral_access();
     case CORE_TEST_ID_GET_CALLER_CLIENT_ID:
diff --git a/test/test_services/tfm_core_test_2/manifest.yaml b/test/test_services/tfm_core_test_2/manifest.yaml
index 77cc5db..b5b4fce 100644
--- a/test/test_services/tfm_core_test_2/manifest.yaml
+++ b/test/test_services/tfm_core_test_2/manifest.yaml
@@ -39,6 +39,14 @@
       "non_secure_clients": true,
       "minor_version": 1,
       "minor_policy": "strict"
+    },
+    {
+      "sfid": "TFM_CORE_TEST_2_SFN_GET_EVERY_SECOND_BYTE",
+      "signal": "TFM_CORE_TEST_2_SFN_GET_EVERY_SECOND_BYTE",
+      "tfm_symbol": "spm_core_test_2_get_every_second_byte",
+      "non_secure_clients": true,
+      "minor_version": 1,
+      "minor_policy": "strict"
     }
   ],
   "source_files": [
diff --git a/test/test_services/tfm_core_test_2/tfm_ss_core_test_2.c b/test/test_services/tfm_core_test_2/tfm_ss_core_test_2.c
index b5b70ef..b86aa55 100644
--- a/test/test_services/tfm_core_test_2/tfm_ss_core_test_2.c
+++ b/test/test_services/tfm_core_test_2/tfm_ss_core_test_2.c
@@ -85,6 +85,27 @@
     return TFM_SUCCESS;
 }
 
+int32_t spm_core_test_2_get_every_second_byte(
+                                     struct psa_invec *in_vec, size_t in_len,
+                                     struct psa_outvec *out_vec, size_t out_len)
+{
+    int i, j;
+
+    if (in_len != out_len) {
+        return TFM_ERROR_INVALID_PARAMETER;
+    }
+    for (i = 0; i < in_len; ++i) {
+        if (in_vec[i].len/2 > out_vec[i].len) {
+            return TFM_ERROR_INVALID_PARAMETER;
+        }
+        for (j = 1; j < in_vec[i].len; j += 2) {
+            ((uint8_t *)out_vec[i].base)[j/2] = ((uint8_t *)in_vec[i].base)[j];
+        }
+        out_vec[i].len = in_vec[i].len/2;
+    }
+    return TFM_SUCCESS;
+}
+
 /* Invert function */
 #define SFN_INVERT_MAX_LEN 128