diff --git a/features/mbedtls/VERSION.txt b/features/mbedtls/VERSION.txt
index 1dc8d39870..bc9d7e9d2c 100644
--- a/features/mbedtls/VERSION.txt
+++ b/features/mbedtls/VERSION.txt
@@ -1 +1 @@
-a592dcc1c6277bb191269e709cdd3d5593e593ed
+8e004104020dd4328434e8a207245b0327bbb9b1
diff --git a/features/mbedtls/inc/mbedtls/cmac.h b/features/mbedtls/inc/mbedtls/cmac.h
index f64ae69b47..75e0b97c4a 100644
--- a/features/mbedtls/inc/mbedtls/cmac.h
+++ b/features/mbedtls/inc/mbedtls/cmac.h
@@ -44,7 +44,6 @@ extern "C" {
  */
 struct mbedtls_cmac_context_t
 {
-
     /** Internal state of the CMAC algorithm  */
     unsigned char       state[MBEDTLS_CIPHER_BLKSIZE_MAX];
 
@@ -54,9 +53,6 @@ struct mbedtls_cmac_context_t
 
     /** Length of data pending to be processed */
     size_t              unprocessed_len;
-
-    /** Flag to indicate if the last block needs padding */
-    int                 padding_flag;
 };
 
 /**
diff --git a/features/mbedtls/inc/mbedtls/config.h b/features/mbedtls/inc/mbedtls/config.h
index e9b862a2b3..40fcf85b94 100644
--- a/features/mbedtls/inc/mbedtls/config.h
+++ b/features/mbedtls/inc/mbedtls/config.h
@@ -955,18 +955,6 @@
  */
 //#define MBEDTLS_SHA256_SMALLER
 
-/**
- * \def MBEDTLS_SSL_AEAD_RANDOM_IV
- *
- * Generate a random IV rather than using the record sequence number as a
- * nonce for ciphersuites using and AEAD algorithm (GCM or CCM).
- *
- * Using the sequence number is generally recommended.
- *
- * Uncomment this macro to always use random IVs with AEAD ciphersuites.
- */
-//#define MBEDTLS_SSL_AEAD_RANDOM_IV
-
 /**
  * \def MBEDTLS_SSL_ALL_ALERT_MESSAGES
  *
diff --git a/features/mbedtls/inc/mbedtls/gcm.h b/features/mbedtls/inc/mbedtls/gcm.h
index 6743ac9a5f..1b77aaedd4 100644
--- a/features/mbedtls/inc/mbedtls/gcm.h
+++ b/features/mbedtls/inc/mbedtls/gcm.h
@@ -190,8 +190,8 @@ int mbedtls_gcm_update( mbedtls_gcm_context *ctx,
  *                  16 bytes.
  *
  * \param ctx       GCM context
- * \param tag       buffer for holding the tag (may be NULL if tag_len is 0)
- * \param tag_len   length of the tag to generate
+ * \param tag       buffer for holding the tag
+ * \param tag_len   length of the tag to generate (must be at least 4)
  *
  * \return          0 if successful or MBEDTLS_ERR_GCM_BAD_INPUT
  */
diff --git a/features/mbedtls/inc/mbedtls/ssl.h b/features/mbedtls/inc/mbedtls/ssl.h
index 1c0513da7f..ba499d2bde 100644
--- a/features/mbedtls/inc/mbedtls/ssl.h
+++ b/features/mbedtls/inc/mbedtls/ssl.h
@@ -107,6 +107,8 @@
 #define MBEDTLS_ERR_SSL_TIMEOUT                           -0x6800  /**< The operation timed out. */
 #define MBEDTLS_ERR_SSL_CLIENT_RECONNECT                  -0x6780  /**< The client initiated a reconnect from the same port. */
 #define MBEDTLS_ERR_SSL_UNEXPECTED_RECORD                 -0x6700  /**< Record header looks valid but is not expected. */
+#define MBEDTLS_ERR_SSL_NON_FATAL                         -0x6680  /**< The alert message received indicates a non-fatal error. */
+#define MBEDTLS_ERR_SSL_INVALID_VERIFY_HASH               -0x6600  /**< Couldn't set the hash for verifying CertificateVerify */
 
 /*
  * Various constants
diff --git a/features/mbedtls/inc/mbedtls/ssl_internal.h b/features/mbedtls/inc/mbedtls/ssl_internal.h
index d63d7d4e7e..668c0f567c 100644
--- a/features/mbedtls/inc/mbedtls/ssl_internal.h
+++ b/features/mbedtls/inc/mbedtls/ssl_internal.h
@@ -355,6 +355,11 @@ int mbedtls_ssl_send_fatal_handshake_failure( mbedtls_ssl_context *ssl );
 void mbedtls_ssl_reset_checksum( mbedtls_ssl_context *ssl );
 int mbedtls_ssl_derive_keys( mbedtls_ssl_context *ssl );
 
+int mbedtls_ssl_read_record_layer( mbedtls_ssl_context *ssl );
+int mbedtls_ssl_handle_message_type( mbedtls_ssl_context *ssl );
+int mbedtls_ssl_prepare_handshake_record( mbedtls_ssl_context *ssl );
+void mbedtls_ssl_update_handshake_status( mbedtls_ssl_context *ssl );
+
 int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl );
 int mbedtls_ssl_fetch_input( mbedtls_ssl_context *ssl, size_t nb_want );
 
@@ -384,6 +389,7 @@ mbedtls_pk_type_t mbedtls_ssl_pk_alg_from_sig( unsigned char sig );
 
 mbedtls_md_type_t mbedtls_ssl_md_alg_from_hash( unsigned char hash );
 unsigned char mbedtls_ssl_hash_from_md_alg( int md );
+int mbedtls_ssl_set_calc_verify_md( mbedtls_ssl_context *ssl, int md );
 
 #if defined(MBEDTLS_ECP_C)
 int mbedtls_ssl_check_curve( const mbedtls_ssl_context *ssl, mbedtls_ecp_group_id grp_id );
diff --git a/features/mbedtls/inc/mbedtls/x509_csr.h b/features/mbedtls/inc/mbedtls/x509_csr.h
index 7a9c2e0550..fe9843cb54 100644
--- a/features/mbedtls/inc/mbedtls/x509_csr.h
+++ b/features/mbedtls/inc/mbedtls/x509_csr.h
@@ -282,7 +282,7 @@ int mbedtls_x509write_csr_der( mbedtls_x509write_csr *ctx, unsigned char *buf, s
  *
  * \note            f_rng may be NULL if RSA is used for signature and the
  *                  signature is made offline (otherwise f_rng is desirable
- *                  for couermeasures against timing attacks).
+ *                  for countermeasures against timing attacks).
  *                  ECDSA signatures always require a non-NULL f_rng.
  */
 int mbedtls_x509write_csr_pem( mbedtls_x509write_csr *ctx, unsigned char *buf, size_t size,
diff --git a/features/mbedtls/src/asn1parse.c b/features/mbedtls/src/asn1parse.c
index ffa2f5299a..4dd65c03c0 100644
--- a/features/mbedtls/src/asn1parse.c
+++ b/features/mbedtls/src/asn1parse.c
@@ -153,7 +153,7 @@ int mbedtls_asn1_get_int( unsigned char **p,
     if( ( ret = mbedtls_asn1_get_tag( p, end, &len, MBEDTLS_ASN1_INTEGER ) ) != 0 )
         return( ret );
 
-    if( len > sizeof( int ) || ( **p & 0x80 ) != 0 )
+    if( len == 0 || len > sizeof( int ) || ( **p & 0x80 ) != 0 )
         return( MBEDTLS_ERR_ASN1_INVALID_LENGTH );
 
     *val = 0;
diff --git a/features/mbedtls/src/cmac.c b/features/mbedtls/src/cmac.c
index 03d939278a..ee2fe056ce 100644
--- a/features/mbedtls/src/cmac.c
+++ b/features/mbedtls/src/cmac.c
@@ -235,7 +235,6 @@ int mbedtls_cipher_cmac_starts( mbedtls_cipher_context_t *ctx,
     ctx->cmac_ctx = cmac_ctx;
 
     mbedtls_zeroize( cmac_ctx->state, sizeof( cmac_ctx->state ) );
-    cmac_ctx->padding_flag = 1;
 
     return 0;
 }
@@ -256,8 +255,8 @@ int mbedtls_cipher_cmac_update( mbedtls_cipher_context_t *ctx,
     block_size = ctx->cipher_info->block_size;
     state = ctx->cmac_ctx->state;
 
-    /* Is their data still to process from the last call, that's equal to
-     * or greater than a block? */
+    /* Is there data still to process from the last call, that's greater in
+     * size than a block? */
     if( cmac_ctx->unprocessed_len > 0 &&
         ilen > block_size - cmac_ctx->unprocessed_len )
     {
@@ -273,9 +272,8 @@ int mbedtls_cipher_cmac_update( mbedtls_cipher_context_t *ctx,
            goto exit;
         }
 
-        ilen -= block_size;
-        input += cmac_ctx->unprocessed_len;
-
+        input += block_size - cmac_ctx->unprocessed_len;
+        ilen -= block_size - cmac_ctx->unprocessed_len;
         cmac_ctx->unprocessed_len = 0;
     }
 
@@ -293,20 +291,15 @@ int mbedtls_cipher_cmac_update( mbedtls_cipher_context_t *ctx,
 
         ilen -= block_size;
         input += block_size;
-
-        cmac_ctx->padding_flag = 0;
     }
 
     /* If there is data left over that wasn't aligned to a block */
     if( ilen > 0 )
     {
-        memcpy( &cmac_ctx->unprocessed_block, input, ilen );
-        cmac_ctx->unprocessed_len = ilen;
-
-        if( ilen % block_size > 0 )
-            cmac_ctx->padding_flag = 1;
-        else
-            cmac_ctx->padding_flag = 0;
+        memcpy( &cmac_ctx->unprocessed_block[cmac_ctx->unprocessed_len],
+                input,
+                ilen );
+        cmac_ctx->unprocessed_len += ilen;
     }
 
 exit:
@@ -339,7 +332,7 @@ int mbedtls_cipher_cmac_finish( mbedtls_cipher_context_t *ctx,
     last_block = cmac_ctx->unprocessed_block;
 
     /* Calculate last block */
-    if( cmac_ctx->padding_flag )
+    if( cmac_ctx->unprocessed_len < block_size )
     {
         cmac_pad( M_last, block_size, last_block, cmac_ctx->unprocessed_len );
         cmac_xor_block( M_last, M_last, K2, block_size );
@@ -366,7 +359,6 @@ exit:
     mbedtls_zeroize( K1, sizeof( K1 ) );
     mbedtls_zeroize( K2, sizeof( K2 ) );
 
-    cmac_ctx->padding_flag = 1;
     cmac_ctx->unprocessed_len = 0;
     mbedtls_zeroize( cmac_ctx->unprocessed_block,
                      sizeof( cmac_ctx->unprocessed_block ) );
@@ -390,7 +382,6 @@ int mbedtls_cipher_cmac_reset( mbedtls_cipher_context_t *ctx )
                      sizeof( cmac_ctx->unprocessed_block ) );
     mbedtls_zeroize( cmac_ctx->state,
                      sizeof( cmac_ctx->state ) );
-    cmac_ctx->padding_flag = 1;
 
     return( 0 );
 }
@@ -746,19 +737,19 @@ static int cmac_test_subkeys( int verbose,
         return( MBEDTLS_ERR_CIPHER_FEATURE_UNAVAILABLE );
     }
 
-    mbedtls_cipher_init( &ctx );
-
     for( i = 0; i < num_tests; i++ )
     {
         if( verbose != 0 )
             mbedtls_printf( "  %s CMAC subkey #%u: ", testname, i + 1 );
 
+        mbedtls_cipher_init( &ctx );
+
         if( ( ret = mbedtls_cipher_setup( &ctx, cipher_info ) ) != 0 )
         {
             if( verbose != 0 )
                 mbedtls_printf( "test execution failed\n" );
 
-            goto exit;
+            goto cleanup;
         }
 
         if( ( ret = mbedtls_cipher_setkey( &ctx, key, keybits,
@@ -767,7 +758,7 @@ static int cmac_test_subkeys( int verbose,
             if( verbose != 0 )
                 mbedtls_printf( "test execution failed\n" );
 
-            goto exit;
+            goto cleanup;
         }
 
         ret = cmac_generate_subkeys( &ctx, K1, K2 );
@@ -775,24 +766,31 @@ static int cmac_test_subkeys( int verbose,
         {
            if( verbose != 0 )
                 mbedtls_printf( "failed\n" );
-            goto exit;
+
+            goto cleanup;
         }
 
-        if( ( ret = memcmp( K1, subkeys, block_size ) != 0 ) ||
-            ( ret = memcmp( K2, &subkeys[block_size], block_size ) != 0 ) )
+        if( ( ret = memcmp( K1, subkeys, block_size ) ) != 0  ||
+            ( ret = memcmp( K2, &subkeys[block_size], block_size ) ) != 0 )
         {
             if( verbose != 0 )
                 mbedtls_printf( "failed\n" );
-            goto exit;
+
+            goto cleanup;
         }
 
         if( verbose != 0 )
             mbedtls_printf( "passed\n" );
+
+        mbedtls_cipher_free( &ctx );
     }
 
-exit:
+    goto exit;
+
+cleanup:
     mbedtls_cipher_free( &ctx );
 
+exit:
     return( ret );
 }
 
@@ -889,7 +887,7 @@ int mbedtls_cmac_self_test( int verbose )
                                    (const unsigned char*)aes_128_subkeys,
                                    MBEDTLS_CIPHER_AES_128_ECB,
                                    MBEDTLS_AES_BLOCK_SIZE,
-                                   NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                   NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -903,7 +901,7 @@ int mbedtls_cmac_self_test( int verbose )
                                       (const unsigned char*)aes_128_expected_result,
                                       MBEDTLS_CIPHER_AES_128_ECB,
                                       MBEDTLS_AES_BLOCK_SIZE,
-                                      NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                      NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -916,7 +914,7 @@ int mbedtls_cmac_self_test( int verbose )
                                    (const unsigned char*)aes_192_subkeys,
                                    MBEDTLS_CIPHER_AES_192_ECB,
                                    MBEDTLS_AES_BLOCK_SIZE,
-                                   NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                   NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -930,7 +928,7 @@ int mbedtls_cmac_self_test( int verbose )
                                       (const unsigned char*)aes_192_expected_result,
                                       MBEDTLS_CIPHER_AES_192_ECB,
                                       MBEDTLS_AES_BLOCK_SIZE,
-                                      NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                      NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -943,7 +941,7 @@ int mbedtls_cmac_self_test( int verbose )
                                    (const unsigned char*)aes_256_subkeys,
                                    MBEDTLS_CIPHER_AES_256_ECB,
                                    MBEDTLS_AES_BLOCK_SIZE,
-                                   NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                   NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -957,7 +955,7 @@ int mbedtls_cmac_self_test( int verbose )
                                        (const unsigned char*)aes_256_expected_result,
                                        MBEDTLS_CIPHER_AES_256_ECB,
                                        MBEDTLS_AES_BLOCK_SIZE,
-                                       NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                       NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -972,7 +970,7 @@ int mbedtls_cmac_self_test( int verbose )
                                    (const unsigned char*)des3_2key_subkeys,
                                    MBEDTLS_CIPHER_DES_EDE3_ECB,
                                    MBEDTLS_DES3_BLOCK_SIZE,
-                                   NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                   NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -986,7 +984,7 @@ int mbedtls_cmac_self_test( int verbose )
                                       (const unsigned char*)des3_2key_expected_result,
                                       MBEDTLS_CIPHER_DES_EDE3_ECB,
                                       MBEDTLS_DES3_BLOCK_SIZE,
-                                      NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                      NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -999,7 +997,7 @@ int mbedtls_cmac_self_test( int verbose )
                                    (const unsigned char*)des3_3key_subkeys,
                                    MBEDTLS_CIPHER_DES_EDE3_ECB,
                                    MBEDTLS_DES3_BLOCK_SIZE,
-                                   NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                   NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
@@ -1013,14 +1011,14 @@ int mbedtls_cmac_self_test( int verbose )
                                       (const unsigned char*)des3_3key_expected_result,
                                       MBEDTLS_CIPHER_DES_EDE3_ECB,
                                       MBEDTLS_DES3_BLOCK_SIZE,
-                                      NB_CMAC_TESTS_PER_KEY ) != 0 ) )
+                                      NB_CMAC_TESTS_PER_KEY ) ) != 0 )
     {
         return( ret );
     }
 #endif /* MBEDTLS_DES_C */
 
 #if defined(MBEDTLS_AES_C)
-    if( ( ret = test_aes128_cmac_prf( verbose ) != 0 ) )
+    if( ( ret = test_aes128_cmac_prf( verbose ) ) != 0 )
         return( ret );
 #endif /* MBEDTLS_AES_C */
 
diff --git a/features/mbedtls/src/error.c b/features/mbedtls/src/error.c
index 71d4faa708..dd2db0c45c 100644
--- a/features/mbedtls/src/error.c
+++ b/features/mbedtls/src/error.c
@@ -435,6 +435,10 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen )
             mbedtls_snprintf( buf, buflen, "SSL - The client initiated a reconnect from the same port" );
         if( use_ret == -(MBEDTLS_ERR_SSL_UNEXPECTED_RECORD) )
             mbedtls_snprintf( buf, buflen, "SSL - Record header looks valid but is not expected" );
