diff --git a/library/pkcs5.c b/library/pkcs5.c
index 9e12434..8e55415 100644
--- a/library/pkcs5.c
+++ b/library/pkcs5.c
@@ -38,6 +38,207 @@
 #if defined(POLARSSL_PKCS5_C)
 
 #include "polarssl/pkcs5.h"
+#include "polarssl/asn1.h"
+#include "polarssl/cipher.h"
+
+#define OID_CMP(oid_str, oid_buf) \
+        ( ( OID_SIZE(oid_str) == (oid_buf)->len ) && \
+                memcmp( (oid_str), (oid_buf)->p, (oid_buf)->len) == 0)
+
+static int pkcs5_parse_pbkdf2_params( unsigned char **p,
+                                      const unsigned char *end,
+                                      asn1_buf *salt, int *iterations,
+                                      int *keylen, md_type_t *md_type )
+{
+    int ret;
+    size_t len = 0;
+    asn1_buf prf_alg_oid;
+
+    /*
+     *  PBKDF2-params ::= SEQUENCE {
+     *    salt              OCTET STRING,
+     *    iterationCount    INTEGER,
+     *    keyLength         INTEGER OPTIONAL
+     *    prf               AlgorithmIdentifier DEFAULT algid-hmacWithSHA1
+     *  }
+     *
+     */
+    if( ( ret = asn1_get_tag( p, end, &len,
+            ASN1_CONSTRUCTED | ASN1_SEQUENCE ) ) != 0 )
+    {
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+    }
+
+    end = *p + len;
+
+    if( ( ret = asn1_get_tag( p, end, &salt->len, ASN1_OCTET_STRING ) ) != 0 )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+
+    salt->p = *p;
+    *p += salt->len;
+
+    if( ( ret = asn1_get_int( p, end, iterations ) ) != 0 )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+
+    if( *p == end )
+        return( 0 );
+
+    if( ( ret = asn1_get_int( p, end, keylen ) ) != 0 )
+    {
+        if( ret != POLARSSL_ERR_ASN1_UNEXPECTED_TAG )
+            return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+    }
+
+    if( *p == end )
+        return( 0 );
+
+    if( ( ret = asn1_get_tag( p, end, &prf_alg_oid.len, ASN1_OID ) ) != 0 )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+
+    if( !OID_CMP( OID_HMAC_SHA1, &prf_alg_oid ) )
+        return( POLARSSL_ERR_PKCS5_FEATURE_UNAVAILABLE );
+
+    *md_type = POLARSSL_MD_SHA1;
+
+    if( *p != end )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT +
+                POLARSSL_ERR_ASN1_LENGTH_MISMATCH );
+
+    return( 0 );
+}
+
+int pkcs5_pbes2( asn1_buf *pbe_params, int mode,
+                 const unsigned char *pwd,  size_t pwdlen,
+                 const unsigned char *data, size_t datalen,
+                 unsigned char *output )
+{
+    int ret, iterations = 0, keylen = 0;
+    unsigned char *p, *end, *end2;
+    asn1_buf kdf_alg_oid, enc_scheme_oid, salt;
+    md_type_t md_type = POLARSSL_MD_SHA1;
+    unsigned char key[32], iv[32];
+    size_t len = 0, olen = 0;
+    const md_info_t *md_info;
+    const cipher_info_t *cipher_info;
+    md_context_t md_ctx;
+    cipher_context_t cipher_ctx;
+
+    p = pbe_params->p;
+    end = p + pbe_params->len;
+
+    /*
+     *  PBES2-params ::= SEQUENCE {
+     *    keyDerivationFunc AlgorithmIdentifier {{PBES2-KDFs}},
+     *    encryptionScheme AlgorithmIdentifier {{PBES2-Encs}}
+     *  }
+     */
+    if( ( ret = asn1_get_tag( &p, end, &len,
+            ASN1_CONSTRUCTED | ASN1_SEQUENCE ) ) != 0 )
+    {
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+    }
+
+    if( ( ret = asn1_get_tag( &p, end, &len,
+            ASN1_CONSTRUCTED | ASN1_SEQUENCE ) ) != 0 )
+    {
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+    }
+
+    end2 = p + len;
+
+    if( ( ret = asn1_get_tag( &p, end2, &kdf_alg_oid.len, ASN1_OID ) ) != 0 )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+
+    kdf_alg_oid.p = p;
+    p += kdf_alg_oid.len;
+
+    // Only PBKDF2 supported at the moment
+    //
+    if( !OID_CMP( OID_PKCS5_PBKDF2, &kdf_alg_oid ) )
+        return( POLARSSL_ERR_PKCS5_FEATURE_UNAVAILABLE );
+
+    if( ( ret = pkcs5_parse_pbkdf2_params( &p, end2,
+                                           &salt, &iterations, &keylen,
+                                           &md_type ) ) != 0 )
+    {
+        return( ret );
+    }
+
+    md_info = md_info_from_type( md_type );
+    if( md_info == NULL )
+        return( POLARSSL_ERR_PKCS5_FEATURE_UNAVAILABLE );
+
+    if( ( ret = asn1_get_tag( &p, end, &len,
+            ASN1_CONSTRUCTED | ASN1_SEQUENCE ) ) != 0 )
+    {
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+    }
+
+    end2 = p + len;
+
+    if( ( ret = asn1_get_tag( &p, end2, &enc_scheme_oid.len, ASN1_OID ) ) != 0 )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+
+    enc_scheme_oid.p = p;
+    p += enc_scheme_oid.len;
+
+#if defined(POLARSSL_DES_C)
+    // Only DES-CBC and DES-EDE3-CBC supported at the moment
+    //
+    if( OID_CMP( OID_DES_EDE3_CBC, &enc_scheme_oid ) )
+    {
+        cipher_info = cipher_info_from_type( POLARSSL_CIPHER_DES_EDE3_CBC );
+    }
+    else if( OID_CMP( OID_DES_CBC, &enc_scheme_oid ) )
+    {
+        cipher_info = cipher_info_from_type( POLARSSL_CIPHER_DES_CBC );
+    }
+    else
+#endif /* POLARSSL_DES_C */
+        return( POLARSSL_ERR_PKCS5_FEATURE_UNAVAILABLE );
+
+    if( cipher_info == NULL )
+        return( POLARSSL_ERR_PKCS5_FEATURE_UNAVAILABLE );
+
+    keylen = cipher_info->key_length / 8;
+
+    if( ( ret = asn1_get_tag( &p, end2, &len, ASN1_OCTET_STRING ) ) != 0 )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT + ret );
+
+    if( len != cipher_info->iv_size )
+        return( POLARSSL_ERR_PKCS5_INVALID_FORMAT );
+
+    memcpy( iv, p, len );
+
+    if( ( ret = md_init_ctx( &md_ctx, md_info ) ) != 0 )
+        return( ret );
+
+    if( ( ret = cipher_init_ctx( &cipher_ctx, cipher_info ) ) != 0 )
+        return( ret );
+
+    if ( ( ret = pkcs5_pbkdf2_hmac( &md_ctx, pwd, pwdlen, salt.p, salt.len,
+                                    iterations, keylen, key ) ) != 0 )
+    {
+        return( ret );
+    }
+
+    if( ( ret = cipher_setkey( &cipher_ctx, key, keylen, mode ) ) != 0 )
+        return( ret );
+
+    if( ( ret = cipher_reset( &cipher_ctx, iv ) ) != 0 )
+        return( ret );
+
+    if( ( ret = cipher_update( &cipher_ctx, data, datalen,
+                                output, &olen ) ) != 0 )
+    {
+        return( ret );
+    }
+
+    if( ( ret = cipher_finish( &cipher_ctx, output + olen, &olen ) ) != 0 )
+        return( POLARSSL_ERR_PKCS5_PASSWORD_MISMATCH );
+
+    return( 0 );
+}
 
 int pkcs5_pbkdf2_hmac( md_context_t *ctx, const unsigned char *password,
                        size_t plen, const unsigned char *salt, size_t slen,