Merge branch 'development-restricted' into iotssl-1260-non-blocking-ecc-restricted

* development-restricted: (578 commits)
  Update library version number to 2.13.1
  Don't define _POSIX_C_SOURCE in header file
  Don't declare and define gmtime()-mutex on Windows platforms
  Correct preprocessor guards determining use of gmtime()
  Correct documentation of mbedtls_platform_gmtime_r()
  Correct typo in documentation of mbedtls_platform_gmtime_r()
  Correct POSIX version check to determine presence of gmtime_r()
  Improve documentation of mbedtls_platform_gmtime_r()
  platform_utils.{c/h} -> platform_util.{c/h}
  Don't include platform_time.h if !MBEDTLS_HAVE_TIME
  Improve wording of documentation of MBEDTLS_PLATFORM_GMTIME_R_ALT
  Fix typo in documentation of MBEDTLS_PLATFORM_GMTIME_R_ALT
  Replace 'thread safe' by 'thread-safe' in the documentation
  Improve documentation of MBEDTLS_HAVE_TIME_DATE
  ChangeLog: Add missing renamings gmtime -> gmtime_r
  Improve documentation of MBEDTLS_HAVE_TIME_DATE
  Minor documentation improvements
  Style: Add missing period in documentation in threading.h
  Rename mbedtls_platform_gmtime() to mbedtls_platform_gmtime_r()
  Guard decl and use of gmtime mutex by HAVE_TIME_DATE and !GMTIME_ALT
  ...

32 files changed, 5280 insertions, 807 deletions

diff --git a/library/CMakeLists.txt b/library/CMakeLists.txt
index 5243baf4..275eda3b 100644
--- a/library/CMakeLists.txt
+++ b/library/CMakeLists.txt
@@ -14,6 +14,8 @@ set(src_crypto
     blowfish.c
     camellia.c
     ccm.c
+    chacha20.c
+    chachapoly.c
     cipher.c
     cipher_wrap.c
     cmac.c
@@ -38,6 +40,7 @@ set(src_crypto
     md5.c
     md_wrap.c
     memory_buffer_alloc.c
+    nist_kw.c
     oid.c
     padlock.c
     pem.c
@@ -49,6 +52,7 @@ set(src_crypto
     pkwrite.c
     platform.c
     platform_util.c
+    poly1305.c
     ripemd160.c
     rsa.c
     rsa_internal.c
@@ -98,6 +102,17 @@ if(WIN32)
     set(libs ${libs} ws2_32)
 endif(WIN32)
 
+if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+    SET(CMAKE_C_ARCHIVE_CREATE   "<CMAKE_AR> Scr <TARGET> <LINK_FLAGS> <OBJECTS>")
+    SET(CMAKE_CXX_ARCHIVE_CREATE "<CMAKE_AR> Scr <TARGET> <LINK_FLAGS> <OBJECTS>")
+    SET(CMAKE_C_ARCHIVE_FINISH   "<CMAKE_RANLIB> -no_warning_for_no_symbols -c <TARGET>")
+    SET(CMAKE_CXX_ARCHIVE_FINISH "<CMAKE_RANLIB> -no_warning_for_no_symbols -c <TARGET>")
+endif()
+
+if(HAIKU)
+    set(libs ${libs} network)
+endif(HAIKU)
+
 if(USE_PKCS11_HELPER_LIBRARY)
     set(libs ${libs} pkcs11-helper)
 endif(USE_PKCS11_HELPER_LIBRARY)
@@ -144,15 +159,15 @@ endif(USE_STATIC_MBEDTLS_LIBRARY)
 
 if(USE_SHARED_MBEDTLS_LIBRARY)
     add_library(mbedcrypto SHARED ${src_crypto})
-    set_target_properties(mbedcrypto PROPERTIES VERSION 2.11.0 SOVERSION 3)
+    set_target_properties(mbedcrypto PROPERTIES VERSION 2.13.1 SOVERSION 3)
     target_link_libraries(mbedcrypto ${libs})
 
     add_library(mbedx509 SHARED ${src_x509})
-    set_target_properties(mbedx509 PROPERTIES VERSION 2.11.0 SOVERSION 0)
+    set_target_properties(mbedx509 PROPERTIES VERSION 2.13.1 SOVERSION 0)
     target_link_libraries(mbedx509 ${libs} mbedcrypto)
 
     add_library(mbedtls SHARED ${src_tls})
-    set_target_properties(mbedtls PROPERTIES VERSION 2.11.0 SOVERSION 11)
+    set_target_properties(mbedtls PROPERTIES VERSION 2.13.1 SOVERSION 12)
     target_link_libraries(mbedtls ${libs} mbedx509)
 
     install(TARGETS mbedtls mbedx509 mbedcrypto
diff --git a/library/Makefile b/library/Makefile
index f7eb8965..430c5988 100644
--- a/library/Makefile
+++ b/library/Makefile
@@ -21,6 +21,10 @@ endif
 # if were running on Windows build for Windows
 ifdef WINDOWS
 WINDOWS_BUILD=1
+else ifeq ($(shell uname -s),Darwin)
+ifeq ($(AR),ar)
+APPLE_BUILD ?= 1
+endif
 endif
 
 # To compile as a shared library:
@@ -31,27 +35,39 @@ LOCAL_CFLAGS += -fPIC -fpic
 endif
 endif
 
-SOEXT_TLS=so.11
+SOEXT_TLS=so.12
 SOEXT_X509=so.0
 SOEXT_CRYPTO=so.3
 
-# Set DLEXT=dylib to compile as a shared library for Mac OS X
-DLEXT ?= so
-
 # Set AR_DASH= (empty string) to use an ar implentation that does not accept
 # the - prefix for command line options (e.g. llvm-ar)
 AR_DASH ?= -
 
-# Windows shared library extension:
+ARFLAGS = $(AR_DASH)src
+ifdef APPLE_BUILD
+ifneq ($(APPLE_BUILD),0)
+ARFLAGS = $(AR_DASH)Src
+RLFLAGS = -no_warning_for_no_symbols -c
+RL ?= ranlib
+endif
+endif
+
+DLEXT ?= so
 ifdef WINDOWS_BUILD
-DLEXT=dll
+# Windows shared library extension:
+DLEXT = dll
+else ifdef APPLE_BUILD
+ifneq ($(APPLE_BUILD),0)
+# Mac OS X shared library extension:
+DLEXT = dylib
+endif
 endif
 
 OBJS_CRYPTO=	aes.o		aesni.o		arc4.o		\
-		asn1parse.o	asn1write.o	base64.o	\
-		bignum.o	blowfish.o	camellia.o	\
-		aria.o						\
-		ccm.o		cipher.o	cipher_wrap.o	\
+		aria.o		asn1parse.o	asn1write.o	\
+		base64.o	bignum.o	blowfish.o	\
+		camellia.o	ccm.o		chacha20.o	\
+		chachapoly.o	cipher.o	cipher_wrap.o	\
 		cmac.o		ctr_drbg.o	des.o		\
 		dhm.o		ecdh.o		ecdsa.o		\
 		ecjpake.o	ecp.o				\
@@ -60,15 +76,15 @@ OBJS_CRYPTO=	aes.o		aesni.o		arc4.o		\
 		hkdf.o						\
 		hmac_drbg.o	md.o		md2.o		\
 		md4.o		md5.o		md_wrap.o	\
-		memory_buffer_alloc.o		oid.o		\
-		padlock.o	pem.o		pk.o		\
-		pk_wrap.o	pkcs12.o	pkcs5.o		\
-		pkparse.o	pkwrite.o	platform.o	\
-		platform_util.o	ripemd160.o	rsa_internal.o	\
-		rsa.o 		sha1.o		sha256.o	\
-		sha512.o	threading.o	timing.o	\
-		version.o	version_features.o		\
-		xtea.o
+		memory_buffer_alloc.o		nist_kw.o	\
+		oid.o		padlock.o	pem.o		\
+		pk.o		pk_wrap.o	pkcs12.o	\
+		pkcs5.o		pkparse.o	pkwrite.o	\
+		platform.o	platform_util.o	poly1305.o	\
+		ripemd160.o	rsa_internal.o	rsa.o  		\
+		sha1.o		sha256.o	sha512.o	\
+		threading.o	timing.o	version.o	\
+		version_features.o		xtea.o
 
 OBJS_X509=	certs.o		pkcs11.o	x509.o		\
 		x509_create.o	x509_crl.o	x509_crt.o	\
@@ -97,9 +113,13 @@ shared: libmbedcrypto.$(DLEXT) libmbedx509.$(DLEXT) libmbedtls.$(DLEXT)
 # tls
 libmbedtls.a: $(OBJS_TLS)
 	echo "  AR    $@"
-	$(AR) $(AR_DASH)rc $@ $(OBJS_TLS)
+	$(AR) $(ARFLAGS) $@ $(OBJS_TLS)
+ifdef APPLE_BUILD
+ifneq ($(APPLE_BUILD),0)
 	echo "  RL    $@"
-	$(AR) $(AR_DASH)s $@
+	$(RL) $(RLFLAGS) $@
+endif
+endif
 
 libmbedtls.$(SOEXT_TLS): $(OBJS_TLS) libmbedx509.so
 	echo "  LD    $@"
@@ -120,9 +140,13 @@ libmbedtls.dll: $(OBJS_TLS) libmbedx509.dll
 # x509
 libmbedx509.a: $(OBJS_X509)
 	echo "  AR    $@"
-	$(AR) $(AR_DASH)rc $@ $(OBJS_X509)
+	$(AR) $(ARFLAGS) $@ $(OBJS_X509)
+ifdef APPLE_BUILD
+ifneq ($(APPLE_BUILD),0)
 	echo "  RL    $@"
-	$(AR) $(AR_DASH)s $@
+	$(RL) $(RLFLAGS) $@
+endif
+endif
 
 libmbedx509.$(SOEXT_X509): $(OBJS_X509) libmbedcrypto.so
 	echo "  LD    $@"
@@ -143,9 +167,13 @@ libmbedx509.dll: $(OBJS_X509) libmbedcrypto.dll
 # crypto
 libmbedcrypto.a: $(OBJS_CRYPTO)
 	echo "  AR    $@"
-	$(AR) $(AR_DASH)rc $@ $(OBJS_CRYPTO)
+	$(AR) $(ARFLAGS) $@ $(OBJS_CRYPTO)
+ifdef APPLE_BUILD
+ifneq ($(APPLE_BUILD),0)
 	echo "  RL    $@"
-	$(AR) $(AR_DASH)s $@
+	$(RL) $(RLFLAGS) $@
+endif
+endif
 
 libmbedcrypto.$(SOEXT_CRYPTO): $(OBJS_CRYPTO)
 	echo "  LD    $@"
diff --git a/library/aria.c b/library/aria.c
index e9bcd6d1..ca9e147f 100644
--- a/library/aria.c
+++ b/library/aria.c
@@ -875,11 +875,11 @@ static const uint8_t aria_test2_ctr_ct[3][48] =         // CTR ciphertext
 #define ARIA_SELF_TEST_IF_FAIL              \
         {                                   \
             if( verbose )                   \
-                printf( "failed\n" );       \
+                mbedtls_printf( "failed\n" );       \
             return( 1 );                    \
         } else {                            \
             if( verbose )                   \
-                printf( "passed\n" );       \
+                mbedtls_printf( "passed\n" );       \
         }
 
 /*
@@ -908,7 +908,7 @@ int mbedtls_aria_self_test( int verbose )
     {
         /* test ECB encryption */
         if( verbose )
-            printf( "  ARIA-ECB-%d (enc): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-ECB-%d (enc): ", 128 + 64 * i );
         mbedtls_aria_setkey_enc( &ctx, aria_test1_ecb_key, 128 + 64 * i );
         mbedtls_aria_crypt_ecb( &ctx, aria_test1_ecb_pt, blk );
         if( memcmp( blk, aria_test1_ecb_ct[i], MBEDTLS_ARIA_BLOCKSIZE ) != 0 )
@@ -916,14 +916,14 @@ int mbedtls_aria_self_test( int verbose )
 
         /* test ECB decryption */
         if( verbose )
-            printf( "  ARIA-ECB-%d (dec): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-ECB-%d (dec): ", 128 + 64 * i );
         mbedtls_aria_setkey_dec( &ctx, aria_test1_ecb_key, 128 + 64 * i );
         mbedtls_aria_crypt_ecb( &ctx, aria_test1_ecb_ct[i], blk );
         if( memcmp( blk, aria_test1_ecb_pt, MBEDTLS_ARIA_BLOCKSIZE ) != 0 )
             ARIA_SELF_TEST_IF_FAIL;
     }
     if( verbose )
-        printf( "\n" );
+        mbedtls_printf( "\n" );
 
     /*
      * Test set 2
@@ -933,7 +933,7 @@ int mbedtls_aria_self_test( int verbose )
     {
         /* Test CBC encryption */
         if( verbose )
-            printf( "  ARIA-CBC-%d (enc): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-CBC-%d (enc): ", 128 + 64 * i );
         mbedtls_aria_setkey_enc( &ctx, aria_test2_key, 128 + 64 * i );
         memcpy( iv, aria_test2_iv, MBEDTLS_ARIA_BLOCKSIZE );
         memset( buf, 0x55, sizeof( buf ) );
@@ -944,7 +944,7 @@ int mbedtls_aria_self_test( int verbose )
 
         /* Test CBC decryption */
         if( verbose )
-            printf( "  ARIA-CBC-%d (dec): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-CBC-%d (dec): ", 128 + 64 * i );
         mbedtls_aria_setkey_dec( &ctx, aria_test2_key, 128 + 64 * i );
         memcpy( iv, aria_test2_iv, MBEDTLS_ARIA_BLOCKSIZE );
         memset( buf, 0xAA, sizeof( buf ) );
@@ -954,7 +954,7 @@ int mbedtls_aria_self_test( int verbose )
             ARIA_SELF_TEST_IF_FAIL;
     }
     if( verbose )
-        printf( "\n" );
+        mbedtls_printf( "\n" );
 
 #endif /* MBEDTLS_CIPHER_MODE_CBC */
 
@@ -963,7 +963,7 @@ int mbedtls_aria_self_test( int verbose )
     {
         /* Test CFB encryption */
         if( verbose )
-            printf( "  ARIA-CFB-%d (enc): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-CFB-%d (enc): ", 128 + 64 * i );
         mbedtls_aria_setkey_enc( &ctx, aria_test2_key, 128 + 64 * i );
         memcpy( iv, aria_test2_iv, MBEDTLS_ARIA_BLOCKSIZE );
         memset( buf, 0x55, sizeof( buf ) );
@@ -975,7 +975,7 @@ int mbedtls_aria_self_test( int verbose )
 
         /* Test CFB decryption */
         if( verbose )
-            printf( "  ARIA-CFB-%d (dec): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-CFB-%d (dec): ", 128 + 64 * i );
         mbedtls_aria_setkey_enc( &ctx, aria_test2_key, 128 + 64 * i );
         memcpy( iv, aria_test2_iv, MBEDTLS_ARIA_BLOCKSIZE );
         memset( buf, 0xAA, sizeof( buf ) );
@@ -986,7 +986,7 @@ int mbedtls_aria_self_test( int verbose )
             ARIA_SELF_TEST_IF_FAIL;
     }
     if( verbose )
-        printf( "\n" );
+        mbedtls_printf( "\n" );
 #endif /* MBEDTLS_CIPHER_MODE_CFB */
 
 #if defined(MBEDTLS_CIPHER_MODE_CTR)
@@ -994,7 +994,7 @@ int mbedtls_aria_self_test( int verbose )
     {
         /* Test CTR encryption */
         if( verbose )
-            printf( "  ARIA-CTR-%d (enc): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-CTR-%d (enc): ", 128 + 64 * i );
         mbedtls_aria_setkey_enc( &ctx, aria_test2_key, 128 + 64 * i );
         memset( iv, 0, MBEDTLS_ARIA_BLOCKSIZE );                    // IV = 0
         memset( buf, 0x55, sizeof( buf ) );
@@ -1006,7 +1006,7 @@ int mbedtls_aria_self_test( int verbose )
 
         /* Test CTR decryption */
         if( verbose )
-            printf( "  ARIA-CTR-%d (dec): ", 128 + 64 * i );
+            mbedtls_printf( "  ARIA-CTR-%d (dec): ", 128 + 64 * i );
         mbedtls_aria_setkey_enc( &ctx, aria_test2_key, 128 + 64 * i );
         memset( iv, 0, MBEDTLS_ARIA_BLOCKSIZE );                    // IV = 0
         memset( buf, 0xAA, sizeof( buf ) );
@@ -1017,7 +1017,7 @@ int mbedtls_aria_self_test( int verbose )
             ARIA_SELF_TEST_IF_FAIL;
     }
     if( verbose )
-        printf( "\n" );
+        mbedtls_printf( "\n" );
 #endif /* MBEDTLS_CIPHER_MODE_CTR */
 
     return( 0 );
diff --git a/library/ccm.c b/library/ccm.c
index 804eaf80..90cab8e1 100644
--- a/library/ccm.c
+++ b/library/ccm.c
@@ -381,7 +381,8 @@ int mbedtls_ccm_auth_decrypt( mbedtls_ccm_context *ctx, size_t length,
  */
 
 #define NB_TESTS 3
-
+#define CCM_SELFTEST_PT_MAX_LEN 24
+#define CCM_SELFTEST_CT_MAX_LEN 32
 /*
  * The data is the same for all tests, only the used length changes
  */
@@ -401,7 +402,7 @@ static const unsigned char ad[] = {
     0x10, 0x11, 0x12, 0x13
 };
 
-static const unsigned char msg[] = {
+static const unsigned char msg[CCM_SELFTEST_PT_MAX_LEN] = {
     0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
     0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f,
     0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
@@ -412,7 +413,7 @@ static const size_t add_len[NB_TESTS] = { 8, 16, 20 };
 static const size_t msg_len[NB_TESTS] = { 4, 16, 24 };
 static const size_t tag_len[NB_TESTS] = { 4, 6,  8  };
 
-static const unsigned char res[NB_TESTS][32] = {
+static const unsigned char res[NB_TESTS][CCM_SELFTEST_CT_MAX_LEN] = {
     {   0x71, 0x62, 0x01, 0x5b, 0x4d, 0xac, 0x25, 0x5d },
     {   0xd2, 0xa1, 0xf0, 0xe0, 0x51, 0xea, 0x5f, 0x62,
         0x08, 0x1a, 0x77, 0x92, 0x07, 0x3d, 0x59, 0x3d,
@@ -426,7 +427,13 @@ static const unsigned char res[NB_TESTS][32] = {
 int mbedtls_ccm_self_test( int verbose )
 {
     mbedtls_ccm_context ctx;
-    unsigned char out[32];
+    /*
+     * Some hardware accelerators require the input and output buffers
+     * would be in RAM, because the flash is not accessible.
+     * Use buffers on the stack to hold the test vectors data.
+     */
+    unsigned char plaintext[CCM_SELFTEST_PT_MAX_LEN];
+    unsigned char ciphertext[CCM_SELFTEST_CT_MAX_LEN];
     size_t i;
     int ret;
 
@@ -445,27 +452,32 @@ int mbedtls_ccm_self_test( int verbose )
         if( verbose != 0 )
             mbedtls_printf( "  CCM-AES #%u: ", (unsigned int) i + 1 );
 
+        memset( plaintext, 0, CCM_SELFTEST_PT_MAX_LEN );
+        memset( ciphertext, 0, CCM_SELFTEST_CT_MAX_LEN );
+        memcpy( plaintext, msg, msg_len[i] );
+
         ret = mbedtls_ccm_encrypt_and_tag( &ctx, msg_len[i],
-                                   iv, iv_len[i], ad, add_len[i],
-                                   msg, out,
-                                   out + msg_len[i], tag_len[i] );
+                                           iv, iv_len[i], ad, add_len[i],
+                                           plaintext, ciphertext,
+                                           ciphertext + msg_len[i], tag_len[i] );
 
         if( ret != 0 ||
-            memcmp( out, res[i], msg_len[i] + tag_len[i] ) != 0 )
+            memcmp( ciphertext, res[i], msg_len[i] + tag_len[i] ) != 0 )
         {
             if( verbose != 0 )
                 mbedtls_printf( "failed\n" );
 
             return( 1 );
         }
+        memset( plaintext, 0, CCM_SELFTEST_PT_MAX_LEN );
 
         ret = mbedtls_ccm_auth_decrypt( &ctx, msg_len[i],
-                                iv, iv_len[i], ad, add_len[i],
-                                res[i], out,
-                                res[i] + msg_len[i], tag_len[i] );
+                                        iv, iv_len[i], ad, add_len[i],
+                                        ciphertext, plaintext,
+                                        ciphertext + msg_len[i], tag_len[i] );
 
         if( ret != 0 ||
-            memcmp( out, msg, msg_len[i] ) != 0 )
+            memcmp( plaintext, msg, msg_len[i] ) != 0 )
         {
             if( verbose != 0 )
                 mbedtls_printf( "failed\n" );
diff --git a/library/chacha20.c b/library/chacha20.c
new file mode 100644
index 00000000..d14a51e0
--- /dev/null
+++ b/library/chacha20.c
@@ -0,0 +1,570 @@
+/**
+ * \file chacha20.c
+ *
+ * \brief ChaCha20 cipher.
+ *
+ * \author Daniel King <damaki.gh@gmail.com>
+ *
+ *  Copyright (C) 2006-2016, ARM Limited, All Rights Reserved
+ *  SPDX-License-Identifier: Apache-2.0
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License"); you may
+ *  not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ *  WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ *
+ *  This file is part of mbed TLS (https://tls.mbed.org)
+ */
+
+#if !defined(MBEDTLS_CONFIG_FILE)
+#include "mbedtls/config.h"
+#else
+#include MBEDTLS_CONFIG_FILE
+#endif
+
+#if defined(MBEDTLS_CHACHA20_C)
+
+#include "mbedtls/chacha20.h"
+#include "mbedtls/platform_util.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#if defined(MBEDTLS_SELF_TEST)
+#if defined(MBEDTLS_PLATFORM_C)
+#include "mbedtls/platform.h"
+#else
+#include <stdio.h>
+#define mbedtls_printf printf
+#endif /* MBEDTLS_PLATFORM_C */
+#endif /* MBEDTLS_SELF_TEST */
+
+#if !defined(MBEDTLS_CHACHA20_ALT)
+
+#if ( defined(__ARMCC_VERSION) || defined(_MSC_VER) ) && \
+    !defined(inline) && !defined(__cplusplus)
+#define inline __inline
+#endif
+
+#define BYTES_TO_U32_LE( data, offset )                           \
+    ( (uint32_t) data[offset]                                     \
+          | (uint32_t) ( (uint32_t) data[( offset ) + 1] << 8 )   \
+          | (uint32_t) ( (uint32_t) data[( offset ) + 2] << 16 )  \
+          | (uint32_t) ( (uint32_t) data[( offset ) + 3] << 24 )  \
+    )
+
+#define ROTL32( value, amount ) \
+        ( (uint32_t) ( value << amount ) | ( value >> ( 32 - amount ) ) )
+
+#define CHACHA20_CTR_INDEX ( 12U )
+
+#define CHACHA20_BLOCK_SIZE_BYTES ( 4U * 16U )
+
+/**
+ * \brief           ChaCha20 quarter round operation.
+ *
+ *                  The quarter round is defined as follows (from RFC 7539):
+ *                      1.  a += b; d ^= a; d <<<= 16;
+ *                      2.  c += d; b ^= c; b <<<= 12;
+ *                      3.  a += b; d ^= a; d <<<= 8;
+ *                      4.  c += d; b ^= c; b <<<= 7;
+ *
+ * \param state     ChaCha20 state to modify.
+ * \param a         The index of 'a' in the state.
+ * \param b         The index of 'b' in the state.
+ * \param c         The index of 'c' in the state.
+ * \param d         The index of 'd' in the state.
+ */
+static inline void chacha20_quarter_round( uint32_t state[16],
+                                           size_t a,
+                                           size_t b,
+                                           size_t c,
+                                           size_t d )
+{
+    /* a += b; d ^= a; d <<<= 16; */
+    state[a] += state[b];
+    state[d] ^= state[a];
+    state[d] = ROTL32( state[d], 16 );
+
+    /* c += d; b ^= c; b <<<= 12 */
+    state[c] += state[d];
+    state[b] ^= state[c];
+    state[b] = ROTL32( state[b], 12 );
+
+    /* a += b; d ^= a; d <<<= 8; */
+    state[a] += state[b];
+    state[d] ^= state[a];
+    state[d] = ROTL32( state[d], 8 );
+
+    /* c += d; b ^= c; b <<<= 7; */
+    state[c] += state[d];
+    state[b] ^= state[c];
+    state[b] = ROTL32( state[b], 7 );
+}
+
+/**
+ * \brief           Perform the ChaCha20 inner block operation.
+ *
+ *                  This function performs two rounds: the column round and the
+ *                  diagonal round.
+ *
+ * \param state     The ChaCha20 state to update.
+ */
+static void chacha20_inner_block( uint32_t state[16] )
+{
+    chacha20_quarter_round( state, 0, 4, 8,  12 );
+    chacha20_quarter_round( state, 1, 5, 9,  13 );
+    chacha20_quarter_round( state, 2, 6, 10, 14 );
+    chacha20_quarter_round( state, 3, 7, 11, 15 );
+
+    chacha20_quarter_round( state, 0, 5, 10, 15 );
+    chacha20_quarter_round( state, 1, 6, 11, 12 );
+    chacha20_quarter_round( state, 2, 7, 8,  13 );
+    chacha20_quarter_round( state, 3, 4, 9,  14 );
+}
+
+/**
+ * \brief               Generates a keystream block.
+ *
+ * \param initial_state The initial ChaCha20 state (key, nonce, counter).
+ * \param keystream     Generated keystream bytes are written to this buffer.
+ */
+static void chacha20_block( const uint32_t initial_state[16],
+                            unsigned char keystream[64] )
+{
+    uint32_t working_state[16];
+    size_t i;
+
+    memcpy( working_state,
+            initial_state,
+            CHACHA20_BLOCK_SIZE_BYTES );
+
+    for( i = 0U; i < 10U; i++ )
+        chacha20_inner_block( working_state );
+
+    working_state[ 0] += initial_state[ 0];
+    working_state[ 1] += initial_state[ 1];
+    working_state[ 2] += initial_state[ 2];
+    working_state[ 3] += initial_state[ 3];
+    working_state[ 4] += initial_state[ 4];
+    working_state[ 5] += initial_state[ 5];
+    working_state[ 6] += initial_state[ 6];
+    working_state[ 7] += initial_state[ 7];
+    working_state[ 8] += initial_state[ 8];
+    working_state[ 9] += initial_state[ 9];
+    working_state[10] += initial_state[10];
+    working_state[11] += initial_state[11];
+    working_state[12] += initial_state[12];
+    working_state[13] += initial_state[13];
+    working_state[14] += initial_state[14];
+    working_state[15] += initial_state[15];
+
+    for( i = 0U; i < 16; i++ )
+    {
+        size_t offset = i * 4U;
+
+        keystream[offset     ] = (unsigned char)( working_state[i]       );
+        keystream[offset + 1U] = (unsigned char)( working_state[i] >>  8 );
+        keystream[offset + 2U] = (unsigned char)( working_state[i] >> 16 );
+        keystream[offset + 3U] = (unsigned char)( working_state[i] >> 24 );
+    }
+
+    mbedtls_platform_zeroize( working_state, sizeof( working_state ) );
+}
+
+void mbedtls_chacha20_init( mbedtls_chacha20_context *ctx )
+{
+    if( ctx != NULL )
+    {
+        mbedtls_platform_zeroize( ctx->state, sizeof( ctx->state ) );
+        mbedtls_platform_zeroize( ctx->keystream8, sizeof( ctx->keystream8 ) );
+
+        /* Initially, there's no keystream bytes available */
+        ctx->keystream_bytes_used = CHACHA20_BLOCK_SIZE_BYTES;
+    }
+}
+
+void mbedtls_chacha20_free( mbedtls_chacha20_context *ctx )
+{
+    if( ctx != NULL )
+    {
+        mbedtls_platform_zeroize( ctx, sizeof( mbedtls_chacha20_context ) );
+    }
+}
+
+int mbedtls_chacha20_setkey( mbedtls_chacha20_context *ctx,
+                            const unsigned char key[32] )
+{
+    if( ( ctx == NULL ) || ( key == NULL ) )
+    {
+        return( MBEDTLS_ERR_CHACHA20_BAD_INPUT_DATA );
+    }
+
+    /* ChaCha20 constants - the string "expand 32-byte k" */
+    ctx->state[0] = 0x61707865;
+    ctx->state[1] = 0x3320646e;
+    ctx->state[2] = 0x79622d32;
+    ctx->state[3] = 0x6b206574;
+
+    /* Set key */
+    ctx->state[4]  = BYTES_TO_U32_LE( key, 0 );
+    ctx->state[5]  = BYTES_TO_U32_LE( key, 4 );
+    ctx->state[6]  = BYTES_TO_U32_LE( key, 8 );
+    ctx->state[7]  = BYTES_TO_U32_LE( key, 12 );
+    ctx->state[8]  = BYTES_TO_U32_LE( key, 16 );
+    ctx->state[9]  = BYTES_TO_U32_LE( key, 20 );
+    ctx->state[10] = BYTES_TO_U32_LE( key, 24 );
+    ctx->state[11] = BYTES_TO_U32_LE( key, 28 );
+
+    return( 0 );
+}
+
+int mbedtls_chacha20_starts( mbedtls_chacha20_context* ctx,
+                             const unsigned char nonce[12],
+                             uint32_t counter )
+{
+    if( ( ctx == NULL ) || ( nonce == NULL ) )
+    {
+        return( MBEDTLS_ERR_CHACHA20_BAD_INPUT_DATA );
+    }
+
+    /* Counter */
+    ctx->state[12] = counter;
+
+    /* Nonce */
+    ctx->state[13] = BYTES_TO_U32_LE( nonce, 0 );
+    ctx->state[14] = BYTES_TO_U32_LE( nonce, 4 );
+    ctx->state[15] = BYTES_TO_U32_LE( nonce, 8 );
+
+    mbedtls_platform_zeroize( ctx->keystream8, sizeof( ctx->keystream8 ) );
+
+    /* Initially, there's no keystream bytes available */
+    ctx->keystream_bytes_used = CHACHA20_BLOCK_SIZE_BYTES;
+
+    return( 0 );
+}
+
+int mbedtls_chacha20_update( mbedtls_chacha20_context *ctx,
+                              size_t size,
+                              const unsigned char *input,
+                              unsigned char *output )
+{
+    size_t offset = 0U;
+    size_t i;
+
+    if( ctx == NULL )
+    {
+        return( MBEDTLS_ERR_CHACHA20_BAD_INPUT_DATA );
+    }
+    else if( ( size > 0U ) && ( ( input == NULL ) || ( output == NULL ) ) )
+    {
+        /* input and output pointers are allowed to be NULL only if size == 0 */
+        return( MBEDTLS_ERR_CHACHA20_BAD_INPUT_DATA );
+    }
+
+    /* Use leftover keystream bytes, if available */
+    while( size > 0U && ctx->keystream_bytes_used < CHACHA20_BLOCK_SIZE_BYTES )
+    {
+        output[offset] = input[offset]
+                       ^ ctx->keystream8[ctx->keystream_bytes_used];
+
+        ctx->keystream_bytes_used++;
+        offset++;
+        size--;
+    }
+
+    /* Process full blocks */
+    while( size >= CHACHA20_BLOCK_SIZE_BYTES )
+    {
+        /* Generate new keystream block and increment counter */
+        chacha20_block( ctx->state, ctx->keystream8 );
+        ctx->state[CHACHA20_CTR_INDEX]++;
+
+        for( i = 0U; i < 64U; i += 8U )
+        {
+            output[offset + i  ] = input[offset + i  ] ^ ctx->keystream8[i  ];
+            output[offset + i+1] = input[offset + i+1] ^ ctx->keystream8[i+1];
+            output[offset + i+2] = input[offset + i+2] ^ ctx->keystream8[i+2];
+            output[offset + i+3] = input[offset + i+3] ^ ctx->keystream8[i+3];
+            output[offset + i+4] = input[offset + i+4] ^ ctx->keystream8[i+4];
+            output[offset + i+5] = input[offset + i+5] ^ ctx->keystream8[i+5];
+            output[offset + i+6] = input[offset + i+6] ^ ctx->keystream8[i+6];
+            output[offset + i+7] = input[offset + i+7] ^ ctx->keystream8[i+7];
+        }
+
+        offset += CHACHA20_BLOCK_SIZE_BYTES;
+        size   -= CHACHA20_BLOCK_SIZE_BYTES;
+    }
+
+    /* Last (partial) block */
+    if( size > 0U )
+    {
+        /* Generate new keystream block and increment counter */
+        chacha20_block( ctx->state, ctx->keystream8 );
+        ctx->state[CHACHA20_CTR_INDEX]++;
+
+        for( i = 0U; i < size; i++)
+        {
+            output[offset + i] = input[offset + i] ^ ctx->keystream8[i];
+        }
+
+        ctx->keystream_bytes_used = size;
+
+    }
+
+    return( 0 );
+}
+
+int mbedtls_chacha20_crypt( const unsigned char key[32],
+                            const unsigned char nonce[12],
+                            uint32_t counter,
+                            size_t data_len,
+                            const unsigned char* input,
+                            unsigned char* output )
+{
+    mbedtls_chacha20_context ctx;
+    int ret;
+
+    mbedtls_chacha20_init( &ctx );
+
+    ret = mbedtls_chacha20_setkey( &ctx, key );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_chacha20_starts( &ctx, nonce, counter );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_chacha20_update( &ctx, data_len, input, output );
+
+cleanup:
+    mbedtls_chacha20_free( &ctx );
+    return( ret );
+}
+
+#endif /* !MBEDTLS_CHACHA20_ALT */
+
+#if defined(MBEDTLS_SELF_TEST)
+
+static const unsigned char test_keys[2][32] =
+{
+    {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+    },
+    {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01
+    }
+};
+
+static const unsigned char test_nonces[2][12] =
+{
+    {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00
+    },
+    {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x02
+    }
+};
+
+static const uint32_t test_counters[2] =
+{
+    0U,
+    1U
+};
+
+static const unsigned char test_input[2][375] =
+{
+    {
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+    },
+    {
+        0x41, 0x6e, 0x79, 0x20, 0x73, 0x75, 0x62, 0x6d,
+        0x69, 0x73, 0x73, 0x69, 0x6f, 0x6e, 0x20, 0x74,
+        0x6f, 0x20, 0x74, 0x68, 0x65, 0x20, 0x49, 0x45,
+        0x54, 0x46, 0x20, 0x69, 0x6e, 0x74, 0x65, 0x6e,
+        0x64, 0x65, 0x64, 0x20, 0x62, 0x79, 0x20, 0x74,
+        0x68, 0x65, 0x20, 0x43, 0x6f, 0x6e, 0x74, 0x72,
+        0x69, 0x62, 0x75, 0x74, 0x6f, 0x72, 0x20, 0x66,
+        0x6f, 0x72, 0x20, 0x70, 0x75, 0x62, 0x6c, 0x69,
+        0x63, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x61,
+        0x73, 0x20, 0x61, 0x6c, 0x6c, 0x20, 0x6f, 0x72,
+        0x20, 0x70, 0x61, 0x72, 0x74, 0x20, 0x6f, 0x66,
+        0x20, 0x61, 0x6e, 0x20, 0x49, 0x45, 0x54, 0x46,
+        0x20, 0x49, 0x6e, 0x74, 0x65, 0x72, 0x6e, 0x65,
+        0x74, 0x2d, 0x44, 0x72, 0x61, 0x66, 0x74, 0x20,
+        0x6f, 0x72, 0x20, 0x52, 0x46, 0x43, 0x20, 0x61,
+        0x6e, 0x64, 0x20, 0x61, 0x6e, 0x79, 0x20, 0x73,
+        0x74, 0x61, 0x74, 0x65, 0x6d, 0x65, 0x6e, 0x74,
+        0x20, 0x6d, 0x61, 0x64, 0x65, 0x20, 0x77, 0x69,
+        0x74, 0x68, 0x69, 0x6e, 0x20, 0x74, 0x68, 0x65,
+        0x20, 0x63, 0x6f, 0x6e, 0x74, 0x65, 0x78, 0x74,
+        0x20, 0x6f, 0x66, 0x20, 0x61, 0x6e, 0x20, 0x49,
+        0x45, 0x54, 0x46, 0x20, 0x61, 0x63, 0x74, 0x69,
+        0x76, 0x69, 0x74, 0x79, 0x20, 0x69, 0x73, 0x20,
+        0x63, 0x6f, 0x6e, 0x73, 0x69, 0x64, 0x65, 0x72,
+        0x65, 0x64, 0x20, 0x61, 0x6e, 0x20, 0x22, 0x49,
+        0x45, 0x54, 0x46, 0x20, 0x43, 0x6f, 0x6e, 0x74,
+        0x72, 0x69, 0x62, 0x75, 0x74, 0x69, 0x6f, 0x6e,
+        0x22, 0x2e, 0x20, 0x53, 0x75, 0x63, 0x68, 0x20,
+        0x73, 0x74, 0x61, 0x74, 0x65, 0x6d, 0x65, 0x6e,
+        0x74, 0x73, 0x20, 0x69, 0x6e, 0x63, 0x6c, 0x75,
+        0x64, 0x65, 0x20, 0x6f, 0x72, 0x61, 0x6c, 0x20,
+        0x73, 0x74, 0x61, 0x74, 0x65, 0x6d, 0x65, 0x6e,
+        0x74, 0x73, 0x20, 0x69, 0x6e, 0x20, 0x49, 0x45,
+        0x54, 0x46, 0x20, 0x73, 0x65, 0x73, 0x73, 0x69,
+        0x6f, 0x6e, 0x73, 0x2c, 0x20, 0x61, 0x73, 0x20,
+        0x77, 0x65, 0x6c, 0x6c, 0x20, 0x61, 0x73, 0x20,
+        0x77, 0x72, 0x69, 0x74, 0x74, 0x65, 0x6e, 0x20,
+        0x61, 0x6e, 0x64, 0x20, 0x65, 0x6c, 0x65, 0x63,
+        0x74, 0x72, 0x6f, 0x6e, 0x69, 0x63, 0x20, 0x63,
+        0x6f, 0x6d, 0x6d, 0x75, 0x6e, 0x69, 0x63, 0x61,
+        0x74, 0x69, 0x6f, 0x6e, 0x73, 0x20, 0x6d, 0x61,
+        0x64, 0x65, 0x20, 0x61, 0x74, 0x20, 0x61, 0x6e,
+        0x79, 0x20, 0x74, 0x69, 0x6d, 0x65, 0x20, 0x6f,
+        0x72, 0x20, 0x70, 0x6c, 0x61, 0x63, 0x65, 0x2c,
+        0x20, 0x77, 0x68, 0x69, 0x63, 0x68, 0x20, 0x61,
+        0x72, 0x65, 0x20, 0x61, 0x64, 0x64, 0x72, 0x65,
+        0x73, 0x73, 0x65, 0x64, 0x20, 0x74, 0x6f
+    }
+};
+
+static const unsigned char test_output[2][375] =
+{
+    {
+        0x76, 0xb8, 0xe0, 0xad, 0xa0, 0xf1, 0x3d, 0x90,
+        0x40, 0x5d, 0x6a, 0xe5, 0x53, 0x86, 0xbd, 0x28,
+        0xbd, 0xd2, 0x19, 0xb8, 0xa0, 0x8d, 0xed, 0x1a,
+        0xa8, 0x36, 0xef, 0xcc, 0x8b, 0x77, 0x0d, 0xc7,
+        0xda, 0x41, 0x59, 0x7c, 0x51, 0x57, 0x48, 0x8d,
+        0x77, 0x24, 0xe0, 0x3f, 0xb8, 0xd8, 0x4a, 0x37,
+        0x6a, 0x43, 0xb8, 0xf4, 0x15, 0x18, 0xa1, 0x1c,
+        0xc3, 0x87, 0xb6, 0x69, 0xb2, 0xee, 0x65, 0x86
+    },
+    {
+        0xa3, 0xfb, 0xf0, 0x7d, 0xf3, 0xfa, 0x2f, 0xde,
+        0x4f, 0x37, 0x6c, 0xa2, 0x3e, 0x82, 0x73, 0x70,
+        0x41, 0x60, 0x5d, 0x9f, 0x4f, 0x4f, 0x57, 0xbd,
+        0x8c, 0xff, 0x2c, 0x1d, 0x4b, 0x79, 0x55, 0xec,
+        0x2a, 0x97, 0x94, 0x8b, 0xd3, 0x72, 0x29, 0x15,
+        0xc8, 0xf3, 0xd3, 0x37, 0xf7, 0xd3, 0x70, 0x05,
+        0x0e, 0x9e, 0x96, 0xd6, 0x47, 0xb7, 0xc3, 0x9f,
+        0x56, 0xe0, 0x31, 0xca, 0x5e, 0xb6, 0x25, 0x0d,
+        0x40, 0x42, 0xe0, 0x27, 0x85, 0xec, 0xec, 0xfa,
+        0x4b, 0x4b, 0xb5, 0xe8, 0xea, 0xd0, 0x44, 0x0e,
+        0x20, 0xb6, 0xe8, 0xdb, 0x09, 0xd8, 0x81, 0xa7,
+        0xc6, 0x13, 0x2f, 0x42, 0x0e, 0x52, 0x79, 0x50,
+        0x42, 0xbd, 0xfa, 0x77, 0x73, 0xd8, 0xa9, 0x05,
+        0x14, 0x47, 0xb3, 0x29, 0x1c, 0xe1, 0x41, 0x1c,
+        0x68, 0x04, 0x65, 0x55, 0x2a, 0xa6, 0xc4, 0x05,
+        0xb7, 0x76, 0x4d, 0x5e, 0x87, 0xbe, 0xa8, 0x5a,
+        0xd0, 0x0f, 0x84, 0x49, 0xed, 0x8f, 0x72, 0xd0,
+        0xd6, 0x62, 0xab, 0x05, 0x26, 0x91, 0xca, 0x66,
+        0x42, 0x4b, 0xc8, 0x6d, 0x2d, 0xf8, 0x0e, 0xa4,
+        0x1f, 0x43, 0xab, 0xf9, 0x37, 0xd3, 0x25, 0x9d,
+        0xc4, 0xb2, 0xd0, 0xdf, 0xb4, 0x8a, 0x6c, 0x91,
+        0x39, 0xdd, 0xd7, 0xf7, 0x69, 0x66, 0xe9, 0x28,
+        0xe6, 0x35, 0x55, 0x3b, 0xa7, 0x6c, 0x5c, 0x87,
+        0x9d, 0x7b, 0x35, 0xd4, 0x9e, 0xb2, 0xe6, 0x2b,
+        0x08, 0x71, 0xcd, 0xac, 0x63, 0x89, 0x39, 0xe2,
+        0x5e, 0x8a, 0x1e, 0x0e, 0xf9, 0xd5, 0x28, 0x0f,
+        0xa8, 0xca, 0x32, 0x8b, 0x35, 0x1c, 0x3c, 0x76,
+        0x59, 0x89, 0xcb, 0xcf, 0x3d, 0xaa, 0x8b, 0x6c,
+        0xcc, 0x3a, 0xaf, 0x9f, 0x39, 0x79, 0xc9, 0x2b,
+        0x37, 0x20, 0xfc, 0x88, 0xdc, 0x95, 0xed, 0x84,
+        0xa1, 0xbe, 0x05, 0x9c, 0x64, 0x99, 0xb9, 0xfd,
+        0xa2, 0x36, 0xe7, 0xe8, 0x18, 0xb0, 0x4b, 0x0b,
+        0xc3, 0x9c, 0x1e, 0x87, 0x6b, 0x19, 0x3b, 0xfe,
+        0x55, 0x69, 0x75, 0x3f, 0x88, 0x12, 0x8c, 0xc0,
+        0x8a, 0xaa, 0x9b, 0x63, 0xd1, 0xa1, 0x6f, 0x80,
+        0xef, 0x25, 0x54, 0xd7, 0x18, 0x9c, 0x41, 0x1f,
+        0x58, 0x69, 0xca, 0x52, 0xc5, 0xb8, 0x3f, 0xa3,
+        0x6f, 0xf2, 0x16, 0xb9, 0xc1, 0xd3, 0x00, 0x62,
+        0xbe, 0xbc, 0xfd, 0x2d, 0xc5, 0xbc, 0xe0, 0x91,
+        0x19, 0x34, 0xfd, 0xa7, 0x9a, 0x86, 0xf6, 0xe6,
+        0x98, 0xce, 0xd7, 0x59, 0xc3, 0xff, 0x9b, 0x64,
+        0x77, 0x33, 0x8f, 0x3d, 0xa4, 0xf9, 0xcd, 0x85,
+        0x14, 0xea, 0x99, 0x82, 0xcc, 0xaf, 0xb3, 0x41,
+        0xb2, 0x38, 0x4d, 0xd9, 0x02, 0xf3, 0xd1, 0xab,
+        0x7a, 0xc6, 0x1d, 0xd2, 0x9c, 0x6f, 0x21, 0xba,
+        0x5b, 0x86, 0x2f, 0x37, 0x30, 0xe3, 0x7c, 0xfd,
+        0xc4, 0xfd, 0x80, 0x6c, 0x22, 0xf2, 0x21
+    }
+};
+
+static const size_t test_lengths[2] =
+{
+    64U,
+    375U
+};
+
+#define ASSERT( cond, args )            \
+    do                                  \
+    {                                   \
+        if( ! ( cond ) )                \
+        {                               \
+            if( verbose != 0 )          \
+                mbedtls_printf args;    \
+                                        \
+            return( -1 );               \
+        }                               \
+    }                                   \
+    while( 0 )
+
+int mbedtls_chacha20_self_test( int verbose )
+{
+    unsigned char output[381];
+    unsigned i;
+    int ret;
+
+    for( i = 0U; i < 2U; i++ )
+    {
+        if( verbose != 0 )
+            mbedtls_printf( "  ChaCha20 test %u ", i );
+
+        ret = mbedtls_chacha20_crypt( test_keys[i],
+                                      test_nonces[i],
+                                      test_counters[i],
+                                      test_lengths[i],
+                                      test_input[i],
+                                      output );
+
+        ASSERT( 0 == ret, ( "error code: %i\n", ret ) );
+
+        ASSERT( 0 == memcmp( output, test_output[i], test_lengths[i] ),
+                ( "failed (output)\n" ) );
+
+        if( verbose != 0 )
+            mbedtls_printf( "passed\n" );
+    }
+
+    if( verbose != 0 )
+        mbedtls_printf( "\n" );
+
+    return( 0 );
+}
+
+#endif /* MBEDTLS_SELF_TEST */
+
+#endif /* !MBEDTLS_CHACHA20_C */
diff --git a/library/chachapoly.c b/library/chachapoly.c
new file mode 100644
index 00000000..860f8776
--- /dev/null
+++ b/library/chachapoly.c
@@ -0,0 +1,547 @@
+/**
+ * \file chachapoly.c
+ *
+ * \brief ChaCha20-Poly1305 AEAD construction based on RFC 7539.
+ *
+ *  Copyright (C) 2006-2016, ARM Limited, All Rights Reserved
+ *  SPDX-License-Identifier: Apache-2.0
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License"); you may
+ *  not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ *  WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ *
+ *  This file is part of mbed TLS (https://tls.mbed.org)
+ */
+#if !defined(MBEDTLS_CONFIG_FILE)
+#include "mbedtls/config.h"
+#else
+#include MBEDTLS_CONFIG_FILE
+#endif
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+
+#include "mbedtls/chachapoly.h"
+#include "mbedtls/platform_util.h"
+
+#include <string.h>
+
+#if defined(MBEDTLS_SELF_TEST)
+#if defined(MBEDTLS_PLATFORM_C)
+#include "mbedtls/platform.h"
+#else
+#include <stdio.h>
+#define mbedtls_printf printf
+#endif /* MBEDTLS_PLATFORM_C */
+#endif /* MBEDTLS_SELF_TEST */
+
+#if !defined(MBEDTLS_CHACHAPOLY_ALT)
+
+#define CHACHAPOLY_STATE_INIT       ( 0 )
+#define CHACHAPOLY_STATE_AAD        ( 1 )
+#define CHACHAPOLY_STATE_CIPHERTEXT ( 2 ) /* Encrypting or decrypting */
+#define CHACHAPOLY_STATE_FINISHED   ( 3 )
+
+/**
+ * \brief           Adds nul bytes to pad the AAD for Poly1305.
+ *
+ * \param ctx       The ChaCha20-Poly1305 context.
+ */
+static int chachapoly_pad_aad( mbedtls_chachapoly_context *ctx )
+{
+    uint32_t partial_block_len = (uint32_t) ( ctx->aad_len % 16U );
+    unsigned char zeroes[15];
+
+    if( partial_block_len == 0U )
+        return( 0 );
+
+    memset( zeroes, 0, sizeof( zeroes ) );
+
+    return( mbedtls_poly1305_update( &ctx->poly1305_ctx,
+                                     zeroes,
+                                     16U - partial_block_len ) );
+}
+
+/**
+ * \brief           Adds nul bytes to pad the ciphertext for Poly1305.
+ *
+ * \param ctx       The ChaCha20-Poly1305 context.
+ */
+static int chachapoly_pad_ciphertext( mbedtls_chachapoly_context *ctx )
+{
+    uint32_t partial_block_len = (uint32_t) ( ctx->ciphertext_len % 16U );
+    unsigned char zeroes[15];
+
+    if( partial_block_len == 0U )
+        return( 0 );
+
+    memset( zeroes, 0, sizeof( zeroes ) );
+    return( mbedtls_poly1305_update( &ctx->poly1305_ctx,
+                                     zeroes,
+                                     16U - partial_block_len ) );
+}
+
+void mbedtls_chachapoly_init( mbedtls_chachapoly_context *ctx )
+{
+    if( ctx != NULL )
+    {
+        mbedtls_chacha20_init( &ctx->chacha20_ctx );
+        mbedtls_poly1305_init( &ctx->poly1305_ctx );
+        ctx->aad_len        = 0U;
+        ctx->ciphertext_len = 0U;
+        ctx->state          = CHACHAPOLY_STATE_INIT;
+        ctx->mode           = MBEDTLS_CHACHAPOLY_ENCRYPT;
+    }
+}
+
+void mbedtls_chachapoly_free( mbedtls_chachapoly_context *ctx )
+{
+    if( ctx != NULL )
+    {
+        mbedtls_chacha20_free( &ctx->chacha20_ctx );
+        mbedtls_poly1305_free( &ctx->poly1305_ctx );
+        ctx->aad_len        = 0U;
+        ctx->ciphertext_len = 0U;
+        ctx->state          = CHACHAPOLY_STATE_INIT;
+        ctx->mode           = MBEDTLS_CHACHAPOLY_ENCRYPT;
+    }
+}
+
+int mbedtls_chachapoly_setkey( mbedtls_chachapoly_context *ctx,
+                               const unsigned char key[32] )
+{
+    int ret;
+
+    if( ( ctx == NULL ) || ( key == NULL ) )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+
+    ret = mbedtls_chacha20_setkey( &ctx->chacha20_ctx, key );
+
+    return( ret );
+}
+
+int mbedtls_chachapoly_starts( mbedtls_chachapoly_context *ctx,
+                               const unsigned char nonce[12],
+                               mbedtls_chachapoly_mode_t mode  )
+{
+    int ret;
+    unsigned char poly1305_key[64];
+
+    if( ( ctx == NULL ) || ( nonce == NULL ) )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+
+    /* Set counter = 0, will be update to 1 when generating Poly1305 key */
+    ret = mbedtls_chacha20_starts( &ctx->chacha20_ctx, nonce, 0U );
+    if( ret != 0 )
+        goto cleanup;
+
+    /* Generate the Poly1305 key by getting the ChaCha20 keystream output with
+     * counter = 0.  This is the same as encrypting a buffer of zeroes.
+     * Only the first 256-bits (32 bytes) of the key is used for Poly1305.
+     * The other 256 bits are discarded.
+     */
+    memset( poly1305_key, 0, sizeof( poly1305_key ) );
+    ret = mbedtls_chacha20_update( &ctx->chacha20_ctx, sizeof( poly1305_key ),
+                                      poly1305_key, poly1305_key );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_poly1305_starts( &ctx->poly1305_ctx, poly1305_key );
+
+    if( ret == 0 )
+    {
+        ctx->aad_len        = 0U;
+        ctx->ciphertext_len = 0U;
+        ctx->state          = CHACHAPOLY_STATE_AAD;
+        ctx->mode           = mode;
+    }
+
+cleanup:
+    mbedtls_platform_zeroize( poly1305_key, 64U );
+    return( ret );
+}
+
+int mbedtls_chachapoly_update_aad( mbedtls_chachapoly_context *ctx,
+                                   const unsigned char *aad,
+                                   size_t aad_len )
+{
+    if( ctx == NULL )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+    else if( ( aad_len > 0U ) && ( aad == NULL ) )
+    {
+        /* aad pointer is allowed to be NULL if aad_len == 0 */
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+    else if( ctx->state != CHACHAPOLY_STATE_AAD )
+    {
+        return( MBEDTLS_ERR_CHACHAPOLY_BAD_STATE );
+    }
+
+    ctx->aad_len += aad_len;
+
+    return( mbedtls_poly1305_update( &ctx->poly1305_ctx, aad, aad_len ) );
+}
+
+int mbedtls_chachapoly_update( mbedtls_chachapoly_context *ctx,
+                               size_t len,
+                               const unsigned char *input,
+                               unsigned char *output )
+{
+    int ret;
+
+    if( ctx == NULL )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+    else if( ( len > 0U ) && ( ( input == NULL ) || ( output == NULL ) ) )
+    {
+        /* input and output pointers are allowed to be NULL if len == 0 */
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+    else if( ( ctx->state != CHACHAPOLY_STATE_AAD ) &&
+              ( ctx->state != CHACHAPOLY_STATE_CIPHERTEXT ) )
+    {
+        return( MBEDTLS_ERR_CHACHAPOLY_BAD_STATE );
+    }
+
+    if( ctx->state == CHACHAPOLY_STATE_AAD )
+    {
+        ctx->state = CHACHAPOLY_STATE_CIPHERTEXT;
+
+        ret = chachapoly_pad_aad( ctx );
+        if( ret != 0 )
+            return( ret );
+    }
+
+    ctx->ciphertext_len += len;
+
+    if( ctx->mode == MBEDTLS_CHACHAPOLY_ENCRYPT )
+    {
+        ret = mbedtls_chacha20_update( &ctx->chacha20_ctx, len, input, output );
+        if( ret != 0 )
+            return( ret );
+
+        ret = mbedtls_poly1305_update( &ctx->poly1305_ctx, output, len );
+        if( ret != 0 )
+            return( ret );
+    }
+    else /* DECRYPT */
+    {
+        ret = mbedtls_poly1305_update( &ctx->poly1305_ctx, input, len );
+        if( ret != 0 )
+            return( ret );
+
+        ret = mbedtls_chacha20_update( &ctx->chacha20_ctx, len, input, output );
+        if( ret != 0 )
+            return( ret );
+    }
+
+    return( 0 );
+}
+
+int mbedtls_chachapoly_finish( mbedtls_chachapoly_context *ctx,
+                               unsigned char mac[16] )
+{
+    int ret;
+    unsigned char len_block[16];
+
+    if( ( ctx == NULL ) || ( mac == NULL ) )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+    else if( ctx->state == CHACHAPOLY_STATE_INIT )
+    {
+        return( MBEDTLS_ERR_CHACHAPOLY_BAD_STATE );
+    }
+
+    if( ctx->state == CHACHAPOLY_STATE_AAD )
+    {
+        ret = chachapoly_pad_aad( ctx );
+        if( ret != 0 )
+            return( ret );
+    }
+    else if( ctx->state == CHACHAPOLY_STATE_CIPHERTEXT )
+    {
+        ret = chachapoly_pad_ciphertext( ctx );
+        if( ret != 0 )
+            return( ret );
+    }
+
+    ctx->state = CHACHAPOLY_STATE_FINISHED;
+
+    /* The lengths of the AAD and ciphertext are processed by
+     * Poly1305 as the final 128-bit block, encoded as little-endian integers.
+     */
+    len_block[ 0] = (unsigned char)( ctx->aad_len       );
+    len_block[ 1] = (unsigned char)( ctx->aad_len >>  8 );
+    len_block[ 2] = (unsigned char)( ctx->aad_len >> 16 );
+    len_block[ 3] = (unsigned char)( ctx->aad_len >> 24 );
+    len_block[ 4] = (unsigned char)( ctx->aad_len >> 32 );
+    len_block[ 5] = (unsigned char)( ctx->aad_len >> 40 );
+    len_block[ 6] = (unsigned char)( ctx->aad_len >> 48 );
+    len_block[ 7] = (unsigned char)( ctx->aad_len >> 56 );
+    len_block[ 8] = (unsigned char)( ctx->ciphertext_len       );
+    len_block[ 9] = (unsigned char)( ctx->ciphertext_len >>  8 );
+    len_block[10] = (unsigned char)( ctx->ciphertext_len >> 16 );
+    len_block[11] = (unsigned char)( ctx->ciphertext_len >> 24 );
+    len_block[12] = (unsigned char)( ctx->ciphertext_len >> 32 );
+    len_block[13] = (unsigned char)( ctx->ciphertext_len >> 40 );
+    len_block[14] = (unsigned char)( ctx->ciphertext_len >> 48 );
+    len_block[15] = (unsigned char)( ctx->ciphertext_len >> 56 );
+
+    ret = mbedtls_poly1305_update( &ctx->poly1305_ctx, len_block, 16U );
+    if( ret != 0 )
+        return( ret );
+
+    ret = mbedtls_poly1305_finish( &ctx->poly1305_ctx, mac );
+
+    return( ret );
+}
+
+static int chachapoly_crypt_and_tag( mbedtls_chachapoly_context *ctx,
+                                     mbedtls_chachapoly_mode_t mode,
+                                     size_t length,
+                                     const unsigned char nonce[12],
+                                     const unsigned char *aad,
+                                     size_t aad_len,
+                                     const unsigned char *input,
+                                     unsigned char *output,
+                                     unsigned char tag[16] )
+{
+    int ret;
+
+    ret = mbedtls_chachapoly_starts( ctx, nonce, mode );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_chachapoly_update_aad( ctx, aad, aad_len );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_chachapoly_update( ctx, length, input, output );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_chachapoly_finish( ctx, tag );
+
+cleanup:
+    return( ret );
+}
+
+int mbedtls_chachapoly_encrypt_and_tag( mbedtls_chachapoly_context *ctx,
+                                        size_t length,
+                                        const unsigned char nonce[12],
+                                        const unsigned char *aad,
+                                        size_t aad_len,
+                                        const unsigned char *input,
+                                        unsigned char *output,
+                                        unsigned char tag[16] )
+{
+    return( chachapoly_crypt_and_tag( ctx, MBEDTLS_CHACHAPOLY_ENCRYPT,
+                                      length, nonce, aad, aad_len,
+                                      input, output, tag ) );
+}
+
+int mbedtls_chachapoly_auth_decrypt( mbedtls_chachapoly_context *ctx,
+                                     size_t length,
+                                     const unsigned char nonce[12],
+                                     const unsigned char *aad,
+                                     size_t aad_len,
+                                     const unsigned char tag[16],
+                                     const unsigned char *input,
+                                     unsigned char *output )
+{
+    int ret;
+    unsigned char check_tag[16];
+    size_t i;
+    int diff;
+
+    if( tag == NULL )
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+
+    if( ( ret = chachapoly_crypt_and_tag( ctx,
+                        MBEDTLS_CHACHAPOLY_DECRYPT, length, nonce,
+                        aad, aad_len, input, output, check_tag ) ) != 0 )
+    {
+        return( ret );
+    }
+
+    /* Check tag in "constant-time" */
+    for( diff = 0, i = 0; i < sizeof( check_tag ); i++ )
+        diff |= tag[i] ^ check_tag[i];
+
+    if( diff != 0 )
+    {
+        mbedtls_platform_zeroize( output, length );
+        return( MBEDTLS_ERR_CHACHAPOLY_AUTH_FAILED );
+    }
+
+    return( 0 );
+}
+
+#endif /* MBEDTLS_CHACHAPOLY_ALT */
+
+#if defined(MBEDTLS_SELF_TEST)
+
+static const unsigned char test_key[1][32] =
+{
+    {
+        0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+        0x88, 0x89, 0x8a, 0x8b, 0x8c, 0x8d, 0x8e, 0x8f,
+        0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97,
+        0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f
+    }
+};
+
+static const unsigned char test_nonce[1][12] =
+{
+    {
+        0x07, 0x00, 0x00, 0x00,                         /* 32-bit common part */
+        0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47  /* 64-bit IV */
+    }
+};
+
+static const unsigned char test_aad[1][12] =
+{
+    {
+        0x50, 0x51, 0x52, 0x53, 0xc0, 0xc1, 0xc2, 0xc3,
+        0xc4, 0xc5, 0xc6, 0xc7
+    }
+};
+
+static const size_t test_aad_len[1] =
+{
+    12U
+};
+
+static const unsigned char test_input[1][114] =
+{
+    {
+        0x4c, 0x61, 0x64, 0x69, 0x65, 0x73, 0x20, 0x61,
+        0x6e, 0x64, 0x20, 0x47, 0x65, 0x6e, 0x74, 0x6c,
+        0x65, 0x6d, 0x65, 0x6e, 0x20, 0x6f, 0x66, 0x20,
+        0x74, 0x68, 0x65, 0x20, 0x63, 0x6c, 0x61, 0x73,
+        0x73, 0x20, 0x6f, 0x66, 0x20, 0x27, 0x39, 0x39,
+        0x3a, 0x20, 0x49, 0x66, 0x20, 0x49, 0x20, 0x63,
+        0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6f, 0x66, 0x66,
+        0x65, 0x72, 0x20, 0x79, 0x6f, 0x75, 0x20, 0x6f,
+        0x6e, 0x6c, 0x79, 0x20, 0x6f, 0x6e, 0x65, 0x20,
+        0x74, 0x69, 0x70, 0x20, 0x66, 0x6f, 0x72, 0x20,
+        0x74, 0x68, 0x65, 0x20, 0x66, 0x75, 0x74, 0x75,
+        0x72, 0x65, 0x2c, 0x20, 0x73, 0x75, 0x6e, 0x73,
+        0x63, 0x72, 0x65, 0x65, 0x6e, 0x20, 0x77, 0x6f,
+        0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x69,
+        0x74, 0x2e
+    }
+};
+
+static const unsigned char test_output[1][114] =
+{
+    {
+        0xd3, 0x1a, 0x8d, 0x34, 0x64, 0x8e, 0x60, 0xdb,
+        0x7b, 0x86, 0xaf, 0xbc, 0x53, 0xef, 0x7e, 0xc2,
+        0xa4, 0xad, 0xed, 0x51, 0x29, 0x6e, 0x08, 0xfe,
+        0xa9, 0xe2, 0xb5, 0xa7, 0x36, 0xee, 0x62, 0xd6,
+        0x3d, 0xbe, 0xa4, 0x5e, 0x8c, 0xa9, 0x67, 0x12,
+        0x82, 0xfa, 0xfb, 0x69, 0xda, 0x92, 0x72, 0x8b,
+        0x1a, 0x71, 0xde, 0x0a, 0x9e, 0x06, 0x0b, 0x29,
+        0x05, 0xd6, 0xa5, 0xb6, 0x7e, 0xcd, 0x3b, 0x36,
+        0x92, 0xdd, 0xbd, 0x7f, 0x2d, 0x77, 0x8b, 0x8c,
+        0x98, 0x03, 0xae, 0xe3, 0x28, 0x09, 0x1b, 0x58,
+        0xfa, 0xb3, 0x24, 0xe4, 0xfa, 0xd6, 0x75, 0x94,
+        0x55, 0x85, 0x80, 0x8b, 0x48, 0x31, 0xd7, 0xbc,
+        0x3f, 0xf4, 0xde, 0xf0, 0x8e, 0x4b, 0x7a, 0x9d,
+        0xe5, 0x76, 0xd2, 0x65, 0x86, 0xce, 0xc6, 0x4b,
+        0x61, 0x16
+    }
+};
+
+static const size_t test_input_len[1] =
+{
+    114U
+};
+
+static const unsigned char test_mac[1][16] =
+{
+    {
+        0x1a, 0xe1, 0x0b, 0x59, 0x4f, 0x09, 0xe2, 0x6a,
+        0x7e, 0x90, 0x2e, 0xcb, 0xd0, 0x60, 0x06, 0x91
+    }
+};
+
+#define ASSERT( cond, args )            \
+    do                                  \
+    {                                   \
+        if( ! ( cond ) )                \
+        {                               \
+            if( verbose != 0 )          \
+                mbedtls_printf args;    \
+                                        \
+            return( -1 );               \
+        }                               \
+    }                                   \
+    while( 0 )
+
+int mbedtls_chachapoly_self_test( int verbose )
+{
+    mbedtls_chachapoly_context ctx;
+    unsigned i;
+    int ret;
+    unsigned char output[200];
+    unsigned char mac[16];
+
+    for( i = 0U; i < 1U; i++ )
+    {
+        if( verbose != 0 )
+            mbedtls_printf( "  ChaCha20-Poly1305 test %u ", i );
+
+        mbedtls_chachapoly_init( &ctx );
+
+        ret = mbedtls_chachapoly_setkey( &ctx, test_key[i] );
+        ASSERT( 0 == ret, ( "setkey() error code: %i\n", ret ) );
+
+        ret = mbedtls_chachapoly_encrypt_and_tag( &ctx,
+                                                  test_input_len[i],
+                                                  test_nonce[i],
+                                                  test_aad[i],
+                                                  test_aad_len[i],
+                                                  test_input[i],
+                                                  output,
+                                                  mac );
+
+        ASSERT( 0 == ret, ( "crypt_and_tag() error code: %i\n", ret ) );
+
+        ASSERT( 0 == memcmp( output, test_output[i], test_input_len[i] ),
+                ( "failure (wrong output)\n" ) );
+
+        ASSERT( 0 == memcmp( mac, test_mac[i], 16U ),
+                ( "failure (wrong MAC)\n" ) );
+
+        mbedtls_chachapoly_free( &ctx );
+
+        if( verbose != 0 )
+            mbedtls_printf( "passed\n" );
+    }
+
+    if( verbose != 0 )
+        mbedtls_printf( "\n" );
+
+    return( 0 );
+}
+
+#endif /* MBEDTLS_SELF_TEST */
+
+#endif /* MBEDTLS_CHACHAPOLY_C */
diff --git a/library/cipher.c b/library/cipher.c
index 2d85228a..7ae6c4ac 100644
--- a/library/cipher.c
+++ b/library/cipher.c
@@ -38,6 +38,10 @@
 #include <stdlib.h>
 #include <string.h>
 
+#if defined(MBEDTLS_CHACHAPOLY_C)
+#include "mbedtls/chachapoly.h"
+#endif
+
 #if defined(MBEDTLS_GCM_C)
 #include "mbedtls/gcm.h"
 #endif
@@ -46,6 +50,10 @@
 #include "mbedtls/ccm.h"
 #endif
 
+#if defined(MBEDTLS_CHACHA20_C)
+#include "mbedtls/chacha20.h"
+#endif
+
 #if defined(MBEDTLS_CMAC_C)
 #include "mbedtls/cmac.h"
 #endif
@@ -57,9 +65,25 @@
 #define mbedtls_free   free
 #endif
 
-#if defined(MBEDTLS_ARC4_C) || defined(MBEDTLS_CIPHER_NULL_CIPHER)
-#define MBEDTLS_CIPHER_MODE_STREAM
-#endif
+#if defined(MBEDTLS_GCM_C) || defined(MBEDTLS_CHACHAPOLY_C)
+/* Compare the contents of two buffers in constant time.
+ * Returns 0 if the contents are bitwise identical, otherwise returns
+ * a non-zero value.
+ * This is currently only used by GCM and ChaCha20+Poly1305.
+ */
+static int mbedtls_constant_time_memcmp( const void *v1, const void *v2, size_t len )
+{
+    const unsigned char *p1 = (const unsigned char*) v1;
+    const unsigned char *p2 = (const unsigned char*) v2;
+    size_t i;
+    unsigned char diff;
+
+    for( diff = 0, i = 0; i < len; i++ )
+        diff |= p1[i] ^ p2[i];
+
+    return (int)diff;
+}
+#endif /* MBEDTLS_GCM_C || MBEDTLS_CHACHAPOLY_C */
 
 static int supported_init = 0;
 
@@ -232,6 +256,18 @@ int mbedtls_cipher_set_iv( mbedtls_cipher_context_t *ctx,
             return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
     }
 
+#if defined(MBEDTLS_CHACHA20_C)
+    if ( ctx->cipher_info->type == MBEDTLS_CIPHER_CHACHA20 )
+    {
+        if ( 0 != mbedtls_chacha20_starts( (mbedtls_chacha20_context*)ctx->cipher_ctx,
+                                           iv,
+                                           0U ) ) /* Initial counter value */
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+    }
+#endif
+
     memcpy( ctx->iv, iv, actual_iv_size );
     ctx->iv_size = actual_iv_size;
 
@@ -248,22 +284,45 @@ int mbedtls_cipher_reset( mbedtls_cipher_context_t *ctx )
     return( 0 );
 }
 
-#if defined(MBEDTLS_GCM_C)
+#if defined(MBEDTLS_GCM_C) || defined(MBEDTLS_CHACHAPOLY_C)
 int mbedtls_cipher_update_ad( mbedtls_cipher_context_t *ctx,
                       const unsigned char *ad, size_t ad_len )
 {
     if( NULL == ctx || NULL == ctx->cipher_info )
         return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
 
+#if defined(MBEDTLS_GCM_C)
     if( MBEDTLS_MODE_GCM == ctx->cipher_info->mode )
     {
         return mbedtls_gcm_starts( (mbedtls_gcm_context *) ctx->cipher_ctx, ctx->operation,
                            ctx->iv, ctx->iv_size, ad, ad_len );
     }
+#endif
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if (MBEDTLS_CIPHER_CHACHA20_POLY1305 == ctx->cipher_info->type )
+    {
+        int result;
+        mbedtls_chachapoly_mode_t mode;
+
+        mode = ( ctx->operation == MBEDTLS_ENCRYPT )
+                ? MBEDTLS_CHACHAPOLY_ENCRYPT
+                : MBEDTLS_CHACHAPOLY_DECRYPT;
+
+        result = mbedtls_chachapoly_starts( (mbedtls_chachapoly_context*) ctx->cipher_ctx,
+                                                        ctx->iv,
+                                                        mode );
+        if ( result != 0 )
+            return( result );
+
+        return mbedtls_chachapoly_update_aad( (mbedtls_chachapoly_context*) ctx->cipher_ctx,
+                                                          ad, ad_len );
+    }
+#endif
 
     return( 0 );
 }
-#endif /* MBEDTLS_GCM_C */
+#endif /* MBEDTLS_GCM_C || MBEDTLS_CHACHAPOLY_C */
 
 int mbedtls_cipher_update( mbedtls_cipher_context_t *ctx, const unsigned char *input,
                    size_t ilen, unsigned char *output, size_t *olen )
@@ -304,6 +363,15 @@ int mbedtls_cipher_update( mbedtls_cipher_context_t *ctx, const unsigned char *i
     }
 #endif
 
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if ( ctx->cipher_info->type == MBEDTLS_CIPHER_CHACHA20_POLY1305 )
+    {
+        *olen = ilen;
+        return mbedtls_chachapoly_update( (mbedtls_chachapoly_context*) ctx->cipher_ctx,
+                                                      ilen, input, output );
+    }
+#endif
+
     if ( 0 == block_size )
     {
         return MBEDTLS_ERR_CIPHER_INVALID_CONTEXT;
@@ -685,6 +753,12 @@ int mbedtls_cipher_finish( mbedtls_cipher_context_t *ctx,
         return( 0 );
     }
 
+    if ( ( MBEDTLS_CIPHER_CHACHA20          == ctx->cipher_info->type ) ||
+         ( MBEDTLS_CIPHER_CHACHA20_POLY1305 == ctx->cipher_info->type ) )
+    {
+        return( 0 );
+    }
+
     if( MBEDTLS_MODE_ECB == ctx->cipher_info->mode )
     {
         if( ctx->unprocessed_len != 0 )
@@ -796,7 +870,7 @@ int mbedtls_cipher_set_padding_mode( mbedtls_cipher_context_t *ctx, mbedtls_ciph
 }
 #endif /* MBEDTLS_CIPHER_MODE_WITH_PADDING */
 
-#if defined(MBEDTLS_GCM_C)
+#if defined(MBEDTLS_GCM_C) || defined(MBEDTLS_CHACHAPOLY_C)
 int mbedtls_cipher_write_tag( mbedtls_cipher_context_t *ctx,
                       unsigned char *tag, size_t tag_len )
 {
@@ -806,8 +880,22 @@ int mbedtls_cipher_write_tag( mbedtls_cipher_context_t *ctx,
     if( MBEDTLS_ENCRYPT != ctx->operation )
         return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
 
+#if defined(MBEDTLS_GCM_C)
     if( MBEDTLS_MODE_GCM == ctx->cipher_info->mode )
         return mbedtls_gcm_finish( (mbedtls_gcm_context *) ctx->cipher_ctx, tag, tag_len );
+#endif
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if ( MBEDTLS_CIPHER_CHACHA20_POLY1305 == ctx->cipher_info->type )
+    {
+        /* Don't allow truncated MAC for Poly1305 */
+        if ( tag_len != 16U )
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+        return mbedtls_chachapoly_finish( (mbedtls_chachapoly_context*) ctx->cipher_ctx,
+                                                      tag );
+    }
+#endif
 
     return( 0 );
 }
@@ -815,6 +903,7 @@ int mbedtls_cipher_write_tag( mbedtls_cipher_context_t *ctx,
 int mbedtls_cipher_check_tag( mbedtls_cipher_context_t *ctx,
                       const unsigned char *tag, size_t tag_len )
 {
+    unsigned char check_tag[16];
     int ret;
 
     if( NULL == ctx || NULL == ctx->cipher_info ||
@@ -823,12 +912,9 @@ int mbedtls_cipher_check_tag( mbedtls_cipher_context_t *ctx,
         return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
     }
 
+#if defined(MBEDTLS_GCM_C)
     if( MBEDTLS_MODE_GCM == ctx->cipher_info->mode )
     {
-        unsigned char check_tag[16];
-        size_t i;
-        int diff;
-
         if( tag_len > sizeof( check_tag ) )
             return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
 
@@ -839,18 +925,38 @@ int mbedtls_cipher_check_tag( mbedtls_cipher_context_t *ctx,
         }
 
         /* Check the tag in "constant-time" */
-        for( diff = 0, i = 0; i < tag_len; i++ )
-            diff |= tag[i] ^ check_tag[i];
+        if( mbedtls_constant_time_memcmp( tag, check_tag, tag_len ) != 0 )
+            return( MBEDTLS_ERR_CIPHER_AUTH_FAILED );
 
-        if( diff != 0 )
+        return( 0 );
+    }
+#endif /* MBEDTLS_GCM_C */
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if ( MBEDTLS_CIPHER_CHACHA20_POLY1305 == ctx->cipher_info->type )
+    {
+        /* Don't allow truncated MAC for Poly1305 */
+        if ( tag_len != sizeof( check_tag ) )
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+        ret = mbedtls_chachapoly_finish( (mbedtls_chachapoly_context*) ctx->cipher_ctx,
+                                                     check_tag );
+        if ( ret != 0 )
+        {
+            return( ret );
+        }
+
+        /* Check the tag in "constant-time" */
+        if( mbedtls_constant_time_memcmp( tag, check_tag, tag_len ) != 0 )
             return( MBEDTLS_ERR_CIPHER_AUTH_FAILED );
 
         return( 0 );
     }
+#endif /* MBEDTLS_CHACHAPOLY_C */
 
     return( 0 );
 }
-#endif /* MBEDTLS_GCM_C */
+#endif /* MBEDTLS_GCM_C || MBEDTLS_CHACHAPOLY_C */
 
 /*
  * Packet-oriented wrapper for non-AEAD modes
@@ -909,6 +1015,21 @@ int mbedtls_cipher_auth_encrypt( mbedtls_cipher_context_t *ctx,
                                      tag, tag_len ) );
     }
 #endif /* MBEDTLS_CCM_C */
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if ( MBEDTLS_CIPHER_CHACHA20_POLY1305 == ctx->cipher_info->type )
+    {
+        /* ChachaPoly has fixed length nonce and MAC (tag) */
+        if ( ( iv_len != ctx->cipher_info->iv_size ) ||
+             ( tag_len != 16U ) )
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        *olen = ilen;
+        return( mbedtls_chachapoly_encrypt_and_tag( ctx->cipher_ctx,
+                                ilen, iv, ad, ad_len, input, output, tag ) );
+    }
+#endif /* MBEDTLS_CHACHAPOLY_C */
 
     return( MBEDTLS_ERR_CIPHER_FEATURE_UNAVAILABLE );
 }
@@ -955,6 +1076,28 @@ int mbedtls_cipher_auth_decrypt( mbedtls_cipher_context_t *ctx,
         return( ret );
     }
 #endif /* MBEDTLS_CCM_C */
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if ( MBEDTLS_CIPHER_CHACHA20_POLY1305 == ctx->cipher_info->type )
+    {
+        int ret;
+
+        /* ChachaPoly has fixed length nonce and MAC (tag) */
+        if ( ( iv_len != ctx->cipher_info->iv_size ) ||
+             ( tag_len != 16U ) )
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        *olen = ilen;
+        ret = mbedtls_chachapoly_auth_decrypt( ctx->cipher_ctx, ilen,
+                                iv, ad, ad_len, tag, input, output );
+
+        if( ret == MBEDTLS_ERR_CHACHAPOLY_AUTH_FAILED )
+            ret = MBEDTLS_ERR_CIPHER_AUTH_FAILED;
+
+        return( ret );
+    }
+#endif /* MBEDTLS_CHACHAPOLY_C */
 
     return( MBEDTLS_ERR_CIPHER_FEATURE_UNAVAILABLE );
 }
diff --git a/library/cipher_wrap.c b/library/cipher_wrap.c
index 16e0a9d9..893490ac 100644
--- a/library/cipher_wrap.c
+++ b/library/cipher_wrap.c
@@ -33,6 +33,10 @@
 
 #include "mbedtls/cipher_internal.h"
 
+#if defined(MBEDTLS_CHACHAPOLY_C)
+#include "mbedtls/chachapoly.h"
+#endif
+
 #if defined(MBEDTLS_AES_C)
 #include "mbedtls/aes.h"
 #endif
@@ -57,6 +61,10 @@
 #include "mbedtls/blowfish.h"
 #endif
 
+#if defined(MBEDTLS_CHACHA20_C)
+#include "mbedtls/chacha20.h"
+#endif
+
 #if defined(MBEDTLS_GCM_C)
 #include "mbedtls/gcm.h"
 #endif
@@ -1886,6 +1894,162 @@ static const mbedtls_cipher_info_t arc4_128_info = {
 };
 #endif /* MBEDTLS_ARC4_C */
 
+#if defined(MBEDTLS_CHACHA20_C)
+
+static int chacha20_setkey_wrap( void *ctx, const unsigned char *key,
+                                 unsigned int key_bitlen )
+{
+    if( key_bitlen != 256U )
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+    if ( 0 != mbedtls_chacha20_setkey( (mbedtls_chacha20_context*)ctx, key ) )
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+    return( 0 );
+}
+
+static int chacha20_stream_wrap( void *ctx,  size_t length,
+                                 const unsigned char *input,
+                                 unsigned char *output )
+{
+    int ret;
+
+    ret = mbedtls_chacha20_update( ctx, length, input, output );
+    if( ret == MBEDTLS_ERR_CHACHA20_BAD_INPUT_DATA )
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+    return( ret );
+}
+
+static void * chacha20_ctx_alloc( void )
+{
+    mbedtls_chacha20_context *ctx;
+    ctx = mbedtls_calloc( 1, sizeof( mbedtls_chacha20_context ) );
+
+    if( ctx == NULL )
+        return( NULL );
+
+    mbedtls_chacha20_init( ctx );
+
+    return( ctx );
+}
+
+static void chacha20_ctx_free( void *ctx )
+{
+    mbedtls_chacha20_free( (mbedtls_chacha20_context *) ctx );
+    mbedtls_free( ctx );
+}
+
+static const mbedtls_cipher_base_t chacha20_base_info = {
+    MBEDTLS_CIPHER_ID_CHACHA20,
+    NULL,
+#if defined(MBEDTLS_CIPHER_MODE_CBC)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_CFB)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_OFB)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_CTR)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_XTS)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_STREAM)
+    chacha20_stream_wrap,
+#endif
+    chacha20_setkey_wrap,
+    chacha20_setkey_wrap,
+    chacha20_ctx_alloc,
+    chacha20_ctx_free
+};
+static const mbedtls_cipher_info_t chacha20_info = {
+    MBEDTLS_CIPHER_CHACHA20,
+    MBEDTLS_MODE_STREAM,
+    256,
+    "CHACHA20",
+    12,
+    0,
+    1,
+    &chacha20_base_info
+};
+#endif /* MBEDTLS_CHACHA20_C */
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+
+static int chachapoly_setkey_wrap( void *ctx,
+                                   const unsigned char *key,
+                                   unsigned int key_bitlen )
+{
+    if( key_bitlen != 256U )
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+    if ( 0 != mbedtls_chachapoly_setkey( (mbedtls_chachapoly_context*)ctx, key ) )
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+    return( 0 );
+}
+
+static void * chachapoly_ctx_alloc( void )
+{
+    mbedtls_chachapoly_context *ctx;
+    ctx = mbedtls_calloc( 1, sizeof( mbedtls_chachapoly_context ) );
+
+    if( ctx == NULL )
+        return( NULL );
+
+    mbedtls_chachapoly_init( ctx );
+
+    return( ctx );
+}
+
+static void chachapoly_ctx_free( void *ctx )
+{
+    mbedtls_chachapoly_free( (mbedtls_chachapoly_context *) ctx );
+    mbedtls_free( ctx );
+}
+
+static const mbedtls_cipher_base_t chachapoly_base_info = {
+    MBEDTLS_CIPHER_ID_CHACHA20,
+    NULL,
+#if defined(MBEDTLS_CIPHER_MODE_CBC)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_CFB)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_OFB)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_CTR)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_XTS)
+    NULL,
+#endif
+#if defined(MBEDTLS_CIPHER_MODE_STREAM)
+    NULL,
+#endif
+    chachapoly_setkey_wrap,
+    chachapoly_setkey_wrap,
+    chachapoly_ctx_alloc,
+    chachapoly_ctx_free
+};
+static const mbedtls_cipher_info_t chachapoly_info = {
+    MBEDTLS_CIPHER_CHACHA20_POLY1305,
+    MBEDTLS_MODE_CHACHAPOLY,
+    256,
+    "CHACHA20-POLY1305",
+    12,
+    0,
+    1,
+    &chachapoly_base_info
+};
+#endif /* MBEDTLS_CHACHAPOLY_C */
+
 #if defined(MBEDTLS_CIPHER_NULL_CIPHER)
 static int null_crypt_stream( void *ctx, size_t length,
                               const unsigned char *input,
@@ -2087,6 +2251,14 @@ const mbedtls_cipher_definition_t mbedtls_cipher_definitions[] =
 #endif
 #endif /* MBEDTLS_DES_C */
 
+#if defined(MBEDTLS_CHACHA20_C)
+    { MBEDTLS_CIPHER_CHACHA20,             &chacha20_info },
+#endif
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    { MBEDTLS_CIPHER_CHACHA20_POLY1305,    &chachapoly_info },
+#endif
+
 #if defined(MBEDTLS_CIPHER_NULL_CIPHER)
     { MBEDTLS_CIPHER_NULL,                 &null_cipher_info },
 #endif /* MBEDTLS_CIPHER_NULL_CIPHER */
diff --git a/library/cmac.c b/library/cmac.c
index 4d7a1f16..5d101e1c 100644
--- a/library/cmac.c
+++ b/library/cmac.c
@@ -828,6 +828,7 @@ static int cmac_test_subkeys( int verbose,
         mbedtls_cipher_free( &ctx );
     }
 
+    ret = 0;
     goto exit;
 
 cleanup:
@@ -883,6 +884,7 @@ static int cmac_test_wth_cipher( int verbose,
         if( verbose != 0 )
             mbedtls_printf( "passed\n" );
     }
+    ret = 0;
 
 exit:
     return( ret );
diff --git a/library/ecp.c b/library/ecp.c
index bbc1ff08..b2ee5235 100644
--- a/library/ecp.c
+++ b/library/ecp.c
@@ -2517,7 +2517,6 @@ int mbedtls_ecp_check_privkey( const mbedtls_ecp_group *grp, const mbedtls_mpi *
             mbedtls_mpi_get_bit( d, 1 ) != 0 ||
             mbedtls_mpi_bitlen( d ) - 1 != grp->nbits ) /* mbedtls_mpi_bitlen is one-based! */
             return( MBEDTLS_ERR_ECP_INVALID_KEY );
-        else
 
         /* see [Curve25519] page 5 */
         if( grp->nbits == 254 && mbedtls_mpi_get_bit( d, 2 ) != 0 )
diff --git a/library/entropy_poll.c b/library/entropy_poll.c
index fd96258c..040aa117 100644
--- a/library/entropy_poll.c
+++ b/library/entropy_poll.c
@@ -19,6 +19,11 @@
  *  This file is part of mbed TLS (https://tls.mbed.org)
  */
 
+#if defined(__linux__)
+/* Ensure that syscall() is available even when compiling with -std=c99 */
+#define _GNU_SOURCE
+#endif
+
 #if !defined(MBEDTLS_CONFIG_FILE)
 #include "mbedtls/config.h"
 #else
@@ -45,7 +50,8 @@
 #if !defined(MBEDTLS_NO_PLATFORM_ENTROPY)
 
 #if !defined(unix) && !defined(__unix__) && !defined(__unix) && \
-    !defined(__APPLE__) && !defined(_WIN32) && !defined(__QNXNTO__)
+    !defined(__APPLE__) && !defined(_WIN32) && !defined(__QNXNTO__) && \
+    !defined(__HAIKU__)
 #error "Platform entropy sources only work on Unix and Windows, see MBEDTLS_NO_PLATFORM_ENTROPY in config.h"
 #endif
 
diff --git a/library/error.c b/library/error.c
index 9bccc8b1..d48ba8f7 100644
--- a/library/error.c
+++ b/library/error.c
@@ -73,6 +73,14 @@
 #include "mbedtls/ccm.h"
 #endif
 
+#if defined(MBEDTLS_CHACHA20_C)
+#include "mbedtls/chacha20.h"
+#endif
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+#include "mbedtls/chachapoly.h"
+#endif
+
 #if defined(MBEDTLS_CIPHER_C)
 #include "mbedtls/cipher.h"
 #endif
@@ -157,6 +165,10 @@
 #include "mbedtls/pkcs5.h"
 #endif
 
+#if defined(MBEDTLS_POLY1305_C)
+#include "mbedtls/poly1305.h"
+#endif
+
 #if defined(MBEDTLS_RIPEMD160_C)
 #include "mbedtls/ripemd160.h"
 #endif
@@ -505,6 +517,8 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen )
             mbedtls_snprintf( buf, buflen, "SSL - Internal-only message signaling that further message-processing should be done" );
         if( use_ret == -(MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS) )
             mbedtls_snprintf( buf, buflen, "SSL - The asynchronous operation is not completed yet" );
+        if( use_ret == -(MBEDTLS_ERR_SSL_EARLY_MESSAGE) )
+            mbedtls_snprintf( buf, buflen, "SSL - Internal-only message signaling that a message arrived early" );
         if( use_ret == -(MBEDTLS_ERR_SSL_CRYPTO_IN_PROGRESS) )
             mbedtls_snprintf( buf, buflen, "SSL - A cryptographic operation is in progress. Try again later" );
 #endif /* MBEDTLS_SSL_TLS_C */
@@ -680,6 +694,22 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen )
         mbedtls_snprintf( buf, buflen, "CCM - CCM hardware accelerator failed" );
 #endif /* MBEDTLS_CCM_C */
 
+#if defined(MBEDTLS_CHACHA20_C)
+    if( use_ret == -(MBEDTLS_ERR_CHACHA20_BAD_INPUT_DATA) )
+        mbedtls_snprintf( buf, buflen, "CHACHA20 - Invalid input parameter(s)" );
+    if( use_ret == -(MBEDTLS_ERR_CHACHA20_FEATURE_UNAVAILABLE) )
+        mbedtls_snprintf( buf, buflen, "CHACHA20 - Feature not available. For example, s part of the API is not implemented" );
+    if( use_ret == -(MBEDTLS_ERR_CHACHA20_HW_ACCEL_FAILED) )
+        mbedtls_snprintf( buf, buflen, "CHACHA20 - Chacha20 hardware accelerator failed" );
+#endif /* MBEDTLS_CHACHA20_C */
+
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if( use_ret == -(MBEDTLS_ERR_CHACHAPOLY_BAD_STATE) )
+        mbedtls_snprintf( buf, buflen, "CHACHAPOLY - The requested operation is not permitted in the current state" );
+    if( use_ret == -(MBEDTLS_ERR_CHACHAPOLY_AUTH_FAILED) )
+        mbedtls_snprintf( buf, buflen, "CHACHAPOLY - Authenticated decryption failed: data was not authentic" );
+#endif /* MBEDTLS_CHACHAPOLY_C */
+
 #if defined(MBEDTLS_CMAC_C)
     if( use_ret == -(MBEDTLS_ERR_CMAC_HW_ACCEL_FAILED) )
         mbedtls_snprintf( buf, buflen, "CMAC - CMAC hardware accelerator failed" );
@@ -797,6 +827,15 @@ void mbedtls_strerror( int ret, char *buf, size_t buflen )
         mbedtls_snprintf( buf, buflen, "PADLOCK - Input data should be aligned" );
 #endif /* MBEDTLS_PADLOCK_C */
 
+#if defined(MBEDTLS_POLY1305_C)
+    if( use_ret == -(MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA) )
+        mbedtls_snprintf( buf, buflen, "POLY1305 - Invalid input parameter(s)" );
+    if( use_ret == -(MBEDTLS_ERR_POLY1305_FEATURE_UNAVAILABLE) )
+        mbedtls_snprintf( buf, buflen, "POLY1305 - Feature not available. For example, s part of the API is not implemented" );
+    if( use_ret == -(MBEDTLS_ERR_POLY1305_HW_ACCEL_FAILED) )
+        mbedtls_snprintf( buf, buflen, "POLY1305 - Poly1305 hardware accelerator failed" );
+#endif /* MBEDTLS_POLY1305_C */
+
 #if defined(MBEDTLS_RIPEMD160_C)
     if( use_ret == -(MBEDTLS_ERR_RIPEMD160_HW_ACCEL_FAILED) )
         mbedtls_snprintf( buf, buflen, "RIPEMD160 - RIPEMD160 hardware accelerator failed" );
diff --git a/library/hkdf.c b/library/hkdf.c
index d2e55e86..82d8a429 100644
--- a/library/hkdf.c
+++ b/library/hkdf.c
@@ -62,6 +62,11 @@ int mbedtls_hkdf_extract( const mbedtls_md_info_t *md,
     {
         size_t hash_len;
 
+        if( salt_len != 0 )
+        {
+            return MBEDTLS_ERR_HKDF_BAD_INPUT_DATA;
+        }
+
         hash_len = mbedtls_md_get_size( md );
 
         if( hash_len == 0 )
@@ -114,6 +119,10 @@ int mbedtls_hkdf_expand( const mbedtls_md_info_t *md, const unsigned char *prk,
         n++;
     }
 
+    /*
+     * Per RFC 5869 Section 2.3, okm_len must not exceed
+     * 255 times the hash length
+     */
     if( n > 255 )
     {
         return( MBEDTLS_ERR_HKDF_BAD_INPUT_DATA );
@@ -126,7 +135,10 @@ int mbedtls_hkdf_expand( const mbedtls_md_info_t *md, const unsigned char *prk,
         goto exit;
     }
 
-    /* RFC 5869 Section 2.3. */
+    /*
+     * Compute T = T(1) | T(2) | T(3) | ... | T(N)
+     * Where T(N) is defined in RFC 5869 Section 2.3
+     */
     for( i = 1; i <= n; i++ )
     {
         size_t num_to_copy;
@@ -150,7 +162,7 @@ int mbedtls_hkdf_expand( const mbedtls_md_info_t *md, const unsigned char *prk,
             goto exit;
         }
 
-        /* The constant concatenated to the end of each t(n) is a single octet.
+        /* The constant concatenated to the end of each T(n) is a single octet.
          * */
         ret = mbedtls_md_hmac_update( &ctx, &c, 1 );
         if( ret != 0 )
diff --git a/library/md5.c b/library/md5.c
index 8238c2b8..2a740cda 100644
--- a/library/md5.c
+++ b/library/md5.c
@@ -309,14 +309,6 @@ void mbedtls_md5_update( mbedtls_md5_context *ctx,
 }
 #endif
 
-static const unsigned char md5_padding[64] =
-{
- 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
-};
-
 /*
  * MD5 final digest
  */
@@ -324,26 +316,48 @@ int mbedtls_md5_finish_ret( mbedtls_md5_context *ctx,
                             unsigned char output[16] )
 {
     int ret;
-    uint32_t last, padn;
+    uint32_t used;
     uint32_t high, low;
-    unsigned char msglen[8];
 
-    high = ( ctx->total[0] >> 29 )
-         | ( ctx->total[1] <<  3 );
-    low  = ( ctx->total[0] <<  3 );
+    /*
+     * Add padding: 0x80 then 0x00 until 8 bytes remain for the length
+     */
+    used = ctx->total[0] & 0x3F;
 
-    PUT_UINT32_LE( low,  msglen, 0 );
-    PUT_UINT32_LE( high, msglen, 4 );
+    ctx->buffer[used++] = 0x80;
 
-    last = ctx->total[0] & 0x3F;
-    padn = ( last < 56 ) ? ( 56 - last ) : ( 120 - last );
+    if( used <= 56 )
+    {
+        /* Enough room for padding + length in current block */
+        memset( ctx->buffer + used, 0, 56 - used );
+    }
+    else
+    {
+        /* We'll need an extra block */
+        memset( ctx->buffer + used, 0, 64 - used );
 
-    if( ( ret = mbedtls_md5_update_ret( ctx, md5_padding, padn ) ) != 0 )
+        if( ( ret = mbedtls_internal_md5_process( ctx, ctx->buffer ) ) != 0 )
             return( ret );
 
-    if( ( ret = mbedtls_md5_update_ret( ctx, msglen, 8 ) ) != 0 )
-            return( ret );
+        memset( ctx->buffer, 0, 56 );
+    }
+
+    /*
+     * Add message length
+     */
+    high = ( ctx->total[0] >> 29 )
+         | ( ctx->total[1] <<  3 );
+    low  = ( ctx->total[0] <<  3 );
+
+    PUT_UINT32_LE( low,  ctx->buffer, 56 );
+    PUT_UINT32_LE( high, ctx->buffer, 60 );
+
+    if( ( ret = mbedtls_internal_md5_process( ctx, ctx->buffer ) ) != 0 )
+        return( ret );
 
+    /*
+     * Output final state
+     */
     PUT_UINT32_LE( ctx->state[0], output,  0 );
     PUT_UINT32_LE( ctx->state[1], output,  4 );
     PUT_UINT32_LE( ctx->state[2], output,  8 );
diff --git a/library/net_sockets.c b/library/net_sockets.c
index 202da017..816b1303 100644
--- a/library/net_sockets.c
+++ b/library/net_sockets.c
@@ -19,6 +19,11 @@
  *  This file is part of mbed TLS (https://tls.mbed.org)
  */
 
+/* Enable definition of getaddrinfo() even when compiling with -std=c99. Must
+ * be set before config.h, which pulls in glibc's features.h indirectly.
+ * Harmless on other platforms. */
+#define _POSIX_C_SOURCE 200112L
+
 #if !defined(MBEDTLS_CONFIG_FILE)
 #include "mbedtls/config.h"
 #else
@@ -28,7 +33,8 @@
 #if defined(MBEDTLS_NET_C)
 
 #if !defined(unix) && !defined(__unix__) && !defined(__unix) && \
-    !defined(__APPLE__) && !defined(_WIN32) && !defined(__QNXNTO__)
+    !defined(__APPLE__) && !defined(_WIN32) && !defined(__QNXNTO__) && \
+    !defined(__HAIKU__)
 #error "This module only works on Unix and Windows, see MBEDTLS_NET_C in config.h"
 #endif
 
diff --git a/library/nist_kw.c b/library/nist_kw.c
new file mode 100644
index 00000000..176af9fe
--- /dev/null
+++ b/library/nist_kw.c
@@ -0,0 +1,755 @@
+/*
+ *  Implementation of NIST SP 800-38F key wrapping, supporting KW and KWP modes
+ *  only
+ *
+ *  Copyright (C) 2018, Arm Limited (or its affiliates), All Rights Reserved
+ *  SPDX-License-Identifier: Apache-2.0
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License"); you may
+ *  not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ *  WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ *
+ *  This file is part of Mbed TLS (https://tls.mbed.org)
+ */
+/*
+ * Definition of Key Wrapping:
+ * https://nvlpubs.nist.gov/nistpubs/SpecialPublications/NIST.SP.800-38F.pdf
+ * RFC 3394 "Advanced Encryption Standard (AES) Key Wrap Algorithm"
+ * RFC 5649 "Advanced Encryption Standard (AES) Key Wrap with Padding Algorithm"
+ *
+ * Note: RFC 3394 defines different methodology for intermediate operations for
+ * the wrapping and unwrapping operation than the definition in NIST SP 800-38F.
+ */
+
+#if !defined(MBEDTLS_CONFIG_FILE)
+#include "mbedtls/config.h"
+#else
+#include MBEDTLS_CONFIG_FILE
+#endif
+
+#if defined(MBEDTLS_NIST_KW_C)
+
+#include "mbedtls/nist_kw.h"
+#include "mbedtls/platform_util.h"
+
+#include <stdint.h>
+#include <string.h>
+
+#if defined(MBEDTLS_SELF_TEST) && defined(MBEDTLS_AES_C)
+#if defined(MBEDTLS_PLATFORM_C)
+#include "mbedtls/platform.h"
+#else
+#include <stdio.h>
+#define mbedtls_printf printf
+#endif /* MBEDTLS_PLATFORM_C */
+#endif /* MBEDTLS_SELF_TEST && MBEDTLS_AES_C */
+
+#if !defined(MBEDTLS_NIST_KW_ALT)
+
+#define KW_SEMIBLOCK_LENGTH    8
+#define MIN_SEMIBLOCKS_COUNT   3
+
+/* constant-time buffer comparison */
+static inline unsigned char mbedtls_nist_kw_safer_memcmp( const void *a, const void *b, size_t n )
+{
+    size_t i;
+    volatile const unsigned char *A = (volatile const unsigned char *) a;
+    volatile const unsigned char *B = (volatile const unsigned char *) b;
+    volatile unsigned char diff = 0;
+
+    for( i = 0; i < n; i++ )
+    {
+        /* Read volatile data in order before computing diff.
+         * This avoids IAR compiler warning:
+         * 'the order of volatile accesses is undefined ..' */
+        unsigned char x = A[i], y = B[i];
+        diff |= x ^ y;
+    }
+
+    return( diff );
+}
+
+/*! The 64-bit default integrity check value (ICV) for KW mode. */
+static const unsigned char NIST_KW_ICV1[] = {0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6};
+/*! The 32-bit default integrity check value (ICV) for KWP mode. */
+static const  unsigned char NIST_KW_ICV2[] = {0xA6, 0x59, 0x59, 0xA6};
+
+#ifndef GET_UINT32_BE
+#define GET_UINT32_BE(n,b,i)                            \
+do {                                                    \
+    (n) = ( (uint32_t) (b)[(i)    ] << 24 )             \
+        | ( (uint32_t) (b)[(i) + 1] << 16 )             \
+        | ( (uint32_t) (b)[(i) + 2] <<  8 )             \
+        | ( (uint32_t) (b)[(i) + 3]       );            \
+} while( 0 )
+#endif
+
+#ifndef PUT_UINT32_BE
+#define PUT_UINT32_BE(n,b,i)                            \
+do {                                                    \
+    (b)[(i)    ] = (unsigned char) ( (n) >> 24 );       \
+    (b)[(i) + 1] = (unsigned char) ( (n) >> 16 );       \
+    (b)[(i) + 2] = (unsigned char) ( (n) >>  8 );       \
+    (b)[(i) + 3] = (unsigned char) ( (n)       );       \
+} while( 0 )
+#endif
+
+/*
+ * Initialize context
+ */
+void mbedtls_nist_kw_init( mbedtls_nist_kw_context *ctx )
+{
+    memset( ctx, 0, sizeof( mbedtls_nist_kw_context ) );
+}
+
+int mbedtls_nist_kw_setkey( mbedtls_nist_kw_context *ctx,
+                            mbedtls_cipher_id_t cipher,
+                            const unsigned char *key,
+                            unsigned int keybits,
+                            const int is_wrap )
+{
+    int ret;
+    const mbedtls_cipher_info_t *cipher_info;
+
+    cipher_info = mbedtls_cipher_info_from_values( cipher,
+                                                   keybits,
+                                                   MBEDTLS_MODE_ECB );
+    if( cipher_info == NULL )
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+    if( cipher_info->block_size != 16 )
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+
+    /*
+     * SP 800-38F currently defines AES cipher as the only block cipher allowed:
+     * "For KW and KWP, the underlying block cipher shall be approved, and the
+     *  block size shall be 128 bits. Currently, the AES block cipher, with key
+     *  lengths of 128, 192, or 256 bits, is the only block cipher that fits
+     *  this profile."
+     *  Currently we don't support other 128 bit block ciphers for key wrapping,
+     *  such as Camellia and Aria.
+     */
+    if( cipher != MBEDTLS_CIPHER_ID_AES )
+        return( MBEDTLS_ERR_CIPHER_FEATURE_UNAVAILABLE );
+
+    mbedtls_cipher_free( &ctx->cipher_ctx );
+
+    if( ( ret = mbedtls_cipher_setup( &ctx->cipher_ctx, cipher_info ) ) != 0 )
+        return( ret );
+
+    if( ( ret = mbedtls_cipher_setkey( &ctx->cipher_ctx, key, keybits,
+                                       is_wrap ? MBEDTLS_ENCRYPT :
+                                                 MBEDTLS_DECRYPT )
+                                                                   ) != 0 )
+    {
+        return( ret );
+    }
+
+    return( 0 );
+}
+
+/*
+ * Free context
+ */
+void mbedtls_nist_kw_free( mbedtls_nist_kw_context *ctx )
+{
+    mbedtls_cipher_free( &ctx->cipher_ctx );
+    mbedtls_platform_zeroize( ctx, sizeof( mbedtls_nist_kw_context ) );
+}
+
+/*
+ * Helper function for Xoring the uint64_t "t" with the encrypted A.
+ * Defined in NIST SP 800-38F section 6.1
+ */
+static void calc_a_xor_t( unsigned char A[KW_SEMIBLOCK_LENGTH], uint64_t t )
+{
+    size_t i = 0;
+    for( i = 0; i < sizeof( t ); i++ )
+    {
+        A[i] ^= ( t >> ( ( sizeof( t ) - 1 - i ) * 8 ) ) & 0xff;
+    }
+}
+
+/*
+ * KW-AE as defined in SP 800-38F section 6.2
+ * KWP-AE as defined in SP 800-38F section 6.3
+ */
+int mbedtls_nist_kw_wrap( mbedtls_nist_kw_context *ctx,
+                          mbedtls_nist_kw_mode_t mode,
+                          const unsigned char *input, size_t in_len,
+                          unsigned char *output, size_t *out_len, size_t out_size )
+{
+    int ret = 0;
+    size_t semiblocks = 0;
+    size_t s;
+    size_t olen, padlen = 0;
+    uint64_t t = 0;
+    unsigned char outbuff[KW_SEMIBLOCK_LENGTH * 2];
+    unsigned char inbuff[KW_SEMIBLOCK_LENGTH * 2];
+    unsigned char *R2 = output + KW_SEMIBLOCK_LENGTH;
+    unsigned char *A = output;
+
+    *out_len = 0;
+    /*
+     * Generate the String to work on
+     */
+    if( mode == MBEDTLS_KW_MODE_KW )
+    {
+        if( out_size < in_len + KW_SEMIBLOCK_LENGTH )
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        /*
+         * According to SP 800-38F Table 1, the plaintext length for KW
+         * must be between 2 to 2^54-1 semiblocks inclusive.
+         */
+        if( in_len < 16 ||
+#if SIZE_MAX > 0x1FFFFFFFFFFFFF8
+            in_len > 0x1FFFFFFFFFFFFF8 ||
+#endif
+            in_len % KW_SEMIBLOCK_LENGTH != 0 )
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        memcpy( output, NIST_KW_ICV1, KW_SEMIBLOCK_LENGTH );
+        memmove( output + KW_SEMIBLOCK_LENGTH, input, in_len );
+    }
+    else
+    {
+        if( in_len % 8 != 0 )
+        {
+            padlen = ( 8 - ( in_len % 8 ) );
+        }
+
+        if( out_size < in_len + KW_SEMIBLOCK_LENGTH + padlen )
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        /*
+         * According to SP 800-38F Table 1, the plaintext length for KWP
+         * must be between 1 and 2^32-1 octets inclusive.
+         */
+        if( in_len < 1
+#if SIZE_MAX > 0xFFFFFFFF
+            || in_len > 0xFFFFFFFF
+#endif
+          )
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        memcpy( output, NIST_KW_ICV2, KW_SEMIBLOCK_LENGTH / 2 );
+        PUT_UINT32_BE( ( in_len & 0xffffffff ), output,
+                       KW_SEMIBLOCK_LENGTH / 2 );
+
+        memcpy( output + KW_SEMIBLOCK_LENGTH, input, in_len );
+        memset( output + KW_SEMIBLOCK_LENGTH + in_len, 0, padlen );
+    }
+    semiblocks = ( ( in_len + padlen ) / KW_SEMIBLOCK_LENGTH ) + 1;
+
+    s = 6 * ( semiblocks - 1 );
+
+    if( mode == MBEDTLS_KW_MODE_KWP
+        && in_len <= KW_SEMIBLOCK_LENGTH )
+    {
+        memcpy( inbuff, output, 16 );
+        ret = mbedtls_cipher_update( &ctx->cipher_ctx,
+                                     inbuff, 16, output, &olen );
+        if( ret != 0 )
+            goto cleanup;
+    }
+    else
+    {
+        /*
+         * Do the wrapping function W, as defined in RFC 3394 section 2.2.1
+         */
+        if( semiblocks < MIN_SEMIBLOCKS_COUNT )
+        {
+            ret = MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA;
+            goto cleanup;
+        }
+
+        /* Calculate intermediate values */
+        for( t = 1; t <= s; t++ )
+        {
+            memcpy( inbuff, A, KW_SEMIBLOCK_LENGTH );
+            memcpy( inbuff + KW_SEMIBLOCK_LENGTH, R2, KW_SEMIBLOCK_LENGTH );
+
+            ret = mbedtls_cipher_update( &ctx->cipher_ctx,
+                                         inbuff, 16, outbuff, &olen );
+            if( ret != 0 )
+                goto cleanup;
+
+            memcpy( A, outbuff, KW_SEMIBLOCK_LENGTH );
+            calc_a_xor_t( A, t );
+
+            memcpy( R2, outbuff + KW_SEMIBLOCK_LENGTH, KW_SEMIBLOCK_LENGTH );
+            R2 += KW_SEMIBLOCK_LENGTH;
+            if( R2 >= output + ( semiblocks * KW_SEMIBLOCK_LENGTH ) )
+                R2 = output + KW_SEMIBLOCK_LENGTH;
+        }
+    }
+
+    *out_len = semiblocks * KW_SEMIBLOCK_LENGTH;
+
+cleanup:
+
+    if( ret != 0)
+    {
+        memset( output, 0, semiblocks * KW_SEMIBLOCK_LENGTH );
+    }
+    mbedtls_platform_zeroize( inbuff, KW_SEMIBLOCK_LENGTH * 2 );
+    mbedtls_platform_zeroize( outbuff, KW_SEMIBLOCK_LENGTH * 2 );
+    mbedtls_cipher_finish( &ctx->cipher_ctx, NULL, &olen );
+    return( ret );
+}
+
+/*
+ * W-1 function as defined in RFC 3394 section 2.2.2
+ * This function assumes the following:
+ * 1. Output buffer is at least of size ( semiblocks - 1 ) * KW_SEMIBLOCK_LENGTH.
+ * 2. The input buffer is of size semiblocks * KW_SEMIBLOCK_LENGTH.
+ * 3. Minimal number of semiblocks is 3.
+ * 4. A is a buffer to hold the first semiblock of the input buffer.
+ */
+static int unwrap( mbedtls_nist_kw_context *ctx,
+                   const unsigned char *input, size_t semiblocks,
+                   unsigned char A[KW_SEMIBLOCK_LENGTH],
+                   unsigned char *output, size_t* out_len )
+{
+    int ret = 0;
+    const size_t s = 6 * ( semiblocks - 1 );
+    size_t olen;
+    uint64_t t = 0;
+    unsigned char outbuff[KW_SEMIBLOCK_LENGTH * 2];
+    unsigned char inbuff[KW_SEMIBLOCK_LENGTH * 2];
+    unsigned char *R = output + ( semiblocks - 2 ) * KW_SEMIBLOCK_LENGTH;
+    *out_len = 0;
+
+    if( semiblocks < MIN_SEMIBLOCKS_COUNT )
+    {
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+    }
+
+    memcpy( A, input, KW_SEMIBLOCK_LENGTH );
+    memmove( output, input + KW_SEMIBLOCK_LENGTH, ( semiblocks - 1 ) * KW_SEMIBLOCK_LENGTH );
+
+    /* Calculate intermediate values */
+    for( t = s; t >= 1; t-- )
+    {
+        calc_a_xor_t( A, t );
+
+        memcpy( inbuff, A, KW_SEMIBLOCK_LENGTH );
+        memcpy( inbuff + KW_SEMIBLOCK_LENGTH, R, KW_SEMIBLOCK_LENGTH );
+
+        ret = mbedtls_cipher_update( &ctx->cipher_ctx,
+                                     inbuff, 16, outbuff, &olen );
+        if( ret != 0 )
+            goto cleanup;
+
+        memcpy( A, outbuff, KW_SEMIBLOCK_LENGTH );
+
+        /* Set R as LSB64 of outbuff */
+        memcpy( R, outbuff + KW_SEMIBLOCK_LENGTH, KW_SEMIBLOCK_LENGTH );
+
+        if( R == output )
+            R = output + ( semiblocks - 2 ) * KW_SEMIBLOCK_LENGTH;
+        else
+            R -= KW_SEMIBLOCK_LENGTH;
+    }
+
+    *out_len = ( semiblocks - 1 ) * KW_SEMIBLOCK_LENGTH;
+
+cleanup:
+    if( ret != 0)
+        memset( output, 0, ( semiblocks - 1 ) * KW_SEMIBLOCK_LENGTH );
+    mbedtls_platform_zeroize( inbuff, sizeof( inbuff )  );
+    mbedtls_platform_zeroize( outbuff, sizeof( outbuff ) );
+
+    return( ret );
+}
+
+/*
+ * KW-AD as defined in SP 800-38F section 6.2
+ * KWP-AD as defined in SP 800-38F section 6.3
+ */
+int mbedtls_nist_kw_unwrap( mbedtls_nist_kw_context *ctx,
+                            mbedtls_nist_kw_mode_t mode,
+                            const unsigned char *input, size_t in_len,
+                            unsigned char *output, size_t *out_len, size_t out_size )
+{
+    int ret = 0;
+    size_t i, olen;
+    unsigned char A[KW_SEMIBLOCK_LENGTH];
+    unsigned char diff, bad_padding = 0;
+
+    *out_len = 0;
+    if( out_size < in_len - KW_SEMIBLOCK_LENGTH )
+    {
+        return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+    }
+
+    if( mode == MBEDTLS_KW_MODE_KW )
+    {
+        /*
+         * According to SP 800-38F Table 1, the ciphertext length for KW
+         * must be between 3 to 2^54 semiblocks inclusive.
+         */
+        if( in_len < 24 ||
+#if SIZE_MAX > 0x200000000000000
+            in_len > 0x200000000000000 ||
+#endif
+            in_len % KW_SEMIBLOCK_LENGTH != 0 )
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        ret = unwrap( ctx, input, in_len / KW_SEMIBLOCK_LENGTH,
+                      A, output, out_len );
+        if( ret != 0 )
+            goto cleanup;
+
+        /* Check ICV in "constant-time" */
+        diff = mbedtls_nist_kw_safer_memcmp( NIST_KW_ICV1, A, KW_SEMIBLOCK_LENGTH );
+
+        if( diff != 0 )
+        {
+            ret = MBEDTLS_ERR_CIPHER_AUTH_FAILED;
+            goto cleanup;
+        }
+
+    }
+    else if( mode == MBEDTLS_KW_MODE_KWP )
+    {
+        size_t padlen = 0;
+        uint32_t Plen;
+        /*
+         * According to SP 800-38F Table 1, the ciphertext length for KWP
+         * must be between 2 to 2^29 semiblocks inclusive.
+         */
+        if( in_len < KW_SEMIBLOCK_LENGTH * 2 ||
+#if SIZE_MAX > 0x100000000
+            in_len > 0x100000000 ||
+#endif
+            in_len % KW_SEMIBLOCK_LENGTH != 0 )
+        {
+            return(  MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+
+        if( in_len == KW_SEMIBLOCK_LENGTH * 2 )
+        {
+            unsigned char outbuff[KW_SEMIBLOCK_LENGTH * 2];
+            ret = mbedtls_cipher_update( &ctx->cipher_ctx,
+                                         input, 16, outbuff, &olen );
+            if( ret != 0 )
+                goto cleanup;
+
+            memcpy( A, outbuff, KW_SEMIBLOCK_LENGTH );
+            memcpy( output, outbuff + KW_SEMIBLOCK_LENGTH, KW_SEMIBLOCK_LENGTH );
+            mbedtls_platform_zeroize( outbuff, sizeof( outbuff ) );
+            *out_len = KW_SEMIBLOCK_LENGTH;
+        }
+        else
+        {
+            /* in_len >=  KW_SEMIBLOCK_LENGTH * 3 */
+            ret = unwrap( ctx, input, in_len / KW_SEMIBLOCK_LENGTH,
+                          A, output, out_len );
+            if( ret != 0 )
+                goto cleanup;
+        }
+
+        /* Check ICV in "constant-time" */
+        diff = mbedtls_nist_kw_safer_memcmp( NIST_KW_ICV2, A, KW_SEMIBLOCK_LENGTH / 2 );
+
+        if( diff != 0 )
+        {
+            ret = MBEDTLS_ERR_CIPHER_AUTH_FAILED;
+        }
+
+        GET_UINT32_BE( Plen, A, KW_SEMIBLOCK_LENGTH / 2 );
+
+        /*
+         * Plen is the length of the plaintext, when the input is valid.
+         * If Plen is larger than the plaintext and padding, padlen will be
+         * larger than 8, because of the type wrap around.
+         */
+        padlen = in_len - KW_SEMIBLOCK_LENGTH - Plen;
+        if ( padlen > 7 )
+        {
+            padlen &= 7;
+            ret = MBEDTLS_ERR_CIPHER_AUTH_FAILED;
+        }
+
+        /* Check padding in "constant-time" */
+        for( diff = 0, i = 0; i < KW_SEMIBLOCK_LENGTH; i++ )
+        {
+             if( i >= KW_SEMIBLOCK_LENGTH - padlen )
+                 diff |= output[*out_len - KW_SEMIBLOCK_LENGTH + i];
+             else
+                 bad_padding |= output[*out_len - KW_SEMIBLOCK_LENGTH + i];
+        }
+
+        if( diff != 0 )
+        {
+            ret = MBEDTLS_ERR_CIPHER_AUTH_FAILED;
+        }
+
+        if( ret != 0 )
+        {
+            goto cleanup;
+        }
+        memset( output + Plen, 0, padlen );
+        *out_len = Plen;
+    }
+    else
+    {
+        ret = MBEDTLS_ERR_CIPHER_FEATURE_UNAVAILABLE;
+        goto cleanup;
+    }
+
+cleanup:
+    if( ret != 0 )
+    {
+        memset( output, 0, *out_len );
+        *out_len = 0;
+    }
+
+    mbedtls_platform_zeroize( &bad_padding, sizeof( bad_padding) );
+    mbedtls_platform_zeroize( &diff, sizeof( diff ) );
+    mbedtls_platform_zeroize( A, sizeof( A ) );
+    mbedtls_cipher_finish( &ctx->cipher_ctx, NULL, &olen );
+    return( ret );
+}
+
+#endif /* !MBEDTLS_NIST_KW_ALT */
+
+#if defined(MBEDTLS_SELF_TEST) && defined(MBEDTLS_AES_C)
+
+#define KW_TESTS 3
+
+/*
+ * Test vectors taken from NIST
+ * https://csrc.nist.gov/Projects/Cryptographic-Algorithm-Validation-Program/CAVP-TESTING-BLOCK-CIPHER-MODES#KW
+ */
+static const unsigned int key_len[KW_TESTS] = { 16, 24, 32 };
+
+static const unsigned char kw_key[KW_TESTS][32] = {
+    { 0x75, 0x75, 0xda, 0x3a, 0x93, 0x60, 0x7c, 0xc2,
+      0xbf, 0xd8, 0xce, 0xc7, 0xaa, 0xdf, 0xd9, 0xa6 },
+    { 0x2d, 0x85, 0x26, 0x08, 0x1d, 0x02, 0xfb, 0x5b,
+      0x85, 0xf6, 0x9a, 0xc2, 0x86, 0xec, 0xd5, 0x7d,
+      0x40, 0xdf, 0x5d, 0xf3, 0x49, 0x47, 0x44, 0xd3 },
+    { 0x11, 0x2a, 0xd4, 0x1b, 0x48, 0x56, 0xc7, 0x25,
+      0x4a, 0x98, 0x48, 0xd3, 0x0f, 0xdd, 0x78, 0x33,
+      0x5b, 0x03, 0x9a, 0x48, 0xa8, 0x96, 0x2c, 0x4d,
+      0x1c, 0xb7, 0x8e, 0xab, 0xd5, 0xda, 0xd7, 0x88 }
+};
+
+static const unsigned char kw_msg[KW_TESTS][40] = {
+    { 0x42, 0x13, 0x6d, 0x3c, 0x38, 0x4a, 0x3e, 0xea,
+      0xc9, 0x5a, 0x06, 0x6f, 0xd2, 0x8f, 0xed, 0x3f },
+    { 0x95, 0xc1, 0x1b, 0xf5, 0x35, 0x3a, 0xfe, 0xdb,
+      0x98, 0xfd, 0xd6, 0xc8, 0xca, 0x6f, 0xdb, 0x6d,
+      0xa5, 0x4b, 0x74, 0xb4, 0x99, 0x0f, 0xdc, 0x45,
+      0xc0, 0x9d, 0x15, 0x8f, 0x51, 0xce, 0x62, 0x9d,
+      0xe2, 0xaf, 0x26, 0xe3, 0x25, 0x0e, 0x6b, 0x4c },
+    { 0x1b, 0x20, 0xbf, 0x19, 0x90, 0xb0, 0x65, 0xd7,
+      0x98, 0xe1, 0xb3, 0x22, 0x64, 0xad, 0x50, 0xa8,
+      0x74, 0x74, 0x92, 0xba, 0x09, 0xa0, 0x4d, 0xd1 }
+};
+
+static const size_t kw_msg_len[KW_TESTS] = { 16, 40, 24 };
+static const size_t kw_out_len[KW_TESTS] = { 24, 48, 32 };
+static const unsigned char kw_res[KW_TESTS][48] = {
+    { 0x03, 0x1f, 0x6b, 0xd7, 0xe6, 0x1e, 0x64, 0x3d,
+      0xf6, 0x85, 0x94, 0x81, 0x6f, 0x64, 0xca, 0xa3,
+      0xf5, 0x6f, 0xab, 0xea, 0x25, 0x48, 0xf5, 0xfb },
+    { 0x44, 0x3c, 0x6f, 0x15, 0x09, 0x83, 0x71, 0x91,
+      0x3e, 0x5c, 0x81, 0x4c, 0xa1, 0xa0, 0x42, 0xec,
+      0x68, 0x2f, 0x7b, 0x13, 0x6d, 0x24, 0x3a, 0x4d,
+      0x6c, 0x42, 0x6f, 0xc6, 0x97, 0x15, 0x63, 0xe8,
+      0xa1, 0x4a, 0x55, 0x8e, 0x09, 0x64, 0x16, 0x19,
+      0xbf, 0x03, 0xfc, 0xaf, 0x90, 0xb1, 0xfc, 0x2d },
+    { 0xba, 0x8a, 0x25, 0x9a, 0x47, 0x1b, 0x78, 0x7d,
+      0xd5, 0xd5, 0x40, 0xec, 0x25, 0xd4, 0x3d, 0x87,
+      0x20, 0x0f, 0xda, 0xdc, 0x6d, 0x1f, 0x05, 0xd9,
+      0x16, 0x58, 0x4f, 0xa9, 0xf6, 0xcb, 0xf5, 0x12 }
+};
+
+static const unsigned char kwp_key[KW_TESTS][32] = {
+    { 0x78, 0x65, 0xe2, 0x0f, 0x3c, 0x21, 0x65, 0x9a,
+      0xb4, 0x69, 0x0b, 0x62, 0x9c, 0xdf, 0x3c, 0xc4 },
+    { 0xf5, 0xf8, 0x96, 0xa3, 0xbd, 0x2f, 0x4a, 0x98,
+      0x23, 0xef, 0x16, 0x2b, 0x00, 0xb8, 0x05, 0xd7,
+      0xde, 0x1e, 0xa4, 0x66, 0x26, 0x96, 0xa2, 0x58 },
+    { 0x95, 0xda, 0x27, 0x00, 0xca, 0x6f, 0xd9, 0xa5,
+      0x25, 0x54, 0xee, 0x2a, 0x8d, 0xf1, 0x38, 0x6f,
+      0x5b, 0x94, 0xa1, 0xa6, 0x0e, 0xd8, 0xa4, 0xae,
+      0xf6, 0x0a, 0x8d, 0x61, 0xab, 0x5f, 0x22, 0x5a }
+};
+
+static const unsigned char kwp_msg[KW_TESTS][31] = {
+    { 0xbd, 0x68, 0x43, 0xd4, 0x20, 0x37, 0x8d, 0xc8,
+      0x96 },
+    { 0x6c, 0xcd, 0xd5, 0x85, 0x18, 0x40, 0x97, 0xeb,
+      0xd5, 0xc3, 0xaf, 0x3e, 0x47, 0xd0, 0x2c, 0x19,
+      0x14, 0x7b, 0x4d, 0x99, 0x5f, 0x96, 0x43, 0x66,
+      0x91, 0x56, 0x75, 0x8c, 0x13, 0x16, 0x8f },
+    { 0xd1 }
+};
+static const size_t kwp_msg_len[KW_TESTS] = { 9, 31, 1 };
+
+static const unsigned char kwp_res[KW_TESTS][48] = {
+    { 0x41, 0xec, 0xa9, 0x56, 0xd4, 0xaa, 0x04, 0x7e,
+      0xb5, 0xcf, 0x4e, 0xfe, 0x65, 0x96, 0x61, 0xe7,
+      0x4d, 0xb6, 0xf8, 0xc5, 0x64, 0xe2, 0x35, 0x00 },
+    { 0x4e, 0x9b, 0xc2, 0xbc, 0xbc, 0x6c, 0x1e, 0x13,
+      0xd3, 0x35, 0xbc, 0xc0, 0xf7, 0x73, 0x6a, 0x88,
+      0xfa, 0x87, 0x53, 0x66, 0x15, 0xbb, 0x8e, 0x63,
+      0x8b, 0xcc, 0x81, 0x66, 0x84, 0x68, 0x17, 0x90,
+      0x67, 0xcf, 0xa9, 0x8a, 0x9d, 0x0e, 0x33, 0x26 },
+    { 0x06, 0xba, 0x7a, 0xe6, 0xf3, 0x24, 0x8c, 0xfd,
+      0xcf, 0x26, 0x75, 0x07, 0xfa, 0x00, 0x1b, 0xc4  }
+};
+static const size_t kwp_out_len[KW_TESTS] = { 24, 40, 16 };
+
+int mbedtls_nist_kw_self_test( int verbose )
+{
+    mbedtls_nist_kw_context ctx;
+    unsigned char out[48];
+    size_t olen;
+    int i;
+    int ret = 0;
+    mbedtls_nist_kw_init( &ctx );
+
+    for( i = 0; i < KW_TESTS; i++ )
+    {
+        if( verbose != 0 )
+            mbedtls_printf( "  KW-AES-%u ", (unsigned int) key_len[i] * 8 );
+
+        ret = mbedtls_nist_kw_setkey( &ctx, MBEDTLS_CIPHER_ID_AES,
+                                      kw_key[i], key_len[i] * 8, 1 );
+        if( ret != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "  KW: setup failed " );
+
+            goto end;
+        }
+
+        ret = mbedtls_nist_kw_wrap( &ctx, MBEDTLS_KW_MODE_KW, kw_msg[i],
+                                    kw_msg_len[i], out, &olen, sizeof( out ) );
+        if( ret != 0 || kw_out_len[i] != olen ||
+            memcmp( out, kw_res[i], kw_out_len[i] ) != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "failed. ");
+
+            ret = 1;
+            goto end;
+        }
+
+        if( ( ret = mbedtls_nist_kw_setkey( &ctx, MBEDTLS_CIPHER_ID_AES,
+                                            kw_key[i], key_len[i] * 8, 0 ) )
+              != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "  KW: setup failed ");
+
+            goto end;
+        }
+
+        ret = mbedtls_nist_kw_unwrap( &ctx, MBEDTLS_KW_MODE_KW,
+                                      out, olen, out, &olen, sizeof( out ) );
+
+        if( ret != 0 || olen != kw_msg_len[i] ||
+            memcmp( out, kw_msg[i], kw_msg_len[i] ) != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "failed\n" );
+
+            ret = 1;
+            goto end;
+        }
+
+        if( verbose != 0 )
+            mbedtls_printf( " passed\n" );
+    }
+
+    for( i = 0; i < KW_TESTS; i++ )
+    {
+        olen = sizeof( out );
+        if( verbose != 0 )
+            mbedtls_printf( "  KWP-AES-%u ", (unsigned int) key_len[i] * 8 );
+
+        ret = mbedtls_nist_kw_setkey( &ctx, MBEDTLS_CIPHER_ID_AES, kwp_key[i],
+                                      key_len[i] * 8, 1 );
+        if( ret  != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "  KWP: setup failed " );
+
+            goto end;
+        }
+        ret = mbedtls_nist_kw_wrap( &ctx, MBEDTLS_KW_MODE_KWP, kwp_msg[i],
+                                    kwp_msg_len[i], out, &olen, sizeof( out ) );
+
+        if( ret != 0 || kwp_out_len[i] != olen ||
+            memcmp( out, kwp_res[i], kwp_out_len[i] ) != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "failed. ");
+
+            ret = 1;
+            goto end;
+        }
+
+        if( ( ret = mbedtls_nist_kw_setkey( &ctx, MBEDTLS_CIPHER_ID_AES,
+                                            kwp_key[i], key_len[i] * 8, 0 ) )
+              != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "  KWP: setup failed ");
+
+            goto end;
+        }
+
+        ret = mbedtls_nist_kw_unwrap(  &ctx, MBEDTLS_KW_MODE_KWP, out,
+                                       olen, out, &olen, sizeof( out ) );
+
+        if( ret != 0 || olen != kwp_msg_len[i] ||
+            memcmp( out, kwp_msg[i], kwp_msg_len[i] ) != 0 )
+        {
+            if( verbose != 0 )
+                mbedtls_printf( "failed. ");
+
+            ret = 1;
+            goto end;
+        }
+
+        if( verbose != 0 )
+            mbedtls_printf( " passed\n" );
+    }
+end:
+    mbedtls_nist_kw_free( &ctx );
+
+    if( verbose != 0 )
+        mbedtls_printf( "\n" );
+
+    return( ret );
+}
+
+#endif /* MBEDTLS_SELF_TEST && MBEDTLS_AES_C */
+
+#endif /* MBEDTLS_NIST_KW_C */
diff --git a/library/pkparse.c b/library/pkparse.c
index ccb7f540..d6ac987e 100644
--- a/library/pkparse.c
+++ b/library/pkparse.c
@@ -1261,7 +1261,6 @@ int mbedtls_pk_parse_key( mbedtls_pk_context *pk,
         return( ret );
 #endif /* MBEDTLS_PKCS12_C || MBEDTLS_PKCS5_C */
 #else
-    ((void) ret);
     ((void) pwd);
     ((void) pwdlen);
 #endif /* MBEDTLS_PEM_PARSE_C */
diff --git a/library/platform_util.c b/library/platform_util.c
index 1a57de93..ca5fe4fb 100644
--- a/library/platform_util.c
+++ b/library/platform_util.c
@@ -20,6 +20,14 @@
  *  This file is part of Mbed TLS (https://tls.mbed.org)
  */
 
+/*
+ * Ensure gmtime_r is available even with -std=c99; must be defined before
+ * config.h, which pulls in glibc's features.h. Harmless on other platforms.
+ */
+#if !defined(_POSIX_C_SOURCE)
+#define _POSIX_C_SOURCE 200112L
+#endif
+
 #if !defined(MBEDTLS_CONFIG_FILE)
 #include "mbedtls/config.h"
 #else
@@ -27,6 +35,7 @@
 #endif
 
 #include "mbedtls/platform_util.h"
+#include "mbedtls/threading.h"
 
 #include <stddef.h>
 #include <string.h>
@@ -65,3 +74,62 @@ void mbedtls_platform_zeroize( void *buf, size_t len )
     memset_func( buf, 0, len );
 }
 #endif /* MBEDTLS_PLATFORM_ZEROIZE_ALT */
+
+#if defined(MBEDTLS_HAVE_TIME_DATE) && !defined(MBEDTLS_PLATFORM_GMTIME_R_ALT)
+#include <time.h>
+#if !defined(_WIN32) && (defined(unix) || \
+    defined(__unix) || defined(__unix__) || (defined(__APPLE__) && \
+    defined(__MACH__)))
+#include <unistd.h>
+#endif /* !_WIN32 && (unix || __unix || __unix__ ||
+        * (__APPLE__ && __MACH__)) */
+
+#if !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) ||     \
+       ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) &&                     \
+         _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) )
+/*
+ * This is a convenience shorthand macro to avoid checking the long
+ * preprocessor conditions above. Ideally, we could expose this macro in
+ * platform_util.h and simply use it in platform_util.c, threading.c and
+ * threading.h. However, this macro is not part of the Mbed TLS public API, so
+ * we keep it private by only defining it in this file
+ */
+#if ! ( defined(_WIN32) && !defined(EFIX64) && !defined(EFI32) )
+#define PLATFORM_UTIL_USE_GMTIME
+#endif /* ! ( defined(_WIN32) && !defined(EFIX64) && !defined(EFI32) ) */
+
+#endif /* !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) ||     \
+             ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) &&                     \
+                _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) */
+
+struct tm *mbedtls_platform_gmtime_r( const mbedtls_time_t *tt,
+                                      struct tm *tm_buf )
+{
+#if defined(_WIN32) && !defined(EFIX64) && !defined(EFI32)
+    return( ( gmtime_s( tm_buf, tt ) == 0 ) ? tm_buf : NULL );
+#elif !defined(PLATFORM_UTIL_USE_GMTIME)
+    return( gmtime_r( tt, tm_buf ) );
+#else
+    struct tm *lt;
+
+#if defined(MBEDTLS_THREADING_C)
+    if( mbedtls_mutex_lock( &mbedtls_threading_gmtime_mutex ) != 0 )
+        return( NULL );
+#endif /* MBEDTLS_THREADING_C */
+
+    lt = gmtime( tt );
+
+    if( lt != NULL )
+    {
+        memcpy( tm_buf, lt, sizeof( struct tm ) );
+    }
+
+#if defined(MBEDTLS_THREADING_C)
+    if( mbedtls_mutex_unlock( &mbedtls_threading_gmtime_mutex ) != 0 )
+        return( NULL );
+#endif /* MBEDTLS_THREADING_C */
+
+    return( ( lt == NULL ) ? NULL : tm_buf );
+#endif /* _WIN32 && !EFIX64 && !EFI32 */
+}
+#endif /* MBEDTLS_HAVE_TIME_DATE && MBEDTLS_PLATFORM_GMTIME_R_ALT */
diff --git a/library/poly1305.c b/library/poly1305.c
new file mode 100644
index 00000000..e22d3afb
--- /dev/null
+++ b/library/poly1305.c
@@ -0,0 +1,563 @@
+/**
+ * \file poly1305.c
+ *
+ * \brief Poly1305 authentication algorithm.
+ *
+ *  Copyright (C) 2006-2016, ARM Limited, All Rights Reserved
+ *  SPDX-License-Identifier: Apache-2.0
+ *
+ *  Licensed under the Apache License, Version 2.0 (the "License"); you may
+ *  not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ *  WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ *
+ *  This file is part of mbed TLS (https://tls.mbed.org)
+ */
+#if !defined(MBEDTLS_CONFIG_FILE)
+#include "mbedtls/config.h"
+#else
+#include MBEDTLS_CONFIG_FILE
+#endif
+
+#if defined(MBEDTLS_POLY1305_C)
+
+#include "mbedtls/poly1305.h"
+#include "mbedtls/platform_util.h"
+
+#include <string.h>
+
+#if defined(MBEDTLS_SELF_TEST)
+#if defined(MBEDTLS_PLATFORM_C)
+#include "mbedtls/platform.h"
+#else
+#include <stdio.h>
+#define mbedtls_printf printf
+#endif /* MBEDTLS_PLATFORM_C */
+#endif /* MBEDTLS_SELF_TEST */
+
+#if !defined(MBEDTLS_POLY1305_ALT)
+
+#if ( defined(__ARMCC_VERSION) || defined(_MSC_VER) ) && \
+    !defined(inline) && !defined(__cplusplus)
+#define inline __inline
+#endif
+
+#define POLY1305_BLOCK_SIZE_BYTES ( 16U )
+
+#define BYTES_TO_U32_LE( data, offset )                           \
+    ( (uint32_t) data[offset]                                     \
+          | (uint32_t) ( (uint32_t) data[( offset ) + 1] << 8 )   \
+          | (uint32_t) ( (uint32_t) data[( offset ) + 2] << 16 )  \
+          | (uint32_t) ( (uint32_t) data[( offset ) + 3] << 24 )  \
+    )
+
+/*
+ * Our implementation is tuned for 32-bit platforms with a 64-bit multiplier.
+ * However we provided an alternative for platforms without such a multiplier.
+ */
+#if defined(MBEDTLS_NO_64BIT_MULTIPLICATION)
+static uint64_t mul64( uint32_t a, uint32_t b )
+{
+    /* a = al + 2**16 ah, b = bl + 2**16 bh */
+    const uint16_t al = (uint16_t) a;
+    const uint16_t bl = (uint16_t) b;
+    const uint16_t ah = a >> 16;
+    const uint16_t bh = b >> 16;
+
+    /* ab = al*bl + 2**16 (ah*bl + bl*bh) + 2**32 ah*bh */
+    const uint32_t lo = (uint32_t) al * bl;
+    const uint64_t me = (uint64_t)( (uint32_t) ah * bl ) + (uint32_t) al * bh;
+    const uint32_t hi = (uint32_t) ah * bh;
+
+    return( lo + ( me << 16 ) + ( (uint64_t) hi << 32 ) );
+}
+#else
+static inline uint64_t mul64( uint32_t a, uint32_t b )
+{
+    return( (uint64_t) a * b );
+}
+#endif
+
+
+/**
+ * \brief                   Process blocks with Poly1305.
+ *
+ * \param ctx               The Poly1305 context.
+ * \param nblocks           Number of blocks to process. Note that this
+ *                          function only processes full blocks.
+ * \param input             Buffer containing the input block(s).
+ * \param needs_padding     Set to 0 if the padding bit has already been
+ *                          applied to the input data before calling this
+ *                          function.  Otherwise, set this parameter to 1.
+ */
+static void poly1305_process( mbedtls_poly1305_context *ctx,
+                              size_t nblocks,
+                              const unsigned char *input,
+                              uint32_t needs_padding )
+{
+    uint64_t d0, d1, d2, d3;
+    uint32_t acc0, acc1, acc2, acc3, acc4;
+    uint32_t r0, r1, r2, r3;
+    uint32_t rs1, rs2, rs3;
+    size_t offset  = 0U;
+    size_t i;
+
+    r0 = ctx->r[0];
+    r1 = ctx->r[1];
+    r2 = ctx->r[2];
+    r3 = ctx->r[3];
+
+    rs1 = r1 + ( r1 >> 2U );
+    rs2 = r2 + ( r2 >> 2U );
+    rs3 = r3 + ( r3 >> 2U );
+
+    acc0 = ctx->acc[0];
+    acc1 = ctx->acc[1];
+    acc2 = ctx->acc[2];
+    acc3 = ctx->acc[3];
+    acc4 = ctx->acc[4];
+
+    /* Process full blocks */
+    for( i = 0U; i < nblocks; i++ )
+    {
+        /* The input block is treated as a 128-bit little-endian integer */
+        d0   = BYTES_TO_U32_LE( input, offset + 0  );
+        d1   = BYTES_TO_U32_LE( input, offset + 4  );
+        d2   = BYTES_TO_U32_LE( input, offset + 8  );
+        d3   = BYTES_TO_U32_LE( input, offset + 12 );
+
+        /* Compute: acc += (padded) block as a 130-bit integer */
+        d0  += (uint64_t) acc0;
+        d1  += (uint64_t) acc1 + ( d0 >> 32U );
+        d2  += (uint64_t) acc2 + ( d1 >> 32U );
+        d3  += (uint64_t) acc3 + ( d2 >> 32U );
+        acc0 = (uint32_t) d0;
+        acc1 = (uint32_t) d1;
+        acc2 = (uint32_t) d2;
+        acc3 = (uint32_t) d3;
+        acc4 += (uint32_t) ( d3 >> 32U ) + needs_padding;
+
+        /* Compute: acc *= r */
+        d0 = mul64( acc0, r0  ) +
+             mul64( acc1, rs3 ) +
+             mul64( acc2, rs2 ) +
+             mul64( acc3, rs1 );
+        d1 = mul64( acc0, r1  ) +
+             mul64( acc1, r0  ) +
+             mul64( acc2, rs3 ) +
+             mul64( acc3, rs2 ) +
+             mul64( acc4, rs1 );
+        d2 = mul64( acc0, r2  ) +
+             mul64( acc1, r1  ) +
+             mul64( acc2, r0  ) +
+             mul64( acc3, rs3 ) +
+             mul64( acc4, rs2 );
+        d3 = mul64( acc0, r3  ) +
+             mul64( acc1, r2  ) +
+             mul64( acc2, r1  ) +
+             mul64( acc3, r0  ) +
+             mul64( acc4, rs3 );
+        acc4 *= r0;
+
+        /* Compute: acc %= (2^130 - 5) (partial remainder) */
+        d1 += ( d0 >> 32 );
+        d2 += ( d1 >> 32 );
+        d3 += ( d2 >> 32 );
+        acc0 = (uint32_t) d0;
+        acc1 = (uint32_t) d1;
+        acc2 = (uint32_t) d2;
+        acc3 = (uint32_t) d3;
+        acc4 = (uint32_t) ( d3 >> 32 ) + acc4;
+
+        d0 = (uint64_t) acc0 + ( acc4 >> 2 ) + ( acc4 & 0xFFFFFFFCU );
+        acc4 &= 3U;
+        acc0 = (uint32_t) d0;
+        d0 = (uint64_t) acc1 + ( d0 >> 32U );
+        acc1 = (uint32_t) d0;
+        d0 = (uint64_t) acc2 + ( d0 >> 32U );
+        acc2 = (uint32_t) d0;
+        d0 = (uint64_t) acc3 + ( d0 >> 32U );
+        acc3 = (uint32_t) d0;
+        d0 = (uint64_t) acc4 + ( d0 >> 32U );
+        acc4 = (uint32_t) d0;
+
+        offset    += POLY1305_BLOCK_SIZE_BYTES;
+    }
+
+    ctx->acc[0] = acc0;
+    ctx->acc[1] = acc1;
+    ctx->acc[2] = acc2;
+    ctx->acc[3] = acc3;
+    ctx->acc[4] = acc4;
+}
+
+/**
+ * \brief                   Compute the Poly1305 MAC
+ *
+ * \param ctx               The Poly1305 context.
+ * \param mac               The buffer to where the MAC is written. Must be
+ *                          big enough to contain the 16-byte MAC.
+ */
+static void poly1305_compute_mac( const mbedtls_poly1305_context *ctx,
+                                  unsigned char mac[16] )
+{
+    uint64_t d;
+    uint32_t g0, g1, g2, g3, g4;
+    uint32_t acc0, acc1, acc2, acc3, acc4;
+    uint32_t mask;
+    uint32_t mask_inv;
+
+    acc0 = ctx->acc[0];
+    acc1 = ctx->acc[1];
+    acc2 = ctx->acc[2];
+    acc3 = ctx->acc[3];
+    acc4 = ctx->acc[4];
+
+    /* Before adding 's' we ensure that the accumulator is mod 2^130 - 5.
+     * We do this by calculating acc - (2^130 - 5), then checking if
+     * the 131st bit is set. If it is, then reduce: acc -= (2^130 - 5)
+     */
+
+    /* Calculate acc + -(2^130 - 5) */
+    d  = ( (uint64_t) acc0 + 5U );
+    g0 = (uint32_t) d;
+    d  = ( (uint64_t) acc1 + ( d >> 32 ) );
+    g1 = (uint32_t) d;
+    d  = ( (uint64_t) acc2 + ( d >> 32 ) );
+    g2 = (uint32_t) d;
+    d  = ( (uint64_t) acc3 + ( d >> 32 ) );
+    g3 = (uint32_t) d;
+    g4 = acc4 + (uint32_t) ( d >> 32U );
+
+    /* mask == 0xFFFFFFFF if 131st bit is set, otherwise mask == 0 */
+    mask = (uint32_t) 0U - ( g4 >> 2U );
+    mask_inv = ~mask;
+
+    /* If 131st bit is set then acc=g, otherwise, acc is unmodified */
+    acc0 = ( acc0 & mask_inv ) | ( g0 & mask );
+    acc1 = ( acc1 & mask_inv ) | ( g1 & mask );
+    acc2 = ( acc2 & mask_inv ) | ( g2 & mask );
+    acc3 = ( acc3 & mask_inv ) | ( g3 & mask );
+
+    /* Add 's' */
+    d = (uint64_t) acc0 + ctx->s[0];
+    acc0 = (uint32_t) d;
+    d = (uint64_t) acc1 + ctx->s[1] + ( d >> 32U );
+    acc1 = (uint32_t) d;
+    d = (uint64_t) acc2 + ctx->s[2] + ( d >> 32U );
+    acc2 = (uint32_t) d;
+    acc3 += ctx->s[3] + (uint32_t) ( d >> 32U );
+
+    /* Compute MAC (128 least significant bits of the accumulator) */
+    mac[ 0] = (unsigned char)( acc0       );
+    mac[ 1] = (unsigned char)( acc0 >>  8 );
+    mac[ 2] = (unsigned char)( acc0 >> 16 );
+    mac[ 3] = (unsigned char)( acc0 >> 24 );
+    mac[ 4] = (unsigned char)( acc1       );
+    mac[ 5] = (unsigned char)( acc1 >>  8 );
+    mac[ 6] = (unsigned char)( acc1 >> 16 );
+    mac[ 7] = (unsigned char)( acc1 >> 24 );
+    mac[ 8] = (unsigned char)( acc2       );
+    mac[ 9] = (unsigned char)( acc2 >>  8 );
+    mac[10] = (unsigned char)( acc2 >> 16 );
+    mac[11] = (unsigned char)( acc2 >> 24 );
+    mac[12] = (unsigned char)( acc3       );
+    mac[13] = (unsigned char)( acc3 >>  8 );
+    mac[14] = (unsigned char)( acc3 >> 16 );
+    mac[15] = (unsigned char)( acc3 >> 24 );
+}
+
+void mbedtls_poly1305_init( mbedtls_poly1305_context *ctx )
+{
+    if( ctx != NULL )
+    {
+        mbedtls_platform_zeroize( ctx, sizeof( mbedtls_poly1305_context ) );
+    }
+}
+
+void mbedtls_poly1305_free( mbedtls_poly1305_context *ctx )
+{
+    if( ctx != NULL )
+    {
+        mbedtls_platform_zeroize( ctx, sizeof( mbedtls_poly1305_context ) );
+    }
+}
+
+int mbedtls_poly1305_starts( mbedtls_poly1305_context *ctx,
+                             const unsigned char key[32] )
+{
+    if( ctx == NULL || key == NULL )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+
+    /* r &= 0x0ffffffc0ffffffc0ffffffc0fffffff */
+    ctx->r[0] = BYTES_TO_U32_LE( key, 0 )  & 0x0FFFFFFFU;
+    ctx->r[1] = BYTES_TO_U32_LE( key, 4 )  & 0x0FFFFFFCU;
+    ctx->r[2] = BYTES_TO_U32_LE( key, 8 )  & 0x0FFFFFFCU;
+    ctx->r[3] = BYTES_TO_U32_LE( key, 12 ) & 0x0FFFFFFCU;
+
+    ctx->s[0] = BYTES_TO_U32_LE( key, 16 );
+    ctx->s[1] = BYTES_TO_U32_LE( key, 20 );
+    ctx->s[2] = BYTES_TO_U32_LE( key, 24 );
+    ctx->s[3] = BYTES_TO_U32_LE( key, 28 );
+
+    /* Initial accumulator state */
+    ctx->acc[0] = 0U;
+    ctx->acc[1] = 0U;
+    ctx->acc[2] = 0U;
+    ctx->acc[3] = 0U;
+    ctx->acc[4] = 0U;
+
+    /* Queue initially empty */
+    mbedtls_platform_zeroize( ctx->queue, sizeof( ctx->queue ) );
+    ctx->queue_len = 0U;
+
+    return( 0 );
+}
+
+int mbedtls_poly1305_update( mbedtls_poly1305_context *ctx,
+                             const unsigned char *input,
+                             size_t ilen )
+{
+    size_t offset    = 0U;
+    size_t remaining = ilen;
+    size_t queue_free_len;
+    size_t nblocks;
+
+    if( ctx == NULL )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+    else if( ( ilen > 0U ) && ( input == NULL ) )
+    {
+        /* input pointer is allowed to be NULL only if ilen == 0 */
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+
+    if( ( remaining > 0U ) && ( ctx->queue_len > 0U ) )
+    {
+        queue_free_len = ( POLY1305_BLOCK_SIZE_BYTES - ctx->queue_len );
+
+        if( ilen < queue_free_len )
+        {
+            /* Not enough data to complete the block.
+             * Store this data with the other leftovers.
+             */
+            memcpy( &ctx->queue[ctx->queue_len],
+                    input,
+                    ilen );
+
+            ctx->queue_len += ilen;
+
+            remaining = 0U;
+        }
+        else
+        {
+            /* Enough data to produce a complete block */
+            memcpy( &ctx->queue[ctx->queue_len],
+                    input,
+                    queue_free_len );
+
+            ctx->queue_len = 0U;
+
+            poly1305_process( ctx, 1U, ctx->queue, 1U ); /* add padding bit */
+
+            offset    += queue_free_len;
+            remaining -= queue_free_len;
+        }
+    }
+
+    if( remaining >= POLY1305_BLOCK_SIZE_BYTES )
+    {
+        nblocks = remaining / POLY1305_BLOCK_SIZE_BYTES;
+
+        poly1305_process( ctx, nblocks, &input[offset], 1U );
+
+        offset += nblocks * POLY1305_BLOCK_SIZE_BYTES;
+        remaining %= POLY1305_BLOCK_SIZE_BYTES;
+    }
+
+    if( remaining > 0U )
+    {
+        /* Store partial block */
+        ctx->queue_len = remaining;
+        memcpy( ctx->queue, &input[offset], remaining );
+    }
+
+    return( 0 );
+}
+
+int mbedtls_poly1305_finish( mbedtls_poly1305_context *ctx,
+                             unsigned char mac[16] )
+{
+    if( ( ctx == NULL ) || ( mac == NULL ) )
+    {
+        return( MBEDTLS_ERR_POLY1305_BAD_INPUT_DATA );
+    }
+
+    /* Process any leftover data */
+    if( ctx->queue_len > 0U )
+    {
+        /* Add padding bit */
+        ctx->queue[ctx->queue_len] = 1U;
+        ctx->queue_len++;
+
+        /* Pad with zeroes */
+        memset( &ctx->queue[ctx->queue_len],
+                0,
+                POLY1305_BLOCK_SIZE_BYTES - ctx->queue_len );
+
+        poly1305_process( ctx, 1U,          /* Process 1 block */
+                          ctx->queue, 0U ); /* Already padded above */
+    }
+
+    poly1305_compute_mac( ctx, mac );
+
+    return( 0 );
+}
+
+int mbedtls_poly1305_mac( const unsigned char key[32],
+                          const unsigned char *input,
+                          size_t ilen,
+                          unsigned char mac[16] )
+{
+    mbedtls_poly1305_context ctx;
+    int ret;
+
+    mbedtls_poly1305_init( &ctx );
+
+    ret = mbedtls_poly1305_starts( &ctx, key );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_poly1305_update( &ctx, input, ilen );
+    if( ret != 0 )
+        goto cleanup;
+
+    ret = mbedtls_poly1305_finish( &ctx, mac );
+
+cleanup:
+    mbedtls_poly1305_free( &ctx );
+    return( ret );
+}
+
+#endif /* MBEDTLS_POLY1305_ALT */
+
+#if defined(MBEDTLS_SELF_TEST)
+
+static const unsigned char test_keys[2][32] =
+{
+    {
+        0x85, 0xd6, 0xbe, 0x78, 0x57, 0x55, 0x6d, 0x33,
+        0x7f, 0x44, 0x52, 0xfe, 0x42, 0xd5, 0x06, 0xa8,
+        0x01, 0x03, 0x80, 0x8a, 0xfb, 0x0d, 0xb2, 0xfd,
+        0x4a, 0xbf, 0xf6, 0xaf, 0x41, 0x49, 0xf5, 0x1b
+    },
+    {
+        0x1c, 0x92, 0x40, 0xa5, 0xeb, 0x55, 0xd3, 0x8a,
+        0xf3, 0x33, 0x88, 0x86, 0x04, 0xf6, 0xb5, 0xf0,
+        0x47, 0x39, 0x17, 0xc1, 0x40, 0x2b, 0x80, 0x09,
+        0x9d, 0xca, 0x5c, 0xbc, 0x20, 0x70, 0x75, 0xc0
+    }
+};
+
+static const unsigned char test_data[2][127] =
+{
+    {
+        0x43, 0x72, 0x79, 0x70, 0x74, 0x6f, 0x67, 0x72,
+        0x61, 0x70, 0x68, 0x69, 0x63, 0x20, 0x46, 0x6f,
+        0x72, 0x75, 0x6d, 0x20, 0x52, 0x65, 0x73, 0x65,
+        0x61, 0x72, 0x63, 0x68, 0x20, 0x47, 0x72, 0x6f,
+        0x75, 0x70
+    },
+    {
+        0x27, 0x54, 0x77, 0x61, 0x73, 0x20, 0x62, 0x72,
+        0x69, 0x6c, 0x6c, 0x69, 0x67, 0x2c, 0x20, 0x61,
+        0x6e, 0x64, 0x20, 0x74, 0x68, 0x65, 0x20, 0x73,
+        0x6c, 0x69, 0x74, 0x68, 0x79, 0x20, 0x74, 0x6f,
+        0x76, 0x65, 0x73, 0x0a, 0x44, 0x69, 0x64, 0x20,
+        0x67, 0x79, 0x72, 0x65, 0x20, 0x61, 0x6e, 0x64,
+        0x20, 0x67, 0x69, 0x6d, 0x62, 0x6c, 0x65, 0x20,
+        0x69, 0x6e, 0x20, 0x74, 0x68, 0x65, 0x20, 0x77,
+        0x61, 0x62, 0x65, 0x3a, 0x0a, 0x41, 0x6c, 0x6c,
+        0x20, 0x6d, 0x69, 0x6d, 0x73, 0x79, 0x20, 0x77,
+        0x65, 0x72, 0x65, 0x20, 0x74, 0x68, 0x65, 0x20,
+        0x62, 0x6f, 0x72, 0x6f, 0x67, 0x6f, 0x76, 0x65,
+        0x73, 0x2c, 0x0a, 0x41, 0x6e, 0x64, 0x20, 0x74,
+        0x68, 0x65, 0x20, 0x6d, 0x6f, 0x6d, 0x65, 0x20,
+        0x72, 0x61, 0x74, 0x68, 0x73, 0x20, 0x6f, 0x75,
+        0x74, 0x67, 0x72, 0x61, 0x62, 0x65, 0x2e
+    }
+};
+
+static const size_t test_data_len[2] =
+{
+    34U,
+    127U
+};
+
+static const unsigned char test_mac[2][16] =
+{
+    {
+        0xa8, 0x06, 0x1d, 0xc1, 0x30, 0x51, 0x36, 0xc6,
+        0xc2, 0x2b, 0x8b, 0xaf, 0x0c, 0x01, 0x27, 0xa9
+    },
+    {
+        0x45, 0x41, 0x66, 0x9a, 0x7e, 0xaa, 0xee, 0x61,
+        0xe7, 0x08, 0xdc, 0x7c, 0xbc, 0xc5, 0xeb, 0x62
+    }
+};
+
+#define ASSERT( cond, args )            \
+    do                                  \
+    {                                   \
+        if( ! ( cond ) )                \
+        {                               \
+            if( verbose != 0 )          \
+                mbedtls_printf args;    \
+                                        \
+            return( -1 );               \
+        }                               \
+    }                                   \
+    while( 0 )
+
+int mbedtls_poly1305_self_test( int verbose )
+{
+    unsigned char mac[16];
+    unsigned i;
+    int ret;
+
+    for( i = 0U; i < 2U; i++ )
+    {
+        if( verbose != 0 )
+            mbedtls_printf( "  Poly1305 test %u ", i );
+
+        ret = mbedtls_poly1305_mac( test_keys[i],
+                                    test_data[i],
+                                    test_data_len[i],
+                                    mac );
+        ASSERT( 0 == ret, ( "error code: %i\n", ret ) );
+
+        ASSERT( 0 == memcmp( mac, test_mac[i], 16U ), ( "failed (mac)\n" ) );
+
+        if( verbose != 0 )
+            mbedtls_printf( "passed\n" );
+    }
+
+    if( verbose != 0 )
+        mbedtls_printf( "\n" );
+
+    return( 0 );
+}
+
+#endif /* MBEDTLS_SELF_TEST */
+
+#endif /* MBEDTLS_POLY1305_C */
diff --git a/library/sha1.c b/library/sha1.c
index 1587de48..bab6087c 100644
--- a/library/sha1.c
+++ b/library/sha1.c
@@ -342,14 +342,6 @@ void mbedtls_sha1_update( mbedtls_sha1_context *ctx,
 }
 #endif
 
-static const unsigned char sha1_padding[64] =
-{
- 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
-};
-
 /*
  * SHA-1 final digest
  */
@@ -357,25 +349,48 @@ int mbedtls_sha1_finish_ret( mbedtls_sha1_context *ctx,
                              unsigned char output[20] )
 {
     int ret;
-    uint32_t last, padn;
+    uint32_t used;
     uint32_t high, low;
-    unsigned char msglen[8];
 
+    /*
+     * Add padding: 0x80 then 0x00 until 8 bytes remain for the length
+     */
+    used = ctx->total[0] & 0x3F;
+
+    ctx->buffer[used++] = 0x80;
+
+    if( used <= 56 )
+    {
+        /* Enough room for padding + length in current block */
+        memset( ctx->buffer + used, 0, 56 - used );
+    }
+    else
+    {
+        /* We'll need an extra block */
+        memset( ctx->buffer + used, 0, 64 - used );
+
+        if( ( ret = mbedtls_internal_sha1_process( ctx, ctx->buffer ) ) != 0 )
+            return( ret );
+
+        memset( ctx->buffer, 0, 56 );
+    }
+
+    /*
+     * Add message length
+     */
     high = ( ctx->total[0] >> 29 )
          | ( ctx->total[1] <<  3 );
     low  = ( ctx->total[0] <<  3 );
 
-    PUT_UINT32_BE( high, msglen, 0 );
-    PUT_UINT32_BE( low,  msglen, 4 );
-
-    last = ctx->total[0] & 0x3F;
-    padn = ( last < 56 ) ? ( 56 - last ) : ( 120 - last );
+    PUT_UINT32_BE( high, ctx->buffer, 56 );
+    PUT_UINT32_BE( low,  ctx->buffer, 60 );
 
-    if( ( ret = mbedtls_sha1_update_ret( ctx, sha1_padding, padn ) ) != 0 )
-        return( ret );
-    if( ( ret = mbedtls_sha1_update_ret( ctx, msglen, 8 ) ) != 0 )
+    if( ( ret = mbedtls_internal_sha1_process( ctx, ctx->buffer ) ) != 0 )
         return( ret );
 
+    /*
+     * Output final state
+     */
     PUT_UINT32_BE( ctx->state[0], output,  0 );
     PUT_UINT32_BE( ctx->state[1], output,  4 );
     PUT_UINT32_BE( ctx->state[2], output,  8 );
diff --git a/library/sha256.c b/library/sha256.c
index 695485d8..dbb4a898 100644
--- a/library/sha256.c
+++ b/library/sha256.c
@@ -311,14 +311,6 @@ void mbedtls_sha256_update( mbedtls_sha256_context *ctx,
 }
 #endif
 
-static const unsigned char sha256_padding[64] =
-{
- 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
-};
-
 /*
  * SHA-256 final digest
  */
@@ -326,26 +318,48 @@ int mbedtls_sha256_finish_ret( mbedtls_sha256_context *ctx,
                                unsigned char output[32] )
 {
     int ret;
-    uint32_t last, padn;
+    uint32_t used;
     uint32_t high, low;
-    unsigned char msglen[8];
 
+    /*
+     * Add padding: 0x80 then 0x00 until 8 bytes remain for the length
+     */
+    used = ctx->total[0] & 0x3F;
+
+    ctx->buffer[used++] = 0x80;
+
+    if( used <= 56 )
+    {
+        /* Enough room for padding + length in current block */
+        memset( ctx->buffer + used, 0, 56 - used );
+    }
+    else
+    {
+        /* We'll need an extra block */
+        memset( ctx->buffer + used, 0, 64 - used );
+
+        if( ( ret = mbedtls_internal_sha256_process( ctx, ctx->buffer ) ) != 0 )
+            return( ret );
+
+        memset( ctx->buffer, 0, 56 );
+    }
+
+    /*
+     * Add message length
+     */
     high = ( ctx->total[0] >> 29 )
          | ( ctx->total[1] <<  3 );
     low  = ( ctx->total[0] <<  3 );
 
-    PUT_UINT32_BE( high, msglen, 0 );
-    PUT_UINT32_BE( low,  msglen, 4 );
-
-    last = ctx->total[0] & 0x3F;
-    padn = ( last < 56 ) ? ( 56 - last ) : ( 120 - last );
-
-    if( ( ret = mbedtls_sha256_update_ret( ctx, sha256_padding, padn ) ) != 0 )
-        return( ret );
+    PUT_UINT32_BE( high, ctx->buffer, 56 );
+    PUT_UINT32_BE( low,  ctx->buffer, 60 );
 
-    if( ( ret = mbedtls_sha256_update_ret( ctx, msglen, 8 ) ) != 0 )
+    if( ( ret = mbedtls_internal_sha256_process( ctx, ctx->buffer ) ) != 0 )
         return( ret );
 
+    /*
+     * Output final state
+     */
     PUT_UINT32_BE( ctx->state[0], output,  0 );
     PUT_UINT32_BE( ctx->state[1], output,  4 );
     PUT_UINT32_BE( ctx->state[2], output,  8 );
diff --git a/library/sha512.c b/library/sha512.c
index 6de94e99..a9440e8a 100644
--- a/library/sha512.c
+++ b/library/sha512.c
@@ -341,18 +341,6 @@ void mbedtls_sha512_update( mbedtls_sha512_context *ctx,
 }
 #endif
 
-static const unsigned char sha512_padding[128] =
-{
- 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
-};
-
 /*
  * SHA-512 final digest
  */
@@ -360,26 +348,48 @@ int mbedtls_sha512_finish_ret( mbedtls_sha512_context *ctx,
                                unsigned char output[64] )
 {
     int ret;
-    size_t last, padn;
+    unsigned used;
     uint64_t high, low;
-    unsigned char msglen[16];
 
-    high = ( ctx->total[0] >> 61 )
-         | ( ctx->total[1] <<  3 );
-    low  = ( ctx->total[0] <<  3 );
+    /*
+     * Add padding: 0x80 then 0x00 until 16 bytes remain for the length
+     */
+    used = ctx->total[0] & 0x7F;
 
-    PUT_UINT64_BE( high, msglen, 0 );
-    PUT_UINT64_BE( low,  msglen, 8 );
+    ctx->buffer[used++] = 0x80;
 
-    last = (size_t)( ctx->total[0] & 0x7F );
-    padn = ( last < 112 ) ? ( 112 - last ) : ( 240 - last );
+    if( used <= 112 )
+    {
+        /* Enough room for padding + length in current block */
+        memset( ctx->buffer + used, 0, 112 - used );
+    }
+    else
+    {
+        /* We'll need an extra block */
+        memset( ctx->buffer + used, 0, 128 - used );
 
-    if( ( ret = mbedtls_sha512_update_ret( ctx, sha512_padding, padn ) ) != 0 )
+        if( ( ret = mbedtls_internal_sha512_process( ctx, ctx->buffer ) ) != 0 )
             return( ret );
 
-    if( ( ret = mbedtls_sha512_update_ret( ctx, msglen, 16 ) ) != 0 )
-            return( ret );
+        memset( ctx->buffer, 0, 112 );
+    }
+
+    /*
+     * Add message length
+     */
+    high = ( ctx->total[0] >> 61 )
+         | ( ctx->total[1] <<  3 );
+    low  = ( ctx->total[0] <<  3 );
+
+    PUT_UINT64_BE( high, ctx->buffer, 112 );
+    PUT_UINT64_BE( low,  ctx->buffer, 120 );
 
+    if( ( ret = mbedtls_internal_sha512_process( ctx, ctx->buffer ) ) != 0 )
+        return( ret );
+
+    /*
+     * Output final state
+     */
     PUT_UINT64_BE( ctx->state[0], output,  0 );
     PUT_UINT64_BE( ctx->state[1], output,  8 );
     PUT_UINT64_BE( ctx->state[2], output, 16 );
diff --git a/library/ssl_ciphersuites.c b/library/ssl_ciphersuites.c
index 2e9a0fd7..745474ef 100644
--- a/library/ssl_ciphersuites.c
+++ b/library/ssl_ciphersuites.c
@@ -47,7 +47,7 @@
  * 1. By key exchange:
  *    Forward-secure non-PSK > forward-secure PSK > ECJPAKE > other non-PSK > other PSK
  * 2. By key length and cipher:
- *    AES-256 > Camellia-256 > ARIA-256 > AES-128 > Camellia-128 > ARIA-128 > 3DES
+ *    ChaCha > AES-256 > Camellia-256 > ARIA-256 > AES-128 > Camellia-128 > ARIA-128 > 3DES
  * 3. By cipher mode when relevant GCM > CCM > CBC > CCM_8
  * 4. By hash function used when relevant
  * 5. By key exchange/auth again: EC > non-EC
@@ -57,6 +57,11 @@ static const int ciphersuite_preference[] =
 #if defined(MBEDTLS_SSL_CIPHERSUITES)
     MBEDTLS_SSL_CIPHERSUITES,
 #else
+    /* Chacha-Poly ephemeral suites */
+    MBEDTLS_TLS_ECDHE_RSA_WITH_CHACHA20_POLY1305_SHA256,
+    MBEDTLS_TLS_ECDHE_ECDSA_WITH_CHACHA20_POLY1305_SHA256,
+    MBEDTLS_TLS_DHE_RSA_WITH_CHACHA20_POLY1305_SHA256,
+
     /* All AES-256 ephemeral suites */
     MBEDTLS_TLS_ECDHE_ECDSA_WITH_AES_256_GCM_SHA384,
     MBEDTLS_TLS_ECDHE_RSA_WITH_AES_256_GCM_SHA384,
@@ -127,6 +132,8 @@ static const int ciphersuite_preference[] =
     MBEDTLS_TLS_DHE_RSA_WITH_3DES_EDE_CBC_SHA,
 
     /* The PSK ephemeral suites */
+    MBEDTLS_TLS_ECDHE_PSK_WITH_CHACHA20_POLY1305_SHA256,
+    MBEDTLS_TLS_DHE_PSK_WITH_CHACHA20_POLY1305_SHA256,
     MBEDTLS_TLS_DHE_PSK_WITH_AES_256_GCM_SHA384,
     MBEDTLS_TLS_DHE_PSK_WITH_AES_256_CCM,
     MBEDTLS_TLS_ECDHE_PSK_WITH_AES_256_CBC_SHA384,
@@ -227,6 +234,7 @@ static const int ciphersuite_preference[] =
     MBEDTLS_TLS_ECDH_ECDSA_WITH_3DES_EDE_CBC_SHA,
 
     /* The RSA PSK suites */
+    MBEDTLS_TLS_RSA_PSK_WITH_CHACHA20_POLY1305_SHA256,
     MBEDTLS_TLS_RSA_PSK_WITH_AES_256_GCM_SHA384,
     MBEDTLS_TLS_RSA_PSK_WITH_AES_256_CBC_SHA384,
     MBEDTLS_TLS_RSA_PSK_WITH_AES_256_CBC_SHA,
@@ -246,6 +254,7 @@ static const int ciphersuite_preference[] =
     MBEDTLS_TLS_RSA_PSK_WITH_3DES_EDE_CBC_SHA,
 
     /* The PSK suites */
+    MBEDTLS_TLS_PSK_WITH_CHACHA20_POLY1305_SHA256,
     MBEDTLS_TLS_PSK_WITH_AES_256_GCM_SHA384,
     MBEDTLS_TLS_PSK_WITH_AES_256_CCM,
     MBEDTLS_TLS_PSK_WITH_AES_256_CBC_SHA384,
@@ -312,6 +321,75 @@ static const int ciphersuite_preference[] =
 
 static const mbedtls_ssl_ciphersuite_t ciphersuite_definitions[] =
 {
+#if defined(MBEDTLS_CHACHAPOLY_C) && \
+    defined(MBEDTLS_SHA256_C) && \
+    defined(MBEDTLS_SSL_PROTO_TLS1_2)
+#if defined(MBEDTLS_KEY_EXCHANGE_ECDHE_RSA_ENABLED)
+    { MBEDTLS_TLS_ECDHE_RSA_WITH_CHACHA20_POLY1305_SHA256,
+      "TLS-ECDHE-RSA-WITH-CHACHA20-POLY1305-SHA256",
+      MBEDTLS_CIPHER_CHACHA20_POLY1305, MBEDTLS_MD_SHA256,
+      MBEDTLS_KEY_EXCHANGE_ECDHE_RSA,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      0 },
+#endif
+#if defined(MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA_ENABLED)
+    { MBEDTLS_TLS_ECDHE_ECDSA_WITH_CHACHA20_POLY1305_SHA256,
+      "TLS-ECDHE-ECDSA-WITH-CHACHA20-POLY1305-SHA256",
+      MBEDTLS_CIPHER_CHACHA20_POLY1305, MBEDTLS_MD_SHA256,
+      MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      0 },
+#endif
+#if defined(MBEDTLS_KEY_EXCHANGE_DHE_RSA_ENABLED)
+    { MBEDTLS_TLS_DHE_RSA_WITH_CHACHA20_POLY1305_SHA256,
+      "TLS-DHE-RSA-WITH-CHACHA20-POLY1305-SHA256",
+      MBEDTLS_CIPHER_CHACHA20_POLY1305, MBEDTLS_MD_SHA256,
+      MBEDTLS_KEY_EXCHANGE_DHE_RSA,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      0 },
+#endif
+#if defined(MBEDTLS_KEY_EXCHANGE_PSK_ENABLED)
+    { MBEDTLS_TLS_PSK_WITH_CHACHA20_POLY1305_SHA256,
+      "TLS-PSK-WITH-CHACHA20-POLY1305-SHA256",
+      MBEDTLS_CIPHER_CHACHA20_POLY1305, MBEDTLS_MD_SHA256,
+      MBEDTLS_KEY_EXCHANGE_PSK,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      0 },
+#endif
+#if defined(MBEDTLS_KEY_EXCHANGE_ECDHE_PSK_ENABLED)
+    { MBEDTLS_TLS_ECDHE_PSK_WITH_CHACHA20_POLY1305_SHA256,
+      "TLS-ECDHE-PSK-WITH-CHACHA20-POLY1305-SHA256",
+      MBEDTLS_CIPHER_CHACHA20_POLY1305, MBEDTLS_MD_SHA256,
+      MBEDTLS_KEY_EXCHANGE_ECDHE_PSK,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      0 },
+#endif
+#if defined(MBEDTLS_KEY_EXCHANGE_DHE_PSK_ENABLED)
+    { MBEDTLS_TLS_DHE_PSK_WITH_CHACHA20_POLY1305_SHA256,
+      "TLS-DHE-PSK-WITH-CHACHA20-POLY1305-SHA256",
+      MBEDTLS_CIPHER_CHACHA20_POLY1305, MBEDTLS_MD_SHA256,
+      MBEDTLS_KEY_EXCHANGE_DHE_PSK,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      0 },
+#endif
+#if defined(MBEDTLS_KEY_EXCHANGE_RSA_PSK_ENABLED)
+    { MBEDTLS_TLS_RSA_PSK_WITH_CHACHA20_POLY1305_SHA256,
+      "TLS-RSA-PSK-WITH-CHACHA20-POLY1305-SHA256",
+      MBEDTLS_CIPHER_CHACHA20_POLY1305, MBEDTLS_MD_SHA256,
+      MBEDTLS_KEY_EXCHANGE_RSA_PSK,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      MBEDTLS_SSL_MAJOR_VERSION_3, MBEDTLS_SSL_MINOR_VERSION_3,
+      0 },
+#endif
+#endif /* MBEDTLS_CHACHAPOLY_C &&
+          MBEDTLS_SHA256_C &&
+          MBEDTLS_SSL_PROTO_TLS1_2 */
 #if defined(MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA_ENABLED)
 #if defined(MBEDTLS_AES_C)
 #if defined(MBEDTLS_SHA1_C)
@@ -2242,7 +2320,8 @@ mbedtls_pk_type_t mbedtls_ssl_get_ciphersuite_sig_alg( const mbedtls_ssl_ciphers
 
 #endif /* MBEDTLS_PK_C */
 
-#if defined(MBEDTLS_ECDH_C) || defined(MBEDTLS_ECDSA_C)
+#if defined(MBEDTLS_ECDH_C) || defined(MBEDTLS_ECDSA_C) || \
+    defined(MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED)
 int mbedtls_ssl_ciphersuite_uses_ec( const mbedtls_ssl_ciphersuite_t *info )
 {
     switch( info->key_exchange )
@@ -2252,13 +2331,14 @@ int mbedtls_ssl_ciphersuite_uses_ec( const mbedtls_ssl_ciphersuite_t *info )
         case MBEDTLS_KEY_EXCHANGE_ECDHE_PSK:
         case MBEDTLS_KEY_EXCHANGE_ECDH_RSA:
         case MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA:
+        case MBEDTLS_KEY_EXCHANGE_ECJPAKE:
             return( 1 );
 
         default:
             return( 0 );
     }
 }
-#endif /* MBEDTLS_ECDH_C || MBEDTLS_ECDSA_C */
+#endif /* MBEDTLS_ECDH_C || MBEDTLS_ECDSA_C || MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED*/
 
 #if defined(MBEDTLS_KEY_EXCHANGE__SOME__PSK_ENABLED)
 int mbedtls_ssl_ciphersuite_uses_psk( const mbedtls_ssl_ciphersuite_t *info )
diff --git a/library/ssl_cli.c b/library/ssl_cli.c
index b3a8ba93..fbf5166d 100644
--- a/library/ssl_cli.c
+++ b/library/ssl_cli.c
@@ -57,7 +57,7 @@ static void ssl_write_hostname_ext( mbedtls_ssl_context *ssl,
                                     size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     size_t hostname_len;
 
     *olen = 0;
@@ -127,7 +127,7 @@ static void ssl_write_renegotiation_ext( mbedtls_ssl_context *ssl,
                                          size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
 
     *olen = 0;
 
@@ -171,7 +171,7 @@ static void ssl_write_signature_algorithms_ext( mbedtls_ssl_context *ssl,
                                                 size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     size_t sig_alg_len = 0;
     const int *md;
 #if defined(MBEDTLS_RSA_C) || defined(MBEDTLS_ECDSA_C)
@@ -256,7 +256,7 @@ static void ssl_write_supported_elliptic_curves_ext( mbedtls_ssl_context *ssl,
                                                      size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     unsigned char *elliptic_curve_list = p + 6;
     size_t elliptic_curve_len = 0;
     const mbedtls_ecp_curve_info *info;
@@ -329,7 +329,7 @@ static void ssl_write_supported_point_formats_ext( mbedtls_ssl_context *ssl,
                                                    size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
 
     *olen = 0;
 
@@ -362,7 +362,7 @@ static void ssl_write_ecjpake_kkpp_ext( mbedtls_ssl_context *ssl,
 {
     int ret;
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     size_t kkpp_len;
 
     *olen = 0;
@@ -439,7 +439,7 @@ static void ssl_write_max_fragment_length_ext( mbedtls_ssl_context *ssl,
                                                size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
 
     *olen = 0;
 
@@ -472,7 +472,7 @@ static void ssl_write_truncated_hmac_ext( mbedtls_ssl_context *ssl,
                                           unsigned char *buf, size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
 
     *olen = 0;
 
@@ -504,7 +504,7 @@ static void ssl_write_encrypt_then_mac_ext( mbedtls_ssl_context *ssl,
                                        unsigned char *buf, size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
 
     *olen = 0;
 
@@ -538,7 +538,7 @@ static void ssl_write_extended_ms_ext( mbedtls_ssl_context *ssl,
                                        unsigned char *buf, size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
 
     *olen = 0;
 
@@ -572,7 +572,7 @@ static void ssl_write_session_ticket_ext( mbedtls_ssl_context *ssl,
                                           unsigned char *buf, size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     size_t tlen = ssl->session_negotiate->ticket_len;
 
     *olen = 0;
@@ -616,7 +616,7 @@ static void ssl_write_alpn_ext( mbedtls_ssl_context *ssl,
                                 unsigned char *buf, size_t *olen )
 {
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     size_t alpnlen = 0;
     const char **cur;
 
@@ -766,6 +766,10 @@ static int ssl_write_client_hello( mbedtls_ssl_context *ssl )
     unsigned char offer_compress;
     const int *ciphersuites;
     const mbedtls_ssl_ciphersuite_t *ciphersuite_info;
+#if defined(MBEDTLS_ECDH_C) || defined(MBEDTLS_ECDSA_C) || \
+    defined(MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED)
+    int uses_ec = 0;
+#endif
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> write client hello" ) );
 
@@ -917,6 +921,11 @@ static int ssl_write_client_hello( mbedtls_ssl_context *ssl )
         MBEDTLS_SSL_DEBUG_MSG( 3, ( "client hello, add ciphersuite: %04x",
                                     ciphersuites[i] ) );
 
+#if defined(MBEDTLS_ECDH_C) || defined(MBEDTLS_ECDSA_C) || \
+    defined(MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED)
+        uses_ec |= mbedtls_ssl_ciphersuite_uses_ec( ciphersuite_info );
+#endif
+
         n++;
         *p++ = (unsigned char)( ciphersuites[i] >> 8 );
         *p++ = (unsigned char)( ciphersuites[i]      );
@@ -1010,11 +1019,14 @@ static int ssl_write_client_hello( mbedtls_ssl_context *ssl )
 
 #if defined(MBEDTLS_ECDH_C) || defined(MBEDTLS_ECDSA_C) || \
     defined(MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED)
-    ssl_write_supported_elliptic_curves_ext( ssl, p + 2 + ext_len, &olen );
-    ext_len += olen;
+    if( uses_ec )
+    {
+        ssl_write_supported_elliptic_curves_ext( ssl, p + 2 + ext_len, &olen );
+        ext_len += olen;
 
-    ssl_write_supported_point_formats_ext( ssl, p + 2 + ext_len, &olen );
-    ext_len += olen;
+        ssl_write_supported_point_formats_ext( ssl, p + 2 + ext_len, &olen );
+        ext_len += olen;
+    }
 #endif
 
 #if defined(MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED)
@@ -1076,12 +1088,21 @@ static int ssl_write_client_hello( mbedtls_ssl_context *ssl )
         mbedtls_ssl_send_flight_completed( ssl );
 #endif
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
+        ( ret = mbedtls_ssl_flight_transmit( ssl ) ) != 0 )
+    {
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_flight_transmit", ret );
+        return( ret );
+    }
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= write client hello" ) );
 
     return( 0 );
@@ -1247,14 +1268,14 @@ static int ssl_parse_supported_point_formats_ext( mbedtls_ssl_context *ssl,
     size_t list_size;
     const unsigned char *p;
 
-    list_size = buf[0];
-    if( list_size + 1 != len )
+    if( len == 0 || (size_t)( buf[0] + 1 ) != len )
     {
         MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad server hello message" ) );
         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 );
     }
+    list_size = buf[0];
 
     p = buf + 1;
     while( list_size > 0 )
@@ -1479,7 +1500,7 @@ static int ssl_parse_server_hello( mbedtls_ssl_context *ssl )
 
     buf = ssl->in_msg;
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         /* No alert on a read error. */
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
@@ -2129,7 +2150,7 @@ static int ssl_write_encrypted_pms( mbedtls_ssl_context *ssl,
     size_t len_bytes = ssl->minor_ver == MBEDTLS_SSL_MINOR_VERSION_0 ? 0 : 2;
     unsigned char *p = ssl->handshake->premaster + pms_offset;
 
-    if( offset + len_bytes > MBEDTLS_SSL_MAX_CONTENT_LEN )
+    if( offset + len_bytes > MBEDTLS_SSL_OUT_CONTENT_LEN )
     {
         MBEDTLS_SSL_DEBUG_MSG( 1, ( "buffer too small for encrypted pms" ) );
         return( MBEDTLS_ERR_SSL_BUFFER_TOO_SMALL );
@@ -2172,7 +2193,7 @@ static int ssl_write_encrypted_pms( mbedtls_ssl_context *ssl,
     if( ( ret = mbedtls_pk_encrypt( &ssl->session_negotiate->peer_cert->pk,
                             p, ssl->handshake->pmslen,
                             ssl->out_msg + offset + len_bytes, olen,
-                            MBEDTLS_SSL_MAX_CONTENT_LEN - offset - len_bytes,
+                            MBEDTLS_SSL_OUT_CONTENT_LEN - offset - len_bytes,
                             ssl->conf->f_rng, ssl->conf->p_rng ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_rsa_pkcs1_encrypt", ret );
@@ -2348,7 +2369,7 @@ static int ssl_parse_server_key_exchange( mbedtls_ssl_context *ssl )
     }
 #endif
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
         return( ret );
@@ -2675,7 +2696,7 @@ static int ssl_parse_certificate_request( mbedtls_ssl_context *ssl )
         return( 0 );
     }
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
         return( ret );
@@ -2827,7 +2848,7 @@ static int ssl_parse_server_hello_done( mbedtls_ssl_context *ssl )
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> parse server hello done" ) );
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
         return( ret );
@@ -2995,7 +3016,7 @@ ecdh_calc_secret:
         i = 4;
         n = ssl->conf->psk_identity_len;
 
-        if( i + 2 + n > MBEDTLS_SSL_MAX_CONTENT_LEN )
+        if( i + 2 + n > MBEDTLS_SSL_OUT_CONTENT_LEN )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "psk identity too long or "
                                         "SSL buffer too short" ) );
@@ -3031,7 +3052,7 @@ ecdh_calc_secret:
              */
             n = ssl->handshake->dhm_ctx.len;
 
-            if( i + 2 + n > MBEDTLS_SSL_MAX_CONTENT_LEN )
+            if( i + 2 + n > MBEDTLS_SSL_OUT_CONTENT_LEN )
             {
                 MBEDTLS_SSL_DEBUG_MSG( 1, ( "psk identity or DHM size too long"
                                             " or SSL buffer too short" ) );
@@ -3060,7 +3081,7 @@ ecdh_calc_secret:
              * ClientECDiffieHellmanPublic public;
              */
             ret = mbedtls_ecdh_make_public( &ssl->handshake->ecdh_ctx, &n,
-                    &ssl->out_msg[i], MBEDTLS_SSL_MAX_CONTENT_LEN - i,
+                    &ssl->out_msg[i], MBEDTLS_SSL_OUT_CONTENT_LEN - i,
                     ssl->conf->f_rng, ssl->conf->p_rng );
             if( ret != 0 )
             {
@@ -3101,7 +3122,7 @@ ecdh_calc_secret:
         i = 4;
 
         ret = mbedtls_ecjpake_write_round_two( &ssl->handshake->ecjpake_ctx,
-                ssl->out_msg + i, MBEDTLS_SSL_MAX_CONTENT_LEN - i, &n,
+                ssl->out_msg + i, MBEDTLS_SSL_OUT_CONTENT_LEN - i, &n,
                 ssl->conf->f_rng, ssl->conf->p_rng );
         if( ret != 0 )
         {
@@ -3132,9 +3153,9 @@ ecdh_calc_secret:
 
     ssl->state++;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
@@ -3342,9 +3363,10 @@ sign:
     ssl->out_msg[0]  = MBEDTLS_SSL_HS_CERTIFICATE_VERIFY;
 
     ssl->state++;
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
@@ -3370,7 +3392,7 @@ static int ssl_parse_new_session_ticket( mbedtls_ssl_context *ssl )
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> parse new session ticket" ) );
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
         return( ret );
@@ -3405,8 +3427,8 @@ static int ssl_parse_new_session_ticket( mbedtls_ssl_context *ssl )
 
     msg = ssl->in_msg + mbedtls_ssl_hs_hdr_len( ssl );
 
-    lifetime = ( msg[0] << 24 ) | ( msg[1] << 16 ) |
-               ( msg[2] <<  8 ) | ( msg[3]       );
+    lifetime = ( ((uint32_t) msg[0]) << 24 ) | ( msg[1] << 16 ) |
+               ( msg[2] << 8 ) | ( msg[3] );
 
     ticket_len = ( msg[4] << 8 ) | ( msg[5] );
 
@@ -3484,10 +3506,10 @@ int mbedtls_ssl_handshake_client_step( mbedtls_ssl_context *ssl )
     if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
         ssl->handshake->retransmit_state == MBEDTLS_SSL_RETRANS_SENDING )
     {
-        if( ( ret = mbedtls_ssl_resend( ssl ) ) != 0 )
+        if( ( ret = mbedtls_ssl_flight_transmit( ssl ) ) != 0 )
             return( ret );
     }
-#endif
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
 
     /* Change state now, so that it is right in mbedtls_ssl_read_record(), used
      * by DTLS for dropping out-of-sequence ChangeCipherSpec records */
diff --git a/library/ssl_srv.c b/library/ssl_srv.c
index 0ccab588..36ca0d69 100644
--- a/library/ssl_srv.c
+++ b/library/ssl_srv.c
@@ -91,6 +91,13 @@ static int ssl_parse_servername_ext( mbedtls_ssl_context *ssl,
 
     MBEDTLS_SSL_DEBUG_MSG( 3, ( "parse ServerName extension" ) );
 
+    if( len < 2 )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad client hello message" ) );
+        mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
+                                       MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
+        return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_HELLO );
+    }
     servername_list_size = ( ( buf[0] << 8 ) | ( buf[1] ) );
     if( servername_list_size + 2 != len )
     {
@@ -101,7 +108,7 @@ static int ssl_parse_servername_ext( mbedtls_ssl_context *ssl,
     }
 
     p = buf + 2;
-    while( servername_list_size > 0 )
+    while( servername_list_size > 2 )
     {
         hostname_len = ( ( p[1] << 8 ) | p[2] );
         if( hostname_len + 3 > servername_list_size )
@@ -205,6 +212,12 @@ static int ssl_parse_signature_algorithms_ext( mbedtls_ssl_context *ssl,
     mbedtls_md_type_t md_cur;
     mbedtls_pk_type_t sig_cur;
 
+    if ( len < 2 ) {
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad client hello message" ) );
+        mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
+                                       MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
+        return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_HELLO );
+    }
     sig_alg_list_size = ( ( buf[0] << 8 ) | ( buf[1] ) );
     if( sig_alg_list_size + 2 != len ||
         sig_alg_list_size % 2 != 0 )
@@ -273,6 +286,12 @@ static int ssl_parse_supported_elliptic_curves( mbedtls_ssl_context *ssl,
     const unsigned char *p;
     const mbedtls_ecp_curve_info *curve_info, **curves;
 
+    if ( len < 2 ) {
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad client hello message" ) );
+        mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
+                                       MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
+        return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_HELLO );
+    }
     list_size = ( ( buf[0] << 8 ) | ( buf[1] ) );
     if( list_size + 2 != len ||
         list_size % 2 != 0 )
@@ -332,14 +351,14 @@ static int ssl_parse_supported_point_formats( mbedtls_ssl_context *ssl,
     size_t list_size;
     const unsigned char *p;
 
-    list_size = buf[0];
-    if( list_size + 1 != len )
+    if( len == 0 || (size_t)( buf[0] + 1 ) != len )
     {
         MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad client hello message" ) );
         mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
                                         MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
         return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_HELLO );
     }
+    list_size = buf[0];
 
     p = buf + 1;
     while( list_size > 0 )
@@ -1275,7 +1294,7 @@ read_record_header:
             return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_HELLO );
         }
 
-        memcpy( ssl->out_ctr + 2, ssl->in_ctr + 2, 6 );
+        memcpy( ssl->cur_out_ctr + 2, ssl->in_ctr + 2, 6 );
 
 #if defined(MBEDTLS_SSL_DTLS_ANTI_REPLAY)
         if( mbedtls_ssl_dtls_replay_check( ssl ) != 0 )
@@ -1303,7 +1322,7 @@ read_record_header:
     else
 #endif
     {
-        if( msg_len > MBEDTLS_SSL_MAX_CONTENT_LEN )
+        if( msg_len > MBEDTLS_SSL_IN_CONTENT_LEN )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad client hello message" ) );
             return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_HELLO );
@@ -1656,10 +1675,16 @@ read_record_header:
 
         while( ext_len != 0 )
         {
-            unsigned int ext_id   = ( ( ext[0] <<  8 )
-                                    | ( ext[1]       ) );
-            unsigned int ext_size = ( ( ext[2] <<  8 )
-                                    | ( ext[3]       ) );
+            unsigned int ext_id;
+            unsigned int ext_size;
+            if ( ext_len < 4 ) {
+                MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad client hello message" ) );
+                mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
+                                               MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
+                return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_HELLO );
+            }
+            ext_id   = ( ( ext[0] <<  8 ) | ( ext[1] ) );
+            ext_size = ( ( ext[2] <<  8 ) | ( ext[3] ) );
 
             if( ext_size + 4 > ext_len )
             {
@@ -2235,7 +2260,7 @@ static void ssl_write_ecjpake_kkpp_ext( mbedtls_ssl_context *ssl,
 {
     int ret;
     unsigned char *p = buf;
-    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char *end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     size_t kkpp_len;
 
     *olen = 0;
@@ -2342,7 +2367,7 @@ static int ssl_write_hello_verify_request( mbedtls_ssl_context *ssl )
     cookie_len_byte = p++;
 
     if( ( ret = ssl->conf->f_cookie_write( ssl->conf->p_cookie,
-                                     &p, ssl->out_buf + MBEDTLS_SSL_BUFFER_LEN,
+                                     &p, ssl->out_buf + MBEDTLS_SSL_OUT_BUFFER_LEN,
                                      ssl->cli_id, ssl->cli_id_len ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "f_cookie_write", ret );
@@ -2359,11 +2384,20 @@ static int ssl_write_hello_verify_request( mbedtls_ssl_context *ssl )
 
     ssl->state = MBEDTLS_SSL_SERVER_HELLO_VERIFY_REQUEST_SENT;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
+    {
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
+        return( ret );
+    }
+
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
+        ( ret = mbedtls_ssl_flight_transmit( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_flight_transmit", ret );
         return( ret );
     }
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= write hello verify request" ) );
 
@@ -2564,8 +2598,12 @@ static int ssl_write_server_hello( mbedtls_ssl_context *ssl )
 
 #if defined(MBEDTLS_ECDH_C) || defined(MBEDTLS_ECDSA_C) || \
     defined(MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED)
-    ssl_write_supported_point_formats_ext( ssl, p + 2 + ext_len, &olen );
-    ext_len += olen;
+    if ( mbedtls_ssl_ciphersuite_uses_ec(
+         mbedtls_ssl_ciphersuite_from_id( ssl->session_negotiate->ciphersuite ) ) )
+    {
+        ssl_write_supported_point_formats_ext( ssl, p + 2 + ext_len, &olen );
+        ext_len += olen;
+    }
 #endif
 
 #if defined(MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED)
@@ -2595,7 +2633,7 @@ static int ssl_write_server_hello( mbedtls_ssl_context *ssl )
     ssl->out_msgtype = MBEDTLS_SSL_MSG_HANDSHAKE;
     ssl->out_msg[0]  = MBEDTLS_SSL_HS_SERVER_HELLO;
 
-    ret = mbedtls_ssl_write_record( ssl );
+    ret = mbedtls_ssl_write_handshake_msg( ssl );
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= write server hello" ) );
 
@@ -2638,7 +2676,7 @@ static int ssl_write_certificate_request( mbedtls_ssl_context *ssl )
     size_t dn_size, total_dn_size; /* excluding length bytes */
     size_t ct_len, sa_len; /* including length bytes */
     unsigned char *buf, *p;
-    const unsigned char * const end = ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN;
+    const unsigned char * const end = ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN;
     const mbedtls_x509_crt *crt;
     int authmode;
 
@@ -2790,7 +2828,7 @@ static int ssl_write_certificate_request( mbedtls_ssl_context *ssl )
     ssl->out_msg[4 + ct_len + sa_len] = (unsigned char)( total_dn_size  >> 8 );
     ssl->out_msg[5 + ct_len + sa_len] = (unsigned char)( total_dn_size       );
 
-    ret = mbedtls_ssl_write_record( ssl );
+    ret = mbedtls_ssl_write_handshake_msg( ssl );
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= write certificate request" ) );
 
@@ -2839,7 +2877,7 @@ static int ssl_resume_server_key_exchange( mbedtls_ssl_context *ssl,
      * ssl_write_server_key_exchange also takes care of incrementing
      * ssl->out_msglen. */
     unsigned char *sig_start = ssl->out_msg + ssl->out_msglen + 2;
-    size_t sig_max_len = ( ssl->out_buf + MBEDTLS_SSL_MAX_CONTENT_LEN
+    size_t sig_max_len = ( ssl->out_buf + MBEDTLS_SSL_OUT_CONTENT_LEN
                            - sig_start );
     int ret = ssl->conf->f_async_resume( ssl,
                                          sig_start, signature_len, sig_max_len );
@@ -2893,7 +2931,7 @@ static int ssl_prepare_server_key_exchange( mbedtls_ssl_context *ssl,
         ret = mbedtls_ecjpake_write_round_two(
             &ssl->handshake->ecjpake_ctx,
             ssl->out_msg + ssl->out_msglen,
-            MBEDTLS_SSL_MAX_CONTENT_LEN - ssl->out_msglen, &len,
+            MBEDTLS_SSL_OUT_CONTENT_LEN - ssl->out_msglen, &len,
             ssl->conf->f_rng, ssl->conf->p_rng );
         if( ret != 0 )
         {
@@ -3020,7 +3058,7 @@ curve_matching_done:
         if( ( ret = mbedtls_ecdh_make_params(
                   &ssl->handshake->ecdh_ctx, &len,
                   ssl->out_msg + ssl->out_msglen,
-                  MBEDTLS_SSL_MAX_CONTENT_LEN - ssl->out_msglen,
+                  MBEDTLS_SSL_OUT_CONTENT_LEN - ssl->out_msglen,
                   ssl->conf->f_rng, ssl->conf->p_rng ) ) != 0 )
         {
             MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ecdh_make_params", ret );
@@ -3307,9 +3345,9 @@ static int ssl_write_server_key_exchange( mbedtls_ssl_context *ssl )
 
     ssl->state++;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
@@ -3334,12 +3372,21 @@ static int ssl_write_server_hello_done( mbedtls_ssl_context *ssl )
         mbedtls_ssl_send_flight_completed( ssl );
 #endif
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
+        ( ret = mbedtls_ssl_flight_transmit( ssl ) ) != 0 )
+    {
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_flight_transmit", ret );
+        return( ret );
+    }
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= write server hello done" ) );
 
     return( 0 );
@@ -3437,6 +3484,10 @@ static int ssl_decrypt_encrypted_pms( mbedtls_ssl_context *ssl,
     defined(MBEDTLS_SSL_PROTO_TLS1_2)
     if( ssl->minor_ver != MBEDTLS_SSL_MINOR_VERSION_0 )
     {
+        if ( p + 2 > end ) {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad client key exchange message" ) );
+            return( MBEDTLS_ERR_SSL_BAD_HS_CLIENT_KEY_EXCHANGE );
+        }
         if( *p++ != ( ( len >> 8 ) & 0xFF ) ||
             *p++ != ( ( len      ) & 0xFF ) )
         {
@@ -3677,7 +3728,7 @@ static int ssl_parse_client_key_exchange( mbedtls_ssl_context *ssl )
     }
     else
 #endif
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
         return( ret );
@@ -3987,25 +4038,10 @@ static int ssl_parse_certificate_verify( mbedtls_ssl_context *ssl )
     }
 
     /* Read the message without adding it to the checksum */
-    do {
-
-        do ret = mbedtls_ssl_read_record_layer( ssl );
-        while( ret == MBEDTLS_ERR_SSL_CONTINUE_PROCESSING );
-
-        if( ret != 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 ||
-             MBEDTLS_ERR_SSL_CONTINUE_PROCESSING == ret );
-
+    ret = mbedtls_ssl_read_record( ssl, 0 /* no checksum update */ );
     if( 0 != ret )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, ( "mbedtls_ssl_handle_message_type" ), ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, ( "mbedtls_ssl_read_record" ), ret );
         return( ret );
     }
 
@@ -4171,7 +4207,7 @@ static int ssl_write_new_session_ticket( mbedtls_ssl_context *ssl )
     if( ( ret = ssl->conf->f_ticket_write( ssl->conf->p_ticket,
                                 ssl->session_negotiate,
                                 ssl->out_msg + 10,
-                                ssl->out_msg + MBEDTLS_SSL_MAX_CONTENT_LEN,
+                                ssl->out_msg + MBEDTLS_SSL_OUT_CONTENT_LEN,
                                 &tlen, &lifetime ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_ticket_write", ret );
@@ -4194,9 +4230,9 @@ static int ssl_write_new_session_ticket( mbedtls_ssl_context *ssl )
      */
     ssl->handshake->new_session_ticket = 0;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
@@ -4225,10 +4261,10 @@ int mbedtls_ssl_handshake_server_step( mbedtls_ssl_context *ssl )
     if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
         ssl->handshake->retransmit_state == MBEDTLS_SSL_RETRANS_SENDING )
     {
-        if( ( ret = mbedtls_ssl_resend( ssl ) ) != 0 )
+        if( ( ret = mbedtls_ssl_flight_transmit( ssl ) ) != 0 )
             return( ret );
     }
-#endif
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
 
     switch( ssl->state )
     {
diff --git a/library/ssl_tls.c b/library/ssl_tls.c
index 6baf3e1e..5dffcf9c 100644
--- a/library/ssl_tls.c
+++ b/library/ssl_tls.c
@@ -54,6 +54,9 @@
 #include "mbedtls/oid.h"
 #endif
 
+static void ssl_reset_in_out_pointers( mbedtls_ssl_context *ssl );
+static uint32_t ssl_get_hs_total_len( mbedtls_ssl_context const *ssl );
+
 /* Length of the "epoch" field in the record header */
 static inline size_t ssl_ep_len( const mbedtls_ssl_context *ssl )
 {
@@ -96,7 +99,101 @@ static int ssl_check_timer( mbedtls_ssl_context *ssl )
     return( 0 );
 }
 
+static void ssl_update_out_pointers( mbedtls_ssl_context *ssl,
+                                     mbedtls_ssl_transform *transform );
+static void ssl_update_in_pointers( mbedtls_ssl_context *ssl,
+                                    mbedtls_ssl_transform *transform );
+
+#define SSL_DONT_FORCE_FLUSH 0
+#define SSL_FORCE_FLUSH      1
+
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
+
+/* Forward declarations for functions related to message buffering. */
+static void ssl_buffering_free( mbedtls_ssl_context *ssl );
+static void ssl_buffering_free_slot( mbedtls_ssl_context *ssl,
+                                     uint8_t slot );
+static void ssl_free_buffered_record( mbedtls_ssl_context *ssl );
+static int ssl_load_buffered_message( mbedtls_ssl_context *ssl );
+static int ssl_load_buffered_record( mbedtls_ssl_context *ssl );
+static int ssl_buffer_message( mbedtls_ssl_context *ssl );
+static int ssl_buffer_future_record( mbedtls_ssl_context *ssl );
+static int ssl_next_record_is_in_datagram( mbedtls_ssl_context *ssl );
+
+static size_t ssl_get_current_mtu( const mbedtls_ssl_context *ssl );
+static size_t ssl_get_maximum_datagram_size( mbedtls_ssl_context const *ssl )
+{
+    size_t mtu = ssl_get_current_mtu( ssl );
+
+    if( mtu != 0 && mtu < MBEDTLS_SSL_OUT_BUFFER_LEN )
+        return( mtu );
+
+    return( MBEDTLS_SSL_OUT_BUFFER_LEN );
+}
+
+static int ssl_get_remaining_space_in_datagram( mbedtls_ssl_context const *ssl )
+{
+    size_t const bytes_written = ssl->out_left;
+    size_t const mtu           = ssl_get_maximum_datagram_size( ssl );
+
+    /* Double-check that the write-index hasn't gone
+     * past what we can transmit in a single datagram. */
+    if( bytes_written > mtu )
+    {
+        /* Should never happen... */
+        return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+    }
+
+    return( (int) ( mtu - bytes_written ) );
+}
+
+static int ssl_get_remaining_payload_in_datagram( mbedtls_ssl_context const *ssl )
+{
+    int ret;
+    size_t remaining, expansion;
+    size_t max_len = MBEDTLS_SSL_MAX_CONTENT_LEN;
+
+#if defined(MBEDTLS_SSL_MAX_FRAGMENT_LENGTH)
+    const size_t mfl = mbedtls_ssl_get_max_frag_len( ssl );
+
+    if( max_len > mfl )
+        max_len = mfl;
+
+    /* By the standard (RFC 6066 Sect. 4), the MFL extension
+     * only limits the maximum record payload size, so in theory
+     * we would be allowed to pack multiple records of payload size
+     * MFL into a single datagram. However, this would mean that there's
+     * no way to explicitly communicate MTU restrictions to the peer.
+     *
+     * The following reduction of max_len makes sure that we never
+     * write datagrams larger than MFL + Record Expansion Overhead.
+     */
+    if( max_len <= ssl->out_left )
+        return( 0 );
+
+    max_len -= ssl->out_left;
+#endif
+
+    ret = ssl_get_remaining_space_in_datagram( ssl );
+    if( ret < 0 )
+        return( ret );
+    remaining = (size_t) ret;
+
+    ret = mbedtls_ssl_get_record_expansion( ssl );
+    if( ret < 0 )
+        return( ret );
+    expansion = (size_t) ret;
+
+    if( remaining <= expansion )
+        return( 0 );
+
+    remaining -= expansion;
+    if( remaining >= max_len )
+        remaining = max_len;
+
+    return( (int) remaining );
+}
+
 /*
  * Double the retransmit timeout value, within the allowed range,
  * returning -1 if the maximum value has already been reached.
@@ -108,6 +205,15 @@ static int ssl_double_retransmit_timeout( mbedtls_ssl_context *ssl )
     if( ssl->handshake->retransmit_timeout >= ssl->conf->hs_timeout_max )
         return( -1 );
 
+    /* Implement the final paragraph of RFC 6347 section 4.1.1.1
+     * in the following way: after the initial transmission and a first
+     * retransmission, back off to a temporary estimated MTU of 508 bytes.
+     * This value is guaranteed to be deliverable (if not guaranteed to be
+     * delivered) of any compliant IPv4 (and IPv6) network, and should work
+     * on most non-IP stacks too. */
+    if( ssl->handshake->retransmit_timeout != ssl->conf->hs_timeout_min )
+        ssl->handshake->mtu = 508;
+
     new_timeout = 2 * ssl->handshake->retransmit_timeout;
 
     /* Avoid arithmetic overflow and range overflow */
@@ -141,14 +247,24 @@ static void ssl_reset_retransmit_timeout( mbedtls_ssl_context *ssl )
  *    } MaxFragmentLength;
  * and we add 0 -> extension unused
  */
-static unsigned int mfl_code_to_length[MBEDTLS_SSL_MAX_FRAG_LEN_INVALID] =
+static unsigned int ssl_mfl_code_to_length( int mfl )
 {
-    MBEDTLS_SSL_MAX_CONTENT_LEN,    /* MBEDTLS_SSL_MAX_FRAG_LEN_NONE */
-    512,                    /* MBEDTLS_SSL_MAX_FRAG_LEN_512  */
-    1024,                   /* MBEDTLS_SSL_MAX_FRAG_LEN_1024 */
-    2048,                   /* MBEDTLS_SSL_MAX_FRAG_LEN_2048 */
-    4096,                   /* MBEDTLS_SSL_MAX_FRAG_LEN_4096 */
-};
+    switch( mfl )
+    {
+    case MBEDTLS_SSL_MAX_FRAG_LEN_NONE:
+        return ( MBEDTLS_TLS_EXT_ADV_CONTENT_LEN );
+    case MBEDTLS_SSL_MAX_FRAG_LEN_512:
+        return 512;
+    case MBEDTLS_SSL_MAX_FRAG_LEN_1024:
+        return 1024;
+    case MBEDTLS_SSL_MAX_FRAG_LEN_2048:
+        return 2048;
+    case MBEDTLS_SSL_MAX_FRAG_LEN_4096:
+        return 4096;
+    default:
+        return ( MBEDTLS_TLS_EXT_ADV_CONTENT_LEN );
+    }
+}
 #endif /* MBEDTLS_SSL_MAX_FRAGMENT_LENGTH */
 
 #if defined(MBEDTLS_SSL_CLI_C)
@@ -688,18 +804,32 @@ int mbedtls_ssl_derive_keys( mbedtls_ssl_context *ssl )
     transform->keylen = cipher_info->key_bitlen / 8;
 
     if( cipher_info->mode == MBEDTLS_MODE_GCM ||
-        cipher_info->mode == MBEDTLS_MODE_CCM )
+        cipher_info->mode == MBEDTLS_MODE_CCM ||
+        cipher_info->mode == MBEDTLS_MODE_CHACHAPOLY )
     {
+        size_t taglen, explicit_ivlen;
+
         transform->maclen = 0;
         mac_key_len = 0;
 
+        /* All modes haves 96-bit IVs;
+         * GCM and CCM has 4 implicit and 8 explicit bytes
+         * ChachaPoly has all 12 bytes implicit
+         */
         transform->ivlen = 12;
-        transform->fixed_ivlen = 4;
+        if( cipher_info->mode == MBEDTLS_MODE_CHACHAPOLY )
+            transform->fixed_ivlen = 12;
+        else
+            transform->fixed_ivlen = 4;
+
+        /* All modes have 128-bit tags, except CCM_8 (ciphersuite flag) */
+        taglen = transform->ciphersuite_info->flags &
+                  MBEDTLS_CIPHERSUITE_SHORT_TAG ? 8 : 16;
 
-        /* Minimum length is expicit IV + tag */
-        transform->minlen = transform->ivlen - transform->fixed_ivlen
-                            + ( transform->ciphersuite_info->flags &
-                                MBEDTLS_CIPHERSUITE_SHORT_TAG ? 8 : 16 );
+
+        /* Minimum length of encrypted record */
+        explicit_ivlen = transform->ivlen - transform->fixed_ivlen;
+        transform->minlen = explicit_ivlen + taglen;
     }
     else
     {
@@ -956,11 +1086,11 @@ int mbedtls_ssl_derive_keys( mbedtls_ssl_context *ssl )
         if( ssl->compress_buf == NULL )
         {
             MBEDTLS_SSL_DEBUG_MSG( 3, ( "Allocating compression buffer" ) );
-            ssl->compress_buf = mbedtls_calloc( 1, MBEDTLS_SSL_BUFFER_LEN );
+            ssl->compress_buf = mbedtls_calloc( 1, MBEDTLS_SSL_COMPRESS_BUFFER_LEN );
             if( ssl->compress_buf == NULL )
             {
                 MBEDTLS_SSL_DEBUG_MSG( 1, ( "alloc(%d bytes) failed",
-                                    MBEDTLS_SSL_BUFFER_LEN ) );
+                                    MBEDTLS_SSL_COMPRESS_BUFFER_LEN ) );
                 return( MBEDTLS_ERR_SSL_ALLOC_FAILED );
             }
         }
@@ -1151,6 +1281,9 @@ int mbedtls_ssl_psk_derive_premaster( mbedtls_ssl_context *ssl, mbedtls_key_exch
          * other_secret already set by the ClientKeyExchange message,
          * and is 48 bytes long
          */
+        if( end - p < 2 )
+            return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
+
         *p++ = 0;
         *p++ = 48;
         p += 48;
@@ -1276,6 +1409,27 @@ static void ssl_mac( mbedtls_md_context_t *md_ctx,
 #define SSL_SOME_MODES_USE_MAC
 #endif
 
+/* The function below is only used in the Lucky 13 counter-measure in
+ * ssl_decrypt_buf(). These are the defines that guard the call site. */
+#if defined(SSL_SOME_MODES_USE_MAC) && \
+    ( defined(MBEDTLS_SSL_PROTO_TLS1) || \
+      defined(MBEDTLS_SSL_PROTO_TLS1_1) || \
+      defined(MBEDTLS_SSL_PROTO_TLS1_2) )
+/* This function makes sure every byte in the memory region is accessed
+ * (in ascending addresses order) */
+static void ssl_read_memory( unsigned char *p, size_t len )
+{
+    unsigned char acc = 0;
+    volatile unsigned char force;
+
+    for( ; len != 0; p++, len-- )
+        acc ^= *p;
+
+    force = acc;
+    (void) force;
+}
+#endif /* SSL_SOME_MODES_USE_MAC && ( TLS1 || TLS1_1 || TLS1_2 ) */
+
 /*
  * Encryption/decryption functions
  */
@@ -1297,14 +1451,6 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
     MBEDTLS_SSL_DEBUG_BUF( 4, "before encrypt: output payload",
                       ssl->out_msg, ssl->out_msglen );
 
-    if( ssl->out_msglen > MBEDTLS_SSL_MAX_CONTENT_LEN )
-    {
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "Record content %u too large, maximum %d",
-                                    (unsigned) ssl->out_msglen,
-                                    MBEDTLS_SSL_MAX_CONTENT_LEN ) );
-        return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
-    }
-
     /*
      * Add MAC before if needed
      */
@@ -1394,17 +1540,26 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
     }
     else
 #endif /* MBEDTLS_ARC4_C || MBEDTLS_CIPHER_NULL_CIPHER */
-#if defined(MBEDTLS_GCM_C) || defined(MBEDTLS_CCM_C)
+#if defined(MBEDTLS_GCM_C) || \
+    defined(MBEDTLS_CCM_C) || \
+    defined(MBEDTLS_CHACHAPOLY_C)
     if( mode == MBEDTLS_MODE_GCM ||
-        mode == MBEDTLS_MODE_CCM )
+        mode == MBEDTLS_MODE_CCM ||
+        mode == MBEDTLS_MODE_CHACHAPOLY )
     {
         int ret;
         size_t enc_msglen, olen;
         unsigned char *enc_msg;
         unsigned char add_data[13];
-        unsigned char taglen = ssl->transform_out->ciphersuite_info->flags &
+        unsigned char iv[12];
+        mbedtls_ssl_transform *transform = ssl->transform_out;
+        unsigned char taglen = transform->ciphersuite_info->flags &
                                MBEDTLS_CIPHERSUITE_SHORT_TAG ? 8 : 16;
+        size_t explicit_ivlen = transform->ivlen - transform->fixed_ivlen;
 
+        /*
+         * Prepare additional authenticated data
+         */
         memcpy( add_data, ssl->out_ctr, 8 );
         add_data[8]  = ssl->out_msgtype;
         mbedtls_ssl_write_version( ssl->major_ver, ssl->minor_ver,
@@ -1412,44 +1567,57 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
         add_data[11] = ( ssl->out_msglen >> 8 ) & 0xFF;
         add_data[12] = ssl->out_msglen & 0xFF;
 
-        MBEDTLS_SSL_DEBUG_BUF( 4, "additional data used for AEAD",
-                       add_data, 13 );
+        MBEDTLS_SSL_DEBUG_BUF( 4, "additional data for AEAD", add_data, 13 );
 
         /*
          * Generate IV
          */
-        if( ssl->transform_out->ivlen - ssl->transform_out->fixed_ivlen != 8 )
+        if( transform->ivlen == 12 && transform->fixed_ivlen == 4 )
+        {
+            /* GCM and CCM: fixed || explicit (=seqnum) */
+            memcpy( iv, transform->iv_enc, transform->fixed_ivlen );
+            memcpy( iv + transform->fixed_ivlen, ssl->out_ctr, 8 );
+            memcpy( ssl->out_iv, ssl->out_ctr, 8 );
+
+        }
+        else if( transform->ivlen == 12 && transform->fixed_ivlen == 12 )
+        {
+            /* ChachaPoly: fixed XOR sequence number */
+            unsigned char i;
+
+            memcpy( iv, transform->iv_enc, transform->fixed_ivlen );
+
+            for( i = 0; i < 8; i++ )
+                iv[i+4] ^= ssl->out_ctr[i];
+        }
+        else
         {
             /* Reminder if we ever add an AEAD mode with a different size */
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
             return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
         }
 
-        memcpy( ssl->transform_out->iv_enc + ssl->transform_out->fixed_ivlen,
-                             ssl->out_ctr, 8 );
-        memcpy( ssl->out_iv, ssl->out_ctr, 8 );
-
-        MBEDTLS_SSL_DEBUG_BUF( 4, "IV used", ssl->out_iv,
-                ssl->transform_out->ivlen - ssl->transform_out->fixed_ivlen );
+        MBEDTLS_SSL_DEBUG_BUF( 4, "IV used (internal)",
+                                  iv, transform->ivlen );
+        MBEDTLS_SSL_DEBUG_BUF( 4, "IV used (transmitted)",
+                                  ssl->out_iv, explicit_ivlen );
 
         /*
-         * Fix pointer positions and message length with added IV
+         * Fix message length with added IV
          */
         enc_msg = ssl->out_msg;
         enc_msglen = ssl->out_msglen;
-        ssl->out_msglen += ssl->transform_out->ivlen -
-                           ssl->transform_out->fixed_ivlen;
+        ssl->out_msglen += explicit_ivlen;
 
         MBEDTLS_SSL_DEBUG_MSG( 3, ( "before encrypt: msglen = %d, "
-                            "including %d bytes of padding",
-                       ssl->out_msglen, 0 ) );
+                                    "including 0 bytes of padding",
+                                    ssl->out_msglen ) );
 
         /*
          * Encrypt and authenticate
          */
-        if( ( ret = mbedtls_cipher_auth_encrypt( &ssl->transform_out->cipher_ctx_enc,
-                                         ssl->transform_out->iv_enc,
-                                         ssl->transform_out->ivlen,
+        if( ( ret = mbedtls_cipher_auth_encrypt( &transform->cipher_ctx_enc,
+                                         iv, transform->ivlen,
                                          add_data, 13,
                                          enc_msg, enc_msglen,
                                          enc_msg, &olen,
@@ -1556,6 +1724,8 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
 #if defined(MBEDTLS_SSL_ENCRYPT_THEN_MAC)
         if( auth_done == 0 )
         {
+            unsigned char mac[MBEDTLS_SSL_MAC_ADD];
+
             /*
              * MAC(MAC_write_key, seq_num +
              *     TLSCipherText.type +
@@ -1578,10 +1748,12 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
             mbedtls_md_hmac_update( &ssl->transform_out->md_ctx_enc, pseudo_hdr, 13 );
             mbedtls_md_hmac_update( &ssl->transform_out->md_ctx_enc,
                              ssl->out_iv, ssl->out_msglen );
-            mbedtls_md_hmac_finish( &ssl->transform_out->md_ctx_enc,
-                             ssl->out_iv + ssl->out_msglen );
+            mbedtls_md_hmac_finish( &ssl->transform_out->md_ctx_enc, mac );
             mbedtls_md_hmac_reset( &ssl->transform_out->md_ctx_enc );
 
+            memcpy( ssl->out_iv + ssl->out_msglen, mac,
+                    ssl->transform_out->maclen );
+
             ssl->out_msglen += ssl->transform_out->maclen;
             auth_done++;
         }
@@ -1609,7 +1781,6 @@ static int ssl_encrypt_buf( mbedtls_ssl_context *ssl )
 
 static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
 {
-    size_t i;
     mbedtls_cipher_mode_t mode;
     int auth_done = 0;
 #if defined(SSL_SOME_MODES_USE_MAC)
@@ -1659,20 +1830,27 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
     }
     else
 #endif /* MBEDTLS_ARC4_C || MBEDTLS_CIPHER_NULL_CIPHER */
-#if defined(MBEDTLS_GCM_C) || defined(MBEDTLS_CCM_C)
+#if defined(MBEDTLS_GCM_C) || \
+    defined(MBEDTLS_CCM_C) || \
+    defined(MBEDTLS_CHACHAPOLY_C)
     if( mode == MBEDTLS_MODE_GCM ||
-        mode == MBEDTLS_MODE_CCM )
+        mode == MBEDTLS_MODE_CCM ||
+        mode == MBEDTLS_MODE_CHACHAPOLY )
     {
         int ret;
         size_t dec_msglen, olen;
         unsigned char *dec_msg;
         unsigned char *dec_msg_result;
         unsigned char add_data[13];
-        unsigned char taglen = ssl->transform_in->ciphersuite_info->flags &
+        unsigned char iv[12];
+        mbedtls_ssl_transform *transform = ssl->transform_in;
+        unsigned char taglen = transform->ciphersuite_info->flags &
                                MBEDTLS_CIPHERSUITE_SHORT_TAG ? 8 : 16;
-        size_t explicit_iv_len = ssl->transform_in->ivlen -
-                                 ssl->transform_in->fixed_ivlen;
+        size_t explicit_iv_len = transform->ivlen - transform->fixed_ivlen;
 
+        /*
+         * Compute and update sizes
+         */
         if( ssl->in_msglen < explicit_iv_len + taglen )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "msglen (%d) < explicit_iv_len (%d) "
@@ -1686,6 +1864,9 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
         dec_msg_result = ssl->in_msg;
         ssl->in_msglen = dec_msglen;
 
+        /*
+         * Prepare additional authenticated data
+         */
         memcpy( add_data, ssl->in_ctr, 8 );
         add_data[8]  = ssl->in_msgtype;
         mbedtls_ssl_write_version( ssl->major_ver, ssl->minor_ver,
@@ -1693,23 +1874,43 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
         add_data[11] = ( ssl->in_msglen >> 8 ) & 0xFF;
         add_data[12] = ssl->in_msglen & 0xFF;
 
-        MBEDTLS_SSL_DEBUG_BUF( 4, "additional data used for AEAD",
-                       add_data, 13 );
+        MBEDTLS_SSL_DEBUG_BUF( 4, "additional data for AEAD", add_data, 13 );
+
+        /*
+         * Prepare IV
+         */
+        if( transform->ivlen == 12 && transform->fixed_ivlen == 4 )
+        {
+            /* GCM and CCM: fixed || explicit (transmitted) */
+            memcpy( iv, transform->iv_dec, transform->fixed_ivlen );
+            memcpy( iv + transform->fixed_ivlen, ssl->in_iv, 8 );
+
+        }
+        else if( transform->ivlen == 12 && transform->fixed_ivlen == 12 )
+        {
+            /* ChachaPoly: fixed XOR sequence number */
+            unsigned char i;
 
-        memcpy( ssl->transform_in->iv_dec + ssl->transform_in->fixed_ivlen,
-                ssl->in_iv,
-                ssl->transform_in->ivlen - ssl->transform_in->fixed_ivlen );
+            memcpy( iv, transform->iv_dec, transform->fixed_ivlen );
 
-        MBEDTLS_SSL_DEBUG_BUF( 4, "IV used", ssl->transform_in->iv_dec,
-                                     ssl->transform_in->ivlen );
+            for( i = 0; i < 8; i++ )
+                iv[i+4] ^= ssl->in_ctr[i];
+        }
+        else
+        {
+            /* Reminder if we ever add an AEAD mode with a different size */
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+            return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+        }
+
+        MBEDTLS_SSL_DEBUG_BUF( 4, "IV used", iv, transform->ivlen );
         MBEDTLS_SSL_DEBUG_BUF( 4, "TAG used", dec_msg + dec_msglen, taglen );
 
         /*
          * Decrypt and authenticate
          */
         if( ( ret = mbedtls_cipher_auth_decrypt( &ssl->transform_in->cipher_ctx_dec,
-                                         ssl->transform_in->iv_dec,
-                                         ssl->transform_in->ivlen,
+                                         iv, transform->ivlen,
                                          add_data, 13,
                                          dec_msg, dec_msglen,
                                          dec_msg_result, &olen,
@@ -1827,6 +2028,7 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
          */
         if( ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_2 )
         {
+            unsigned char i;
             dec_msglen -= ssl->transform_in->ivlen;
             ssl->in_msglen -= ssl->transform_in->ivlen;
 
@@ -1900,27 +2102,28 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
              * and fake check up to 256 bytes of padding
              */
             size_t pad_count = 0, real_count = 1;
-            size_t padding_idx = ssl->in_msglen - padlen - 1;
+            size_t padding_idx = ssl->in_msglen - padlen;
+            size_t i;
 
             /*
              * Padding is guaranteed to be incorrect if:
-             *   1. padlen >= ssl->in_msglen
+             *   1. padlen > ssl->in_msglen
              *
-             *   2. padding_idx >= MBEDTLS_SSL_MAX_CONTENT_LEN +
+             *   2. padding_idx > MBEDTLS_SSL_IN_CONTENT_LEN +
              *                     ssl->transform_in->maclen
              *
              * In both cases we reset padding_idx to a safe value (0) to
              * prevent out-of-buffer reads.
              */
-            correct &= ( ssl->in_msglen >= padlen + 1 );
-            correct &= ( padding_idx < MBEDTLS_SSL_MAX_CONTENT_LEN +
+            correct &= ( padlen <= ssl->in_msglen );
+            correct &= ( padding_idx <= MBEDTLS_SSL_IN_CONTENT_LEN +
                                        ssl->transform_in->maclen );
 
             padding_idx *= correct;
 
-            for( i = 1; i <= 256; i++ )
+            for( i = 0; i < 256; i++ )
             {
-                real_count &= ( i <= padlen );
+                real_count &= ( i < padlen );
                 pad_count += real_count *
                              ( ssl->in_msg[padding_idx + i] == padlen - 1 );
             }
@@ -1951,8 +2154,10 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
         return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
     }
 
+#if defined(MBEDTLS_SSL_DEBUG_ALL)
     MBEDTLS_SSL_DEBUG_BUF( 4, "raw buffer after decryption",
                    ssl->in_msg, ssl->in_msglen );
+#endif
 
     /*
      * Authenticate if not done yet.
@@ -1985,20 +2190,69 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
         {
             /*
              * Process MAC and always update for padlen afterwards to make
-             * total time independent of padlen
-             *
-             * extra_run compensates MAC check for padlen
+             * total time independent of padlen.
              *
              * Known timing attacks:
              *  - Lucky Thirteen (http://www.isg.rhul.ac.uk/tls/TLStiming.pdf)
              *
-             * We use ( ( Lx + 8 ) / 64 ) to handle 'negative Lx' values
-             * correctly. (We round down instead of up, so -56 is the correct
-             * value for our calculations instead of -55)
+             * To compensate for different timings for the MAC calculation
+             * depending on how much padding was removed (which is determined
+             * by padlen), process extra_run more blocks through the hash
+             * function.
+             *
+             * The formula in the paper is
+             *   extra_run = ceil( (L1-55) / 64 ) - ceil( (L2-55) / 64 )
+             * where L1 is the size of the header plus the decrypted message
+             * plus CBC padding and L2 is the size of the header plus the
+             * decrypted message. This is for an underlying hash function
+             * with 64-byte blocks.
+             * We use ( (Lx+8) / 64 ) to handle 'negative Lx' values
+             * correctly. We round down instead of up, so -56 is the correct
+             * value for our calculations instead of -55.
+             *
+             * Repeat the formula rather than defining a block_size variable.
+             * This avoids requiring division by a variable at runtime
+             * (which would be marginally less efficient and would require
+             * linking an extra division function in some builds).
              */
             size_t j, extra_run = 0;
-            extra_run = ( 13 + ssl->in_msglen + padlen + 8 ) / 64 -
-                        ( 13 + ssl->in_msglen          + 8 ) / 64;
+
+            /*
+             * The next two sizes are the minimum and maximum values of
+             * in_msglen over all padlen values.
+             *
+             * They're independent of padlen, since we previously did
+             * in_msglen -= padlen.
+             *
+             * Note that max_len + maclen is never more than the buffer
+             * length, as we previously did in_msglen -= maclen too.
+             */
+            const size_t max_len = ssl->in_msglen + padlen;
+            const size_t min_len = ( max_len > 256 ) ? max_len - 256 : 0;
+
+            switch( ssl->transform_in->ciphersuite_info->mac )
+            {
+#if defined(MBEDTLS_MD5_C) || defined(MBEDTLS_SHA1_C) || \
+    defined(MBEDTLS_SHA256_C)
+                case MBEDTLS_MD_MD5:
+                case MBEDTLS_MD_SHA1:
+                case MBEDTLS_MD_SHA256:
+                    /* 8 bytes of message size, 64-byte compression blocks */
+                    extra_run = ( 13 + ssl->in_msglen + padlen + 8 ) / 64 -
+                                ( 13 + ssl->in_msglen          + 8 ) / 64;
+                    break;
+#endif
+#if defined(MBEDTLS_SHA512_C)
+                case MBEDTLS_MD_SHA384:
+                    /* 16 bytes of message size, 128-byte compression blocks */
+                    extra_run = ( 13 + ssl->in_msglen + padlen + 16 ) / 128 -
+                                ( 13 + ssl->in_msglen          + 16 ) / 128;
+                    break;
+#endif
+                default:
+                    MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+                    return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+            }
 
             extra_run &= correct * 0xFF;
 
@@ -2007,12 +2261,25 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
             mbedtls_md_hmac_update( &ssl->transform_in->md_ctx_dec, ssl->in_len, 2 );
             mbedtls_md_hmac_update( &ssl->transform_in->md_ctx_dec, ssl->in_msg,
                              ssl->in_msglen );
+            /* Make sure we access everything even when padlen > 0. This
+             * makes the synchronisation requirements for just-in-time
+             * Prime+Probe attacks much tighter and hopefully impractical. */
+            ssl_read_memory( ssl->in_msg + ssl->in_msglen, padlen );
             mbedtls_md_hmac_finish( &ssl->transform_in->md_ctx_dec, mac_expect );
-            /* Call mbedtls_md_process at least once due to cache attacks */
+
+            /* Call mbedtls_md_process at least once due to cache attacks
+             * that observe whether md_process() was called of not */
             for( j = 0; j < extra_run + 1; j++ )
                 mbedtls_md_process( &ssl->transform_in->md_ctx_dec, ssl->in_msg );
 
             mbedtls_md_hmac_reset( &ssl->transform_in->md_ctx_dec );
+
+            /* Make sure we access all the memory that could contain the MAC,
+             * before we check it in the next code block. This makes the
+             * synchronisation requirements for just-in-time Prime+Probe
+             * attacks much tighter and hopefully impractical. */
+            ssl_read_memory( ssl->in_msg + min_len,
+                                 max_len - min_len + ssl->transform_in->maclen );
         }
         else
 #endif /* MBEDTLS_SSL_PROTO_TLS1 || MBEDTLS_SSL_PROTO_TLS1_1 || \
@@ -2022,9 +2289,11 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
             return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
         }
 
+#if defined(MBEDTLS_SSL_DEBUG_ALL)
         MBEDTLS_SSL_DEBUG_BUF( 4, "expected mac", mac_expect, ssl->transform_in->maclen );
         MBEDTLS_SSL_DEBUG_BUF( 4, "message  mac", ssl->in_msg + ssl->in_msglen,
                                ssl->transform_in->maclen );
+#endif
 
         if( mbedtls_ssl_safer_memcmp( ssl->in_msg + ssl->in_msglen, mac_expect,
                                       ssl->transform_in->maclen ) != 0 )
@@ -2053,6 +2322,16 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
 
     if( ssl->in_msglen == 0 )
     {
+#if defined(MBEDTLS_SSL_PROTO_TLS1_2)
+        if( ssl->minor_ver == MBEDTLS_SSL_MINOR_VERSION_3
+            && ssl->in_msgtype != MBEDTLS_SSL_MSG_APPLICATION_DATA )
+        {
+            /* TLS v1.2 explicitly disallows zero-length messages which are not application data */
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "invalid zero-length message type: %d", ssl->in_msgtype ) );
+            return( MBEDTLS_ERR_SSL_INVALID_RECORD );
+        }
+#endif /* MBEDTLS_SSL_PROTO_TLS1_2 */
+
         ssl->nb_zero++;
 
         /*
@@ -2077,6 +2356,7 @@ static int ssl_decrypt_buf( mbedtls_ssl_context *ssl )
     else
 #endif
     {
+        unsigned char i;
         for( i = 8; i > ssl_ep_len( ssl ); i-- )
             if( ++ssl->in_ctr[i - 1] != 0 )
                 break;
@@ -2126,7 +2406,7 @@ static int ssl_compress_buf( mbedtls_ssl_context *ssl )
     ssl->transform_out->ctx_deflate.next_in = msg_pre;
     ssl->transform_out->ctx_deflate.avail_in = len_pre;
     ssl->transform_out->ctx_deflate.next_out = msg_post;
-    ssl->transform_out->ctx_deflate.avail_out = MBEDTLS_SSL_BUFFER_LEN - bytes_written;
+    ssl->transform_out->ctx_deflate.avail_out = MBEDTLS_SSL_OUT_BUFFER_LEN - bytes_written;
 
     ret = deflate( &ssl->transform_out->ctx_deflate, Z_SYNC_FLUSH );
     if( ret != Z_OK )
@@ -2135,7 +2415,7 @@ static int ssl_compress_buf( mbedtls_ssl_context *ssl )
         return( MBEDTLS_ERR_SSL_COMPRESSION_FAILED );
     }
 
-    ssl->out_msglen = MBEDTLS_SSL_BUFFER_LEN -
+    ssl->out_msglen = MBEDTLS_SSL_OUT_BUFFER_LEN -
                       ssl->transform_out->ctx_deflate.avail_out - bytes_written;
 
     MBEDTLS_SSL_DEBUG_MSG( 3, ( "after compression: msglen = %d, ",
@@ -2173,7 +2453,7 @@ static int ssl_decompress_buf( mbedtls_ssl_context *ssl )
     ssl->transform_in->ctx_inflate.next_in = msg_pre;
     ssl->transform_in->ctx_inflate.avail_in = len_pre;
     ssl->transform_in->ctx_inflate.next_out = msg_post;
-    ssl->transform_in->ctx_inflate.avail_out = MBEDTLS_SSL_BUFFER_LEN -
+    ssl->transform_in->ctx_inflate.avail_out = MBEDTLS_SSL_IN_BUFFER_LEN -
                                                header_bytes;
 
     ret = inflate( &ssl->transform_in->ctx_inflate, Z_SYNC_FLUSH );
@@ -2183,7 +2463,7 @@ static int ssl_decompress_buf( mbedtls_ssl_context *ssl )
         return( MBEDTLS_ERR_SSL_COMPRESSION_FAILED );
     }
 
-    ssl->in_msglen = MBEDTLS_SSL_BUFFER_LEN -
+    ssl->in_msglen = MBEDTLS_SSL_IN_BUFFER_LEN -
                      ssl->transform_in->ctx_inflate.avail_out - header_bytes;
 
     MBEDTLS_SSL_DEBUG_MSG( 3, ( "after decompression: msglen = %d, ",
@@ -2258,7 +2538,7 @@ int mbedtls_ssl_fetch_input( mbedtls_ssl_context *ssl, size_t nb_want )
         return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
     }
 
-    if( nb_want > MBEDTLS_SSL_BUFFER_LEN - (size_t)( ssl->in_hdr - ssl->in_buf ) )
+    if( nb_want > MBEDTLS_SSL_IN_BUFFER_LEN - (size_t)( ssl->in_hdr - ssl->in_buf ) )
     {
         MBEDTLS_SSL_DEBUG_MSG( 1, ( "requesting more data than fits" ) );
         return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
@@ -2344,7 +2624,7 @@ int mbedtls_ssl_fetch_input( mbedtls_ssl_context *ssl, size_t nb_want )
         }
         else
         {
-            len = MBEDTLS_SSL_BUFFER_LEN - ( ssl->in_hdr - ssl->in_buf );
+            len = MBEDTLS_SSL_IN_BUFFER_LEN - ( ssl->in_hdr - ssl->in_buf );
 
             if( ssl->state != MBEDTLS_SSL_HANDSHAKE_OVER )
                 timeout = ssl->handshake->retransmit_timeout;
@@ -2466,7 +2746,7 @@ int mbedtls_ssl_fetch_input( mbedtls_ssl_context *ssl, size_t nb_want )
 int mbedtls_ssl_flush_output( mbedtls_ssl_context *ssl )
 {
     int ret;
-    unsigned char *buf, i;
+    unsigned char *buf;
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> flush output" ) );
 
@@ -2489,8 +2769,7 @@ int mbedtls_ssl_flush_output( mbedtls_ssl_context *ssl )
         MBEDTLS_SSL_DEBUG_MSG( 2, ( "message length: %d, out_left: %d",
                        mbedtls_ssl_hdr_len( ssl ) + ssl->out_msglen, ssl->out_left ) );
 
-        buf = ssl->out_hdr + mbedtls_ssl_hdr_len( ssl ) +
-              ssl->out_msglen - ssl->out_left;
+        buf = ssl->out_hdr - ssl->out_left;
         ret = ssl->f_send( ssl->p_bio, buf, ssl->out_left );
 
         MBEDTLS_SSL_DEBUG_RET( 2, "ssl->f_send", ret );
@@ -2509,16 +2788,17 @@ int mbedtls_ssl_flush_output( mbedtls_ssl_context *ssl )
         ssl->out_left -= ret;
     }
 
-    for( i = 8; i > ssl_ep_len( ssl ); i-- )
-        if( ++ssl->out_ctr[i - 1] != 0 )
-            break;
-
-    /* The loop goes to its end iff the counter is wrapping */
-    if( i == ssl_ep_len( ssl ) )
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        ssl->out_hdr = ssl->out_buf;
+    }
+    else
+#endif
     {
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "outgoing message counter would wrap" ) );
-        return( MBEDTLS_ERR_SSL_COUNTER_WRAPPING );
+        ssl->out_hdr = ssl->out_buf + 8;
     }
+    ssl_update_out_pointers( ssl, ssl->transform_out );
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= flush output" ) );
 
@@ -2535,6 +2815,9 @@ int mbedtls_ssl_flush_output( mbedtls_ssl_context *ssl )
 static int ssl_flight_append( mbedtls_ssl_context *ssl )
 {
     mbedtls_ssl_flight_item *msg;
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> ssl_flight_append" ) );
+    MBEDTLS_SSL_DEBUG_BUF( 4, "message appended to flight",
+                           ssl->out_msg, ssl->out_msglen );
 
     /* Allocate space for current message */
     if( ( msg = mbedtls_calloc( 1, sizeof(  mbedtls_ssl_flight_item ) ) ) == NULL )
@@ -2568,6 +2851,7 @@ static int ssl_flight_append( mbedtls_ssl_context *ssl )
         cur->next = msg;
     }
 
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= ssl_flight_append" ) );
     return( 0 );
 }
 
@@ -2616,19 +2900,12 @@ static void ssl_swap_epochs( mbedtls_ssl_context *ssl )
     ssl->handshake->alt_transform_out = tmp_transform;
 
     /* Swap epoch + sequence_number */
-    memcpy( tmp_out_ctr,                 ssl->out_ctr,                8 );
-    memcpy( ssl->out_ctr,                ssl->handshake->alt_out_ctr, 8 );
+    memcpy( tmp_out_ctr,                 ssl->cur_out_ctr,            8 );
+    memcpy( ssl->cur_out_ctr,            ssl->handshake->alt_out_ctr, 8 );
     memcpy( ssl->handshake->alt_out_ctr, tmp_out_ctr,                 8 );
 
     /* Adjust to the newly activated transform */
-    if( ssl->transform_out != NULL &&
-        ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_2 )
-    {
-        ssl->out_msg = ssl->out_iv + ssl->transform_out->ivlen -
-                                     ssl->transform_out->fixed_ivlen;
-    }
-    else
-        ssl->out_msg = ssl->out_iv;
+    ssl_update_out_pointers( ssl, ssl->transform_out );
 
 #if defined(MBEDTLS_SSL_HW_RECORD_ACCEL)
     if( mbedtls_ssl_hw_record_activate != NULL )
@@ -2644,20 +2921,38 @@ static void ssl_swap_epochs( mbedtls_ssl_context *ssl )
 
 /*
  * Retransmit the current flight of messages.
+ */
+int mbedtls_ssl_resend( mbedtls_ssl_context *ssl )
+{
+    int ret = 0;
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> mbedtls_ssl_resend" ) );
+
+    ret = mbedtls_ssl_flight_transmit( ssl );
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= mbedtls_ssl_resend" ) );
+
+    return( ret );
+}
+
+/*
+ * Transmit or retransmit the current flight of messages.
  *
  * Need to remember the current message in case flush_output returns
  * WANT_WRITE, causing us to exit this function and come back later.
  * This function must be called until state is no longer SENDING.
  */
-int mbedtls_ssl_resend( mbedtls_ssl_context *ssl )
+int mbedtls_ssl_flight_transmit( mbedtls_ssl_context *ssl )
 {
-    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> mbedtls_ssl_resend" ) );
+    int ret;
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> mbedtls_ssl_flight_transmit" ) );
 
     if( ssl->handshake->retransmit_state != MBEDTLS_SSL_RETRANS_SENDING )
     {
-        MBEDTLS_SSL_DEBUG_MSG( 2, ( "initialise resending" ) );
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "initialise flight transmission" ) );
 
         ssl->handshake->cur_msg = ssl->handshake->flight;
+        ssl->handshake->cur_msg_p = ssl->handshake->flight->p + 12;
         ssl_swap_epochs( ssl );
 
         ssl->handshake->retransmit_state = MBEDTLS_SSL_RETRANS_SENDING;
@@ -2665,33 +2960,129 @@ int mbedtls_ssl_resend( mbedtls_ssl_context *ssl )
 
     while( ssl->handshake->cur_msg != NULL )
     {
-        int ret;
-        mbedtls_ssl_flight_item *cur = ssl->handshake->cur_msg;
+        size_t max_frag_len;
+        const mbedtls_ssl_flight_item * const cur = ssl->handshake->cur_msg;
+
+        int const is_finished =
+            ( cur->type == MBEDTLS_SSL_MSG_HANDSHAKE &&
+              cur->p[0] == MBEDTLS_SSL_HS_FINISHED );
+
+        uint8_t const force_flush = ssl->disable_datagram_packing == 1 ?
+            SSL_FORCE_FLUSH : SSL_DONT_FORCE_FLUSH;
 
         /* Swap epochs before sending Finished: we can't do it after
          * sending ChangeCipherSpec, in case write returns WANT_READ.
          * Must be done before copying, may change out_msg pointer */
-        if( cur->type == MBEDTLS_SSL_MSG_HANDSHAKE &&
-            cur->p[0] == MBEDTLS_SSL_HS_FINISHED )
+        if( is_finished && ssl->handshake->cur_msg_p == ( cur->p + 12 ) )
         {
+            MBEDTLS_SSL_DEBUG_MSG( 2, ( "swap epochs to send finished message" ) );
             ssl_swap_epochs( ssl );
         }
 
-        memcpy( ssl->out_msg, cur->p, cur->len );
-        ssl->out_msglen = cur->len;
-        ssl->out_msgtype = cur->type;
+        ret = ssl_get_remaining_payload_in_datagram( ssl );
+        if( ret < 0 )
+            return( ret );
+        max_frag_len = (size_t) ret;
+
+        /* CCS is copied as is, while HS messages may need fragmentation */
+        if( cur->type == MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC )
+        {
+            if( max_frag_len == 0 )
+            {
+                if( ( ret = mbedtls_ssl_flush_output( ssl ) ) != 0 )
+                    return( ret );
 
-        ssl->handshake->cur_msg = cur->next;
+                continue;
+            }
 
-        MBEDTLS_SSL_DEBUG_BUF( 3, "resent handshake message header", ssl->out_msg, 12 );
+            memcpy( ssl->out_msg, cur->p, cur->len );
+            ssl->out_msglen  = cur->len;
+            ssl->out_msgtype = cur->type;
 
-        if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+            /* Update position inside current message */
+            ssl->handshake->cur_msg_p += cur->len;
+        }
+        else
+        {
+            const unsigned char * const p = ssl->handshake->cur_msg_p;
+            const size_t hs_len = cur->len - 12;
+            const size_t frag_off = p - ( cur->p + 12 );
+            const size_t rem_len = hs_len - frag_off;
+            size_t cur_hs_frag_len, max_hs_frag_len;
+
+            if( ( max_frag_len < 12 ) || ( max_frag_len == 12 && hs_len != 0 ) )
+            {
+                if( is_finished )
+                    ssl_swap_epochs( ssl );
+
+                if( ( ret = mbedtls_ssl_flush_output( ssl ) ) != 0 )
+                    return( ret );
+
+                continue;
+            }
+            max_hs_frag_len = max_frag_len - 12;
+
+            cur_hs_frag_len = rem_len > max_hs_frag_len ?
+                max_hs_frag_len : rem_len;
+
+            if( frag_off == 0 && cur_hs_frag_len != hs_len )
+            {
+                MBEDTLS_SSL_DEBUG_MSG( 2, ( "fragmenting handshake message (%u > %u)",
+                                            (unsigned) cur_hs_frag_len,
+                                            (unsigned) max_hs_frag_len ) );
+            }
+
+            /* Messages are stored with handshake headers as if not fragmented,
+             * copy beginning of headers then fill fragmentation fields.
+             * Handshake headers: type(1) len(3) seq(2) f_off(3) f_len(3) */
+            memcpy( ssl->out_msg, cur->p, 6 );
+
+            ssl->out_msg[6] = ( ( frag_off >> 16 ) & 0xff );
+            ssl->out_msg[7] = ( ( frag_off >>  8 ) & 0xff );
+            ssl->out_msg[8] = ( ( frag_off       ) & 0xff );
+
+            ssl->out_msg[ 9] = ( ( cur_hs_frag_len >> 16 ) & 0xff );
+            ssl->out_msg[10] = ( ( cur_hs_frag_len >>  8 ) & 0xff );
+            ssl->out_msg[11] = ( ( cur_hs_frag_len       ) & 0xff );
+
+            MBEDTLS_SSL_DEBUG_BUF( 3, "handshake header", ssl->out_msg, 12 );
+
+            /* Copy the handshake message content and set records fields */
+            memcpy( ssl->out_msg + 12, p, cur_hs_frag_len );
+            ssl->out_msglen = cur_hs_frag_len + 12;
+            ssl->out_msgtype = cur->type;
+
+            /* Update position inside current message */
+            ssl->handshake->cur_msg_p += cur_hs_frag_len;
+        }
+
+        /* If done with the current message move to the next one if any */
+        if( ssl->handshake->cur_msg_p >= cur->p + cur->len )
+        {
+            if( cur->next != NULL )
+            {
+                ssl->handshake->cur_msg = cur->next;
+                ssl->handshake->cur_msg_p = cur->next->p + 12;
+            }
+            else
+            {
+                ssl->handshake->cur_msg = NULL;
+                ssl->handshake->cur_msg_p = NULL;
+            }
+        }
+
+        /* Actually send the message out */
+        if( ( ret = mbedtls_ssl_write_record( ssl, force_flush ) ) != 0 )
         {
             MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
             return( ret );
         }
     }
 
+    if( ( ret = mbedtls_ssl_flush_output( ssl ) ) != 0 )
+        return( ret );
+
+    /* Update state and set timer */
     if( ssl->state == MBEDTLS_SSL_HANDSHAKE_OVER )
         ssl->handshake->retransmit_state = MBEDTLS_SSL_RETRANS_FINISHED;
     else
@@ -2700,7 +3091,7 @@ int mbedtls_ssl_resend( mbedtls_ssl_context *ssl )
         ssl_set_timer( ssl, ssl->handshake->retransmit_timeout );
     }
 
-    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= mbedtls_ssl_resend" ) );
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= mbedtls_ssl_flight_transmit" ) );
 
     return( 0 );
 }
@@ -2718,6 +3109,12 @@ void mbedtls_ssl_recv_flight_completed( mbedtls_ssl_context *ssl )
     /* The next incoming flight will start with this msg_seq */
     ssl->handshake->in_flight_start_seq = ssl->handshake->in_msg_seq;
 
+    /* We don't want to remember CCS's across flight boundaries. */
+    ssl->handshake->buffering.seen_ccs = 0;
+
+    /* Clear future message buffering structure. */
+    ssl_buffering_free( ssl );
+
     /* Cancel timer */
     ssl_set_timer( ssl, 0 );
 
@@ -2749,43 +3146,100 @@ void mbedtls_ssl_send_flight_completed( mbedtls_ssl_context *ssl )
 #endif /* MBEDTLS_SSL_PROTO_DTLS */
 
 /*
- * Record layer functions
+ * Handshake layer functions
  */
 
 /*
- * Write current record.
- * Uses ssl->out_msgtype, ssl->out_msglen and bytes at ssl->out_msg.
+ * Write (DTLS: or queue) current handshake (including CCS) message.
+ *
+ *  - fill in handshake headers
+ *  - update handshake checksum
+ *  - DTLS: save message for resending
+ *  - then pass to the record layer
+ *
+ * DTLS: except for HelloRequest, messages are only queued, and will only be
+ * actually sent when calling flight_transmit() or resend().
+ *
+ * Inputs:
+ *  - ssl->out_msglen: 4 + actual handshake message len
+ *      (4 is the size of handshake headers for TLS)
+ *  - ssl->out_msg[0]: the handshake type (ClientHello, ServerHello, etc)
+ *  - ssl->out_msg + 4: the handshake message body
+ *
+ * Outputs, ie state before passing to flight_append() or write_record():
+ *   - ssl->out_msglen: the length of the record contents
+ *      (including handshake headers but excluding record headers)
+ *   - ssl->out_msg: the record contents (handshake headers + content)
  */
-int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl )
+int mbedtls_ssl_write_handshake_msg( mbedtls_ssl_context *ssl )
 {
-    int ret, done = 0, out_msg_type;
-    size_t len = ssl->out_msglen;
+    int ret;
+    const size_t hs_len = ssl->out_msglen - 4;
+    const unsigned char hs_type = ssl->out_msg[0];
 
-    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> write record" ) );
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> write handshake message" ) );
+
+    /*
+     * Sanity checks
+     */
+    if( ssl->out_msgtype != MBEDTLS_SSL_MSG_HANDSHAKE          &&
+        ssl->out_msgtype != MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC )
+    {
+        /* In SSLv3, the client might send a NoCertificate alert. */
+#if defined(MBEDTLS_SSL_PROTO_SSL3) && defined(MBEDTLS_SSL_CLI_C)
+        if( ! ( ssl->minor_ver      == MBEDTLS_SSL_MINOR_VERSION_0 &&
+                ssl->out_msgtype    == MBEDTLS_SSL_MSG_ALERT       &&
+                ssl->conf->endpoint == MBEDTLS_SSL_IS_CLIENT ) )
+#endif /* MBEDTLS_SSL_PROTO_SSL3 && MBEDTLS_SSL_SRV_C */
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+            return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+        }
+    }
+
+    if( ssl->out_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE &&
+        hs_type != MBEDTLS_SSL_HS_HELLO_REQUEST &&
+        ssl->handshake == NULL )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+        return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+    }
 
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
     if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
         ssl->handshake != NULL &&
         ssl->handshake->retransmit_state == MBEDTLS_SSL_RETRANS_SENDING )
     {
-        ; /* Skip special handshake treatment when resending */
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+        return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
     }
-    else
 #endif
-    if( ssl->out_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE )
-    {
-        out_msg_type = ssl->out_msg[0];
 
-        if( out_msg_type != MBEDTLS_SSL_HS_HELLO_REQUEST &&
-            ssl->handshake == NULL )
-        {
-            MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
-            return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
-        }
+    /* Double-check that we did not exceed the bounds
+     * of the outgoing record buffer.
+     * This should never fail as the various message
+     * writing functions must obey the bounds of the
+     * outgoing record buffer, but better be safe.
+     *
+     * Note: We deliberately do not check for the MTU or MFL here.
+     */
+    if( ssl->out_msglen > MBEDTLS_SSL_OUT_CONTENT_LEN )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "Record too large: "
+                                    "size %u, maximum %u",
+                                    (unsigned) ssl->out_msglen,
+                                    (unsigned) MBEDTLS_SSL_OUT_CONTENT_LEN ) );
+        return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+    }
 
-        ssl->out_msg[1] = (unsigned char)( ( len - 4 ) >> 16 );
-        ssl->out_msg[2] = (unsigned char)( ( len - 4 ) >>  8 );
-        ssl->out_msg[3] = (unsigned char)( ( len - 4 )       );
+    /*
+     * Fill handshake headers
+     */
+    if( ssl->out_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE )
+    {
+        ssl->out_msg[1] = (unsigned char)( hs_len >> 16 );
+        ssl->out_msg[2] = (unsigned char)( hs_len >>  8 );
+        ssl->out_msg[3] = (unsigned char)( hs_len       );
 
         /*
          * DTLS has additional fields in the Handshake layer,
@@ -2798,21 +3252,20 @@ int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl )
         if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
         {
             /* Make room for the additional DTLS fields */
-            if( MBEDTLS_SSL_MAX_CONTENT_LEN - ssl->out_msglen < 8 )
+            if( MBEDTLS_SSL_OUT_CONTENT_LEN - ssl->out_msglen < 8 )
             {
                 MBEDTLS_SSL_DEBUG_MSG( 1, ( "DTLS handshake message too large: "
                               "size %u, maximum %u",
-                               (unsigned) ( ssl->in_hslen - 4 ),
-                               (unsigned) ( MBEDTLS_SSL_MAX_CONTENT_LEN - 12 ) ) );
+                               (unsigned) ( hs_len ),
+                               (unsigned) ( MBEDTLS_SSL_OUT_CONTENT_LEN - 12 ) ) );
                 return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
             }
 
-            memmove( ssl->out_msg + 12, ssl->out_msg + 4, len - 4 );
+            memmove( ssl->out_msg + 12, ssl->out_msg + 4, hs_len );
             ssl->out_msglen += 8;
-            len += 8;
 
             /* Write message_seq and update it, except for HelloRequest */
-            if( out_msg_type != MBEDTLS_SSL_HS_HELLO_REQUEST )
+            if( hs_type != MBEDTLS_SSL_HS_HELLO_REQUEST )
             {
                 ssl->out_msg[4] = ( ssl->handshake->out_msg_seq >> 8 ) & 0xFF;
                 ssl->out_msg[5] = ( ssl->handshake->out_msg_seq      ) & 0xFF;
@@ -2824,23 +3277,23 @@ int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl )
                 ssl->out_msg[5] = 0;
             }
 
-            /* We don't fragment, so frag_offset = 0 and frag_len = len */
+            /* Handshake hashes are computed without fragmentation,
+             * so set frag_offset = 0 and frag_len = hs_len for now */
             memset( ssl->out_msg + 6, 0x00, 3 );
             memcpy( ssl->out_msg + 9, ssl->out_msg + 1, 3 );
         }
 #endif /* MBEDTLS_SSL_PROTO_DTLS */
 
-        if( out_msg_type != MBEDTLS_SSL_HS_HELLO_REQUEST )
-            ssl->handshake->update_checksum( ssl, ssl->out_msg, len );
+        /* Update running hashes of handshake messages seen */
+        if( hs_type != MBEDTLS_SSL_HS_HELLO_REQUEST )
+            ssl->handshake->update_checksum( ssl, ssl->out_msg, ssl->out_msglen );
     }
 
-    /* Save handshake and CCS messages for resending */
+    /* Either send now, or just save to be sent (and resent) later */
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
     if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
-        ssl->handshake != NULL &&
-        ssl->handshake->retransmit_state != MBEDTLS_SSL_RETRANS_SENDING &&
-        ( ssl->out_msgtype == MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC ||
-          ssl->out_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE ) )
+        ( ssl->out_msgtype != MBEDTLS_SSL_MSG_HANDSHAKE ||
+          hs_type != MBEDTLS_SSL_HS_HELLO_REQUEST ) )
     {
         if( ( ret = ssl_flight_append( ssl ) ) != 0 )
         {
@@ -2848,7 +3301,40 @@ int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl )
             return( ret );
         }
     }
+    else
 #endif
+    {
+        if( ( ret = mbedtls_ssl_write_record( ssl, SSL_FORCE_FLUSH ) ) != 0 )
+        {
+            MBEDTLS_SSL_DEBUG_RET( 1, "ssl_write_record", ret );
+            return( ret );
+        }
+    }
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= write handshake message" ) );
+
+    return( 0 );
+}
+
+/*
+ * Record layer functions
+ */
+
+/*
+ * Write current record.
+ *
+ * Uses:
+ *  - ssl->out_msgtype: type of the message (AppData, Handshake, Alert, CCS)
+ *  - ssl->out_msglen: length of the record content (excl headers)
+ *  - ssl->out_msg: record content
+ */
+int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl, uint8_t force_flush )
+{
+    int ret, done = 0;
+    size_t len = ssl->out_msglen;
+    uint8_t flush = force_flush;
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> write record" ) );
 
 #if defined(MBEDTLS_ZLIB_SUPPORT)
     if( ssl->transform_out != NULL &&
@@ -2882,10 +3368,14 @@ int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl )
 #endif /* MBEDTLS_SSL_HW_RECORD_ACCEL */
     if( !done )
     {
+        unsigned i;
+        size_t protected_record_size;
+
         ssl->out_hdr[0] = (unsigned char) ssl->out_msgtype;
         mbedtls_ssl_write_version( ssl->major_ver, ssl->minor_ver,
                            ssl->conf->transport, ssl->out_hdr + 1 );
 
+        memcpy( ssl->out_ctr, ssl->cur_out_ctr, 8 );
         ssl->out_len[0] = (unsigned char)( len >> 8 );
         ssl->out_len[1] = (unsigned char)( len      );
 
@@ -2902,18 +3392,76 @@ int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl )
             ssl->out_len[1] = (unsigned char)( len      );
         }
 
-        ssl->out_left = mbedtls_ssl_hdr_len( ssl ) + ssl->out_msglen;
+        protected_record_size = len + mbedtls_ssl_hdr_len( ssl );
+
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+        /* In case of DTLS, double-check that we don't exceed
+         * the remaining space in the datagram. */
+        if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+        {
+            ret = ssl_get_remaining_space_in_datagram( ssl );
+            if( ret < 0 )
+                return( ret );
+
+            if( protected_record_size > (size_t) ret )
+            {
+                /* Should never happen */
+                return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+            }
+        }
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
 
         MBEDTLS_SSL_DEBUG_MSG( 3, ( "output record: msgtype = %d, "
-                            "version = [%d:%d], msglen = %d",
-                       ssl->out_hdr[0], ssl->out_hdr[1], ssl->out_hdr[2],
-                     ( ssl->out_len[0] << 8 ) | ssl->out_len[1] ) );
+                                    "version = [%d:%d], msglen = %d",
+                                    ssl->out_hdr[0], ssl->out_hdr[1],
+                                    ssl->out_hdr[2], len ) );
 
         MBEDTLS_SSL_DEBUG_BUF( 4, "output record sent to network",
-                       ssl->out_hdr, mbedtls_ssl_hdr_len( ssl ) + ssl->out_msglen );
+                               ssl->out_hdr, protected_record_size );
+
+        ssl->out_left += protected_record_size;
+        ssl->out_hdr  += protected_record_size;
+        ssl_update_out_pointers( ssl, ssl->transform_out );
+
+        for( i = 8; i > ssl_ep_len( ssl ); i-- )
+            if( ++ssl->cur_out_ctr[i - 1] != 0 )
+                break;
+
+        /* The loop goes to its end iff the counter is wrapping */
+        if( i == ssl_ep_len( ssl ) )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "outgoing message counter would wrap" ) );
+            return( MBEDTLS_ERR_SSL_COUNTER_WRAPPING );
+        }
     }
 
-    if( ( ret = mbedtls_ssl_flush_output( ssl ) ) != 0 )
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
+        flush == SSL_DONT_FORCE_FLUSH )
+    {
+        size_t remaining;
+        ret = ssl_get_remaining_payload_in_datagram( ssl );
+        if( ret < 0 )
+        {
+            MBEDTLS_SSL_DEBUG_RET( 1, "ssl_get_remaining_payload_in_datagram",
+                                   ret );
+            return( ret );
+        }
+
+        remaining = (size_t) ret;
+        if( remaining == 0 )
+        {
+            flush = SSL_FORCE_FLUSH;
+        }
+        else
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 2, ( "Still %u bytes available in current datagram", (unsigned) remaining ) );
+        }
+    }
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
+    if( ( flush == SSL_FORCE_FLUSH ) &&
+        ( ret = mbedtls_ssl_flush_output( ssl ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_flush_output", ret );
         return( ret );
@@ -2925,6 +3473,52 @@ int mbedtls_ssl_write_record( mbedtls_ssl_context *ssl )
 }
 
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
+
+static int ssl_hs_is_proper_fragment( mbedtls_ssl_context *ssl )
+{
+    if( ssl->in_msglen < ssl->in_hslen ||
+        memcmp( ssl->in_msg + 6, "\0\0\0",        3 ) != 0 ||
+        memcmp( ssl->in_msg + 9, ssl->in_msg + 1, 3 ) != 0 )
+    {
+        return( 1 );
+    }
+    return( 0 );
+}
+
+static uint32_t ssl_get_hs_frag_len( mbedtls_ssl_context const *ssl )
+{
+    return( ( ssl->in_msg[9] << 16  ) |
+            ( ssl->in_msg[10] << 8  ) |
+              ssl->in_msg[11] );
+}
+
+static uint32_t ssl_get_hs_frag_off( mbedtls_ssl_context const *ssl )
+{
+    return( ( ssl->in_msg[6] << 16 ) |
+            ( ssl->in_msg[7] << 8  ) |
+              ssl->in_msg[8] );
+}
+
+static int ssl_check_hs_header( mbedtls_ssl_context const *ssl )
+{
+    uint32_t msg_len, frag_off, frag_len;
+
+    msg_len  = ssl_get_hs_total_len( ssl );
+    frag_off = ssl_get_hs_frag_off( ssl );
+    frag_len = ssl_get_hs_frag_len( ssl );
+
+    if( frag_off > msg_len )
+        return( -1 );
+
+    if( frag_len > msg_len - frag_off )
+        return( -1 );
+
+    if( frag_len + 12 > ssl->in_msglen )
+        return( -1 );
+
+    return( 0 );
+}
+
 /*
  * Mark bits in bitmask (used for DTLS HS reassembly)
  */
@@ -2986,161 +3580,29 @@ static int ssl_bitmask_check( unsigned char *mask, size_t len )
     return( 0 );
 }
 
-/*
- * Reassemble fragmented DTLS handshake messages.
- *
- * Use a temporary buffer for reassembly, divided in two parts:
- * - the first holds the reassembled message (including handshake header),
- * - the second holds a bitmask indicating which parts of the message
- *   (excluding headers) have been received so far.
- */
-static int ssl_reassemble_dtls_handshake( mbedtls_ssl_context *ssl )
+/* msg_len does not include the handshake header */
+static size_t ssl_get_reassembly_buffer_size( size_t msg_len,
+                                              unsigned add_bitmap )
 {
-    unsigned char *msg, *bitmask;
-    size_t frag_len, frag_off;
-    size_t msg_len = ssl->in_hslen - 12; /* Without headers */
-
-    if( ssl->handshake == NULL )
-    {
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "not supported outside handshake (for now)" ) );
-        return( MBEDTLS_ERR_SSL_FEATURE_UNAVAILABLE );
-    }
-
-    /*
-     * For first fragment, check size and allocate buffer
-     */
-    if( ssl->handshake->hs_msg == NULL )
-    {
-        size_t alloc_len;
-
-        MBEDTLS_SSL_DEBUG_MSG( 2, ( "initialize reassembly, total length = %d",
-                            msg_len ) );
-
-        if( ssl->in_hslen > MBEDTLS_SSL_MAX_CONTENT_LEN )
-        {
-            MBEDTLS_SSL_DEBUG_MSG( 1, ( "handshake message too large" ) );
-            return( MBEDTLS_ERR_SSL_FEATURE_UNAVAILABLE );
-        }
-
-        /* The bitmask needs one bit per byte of message excluding header */
-        alloc_len = 12 + msg_len + msg_len / 8 + ( msg_len % 8 != 0 );
-
-        ssl->handshake->hs_msg = mbedtls_calloc( 1, alloc_len );
-        if( ssl->handshake->hs_msg == NULL )
-        {
-            MBEDTLS_SSL_DEBUG_MSG( 1, ( "alloc failed (%d bytes)", alloc_len ) );
-            return( MBEDTLS_ERR_SSL_ALLOC_FAILED );
-        }
-
-        /* Prepare final header: copy msg_type, length and message_seq,
-         * then add standardised fragment_offset and fragment_length */
-        memcpy( ssl->handshake->hs_msg, ssl->in_msg, 6 );
-        memset( ssl->handshake->hs_msg + 6, 0, 3 );
-        memcpy( ssl->handshake->hs_msg + 9,
-                ssl->handshake->hs_msg + 1, 3 );
-    }
-    else
-    {
-        /* Make sure msg_type and length are consistent */
-        if( memcmp( ssl->handshake->hs_msg, ssl->in_msg, 4 ) != 0 )
-        {
-            MBEDTLS_SSL_DEBUG_MSG( 1, ( "fragment header mismatch" ) );
-            return( MBEDTLS_ERR_SSL_INVALID_RECORD );
-        }
-    }
-
-    msg = ssl->handshake->hs_msg + 12;
-    bitmask = msg + msg_len;
-
-    /*
-     * Check and copy current fragment
-     */
-    frag_off = ( ssl->in_msg[6]  << 16 ) |
-               ( ssl->in_msg[7]  << 8  ) |
-                 ssl->in_msg[8];
-    frag_len = ( ssl->in_msg[9]  << 16 ) |
-               ( ssl->in_msg[10] << 8  ) |
-                 ssl->in_msg[11];
-
-    if( frag_off + frag_len > msg_len )
-    {
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "invalid fragment offset/len: %d + %d > %d",
-                          frag_off, frag_len, msg_len ) );
-        return( MBEDTLS_ERR_SSL_INVALID_RECORD );
-    }
-
-    if( frag_len + 12 > ssl->in_msglen )
-    {
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "invalid fragment length: %d + 12 > %d",
-                          frag_len, ssl->in_msglen ) );
-        return( MBEDTLS_ERR_SSL_INVALID_RECORD );
-    }
-
-    MBEDTLS_SSL_DEBUG_MSG( 2, ( "adding fragment, offset = %d, length = %d",
-                        frag_off, frag_len ) );
-
-    memcpy( msg + frag_off, ssl->in_msg + 12, frag_len );
-    ssl_bitmask_set( bitmask, frag_off, frag_len );
-
-    /*
-     * Do we have the complete message by now?
-     * If yes, finalize it, else ask to read the next record.
-     */
-    if( ssl_bitmask_check( bitmask, msg_len ) != 0 )
-    {
-        MBEDTLS_SSL_DEBUG_MSG( 2, ( "message is not complete yet" ) );
-        return( MBEDTLS_ERR_SSL_CONTINUE_PROCESSING );
-    }
-
-    MBEDTLS_SSL_DEBUG_MSG( 2, ( "handshake message completed" ) );
-
-    if( frag_len + 12 < ssl->in_msglen )
-    {
-        /*
-         * We'got more handshake messages in the same record.
-         * This case is not handled now because no know implementation does
-         * that and it's hard to test, so we prefer to fail cleanly for now.
-         */
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "last fragment not alone in its record" ) );
-        return( MBEDTLS_ERR_SSL_FEATURE_UNAVAILABLE );
-    }
-
-    if( ssl->in_left > ssl->next_record_offset )
-    {
-        /*
-         * We've got more data in the buffer after the current record,
-         * that we don't want to overwrite. Move it before writing the
-         * reassembled message, and adjust in_left and next_record_offset.
-         */
-        unsigned char *cur_remain = ssl->in_hdr + ssl->next_record_offset;
-        unsigned char *new_remain = ssl->in_msg + ssl->in_hslen;
-        size_t remain_len = ssl->in_left - ssl->next_record_offset;
-
-        /* First compute and check new lengths */
-        ssl->next_record_offset = new_remain - ssl->in_hdr;
-        ssl->in_left = ssl->next_record_offset + remain_len;
-
-        if( ssl->in_left > MBEDTLS_SSL_BUFFER_LEN -
-                           (size_t)( ssl->in_hdr - ssl->in_buf ) )
-        {
-            MBEDTLS_SSL_DEBUG_MSG( 1, ( "reassembled message too large for buffer" ) );
-            return( MBEDTLS_ERR_SSL_BUFFER_TOO_SMALL );
-        }
+    size_t alloc_len;
 
-        memmove( new_remain, cur_remain, remain_len );
-    }
+    alloc_len  = 12;                                 /* Handshake header */
+    alloc_len += msg_len;                            /* Content buffer   */
 
-    memcpy( ssl->in_msg, ssl->handshake->hs_msg, ssl->in_hslen );
+    if( add_bitmap )
+        alloc_len += msg_len / 8 + ( msg_len % 8 != 0 ); /* Bitmap       */
 
-    mbedtls_free( ssl->handshake->hs_msg );
-    ssl->handshake->hs_msg = NULL;
+    return( alloc_len );
+}
 
-    MBEDTLS_SSL_DEBUG_BUF( 3, "reassembled handshake message",
-                   ssl->in_msg, ssl->in_hslen );
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
 
-    return( 0 );
+static uint32_t ssl_get_hs_total_len( mbedtls_ssl_context const *ssl )
+{
+    return( ( ssl->in_msg[1] << 16 ) |
+            ( ssl->in_msg[2] << 8  ) |
+              ssl->in_msg[3] );
 }
-#endif /* MBEDTLS_SSL_PROTO_DTLS */
 
 int mbedtls_ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
 {
@@ -3151,10 +3613,7 @@ int mbedtls_ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
         return( MBEDTLS_ERR_SSL_INVALID_RECORD );
     }
 
-    ssl->in_hslen = mbedtls_ssl_hs_hdr_len( ssl ) + (
-                    ( ssl->in_msg[1] << 16 ) |
-                    ( ssl->in_msg[2] << 8  ) |
-                      ssl->in_msg[3] );
+    ssl->in_hslen = mbedtls_ssl_hs_hdr_len( ssl ) + ssl_get_hs_total_len( ssl );
 
     MBEDTLS_SSL_DEBUG_MSG( 3, ( "handshake message: msglen ="
                         " %d, type = %d, hslen = %d",
@@ -3166,12 +3625,26 @@ int mbedtls_ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
         int ret;
         unsigned int recv_msg_seq = ( ssl->in_msg[4] << 8 ) | ssl->in_msg[5];
 
+        if( ssl_check_hs_header( ssl ) != 0 )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "invalid handshake header" ) );
+            return( MBEDTLS_ERR_SSL_INVALID_RECORD );
+        }
+
         if( ssl->handshake != NULL &&
             ( ( ssl->state   != MBEDTLS_SSL_HANDSHAKE_OVER &&
                 recv_msg_seq != ssl->handshake->in_msg_seq ) ||
               ( ssl->state  == MBEDTLS_SSL_HANDSHAKE_OVER &&
                 ssl->in_msg[0] != MBEDTLS_SSL_HS_CLIENT_HELLO ) ) )
         {
+            if( recv_msg_seq > ssl->handshake->in_msg_seq )
+            {
+                MBEDTLS_SSL_DEBUG_MSG( 2, ( "received future handshake message of sequence number %u (next %u)",
+                                            recv_msg_seq,
+                                            ssl->handshake->in_msg_seq ) );
+                return( MBEDTLS_ERR_SSL_EARLY_MESSAGE );
+            }
+
             /* Retransmit only on last message from previous flight, to avoid
              * too many retransmissions.
              * Besides, No sane server ever retransmits HelloVerifyRequest */
@@ -3201,20 +3674,14 @@ int mbedtls_ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
         }
         /* Wait until message completion to increment in_msg_seq */
 
-        /* Reassemble if current message is fragmented or reassembly is
-         * already in progress */
-        if( ssl->in_msglen < ssl->in_hslen ||
-            memcmp( ssl->in_msg + 6, "\0\0\0",        3 ) != 0 ||
-            memcmp( ssl->in_msg + 9, ssl->in_msg + 1, 3 ) != 0 ||
-            ( ssl->handshake != NULL && ssl->handshake->hs_msg != NULL ) )
+        /* Message reassembly is handled alongside buffering of future
+         * messages; the commonality is that both handshake fragments and
+         * future messages cannot be forwarded immediately to the
+         * handshake logic layer. */
+        if( ssl_hs_is_proper_fragment( ssl ) == 1 )
         {
             MBEDTLS_SSL_DEBUG_MSG( 2, ( "found fragmented DTLS handshake message" ) );
-
-            if( ( ret = ssl_reassemble_dtls_handshake( ssl ) ) != 0 )
-            {
-                MBEDTLS_SSL_DEBUG_RET( 1, "ssl_reassemble_dtls_handshake", ret );
-                return( ret );
-            }
+            return( MBEDTLS_ERR_SSL_EARLY_MESSAGE );
         }
     }
     else
@@ -3231,9 +3698,9 @@ int mbedtls_ssl_prepare_handshake_record( mbedtls_ssl_context *ssl )
 
 void mbedtls_ssl_update_handshake_status( mbedtls_ssl_context *ssl )
 {
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
 
-    if( ssl->state != MBEDTLS_SSL_HANDSHAKE_OVER &&
-        ssl->handshake != NULL )
+    if( ssl->state != MBEDTLS_SSL_HANDSHAKE_OVER && hs != NULL )
     {
         ssl->handshake->update_checksum( ssl, ssl->in_msg, ssl->in_hslen );
     }
@@ -3243,7 +3710,29 @@ void mbedtls_ssl_update_handshake_status( mbedtls_ssl_context *ssl )
     if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
         ssl->handshake != NULL )
     {
-        ssl->handshake->in_msg_seq++;
+        unsigned offset;
+        mbedtls_ssl_hs_buffer *hs_buf;
+
+        /* Increment handshake sequence number */
+        hs->in_msg_seq++;
+
+        /*
+         * Clear up handshake buffering and reassembly structure.
+         */
+
+        /* Free first entry */
+        ssl_buffering_free_slot( ssl, 0 );
+
+        /* Shift all other entries */
+        for( offset = 0, hs_buf = &hs->buffering.hs[0];
+             offset + 1 < MBEDTLS_SSL_MAX_BUFFERED_HS;
+             offset++, hs_buf++ )
+        {
+            *hs_buf = *(hs_buf + 1);
+        }
+
+        /* Create a fresh last entry */
+        memset( hs_buf, 0, sizeof( mbedtls_ssl_hs_buffer ) );
     }
 #endif
 }
@@ -3496,7 +3985,7 @@ static int ssl_handle_possible_reconnect( mbedtls_ssl_context *ssl )
             ssl->conf->p_cookie,
             ssl->cli_id, ssl->cli_id_len,
             ssl->in_buf, ssl->in_left,
-            ssl->out_buf, MBEDTLS_SSL_MAX_CONTENT_LEN, &len );
+            ssl->out_buf, MBEDTLS_SSL_OUT_CONTENT_LEN, &len );
 
     MBEDTLS_SSL_DEBUG_RET( 2, "ssl_check_dtls_clihlo_cookie", ret );
 
@@ -3593,7 +4082,7 @@ static int ssl_parse_record_header( mbedtls_ssl_context *ssl )
     }
 
     /* Check length against the size of our buffer */
-    if( ssl->in_msglen > MBEDTLS_SSL_BUFFER_LEN
+    if( ssl->in_msglen > MBEDTLS_SSL_IN_BUFFER_LEN
                          - (size_t)( ssl->in_msg - ssl->in_buf ) )
     {
         MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad message length" ) );
@@ -3644,7 +4133,16 @@ static int ssl_parse_record_header( mbedtls_ssl_context *ssl )
             }
             else
 #endif /* MBEDTLS_SSL_DTLS_CLIENT_PORT_REUSE && MBEDTLS_SSL_SRV_C */
+            {
+                /* Consider buffering the record. */
+                if( rec_epoch == (unsigned int) ssl->in_epoch + 1 )
+                {
+                    MBEDTLS_SSL_DEBUG_MSG( 2, ( "Consider record for buffering" ) );
+                    return( MBEDTLS_ERR_SSL_EARLY_MESSAGE );
+                }
+
                 return( MBEDTLS_ERR_SSL_UNEXPECTED_RECORD );
+            }
         }
 
 #if defined(MBEDTLS_SSL_DTLS_ANTI_REPLAY)
@@ -3657,15 +4155,6 @@ static int ssl_parse_record_header( mbedtls_ssl_context *ssl )
         }
 #endif
 
-        /* Drop unexpected ChangeCipherSpec messages */
-        if( ssl->in_msgtype == MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC &&
-            ssl->state != MBEDTLS_SSL_CLIENT_CHANGE_CIPHER_SPEC &&
-            ssl->state != MBEDTLS_SSL_SERVER_CHANGE_CIPHER_SPEC )
-        {
-            MBEDTLS_SSL_DEBUG_MSG( 1, ( "dropping unexpected ChangeCipherSpec" ) );
-            return( MBEDTLS_ERR_SSL_UNEXPECTED_RECORD );
-        }
-
         /* Drop unexpected ApplicationData records,
          * except at the beginning of renegotiations */
         if( ssl->in_msgtype == MBEDTLS_SSL_MSG_APPLICATION_DATA &&
@@ -3687,7 +4176,7 @@ static int ssl_parse_record_header( mbedtls_ssl_context *ssl )
     if( ssl->transform_in == NULL )
     {
         if( ssl->in_msglen < 1 ||
-            ssl->in_msglen > MBEDTLS_SSL_MAX_CONTENT_LEN )
+            ssl->in_msglen > MBEDTLS_SSL_IN_CONTENT_LEN )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad message length" ) );
             return( MBEDTLS_ERR_SSL_INVALID_RECORD );
@@ -3703,7 +4192,7 @@ static int ssl_parse_record_header( mbedtls_ssl_context *ssl )
 
 #if defined(MBEDTLS_SSL_PROTO_SSL3)
         if( ssl->minor_ver == MBEDTLS_SSL_MINOR_VERSION_0 &&
-            ssl->in_msglen > ssl->transform_in->minlen + MBEDTLS_SSL_MAX_CONTENT_LEN )
+            ssl->in_msglen > ssl->transform_in->minlen + MBEDTLS_SSL_IN_CONTENT_LEN )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad message length" ) );
             return( MBEDTLS_ERR_SSL_INVALID_RECORD );
@@ -3716,7 +4205,7 @@ static int ssl_parse_record_header( mbedtls_ssl_context *ssl )
          */
         if( ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_1 &&
             ssl->in_msglen > ssl->transform_in->minlen +
-                             MBEDTLS_SSL_MAX_CONTENT_LEN + 256 )
+                             MBEDTLS_SSL_IN_CONTENT_LEN + 256 )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad message length" ) );
             return( MBEDTLS_ERR_SSL_INVALID_RECORD );
@@ -3764,7 +4253,7 @@ static int ssl_prepare_record_content( mbedtls_ssl_context *ssl )
         MBEDTLS_SSL_DEBUG_BUF( 4, "input payload after decrypt",
                        ssl->in_msg, ssl->in_msglen );
 
-        if( ssl->in_msglen > MBEDTLS_SSL_MAX_CONTENT_LEN )
+        if( ssl->in_msglen > MBEDTLS_SSL_IN_CONTENT_LEN )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad message length" ) );
             return( MBEDTLS_ERR_SSL_INVALID_RECORD );
@@ -3802,7 +4291,14 @@ static void ssl_handshake_wrapup_free_hs_transform( mbedtls_ssl_context *ssl );
  * RFC 6347 4.1.2.7) and continue reading until a valid record is found.
  *
  */
-int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl )
+
+/* Helper functions for mbedtls_ssl_read_record(). */
+static int ssl_consume_current_message( mbedtls_ssl_context *ssl );
+static int ssl_get_next_record( mbedtls_ssl_context *ssl );
+static int ssl_record_is_in_progress( mbedtls_ssl_context *ssl );
+
+int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl,
+                             unsigned update_hs_digest )
 {
     int ret;
 
@@ -3812,17 +4308,53 @@ int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl )
     {
         do {
 
-            do ret = mbedtls_ssl_read_record_layer( ssl );
-            while( ret == MBEDTLS_ERR_SSL_CONTINUE_PROCESSING );
-
+            ret = ssl_consume_current_message( ssl );
             if( ret != 0 )
-            {
-                MBEDTLS_SSL_DEBUG_RET( 1, ( "mbedtls_ssl_read_record_layer" ), ret );
                 return( ret );
+
+            if( ssl_record_is_in_progress( ssl ) == 0 )
+            {
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+                int have_buffered = 0;
+
+                /* We only check for buffered messages if the
+                 * current datagram is fully consumed. */
+                if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
+                    ssl_next_record_is_in_datagram( ssl ) == 0 )
+                {
+                    if( ssl_load_buffered_message( ssl ) == 0 )
+                        have_buffered = 1;
+                }
+
+                if( have_buffered == 0 )
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+                {
+                    ret = ssl_get_next_record( ssl );
+                    if( ret == MBEDTLS_ERR_SSL_CONTINUE_PROCESSING )
+                        continue;
+
+                    if( ret != 0 )
+                    {
+                        MBEDTLS_SSL_DEBUG_RET( 1, ( "ssl_get_next_record" ), ret );
+                        return( ret );
+                    }
+                }
             }
 
             ret = mbedtls_ssl_handle_message_type( ssl );
 
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+            if( ret == MBEDTLS_ERR_SSL_EARLY_MESSAGE )
+            {
+                /* Buffer future message */
+                ret = ssl_buffer_message( ssl );
+                if( ret != 0 )
+                    return( ret );
+
+                ret = MBEDTLS_ERR_SSL_CONTINUE_PROCESSING;
+            }
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
         } while( MBEDTLS_ERR_SSL_NON_FATAL           == ret  ||
                  MBEDTLS_ERR_SSL_CONTINUE_PROCESSING == ret );
 
@@ -3832,14 +4364,15 @@ int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl )
             return( ret );
         }
 
-        if( ssl->in_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE )
+        if( ssl->in_msgtype == MBEDTLS_SSL_MSG_HANDSHAKE &&
+            update_hs_digest == 1 )
         {
             mbedtls_ssl_update_handshake_status( ssl );
         }
     }
     else
     {
-        MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= reuse previously read message" ) );
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "reuse previously read message" ) );
         ssl->keep_current_message = 0;
     }
 
@@ -3848,13 +4381,350 @@ int mbedtls_ssl_read_record( mbedtls_ssl_context *ssl )
     return( 0 );
 }
 
-int mbedtls_ssl_read_record_layer( mbedtls_ssl_context *ssl )
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+static int ssl_next_record_is_in_datagram( mbedtls_ssl_context *ssl )
 {
-    int ret;
+    if( ssl->in_left > ssl->next_record_offset )
+        return( 1 );
+
+    return( 0 );
+}
+
+static int ssl_load_buffered_message( mbedtls_ssl_context *ssl )
+{
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+    mbedtls_ssl_hs_buffer * hs_buf;
+    int ret = 0;
+
+    if( hs == NULL )
+        return( -1 );
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> ssl_load_buffered_messsage" ) );
+
+    if( ssl->state == MBEDTLS_SSL_CLIENT_CHANGE_CIPHER_SPEC ||
+        ssl->state == MBEDTLS_SSL_SERVER_CHANGE_CIPHER_SPEC )
+    {
+        /* Check if we have seen a ChangeCipherSpec before.
+         * If yes, synthesize a CCS record. */
+        if( !hs->buffering.seen_ccs )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 2, ( "CCS not seen in the current flight" ) );
+            ret = -1;
+            goto exit;
+        }
+
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Injecting buffered CCS message" ) );
+        ssl->in_msgtype = MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC;
+        ssl->in_msglen = 1;
+        ssl->in_msg[0] = 1;
+
+        /* As long as they are equal, the exact value doesn't matter. */
+        ssl->in_left            = 0;
+        ssl->next_record_offset = 0;
+
+        hs->buffering.seen_ccs = 0;
+        goto exit;
+    }
+
+#if defined(MBEDTLS_DEBUG_C)
+    /* Debug only */
+    {
+        unsigned offset;
+        for( offset = 1; offset < MBEDTLS_SSL_MAX_BUFFERED_HS; offset++ )
+        {
+            hs_buf = &hs->buffering.hs[offset];
+            if( hs_buf->is_valid == 1 )
+            {
+                MBEDTLS_SSL_DEBUG_MSG( 2, ( "Future message with sequence number %u %s buffered.",
+                            hs->in_msg_seq + offset,
+                            hs_buf->is_complete ? "fully" : "partially" ) );
+            }
+        }
+    }
+#endif /* MBEDTLS_DEBUG_C */
+
+    /* Check if we have buffered and/or fully reassembled the
+     * next handshake message. */
+    hs_buf = &hs->buffering.hs[0];
+    if( ( hs_buf->is_valid == 1 ) && ( hs_buf->is_complete == 1 ) )
+    {
+        /* Synthesize a record containing the buffered HS message. */
+        size_t msg_len = ( hs_buf->data[1] << 16 ) |
+                         ( hs_buf->data[2] << 8  ) |
+                           hs_buf->data[3];
+
+        /* Double-check that we haven't accidentally buffered
+         * a message that doesn't fit into the input buffer. */
+        if( msg_len + 12 > MBEDTLS_SSL_IN_CONTENT_LEN )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+            return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+        }
+
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Next handshake message has been buffered - load" ) );
+        MBEDTLS_SSL_DEBUG_BUF( 3, "Buffered handshake message (incl. header)",
+                               hs_buf->data, msg_len + 12 );
+
+        ssl->in_msgtype = MBEDTLS_SSL_MSG_HANDSHAKE;
+        ssl->in_hslen   = msg_len + 12;
+        ssl->in_msglen  = msg_len + 12;
+        memcpy( ssl->in_msg, hs_buf->data, ssl->in_hslen );
+
+        ret = 0;
+        goto exit;
+    }
+    else
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Next handshake message %u not or only partially bufffered",
+                                    hs->in_msg_seq ) );
+    }
+
+    ret = -1;
+
+exit:
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= ssl_load_buffered_message" ) );
+    return( ret );
+}
+
+static int ssl_buffer_make_space( mbedtls_ssl_context *ssl,
+                                  size_t desired )
+{
+    int offset;
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "Attempt to free buffered messages to have %u bytes available",
+                                (unsigned) desired ) );
+
+    /* Get rid of future records epoch first, if such exist. */
+    ssl_free_buffered_record( ssl );
+
+    /* Check if we have enough space available now. */
+    if( desired <= ( MBEDTLS_SSL_DTLS_MAX_BUFFERING -
+                     hs->buffering.total_bytes_buffered ) )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Enough space available after freeing future epoch record" ) );
+        return( 0 );
+    }
+
+    /* We don't have enough space to buffer the next expected handshake
+     * message. Remove buffers used for future messages to gain space,
+     * starting with the most distant one. */
+    for( offset = MBEDTLS_SSL_MAX_BUFFERED_HS - 1;
+         offset >= 0; offset-- )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Free buffering slot %d to make space for reassembly of next handshake message",
+                                    offset ) );
+
+        ssl_buffering_free_slot( ssl, (uint8_t) offset );
+
+        /* Check if we have enough space available now. */
+        if( desired <= ( MBEDTLS_SSL_DTLS_MAX_BUFFERING -
+                         hs->buffering.total_bytes_buffered ) )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 2, ( "Enough space available after freeing buffered HS messages" ) );
+            return( 0 );
+        }
+    }
+
+    return( -1 );
+}
+
+static int ssl_buffer_message( mbedtls_ssl_context *ssl )
+{
+    int ret = 0;
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+
+    if( hs == NULL )
+        return( 0 );
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> ssl_buffer_message" ) );
+
+    switch( ssl->in_msgtype )
+    {
+        case MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC:
+            MBEDTLS_SSL_DEBUG_MSG( 2, ( "Remember CCS message" ) );
+
+            hs->buffering.seen_ccs = 1;
+            break;
+
+        case MBEDTLS_SSL_MSG_HANDSHAKE:
+        {
+            unsigned recv_msg_seq_offset;
+            unsigned recv_msg_seq = ( ssl->in_msg[4] << 8 ) | ssl->in_msg[5];
+            mbedtls_ssl_hs_buffer *hs_buf;
+            size_t msg_len = ssl->in_hslen - 12;
+
+            /* We should never receive an old handshake
+             * message - double-check nonetheless. */
+            if( recv_msg_seq < ssl->handshake->in_msg_seq )
+            {
+                MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+                return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+            }
+
+            recv_msg_seq_offset = recv_msg_seq - ssl->handshake->in_msg_seq;
+            if( recv_msg_seq_offset >= MBEDTLS_SSL_MAX_BUFFERED_HS )
+            {
+                /* Silently ignore -- message too far in the future */
+                MBEDTLS_SSL_DEBUG_MSG( 2,
+                 ( "Ignore future HS message with sequence number %u, "
+                   "buffering window %u - %u",
+                   recv_msg_seq, ssl->handshake->in_msg_seq,
+                   ssl->handshake->in_msg_seq + MBEDTLS_SSL_MAX_BUFFERED_HS - 1 ) );
+
+                goto exit;
+            }
+
+            MBEDTLS_SSL_DEBUG_MSG( 2, ( "Buffering HS message with sequence number %u, offset %u ",
+                                        recv_msg_seq, recv_msg_seq_offset ) );
+
+            hs_buf = &hs->buffering.hs[ recv_msg_seq_offset ];
+
+            /* Check if the buffering for this seq nr has already commenced. */
+            if( !hs_buf->is_valid )
+            {
+                size_t reassembly_buf_sz;
 
+                hs_buf->is_fragmented =
+                    ( ssl_hs_is_proper_fragment( ssl ) == 1 );
+
+                /* We copy the message back into the input buffer
+                 * after reassembly, so check that it's not too large.
+                 * This is an implementation-specific limitation
+                 * and not one from the standard, hence it is not
+                 * checked in ssl_check_hs_header(). */
+                if( msg_len + 12 > MBEDTLS_SSL_IN_CONTENT_LEN )
+                {
+                    /* Ignore message */
+                    goto exit;
+                }
+
+                /* Check if we have enough space to buffer the message. */
+                if( hs->buffering.total_bytes_buffered >
+                    MBEDTLS_SSL_DTLS_MAX_BUFFERING )
+                {
+                    MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+                    return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+                }
+
+                reassembly_buf_sz = ssl_get_reassembly_buffer_size( msg_len,
+                                                       hs_buf->is_fragmented );
+
+                if( reassembly_buf_sz > ( MBEDTLS_SSL_DTLS_MAX_BUFFERING -
+                                          hs->buffering.total_bytes_buffered ) )
+                {
+                    if( recv_msg_seq_offset > 0 )
+                    {
+                        /* If we can't buffer a future message because
+                         * of space limitations -- ignore. */
+                        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Buffering of future message of size %u would exceed the compile-time limit %u (already %u bytes buffered) -- ignore\n",
+                             (unsigned) msg_len, MBEDTLS_SSL_DTLS_MAX_BUFFERING,
+                             (unsigned) hs->buffering.total_bytes_buffered ) );
+                        goto exit;
+                    }
+                    else
+                    {
+                        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Buffering of future message of size %u would exceed the compile-time limit %u (already %u bytes buffered) -- attempt to make space by freeing buffered future messages\n",
+                             (unsigned) msg_len, MBEDTLS_SSL_DTLS_MAX_BUFFERING,
+                             (unsigned) hs->buffering.total_bytes_buffered ) );
+                    }
+
+                    if( ssl_buffer_make_space( ssl, reassembly_buf_sz ) != 0 )
+                    {
+                        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Reassembly of next message of size %u (%u with bitmap) would exceed the compile-time limit %u (already %u bytes buffered) -- fail\n",
+                             (unsigned) msg_len,
+                             (unsigned) reassembly_buf_sz,
+                             MBEDTLS_SSL_DTLS_MAX_BUFFERING,
+                             (unsigned) hs->buffering.total_bytes_buffered ) );
+                        ret = MBEDTLS_ERR_SSL_BUFFER_TOO_SMALL;
+                        goto exit;
+                    }
+                }
+
+                MBEDTLS_SSL_DEBUG_MSG( 2, ( "initialize reassembly, total length = %d",
+                                            msg_len ) );
+
+                hs_buf->data = mbedtls_calloc( 1, reassembly_buf_sz );
+                if( hs_buf->data == NULL )
+                {
+                    ret = MBEDTLS_ERR_SSL_ALLOC_FAILED;
+                    goto exit;
+                }
+                hs_buf->data_len = reassembly_buf_sz;
+
+                /* Prepare final header: copy msg_type, length and message_seq,
+                 * then add standardised fragment_offset and fragment_length */
+                memcpy( hs_buf->data, ssl->in_msg, 6 );
+                memset( hs_buf->data + 6, 0, 3 );
+                memcpy( hs_buf->data + 9, hs_buf->data + 1, 3 );
+
+                hs_buf->is_valid = 1;
+
+                hs->buffering.total_bytes_buffered += reassembly_buf_sz;
+            }
+            else
+            {
+                /* Make sure msg_type and length are consistent */
+                if( memcmp( hs_buf->data, ssl->in_msg, 4 ) != 0 )
+                {
+                    MBEDTLS_SSL_DEBUG_MSG( 1, ( "Fragment header mismatch - ignore" ) );
+                    /* Ignore */
+                    goto exit;
+                }
+            }
+
+            if( !hs_buf->is_complete )
+            {
+                size_t frag_len, frag_off;
+                unsigned char * const msg = hs_buf->data + 12;
+
+                /*
+                 * Check and copy current fragment
+                 */
+
+                /* Validation of header fields already done in
+                 * mbedtls_ssl_prepare_handshake_record(). */
+                frag_off = ssl_get_hs_frag_off( ssl );
+                frag_len = ssl_get_hs_frag_len( ssl );
+
+                MBEDTLS_SSL_DEBUG_MSG( 2, ( "adding fragment, offset = %d, length = %d",
+                                            frag_off, frag_len ) );
+                memcpy( msg + frag_off, ssl->in_msg + 12, frag_len );
+
+                if( hs_buf->is_fragmented )
+                {
+                    unsigned char * const bitmask = msg + msg_len;
+                    ssl_bitmask_set( bitmask, frag_off, frag_len );
+                    hs_buf->is_complete = ( ssl_bitmask_check( bitmask,
+                                                               msg_len ) == 0 );
+                }
+                else
+                {
+                    hs_buf->is_complete = 1;
+                }
+
+                MBEDTLS_SSL_DEBUG_MSG( 2, ( "message %scomplete",
+                                   hs_buf->is_complete ? "" : "not yet " ) );
+            }
+
+            break;
+        }
+
+        default:
+            /* We don't buffer other types of messages. */
+            break;
+    }
+
+exit:
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= ssl_buffer_message" ) );
+    return( ret );
+}
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
+static int ssl_consume_current_message( mbedtls_ssl_context *ssl )
+{
     /*
-     * Step A
-     *
      * Consume last content-layer message and potentially
      * update in_msglen which keeps track of the contents'
      * consumption state.
@@ -3936,20 +4806,161 @@ int mbedtls_ssl_read_record_layer( mbedtls_ssl_context *ssl )
         ssl->in_msglen = 0;
     }
 
-    /*
-     * Step B
-     *
-     * Fetch and decode new record if current one is fully consumed.
-     *
-     */
+    return( 0 );
+}
 
+static int ssl_record_is_in_progress( mbedtls_ssl_context *ssl )
+{
     if( ssl->in_msglen > 0 )
+        return( 1 );
+
+    return( 0 );
+}
+
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+
+static void ssl_free_buffered_record( mbedtls_ssl_context *ssl )
+{
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+    if( hs == NULL )
+        return;
+
+    if( hs->buffering.future_record.data != NULL )
     {
-        /* There's something left to be processed in the current record. */
+        hs->buffering.total_bytes_buffered -=
+            hs->buffering.future_record.len;
+
+        mbedtls_free( hs->buffering.future_record.data );
+        hs->buffering.future_record.data = NULL;
+    }
+}
+
+static int ssl_load_buffered_record( mbedtls_ssl_context *ssl )
+{
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+    unsigned char * rec;
+    size_t rec_len;
+    unsigned rec_epoch;
+
+    if( ssl->conf->transport != MBEDTLS_SSL_TRANSPORT_DATAGRAM )
         return( 0 );
+
+    if( hs == NULL )
+        return( 0 );
+
+    rec       = hs->buffering.future_record.data;
+    rec_len   = hs->buffering.future_record.len;
+    rec_epoch = hs->buffering.future_record.epoch;
+
+    if( rec == NULL )
+        return( 0 );
+
+    /* Only consider loading future records if the
+     * input buffer is empty. */
+    if( ssl_next_record_is_in_datagram( ssl ) == 1 )
+        return( 0 );
+
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> ssl_load_buffered_record" ) );
+
+    if( rec_epoch != ssl->in_epoch )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Buffered record not from current epoch." ) );
+        goto exit;
     }
 
-    /* Current record either fully processed or to be discarded. */
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "Found buffered record from current epoch - load" ) );
+
+    /* Double-check that the record is not too large */
+    if( rec_len > MBEDTLS_SSL_IN_BUFFER_LEN -
+        (size_t)( ssl->in_hdr - ssl->in_buf ) )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "should never happen" ) );
+        return( MBEDTLS_ERR_SSL_INTERNAL_ERROR );
+    }
+
+    memcpy( ssl->in_hdr, rec, rec_len );
+    ssl->in_left = rec_len;
+    ssl->next_record_offset = 0;
+
+    ssl_free_buffered_record( ssl );
+
+exit:
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= ssl_load_buffered_record" ) );
+    return( 0 );
+}
+
+static int ssl_buffer_future_record( mbedtls_ssl_context *ssl )
+{
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+    size_t const rec_hdr_len = 13;
+    size_t const total_buf_sz = rec_hdr_len + ssl->in_msglen;
+
+    /* Don't buffer future records outside handshakes. */
+    if( hs == NULL )
+        return( 0 );
+
+    /* Only buffer handshake records (we are only interested
+     * in Finished messages). */
+    if( ssl->in_msgtype != MBEDTLS_SSL_MSG_HANDSHAKE )
+        return( 0 );
+
+    /* Don't buffer more than one future epoch record. */
+    if( hs->buffering.future_record.data != NULL )
+        return( 0 );
+
+    /* Don't buffer record if there's not enough buffering space remaining. */
+    if( total_buf_sz > ( MBEDTLS_SSL_DTLS_MAX_BUFFERING -
+                         hs->buffering.total_bytes_buffered ) )
+    {
+        MBEDTLS_SSL_DEBUG_MSG( 2, ( "Buffering of future epoch record of size %u would exceed the compile-time limit %u (already %u bytes buffered) -- ignore\n",
+                        (unsigned) total_buf_sz, MBEDTLS_SSL_DTLS_MAX_BUFFERING,
+                        (unsigned) hs->buffering.total_bytes_buffered ) );
+        return( 0 );
+    }
+
+    /* Buffer record */
+    MBEDTLS_SSL_DEBUG_MSG( 2, ( "Buffer record from epoch %u",
+                                ssl->in_epoch + 1 ) );
+    MBEDTLS_SSL_DEBUG_BUF( 3, "Buffered record", ssl->in_hdr,
+                           rec_hdr_len + ssl->in_msglen );
+
+    /* ssl_parse_record_header() only considers records
+     * of the next epoch as candidates for buffering. */
+    hs->buffering.future_record.epoch = ssl->in_epoch + 1;
+    hs->buffering.future_record.len   = total_buf_sz;
+
+    hs->buffering.future_record.data =
+        mbedtls_calloc( 1, hs->buffering.future_record.len );
+    if( hs->buffering.future_record.data == NULL )
+    {
+        /* If we run out of RAM trying to buffer a
+         * record from the next epoch, just ignore. */
+        return( 0 );
+    }
+
+    memcpy( hs->buffering.future_record.data, ssl->in_hdr, total_buf_sz );
+
+    hs->buffering.total_bytes_buffered += total_buf_sz;
+    return( 0 );
+}
+
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
+static int ssl_get_next_record( mbedtls_ssl_context *ssl )
+{
+    int ret;
+
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    /* We might have buffered a future record; if so,
+     * and if the epoch matches now, load it.
+     * On success, this call will set ssl->in_left to
+     * the length of the buffered record, so that
+     * the calls to ssl_fetch_input() below will
+     * essentially be no-ops. */
+    ret = ssl_load_buffered_record( ssl );
+    if( ret != 0 )
+        return( ret );
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
 
     if( ( ret = mbedtls_ssl_fetch_input( ssl, mbedtls_ssl_hdr_len( ssl ) ) ) != 0 )
     {
@@ -3963,6 +4974,16 @@ int mbedtls_ssl_read_record_layer( mbedtls_ssl_context *ssl )
         if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
             ret != MBEDTLS_ERR_SSL_CLIENT_RECONNECT )
         {
+            if( ret == MBEDTLS_ERR_SSL_EARLY_MESSAGE )
+            {
+                ret = ssl_buffer_future_record( ssl );
+                if( ret != 0 )
+                    return( ret );
+
+                /* Fall through to handling of unexpected records */
+                ret = MBEDTLS_ERR_SSL_UNEXPECTED_RECORD;
+            }
+
             if( ret == MBEDTLS_ERR_SSL_UNEXPECTED_RECORD )
             {
                 /* Skip unexpected record (but not whole datagram) */
@@ -4094,8 +5115,51 @@ int mbedtls_ssl_handle_message_type( mbedtls_ssl_context *ssl )
         }
     }
 
+    if( ssl->in_msgtype == MBEDTLS_SSL_MSG_CHANGE_CIPHER_SPEC )
+    {
+        if( ssl->in_msglen != 1 )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "invalid CCS message, len: %d",
+                           ssl->in_msglen ) );
+            return( MBEDTLS_ERR_SSL_INVALID_RECORD );
+        }
+
+        if( ssl->in_msg[0] != 1 )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "invalid CCS message, content: %02x",
+                                        ssl->in_msg[0] ) );
+            return( MBEDTLS_ERR_SSL_INVALID_RECORD );
+        }
+
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+        if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
+            ssl->state != MBEDTLS_SSL_CLIENT_CHANGE_CIPHER_SPEC    &&
+            ssl->state != MBEDTLS_SSL_SERVER_CHANGE_CIPHER_SPEC )
+        {
+            if( ssl->handshake == NULL )
+            {
+                MBEDTLS_SSL_DEBUG_MSG( 1, ( "dropping ChangeCipherSpec outside handshake" ) );
+                return( MBEDTLS_ERR_SSL_UNEXPECTED_RECORD );
+            }
+
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "received out-of-order ChangeCipherSpec - remember" ) );
+            return( MBEDTLS_ERR_SSL_EARLY_MESSAGE );
+        }
+#endif
+    }
+
     if( ssl->in_msgtype == MBEDTLS_SSL_MSG_ALERT )
     {
+        if( ssl->in_msglen != 2 )
+        {
+            /* Note: Standard allows for more than one 2 byte alert
+               to be packed in a single message, but Mbed TLS doesn't
+               currently support this. */
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "invalid alert message, len: %d",
+                           ssl->in_msglen ) );
+            return( MBEDTLS_ERR_SSL_INVALID_RECORD );
+        }
+
         MBEDTLS_SSL_DEBUG_MSG( 2, ( "got an alert message, type: [%d:%d]",
                        ssl->in_msg[0], ssl->in_msg[1] ) );
 
@@ -4185,7 +5249,7 @@ int mbedtls_ssl_send_alert_message( mbedtls_ssl_context *ssl,
     ssl->out_msg[0] = level;
     ssl->out_msg[1] = message;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_record( ssl, SSL_FORCE_FLUSH ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
         return( ret );
@@ -4325,10 +5389,10 @@ int mbedtls_ssl_write_certificate( mbedtls_ssl_context *ssl )
     while( crt != NULL )
     {
         n = crt->raw.len;
-        if( n > MBEDTLS_SSL_MAX_CONTENT_LEN - 3 - i )
+        if( n > MBEDTLS_SSL_OUT_CONTENT_LEN - 3 - i )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "certificate too large, %d > %d",
-                           i + 3 + n, MBEDTLS_SSL_MAX_CONTENT_LEN ) );
+                           i + 3 + n, MBEDTLS_SSL_OUT_CONTENT_LEN ) );
             return( MBEDTLS_ERR_SSL_CERTIFICATE_TOO_LARGE );
         }
 
@@ -4354,9 +5418,9 @@ write_msg:
 
     ssl->state++;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
@@ -4478,6 +5542,12 @@ static int ssl_parse_certificate_chain( mbedtls_ssl_context *ssl )
 
     while( i < ssl->in_hslen )
     {
+        if ( i + 3 > ssl->in_hslen ) {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad certificate message" ) );
+            mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
+                                           MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
+            return( MBEDTLS_ERR_SSL_BAD_HS_CERTIFICATE );
+        }
         if( ssl->in_msg[i] != 0 )
         {
             MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad certificate message" ) );
@@ -4616,7 +5686,7 @@ int mbedtls_ssl_parse_certificate( mbedtls_ssl_context *ssl )
     }
 #endif
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         /* mbedtls_ssl_read_record may have sent an alert already. We
            let it decide whether to alert. */
@@ -4808,9 +5878,9 @@ int mbedtls_ssl_write_change_cipher_spec( mbedtls_ssl_context *ssl )
 
     ssl->state++;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
@@ -4825,7 +5895,7 @@ int mbedtls_ssl_parse_change_cipher_spec( mbedtls_ssl_context *ssl )
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> parse change cipher spec" ) );
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
         return( ret );
@@ -4839,13 +5909,8 @@ int mbedtls_ssl_parse_change_cipher_spec( mbedtls_ssl_context *ssl )
         return( MBEDTLS_ERR_SSL_UNEXPECTED_MESSAGE );
     }
 
-    if( ssl->in_msglen != 1 || ssl->in_msg[0] != 1 )
-    {
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "bad change cipher spec message" ) );
-        mbedtls_ssl_send_alert_message( ssl, MBEDTLS_SSL_ALERT_LEVEL_FATAL,
-                                        MBEDTLS_SSL_ALERT_MSG_DECODE_ERROR );
-        return( MBEDTLS_ERR_SSL_BAD_HS_CHANGE_CIPHER_SPEC );
-    }
+    /* CCS records are only accepted if they have length 1 and content '1',
+     * so we don't need to check this here. */
 
     /*
      * Switch to our negotiated transform and session parameters for inbound
@@ -4875,16 +5940,7 @@ int mbedtls_ssl_parse_change_cipher_spec( mbedtls_ssl_context *ssl )
 #endif /* MBEDTLS_SSL_PROTO_DTLS */
     memset( ssl->in_ctr, 0, 8 );
 
-    /*
-     * Set the in_msg pointer to the correct location based on IV length
-     */
-    if( ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_2 )
-    {
-        ssl->in_msg = ssl->in_iv + ssl->transform_negotiate->ivlen -
-                      ssl->transform_negotiate->fixed_ivlen;
-    }
-    else
-        ssl->in_msg = ssl->in_iv;
+    ssl_update_in_pointers( ssl, ssl->transform_negotiate );
 
 #if defined(MBEDTLS_SSL_HW_RECORD_ACCEL)
     if( mbedtls_ssl_hw_record_activate != NULL )
@@ -5335,16 +6391,7 @@ int mbedtls_ssl_write_finished( mbedtls_ssl_context *ssl )
 
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "=> write finished" ) );
 
-    /*
-     * Set the out_msg pointer to the correct location based on IV length
-     */
-    if( ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_2 )
-    {
-        ssl->out_msg = ssl->out_iv + ssl->transform_negotiate->ivlen -
-                       ssl->transform_negotiate->fixed_ivlen;
-    }
-    else
-        ssl->out_msg = ssl->out_iv;
+    ssl_update_out_pointers( ssl, ssl->transform_negotiate );
 
     ssl->handshake->calc_finished( ssl, ssl->out_msg + 4, ssl->conf->endpoint );
 
@@ -5396,14 +6443,14 @@ int mbedtls_ssl_write_finished( mbedtls_ssl_context *ssl )
 
         /* Remember current epoch settings for resending */
         ssl->handshake->alt_transform_out = ssl->transform_out;
-        memcpy( ssl->handshake->alt_out_ctr, ssl->out_ctr, 8 );
+        memcpy( ssl->handshake->alt_out_ctr, ssl->cur_out_ctr, 8 );
 
         /* Set sequence_number to zero */
-        memset( ssl->out_ctr + 2, 0, 6 );
+        memset( ssl->cur_out_ctr + 2, 0, 6 );
 
         /* Increment epoch */
         for( i = 2; i > 0; i-- )
-            if( ++ssl->out_ctr[i - 1] != 0 )
+            if( ++ssl->cur_out_ctr[i - 1] != 0 )
                 break;
 
         /* The loop goes to its end iff the counter is wrapping */
@@ -5415,7 +6462,7 @@ int mbedtls_ssl_write_finished( mbedtls_ssl_context *ssl )
     }
     else
 #endif /* MBEDTLS_SSL_PROTO_DTLS */
-    memset( ssl->out_ctr, 0, 8 );
+    memset( ssl->cur_out_ctr, 0, 8 );
 
     ssl->transform_out = ssl->transform_negotiate;
     ssl->session_out = ssl->session_negotiate;
@@ -5436,12 +6483,21 @@ int mbedtls_ssl_write_finished( mbedtls_ssl_context *ssl )
         mbedtls_ssl_send_flight_completed( ssl );
 #endif
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM &&
+        ( ret = mbedtls_ssl_flight_transmit( ssl ) ) != 0 )
+    {
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_flight_transmit", ret );
+        return( ret );
+    }
+#endif
+
     MBEDTLS_SSL_DEBUG_MSG( 2, ( "<= write finished" ) );
 
     return( 0 );
@@ -5463,7 +6519,7 @@ int mbedtls_ssl_parse_finished( mbedtls_ssl_context *ssl )
 
     ssl->handshake->calc_finished( ssl, buf, ssl->conf->endpoint ^ 1 );
 
-    if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
     {
         MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_read_record", ret );
         return( ret );
@@ -5698,6 +6754,78 @@ static int ssl_cookie_check_dummy( void *ctx,
 }
 #endif /* MBEDTLS_SSL_DTLS_HELLO_VERIFY && MBEDTLS_SSL_SRV_C */
 
+/* Once ssl->out_hdr as the address of the beginning of the
+ * next outgoing record is set, deduce the other pointers.
+ *
+ * Note: For TLS, we save the implicit record sequence number
+ *       (entering MAC computation) in the 8 bytes before ssl->out_hdr,
+ *       and the caller has to make sure there's space for this.
+ */
+
+static void ssl_update_out_pointers( mbedtls_ssl_context *ssl,
+                                     mbedtls_ssl_transform *transform )
+{
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        ssl->out_ctr = ssl->out_hdr +  3;
+        ssl->out_len = ssl->out_hdr + 11;
+        ssl->out_iv  = ssl->out_hdr + 13;
+    }
+    else
+#endif
+    {
+        ssl->out_ctr = ssl->out_hdr - 8;
+        ssl->out_len = ssl->out_hdr + 3;
+        ssl->out_iv  = ssl->out_hdr + 5;
+    }
+
+    /* Adjust out_msg to make space for explicit IV, if used. */
+    if( transform != NULL &&
+        ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_2 )
+    {
+        ssl->out_msg = ssl->out_iv + transform->ivlen - transform->fixed_ivlen;
+    }
+    else
+        ssl->out_msg = ssl->out_iv;
+}
+
+/* Once ssl->in_hdr as the address of the beginning of the
+ * next incoming record is set, deduce the other pointers.
+ *
+ * Note: For TLS, we save the implicit record sequence number
+ *       (entering MAC computation) in the 8 bytes before ssl->in_hdr,
+ *       and the caller has to make sure there's space for this.
+ */
+
+static void ssl_update_in_pointers( mbedtls_ssl_context *ssl,
+                                    mbedtls_ssl_transform *transform )
+{
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        ssl->in_ctr = ssl->in_hdr +  3;
+        ssl->in_len = ssl->in_hdr + 11;
+        ssl->in_iv  = ssl->in_hdr + 13;
+    }
+    else
+#endif
+    {
+        ssl->in_ctr = ssl->in_hdr - 8;
+        ssl->in_len = ssl->in_hdr + 3;
+        ssl->in_iv  = ssl->in_hdr + 5;
+    }
+
+    /* Offset in_msg from in_iv to allow space for explicit IV, if used. */
+    if( transform != NULL &&
+        ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_2 )
+    {
+        ssl->in_msg = ssl->in_iv + transform->ivlen - transform->fixed_ivlen;
+    }
+    else
+        ssl->in_msg = ssl->in_iv;
+}
+
 /*
  * Initialize an SSL context
  */
@@ -5709,61 +6837,87 @@ void mbedtls_ssl_init( mbedtls_ssl_context *ssl )
 /*
  * Setup an SSL context
  */
+
+static void ssl_reset_in_out_pointers( mbedtls_ssl_context *ssl )
+{
+    /* Set the incoming and outgoing record pointers. */
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl->conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    {
+        ssl->out_hdr = ssl->out_buf;
+        ssl->in_hdr  = ssl->in_buf;
+    }
+    else
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+    {
+        ssl->out_hdr = ssl->out_buf + 8;
+        ssl->in_hdr  = ssl->in_buf  + 8;
+    }
+
+    /* Derive other internal pointers. */
+    ssl_update_out_pointers( ssl, NULL /* no transform enabled */ );
+    ssl_update_in_pointers ( ssl, NULL /* no transform enabled */ );
+}
+
 int mbedtls_ssl_setup( mbedtls_ssl_context *ssl,
                        const mbedtls_ssl_config *conf )
 {
     int ret;
-    const size_t len = MBEDTLS_SSL_BUFFER_LEN;
 
     ssl->conf = conf;
 
     /*
      * Prepare base structures
      */
-    if( ( ssl-> in_buf = mbedtls_calloc( 1, len ) ) == NULL ||
-        ( ssl->out_buf = mbedtls_calloc( 1, len ) ) == NULL )
+
+    /* Set to NULL in case of an error condition */
+    ssl->out_buf = NULL;
+
+    ssl->in_buf = mbedtls_calloc( 1, MBEDTLS_SSL_IN_BUFFER_LEN );
+    if( ssl->in_buf == NULL )
     {
-        MBEDTLS_SSL_DEBUG_MSG( 1, ( "alloc(%d bytes) failed", len ) );
-        mbedtls_free( ssl->in_buf );
-        ssl->in_buf = NULL;
-        return( MBEDTLS_ERR_SSL_ALLOC_FAILED );
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "alloc(%d bytes) failed", MBEDTLS_SSL_IN_BUFFER_LEN) );
+        ret = MBEDTLS_ERR_SSL_ALLOC_FAILED;
+        goto error;
     }
 
-#if defined(MBEDTLS_SSL_PROTO_DTLS)
-    if( conf->transport == MBEDTLS_SSL_TRANSPORT_DATAGRAM )
+    ssl->out_buf = mbedtls_calloc( 1, MBEDTLS_SSL_OUT_BUFFER_LEN );
+    if( ssl->out_buf == NULL )
     {
-        ssl->out_hdr = ssl->out_buf;
-        ssl->out_ctr = ssl->out_buf +  3;
-        ssl->out_len = ssl->out_buf + 11;
-        ssl->out_iv  = ssl->out_buf + 13;
-        ssl->out_msg = ssl->out_buf + 13;
-
-        ssl->in_hdr = ssl->in_buf;
-        ssl->in_ctr = ssl->in_buf +  3;
-        ssl->in_len = ssl->in_buf + 11;
-        ssl->in_iv  = ssl->in_buf + 13;
-        ssl->in_msg = ssl->in_buf + 13;
+        MBEDTLS_SSL_DEBUG_MSG( 1, ( "alloc(%d bytes) failed", MBEDTLS_SSL_OUT_BUFFER_LEN) );
+        ret = MBEDTLS_ERR_SSL_ALLOC_FAILED;
+        goto error;
     }
-    else
-#endif
-    {
-        ssl->out_ctr = ssl->out_buf;
-        ssl->out_hdr = ssl->out_buf +  8;
-        ssl->out_len = ssl->out_buf + 11;
-        ssl->out_iv  = ssl->out_buf + 13;
-        ssl->out_msg = ssl->out_buf + 13;
 
-        ssl->in_ctr = ssl->in_buf;
-        ssl->in_hdr = ssl->in_buf +  8;
-        ssl->in_len = ssl->in_buf + 11;
-        ssl->in_iv  = ssl->in_buf + 13;
-        ssl->in_msg = ssl->in_buf + 13;
-    }
+    ssl_reset_in_out_pointers( ssl );
 
     if( ( ret = ssl_handshake_init( ssl ) ) != 0 )
-        return( ret );
+        goto error;
 
     return( 0 );
+
+error:
+    mbedtls_free( ssl->in_buf );
+    mbedtls_free( ssl->out_buf );
+
+    ssl->conf = NULL;
+
+    ssl->in_buf = NULL;
+    ssl->out_buf = NULL;
+
+    ssl->in_hdr = NULL;
+    ssl->in_ctr = NULL;
+    ssl->in_len = NULL;
+    ssl->in_iv = NULL;
+    ssl->in_msg = NULL;
+
+    ssl->out_hdr = NULL;
+    ssl->out_ctr = NULL;
+    ssl->out_len = NULL;
+    ssl->out_iv = NULL;
+    ssl->out_msg = NULL;
+
+    return( ret );
 }
 
 /*
@@ -5777,6 +6931,11 @@ static int ssl_session_reset_int( mbedtls_ssl_context *ssl, int partial )
 {
     int ret;
 
+#if !defined(MBEDTLS_SSL_DTLS_CLIENT_PORT_REUSE) ||     \
+    !defined(MBEDTLS_SSL_SRV_C)
+    ((void) partial);
+#endif
+
     ssl->state = MBEDTLS_SSL_HELLO_REQUEST;
 
     /* Cancel any possibly running timer */
@@ -5793,12 +6952,10 @@ static int ssl_session_reset_int( mbedtls_ssl_context *ssl, int partial )
     ssl->secure_renegotiation = MBEDTLS_SSL_LEGACY_RENEGOTIATION;
 
     ssl->in_offt = NULL;
+    ssl_reset_in_out_pointers( ssl );
 
-    ssl->in_msg = ssl->in_buf + 13;
     ssl->in_msgtype = 0;
     ssl->in_msglen = 0;
-    if( partial == 0 )
-        ssl->in_left = 0;
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
     ssl->next_record_offset = 0;
     ssl->in_epoch = 0;
@@ -5812,7 +6969,6 @@ static int ssl_session_reset_int( mbedtls_ssl_context *ssl, int partial )
 
     ssl->keep_current_message = 0;
 
-    ssl->out_msg = ssl->out_buf + 13;
     ssl->out_msgtype = 0;
     ssl->out_msglen = 0;
     ssl->out_left = 0;
@@ -5821,12 +6977,23 @@ static int ssl_session_reset_int( mbedtls_ssl_context *ssl, int partial )
         ssl->split_done = 0;
 #endif
 
+    memset( ssl->cur_out_ctr, 0, sizeof( ssl->cur_out_ctr ) );
+
     ssl->transform_in = NULL;
     ssl->transform_out = NULL;
 
-    memset( ssl->out_buf, 0, MBEDTLS_SSL_BUFFER_LEN );
+    ssl->session_in = NULL;
+    ssl->session_out = NULL;
+
+    memset( ssl->out_buf, 0, MBEDTLS_SSL_OUT_BUFFER_LEN );
+
+#if defined(MBEDTLS_SSL_DTLS_CLIENT_PORT_REUSE) && defined(MBEDTLS_SSL_SRV_C)
     if( partial == 0 )
-        memset( ssl->in_buf, 0, MBEDTLS_SSL_BUFFER_LEN );
+#endif /* MBEDTLS_SSL_DTLS_CLIENT_PORT_REUSE && MBEDTLS_SSL_SRV_C */
+    {
+        ssl->in_left = 0;
+        memset( ssl->in_buf, 0, MBEDTLS_SSL_IN_BUFFER_LEN );
+    }
 
 #if defined(MBEDTLS_SSL_HW_RECORD_ACCEL)
     if( mbedtls_ssl_hw_record_reset != NULL )
@@ -5859,7 +7026,9 @@ static int ssl_session_reset_int( mbedtls_ssl_context *ssl, int partial )
 #endif
 
 #if defined(MBEDTLS_SSL_DTLS_HELLO_VERIFY) && defined(MBEDTLS_SSL_SRV_C)
+#if defined(MBEDTLS_SSL_DTLS_CLIENT_PORT_REUSE)
     if( partial == 0 )
+#endif
     {
         mbedtls_free( ssl->cli_id );
         ssl->cli_id = NULL;
@@ -5910,7 +7079,15 @@ void mbedtls_ssl_conf_dtls_badmac_limit( mbedtls_ssl_config *conf, unsigned limi
 #endif
 
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
-void mbedtls_ssl_conf_handshake_timeout( mbedtls_ssl_config *conf, uint32_t min, uint32_t max )
+
+void mbedtls_ssl_set_datagram_packing( mbedtls_ssl_context *ssl,
+                                       unsigned allow_packing )
+{
+    ssl->disable_datagram_packing = !allow_packing;
+}
+
+void mbedtls_ssl_conf_handshake_timeout( mbedtls_ssl_config *conf,
+                                         uint32_t min, uint32_t max )
 {
     conf->hs_timeout_min = min;
     conf->hs_timeout_max = max;
@@ -5960,6 +7137,13 @@ void mbedtls_ssl_set_bio( mbedtls_ssl_context *ssl,
     ssl->f_recv_timeout = f_recv_timeout;
 }
 
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+void mbedtls_ssl_set_mtu( mbedtls_ssl_context *ssl, uint16_t mtu )
+{
+    ssl->mtu = mtu;
+}
+#endif
+
 void mbedtls_ssl_conf_read_timeout( mbedtls_ssl_config *conf, uint32_t timeout )
 {
     conf->read_timeout   = timeout;
@@ -6046,27 +7230,27 @@ static int ssl_append_key_cert( mbedtls_ssl_key_cert **head,
                                 mbedtls_x509_crt *cert,
                                 mbedtls_pk_context *key )
 {
-    mbedtls_ssl_key_cert *new;
+    mbedtls_ssl_key_cert *new_cert;
 
-    new = mbedtls_calloc( 1, sizeof( mbedtls_ssl_key_cert ) );
-    if( new == NULL )
+    new_cert = mbedtls_calloc( 1, sizeof( mbedtls_ssl_key_cert ) );
+    if( new_cert == NULL )
         return( MBEDTLS_ERR_SSL_ALLOC_FAILED );
 
-    new->cert = cert;
-    new->key  = key;
-    new->next = NULL;
+    new_cert->cert = cert;
+    new_cert->key  = key;
+    new_cert->next = NULL;
 
     /* Update head is the list was null, else add to the end */
     if( *head == NULL )
     {
-        *head = new;
+        *head = new_cert;
     }
     else
     {
         mbedtls_ssl_key_cert *cur = *head;
         while( cur->next != NULL )
             cur = cur->next;
-        cur->next = new;
+        cur->next = new_cert;
     }
 
     return( 0 );
@@ -6151,7 +7335,7 @@ int mbedtls_ssl_conf_psk( mbedtls_ssl_config *conf,
 
     /* Identity len will be encoded on two bytes */
     if( ( psk_identity_len >> 16 ) != 0 ||
-        psk_identity_len > MBEDTLS_SSL_MAX_CONTENT_LEN )
+        psk_identity_len > MBEDTLS_SSL_OUT_CONTENT_LEN )
     {
         return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
     }
@@ -6452,7 +7636,7 @@ void mbedtls_ssl_conf_arc4_support( mbedtls_ssl_config *conf, char arc4 )
 int mbedtls_ssl_conf_max_frag_len( mbedtls_ssl_config *conf, unsigned char mfl_code )
 {
     if( mfl_code >= MBEDTLS_SSL_MAX_FRAG_LEN_INVALID ||
-        mfl_code_to_length[mfl_code] > MBEDTLS_SSL_MAX_CONTENT_LEN )
+        ssl_mfl_code_to_length( mfl_code ) > MBEDTLS_TLS_EXT_ADV_CONTENT_LEN )
     {
         return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
     }
@@ -6623,7 +7807,7 @@ int mbedtls_ssl_check_pending( const mbedtls_ssl_context *ssl )
 
     /*
      * In all other cases, the rest of the message can be dropped.
-     * As in ssl_read_record_layer, this needs to be adapted if
+     * As in ssl_get_next_record, this needs to be adapted if
      * we implement support for multiple alerts in single records.
      */
 
@@ -6690,28 +7874,47 @@ const char *mbedtls_ssl_get_version( const mbedtls_ssl_context *ssl )
 
 int mbedtls_ssl_get_record_expansion( const mbedtls_ssl_context *ssl )
 {
-    size_t transform_expansion;
+    size_t transform_expansion = 0;
     const mbedtls_ssl_transform *transform = ssl->transform_out;
+    unsigned block_size;
+
+    if( transform == NULL )
+        return( (int) mbedtls_ssl_hdr_len( ssl ) );
 
 #if defined(MBEDTLS_ZLIB_SUPPORT)
     if( ssl->session_out->compression != MBEDTLS_SSL_COMPRESS_NULL )
         return( MBEDTLS_ERR_SSL_FEATURE_UNAVAILABLE );
 #endif
 
-    if( transform == NULL )
-        return( (int) mbedtls_ssl_hdr_len( ssl ) );
-
     switch( mbedtls_cipher_get_cipher_mode( &transform->cipher_ctx_enc ) )
     {
         case MBEDTLS_MODE_GCM:
         case MBEDTLS_MODE_CCM:
+        case MBEDTLS_MODE_CHACHAPOLY:
         case MBEDTLS_MODE_STREAM:
             transform_expansion = transform->minlen;
             break;
 
         case MBEDTLS_MODE_CBC:
-            transform_expansion = transform->maclen
-                      + mbedtls_cipher_get_block_size( &transform->cipher_ctx_enc );
+
+            block_size = mbedtls_cipher_get_block_size(
+                &transform->cipher_ctx_enc );
+
+            /* Expansion due to the addition of the MAC. */
+            transform_expansion += transform->maclen;
+
+            /* Expansion due to the addition of CBC padding;
+             * Theoretically up to 256 bytes, but we never use
+             * more than the block size of the underlying cipher. */
+            transform_expansion += block_size;
+
+            /* For TLS 1.1 or higher, an explicit IV is added
+             * after the record header. */
+#if defined(MBEDTLS_SSL_PROTO_TLS1_1) || defined(MBEDTLS_SSL_PROTO_TLS1_2)
+            if( ssl->minor_ver >= MBEDTLS_SSL_MINOR_VERSION_2 )
+                transform_expansion += block_size;
+#endif /* MBEDTLS_SSL_PROTO_TLS1_1 || MBEDTLS_SSL_PROTO_TLS1_2 */
+
             break;
 
         default:
@@ -6730,21 +7933,85 @@ size_t mbedtls_ssl_get_max_frag_len( const mbedtls_ssl_context *ssl )
     /*
      * Assume mfl_code is correct since it was checked when set
      */
-    max_len = mfl_code_to_length[ssl->conf->mfl_code];
+    max_len = ssl_mfl_code_to_length( ssl->conf->mfl_code );
 
-    /*
-     * Check if a smaller max length was negotiated
-     */
+    /* Check if a smaller max length was negotiated */
     if( ssl->session_out != NULL &&
-        mfl_code_to_length[ssl->session_out->mfl_code] < max_len )
+        ssl_mfl_code_to_length( ssl->session_out->mfl_code ) < max_len )
     {
-        max_len = mfl_code_to_length[ssl->session_out->mfl_code];
+        max_len = ssl_mfl_code_to_length( ssl->session_out->mfl_code );
     }
 
-    return max_len;
+    /* During a handshake, use the value being negotiated */
+    if( ssl->session_negotiate != NULL &&
+        ssl_mfl_code_to_length( ssl->session_negotiate->mfl_code ) < max_len )
+    {
+        max_len = ssl_mfl_code_to_length( ssl->session_negotiate->mfl_code );
+    }
+
+    return( max_len );
 }
 #endif /* MBEDTLS_SSL_MAX_FRAGMENT_LENGTH */
 
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+static size_t ssl_get_current_mtu( const mbedtls_ssl_context *ssl )
+{
+    if( ssl->handshake == NULL || ssl->handshake->mtu == 0 )
+        return( ssl->mtu );
+
+    if( ssl->mtu == 0 )
+        return( ssl->handshake->mtu );
+
+    return( ssl->mtu < ssl->handshake->mtu ?
+            ssl->mtu : ssl->handshake->mtu );
+}
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
+int mbedtls_ssl_get_max_out_record_payload( const mbedtls_ssl_context *ssl )
+{
+    size_t max_len = MBEDTLS_SSL_OUT_CONTENT_LEN;
+
+#if !defined(MBEDTLS_SSL_MAX_FRAGMENT_LENGTH) && \
+    !defined(MBEDTLS_SSL_PROTO_DTLS)
+    (void) ssl;
+#endif
+
+#if defined(MBEDTLS_SSL_MAX_FRAGMENT_LENGTH)
+    const size_t mfl = mbedtls_ssl_get_max_frag_len( ssl );
+
+    if( max_len > mfl )
+        max_len = mfl;
+#endif
+
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+    if( ssl_get_current_mtu( ssl ) != 0 )
+    {
+        const size_t mtu = ssl_get_current_mtu( ssl );
+        const int ret = mbedtls_ssl_get_record_expansion( ssl );
+        const size_t overhead = (size_t) ret;
+
+        if( ret < 0 )
+            return( ret );
+
+        if( mtu <= overhead )
+        {
+            MBEDTLS_SSL_DEBUG_MSG( 1, ( "MTU too low for record expansion" ) );
+            return( MBEDTLS_ERR_SSL_FEATURE_UNAVAILABLE );
+        }
+
+        if( max_len > mtu - overhead )
+            max_len = mtu - overhead;
+    }
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
+#if !defined(MBEDTLS_SSL_MAX_FRAGMENT_LENGTH) &&        \
+    !defined(MBEDTLS_SSL_PROTO_DTLS)
+    ((void) ssl);
+#endif
+
+    return( (int) max_len );
+}
+
 #if defined(MBEDTLS_X509_CRT_PARSE_C)
 const mbedtls_x509_crt *mbedtls_ssl_get_peer_cert( const mbedtls_ssl_context *ssl )
 {
@@ -6832,9 +8099,9 @@ static int ssl_write_hello_request( mbedtls_ssl_context *ssl )
     ssl->out_msgtype = MBEDTLS_SSL_MSG_HANDSHAKE;
     ssl->out_msg[0]  = MBEDTLS_SSL_HS_HELLO_REQUEST;
 
-    if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+    if( ( ret = mbedtls_ssl_write_handshake_msg( ssl ) ) != 0 )
     {
-        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_handshake_msg", ret );
         return( ret );
     }
 
@@ -6964,7 +8231,7 @@ static int ssl_check_ctr_renegotiate( mbedtls_ssl_context *ssl )
 
     in_ctr_cmp = memcmp( ssl->in_ctr + ep_len,
                         ssl->conf->renego_period + ep_len, 8 - ep_len );
-    out_ctr_cmp = memcmp( ssl->out_ctr + ep_len,
+    out_ctr_cmp = memcmp( ssl->cur_out_ctr + ep_len,
                           ssl->conf->renego_period + ep_len, 8 - ep_len );
 
     if( in_ctr_cmp <= 0 && out_ctr_cmp <= 0 )
@@ -6999,7 +8266,7 @@ int mbedtls_ssl_read( mbedtls_ssl_context *ssl, unsigned char *buf, size_t len )
         if( ssl->handshake != NULL &&
             ssl->handshake->retransmit_state == MBEDTLS_SSL_RETRANS_SENDING )
         {
-            if( ( ret = mbedtls_ssl_resend( ssl ) ) != 0 )
+            if( ( ret = mbedtls_ssl_flight_transmit( ssl ) ) != 0 )
                 return( ret );
         }
     }
@@ -7048,7 +8315,7 @@ int mbedtls_ssl_read( mbedtls_ssl_context *ssl, unsigned char *buf, size_t len )
             ssl_set_timer( ssl, ssl->conf->read_timeout );
         }
 
-        if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+        if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
         {
             if( ret == MBEDTLS_ERR_SSL_CONN_EOF )
                 return( 0 );
@@ -7063,7 +8330,7 @@ int mbedtls_ssl_read( mbedtls_ssl_context *ssl, unsigned char *buf, size_t len )
             /*
              * OpenSSL sends empty messages to randomize the IV
              */
-            if( ( ret = mbedtls_ssl_read_record( ssl ) ) != 0 )
+            if( ( ret = mbedtls_ssl_read_record( ssl, 1 ) ) != 0 )
             {
                 if( ret == MBEDTLS_ERR_SSL_CONN_EOF )
                     return( 0 );
@@ -7282,18 +8549,29 @@ int mbedtls_ssl_read( mbedtls_ssl_context *ssl, unsigned char *buf, size_t len )
 }
 
 /*
- * Send application data to be encrypted by the SSL layer,
- * taking care of max fragment length and buffer size
+ * Send application data to be encrypted by the SSL layer, taking care of max
+ * fragment length and buffer size.
+ *
+ * According to RFC 5246 Section 6.2.1:
+ *
+ *      Zero-length fragments of Application data MAY be sent as they are
+ *      potentially useful as a traffic analysis countermeasure.
+ *
+ * Therefore, it is possible that the input message length is 0 and the
+ * corresponding return code is 0 on success.
  */
 static int ssl_write_real( mbedtls_ssl_context *ssl,
                            const unsigned char *buf, size_t len )
 {
-    int ret;
-#if defined(MBEDTLS_SSL_MAX_FRAGMENT_LENGTH)
-    size_t max_len = mbedtls_ssl_get_max_frag_len( ssl );
-#else
-    size_t max_len = MBEDTLS_SSL_MAX_CONTENT_LEN;
-#endif /* MBEDTLS_SSL_MAX_FRAGMENT_LENGTH */
+    int ret = mbedtls_ssl_get_max_out_record_payload( ssl );
+    const size_t max_len = (size_t) ret;
+
+    if( ret < 0 )
+    {
+        MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_get_max_out_record_payload", ret );
+        return( ret );
+    }
+
     if( len > max_len )
     {
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
@@ -7311,6 +8589,12 @@ static int ssl_write_real( mbedtls_ssl_context *ssl,
 
     if( ssl->out_left != 0 )
     {
+        /*
+         * The user has previously tried to send the data and
+         * MBEDTLS_ERR_SSL_WANT_WRITE or the message was only partially
+         * written. In this case, we expect the high-level write function
+         * (e.g. mbedtls_ssl_write()) to be called with the same parameters
+         */
         if( ( ret = mbedtls_ssl_flush_output( ssl ) ) != 0 )
         {
             MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_flush_output", ret );
@@ -7319,11 +8603,16 @@ static int ssl_write_real( mbedtls_ssl_context *ssl,
     }
     else
     {
+        /*
+         * The user is trying to send a message the first time, so we need to
+         * copy the data into the internal buffers and setup the data structure
+         * to keep track of partial writes
+         */
         ssl->out_msglen  = len;
         ssl->out_msgtype = MBEDTLS_SSL_MSG_APPLICATION_DATA;
         memcpy( ssl->out_msg, buf, len );
 
-        if( ( ret = mbedtls_ssl_write_record( ssl ) ) != 0 )
+        if( ( ret = mbedtls_ssl_write_record( ssl, SSL_FORCE_FLUSH ) ) != 0 )
         {
             MBEDTLS_SSL_DEBUG_RET( 1, "mbedtls_ssl_write_record", ret );
             return( ret );
@@ -7475,6 +8764,41 @@ static void ssl_key_cert_free( mbedtls_ssl_key_cert *key_cert )
 }
 #endif /* MBEDTLS_X509_CRT_PARSE_C */
 
+#if defined(MBEDTLS_SSL_PROTO_DTLS)
+
+static void ssl_buffering_free( mbedtls_ssl_context *ssl )
+{
+    unsigned offset;
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+
+    if( hs == NULL )
+        return;
+
+    ssl_free_buffered_record( ssl );
+
+    for( offset = 0; offset < MBEDTLS_SSL_MAX_BUFFERED_HS; offset++ )
+        ssl_buffering_free_slot( ssl, offset );
+}
+
+static void ssl_buffering_free_slot( mbedtls_ssl_context *ssl,
+                                     uint8_t slot )
+{
+    mbedtls_ssl_handshake_params * const hs = ssl->handshake;
+    mbedtls_ssl_hs_buffer * const hs_buf = &hs->buffering.hs[slot];
+
+    if( slot >= MBEDTLS_SSL_MAX_BUFFERED_HS )
+        return;
+
+    if( hs_buf->is_valid == 1 )
+    {
+        hs->buffering.total_bytes_buffered -= hs_buf->data_len;
+        mbedtls_free( hs_buf->data );
+        memset( hs_buf, 0, sizeof( mbedtls_ssl_hs_buffer ) );
+    }
+}
+
+#endif /* MBEDTLS_SSL_PROTO_DTLS */
+
 void mbedtls_ssl_handshake_free( mbedtls_ssl_context *ssl )
 {
     mbedtls_ssl_handshake_params *handshake = ssl->handshake;
@@ -7558,8 +8882,8 @@ void mbedtls_ssl_handshake_free( mbedtls_ssl_context *ssl )
 
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
     mbedtls_free( handshake->verify_cookie );
-    mbedtls_free( handshake->hs_msg );
     ssl_flight_free( handshake->flight );
+    ssl_buffering_free( ssl );
 #endif
 
     mbedtls_platform_zeroize( handshake,
@@ -7598,20 +8922,20 @@ void mbedtls_ssl_free( mbedtls_ssl_context *ssl )
 
     if( ssl->out_buf != NULL )
     {
-        mbedtls_platform_zeroize( ssl->out_buf, MBEDTLS_SSL_BUFFER_LEN );
+        mbedtls_platform_zeroize( ssl->out_buf, MBEDTLS_SSL_OUT_BUFFER_LEN );
         mbedtls_free( ssl->out_buf );
     }
 
     if( ssl->in_buf != NULL )
     {
-        mbedtls_platform_zeroize( ssl->in_buf, MBEDTLS_SSL_BUFFER_LEN );
+        mbedtls_platform_zeroize( ssl->in_buf, MBEDTLS_SSL_IN_BUFFER_LEN );
         mbedtls_free( ssl->in_buf );
     }
 
 #if defined(MBEDTLS_ZLIB_SUPPORT)
     if( ssl->compress_buf != NULL )
     {
-        mbedtls_platform_zeroize( ssl->compress_buf, MBEDTLS_SSL_BUFFER_LEN );
+        mbedtls_platform_zeroize( ssl->compress_buf, MBEDTLS_SSL_COMPRESS_BUFFER_LEN );
         mbedtls_free( ssl->compress_buf );
     }
 #endif
diff --git a/library/threading.c b/library/threading.c
index f1c37245..7c90c7c5 100644
--- a/library/threading.c
+++ b/library/threading.c
@@ -19,6 +19,14 @@
  *  This file is part of mbed TLS (https://tls.mbed.org)
  */
 
+/*
+ * Ensure gmtime_r is available even with -std=c99; must be defined before
+ * config.h, which pulls in glibc's features.h. Harmless on other platforms.
+ */
+#if !defined(_POSIX_C_SOURCE)
+#define _POSIX_C_SOURCE 200112L
+#endif
+
 #if !defined(MBEDTLS_CONFIG_FILE)
 #include "mbedtls/config.h"
 #else
@@ -29,6 +37,36 @@
 
 #include "mbedtls/threading.h"
 
+#if defined(MBEDTLS_HAVE_TIME_DATE) && !defined(MBEDTLS_PLATFORM_GMTIME_R_ALT)
+
+#if !defined(_WIN32) && (defined(unix) || \
+    defined(__unix) || defined(__unix__) || (defined(__APPLE__) && \
+    defined(__MACH__)))
+#include <unistd.h>
+#endif /* !_WIN32 && (unix || __unix || __unix__ ||
+        * (__APPLE__ && __MACH__)) */
+
+#if !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) ||     \
+       ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) &&                     \
+         _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) )
+/*
+ * This is a convenience shorthand macro to avoid checking the long
+ * preprocessor conditions above. Ideally, we could expose this macro in
+ * platform_util.h and simply use it in platform_util.c, threading.c and
+ * threading.h. However, this macro is not part of the Mbed TLS public API, so
+ * we keep it private by only defining it in this file
+ */
+
+#if ! ( defined(_WIN32) && !defined(EFIX64) && !defined(EFI32) )
+#define THREADING_USE_GMTIME
+#endif /* ! ( defined(_WIN32) && !defined(EFIX64) && !defined(EFI32) ) */
+
+#endif /* !( ( defined(_POSIX_VERSION) && _POSIX_VERSION >= 200809L ) ||     \
+             ( defined(_POSIX_THREAD_SAFE_FUNCTIONS ) &&                     \
+                _POSIX_THREAD_SAFE_FUNCTIONS >= 20112L ) ) */
+
+#endif /* MBEDTLS_HAVE_TIME_DATE && !MBEDTLS_PLATFORM_GMTIME_R_ALT */
+
 #if defined(MBEDTLS_THREADING_PTHREAD)
 static void threading_mutex_init_pthread( mbedtls_threading_mutex_t *mutex )
 {
@@ -114,7 +152,7 @@ void mbedtls_threading_set_alt( void (*mutex_init)( mbedtls_threading_mutex_t *
 #if defined(MBEDTLS_FS_IO)
     mbedtls_mutex_init( &mbedtls_threading_readdir_mutex );
 #endif
-#if defined(MBEDTLS_HAVE_TIME_DATE)
+#if defined(THREADING_USE_GMTIME)
     mbedtls_mutex_init( &mbedtls_threading_gmtime_mutex );
 #endif
 }
@@ -127,7 +165,7 @@ void mbedtls_threading_free_alt( void )
 #if defined(MBEDTLS_FS_IO)
     mbedtls_mutex_free( &mbedtls_threading_readdir_mutex );
 #endif
-#if defined(MBEDTLS_HAVE_TIME_DATE)
+#if defined(THREADING_USE_GMTIME)
     mbedtls_mutex_free( &mbedtls_threading_gmtime_mutex );
 #endif
 }
@@ -142,7 +180,7 @@ void mbedtls_threading_free_alt( void )
 #if defined(MBEDTLS_FS_IO)
 mbedtls_threading_mutex_t mbedtls_threading_readdir_mutex MUTEX_INIT;
 #endif
-#if defined(MBEDTLS_HAVE_TIME_DATE)
+#if defined(THREADING_USE_GMTIME)
 mbedtls_threading_mutex_t mbedtls_threading_gmtime_mutex MUTEX_INIT;
 #endif
 
diff --git a/library/timing.c b/library/timing.c
index 6a30e512..3e8139f1 100644
--- a/library/timing.c
+++ b/library/timing.c
@@ -39,7 +39,8 @@
 #if !defined(MBEDTLS_TIMING_ALT)
 
 #if !defined(unix) && !defined(__unix__) && !defined(__unix) && \
-    !defined(__APPLE__) && !defined(_WIN32) && !defined(__QNXNTO__)
+    !defined(__APPLE__) && !defined(_WIN32) && !defined(__QNXNTO__) && \
+    !defined(__HAIKU__)
 #error "This module only works on Unix and Windows, see MBEDTLS_TIMING_C in config.h"
 #endif
 
diff --git a/library/version_features.c b/library/version_features.c
index b1b5f306..f1798a7f 100644
--- a/library/version_features.c
+++ b/library/version_features.c
@@ -39,6 +39,9 @@ static const char *features[] = {
 #if defined(MBEDTLS_NO_UDBL_DIVISION)
     "MBEDTLS_NO_UDBL_DIVISION",
 #endif /* MBEDTLS_NO_UDBL_DIVISION */
+#if defined(MBEDTLS_NO_64BIT_MULTIPLICATION)
+    "MBEDTLS_NO_64BIT_MULTIPLICATION",
+#endif /* MBEDTLS_NO_64BIT_MULTIPLICATION */
 #if defined(MBEDTLS_HAVE_SSE2)
     "MBEDTLS_HAVE_SSE2",
 #endif /* MBEDTLS_HAVE_SSE2 */
@@ -102,6 +105,12 @@ static const char *features[] = {
 #if defined(MBEDTLS_CCM_ALT)
     "MBEDTLS_CCM_ALT",
 #endif /* MBEDTLS_CCM_ALT */
+#if defined(MBEDTLS_CHACHA20_ALT)
+    "MBEDTLS_CHACHA20_ALT",
+#endif /* MBEDTLS_CHACHA20_ALT */
+#if defined(MBEDTLS_CHACHAPOLY_ALT)
+    "MBEDTLS_CHACHAPOLY_ALT",
+#endif /* MBEDTLS_CHACHAPOLY_ALT */
 #if defined(MBEDTLS_CMAC_ALT)
     "MBEDTLS_CMAC_ALT",
 #endif /* MBEDTLS_CMAC_ALT */
@@ -117,6 +126,9 @@ static const char *features[] = {
 #if defined(MBEDTLS_GCM_ALT)
     "MBEDTLS_GCM_ALT",
 #endif /* MBEDTLS_GCM_ALT */
+#if defined(MBEDTLS_NIST_KW_ALT)
+    "MBEDTLS_NIST_KW_ALT",
+#endif /* MBEDTLS_NIST_KW_ALT */
 #if defined(MBEDTLS_MD2_ALT)
     "MBEDTLS_MD2_ALT",
 #endif /* MBEDTLS_MD2_ALT */
@@ -126,6 +138,9 @@ static const char *features[] = {
 #if defined(MBEDTLS_MD5_ALT)
     "MBEDTLS_MD5_ALT",
 #endif /* MBEDTLS_MD5_ALT */
+#if defined(MBEDTLS_POLY1305_ALT)
+    "MBEDTLS_POLY1305_ALT",
+#endif /* MBEDTLS_POLY1305_ALT */
 #if defined(MBEDTLS_RIPEMD160_ALT)
     "MBEDTLS_RIPEMD160_ALT",
 #endif /* MBEDTLS_RIPEMD160_ALT */
@@ -555,6 +570,12 @@ static const char *features[] = {
 #if defined(MBEDTLS_CERTS_C)
     "MBEDTLS_CERTS_C",
 #endif /* MBEDTLS_CERTS_C */
+#if defined(MBEDTLS_CHACHA20_C)
+    "MBEDTLS_CHACHA20_C",
+#endif /* MBEDTLS_CHACHA20_C */
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    "MBEDTLS_CHACHAPOLY_C",
+#endif /* MBEDTLS_CHACHAPOLY_C */
 #if defined(MBEDTLS_CIPHER_C)
     "MBEDTLS_CIPHER_C",
 #endif /* MBEDTLS_CIPHER_C */
@@ -603,6 +624,9 @@ static const char *features[] = {
 #if defined(MBEDTLS_HMAC_DRBG_C)
     "MBEDTLS_HMAC_DRBG_C",
 #endif /* MBEDTLS_HMAC_DRBG_C */
+#if defined(MBEDTLS_NIST_KW_C)
+    "MBEDTLS_NIST_KW_C",
+#endif /* MBEDTLS_NIST_KW_C */
 #if defined(MBEDTLS_MD_C)
     "MBEDTLS_MD_C",
 #endif /* MBEDTLS_MD_C */
@@ -654,6 +678,9 @@ static const char *features[] = {
 #if defined(MBEDTLS_PLATFORM_C)
     "MBEDTLS_PLATFORM_C",
 #endif /* MBEDTLS_PLATFORM_C */
+#if defined(MBEDTLS_POLY1305_C)
+    "MBEDTLS_POLY1305_C",
+#endif /* MBEDTLS_POLY1305_C */
 #if defined(MBEDTLS_RIPEMD160_C)
     "MBEDTLS_RIPEMD160_C",
 #endif /* MBEDTLS_RIPEMD160_C */
diff --git a/library/x509.c b/library/x509.c
index 371d6da1..52b5b649 100644
--- a/library/x509.c
+++ b/library/x509.c
@@ -59,26 +59,14 @@
 #define mbedtls_snprintf  snprintf
 #endif
 
-
 #if defined(MBEDTLS_HAVE_TIME)
 #include "mbedtls/platform_time.h"
 #endif
-
-#if defined(_WIN32) && !defined(EFIX64) && !defined(EFI32)
-#include <windows.h>
-#else
+#if defined(MBEDTLS_HAVE_TIME_DATE)
+#include "mbedtls/platform_util.h"
 #include <time.h>
 #endif
 
-#if defined(MBEDTLS_FS_IO)
-#include <stdio.h>
-#if !defined(_WIN32)
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <dirent.h>
-#endif
-#endif
-
 #define CHECK(code) if( ( ret = code ) != 0 ){ return( ret ); }
 #define CHECK_RANGE(min, max, val) if( val < min || val > max ){ return( ret ); }
 
@@ -903,36 +891,14 @@ int mbedtls_x509_key_size_helper( char *buf, size_t buf_size, const char *name )
  * Set the time structure to the current time.
  * Return 0 on success, non-zero on failure.
  */
-#if defined(_WIN32) && !defined(EFIX64) && !defined(EFI32)
-static int x509_get_current_time( mbedtls_x509_time *now )
-{
-    SYSTEMTIME st;
-
-    GetSystemTime( &st );
-
-    now->year = st.wYear;
-    now->mon  = st.wMonth;
-    now->day  = st.wDay;
-    now->hour = st.wHour;
-    now->min  = st.wMinute;
-    now->sec  = st.wSecond;
-
-    return( 0 );
-}
-#else
 static int x509_get_current_time( mbedtls_x509_time *now )
 {
-    struct tm *lt;
+    struct tm *lt, tm_buf;
     mbedtls_time_t tt;
     int ret = 0;
 
-#if defined(MBEDTLS_THREADING_C)
-    if( mbedtls_mutex_lock( &mbedtls_threading_gmtime_mutex ) != 0 )
-        return( MBEDTLS_ERR_THREADING_MUTEX_ERROR );
-#endif
-
     tt = mbedtls_time( NULL );
-    lt = gmtime( &tt );
+    lt = mbedtls_platform_gmtime_r( &tt, &tm_buf );
 
     if( lt == NULL )
         ret = -1;
@@ -946,14 +912,8 @@ static int x509_get_current_time( mbedtls_x509_time *now )
         now->sec  = lt->tm_sec;
     }
 
-#if defined(MBEDTLS_THREADING_C)
-    if( mbedtls_mutex_unlock( &mbedtls_threading_gmtime_mutex ) != 0 )
-        return( MBEDTLS_ERR_THREADING_MUTEX_ERROR );
-#endif
-
     return( ret );
 }
-#endif /* _WIN32 && !EFIX64 && !EFI32 */
 
 /*
  * Return 0 if before <= after, 1 otherwise
diff --git a/library/x509_crt.c b/library/x509_crt.c
index 588b484f..5d9c6230 100644
--- a/library/x509_crt.c
+++ b/library/x509_crt.c
@@ -163,6 +163,9 @@ const mbedtls_x509_crt_profile mbedtls_x509_crt_profile_suiteb =
 static int x509_profile_check_md_alg( const mbedtls_x509_crt_profile *profile,
                                       mbedtls_md_type_t md_alg )
 {
+    if( md_alg == MBEDTLS_MD_NONE )
+        return( -1 );
+
     if( ( profile->allowed_mds & MBEDTLS_X509_ID_FLAG( md_alg ) ) != 0 )
         return( 0 );
 
@@ -176,6 +179,9 @@ static int x509_profile_check_md_alg( const mbedtls_x509_crt_profile *profile,
 static int x509_profile_check_pk_alg( const mbedtls_x509_crt_profile *profile,
                                       mbedtls_pk_type_t pk_alg )
 {
+    if( pk_alg == MBEDTLS_PK_NONE )
+        return( -1 );
+
     if( ( profile->allowed_pks & MBEDTLS_X509_ID_FLAG( pk_alg ) ) != 0 )
         return( 0 );
 
@@ -208,6 +214,9 @@ static int x509_profile_check_key( const mbedtls_x509_crt_profile *profile,
     {
         const mbedtls_ecp_group_id gid = mbedtls_pk_ec( *pk )->grp.id;
 
+        if( gid == MBEDTLS_ECP_DP_NONE )
+            return( -1 );
+
         if( ( profile->allowed_curves & MBEDTLS_X509_ID_FLAG( gid ) ) != 0 )
             return( 0 );
 
@@ -587,18 +596,14 @@ static int x509_get_crt_ext( unsigned char **p,
         end_ext_data = *p + len;
 
         /* Get extension ID */
-        extn_oid.tag = **p;
-
-        if( ( ret = mbedtls_asn1_get_tag( p, end, &extn_oid.len, MBEDTLS_ASN1_OID ) ) != 0 )
+        if( ( ret = mbedtls_asn1_get_tag( p, end_ext_data, &extn_oid.len,
+                                          MBEDTLS_ASN1_OID ) ) != 0 )
             return( MBEDTLS_ERR_X509_INVALID_EXTENSIONS + ret );
 
+        extn_oid.tag = MBEDTLS_ASN1_OID;
         extn_oid.p = *p;
         *p += extn_oid.len;
 
-        if( ( end - *p ) < 1 )
-            return( MBEDTLS_ERR_X509_INVALID_EXTENSIONS +
-                    MBEDTLS_ERR_ASN1_OUT_OF_DATA );
-
         /* Get optional critical */
         if( ( ret = mbedtls_asn1_get_bool( p, end_ext_data, &is_critical ) ) != 0 &&
             ( ret != MBEDTLS_ERR_ASN1_UNEXPECTED_TAG ) )
diff --git a/library/x509_csr.c b/library/x509_csr.c
index 3e8e8fbc..f8442572 100644
--- a/library/x509_csr.c
+++ b/library/x509_csr.c
@@ -274,34 +274,25 @@ int mbedtls_x509_csr_parse( mbedtls_x509_csr *csr, const unsigned char *buf, siz
         return( MBEDTLS_ERR_X509_BAD_INPUT_DATA );
 
 #if defined(MBEDTLS_PEM_PARSE_C)
-    mbedtls_pem_init( &pem );
-
     /* Avoid calling mbedtls_pem_read_buffer() on non-null-terminated string */
-    if( buf[buflen - 1] != '\0' )
-        ret = MBEDTLS_ERR_PEM_NO_HEADER_FOOTER_PRESENT;
-    else
+    if( buf[buflen - 1] == '\0' )
+    {
+        mbedtls_pem_init( &pem );
         ret = mbedtls_pem_read_buffer( &pem,
                                "-----BEGIN CERTIFICATE REQUEST-----",
                                "-----END CERTIFICATE REQUEST-----",
                                buf, NULL, 0, &use_len );
 
-    if( ret == 0 )
-    {
-        /*
-         * Was PEM encoded, parse the result
-         */
-        if( ( ret = mbedtls_x509_csr_parse_der( csr, pem.buf, pem.buflen ) ) != 0 )
-            return( ret );
+        if( ret == 0 )
+            /*
+             * Was PEM encoded, parse the result
+             */
+            ret = mbedtls_x509_csr_parse_der( csr, pem.buf, pem.buflen );
 
         mbedtls_pem_free( &pem );
-        return( 0 );
-    }
-    else if( ret != MBEDTLS_ERR_PEM_NO_HEADER_FOOTER_PRESENT )
-    {
-        mbedtls_pem_free( &pem );
-        return( ret );
+        if( ret != MBEDTLS_ERR_PEM_NO_HEADER_FOOTER_PRESENT )
+            return( ret );
     }
-    else
 #endif /* MBEDTLS_PEM_PARSE_C */
     return( mbedtls_x509_csr_parse_der( csr, buf, buflen ) );
 }