+        if( use_ret == -(MBEDTLS_ERR_SSL_NON_FATAL) )
+            mbedtls_snprintf( buf, buflen, "SSL - The alert message received indicates a non-fatal error" );
+        if( use_ret == -(MBEDTLS_ERR_SSL_INVALID_VERIFY_HASH) )
+            mbedtls_snprintf( buf, buflen, "SSL - Couldn't set the hash for verifying CertificateVerify" );
 #endif /* MBEDTLS_SSL_TLS_C */
 
 #if defined(MBEDTLS_X509_USE_C) || defined(MBEDTLS_X509_CREATE_C)
diff --git a/features/mbedtls/src/gcm.c b/features/mbedtls/src/gcm.c
index aaacf97d61..f1210c52c3 100644
--- a/features/mbedtls/src/gcm.c
+++ b/features/mbedtls/src/gcm.c
@@ -415,8 +415,7 @@ int mbedtls_gcm_finish( mbedtls_gcm_context *ctx,
     if( tag_len > 16 || tag_len < 4 )
         return( MBEDTLS_ERR_GCM_BAD_INPUT );
 
-    if( tag_len != 0 )
-        memcpy( tag, ctx->base_ectr, tag_len );
+    memcpy( tag, ctx->base_ectr, tag_len );
 
     if( orig_len || orig_add_len )
     {
diff --git a/features/mbedtls/src/rsa.c b/features/mbedtls/src/rsa.c
index 7a33689b2e..40ef2a9480 100644
--- a/features/mbedtls/src/rsa.c
+++ b/features/mbedtls/src/rsa.c
@@ -102,7 +102,10 @@ int mbedtls_rsa_gen_key( mbedtls_rsa_context *ctx,
     if( f_rng == NULL || nbits < 128 || exponent < 3 )
         return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
 
-    mbedtls_mpi_init( &P1 ); mbedtls_mpi_init( &Q1 ); 
+    if( nbits % 2 )
+        return( MBEDTLS_ERR_RSA_BAD_INPUT_DATA );
+
+    mbedtls_mpi_init( &P1 ); mbedtls_mpi_init( &Q1 );
     mbedtls_mpi_init( &H ); mbedtls_mpi_init( &G );
 
     /*
@@ -116,16 +119,8 @@ int mbedtls_rsa_gen_key( mbedtls_rsa_context *ctx,
         MBEDTLS_MPI_CHK( mbedtls_mpi_gen_prime( &ctx->P, nbits >> 1, 0,
                                 f_rng, p_rng ) );
 
-        if( nbits % 2 )
-        {
-            MBEDTLS_MPI_CHK( mbedtls_mpi_gen_prime( &ctx->Q, ( nbits >> 1 ) + 1, 0,
+        MBEDTLS_MPI_CHK( mbedtls_mpi_gen_prime( &ctx->Q, nbits >> 1, 0,
                                 f_rng, p_rng ) );
-        }
-        else
-        {
-            MBEDTLS_MPI_CHK( mbedtls_mpi_gen_prime( &ctx->Q, nbits >> 1, 0,
-                                f_rng, p_rng ) );
-        }
 
         if( mbedtls_mpi_cmp_mpi( &ctx->P, &ctx->Q ) == 0 )
             continue;
@@ -134,6 +129,9 @@ int mbedtls_rsa_gen_key( mbedtls_rsa_context *ctx,
         if( mbedtls_mpi_bitlen( &ctx->N ) != nbits )
             continue;
 
+        if( mbedtls_mpi_cmp_mpi( &ctx->P, &ctx->Q ) < 0 )
+                                mbedtls_mpi_swap( &ctx->P, &ctx->Q );
+
         MBEDTLS_MPI_CHK( mbedtls_mpi_sub_int( &P1, &ctx->P, 1 ) );
         MBEDTLS_MPI_CHK( mbedtls_mpi_sub_int( &Q1, &ctx->Q, 1 ) );
         MBEDTLS_MPI_CHK( mbedtls_mpi_mul_mpi( &H, &P1, &Q1 ) );
diff --git a/features/mbedtls/src/sha256.c b/features/mbedtls/src/sha256.c
index 4e82c0b793..ad25d38333 100644
--- a/features/mbedtls/src/sha256.c
+++ b/features/mbedtls/src/sha256.c
@@ -41,7 +41,10 @@
 #include "mbedtls/platform.h"
 #else
 #include <stdio.h>
+#include <stdlib.h>
 #define mbedtls_printf printf
+#define mbedtls_calloc    calloc
+#define mbedtls_free       free
 #endif /* MBEDTLS_PLATFORM_C */
 #endif /* MBEDTLS_SELF_TEST */
 
@@ -389,10 +392,19 @@ static const unsigned char sha256_test_sum[6][32] =
 int mbedtls_sha256_self_test( int verbose )
 {
     int i, j, k, buflen, ret = 0;
-    unsigned char buf[1024];
+    unsigned char *buf;
     unsigned char sha256sum[32];
     mbedtls_sha256_context ctx;
 
+    buf = mbedtls_calloc( 1024, sizeof(unsigned char) );
+    if( NULL == buf )
+    {
+        if( verbose != 0 )
+            mbedtls_printf( "Buffer allocation failed\n" );
+
+        return( 1 );
+    }
+
     mbedtls_sha256_init( &ctx );
 
     for( i = 0; i < 6; i++ )
@@ -436,6 +448,7 @@ int mbedtls_sha256_self_test( int verbose )
 
 exit:
     mbedtls_sha256_free( &ctx );
+    mbedtls_free( buf );
 
     return( ret );
 }
diff --git a/features/mbedtls/src/sha512.c b/features/mbedtls/src/sha512.c
index 0f9e1e5352..724522ac68 100644
--- a/features/mbedtls/src/sha512.c
+++ b/features/mbedtls/src/sha512.c
@@ -47,7 +47,10 @@
 #include "mbedtls/platform.h"
 #else
 #include <stdio.h>
+#include <stdlib.h>
 #define mbedtls_printf printf
+#define mbedtls_calloc    calloc
+#define mbedtls_free       free
 #endif /* MBEDTLS_PLATFORM_C */
 #endif /* MBEDTLS_SELF_TEST */
 
@@ -445,10 +448,19 @@ static const unsigned char sha512_test_sum[6][64] =
 int mbedtls_sha512_self_test( int verbose )
 {
     int i, j, k, buflen, ret = 0;
-    unsigned char buf[1024];
+    unsigned char *buf;
     unsigned char sha512sum[64];
     mbedtls_sha512_context ctx;
 
+    buf = mbedtls_calloc( 1024, sizeof(unsigned char) );
+    if( NULL == buf )
+    {
+        if( verbose != 0 )
+            mbedtls_printf( "Buffer allocation failed\n" );
+
+        return( 1 );
+    }
+
     mbedtls_sha512_init( &ctx );
 
     for( i = 0; i < 6; i++ )
@@ -492,6 +504,7 @@ int mbedtls_sha512_self_test( int verbose )
 
 exit:
     mbedtls_sha512_free( &ctx );
+    mbedtls_free( buf );
 
     return( ret );
 }
diff --git a/features/mbedtls/src/ssl_cli.c b/features/mbedtls/src/ssl_cli.c
index 29a3943584..223823b3cd 100644
--- a/features/mbedtls/src/ssl_cli.c
+++ b/features/mbedtls/src/ssl_cli.c
@@ -1355,6 +1355,15 @@ static int ssl_parse_hello_verify_request( mbedtls_ssl_context *ssl )
     cookie_len = *p++;
     MBEDTLS_SSL_DEBUG_BUF( 3, "cookie", p, cookie_len );
 
+    if( ( ssl->in_msg + ssl->in_msglen ) - p < cookie_len )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 1,
+            ( "cookie length does not match incoming message size" ) );
+        mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
+                                    MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
+        return( MBEDTLS_ERR_SSL_BAD_HS_SERVER_HELLO );
+    }
+
     mbedtls_free( ssl->handshake->verify_cookie );
 
     ssl->handshake->verify_cookie = mbedtls_calloc( 1, cookie_len );
@@ -2630,6 +2639,15 @@ static int ssl_parse_certificate_request( mbedtls_ssl_context *ssl )
     {
         size_t sig_alg_len = ( ( buf[mbedtls_ssl_hs_hdr_len( ssl ) + 1 + n] <<  8 )
                              | ( buf[mbedtls_ssl_hs_hdr_len( ssl ) + 2 + n]       ) );
+#if defined(MBEDTLS_DEBUG_C)
+        unsigned char* sig_alg = buf + mbedtls_ssl_hs_hdr_len( ssl ) + 3 + n;
+        size_t i;
+
+        for( i = 0; i < sig_alg_len; i += 2 )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 3, ( "Supported Signature Algorithm found: %d,%d", sig_alg[i], sig_alg[i + 1]  ) );
+        }
+#endif
 
         n += 2 + sig_alg_len;
 
