mbedtls: improve debug output and fix AES-ICM issues

- aes_gcm: Add debug print when PSA status fails.
- aes_icm: Destroy previous key before importing new key.
- aes_icm: Add missing destination buffer argument to encrypt function.

Signed-off-by: Sayed Naser Moravej <seyednasermoravej@gmail.com>

Author: seyednasermoravej

crypto/cipher/aes_gcm_mbedtls.c

@@ -455,7 +455,9 @@ static srtp_err_status_t srtp_aes_gcm_mbedtls_decrypt(void *cv,
     *dst_len = out_len;
     c->aad_size = 0;
     if (status != PSA_SUCCESS) {
+        // debug_print(srtp_mod_aes_gcm, "mbedtls error code:  %d", status);
         return srtp_err_status_auth_fail;
     }
+
     return srtp_err_status_ok;
 }

crypto/cipher/aes_icm_mbedtls.c

@@ -310,6 +310,11 @@ static srtp_err_status_t srtp_aes_icm_mbedtls_context_init(void *cv,
 
     status = psa_crypto_init();
 
+    if (status != PSA_SUCCESS) {
+        debug_print(srtp_mod_aes_icm, "status: %d", status);
+        return srtp_err_status_cipher_fail;
+    }
+
     /*
      * set counter and initial values to 'offset' value, being careful not to
      * go past the end of the key buffer
@@ -346,6 +351,7 @@ static srtp_err_status_t srtp_aes_icm_mbedtls_context_init(void *cv,
     psa_set_key_algorithm(&attr, PSA_ALG_CTR);
 
     if (c->ctx->key_id != PSA_KEY_ID_NULL) {
+        psa_destroy_key(c->ctx->key_id);
         c->ctx->key_id = PSA_KEY_ID_NULL;
     }
 
@@ -355,6 +361,7 @@ static srtp_err_status_t srtp_aes_icm_mbedtls_context_init(void *cv,
     if (status != PSA_SUCCESS) {
         psa_destroy_key(c->ctx->key_id);
         debug_print(srtp_mod_aes_icm, "status: %d", status);
+        return srtp_err_status_cipher_fail;
     }
 
     return srtp_err_status_ok;
@@ -431,6 +438,7 @@ static srtp_err_status_t srtp_aes_icm_mbedtls_encrypt(void *cv,
 
     psa_status_t status = PSA_SUCCESS;
     size_t out_len = 0;
+    uint8_t *buffer = malloc(*dst_len);
 
     debug_print(srtp_mod_aes_icm, "rs0: %s", v128_hex_string(&c->counter));
     debug_print(srtp_mod_aes_icm, "source: %s",
@@ -440,13 +448,16 @@ static srtp_err_status_t srtp_aes_icm_mbedtls_encrypt(void *cv,
         return srtp_err_status_buffer_small;
     }
     status =
-        psa_cipher_update(&(c->ctx->op), src, src_len, dst, *dst_len, &out_len);
-
+        psa_cipher_update(&(c->ctx->op), src, src_len, buffer, *dst_len, 
+                          &out_len);
+    memcpy(dst, buffer, out_len);
+    free(buffer);
     if (status != PSA_SUCCESS) {
         debug_print(srtp_mod_aes_icm, "encrypt error: %d", status);
         psa_cipher_abort(&c->ctx->op);
         return srtp_err_status_cipher_fail;
     }
+
     *dst_len = out_len;
     debug_print(srtp_mod_aes_icm, "encrypted: %s",
                 srtp_octet_string_hex_string(dst, *dst_len));