diff --git a/library/rsa.c b/library/rsa.c
index 8c0d8c3..9e4a213 100644
--- a/library/rsa.c
+++ b/library/rsa.c
@@ -1523,6 +1523,112 @@
  * Implementation of the PKCS#1 v2.1 RSASSA-PKCS1-V1_5-SIGN function
  */
 
+/* Encode a hash into a DigestInfo structure as specified by PKCS#1
+ * (RFC 8017, EMSA-PKCS1-v1_5-ENCODE step 2).
+ * Write to the left of p and set *p to the leftmost byte written. */
+static int rsa_emsa_pkcs1_v15_encode_digestinfo( unsigned char **p,
+                                                 unsigned char *start,
+                                                 mbedtls_md_type_t md_alg,
+                                                 const unsigned char *hash,
+                                                 size_t hashlen )
+{
+    const mbedtls_md_info_t *md_info;
+    const char *oid;
+    size_t oid_size;
+
+    if( md_alg == MBEDTLS_MD_NONE )
+    {
+        if( *p < start + hashlen )
+            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+        *p -= hashlen;
+        memcpy( *p, hash, hashlen );
+        return( 0 );
+    }
+
+    md_info = mbedtls_md_info_from_type( md_alg );
+    if( md_info == NULL )
+        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+    if( hashlen == 0 )
+        hashlen = mbedtls_md_get_size( md_info );
+    else if ( hashlen != mbedtls_md_get_size( md_info ) )
+        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+    if( mbedtls_oid_get_oid_by_md( md_alg, &oid, &oid_size ) != 0 )
+        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+
+    /* Double-check that 8 + hashlen + oid_size can be used as a
+     * 1-byte ASN.1 length encoding and that there's no overflow. */
+    if( 8 + hashlen + oid_size  >= 0x80         ||
+        10 + hashlen            <  hashlen      ||
+        10 + hashlen + oid_size <  10 + hashlen )
+        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+
+    /*
+     * Static bounds check:
+     * - Need 10 bytes for five tag-length pairs.
+     *   (Insist on 1-byte length encodings to protect against variants of
+     *    Bleichenbacher's forgery attack against lax PKCS#1v1.5 verification)
+     * - Need hashlen bytes for hash
+     * - Need oid_size bytes for hash alg OID.
+     */
+    if( *p < start + 10 + oid_size + hashlen )
+        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+    *p -= 10 + oid_size + hashlen;
+    start = *p;
+
+    /* Signing hashed data, add corresponding ASN.1 structure
+     *
+     * DigestInfo ::= SEQUENCE {
+     *   digestAlgorithm DigestAlgorithmIdentifier,
+     *   digest Digest }
+     * DigestAlgorithmIdentifier ::= AlgorithmIdentifier
+     * Digest ::= OCTET STRING
+     *
+     * Schematic:
+     * TAG-SEQ + LEN [ TAG-SEQ + LEN [ TAG-OID  + LEN [ OID  ]
+     *                                 TAG-NULL + LEN [ NULL ] ]
+     *                 TAG-OCTET + LEN [ HASH ] ]
+     */
+    *start++ = MBEDTLS_ASN1_SEQUENCE | MBEDTLS_ASN1_CONSTRUCTED;
+    *start++ = (unsigned char)( 0x08 + oid_size + hashlen );
+    *start++ = MBEDTLS_ASN1_SEQUENCE | MBEDTLS_ASN1_CONSTRUCTED;
+    *start++ = (unsigned char)( 0x04 + oid_size );
+    *start++ = MBEDTLS_ASN1_OID;
+    *start++ = (unsigned char) oid_size;
+    memcpy( start, oid, oid_size );
+    start += oid_size;
+    *start++ = MBEDTLS_ASN1_NULL;
+    *start++ = 0x00;
+    *start++ = MBEDTLS_ASN1_OCTET_STRING;
+    *start++ = (unsigned char) hashlen;
+    memcpy( start, hash, hashlen );
+    start += hashlen;
+    return( 0 );
+}
+
+/* Pad data as specified by PKCS#1
+ * (RFC 8017, EMSA-PKCS1-v1_5-ENCODE steps 3-5 without T). */
+static int rsa_rsassa_pkcs1_v15_encode_pad( unsigned char *p,
+                                            size_t nb_pad )
+{
+    /* Need space for signature header and padding delimiter (3 bytes),
+     * and 8 bytes for the minimal padding */
+    if( nb_pad < 3 + 8 )
+        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+    nb_pad -= 3;
+
+    /* Now nb_pad is the amount of memory to be filled
+     * with padding, and at least 8 bytes long. */
+
+    /* Write signature header and padding */
+    *p++ = 0;
+    *p++ = MBEDTLS_RSA_SIGN;
+    memset( p, 0xFF, nb_pad );
+    p += nb_pad;
+    *p++ = 0;
+
+    return( 0 );
+}
+
 /* Construct a PKCS v1.5 encoding of a hashed message
  *
  * This is used both for signature generation and verification.
@@ -1547,107 +1653,24 @@
                                         size_t dst_len,
                                         unsigned char *dst )
 {
-    size_t oid_size  = 0;
-    size_t nb_pad    = dst_len;
-    unsigned char *p = dst;
-    const char *oid  = NULL;
+    int ret;
+    unsigned char *p = dst + dst_len;
 
-    /* Are we signing hashed or raw data? */
+    /* Ignore hashlen if a hash algorithm is specified. This is
+     * fragile, but documented, bhavior. */
     if( md_alg != MBEDTLS_MD_NONE )