diff --git a/features/mbedtls/src/ssl_srv.c b/features/mbedtls/src/ssl_srv.c
index 4b0f997143..fc0d2d7b42 100644
--- a/features/mbedtls/src/ssl_srv.c
+++ b/features/mbedtls/src/ssl_srv.c
@@ -1043,7 +1043,6 @@ have_ciphersuite_v2:
 
     ssl->session_negotiate->ciphersuite = ciphersuites[i];
     ssl->transform_negotiate->ciphersuite_info = ciphersuite_info;
-    mbedtls_ssl_optimize_checksum( ssl, ssl->transform_negotiate->ciphersuite_info );
 
     /*
      * SSLv2 Client Hello relevant renegotiation security checks
@@ -1840,7 +1839,6 @@ have_ciphersuite:
 
     ssl->session_negotiate->ciphersuite = ciphersuites[i];
     ssl->transform_negotiate->ciphersuite_info = ciphersuite_info;
-    mbedtls_ssl_optimize_checksum( ssl, ssl->transform_negotiate->ciphersuite_info );
 
     ssl->state++;
 
@@ -2556,29 +2554,27 @@ static int ssl_write_certificate_request( mbedtls_ssl_context *ssl )
      */
     if( ssl->minor_ver == MBEDTLS_SSL_MINOR_VERSION_3 )
     {
-        /*
-         * Only use current running hash algorithm that is already required
-         * for requested ciphersuite.
-         */
-        ssl->handshake->verify_sig_alg = MBEDTLS_SSL_HASH_SHA256;
-
-        if( ssl->transform_negotiate->ciphersuite_info->mac ==
-            MBEDTLS_MD_SHA384 )
-        {
-            ssl->handshake->verify_sig_alg = MBEDTLS_SSL_HASH_SHA384;
-        }
+        const int *cur;
 
         /*
          * Supported signature algorithms
          */
+        for( cur = ssl->conf->sig_hashes; *cur != MBEDTLS_MD_NONE; cur++ )
+        {
+            unsigned char hash = mbedtls_ssl_hash_from_md_alg( *cur );
+
+            if( MBEDTLS_SSL_HASH_NONE == hash || mbedtls_ssl_set_calc_verify_md( ssl, hash ) )
+                continue;
+
 #if defined(MBEDTLS_RSA_C)
-        p[2 + sa_len++] = ssl->handshake->verify_sig_alg;
-        p[2 + sa_len++] = MBEDTLS_SSL_SIG_RSA;
+            p[2 + sa_len++] = hash;
+            p[2 + sa_len++] = MBEDTLS_SSL_SIG_RSA;
 #endif
 #if defined(MBEDTLS_ECDSA_C)
-        p[2 + sa_len++] = ssl->handshake->verify_sig_alg;
-        p[2 + sa_len++] = MBEDTLS_SSL_SIG_ECDSA;
+            p[2 + sa_len++] = hash;
+            p[2 + sa_len++] = MBEDTLS_SSL_SIG_ECDSA;
 #endif
+        }
 
         p[0] = (unsigned char)( sa_len >> 8 );
         p[1] = (unsigned char)( sa_len      );
@@ -3581,17 +3577,28 @@ static int ssl_parse_certificate_verify( mbedtls_ssl_context *ssl )
         return( 0 );
     }
 
