diff --git a/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_P256_trustm.cpp b/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_P256_trustm.cpp
index afa988d3ce8cb7..895b53222672b4 100644
--- a/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_P256_trustm.cpp
+++ b/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_P256_trustm.cpp
@@ -252,15 +252,16 @@ CHIP_ERROR P256Keypair::ECDH_derive_secret(const P256PublicKey & remote_public_k
 
     return_status = trustm_ecdh_derive_secret(OPTIGA_KEY_ID_E100, (uint8_t *) remote_key, (uint16_t) rem_pubKeyLen + 3,
                                               out_secret.Bytes(), (uint8_t) secret_length);
-
     VerifyOrExit(return_status == OPTIGA_LIB_SUCCESS, error = CHIP_ERROR_INTERNAL);
+    out_secret.SetLength(secret_length);
+    error = CHIP_NO_ERROR;
 
 exit:
     if (error != CHIP_NO_ERROR)
     {
         trustm_close();
     }
-    return out_secret.SetLength(secret_length);
+    return error;
 #endif
 }
 
@@ -295,7 +296,7 @@ CHIP_ERROR P256PublicKey::ECDSA_validate_hash_signature(const uint8_t * hash, si
                                         (uint8_t *) bytes, (uint8_t) kP256_PublicKey_Length);
 
     VerifyOrExit(return_status == OPTIGA_LIB_SUCCESS, error = CHIP_ERROR_INTERNAL);
-
+    error = CHIP_NO_ERROR;
 exit:
     if (error != CHIP_NO_ERROR)
     {
@@ -407,7 +408,7 @@ CHIP_ERROR P256PublicKey::ECDSA_validate_msg_signature(const uint8_t * msg, size
                                         (uint8_t *) bytes, (uint8_t) kP256_PublicKey_Length);
 
     VerifyOrExit(return_status == OPTIGA_LIB_SUCCESS, error = CHIP_ERROR_INTERNAL);
-
+    error = CHIP_NO_ERROR;
 exit:
     if (error != CHIP_NO_ERROR)
     {
diff --git a/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_utils_trustm.cpp b/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_utils_trustm.cpp
index fa0ee0b2cc389b..be25fd89b824d6 100644
--- a/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_utils_trustm.cpp
+++ b/src/platform/Infineon/crypto/trustm/CHIPCryptoPALHsm_utils_trustm.cpp
@@ -426,8 +426,9 @@ optiga_lib_status_t deriveKey_HKDF(const uint8_t * salt, uint16_t salt_length, c
             break;
         }
 
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
+
         if (OPTIGA_LIB_SUCCESS != optiga_lib_status)
         {
             // optiga_crypt_hkdf failed
@@ -539,8 +540,9 @@ optiga_lib_status_t hmac_sha256(optiga_hmac_type_t type, const uint8_t * input_d
             break;
         }
 
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
+
         if (OPTIGA_LIB_SUCCESS != optiga_lib_status)
         {
             // optiga_crypt_hkdf failed
@@ -578,8 +580,9 @@ optiga_lib_status_t optiga_crypt_rng(uint8_t * random_data, uint16_t random_data
             break;
         }
 
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
+
         if (OPTIGA_LIB_SUCCESS != optiga_lib_status)
         {
             // optiga_crypt_random failed
@@ -626,7 +629,7 @@ optiga_lib_status_t trustm_ecc_keygen(uint16_t optiga_key_id, uint8_t key_type,
             break;
         }
 
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
 
     } while (0);
@@ -696,8 +699,10 @@ optiga_lib_status_t trustm_hash(uint8_t * msg, uint16_t msg_length, uint8_t * di
             optiga_lib_print_message("optiga_crypt_hash api returns error !!!", OPTIGA_UTIL_SERVICE, OPTIGA_UTIL_SERVICE_COLOR);
             break;
         }
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
+
     } while (0);
 
     if (p_local_crypt)
@@ -729,7 +734,8 @@ optiga_lib_status_t trustm_ecdsa_sign(optiga_key_id_t optiga_key_id, uint8_t * d
                                      OPTIGA_UTIL_SERVICE_COLOR);
             break;
         }
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
 
         for (i = (*signature_length - 1); i >= 0; i--)
@@ -803,8 +809,10 @@ optiga_lib_status_t trustm_ecdsa_verify(uint8_t * digest, uint8_t digest_length,
                                      OPTIGA_UTIL_SERVICE_COLOR);
             break;
         }
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
+
     } while (0);
 
     if (p_local_crypt)
@@ -852,14 +860,6 @@ CHIP_ERROR trustmGetCertificate(uint16_t optiga_oid, uint8_t * buf, uint16_t * b
 
         memcpy(buf, ifx_cert_hex, ifx_cert_hex_len);
         *buflen = ifx_cert_hex_len;
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
-            ;
-        if (OPTIGA_LIB_SUCCESS != optiga_lib_status)
-        {
-            // optiga_util_read_data failed
-            optiga_lib_print_message("optiga_util_read_data failed", OPTIGA_UTIL_SERVICE, OPTIGA_UTIL_SERVICE_COLOR);
-            break;
-        }
     } while (0);
 
     if (p_local_util)
@@ -896,8 +896,10 @@ optiga_lib_status_t trustm_ecdh_derive_secret(optiga_key_id_t optiga_key_id, uin
             optiga_lib_print_message("optiga_crypt_ecdh api returns error !!!", OPTIGA_UTIL_SERVICE, OPTIGA_UTIL_SERVICE_COLOR);
             break;
         }
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
+
     } while (0);
 
     if (p_local_crypt)
@@ -957,7 +959,7 @@ optiga_lib_status_t trustm_PBKDF2_HMAC(const unsigned char * salt, size_t slen,
             }
         }
 
-        while (optiga_lib_status == OPTIGA_LIB_BUSY)
+        while (p_local_crypt->instance_state != OPTIGA_LIB_INSTANCE_FREE)
             ;
 
         if (OPTIGA_LIB_SUCCESS != optiga_lib_status)
@@ -978,4 +980,4 @@ optiga_lib_status_t trustm_PBKDF2_HMAC(const unsigned char * salt, size_t slen,
         optiga_crypt_destroy(p_local_crypt);
     }
     return return_status;
-}
\ No newline at end of file
+}