diff --git a/library/pkcs5.c b/library/pkcs5.c
index 2b014d9..1d43a3a 100644
--- a/library/pkcs5.c
+++ b/library/pkcs5.c
@@ -49,6 +49,9 @@
 #define mbedtls_printf printf
 #endif
 
+#include "hash_info.h"
+#include "mbedtls/psa_util.h"
+
 #if defined(MBEDTLS_ASN1_PARSE_C)
 static int pkcs5_parse_pbkdf2_params( const mbedtls_asn1_buf *params,
                                       mbedtls_asn1_buf *salt, int *iterations,
@@ -118,9 +121,7 @@
     mbedtls_md_type_t md_type = MBEDTLS_MD_SHA1;
     unsigned char key[32], iv[32];
     size_t olen = 0;
-    const mbedtls_md_info_t *md_info;
     const mbedtls_cipher_info_t *cipher_info;
-    mbedtls_md_context_t md_ctx;
     mbedtls_cipher_type_t cipher_alg;
     mbedtls_cipher_context_t cipher_ctx;
 
@@ -153,10 +154,6 @@
         return( ret );
     }
 
-    md_info = mbedtls_md_info_from_type( md_type );
-    if( md_info == NULL )
-        return( MBEDTLS_ERR_PKCS5_FEATURE_UNAVAILABLE );
-
     if( ( ret = mbedtls_asn1_get_alg( &p, end, &enc_scheme_oid,
                               &enc_scheme_params ) ) != 0 )
     {
@@ -182,16 +179,13 @@
         return( MBEDTLS_ERR_PKCS5_INVALID_FORMAT );
     }
 
-    mbedtls_md_init( &md_ctx );
     mbedtls_cipher_init( &cipher_ctx );
 
     memcpy( iv, enc_scheme_params.p, enc_scheme_params.len );
 
-    if( ( ret = mbedtls_md_setup( &md_ctx, md_info, 1 ) ) != 0 )
-        goto exit;
-
-    if( ( ret = mbedtls_pkcs5_pbkdf2_hmac( &md_ctx, pwd, pwdlen, salt.p, salt.len,
-                                   iterations, keylen, key ) ) != 0 )
+    if( ( ret = mbedtls_pkcs5_pbkdf2_hmac_ext( md_type, pwd, pwdlen, salt.p,
+                                               salt.len, iterations, keylen,
+                                               key ) ) != 0 )
     {
         goto exit;
     }
@@ -208,7 +202,6 @@
         ret = MBEDTLS_ERR_PKCS5_PASSWORD_MISMATCH;
 
 exit:
-    mbedtls_md_free( &md_ctx );
     mbedtls_cipher_free( &cipher_ctx );
 
     return( ret );
@@ -297,6 +290,134 @@
     return( ret );
 }
 