-    /* Needs to be done before read_record() to exclude current message */
-    ssl->handshake->calc_verify( ssl, hash );
+    /* Read the message without adding it to the checksum */
+    do {
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+        if( ( ret = mbedtls_ssl_read_record_layer( ssl ) ) != 0 )
+        {
+            MBEDTLS_SSL_DEBUG_RET( 1, ( "mbedtls_ssl_read_record_layer" ), ret );
+            return( ret );
+        }
+
+        ret = mbedtls_ssl_handle_message_type( ssl );
+
+    } while( MBEDTLS_ERR_SSL_NON_FATAL == ret );
+
+    if( 0 != ret )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, ( "mbedtls_ssl_handle_message_type" ), ret );
         return( ret );
     }
 
     ssl->state++;
 
+    /* Process the message contents */
     if( ssl->in_msgtype != MBEDTLS_SSL_MSG_HANDSHAKE ||
         ssl->in_msg[0] != MBEDTLS_SSL_HS_CERTIFICATE_VERIFY )
     {
@@ -3638,14 +3645,19 @@ static int ssl_parse_certificate_verify( mbedtls_ssl_context *ssl )
         /*
          * Hash
          */
-        if( ssl->in_msg[i] != ssl->handshake->verify_sig_alg )
+        md_alg = mbedtls_ssl_md_alg_from_hash( ssl->in_msg[i] );
+
+        if( md_alg == MBEDTLS_MD_NONE || mbedtls_ssl_set_calc_verify_md( ssl, ssl->in_msg[i] ) )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "peer not adhering to requested sig_alg"
                                 " for verify message" ) );
             return( MBEDTLS_ERR_SSL_BAD_HS_CERTIFICATE_VERIFY );
         }
 
