SPM: Stateless services can be searched by sid

Stateless services need to be able to be found by SID. This is
required in 'psa_connect()' and 'psa_version()' API.
Error check order in 'psa_connect()' is also updated: If a
stateless service SID is detected, block the connection directly.

Change-Id: If5182d72986173eb33ff1af99dff79cb3661f0fe
Signed-off-by: Mingyang Sun <mingyang.sun@arm.com>
diff --git a/secure_fw/spm/cmsis_psa/spm_ipc.c b/secure_fw/spm/cmsis_psa/spm_ipc.c
index 02fa3dc..c633256 100644
--- a/secure_fw/spm/cmsis_psa/spm_ipc.c
+++ b/secure_fw/spm/cmsis_psa/spm_ipc.c
@@ -322,8 +322,15 @@
 
 struct service_t *tfm_spm_get_service_by_sid(uint32_t sid)
 {
+    uint32_t i = 0;
     struct service_t *p_serv = connection_services_listhead;
 
+    for (i = 0; i < STATIC_HANDLE_NUM_LIMIT; i++) {
+        if (stateless_services_ref_tbl[i]->p_ldinf->sid == sid) {
+            return stateless_services_ref_tbl[i];
+        }
+    }
+
     while (p_serv && p_serv->p_ldinf->sid != sid) {
         p_serv = TO_CONTAINER(BI_LIST_NEXT_NODE(&p_serv->list),
                               struct service_t, list);