+int mbedtls_pkcs5_pbkdf2_hmac_ext( mbedtls_md_type_t md_alg,
+                       const unsigned char *password,
+                       size_t plen, const unsigned char *salt, size_t slen,
+                       unsigned int iteration_count,
+                       uint32_t key_length, unsigned char *output )
+{
+#if defined(MBEDTLS_MD_C)
+    mbedtls_md_context_t md_ctx;
+    const mbedtls_md_info_t *md_info;
+    int ret;
+
+    mbedtls_md_init( &md_ctx );
+
+    md_info = mbedtls_md_info_from_type( md_alg );
+    if( md_info == NULL )
+        return( MBEDTLS_ERR_PKCS5_FEATURE_UNAVAILABLE );
+
+    if( ( ret = mbedtls_md_setup( &md_ctx, md_info, 1 ) ) != 0 )
+        goto exit;
+    ret = mbedtls_pkcs5_pbkdf2_hmac( &md_ctx, password, plen, salt, slen,
+                                     iteration_count, key_length, output );
+exit:
+    mbedtls_md_free( &md_ctx );
+    return( ret );
+#else
+    int ret = MBEDTLS_ERR_ERROR_CORRUPTION_DETECTED;
+    int j;
+    unsigned int i;
+    unsigned char md1[PSA_HASH_MAX_SIZE];
+    unsigned char work[PSA_HASH_MAX_SIZE];
+    unsigned char md_size = mbedtls_hash_info_get_size( md_alg );
+    psa_mac_operation_t operation = PSA_MAC_OPERATION_INIT;
+
+    psa_status_t status = PSA_ERROR_CORRUPTION_DETECTED;
+    size_t use_len, out_len, out_size;
+    unsigned char *out_p = output;
+    unsigned char counter[4];
+    mbedtls_svc_key_id_t psa_hmac_key;
+    psa_key_attributes_t attributes = PSA_KEY_ATTRIBUTES_INIT;
+
+
+    memset( counter, 0, 4 );
+    counter[3] = 1;
+    psa_algorithm_t alg = PSA_ALG_HMAC( mbedtls_hash_info_psa_from_md( md_alg ) );
+    out_size = PSA_MAC_LENGTH( PSA_KEY_TYPE_HMAC, 0, alg );
+    psa_set_key_usage_flags( &attributes, PSA_KEY_USAGE_SIGN_MESSAGE );
+    psa_set_key_algorithm( &attributes,  alg );
+    psa_set_key_type( &attributes, PSA_KEY_TYPE_HMAC );
+
+    if( key_length == 0 )
+        return 0;
+    if( ( status = psa_import_key( &attributes,
+                                   password, plen,
+                                   &psa_hmac_key ) ) != PSA_SUCCESS )
+    {
+        return MBEDTLS_ERR_ERROR_GENERIC_ERROR;
+    }
+
+#if UINT_MAX > 0xFFFFFFFF
+    if( iteration_count > 0xFFFFFFFF )
+        return( MBEDTLS_ERR_PKCS5_BAD_INPUT_DATA );
+#endif
+
+    while( key_length )
+    {
+        status = psa_mac_sign_setup( &operation, psa_hmac_key,
+            PSA_ALG_HMAC( alg ) );
+        if( status != PSA_SUCCESS )
+            goto cleanup;
+        // U1 ends up in work
+        if( ( status = psa_mac_update( &operation, salt, slen ) ) != PSA_SUCCESS )
+            goto cleanup;
+
+        if( ( status = psa_mac_update( &operation, counter, 4 ) ) != PSA_SUCCESS )
+            goto cleanup;
+
+        if( ( status = psa_mac_sign_finish( &operation, work, out_size, &out_len ) )
+            != PSA_SUCCESS )
+            goto cleanup;
+
+        memcpy( md1, work, md_size );
+
+        for( i = 1; i < iteration_count; i++ )
+        {
+            // U2 ends up in md1
+            //
+            status = psa_mac_sign_setup( &operation, psa_hmac_key,
+                PSA_ALG_HMAC( alg ) );
+            if( status != PSA_SUCCESS )
+                goto cleanup;
+            if( ( status = psa_mac_update( &operation, md1, md_size ) ) != PSA_SUCCESS )
+                goto cleanup;
+
+            if( ( status = psa_mac_sign_finish( &operation, md1, out_size, &out_len ) ) != PSA_SUCCESS )
+                goto cleanup;
+
+
+            // U1 xor U2
+            //
+            for( j = 0; j < md_size; j++ )
+                work[j] ^= md1[j];
+        }
+
+        use_len = ( key_length < md_size ) ? key_length : md_size;
+        memcpy( out_p, work, use_len );
+
+        key_length -= (uint32_t) use_len;
+        out_p += use_len;
+
+        for( i = 4; i > 0; i-- )
+            if( ++counter[i - 1] != 0 )
+                break;
+    }
+
+cleanup:
+    /* Zeroise buffers to clear sensitive data from memory. */
+    mbedtls_platform_zeroize( work, PSA_HASH_MAX_SIZE );
+    mbedtls_platform_zeroize( md1, PSA_HASH_MAX_SIZE );
+    psa_destroy_key( psa_hmac_key );
+    ret = (status != PSA_SUCCESS? MBEDTLS_ERR_ERROR_GENERIC_ERROR: 0);
+    status = psa_mac_abort( &operation );
+    if( ret == 0 && status != PSA_SUCCESS )
+        ret = MBEDTLS_ERR_ERROR_GENERIC_ERROR;
+
+    return ( ret );
+#endif
+}
+
 #if defined(MBEDTLS_SELF_TEST)
 
 #if !defined(MBEDTLS_SHA1_C)
@@ -362,32 +483,15 @@
 
 int mbedtls_pkcs5_self_test( int verbose )
 {
-    mbedtls_md_context_t sha1_ctx;
-    const mbedtls_md_info_t *info_sha1;
     int ret, i;
     unsigned char key[64];
 
-    mbedtls_md_init( &sha1_ctx );
-
-    info_sha1 = mbedtls_md_info_from_type( MBEDTLS_MD_SHA1 );
-    if( info_sha1 == NULL )
-    {
-        ret = 1;
-        goto exit;
-    }
-
-    if( ( ret = mbedtls_md_setup( &sha1_ctx, info_sha1, 1 ) ) != 0 )
-    {
-        ret = 1;
-        goto exit;
-    }
-
     for( i = 0; i < MAX_TESTS; i++ )
     {
         if( verbose != 0 )
             mbedtls_printf( "  PBKDF2 (SHA1) #%d: ", i );
 
-        ret = mbedtls_pkcs5_pbkdf2_hmac( &sha1_ctx, password_test_data[i],
+        ret = mbedtls_pkcs5_pbkdf2_hmac_ext( MBEDTLS_MD_SHA1, password_test_data[i],
                                          plen_test_data[i], salt_test_data[i],
                                          slen_test_data[i], it_cnt_test_data[i],
                                          key_len_test_data[i], key );
@@ -409,8 +513,6 @@
         mbedtls_printf( "\n" );
 
 exit:
-    mbedtls_md_free( &sha1_ctx );
-
     return( ret );
 }
 #endif /* MBEDTLS_SHA1_C */