-        md_alg = mbedtls_ssl_md_alg_from_hash( ssl->handshake->verify_sig_alg );
+#if !defined(MBEDTLS_MD_SHA1)
+        if( MBEDTLS_MD_SHA1 == md_alg )
+            hash_start += 16;
+#endif
 
         /* Info from md_alg will be used instead */
         hashlen = 0;
@@ -3696,6 +3708,9 @@ static int ssl_parse_certificate_verify( mbedtls_ssl_context *ssl )
         return( MBEDTLS_ERR_SSL_BAD_HS_CERTIFICATE_VERIFY );
     }
 
+    /* Calculate hash and verify signature */
+    ssl->handshake->calc_verify( ssl, hash );
+
     if( ( ret = mbedtls_pk_verify( &ssl->session_negotiate->peer_cert->pk,
                            md_alg, hash_start, hashlen,
                            ssl->in_msg + i, sig_len ) ) != 0 )
@@ -3704,6 +3719,8 @@ static int ssl_parse_certificate_verify( mbedtls_ssl_context *ssl )
         return( ret );
     }
 
+    mbedtls_ssl_update_handshake_status( ssl );
+
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= parse certificate verify" ) );
 
     return( ret );
diff --git a/features/mbedtls/src/ssl_tls.c b/features/mbedtls/src/ssl_tls.c
index 505bb6cb3a..84a04ae53f 100644
--- a/features/mbedtls/src/ssl_tls.c
+++ b/features/mbedtls/src/ssl_tls.c
@@ -49,8 +49,7 @@
 
 #include <string.h>
 
