Fix handshake issues and validation

- Fixed secure SVC to monitor handshake
- Cleaned-up and completed handshake validation tests.

Signed-off-by: Greg Bellows <greg.bellows@linaro.org>

2 files changed, 62 insertions, 47 deletions

diff --git a/arm/tztest.c b/arm/tztest.c
index d7e2745..fb81026 100644
--- a/arm/tztest.c
+++ b/arm/tztest.c
@@ -150,50 +150,6 @@ uint32_t P1_nonsecure_check_mask_bits()
     return ret;
 }
 
-uint32_t tztest_secure_usr_test1(uint32_t arg)
-{
-    DEBUG_MSG("Entered arg = 0x%x\n", arg);
-    __svc(1,0);
-    DEBUG_MSG("Exiting\n");
-
-    return arg/2;
-}
-SECURE_USR_FUNC(tztest_secure_usr_test1);
-
-uint32_t tztest_secure_svc_test1(uint32_t arg)
-{
-    DEBUG_MSG("arg = 0x%x\n", arg);
-    return arg*2;
-}
-
-uint32_t tztest_check_secure_usr_handshake()
-{
-    uint32_t ret = 0;
-    DISPATCH_SECURE_USR(tztest_secure_usr_test1,
-                        TZTEST_SVAL, ret);
-
-    if (TZTEST_SVAL/2 != ret) {
-        DEBUG_MSG("\n***** Failed secure usr handshake *****\n");
-        assert(TZTEST_SVAL/2 == ret);
-    }
-
-    return ret;
-}
-
-uint32_t tztest_check_secure_svc_handshake()
-{
-    uint32_t ret = 0;
-    DISPATCH_SECURE_SVC(tztest_secure_svc_test1,
-                        TZTEST_NSVAL, ret);
-
-    if (TZTEST_NSVAL*2 != ret) {
-        DEBUG_MSG("\n***** Failed secure svc handshake *****\n");
-        assert(TZTEST_NSVAL*2 == ret);
-    }
-
-    return ret;
-}
-
 uint32_t MON_check_state()
 {
     printf("\nValidating monitor mode:\n");
@@ -326,6 +282,62 @@ uint32_t MON_check_banked_regs()
     return 0;
 }
 
+#ifdef DEBUG
+uint32_t tztest_div_by_2(uint32_t arg) {return arg/2;}
+SECURE_USR_FUNC(tztest_div_by_2);
+
+#define TZTEST_HANDSHAKE_FUNC(_name, _remote)       \
+    uint32_t _name(uint32_t arg) {                  \
+        uint32_t ret = 0;                           \
+        _remote(tztest_div_by_2, arg, ret);         \
+        assert(arg/2 == ret);                       \
+        return ret;                                 \
+    }
+
+TZTEST_HANDSHAKE_FUNC(hs_with_ns_svc, DISPATCH_NONSECURE_SVC)
+TZTEST_HANDSHAKE_FUNC(hs_with_s_svc, DISPATCH_SECURE_SVC)
+TZTEST_HANDSHAKE_FUNC(hs_with_s_usr, DISPATCH_SECURE_USR)
+TZTEST_HANDSHAKE_FUNC(hs_with_mon, DISPATCH_MONITOR)
+
+#define CHECK_HANDSHAKE(_func)                                      \
+            _func;                                            \
+            if (val/2 != ret) {                                     \
+                DEBUG_MSG("Handshake %s FAILED (0x%x != 0x%x)\n",   \
+                          #_func, val/2, ret);                      \
+                assert(val/2 == ret);                               \
+            }                                                       \
+
+void test_handshake()
+{
+    uint32_t ret = 0;
+    uint32_t val = TZTEST_SVAL;
+
+    /* NS_USR -> NS_SVC */
+    CHECK_HANDSHAKE(ret = hs_with_ns_svc(val));
+
+    /* NS_USR -> NS_SVC -> S_SVC */
+    CHECK_HANDSHAKE(ret = hs_with_s_svc(val));
+
+    /* NS_USR -> NS_SVC -> S_SVC -> S_USR */
+    CHECK_HANDSHAKE(ret = hs_with_s_usr(val));
+
+    /* NS_USR -> NS_SVC -> MON */
+    CHECK_HANDSHAKE(ret = hs_with_mon(val));
+
+    /* NS_SVC -> S_SVC */
+    CHECK_HANDSHAKE(DISPATCH_NONSECURE_SVC(hs_with_s_svc, val, ret));
+
+    /* NS_SVC -> S_USR */
+    CHECK_HANDSHAKE(DISPATCH_NONSECURE_SVC(hs_with_s_usr, val, ret));
+
+    /* NS_SVC -> MON */
+    CHECK_HANDSHAKE(DISPATCH_NONSECURE_SVC(hs_with_mon, val, ret));
+
+    /* S_SVC -> MON */
+    CHECK_HANDSHAKE(DISPATCH_SECURE_SVC(hs_with_mon, val, ret));
+}
+#endif
+
 uint32_t tztest_nonsecure_usr_main()
 {
     uint32_t ret = 0;
@@ -338,8 +350,7 @@ uint32_t tztest_nonsecure_usr_main()
     P0_nonsecure_check_register_access();
 
 #ifdef DEBUG
-    tztest_check_secure_usr_handshake();
-    tztest_check_secure_svc_handshake();
+    test_handshake();
 #endif
 
     DISPATCH_SECURE_USR(P0_secure_check_register_access, 0, ret);
diff --git a/arm/tztest_secure_svc.c b/arm/tztest_secure_svc.c
index cd00a12..306f851 100644
--- a/arm/tztest_secure_svc.c
+++ b/arm/tztest_secure_svc.c
@@ -63,6 +63,10 @@ void sec_svc_handler(volatile uint32_t op, volatile tztest_svc_desc_t *desc)
 {
     DEBUG_MSG("Entered\n");
     switch (op) {
+        case SVC_DISPATCH_MONITOR:
+            op = SMC_DISPATCH_MONITOR;
+            __smc(op, desc);
+            break;
         case SVC_GET_SECURE_STATE:
             /* This SVC handler is only accessible from the secure vector
              * table, so unless something went drastically wrong with the
@@ -202,6 +206,6 @@ void tztest_dispatch_monitor(tztest_smc_desc_t *desc)
 {
     uint32_t (*func)(uint32_t) = desc->dispatch.func;
     DEBUG_MSG("Entered\n");
-    func(desc->dispatch.arg);
+    desc->dispatch.ret = func(desc->dispatch.arg);
     DEBUG_MSG("Exiting\n");
 }