+        hashlen = 0;
+
+    ret = rsa_emsa_pkcs1_v15_encode_digestinfo( &p, dst,
+                                                md_alg, hash, hashlen );
+    if( ret != 0 )
+        return( ret );
+
+    ret = rsa_rsassa_pkcs1_v15_encode_pad( dst, p - dst );
+    if( ret != 0 )
     {
-        const mbedtls_md_info_t *md_info = mbedtls_md_info_from_type( md_alg );
-        if( md_info == NULL )
-            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
-
-        if( mbedtls_oid_get_oid_by_md( md_alg, &oid, &oid_size ) != 0 )
-            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
-
-        hashlen = mbedtls_md_get_size( md_info );
-
-        /* Double-check that 8 + hashlen + oid_size can be used as a
-         * 1-byte ASN.1 length encoding and that there's no overflow. */
-        if( 8 + hashlen + oid_size  >= 0x80         ||
-            10 + hashlen            <  hashlen      ||
-            10 + hashlen + oid_size <  10 + hashlen )
-            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
-
-        /*
-         * Static bounds check:
-         * - Need 10 bytes for five tag-length pairs.
-         *   (Insist on 1-byte length encodings to protect against variants of
-         *    Bleichenbacher's forgery attack against lax PKCS#1v1.5 verification)
-         * - Need hashlen bytes for hash
-         * - Need oid_size bytes for hash alg OID.
-         */
-        if( nb_pad < 10 + hashlen + oid_size )
-            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
-        nb_pad -= 10 + hashlen + oid_size;
-    }
-    else
-    {
-        if( nb_pad < hashlen )
-            return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
-
-        nb_pad -= hashlen;
-    }
-
-    /* Need space for signature header and padding delimiter (3 bytes),
-     * and 8 bytes for the minimal padding */
-    if( nb_pad < 3 + 8 )
-        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
-    nb_pad -= 3;
-
-    /* Now nb_pad is the amount of memory to be filled
-     * with padding, and at least 8 bytes long. */
-
-    /* Write signature header and padding */
-    *p++ = 0;
-    *p++ = MBEDTLS_RSA_SIGN;
-    memset( p, 0xFF, nb_pad );
-    p += nb_pad;
-    *p++ = 0;
-
-    /* Are we signing raw data? */
-    if( md_alg == MBEDTLS_MD_NONE )
-    {
-        memcpy( p, hash, hashlen );
-        return( 0 );
-    }
-
-    /* Signing hashed data, add corresponding ASN.1 structure
-     *
-     * DigestInfo ::= SEQUENCE {
-     *   digestAlgorithm DigestAlgorithmIdentifier,
-     *   digest Digest }
-     * DigestAlgorithmIdentifier ::= AlgorithmIdentifier
-     * Digest ::= OCTET STRING
-     *
-     * Schematic:
-     * TAG-SEQ + LEN [ TAG-SEQ + LEN [ TAG-OID  + LEN [ OID  ]
-     *                                 TAG-NULL + LEN [ NULL ] ]
-     *                 TAG-OCTET + LEN [ HASH ] ]
-     */
-    *p++ = MBEDTLS_ASN1_SEQUENCE | MBEDTLS_ASN1_CONSTRUCTED;
-    *p++ = (unsigned char)( 0x08 + oid_size + hashlen );
-    *p++ = MBEDTLS_ASN1_SEQUENCE | MBEDTLS_ASN1_CONSTRUCTED;
-    *p++ = (unsigned char)( 0x04 + oid_size );
-    *p++ = MBEDTLS_ASN1_OID;
-    *p++ = (unsigned char) oid_size;
-    memcpy( p, oid, oid_size );
-    p += oid_size;
-    *p++ = MBEDTLS_ASN1_NULL;
-    *p++ = 0x00;
-    *p++ = MBEDTLS_ASN1_OCTET_STRING;
-    *p++ = (unsigned char) hashlen;
-    memcpy( p, hash, hashlen );
-    p += hashlen;
-
-    /* Just a sanity-check, should be automatic
-     * after the initial bounds check. */
-    if( p != dst + dst_len )
-    {
-        mbedtls_zeroize( dst, dst_len );
-        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+        memset( dst, 0, dst_len );
+        return( ret );
     }
 
     return( 0 );