-#if defined(MBEDTLS_X509_CRT_PARSE_C) && \
-    defined(MBEDTLS_X509_CHECK_EXTENDED_KEY_USAGE)
+#if defined(MBEDTLS_X509_CRT_PARSE_C)
 #include "mbedtls/oid.h"
 #endif
 
@@ -1374,17 +1373,6 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
         /*
          * Generate IV
          */
-#if defined(MBEDTLS_SSL_AEAD_RANDOM_IV)
-        ret = ssl->conf->f_rng( ssl->conf->p_rng,
-                ssl->transform_out->iv_enc + ssl->transform_out->fixed_ivlen,
-                ssl->transform_out->ivlen - ssl->transform_out->fixed_ivlen );
-        if( ret != 0 )
-            return( ret );
-
-        memcpy( ssl->out_iv,
-                ssl->transform_out->iv_enc + ssl->transform_out->fixed_ivlen,
-                ssl->transform_out->ivlen - ssl->transform_out->fixed_ivlen );
-#else
         if( ssl->transform_out->ivlen - ssl->transform_out->fixed_ivlen != 8 )
         {
             /* Reminder if we ever add an AEAD mode with a different size */
@@ -1395,7 +1383,6 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
         memcpy( ssl->transform_out->iv_enc + ssl->transform_out->fixed_ivlen,
                              ssl->out_ctr, 8 );
         memcpy( ssl->out_iv, ssl->out_ctr, 8 );
-#endif
 
         MBEDTLS_SSL_DEBUG_BUF( 4, "IV used", ssl->out_iv,
                 ssl->transform_out->ivlen - ssl->transform_out->fixed_ivlen );
@@ -3083,7 +3070,7 @@ static int ssl_reassemble_dtls_handshake( mbedtls_ssl_context *ssl )
 }
 #endif /* MBEDTLS_SSL_PROTO_DTLS */
 
-static int ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
+int mbedtls_ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
 {
     if( ssl->in_msglen < mbedtls_ssl_hs_hdr_len( ssl ) )
     {
@@ -3165,6 +3152,12 @@ static int ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
         return( MBEDTLS_ERR_SSL_FEATURE_UNAVAILABLE );
     }
 
+    return( 0 );
+}
+
+void mbedtls_ssl_update_handshake_status( mbedtls_ssl_context *ssl )
+{
+
     if( ssl->state != MBEDTLS_SSL_HANDSHAKE_OVER &&
         ssl->handshake != NULL )
     {
@@ -3179,8 +3172,6 @@ static int ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
         ssl->handshake->in_msg_seq++;
     }
 #endif
-
-    return( 0 );
 }
 
 /*
@@ -3736,6 +3727,38 @@ int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl )
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> read record" ) );
 
+    do {
+
+        if( ( ret = mbedtls_ssl_read_record_layer( ssl ) ) != 0 )
+        {
+            MBEDTLS_SSL_DEBUG_RET( 1, ( "mbedtls_ssl_read_record_layer" ), ret );
+            return( ret );
+        }
+
+        ret = mbedtls_ssl_handle_message_type( ssl );
+
+    } while( MBEDTLS_ERR_SSL_NON_FATAL == ret );
+
+    if( 0 != ret )
+    {
+        MBEDTLS_SSL_DEBUG_RET( 1, ( "mbedtls_ssl_handle_message_type" ), ret );
+        return( ret );
+    }
+
+    if( ssl->in_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE )
+    {
+        mbedtls_ssl_update_handshake_status( ssl );
+    }
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= read record" ) );
+
+    return( 0 );
+}
+
+int mbedtls_ssl_read_record_layer( mbedtls_ssl_context *ssl )
+{
+    int ret;
+
     if( ssl->in_hslen != 0 && ssl->in_hslen < ssl->in_msglen )
     {
         /*
@@ -3749,9 +3772,6 @@ int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl )
         MBEDTLS_SSL_DEBUG_BUF( 4, "remaining content in record",
                            ssl->in_msg, ssl->in_msglen );
 
-        if( ( ret = ssl_prepare_handshake_record( ssl ) ) != 0 )
-            return( ret );
-
         return( 0 );
     }
 
@@ -3760,7 +3780,10 @@ int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl )
     /*
      * Read the record header and parse it
      */
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
 read_record_header:
+#endif
+
     if( ( ret = mbedtls_ssl_fetch_input( ssl, mbedtls_ssl_hdr_len( ssl ) ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_fetch_input", ret );
@@ -3914,13 +3937,22 @@ read_record_header:
     }
 #endif
 
+    return( 0 );
+}
+
+int mbedtls_ssl_handle_message_type( mbedtls_ssl_context *ssl )
+{
+    int ret;
+
     /*
      * Handle particular types of records
      */
     if( ssl->in_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE )
     {
-        if( ( ret = ssl_prepare_handshake_record( ssl ) ) != 0 )
+        if( ( ret = mbedtls_ssl_prepare_handshake_record( ssl ) ) != 0 )
+        {
             return( ret );
+        }
     }
 
     if( ssl->in_msgtype == MBEDTLS_SSL_MSG_ALERT )
@@ -3968,11 +4000,9 @@ read_record_header:
 #endif /* MBEDTLS_SSL_PROTO_SSL3 && MBEDTLS_SSL_SRV_C */
 
         /* Silently ignore: fetch new message */
-        goto read_record_header;
+        return MBEDTLS_ERR_SSL_NON_FATAL;
     }
 
-    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= read record" ) );
-
     return( 0 );
 }
 
@@ -4347,7 +4377,7 @@ int mbedtls_ssl_parse_certificate( mbedtls_ssl_context *ssl )
 
         ret = mbedtls_x509_crt_parse_der( ssl->session_negotiate->peer_cert,
                                   ssl->in_msg + i, n );
-        if( ret != 0 )
+        if( 0 != ret && ( MBEDTLS_ERR_X509_UNKNOWN_SIG_ALG + MBEDTLS_ERR_OID_NOT_FOUND ) != ret )
         {
             MBEDTLS_SSL_DEBUG_RET( 1, " mbedtls_x509_crt_parse_der", ret );
             return( ret );
@@ -7603,4 +7633,47 @@ void mbedtls_ssl_read_version( int *major, int *minor, int transport,
     }
 }
 
+int mbedtls_ssl_set_calc_verify_md( mbedtls_ssl_context *ssl, int md )
+{
+#if defined(MBEDTLS_SSL_PROTO_TLS1_2)
+    if( ssl->minor_ver != MBEDTLS_SSL_MINOR_VERSION_3 )
+        return MBEDTLS_ERR_SSL_INVALID_VERIFY_HASH;
+
+    switch( md )
+    {
+#if defined(MBEDTLS_SSL_PROTO_TLS1) || defined(MBEDTLS_SSL_PROTO_TLS1_1)
+#if defined(MBEDTLS_MD5_C)
+        case MBEDTLS_SSL_HASH_MD5:
+            ssl->handshake->calc_verify = ssl_calc_verify_tls;
+            break;
+#endif
+#if defined(MBEDTLS_SHA1_C)
+        case MBEDTLS_SSL_HASH_SHA1:
+            ssl->handshake->calc_verify = ssl_calc_verify_tls;
+            break;
+#endif
+#endif /* MBEDTLS_SSL_PROTO_TLS1 || MBEDTLS_SSL_PROTO_TLS1_1 */
+#if defined(MBEDTLS_SHA512_C)
+        case MBEDTLS_SSL_HASH_SHA384:
+            ssl->handshake->calc_verify = ssl_calc_verify_tls_sha384;
+            break;
+#endif
+#if defined(MBEDTLS_SHA256_C)
+        case MBEDTLS_SSL_HASH_SHA256:
+            ssl->handshake->calc_verify = ssl_calc_verify_tls_sha256;
+            break;
+#endif
+        default:
+            return MBEDTLS_ERR_SSL_INVALID_VERIFY_HASH;
+    }
+
+    return 0;
+#else /* !MBEDTLS_SSL_PROTO_TLS1_2 */
+    (void) ssl;
+    (void) md;
+
+    return MBEDTLS_ERR_SSL_INVALID_VERIFY_HASH;
+#endif /* MBEDTLS_SSL_PROTO_TLS1_2 */
+}
+
 #endif /* MBEDTLS_SSL_TLS_C */
diff --git a/features/mbedtls/src/threading.c b/features/mbedtls/src/threading.c
index 1b6d9cd445..83ec01a45f 100644
--- a/features/mbedtls/src/threading.c
+++ b/features/mbedtls/src/threading.c
@@ -32,7 +32,7 @@
 #if defined(MBEDTLS_THREADING_PTHREAD)
 static void threading_mutex_init_pthread( mbedtls_threading_mutex_t *mutex )
 {
-    if( mutex == NULL )
+    if( mutex == NULL || mutex->is_valid )
         return;
 
     mutex->is_valid = pthread_mutex_init( &mutex->mutex, NULL ) == 0;
@@ -40,10 +40,11 @@ static void threading_mutex_init_pthread( mbedtls_threading_mutex_t *mutex )
 
 static void threading_mutex_free_pthread( mbedtls_threading_mutex_t *mutex )
 {
-    if( mutex == NULL )
+    if( mutex == NULL || !mutex->is_valid )
         return;
 
     (void) pthread_mutex_destroy( &mutex->mutex );
+    mutex->is_valid = 0;
 }
 
 static int threading_mutex_lock_pthread( mbedtls_threading_mutex_t *mutex )
diff --git a/features/mbedtls/src/version_features.c b/features/mbedtls/src/version_features.c
index 0a2f065751..e866e67a23 100644
--- a/features/mbedtls/src/version_features.c
+++ b/features/mbedtls/src/version_features.c
@@ -324,9 +324,6 @@ static const char *features[] = {
 #if defined(MBEDTLS_SHA256_SMALLER)
     "MBEDTLS_SHA256_SMALLER",
 #endif /* MBEDTLS_SHA256_SMALLER */
-#if defined(MBEDTLS_SSL_AEAD_RANDOM_IV)
-    "MBEDTLS_SSL_AEAD_RANDOM_IV",
-#endif /* MBEDTLS_SSL_AEAD_RANDOM_IV */
 #if defined(MBEDTLS_SSL_ALL_ALERT_MESSAGES)
     "MBEDTLS_SSL_ALL_ALERT_MESSAGES",
 #endif /* MBEDTLS_SSL_ALL_ALERT_MESSAGES */
diff --git a/features/mbedtls/src/x509.c b/features/mbedtls/src/x509.c
index bc3bfe018f..fad390d857 100644
--- a/features/mbedtls/src/x509.c
+++ b/features/mbedtls/src/x509.c
@@ -80,6 +80,7 @@
 #endif
 
 #define CHECK(code) if( ( ret = code ) != 0 ){ return( ret ); }
+#define CHECK_RANGE(min, max, val) if( val < min || val > max ){ return( ret ); }
 
 /*
  *  CertificateSerialNumber  ::=  INTEGER
@@ -489,6 +490,33 @@ static int x509_parse_int(unsigned char **p, unsigned n, int *res){
     return 0;
 }
 
+static int x509_date_is_valid(const mbedtls_x509_time *time)
+{
+    int ret = MBEDTLS_ERR_X509_INVALID_DATE;
+
+    CHECK_RANGE( 0, 9999, time->year );
+    CHECK_RANGE( 0, 23,   time->hour );
+    CHECK_RANGE( 0, 59,   time->min  );
+    CHECK_RANGE( 0, 59,   time->sec  );
+
+    switch( time->mon )
+    {
+        case 1: case 3: case 5: case 7: case 8: case 10: case 12:
+            CHECK_RANGE( 1, 31, time->day );
+            break;
+        case 4: case 6: case 9: case 11:
+            CHECK_RANGE( 1, 30, time->day );
+            break;
+        case 2:
+            CHECK_RANGE( 1, 28 + (time->year % 4 == 0), time->day );
+            break;
+        default:
+            return( ret );
+    }
+
+    return( 0 );
+}
+
 /*
  *  Time ::= CHOICE {
  *       utcTime        UTCTime,
@@ -528,6 +556,8 @@ int mbedtls_x509_get_time( unsigned char **p, const unsigned char *end,
         time->year +=  100 * ( time->year < 50 );
         time->year += 1900;
 
+        CHECK( x509_date_is_valid( time ) );
+
         return( 0 );
     }
     else if( tag == MBEDTLS_ASN1_GENERALIZED_TIME )
@@ -548,6 +578,8 @@ int mbedtls_x509_get_time( unsigned char **p, const unsigned char *end,
         if( len > 14 && *(*p)++ != 'Z' )
             return( MBEDTLS_ERR_X509_INVALID_DATE );
 
+        CHECK( x509_date_is_valid( time ) );
+
         return( 0 );
     }
     else
@@ -559,16 +591,18 @@ int mbedtls_x509_get_sig( unsigned char **p, const unsigned char *end, mbedtls_x
 {
     int ret;
     size_t len;
+    int tag_type;
 
     if( ( end - *p ) < 1 )
         return( MBEDTLS_ERR_X509_INVALID_SIGNATURE +
                 MBEDTLS_ERR_ASN1_OUT_OF_DATA );
 
-    sig->tag = **p;
+    tag_type = **p;
 
     if( ( ret = mbedtls_asn1_get_bitstring_null( p, end, &len ) ) != 0 )
         return( MBEDTLS_ERR_X509_INVALID_SIGNATURE + ret );
 
+    sig->tag = tag_type;
     sig->len = len;
     sig->p = *p;
 
diff --git a/features/mbedtls/src/x509write_crt.c b/features/mbedtls/src/x509write_crt.c
index 9041d440ff..d1d9a22a7e 100644
--- a/features/mbedtls/src/x509write_crt.c
+++ b/features/mbedtls/src/x509write_crt.c
@@ -413,6 +413,9 @@ int mbedtls_x509write_crt_der( mbedtls_x509write_cert *ctx, unsigned char *buf,
     MBEDTLS_ASN1_CHK_ADD( sig_and_oid_len, mbedtls_x509_write_sig( &c2, buf,
                                         sig_oid, sig_oid_len, sig, sig_len ) );
 
+    if( len > (size_t)( c2 - buf ) )
+        return( MBEDTLS_ERR_ASN1_BUF_TOO_SMALL );
+
     c2 -= len;
     memcpy( c2, c, len );
 
diff --git a/features/mbedtls/src/x509write_csr.c b/features/mbedtls/src/x509write_csr.c
index 0b9a2851e0..8fd856b2a2 100644
--- a/features/mbedtls/src/x509write_csr.c
+++ b/features/mbedtls/src/x509write_csr.c
@@ -213,6 +213,9 @@ int mbedtls_x509write_csr_der( mbedtls_x509write_csr *ctx, unsigned char *buf, s
     MBEDTLS_ASN1_CHK_ADD( sig_and_oid_len, mbedtls_x509_write_sig( &c2, buf,
                                         sig_oid, sig_oid_len, sig, sig_len ) );
 
+    if( len > (size_t)( c2 - buf ) )
+        return( MBEDTLS_ERR_ASN1_BUF_TOO_SMALL );
+
     c2 -= len;
     memcpy( c2, c, len );