diff --git a/programs/Makefile b/programs/Makefile
index 443689b..25f184f 100644
--- a/programs/Makefile
+++ b/programs/Makefile
@@ -35,7 +35,7 @@
 SHARED_SUFFIX=.$(DLEXT)
 endif
 else
-DLEXT=so
+DLEXT ?= so
 EXEXT=
 SHARED_SUFFIX=
 endif
diff --git a/programs/hash/hello.c b/programs/hash/hello.c
index df420f2..2e8c224 100644
--- a/programs/hash/hello.c
+++ b/programs/hash/hello.c
@@ -28,8 +28,11 @@
 #if defined(MBEDTLS_PLATFORM_C)
 #include "mbedtls/platform.h"
 #else
+#include <stdlib.h>
 #include <stdio.h>
-#define mbedtls_printf     printf
+#define mbedtls_printf       printf
+#define MBEDTLS_EXIT_SUCCESS EXIT_SUCCESS
+#define MBEDTLS_EXIT_FAILURE EXIT_FAILURE
 #endif
 
 #if defined(MBEDTLS_MD5_C)
@@ -45,13 +48,14 @@
 #else
 int main( void )
 {
-    int i;
+    int i, ret;
     unsigned char digest[16];
     char str[] = "Hello, world!";
 
     mbedtls_printf( "\n  MD5('%s') = ", str );
 
-    mbedtls_md5( (unsigned char *) str, 13, digest );
+    if( ( ret = mbedtls_md5_ret( (unsigned char *) str, 13, digest ) ) != 0 )
+        return( MBEDTLS_EXIT_FAILURE );
 
     for( i = 0; i < 16; i++ )
         mbedtls_printf( "%02x", digest[i] );
@@ -63,6 +67,6 @@
     fflush( stdout ); getchar();
 #endif
 
-    return( 0 );
+    return( MBEDTLS_EXIT_SUCCESS );
 }
 #endif /* MBEDTLS_MD5_C */
diff --git a/programs/pkey/dh_client.c b/programs/pkey/dh_client.c
index 875d0b08..0978408 100644
--- a/programs/pkey/dh_client.c
+++ b/programs/pkey/dh_client.c
@@ -212,7 +212,11 @@
         goto exit;
     }
 
-    mbedtls_sha1( buf, (int)( p - 2 - buf ), hash );
+    if( ( ret = mbedtls_sha1_ret( buf, (int)( p - 2 - buf ), hash ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_sha1_ret returned %d\n\n", ret );
+        goto exit;
+    }
 
     if( ( ret = mbedtls_rsa_pkcs1_verify( &rsa, NULL, NULL, MBEDTLS_RSA_PUBLIC,
                                   MBEDTLS_MD_SHA256, 0, hash, p ) ) != 0 )
diff --git a/programs/pkey/dh_server.c b/programs/pkey/dh_server.c
index 8bf2b1b..4304231 100644
--- a/programs/pkey/dh_server.c
+++ b/programs/pkey/dh_server.c
@@ -86,6 +86,8 @@
     mbedtls_dhm_context dhm;
     mbedtls_aes_context aes;
 
+    mbedtls_mpi N, P, Q, D, E;
+
     mbedtls_net_init( &listen_fd );
     mbedtls_net_init( &client_fd );
     mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, MBEDTLS_MD_SHA256 );
@@ -93,6 +95,9 @@
     mbedtls_aes_init( &aes );
     mbedtls_ctr_drbg_init( &ctr_drbg );
 
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &P ); mbedtls_mpi_init( &Q );
+    mbedtls_mpi_init( &D ); mbedtls_mpi_init( &E );
+
     /*
      * 1. Setup the RNG
      */
@@ -124,24 +129,33 @@
 
     mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, 0 );
 
-    if( ( ret = mbedtls_mpi_read_file( &rsa.N , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.E , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.D , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.P , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.Q , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.DP, 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.DQ, 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.QP, 16, f ) ) != 0 )
+    if( ( ret = mbedtls_mpi_read_file( &N , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &E , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &D , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &P , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &Q , 16, f ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_mpi_read_file returned %d\n\n", ret );
+        mbedtls_printf( " failed\n  ! mbedtls_mpi_read_file returned %d\n\n",
+                        ret );
         fclose( f );
         goto exit;
     }
-
-    rsa.len = ( mbedtls_mpi_bitlen( &rsa.N ) + 7 ) >> 3;
-
     fclose( f );
 
+    if( ( ret = mbedtls_rsa_import( &rsa, &N, &P, &Q, &D, &E ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_rsa_import returned %d\n\n",
+                        ret );
+        goto exit;
+    }
+
+    if( ( ret = mbedtls_rsa_complete( &rsa ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_rsa_complete returned %d\n\n",
+                        ret );
+        goto exit;
+    }
+
     /*
      * 2b. Get the DHM modulus and generator
      */
@@ -203,7 +217,11 @@
     /*
      * 5. Sign the parameters and send them
      */
-    mbedtls_sha1( buf, n, hash );
+    if( ( ret = mbedtls_sha1_ret( buf, n, hash ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_sha1_ret returned %d\n\n", ret );
+        goto exit;
+    }
 
     buf[n    ] = (unsigned char)( rsa.len >> 8 );
     buf[n + 1] = (unsigned char)( rsa.len      );
@@ -234,6 +252,7 @@
 
     memset( buf, 0, sizeof( buf ) );
 
+    n = dhm.len;
     if( ( ret = mbedtls_net_recv( &client_fd, buf, n ) ) != (int) n )
     {
         mbedtls_printf( " failed\n  ! mbedtls_net_recv returned %d\n\n", ret );
@@ -287,6 +306,9 @@
 
 exit:
 
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &P ); mbedtls_mpi_free( &Q );
+    mbedtls_mpi_free( &D ); mbedtls_mpi_free( &E );
+
     mbedtls_net_free( &client_fd );
     mbedtls_net_free( &listen_fd );
 
diff --git a/programs/pkey/ecdsa.c b/programs/pkey/ecdsa.c
index c3ce56a..b474060 100644
--- a/programs/pkey/ecdsa.c
+++ b/programs/pkey/ecdsa.c
@@ -102,7 +102,6 @@
     mbedtls_ecdsa_context ctx_sign, ctx_verify;
     mbedtls_entropy_context entropy;
     mbedtls_ctr_drbg_context ctr_drbg;
-    mbedtls_sha256_context sha256_ctx;
     unsigned char message[100];
     unsigned char hash[32];
     unsigned char sig[MBEDTLS_ECDSA_MAX_LEN];
@@ -113,7 +112,6 @@
     mbedtls_ecdsa_init( &ctx_sign );
     mbedtls_ecdsa_init( &ctx_verify );
     mbedtls_ctr_drbg_init( &ctr_drbg );
-    mbedtls_sha256_init( &sha256_ctx );
 
     memset( sig, 0, sizeof( sig ) );
     memset( message, 0x25, sizeof( message ) );
@@ -165,9 +163,11 @@
     mbedtls_printf( "  . Computing message hash..." );
     fflush( stdout );
 
-    mbedtls_sha256_starts( &sha256_ctx, 0 );
-    mbedtls_sha256_update( &sha256_ctx, message, sizeof( message ) );
-    mbedtls_sha256_finish( &sha256_ctx, hash );
+    if( ( ret = mbedtls_sha256_ret( message, sizeof( message ), hash, 0 ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_sha256_ret returned %d\n", ret );
+        goto exit;
+    }
 
     mbedtls_printf( " ok\n" );
 
@@ -242,7 +242,6 @@
     mbedtls_ecdsa_free( &ctx_sign );
     mbedtls_ctr_drbg_free( &ctr_drbg );
     mbedtls_entropy_free( &entropy );
-    mbedtls_sha256_free( &sha256_ctx );
 
     return( ret );
 }
diff --git a/programs/pkey/gen_key.c b/programs/pkey/gen_key.c
index 4812694..a7f5c90 100644
--- a/programs/pkey/gen_key.c
+++ b/programs/pkey/gen_key.c
@@ -191,6 +191,7 @@
     char buf[1024];
     int i;
     char *p, *q;
+    mbedtls_mpi N, P, Q, D, E, DP, DQ, QP;
     mbedtls_entropy_context entropy;
     mbedtls_ctr_drbg_context ctr_drbg;
     const char *pers = "gen_key";
@@ -201,6 +202,11 @@
     /*
      * Set to sane values
      */
+
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &P ); mbedtls_mpi_init( &Q );
+    mbedtls_mpi_init( &D ); mbedtls_mpi_init( &E ); mbedtls_mpi_init( &DP );
+    mbedtls_mpi_init( &DQ ); mbedtls_mpi_init( &QP );
+
     mbedtls_pk_init( &key );
     mbedtls_ctr_drbg_init( &ctr_drbg );
     memset( buf, 0, sizeof( buf ) );
@@ -323,7 +329,7 @@
     if( opt.type == MBEDTLS_PK_RSA )
     {
         ret = mbedtls_rsa_gen_key( mbedtls_pk_rsa( key ), mbedtls_ctr_drbg_random, &ctr_drbg,
-                           opt.rsa_keysize, 65537 );
+                                   opt.rsa_keysize, 65537 );
         if( ret != 0 )
         {
             mbedtls_printf( " failed\n  !  mbedtls_rsa_gen_key returned -0x%04x", -ret );
@@ -336,10 +342,10 @@
     if( opt.type == MBEDTLS_PK_ECKEY )
     {
         ret = mbedtls_ecp_gen_key( opt.ec_curve, mbedtls_pk_ec( key ),
-                          mbedtls_ctr_drbg_random, &ctr_drbg );
+                                   mbedtls_ctr_drbg_random, &ctr_drbg );
         if( ret != 0 )
         {
-            mbedtls_printf( " failed\n  !  mbedtls_rsa_gen_key returned -0x%04x", -ret );
+            mbedtls_printf( " failed\n  !  mbedtls_ecp_gen_key returned -0x%04x", -ret );
             goto exit;
         }
     }
@@ -359,14 +365,22 @@
     if( mbedtls_pk_get_type( &key ) == MBEDTLS_PK_RSA )
     {
         mbedtls_rsa_context *rsa = mbedtls_pk_rsa( key );
-        mbedtls_mpi_write_file( "N:  ",  &rsa->N,  16, NULL );
-        mbedtls_mpi_write_file( "E:  ",  &rsa->E,  16, NULL );
-        mbedtls_mpi_write_file( "D:  ",  &rsa->D,  16, NULL );
-        mbedtls_mpi_write_file( "P:  ",  &rsa->P,  16, NULL );
-        mbedtls_mpi_write_file( "Q:  ",  &rsa->Q,  16, NULL );
-        mbedtls_mpi_write_file( "DP: ",  &rsa->DP, 16, NULL );
-        mbedtls_mpi_write_file( "DQ:  ", &rsa->DQ, 16, NULL );
-        mbedtls_mpi_write_file( "QP:  ", &rsa->QP, 16, NULL );
+
+        if( ( ret = mbedtls_rsa_export    ( rsa, &N, &P, &Q, &D, &E ) ) != 0 ||
+            ( ret = mbedtls_rsa_export_crt( rsa, &DP, &DQ, &QP ) )      != 0 )
+        {
+            mbedtls_printf( " failed\n  ! could not export RSA parameters\n\n" );
+            goto exit;
+        }
+
+        mbedtls_mpi_write_file( "N:  ",  &N,  16, NULL );
+        mbedtls_mpi_write_file( "E:  ",  &E,  16, NULL );
+        mbedtls_mpi_write_file( "D:  ",  &D,  16, NULL );
+        mbedtls_mpi_write_file( "P:  ",  &P,  16, NULL );
+        mbedtls_mpi_write_file( "Q:  ",  &Q,  16, NULL );
+        mbedtls_mpi_write_file( "DP: ",  &DP, 16, NULL );
+        mbedtls_mpi_write_file( "DQ:  ", &DQ, 16, NULL );
+        mbedtls_mpi_write_file( "QP:  ", &QP, 16, NULL );
     }
     else
 #endif
@@ -409,6 +423,10 @@
 #endif
     }
 
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &P ); mbedtls_mpi_free( &Q );
+    mbedtls_mpi_free( &D ); mbedtls_mpi_free( &E ); mbedtls_mpi_free( &DP );
+    mbedtls_mpi_free( &DQ ); mbedtls_mpi_free( &QP );
+
     mbedtls_pk_free( &key );
     mbedtls_ctr_drbg_free( &ctr_drbg );
     mbedtls_entropy_free( &entropy );
@@ -422,4 +440,3 @@
 }
 #endif /* MBEDTLS_PK_WRITE_C && MBEDTLS_PEM_WRITE_C && MBEDTLS_FS_IO &&
         * MBEDTLS_ENTROPY_C && MBEDTLS_CTR_DRBG_C */
-
diff --git a/programs/pkey/key_app.c b/programs/pkey/key_app.c
index b6b8446..f1b548d 100644
--- a/programs/pkey/key_app.c
+++ b/programs/pkey/key_app.c
@@ -84,17 +84,23 @@
 int main( int argc, char *argv[] )
 {
     int ret = 0;
-    mbedtls_pk_context pk;
     char buf[1024];
     int i;
     char *p, *q;
 
+    mbedtls_pk_context pk;
+    mbedtls_mpi N, P, Q, D, E, DP, DQ, QP;
+
     /*
      * Set to sane values
      */
     mbedtls_pk_init( &pk );
     memset( buf, 0, sizeof(buf) );
 
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &P ); mbedtls_mpi_init( &Q );
+    mbedtls_mpi_init( &D ); mbedtls_mpi_init( &E ); mbedtls_mpi_init( &DP );
+    mbedtls_mpi_init( &DQ ); mbedtls_mpi_init( &QP );
+
     if( argc == 0 )
     {
     usage:
@@ -189,14 +195,22 @@
         if( mbedtls_pk_get_type( &pk ) == MBEDTLS_PK_RSA )
         {
             mbedtls_rsa_context *rsa = mbedtls_pk_rsa( pk );
-            mbedtls_mpi_write_file( "N:  ", &rsa->N, 16, NULL );
-            mbedtls_mpi_write_file( "E:  ", &rsa->E, 16, NULL );
-            mbedtls_mpi_write_file( "D:  ", &rsa->D, 16, NULL );
-            mbedtls_mpi_write_file( "P:  ", &rsa->P, 16, NULL );
-            mbedtls_mpi_write_file( "Q:  ", &rsa->Q, 16, NULL );
-            mbedtls_mpi_write_file( "DP: ", &rsa->DP, 16, NULL );
-            mbedtls_mpi_write_file( "DQ:  ", &rsa->DQ, 16, NULL );
-            mbedtls_mpi_write_file( "QP:  ", &rsa->QP, 16, NULL );
+
+            if( ( ret = mbedtls_rsa_export    ( rsa, &N, &P, &Q, &D, &E ) ) != 0 ||
+                ( ret = mbedtls_rsa_export_crt( rsa, &DP, &DQ, &QP ) )      != 0 )
+            {
+                mbedtls_printf( " failed\n  ! could not export RSA parameters\n\n" );
+                goto exit;
+            }
+
+            mbedtls_mpi_write_file( "N:  ",  &N,  16, NULL );
+            mbedtls_mpi_write_file( "E:  ",  &E,  16, NULL );
+            mbedtls_mpi_write_file( "D:  ",  &D,  16, NULL );
+            mbedtls_mpi_write_file( "P:  ",  &P,  16, NULL );
+            mbedtls_mpi_write_file( "Q:  ",  &Q,  16, NULL );
+            mbedtls_mpi_write_file( "DP: ",  &DP, 16, NULL );
+            mbedtls_mpi_write_file( "DQ:  ", &DQ, 16, NULL );
+            mbedtls_mpi_write_file( "QP:  ", &QP, 16, NULL );
         }
         else
 #endif
@@ -239,8 +253,15 @@
         if( mbedtls_pk_get_type( &pk ) == MBEDTLS_PK_RSA )
         {
             mbedtls_rsa_context *rsa = mbedtls_pk_rsa( pk );
-            mbedtls_mpi_write_file( "N:  ", &rsa->N, 16, NULL );
-            mbedtls_mpi_write_file( "E:  ", &rsa->E, 16, NULL );
+
+            if( ( ret = mbedtls_rsa_export( rsa, &N, NULL, NULL,
+                                            NULL, &E ) ) != 0 )
+            {
+                mbedtls_printf( " failed\n  ! could not export RSA parameters\n\n" );
+                goto exit;
+            }
+            mbedtls_mpi_write_file( "N: ", &N, 16, NULL );
+            mbedtls_mpi_write_file( "E: ", &E, 16, NULL );
         }
         else
 #endif
@@ -265,11 +286,17 @@
 exit:
 
 #if defined(MBEDTLS_ERROR_C)
-    mbedtls_strerror( ret, buf, sizeof(buf) );
-    mbedtls_printf( "  !  Last error was: %s\n", buf );
+    if( ret != 0 )
+    {
+        mbedtls_strerror( ret, buf, sizeof(buf) );
+        mbedtls_printf( "  !  Last error was: %s\n", buf );
+    }
 #endif
 
     mbedtls_pk_free( &pk );
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &P ); mbedtls_mpi_free( &Q );
+    mbedtls_mpi_free( &D ); mbedtls_mpi_free( &E ); mbedtls_mpi_free( &DP );
+    mbedtls_mpi_free( &DQ ); mbedtls_mpi_free( &QP );
 
 #if defined(_WIN32)
     mbedtls_printf( "  + Press Enter to exit this program.\n" );
diff --git a/programs/pkey/key_app_writer.c b/programs/pkey/key_app_writer.c
index 9d12077..52b0f8e 100644
--- a/programs/pkey/key_app_writer.c
+++ b/programs/pkey/key_app_writer.c
@@ -76,7 +76,7 @@
 #define OUTPUT_FORMAT_DER              1
 
 #define USAGE \
-    "\n usage: key_app param=<>...\n"                   \
+    "\n usage: key_app_writer param=<>...\n"            \
     "\n acceptable parameters:\n"                       \
     "    mode=private|public default: none\n"           \
     "    filename=%%s         default: keyfile.key\n"   \
@@ -190,17 +190,23 @@
 int main( int argc, char *argv[] )
 {
     int ret = 0;
-    mbedtls_pk_context key;
     char buf[1024];
     int i;
     char *p, *q;
 
+    mbedtls_pk_context key;
+    mbedtls_mpi N, P, Q, D, E, DP, DQ, QP;
+
     /*
      * Set to sane values
      */
     mbedtls_pk_init( &key );
     memset( buf, 0, sizeof( buf ) );
 
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &P ); mbedtls_mpi_init( &Q );
+    mbedtls_mpi_init( &D ); mbedtls_mpi_init( &E ); mbedtls_mpi_init( &DP );
+    mbedtls_mpi_init( &DQ ); mbedtls_mpi_init( &QP );
+
     if( argc == 0 )
     {
     usage:
@@ -300,14 +306,22 @@
         if( mbedtls_pk_get_type( &key ) == MBEDTLS_PK_RSA )
         {
             mbedtls_rsa_context *rsa = mbedtls_pk_rsa( key );
-            mbedtls_mpi_write_file( "N:  ",  &rsa->N,  16, NULL );
-            mbedtls_mpi_write_file( "E:  ",  &rsa->E,  16, NULL );
-            mbedtls_mpi_write_file( "D:  ",  &rsa->D,  16, NULL );
-            mbedtls_mpi_write_file( "P:  ",  &rsa->P,  16, NULL );
-            mbedtls_mpi_write_file( "Q:  ",  &rsa->Q,  16, NULL );
-            mbedtls_mpi_write_file( "DP: ",  &rsa->DP, 16, NULL );
-            mbedtls_mpi_write_file( "DQ:  ", &rsa->DQ, 16, NULL );
-            mbedtls_mpi_write_file( "QP:  ", &rsa->QP, 16, NULL );
+
+            if( ( ret = mbedtls_rsa_export    ( rsa, &N, &P, &Q, &D, &E ) ) != 0 ||
+                ( ret = mbedtls_rsa_export_crt( rsa, &DP, &DQ, &QP ) )      != 0 )
+            {
+                mbedtls_printf( " failed\n  ! could not export RSA parameters\n\n" );
+                goto exit;
+            }
+
+            mbedtls_mpi_write_file( "N:  ",  &N,  16, NULL );
+            mbedtls_mpi_write_file( "E:  ",  &E,  16, NULL );
+            mbedtls_mpi_write_file( "D:  ",  &D,  16, NULL );
+            mbedtls_mpi_write_file( "P:  ",  &P,  16, NULL );
+            mbedtls_mpi_write_file( "Q:  ",  &Q,  16, NULL );
+            mbedtls_mpi_write_file( "DP: ",  &DP, 16, NULL );
+            mbedtls_mpi_write_file( "DQ:  ", &DQ, 16, NULL );
+            mbedtls_mpi_write_file( "QP:  ", &QP, 16, NULL );
         }
         else
 #endif
@@ -353,8 +367,15 @@
         if( mbedtls_pk_get_type( &key ) == MBEDTLS_PK_RSA )
         {
             mbedtls_rsa_context *rsa = mbedtls_pk_rsa( key );
-            mbedtls_mpi_write_file( "N: ", &rsa->N, 16, NULL );
-            mbedtls_mpi_write_file( "E: ", &rsa->E, 16, NULL );
+
+            if( ( ret = mbedtls_rsa_export( rsa, &N, NULL, NULL,
+                                            NULL, &E ) ) != 0 )
+            {
+                mbedtls_printf( " failed\n  ! could not export RSA parameters\n\n" );
+                goto exit;
+            }
+            mbedtls_mpi_write_file( "N: ", &N, 16, NULL );
+            mbedtls_mpi_write_file( "E: ", &E, 16, NULL );
         }
         else
 #endif
@@ -394,6 +415,10 @@
 #endif
     }
 
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &P ); mbedtls_mpi_free( &Q );
+    mbedtls_mpi_free( &D ); mbedtls_mpi_free( &E ); mbedtls_mpi_free( &DP );
+    mbedtls_mpi_free( &DQ ); mbedtls_mpi_free( &QP );
+
     mbedtls_pk_free( &key );
 
 #if defined(_WIN32)
diff --git a/programs/pkey/pk_sign.c b/programs/pkey/pk_sign.c
index daf08a9..55df95e 100644
--- a/programs/pkey/pk_sign.c
+++ b/programs/pkey/pk_sign.c
@@ -29,6 +29,7 @@
 #include "mbedtls/platform.h"
 #else
 #include <stdio.h>
+#include <stdlib.h>
 #define mbedtls_snprintf   snprintf
 #define mbedtls_printf     printf
 #endif
@@ -100,8 +101,7 @@
 
     if( ( ret = mbedtls_pk_parse_keyfile( &pk, argv[1], "" ) ) != 0 )
     {
-        ret = 1;
-        mbedtls_printf( " failed\n  ! Could not open '%s'\n", argv[1] );
+        mbedtls_printf( " failed\n  ! Could not parse '%s'\n", argv[1] );
         goto exit;
     }
 
@@ -141,6 +141,7 @@
 
     if( fwrite( buf, 1, olen, f ) != olen )
     {
+        ret = 1;
         mbedtls_printf( "failed\n  ! fwrite failed\n\n" );
         fclose( f );
         goto exit;
@@ -168,7 +169,7 @@
     fflush( stdout ); getchar();
 #endif
 
-    return( ret );
+    return( ret ? EXIT_FAILURE : EXIT_SUCCESS );
 }
 #endif /* MBEDTLS_BIGNUM_C && MBEDTLS_ENTROPY_C &&
           MBEDTLS_SHA256_C && MBEDTLS_PK_PARSE_C && MBEDTLS_FS_IO &&
diff --git a/programs/pkey/rsa_decrypt.c b/programs/pkey/rsa_decrypt.c
index b64e156..2da3fbf 100644
--- a/programs/pkey/rsa_decrypt.c
+++ b/programs/pkey/rsa_decrypt.c
@@ -64,6 +64,7 @@
     int return_val, exit_val, c;
     size_t i;
     mbedtls_rsa_context rsa;
+    mbedtls_mpi N, P, Q, D, E, DP, DQ, QP;
     mbedtls_entropy_context entropy;
     mbedtls_ctr_drbg_context ctr_drbg;
     unsigned char result[1024];
@@ -91,6 +92,9 @@
     mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, 0 );
     mbedtls_ctr_drbg_init( &ctr_drbg );
     mbedtls_entropy_init( &entropy );
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &P ); mbedtls_mpi_init( &Q );
+    mbedtls_mpi_init( &D ); mbedtls_mpi_init( &E ); mbedtls_mpi_init( &DP );
+    mbedtls_mpi_init( &DQ ); mbedtls_mpi_init( &QP );
 
     return_val = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func,
                                         &entropy, (const unsigned char *) pers,
@@ -114,14 +118,14 @@
         goto exit;
     }
 
-    if( ( return_val = mbedtls_mpi_read_file( &rsa.N , 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.E , 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.D , 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.P , 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.Q , 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.DP, 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.DQ, 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.QP, 16, f ) ) != 0 )
+    if( ( return_val = mbedtls_mpi_read_file( &N , 16, f ) )  != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &E , 16, f ) )  != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &D , 16, f ) )  != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &P , 16, f ) )  != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &Q , 16, f ) )  != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &DP , 16, f ) ) != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &DQ , 16, f ) ) != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &QP , 16, f ) ) != 0 )
     {
         exit_val = MBEDTLS_EXIT_FAILURE;
         mbedtls_printf( " failed\n  ! mbedtls_mpi_read_file returned %d\n\n",
@@ -129,11 +133,22 @@
         fclose( f );
         goto exit;
     }
-
-    rsa.len = ( mbedtls_mpi_bitlen( &rsa.N ) + 7 ) >> 3;
-
     fclose( f );
 
+    if( ( return_val = mbedtls_rsa_import( &rsa, &N, &P, &Q, &D, &E ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_rsa_import returned %d\n\n",
+                        return_val );
+        goto exit;
+    }
+
+    if( ( return_val = mbedtls_rsa_complete( &rsa ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_rsa_complete returned %d\n\n",
+                        return_val );
+        goto exit;
+    }
+
     /*
      * Extract the RSA encrypted value from the text file
      */
@@ -184,6 +199,9 @@
     mbedtls_ctr_drbg_free( &ctr_drbg );
     mbedtls_entropy_free( &entropy );
     mbedtls_rsa_free( &rsa );
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &P ); mbedtls_mpi_free( &Q );
+    mbedtls_mpi_free( &D ); mbedtls_mpi_free( &E ); mbedtls_mpi_free( &DP );
+    mbedtls_mpi_free( &DQ ); mbedtls_mpi_free( &QP );
 
 #if defined(_WIN32)
     mbedtls_printf( "  + Press Enter to exit this program.\n" );
@@ -193,4 +211,3 @@
     return( exit_val );
 }
 #endif /* MBEDTLS_BIGNUM_C && MBEDTLS_RSA_C && MBEDTLS_FS_IO */
-
diff --git a/programs/pkey/rsa_encrypt.c b/programs/pkey/rsa_encrypt.c
index b9cb187..81c27d8 100644
--- a/programs/pkey/rsa_encrypt.c
+++ b/programs/pkey/rsa_encrypt.c
@@ -69,6 +69,7 @@
     unsigned char input[1024];
     unsigned char buf[512];
     const char *pers = "rsa_encrypt";
+    mbedtls_mpi N, E;
 
     exit_val = MBEDTLS_EXIT_SUCCESS;
 
@@ -86,6 +87,7 @@
     mbedtls_printf( "\n  . Seeding the random number generator..." );
     fflush( stdout );
 
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &E );
     mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, 0 );
     mbedtls_ctr_drbg_init( &ctr_drbg );
     mbedtls_entropy_init( &entropy );
@@ -112,8 +114,8 @@
         goto exit;
     }
 
-    if( ( return_val = mbedtls_mpi_read_file( &rsa.N, 16, f ) ) != 0 ||
-        ( return_val = mbedtls_mpi_read_file( &rsa.E, 16, f ) ) != 0 )
+    if( ( return_val = mbedtls_mpi_read_file( &N, 16, f ) ) != 0 ||
+        ( return_val = mbedtls_mpi_read_file( &E, 16, f ) ) != 0 )
     {
         exit_val = MBEDTLS_EXIT_FAILURE;
         mbedtls_printf( " failed\n  ! mbedtls_mpi_read_file returned %d\n\n",
@@ -121,11 +123,17 @@
         fclose( f );
         goto exit;
     }
-
-    rsa.len = ( mbedtls_mpi_bitlen( &rsa.N ) + 7 ) >> 3;
-
     fclose( f );
 
+    if( ( return_val = mbedtls_rsa_import( &rsa, &N, NULL,
+                                           NULL, NULL, &E ) ) != 0 )
+    {
+        exit_val = MBEDTLS_EXIT_FAILURE;
+        mbedtls_printf( " failed\n  ! mbedtls_rsa_import returned %d\n\n",
+                        return_val );
+        goto exit;
+    }
+
     if( strlen( argv[1] ) > 100 )
     {
         exit_val = MBEDTLS_EXIT_FAILURE;
@@ -171,6 +179,7 @@
     mbedtls_printf( "\n  . Done (created \"%s\")\n\n", "result-enc.txt" );
 
 exit:
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &E );
     mbedtls_ctr_drbg_free( &ctr_drbg );
     mbedtls_entropy_free( &entropy );
     mbedtls_rsa_free( &rsa );
diff --git a/programs/pkey/rsa_genkey.c b/programs/pkey/rsa_genkey.c
index e199ad2..9399217 100644
--- a/programs/pkey/rsa_genkey.c
+++ b/programs/pkey/rsa_genkey.c
@@ -65,11 +65,16 @@
     mbedtls_rsa_context rsa;
     mbedtls_entropy_context entropy;
     mbedtls_ctr_drbg_context ctr_drbg;
+    mbedtls_mpi N, P, Q, D, E, DP, DQ, QP;
     FILE *fpub  = NULL;
     FILE *fpriv = NULL;
     const char *pers = "rsa_genkey";
 
     mbedtls_ctr_drbg_init( &ctr_drbg );
+    mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, 0 );
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &P ); mbedtls_mpi_init( &Q );
+    mbedtls_mpi_init( &D ); mbedtls_mpi_init( &E ); mbedtls_mpi_init( &DP );
+    mbedtls_mpi_init( &DQ ); mbedtls_mpi_init( &QP );
 
     mbedtls_printf( "\n  . Seeding the random number generator..." );
     fflush( stdout );
@@ -86,10 +91,8 @@
     mbedtls_printf( " ok\n  . Generating the RSA key [ %d-bit ]...", KEY_SIZE );
     fflush( stdout );
 
-    mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, 0 );
-
     if( ( ret = mbedtls_rsa_gen_key( &rsa, mbedtls_ctr_drbg_random, &ctr_drbg, KEY_SIZE,
-                             EXPONENT ) ) != 0 )
+                                     EXPONENT ) ) != 0 )
     {
         mbedtls_printf( " failed\n  ! mbedtls_rsa_gen_key returned %d\n\n", ret );
         goto exit;
@@ -98,6 +101,14 @@
     mbedtls_printf( " ok\n  . Exporting the public  key in rsa_pub.txt...." );
     fflush( stdout );
 
+    if( ( ret = mbedtls_rsa_export    ( &rsa, &N, &P, &Q, &D, &E ) ) != 0 ||
+        ( ret = mbedtls_rsa_export_crt( &rsa, &DP, &DQ, &QP ) )      != 0 )
+    {
+        mbedtls_printf( " failed\n  ! could not export RSA parameters\n\n" );
+        ret = 1;
+        goto exit;
+    }
+
     if( ( fpub = fopen( "rsa_pub.txt", "wb+" ) ) == NULL )
     {
         mbedtls_printf( " failed\n  ! could not open rsa_pub.txt for writing\n\n" );
@@ -105,8 +116,8 @@
         goto exit;
     }
 
-    if( ( ret = mbedtls_mpi_write_file( "N = ", &rsa.N, 16, fpub ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "E = ", &rsa.E, 16, fpub ) ) != 0 )
+    if( ( ret = mbedtls_mpi_write_file( "N = ", &N, 16, fpub ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "E = ", &E, 16, fpub ) ) != 0 )
     {
         mbedtls_printf( " failed\n  ! mbedtls_mpi_write_file returned %d\n\n", ret );
         goto exit;
@@ -122,14 +133,14 @@
         goto exit;
     }
 
-    if( ( ret = mbedtls_mpi_write_file( "N = " , &rsa.N , 16, fpriv ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "E = " , &rsa.E , 16, fpriv ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "D = " , &rsa.D , 16, fpriv ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "P = " , &rsa.P , 16, fpriv ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "Q = " , &rsa.Q , 16, fpriv ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "DP = ", &rsa.DP, 16, fpriv ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "DQ = ", &rsa.DQ, 16, fpriv ) ) != 0 ||
-        ( ret = mbedtls_mpi_write_file( "QP = ", &rsa.QP, 16, fpriv ) ) != 0 )
+    if( ( ret = mbedtls_mpi_write_file( "N = " , &N , 16, fpriv ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "E = " , &E , 16, fpriv ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "D = " , &D , 16, fpriv ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "P = " , &P , 16, fpriv ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "Q = " , &Q , 16, fpriv ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "DP = ", &DP, 16, fpriv ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "DQ = ", &DQ, 16, fpriv ) ) != 0 ||
+        ( ret = mbedtls_mpi_write_file( "QP = ", &QP, 16, fpriv ) ) != 0 )
     {
         mbedtls_printf( " failed\n  ! mbedtls_mpi_write_file returned %d\n\n", ret );
         goto exit;
@@ -157,6 +168,9 @@
     if( fpriv != NULL )
         fclose( fpriv );
 
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &P ); mbedtls_mpi_free( &Q );
+    mbedtls_mpi_free( &D ); mbedtls_mpi_free( &E ); mbedtls_mpi_free( &DP );
+    mbedtls_mpi_free( &DQ ); mbedtls_mpi_free( &QP );
     mbedtls_rsa_free( &rsa );
     mbedtls_ctr_drbg_free( &ctr_drbg );
     mbedtls_entropy_free( &entropy );
diff --git a/programs/pkey/rsa_sign.c b/programs/pkey/rsa_sign.c
index affbf7a..89018cb 100644
--- a/programs/pkey/rsa_sign.c
+++ b/programs/pkey/rsa_sign.c
@@ -61,8 +61,14 @@
     unsigned char hash[32];
     unsigned char buf[MBEDTLS_MPI_MAX_SIZE];
     char filename[512];
+    mbedtls_mpi N, P, Q, D, E, DP, DQ, QP;
 
     mbedtls_rsa_init( &rsa, MBEDTLS_RSA_PKCS_V15, 0 );
+
+    mbedtls_mpi_init( &N ); mbedtls_mpi_init( &P ); mbedtls_mpi_init( &Q );
+    mbedtls_mpi_init( &D ); mbedtls_mpi_init( &E ); mbedtls_mpi_init( &DP );
+    mbedtls_mpi_init( &DQ ); mbedtls_mpi_init( &QP );
+
     ret = 1;
 
     if( argc != 2 )
@@ -87,24 +93,35 @@
         goto exit;
     }
 
-    if( ( ret = mbedtls_mpi_read_file( &rsa.N , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.E , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.D , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.P , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.Q , 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.DP, 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.DQ, 16, f ) ) != 0 ||
-        ( ret = mbedtls_mpi_read_file( &rsa.QP, 16, f ) ) != 0 )
+    if( ( ret = mbedtls_mpi_read_file( &N , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &E , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &D , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &P , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &Q , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &DP , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &DQ , 16, f ) ) != 0 ||
+        ( ret = mbedtls_mpi_read_file( &QP , 16, f ) ) != 0 )
     {
         mbedtls_printf( " failed\n  ! mbedtls_mpi_read_file returned %d\n\n", ret );
         fclose( f );
         goto exit;
     }
-
-    rsa.len = ( mbedtls_mpi_bitlen( &rsa.N ) + 7 ) >> 3;
-
     fclose( f );
 
+    if( ( ret = mbedtls_rsa_import( &rsa, &N, &P, &Q, &D, &E ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_rsa_import returned %d\n\n",
+                        ret );
+        goto exit;
+    }
+
+    if( ( ret = mbedtls_rsa_complete( &rsa ) ) != 0 )
+    {
+        mbedtls_printf( " failed\n  ! mbedtls_rsa_complete returned %d\n\n",
+                        ret );
+        goto exit;
+    }
+
     mbedtls_printf( "\n  . Checking the private key" );
     fflush( stdout );
     if( ( ret = mbedtls_rsa_check_privkey( &rsa ) ) != 0 )
@@ -158,6 +175,9 @@
 exit:
 
     mbedtls_rsa_free( &rsa );
+    mbedtls_mpi_free( &N ); mbedtls_mpi_free( &P ); mbedtls_mpi_free( &Q );
+    mbedtls_mpi_free( &D ); mbedtls_mpi_free( &E ); mbedtls_mpi_free( &DP );
+    mbedtls_mpi_free( &DQ ); mbedtls_mpi_free( &QP );
 
 #if defined(_WIN32)
     mbedtls_printf( "  + Press Enter to exit this program.\n" );
diff --git a/programs/ssl/ssl_client2.c b/programs/ssl/ssl_client2.c
index 8e2feb1..5d8969d 100644
--- a/programs/ssl/ssl_client2.c
+++ b/programs/ssl/ssl_client2.c
@@ -73,6 +73,7 @@
 #define DFL_REQUEST_SIZE        -1
 #define DFL_DEBUG_LEVEL         0
 #define DFL_NBIO                0
+#define DFL_EVENT               0
 #define DFL_READ_TIMEOUT        0
 #define DFL_MAX_RESEND          0
 #define DFL_CA_FILE             ""
@@ -245,24 +246,26 @@
     "    server_addr=%%s      default: given by name\n"     \
     "    server_port=%%d      default: 4433\n"              \
     "    request_page=%%s     default: \".\"\n"             \
-    "    request_size=%%d     default: about 34 (basic request)\n"           \
+    "    request_size=%%d     default: about 34 (basic request)\n" \
     "                        (minimum: 0, max: " MAX_REQUEST_SIZE_STR " )\n" \
-    "    debug_level=%%d      default: 0 (disabled)\n"      \
-    "    nbio=%%d             default: 0 (blocking I/O)\n"  \
-    "                        options: 1 (non-blocking), 2 (added delays)\n" \
-    "    read_timeout=%%d     default: 0 ms (no timeout)\n"    \
+    "    debug_level=%%d      default: 0 (disabled)\n"             \
+    "    nbio=%%d             default: 0 (blocking I/O)\n"         \
+    "                        options: 1 (non-blocking), 2 (added delays)\n"   \
+    "    event=%%d            default: 0 (loop)\n"                            \
+    "                        options: 1 (level-triggered, implies nbio=1),\n" \
+    "    read_timeout=%%d     default: 0 ms (no timeout)\n"        \
     "    max_resend=%%d       default: 0 (no resend on timeout)\n" \
     "\n"                                                    \
     USAGE_DTLS                                              \
     "\n"                                                    \
-    "    auth_mode=%%s        default: (library default: none)\n"      \
+    "    auth_mode=%%s        default: (library default: none)\n" \
     "                        options: none, optional, required\n" \
     USAGE_IO                                                \
     "\n"                                                    \
     USAGE_PSK                                               \
     USAGE_ECJPAKE                                           \
     "\n"                                                    \
-    "    allow_legacy=%%d     default: (library default: no)\n"      \
+    "    allow_legacy=%%d     default: (library default: no)\n"   \
     USAGE_RENEGO                                            \
     "    exchanges=%%d        default: 1\n"                 \
     "    reconnect=%%d        default: 0 (disabled)\n"      \
@@ -302,7 +305,8 @@
     const char *server_port;    /* port on which the ssl service runs       */
     int debug_level;            /* level of debugging                       */
     int nbio;                   /* should I/O be blocking?                  */
-    uint32_t read_timeout;      /* timeout on mbedtls_ssl_read() in milliseconds    */
+    int event;                  /* loop or event-driven IO? level or edge triggered? */
+    uint32_t read_timeout;      /* timeout on mbedtls_ssl_read() in milliseconds     */
     int max_resend;             /* DTLS times to resend on read timeout     */
     const char *request_page;   /* page on server to request                */
     int request_size;           /* pad request with header to requested size */
@@ -353,7 +357,8 @@
         if( *p == '/' || *p == '\\' )
             basename = p + 1;
 
-    mbedtls_fprintf( (FILE *) ctx, "%s:%04d: |%d| %s", basename, line, level, str );
+    mbedtls_fprintf( (FILE *) ctx, "%s:%04d: |%d| %s",
+                     basename, line, level, str );
     fflush(  (FILE *) ctx  );
 }
 
@@ -399,7 +404,8 @@
 /*
  * Enabled if debug_level > 1 in code below
  */
-static int my_verify( void *data, mbedtls_x509_crt *crt, int depth, uint32_t *flags )
+static int my_verify( void *data, mbedtls_x509_crt *crt,
+                      int depth, uint32_t *flags )
 {
     char buf[1024];
     ((void) data);
@@ -436,6 +442,57 @@
 };
 #endif /* MBEDTLS_X509_CRT_PARSE_C */
 
+/*
+ * Wait for an event from the underlying transport or the timer
+ * (Used in event-driven IO mode).
+ */
+#if !defined(MBEDTLS_TIMING_C)
+int idle( mbedtls_net_context *fd,
+          int idle_reason )
+#else
+int idle( mbedtls_net_context *fd,
+          mbedtls_timing_delay_context *timer,
+          int idle_reason )
+#endif
+{
+
+    int ret;
+    int poll_type = 0;
+
+    if( idle_reason == MBEDTLS_ERR_SSL_WANT_WRITE )
+        poll_type = MBEDTLS_NET_POLL_WRITE;
+    else if( idle_reason == MBEDTLS_ERR_SSL_WANT_READ )
+        poll_type = MBEDTLS_NET_POLL_READ;
+#if !defined(MBEDTLS_TIMING_C)
+    else
+        return( 0 );
+#endif
+
+    while( 1 )
+    {
+        /* Check if timer has expired */
+#if defined(MBEDTLS_TIMING_C)
+        if( timer != NULL &&
+            mbedtls_timing_get_delay( timer ) == 2 )
+        {
+            break;
+        }
+#endif /* MBEDTLS_TIMING_C */
+
+        /* Check if underlying transport became available */
+        if( poll_type != 0 )
+        {
+            ret = mbedtls_net_poll( fd, poll_type, 0 );
+            if( ret < 0 )
+                return( ret );
+            if( ret == poll_type )
+                break;
+        }
+    }
+
+    return( 0 );
+}
+
 int main( int argc, char *argv[] )
 {
     int ret = 0, len, tail_len, i, written, frags, retry_left;
@@ -521,6 +578,7 @@
     opt.server_port         = DFL_SERVER_PORT;
     opt.debug_level         = DFL_DEBUG_LEVEL;
     opt.nbio                = DFL_NBIO;
+    opt.event               = DFL_EVENT;
     opt.read_timeout        = DFL_READ_TIMEOUT;
     opt.max_resend          = DFL_MAX_RESEND;
     opt.request_page        = DFL_REQUEST_PAGE;
@@ -594,6 +652,12 @@
             if( opt.nbio < 0 || opt.nbio > 2 )
                 goto usage;
         }
+        else if( strcmp( p, "event" ) == 0 )
+        {
+            opt.event = atoi( q );
+            if( opt.event < 0 || opt.event > 2 )
+                goto usage;
+        }
         else if( strcmp( p, "read_timeout" ) == 0 )
             opt.read_timeout = atoi( q );
         else if( strcmp( p, "max_resend" ) == 0 )
@@ -638,16 +702,23 @@
         }
         else if( strcmp( p, "renegotiation" ) == 0 )
         {
-            opt.renegotiation = (atoi( q )) ? MBEDTLS_SSL_RENEGOTIATION_ENABLED :
-                                              MBEDTLS_SSL_RENEGOTIATION_DISABLED;
+            opt.renegotiation = (atoi( q )) ?
+                MBEDTLS_SSL_RENEGOTIATION_ENABLED :
+                MBEDTLS_SSL_RENEGOTIATION_DISABLED;
         }
         else if( strcmp( p, "allow_legacy" ) == 0 )
         {
             switch( atoi( q ) )
             {
-                case -1: opt.allow_legacy = MBEDTLS_SSL_LEGACY_BREAK_HANDSHAKE; break;
-                case 0:  opt.allow_legacy = MBEDTLS_SSL_LEGACY_NO_RENEGOTIATION; break;
-                case 1:  opt.allow_legacy = MBEDTLS_SSL_LEGACY_ALLOW_RENEGOTIATION; break;
+                case -1:
+                    opt.allow_legacy = MBEDTLS_SSL_LEGACY_BREAK_HANDSHAKE;
+                    break;
+                case 0:
+                    opt.allow_legacy = MBEDTLS_SSL_LEGACY_NO_RENEGOTIATION;
+                    break;
+                case 1:
+                    opt.allow_legacy = MBEDTLS_SSL_LEGACY_ALLOW_RENEGOTIATION;
+                    break;
                 default: goto usage;
             }
         }
@@ -704,8 +775,12 @@
         {
             switch( atoi( q ) )
             {
-                case 0: opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_DISABLED; break;
-                case 1: opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_ENABLED; break;
+                case 0:
+                    opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_DISABLED;
+                    break;
+                case 1:
+                    opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_ENABLED;
+                    break;
                 default: goto usage;
             }
         }
@@ -864,6 +939,15 @@
             goto usage;
     }
 
+    /* Event-driven IO is incompatible with the above custom
+     * receive and send functions, as the polling builds on
+     * refers to the underlying net_context. */
+    if( opt.event == 1 && opt.nbio != 1 )
+    {
+        mbedtls_printf( "Warning: event-driven IO mandates nbio=1 - overwrite\n" );
+        opt.nbio = 1;
+    }
+
 #if defined(MBEDTLS_DEBUG_C)
     mbedtls_debug_set_threshold( opt.debug_level );
 #endif
@@ -871,19 +955,20 @@
     if( opt.force_ciphersuite[0] > 0 )
     {
         const mbedtls_ssl_ciphersuite_t *ciphersuite_info;
-        ciphersuite_info = mbedtls_ssl_ciphersuite_from_id( opt.force_ciphersuite[0] );
+        ciphersuite_info =
+            mbedtls_ssl_ciphersuite_from_id( opt.force_ciphersuite[0] );
 
         if( opt.max_version != -1 &&
             ciphersuite_info->min_minor_ver > opt.max_version )
         {
-            mbedtls_printf("forced ciphersuite not allowed with this protocol version\n");
+            mbedtls_printf( "forced ciphersuite not allowed with this protocol version\n" );
             ret = 2;
             goto usage;
         }
         if( opt.min_version != -1 &&
             ciphersuite_info->max_minor_ver < opt.min_version )
         {
-            mbedtls_printf("forced ciphersuite not allowed with this protocol version\n");
+            mbedtls_printf( "forced ciphersuite not allowed with this protocol version\n" );
             ret = 2;
             goto usage;
         }
@@ -909,7 +994,7 @@
         {
             if( opt.arc4 == MBEDTLS_SSL_ARC4_DISABLED )
             {
-                mbedtls_printf("forced RC4 ciphersuite with RC4 disabled\n");
+                mbedtls_printf( "forced RC4 ciphersuite with RC4 disabled\n" );
                 ret = 2;
                 goto usage;
             }
@@ -929,7 +1014,7 @@
 
         if( strlen( opt.psk ) % 2 != 0 )
         {
-            mbedtls_printf("pre-shared key not valid hex\n");
+            mbedtls_printf( "pre-shared key not valid hex\n" );
             goto exit;
         }
 
@@ -946,7 +1031,7 @@
                 c -= 'A' - 10;
             else
             {
-                mbedtls_printf("pre-shared key not valid hex\n");
+                mbedtls_printf( "pre-shared key not valid hex\n" );
                 goto exit;
             }
             psk[ j / 2 ] = c << 4;
@@ -960,7 +1045,7 @@
                 c -= 'A' - 10;
             else
             {
-                mbedtls_printf("pre-shared key not valid hex\n");
+                mbedtls_printf( "pre-shared key not valid hex\n" );
                 goto exit;
             }
             psk[ j / 2 ] |= c;
@@ -1051,11 +1136,12 @@
     fflush( stdout );
 
     mbedtls_entropy_init( &entropy );
-    if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy,
-                               (const unsigned char *) pers,
-                               strlen( pers ) ) ) != 0 )
+    if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func,
+                                       &entropy, (const unsigned char *) pers,
+                                       strlen( pers ) ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_ctr_drbg_seed returned -0x%x\n", -ret );
+        mbedtls_printf( " failed\n  ! mbedtls_ctr_drbg_seed returned -0x%x\n",
+                        -ret );
         goto exit;
     }
 
@@ -1093,12 +1179,13 @@
 #else
     {
         ret = 1;
-        mbedtls_printf("MBEDTLS_CERTS_C not defined.");
+        mbedtls_printf( "MBEDTLS_CERTS_C not defined." );
     }
 #endif
     if( ret < 0 )
     {
-        mbedtls_printf( " failed\n  !  mbedtls_x509_crt_parse returned -0x%x\n\n", -ret );
+        mbedtls_printf( " failed\n  !  mbedtls_x509_crt_parse returned -0x%x\n\n",
+                        -ret );
         goto exit;
     }
 
@@ -1121,7 +1208,8 @@
     else
 #endif
 #if defined(MBEDTLS_CERTS_C)
-        ret = mbedtls_x509_crt_parse( &clicert, (const unsigned char *) mbedtls_test_cli_crt,
+        ret = mbedtls_x509_crt_parse( &clicert,
+                (const unsigned char *) mbedtls_test_cli_crt,
                 mbedtls_test_cli_crt_len );
 #else
     {
@@ -1131,7 +1219,8 @@
 #endif
     if( ret != 0 )
     {
-        mbedtls_printf( " failed\n  !  mbedtls_x509_crt_parse returned -0x%x\n\n", -ret );
+        mbedtls_printf( " failed\n  !  mbedtls_x509_crt_parse returned -0x%x\n\n",
+                        -ret );
         goto exit;
     }
 
@@ -1144,7 +1233,8 @@
     else
 #endif
 #if defined(MBEDTLS_CERTS_C)
-        ret = mbedtls_pk_parse_key( &pkey, (const unsigned char *) mbedtls_test_cli_key,
+        ret = mbedtls_pk_parse_key( &pkey,
+                (const unsigned char *) mbedtls_test_cli_key,
                 mbedtls_test_cli_key_len, NULL, 0 );
 #else
     {
@@ -1154,7 +1244,8 @@
 #endif
     if( ret != 0 )
     {
-        mbedtls_printf( " failed\n  !  mbedtls_pk_parse_key returned -0x%x\n\n", -ret );
+        mbedtls_printf( " failed\n  !  mbedtls_pk_parse_key returned -0x%x\n\n",
+                        -ret );
         goto exit;
     }
 
@@ -1172,11 +1263,13 @@
             opt.server_addr, opt.server_port );
     fflush( stdout );
 
-    if( ( ret = mbedtls_net_connect( &server_fd, opt.server_addr, opt.server_port,
-                             opt.transport == MBEDTLS_SSL_TRANSPORT_STREAM ?
-                             MBEDTLS_NET_PROTO_TCP : MBEDTLS_NET_PROTO_UDP ) ) != 0 )
+    if( ( ret = mbedtls_net_connect( &server_fd,
+                       opt.server_addr, opt.server_port,
+                       opt.transport == MBEDTLS_SSL_TRANSPORT_STREAM ?
+                       MBEDTLS_NET_PROTO_TCP : MBEDTLS_NET_PROTO_UDP ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_net_connect returned -0x%x\n\n", -ret );
+        mbedtls_printf( " failed\n  ! mbedtls_net_connect returned -0x%x\n\n",
+                        -ret );
         goto exit;
     }
 
@@ -1186,7 +1279,8 @@
         ret = mbedtls_net_set_block( &server_fd );
     if( ret != 0 )
     {
-        mbedtls_printf( " failed\n  ! net_set_(non)block() returned -0x%x\n\n", -ret );
+        mbedtls_printf( " failed\n  ! net_set_(non)block() returned -0x%x\n\n",
+                        -ret );
         goto exit;
     }
 
@@ -1203,7 +1297,8 @@
                     opt.transport,
                     MBEDTLS_SSL_PRESET_DEFAULT ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_ssl_config_defaults returned -0x%x\n\n", -ret );
+        mbedtls_printf( " failed\n  ! mbedtls_ssl_config_defaults returned -0x%x\n\n",
+                        -ret );
         goto exit;
     }
 
@@ -1226,13 +1321,15 @@
 
 #if defined(MBEDTLS_SSL_PROTO_DTLS)
     if( opt.hs_to_min != DFL_HS_TO_MIN || opt.hs_to_max != DFL_HS_TO_MAX )
-        mbedtls_ssl_conf_handshake_timeout( &conf, opt.hs_to_min, opt.hs_to_max );
+        mbedtls_ssl_conf_handshake_timeout( &conf, opt.hs_to_min,
+                                            opt.hs_to_max );
 #endif /* MBEDTLS_SSL_PROTO_DTLS */
 
 #if defined(MBEDTLS_SSL_MAX_FRAGMENT_LENGTH)
     if( ( ret = mbedtls_ssl_conf_max_frag_len( &conf, opt.mfl_code ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_max_frag_len returned %d\n\n", ret );
+        mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_max_frag_len returned %d\n\n",
+                        ret );
         goto exit;
     }
 #endif
@@ -1255,8 +1352,8 @@
 #if defined(MBEDTLS_SSL_CBC_RECORD_SPLITTING)
     if( opt.recsplit != DFL_RECSPLIT )
         mbedtls_ssl_conf_cbc_record_splitting( &conf, opt.recsplit
-                                    ? MBEDTLS_SSL_CBC_RECORD_SPLITTING_ENABLED
-                                    : MBEDTLS_SSL_CBC_RECORD_SPLITTING_DISABLED );
+                                  ? MBEDTLS_SSL_CBC_RECORD_SPLITTING_ENABLED
+                                  : MBEDTLS_SSL_CBC_RECORD_SPLITTING_DISABLED );
 #endif
 
 #if defined(MBEDTLS_DHM_C)
@@ -1268,7 +1365,8 @@
     if( opt.alpn_string != NULL )
         if( ( ret = mbedtls_ssl_conf_alpn_protocols( &conf, alpn_list ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_alpn_protocols returned %d\n\n", ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_alpn_protocols returned %d\n\n",
+                            ret );
             goto exit;
         }
 #endif
@@ -1307,7 +1405,8 @@
     {
         if( ( ret = mbedtls_ssl_conf_own_cert( &conf, &clicert, &pkey ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_own_cert returned %d\n\n", ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_own_cert returned %d\n\n",
+                            ret );
             goto exit;
         }
     }
@@ -1326,16 +1425,19 @@
                              (const unsigned char *) opt.psk_identity,
                              strlen( opt.psk_identity ) ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_psk returned %d\n\n", ret );
+        mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_psk returned %d\n\n",
+                        ret );
         goto exit;
     }
 #endif
 
     if( opt.min_version != DFL_MIN_VERSION )
-        mbedtls_ssl_conf_min_version( &conf, MBEDTLS_SSL_MAJOR_VERSION_3, opt.min_version );
+        mbedtls_ssl_conf_min_version( &conf, MBEDTLS_SSL_MAJOR_VERSION_3,
+                                      opt.min_version );
 
     if( opt.max_version != DFL_MAX_VERSION )
-        mbedtls_ssl_conf_max_version( &conf, MBEDTLS_SSL_MAJOR_VERSION_3, opt.max_version );
+        mbedtls_ssl_conf_max_version( &conf, MBEDTLS_SSL_MAJOR_VERSION_3,
+                                      opt.max_version );
 
 #if defined(MBEDTLS_SSL_FALLBACK_SCSV)
     if( opt.fallback != DFL_FALLBACK )
@@ -1344,14 +1446,16 @@
 
     if( ( ret = mbedtls_ssl_setup( &ssl, &conf ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_ssl_setup returned -0x%x\n\n", -ret );
+        mbedtls_printf( " failed\n  ! mbedtls_ssl_setup returned -0x%x\n\n",
+                        -ret );
         goto exit;
     }
 
 #if defined(MBEDTLS_X509_CRT_PARSE_C)
     if( ( ret = mbedtls_ssl_set_hostname( &ssl, opt.server_name ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_ssl_set_hostname returned %d\n\n", ret );
+        mbedtls_printf( " failed\n  ! mbedtls_ssl_set_hostname returned %d\n\n",
+                        ret );
         goto exit;
     }
 #endif
@@ -1363,7 +1467,8 @@
                         (const unsigned char *) opt.ecjpake_pw,
                                         strlen( opt.ecjpake_pw ) ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_set_hs_ecjpake_password returned %d\n\n", ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_set_hs_ecjpake_password returned %d\n\n",
+                            ret );
             goto exit;
         }
     }
@@ -1372,7 +1477,8 @@
     if( opt.nbio == 2 )
         mbedtls_ssl_set_bio( &ssl, &server_fd, my_send, my_recv, NULL );
     else
-        mbedtls_ssl_set_bio( &ssl, &server_fd, mbedtls_net_send, mbedtls_net_recv,
+        mbedtls_ssl_set_bio( &ssl, &server_fd,
+                             mbedtls_net_send, mbedtls_net_recv,
                              opt.nbio == 0 ? mbedtls_net_recv_timeout : NULL );
 
 #if defined(MBEDTLS_TIMING_C)
@@ -1390,9 +1496,11 @@
 
     while( ( ret = mbedtls_ssl_handshake( &ssl ) ) != 0 )
     {
-        if( ret != MBEDTLS_ERR_SSL_WANT_READ && ret != MBEDTLS_ERR_SSL_WANT_WRITE )
+        if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
+            ret != MBEDTLS_ERR_SSL_WANT_WRITE )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_handshake returned -0x%x\n", -ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_handshake returned -0x%x\n",
+                            -ret );
             if( ret == MBEDTLS_ERR_X509_CERT_VERIFY_FAILED )
                 mbedtls_printf(
                     "    Unable to verify the server's certificate. "
@@ -1404,10 +1512,23 @@
             mbedtls_printf( "\n" );
             goto exit;
         }
+
+        /* For event-driven IO, wait for socket to become available */
+        if( opt.event == 1 /* level triggered IO */ )
+        {
+#if defined(MBEDTLS_TIMING_C)
+            ret = idle( &server_fd, &timer, ret );
+#else
+            ret = idle( &server_fd, ret );
+#endif
+            if( ret != 0 )
+                goto exit;
+        }
     }
 
     mbedtls_printf( " ok\n    [ Protocol is %s ]\n    [ Ciphersuite is %s ]\n",
-            mbedtls_ssl_get_version( &ssl ), mbedtls_ssl_get_ciphersuite( &ssl ) );
+                    mbedtls_ssl_get_version( &ssl ),
+                    mbedtls_ssl_get_ciphersuite( &ssl ) );
 
     if( ( ret = mbedtls_ssl_get_record_expansion( &ssl ) ) >= 0 )
         mbedtls_printf( "    [ Record expansion is %d ]\n", ret );
@@ -1435,7 +1556,8 @@
 
         if( ( ret = mbedtls_ssl_get_session( &ssl, &saved_session ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_get_session returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_get_session returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
 
@@ -1454,7 +1576,8 @@
 
         mbedtls_printf( " failed\n" );
 
-        mbedtls_x509_crt_verify_info( vrfy_buf, sizeof( vrfy_buf ), "  ! ", flags );
+        mbedtls_x509_crt_verify_info( vrfy_buf, sizeof( vrfy_buf ),
+                                      "  ! ", flags );
 
         mbedtls_printf( "%s\n", vrfy_buf );
     }
@@ -1484,9 +1607,21 @@
             if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
                 ret != MBEDTLS_ERR_SSL_WANT_WRITE )
             {
-                mbedtls_printf( " failed\n  ! mbedtls_ssl_renegotiate returned %d\n\n", ret );
+                mbedtls_printf( " failed\n  ! mbedtls_ssl_renegotiate returned %d\n\n",
+                                ret );
                 goto exit;
             }
+
+            /* For event-driven IO, wait for socket to become available */
+            if( opt.event == 1 /* level triggered IO */ )
+            {
+#if defined(MBEDTLS_TIMING_C)
+                idle( &server_fd, &timer, ret );
+#else
+                idle( &server_fd, ret );
+#endif
+            }
+
         }
         mbedtls_printf( " ok\n" );
     }
@@ -1530,27 +1665,54 @@
     {
         for( written = 0, frags = 0; written < len; written += ret, frags++ )
         {
-            while( ( ret = mbedtls_ssl_write( &ssl, buf + written, len - written ) )
-                           <= 0 )
+            while( ( ret = mbedtls_ssl_write( &ssl, buf + written,
+                                              len - written ) ) <= 0 )
             {
                 if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
                     ret != MBEDTLS_ERR_SSL_WANT_WRITE )
                 {
-                    mbedtls_printf( " failed\n  ! mbedtls_ssl_write returned -0x%x\n\n", -ret );
+                    mbedtls_printf( " failed\n  ! mbedtls_ssl_write returned -0x%x\n\n",
+                                    -ret );
                     goto exit;
                 }
+
+                /* For event-driven IO, wait for socket to become available */
+                if( opt.event == 1 /* level triggered IO */ )
+                {
+#if defined(MBEDTLS_TIMING_C)
+                    idle( &server_fd, &timer, ret );
+#else
+                    idle( &server_fd, ret );
+#endif
+                }
             }
         }
     }
     else /* Not stream, so datagram */
     {
-        do ret = mbedtls_ssl_write( &ssl, buf, len );
-        while( ret == MBEDTLS_ERR_SSL_WANT_READ ||
-               ret == MBEDTLS_ERR_SSL_WANT_WRITE );
+        while( 1 )
+        {
+            ret = mbedtls_ssl_write( &ssl, buf, len );
+
+            if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
+                ret != MBEDTLS_ERR_SSL_WANT_WRITE )
+                break;
+
+            /* For event-driven IO, wait for socket to become available */
+            if( opt.event == 1 /* level triggered IO */ )
+            {
+#if defined(MBEDTLS_TIMING_C)
+                idle( &server_fd, &timer, ret );
+#else
+                idle( &server_fd, ret );
+#endif
+            }
+        }
 
         if( ret < 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_write returned %d\n\n", ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_write returned %d\n\n",
+                            ret );
             goto exit;
         }
 
@@ -1565,7 +1727,8 @@
     }
 
     buf[written] = '\0';
-    mbedtls_printf( " %d bytes written in %d fragments\n\n%s\n", written, frags, (char *) buf );
+    mbedtls_printf( " %d bytes written in %d fragments\n\n%s\n",
+                    written, frags, (char *) buf );
 
     /*
      * 7. Read the HTTP response
@@ -1586,7 +1749,18 @@
 
             if( ret == MBEDTLS_ERR_SSL_WANT_READ ||
                 ret == MBEDTLS_ERR_SSL_WANT_WRITE )
+            {
+                /* For event-driven IO, wait for socket to become available */
+                if( opt.event == 1 /* level triggered IO */ )
+                {
+#if defined(MBEDTLS_TIMING_C)
+                    idle( &server_fd, &timer, ret );
+#else
+                    idle( &server_fd, ret );
+#endif
+                }
                 continue;
+            }
 
             if( ret <= 0 )
             {
@@ -1604,7 +1778,8 @@
                         goto reconnect;
 
                     default:
-                        mbedtls_printf( " mbedtls_ssl_read returned -0x%x\n", -ret );
+                        mbedtls_printf( " mbedtls_ssl_read returned -0x%x\n",
+                                        -ret );
                         goto exit;
                 }
             }
@@ -1628,9 +1803,24 @@
         len = sizeof( buf ) - 1;
         memset( buf, 0, sizeof( buf ) );
 
-        do ret = mbedtls_ssl_read( &ssl, buf, len );
-        while( ret == MBEDTLS_ERR_SSL_WANT_READ ||
-               ret == MBEDTLS_ERR_SSL_WANT_WRITE );
+        while( 1 )
+        {
+            ret = mbedtls_ssl_read( &ssl, buf, len );
+
+            if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
+                ret != MBEDTLS_ERR_SSL_WANT_WRITE )
+                break;
+
+            /* For event-driven IO, wait for socket to become available */
+            if( opt.event == 1 /* level triggered IO */ )
+            {
+#if defined(MBEDTLS_TIMING_C)
+                idle( &server_fd, &timer, ret );
+#else
+                idle( &server_fd, ret );
+#endif
+            }
+        }
 
         if( ret <= 0 )
         {
@@ -1671,7 +1861,8 @@
 
         if( ( ret = mbedtls_ssl_session_reset( &ssl ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_session_reset returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_session_reset returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
 
@@ -1680,9 +1871,20 @@
             if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
                 ret != MBEDTLS_ERR_SSL_WANT_WRITE )
             {
-                mbedtls_printf( " failed\n  ! mbedtls_ssl_handshake returned -0x%x\n\n", -ret );
+                mbedtls_printf( " failed\n  ! mbedtls_ssl_handshake returned -0x%x\n\n",
+                                -ret );
                 goto exit;
             }
+
+            /* For event-driven IO, wait for socket to become available */
+            if( opt.event == 1 /* level triggered IO */ )
+            {
+#if defined(MBEDTLS_TIMING_C)
+                idle( &server_fd, &timer, ret );
+#else
+                idle( &server_fd, ret );
+#endif
+            }
         }
 
         mbedtls_printf( " ok\n" );
@@ -1729,21 +1931,25 @@
 
         if( ( ret = mbedtls_ssl_session_reset( &ssl ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_session_reset returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_session_reset returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
 
         if( ( ret = mbedtls_ssl_set_session( &ssl, &saved_session ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_session returned %d\n\n", ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_conf_session returned %d\n\n",
+                            ret );
             goto exit;
         }
 
-        if( ( ret = mbedtls_net_connect( &server_fd, opt.server_addr, opt.server_port,
-                                 opt.transport == MBEDTLS_SSL_TRANSPORT_STREAM ?
-                                 MBEDTLS_NET_PROTO_TCP : MBEDTLS_NET_PROTO_UDP ) ) != 0 )
+        if( ( ret = mbedtls_net_connect( &server_fd,
+                        opt.server_addr, opt.server_port,
+                        opt.transport == MBEDTLS_SSL_TRANSPORT_STREAM ?
+                        MBEDTLS_NET_PROTO_TCP : MBEDTLS_NET_PROTO_UDP ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! mbedtls_net_connect returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  ! mbedtls_net_connect returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
 
@@ -1754,7 +1960,7 @@
         if( ret != 0 )
         {
             mbedtls_printf( " failed\n  ! net_set_(non)block() returned -0x%x\n\n",
-                    -ret );
+                            -ret );
             goto exit;
         }
 
@@ -1763,7 +1969,8 @@
             if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
                 ret != MBEDTLS_ERR_SSL_WANT_WRITE )
             {
-                mbedtls_printf( " failed\n  ! mbedtls_ssl_handshake returned -0x%x\n\n", -ret );
+                mbedtls_printf( " failed\n  ! mbedtls_ssl_handshake returned -0x%x\n\n",
+                                -ret );
                 goto exit;
             }
         }
diff --git a/programs/ssl/ssl_server2.c b/programs/ssl/ssl_server2.c
index 2f3908d..02f9633 100644
--- a/programs/ssl/ssl_server2.c
+++ b/programs/ssl/ssl_server2.c
@@ -101,6 +101,7 @@
 #define DFL_SERVER_PORT         "4433"
 #define DFL_DEBUG_LEVEL         0
 #define DFL_NBIO                0
+#define DFL_EVENT               0
 #define DFL_READ_TIMEOUT        0
 #define DFL_CA_FILE             ""
 #define DFL_CA_PATH             ""
@@ -347,6 +348,8 @@
     "    debug_level=%%d      default: 0 (disabled)\n"      \
     "    nbio=%%d             default: 0 (blocking I/O)\n"  \
     "                        options: 1 (non-blocking), 2 (added delays)\n" \
+    "    event=%%d            default: 0 (loop)\n"                            \
+    "                        options: 1 (level-triggered, implies nbio=1),\n" \
     "    read_timeout=%%d     default: 0 ms (no timeout)\n"    \
     "\n"                                                    \
     USAGE_DTLS                                              \
@@ -416,6 +419,7 @@
     const char *server_port;    /* port on which the ssl service runs       */
     int debug_level;            /* level of debugging                       */
     int nbio;                   /* should I/O be blocking?                  */
+    int event;                  /* loop or event-driven IO? level or edge triggered? */
     uint32_t read_timeout;      /* timeout on mbedtls_ssl_read() in milliseconds    */
     const char *ca_file;        /* the file with the CA certificate(s)      */
     const char *ca_path;        /* the path with the CA certificate(s) reside */
@@ -1041,6 +1045,56 @@
 }
 #endif /* MBEDTLS_SSL_ASYNC_PRIVATE_C */
 
+/*
+ * Wait for an event from the underlying transport or the timer
+ * (Used in event-driven IO mode).
+ */
+#if !defined(MBEDTLS_TIMING_C)
+int idle( mbedtls_net_context *fd,
+          int idle_reason )
+#else
+int idle( mbedtls_net_context *fd,
+          mbedtls_timing_delay_context *timer,
+          int idle_reason )
+#endif
+{
+    int ret;
+    int poll_type = 0;
+
+    if( idle_reason == MBEDTLS_ERR_SSL_WANT_WRITE )
+        poll_type = MBEDTLS_NET_POLL_WRITE;
+    else if( idle_reason == MBEDTLS_ERR_SSL_WANT_READ )
+        poll_type = MBEDTLS_NET_POLL_READ;
+#if !defined(MBEDTLS_TIMING_C)
+    else
+        return( 0 );
+#endif
+
+    while( 1 )
+    {
+        /* Check if timer has expired */
+#if defined(MBEDTLS_TIMING_C)
+        if( timer != NULL &&
+            mbedtls_timing_get_delay( timer ) == 2 )
+        {
+            break;
+        }
+#endif /* MBEDTLS_TIMING_C */
+
+        /* Check if underlying transport became available */
+        if( poll_type != 0 )
+        {
+            ret = mbedtls_net_poll( fd, poll_type, 0 );
+            if( ret < 0 )
+                return( ret );
+            if( ret == poll_type )
+                break;
+        }
+    }
+
+    return( 0 );
+}
+
 int main( int argc, char *argv[] )
 {
     int ret = 0, len, written, frags, exchanges_left;
@@ -1176,6 +1230,7 @@
     opt.server_addr         = DFL_SERVER_ADDR;
     opt.server_port         = DFL_SERVER_PORT;
     opt.debug_level         = DFL_DEBUG_LEVEL;
+    opt.event               = DFL_EVENT;
     opt.nbio                = DFL_NBIO;
     opt.read_timeout        = DFL_READ_TIMEOUT;
     opt.ca_file             = DFL_CA_FILE;
@@ -1258,6 +1313,12 @@
             if( opt.nbio < 0 || opt.nbio > 2 )
                 goto usage;
         }
+        else if( strcmp( p, "event" ) == 0 )
+        {
+            opt.event = atoi( q );
+            if( opt.event < 0 || opt.event > 2 )
+                goto usage;
+        }
         else if( strcmp( p, "read_timeout" ) == 0 )
             opt.read_timeout = atoi( q );
         else if( strcmp( p, "ca_file" ) == 0 )
@@ -1318,16 +1379,23 @@
             opt.version_suites = q;
         else if( strcmp( p, "renegotiation" ) == 0 )
         {
-            opt.renegotiation = (atoi( q )) ? MBEDTLS_SSL_RENEGOTIATION_ENABLED :
-                                              MBEDTLS_SSL_RENEGOTIATION_DISABLED;
+            opt.renegotiation = (atoi( q )) ?
+                MBEDTLS_SSL_RENEGOTIATION_ENABLED :
+                MBEDTLS_SSL_RENEGOTIATION_DISABLED;
         }
         else if( strcmp( p, "allow_legacy" ) == 0 )
         {
             switch( atoi( q ) )
             {
-                case -1: opt.allow_legacy = MBEDTLS_SSL_LEGACY_BREAK_HANDSHAKE; break;
-                case 0:  opt.allow_legacy = MBEDTLS_SSL_LEGACY_NO_RENEGOTIATION; break;
-                case 1:  opt.allow_legacy = MBEDTLS_SSL_LEGACY_ALLOW_RENEGOTIATION; break;
+                case -1:
+                    opt.allow_legacy = MBEDTLS_SSL_LEGACY_BREAK_HANDSHAKE;
+                    break;
+                case 0:
+                    opt.allow_legacy = MBEDTLS_SSL_LEGACY_NO_RENEGOTIATION;
+                    break;
+                case 1:
+                    opt.allow_legacy = MBEDTLS_SSL_LEGACY_ALLOW_RENEGOTIATION;
+                    break;
                 default: goto usage;
             }
         }
@@ -1484,8 +1552,12 @@
         {
             switch( atoi( q ) )
             {
-                case 0: opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_DISABLED; break;
-                case 1: opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_ENABLED; break;
+                case 0:
+                    opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_DISABLED;
+                    break;
+                case 1:
+                    opt.extended_ms = MBEDTLS_SSL_EXTENDED_MS_ENABLED;
+                    break;
                 default: goto usage;
             }
         }
@@ -1558,6 +1630,15 @@
             goto usage;
     }
 
+    /* Event-driven IO is incompatible with the above custom
+     * receive and send functions, as the polling builds on
+     * refers to the underlying net_context. */
+    if( opt.event == 1 && opt.nbio != 1 )
+    {
+        mbedtls_printf( "Warning: event-driven IO mandates nbio=1 - overwrite\n" );
+        opt.nbio = 1;
+    }
+
 #if defined(MBEDTLS_DEBUG_C)
     mbedtls_debug_set_threshold( opt.debug_level );
 #endif
@@ -1565,19 +1646,20 @@
     if( opt.force_ciphersuite[0] > 0 )
     {
         const mbedtls_ssl_ciphersuite_t *ciphersuite_info;
-        ciphersuite_info = mbedtls_ssl_ciphersuite_from_id( opt.force_ciphersuite[0] );
+        ciphersuite_info =
+            mbedtls_ssl_ciphersuite_from_id( opt.force_ciphersuite[0] );
 
         if( opt.max_version != -1 &&
             ciphersuite_info->min_minor_ver > opt.max_version )
         {
-            mbedtls_printf("forced ciphersuite not allowed with this protocol version\n");
+            mbedtls_printf( "forced ciphersuite not allowed with this protocol version\n" );
             ret = 2;
             goto usage;
         }
         if( opt.min_version != -1 &&
             ciphersuite_info->max_minor_ver < opt.min_version )
         {
-            mbedtls_printf("forced ciphersuite not allowed with this protocol version\n");
+            mbedtls_printf( "forced ciphersuite not allowed with this protocol version\n" );
             ret = 2;
             goto usage;
         }
@@ -1756,11 +1838,12 @@
     fflush( stdout );
 
     mbedtls_entropy_init( &entropy );
-    if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy,
-                               (const unsigned char *) pers,
-                               strlen( pers ) ) ) != 0 )
+    if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func,
+                                       &entropy, (const unsigned char *) pers,
+                                       strlen( pers ) ) ) != 0 )
     {
-        mbedtls_printf( " failed\n  ! mbedtls_ctr_drbg_seed returned -0x%x\n", -ret );
+        mbedtls_printf( " failed\n  ! mbedtls_ctr_drbg_seed returned -0x%x\n",
+                        -ret );
         goto exit;
     }
 
@@ -1857,7 +1940,7 @@
         if( ( ret = mbedtls_pk_parse_keyfile( &pkey2, opt.key_file2, "" ) ) != 0 )
         {
             mbedtls_printf( " failed\n  !  mbedtls_pk_parse_keyfile(2) returned -0x%x\n\n",
-                    -ret );
+                            -ret );
             goto exit;
         }
     }
@@ -1875,8 +1958,7 @@
         strcmp( opt.key_file2, "none" ) != 0 )
     {
 #if !defined(MBEDTLS_CERTS_C)
-        mbedtls_printf( "Not certificated or key provided, and \n"
-                "MBEDTLS_CERTS_C not defined!\n" );
+        mbedtls_printf( "Not certificated or key provided, and \nMBEDTLS_CERTS_C not defined!\n" );
         goto exit;
 #else
 #if defined(MBEDTLS_RSA_C)
@@ -1884,14 +1966,16 @@
                                     (const unsigned char *) mbedtls_test_srv_crt_rsa,
                                     mbedtls_test_srv_crt_rsa_len ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  !  mbedtls_x509_crt_parse returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  !  mbedtls_x509_crt_parse returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
         if( ( ret = mbedtls_pk_parse_key( &pkey,
                                   (const unsigned char *) mbedtls_test_srv_key_rsa,
                                   mbedtls_test_srv_key_rsa_len, NULL, 0 ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  !  mbedtls_pk_parse_key returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  !  mbedtls_pk_parse_key returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
         key_cert_init = 2;
@@ -1901,14 +1985,16 @@
                                     (const unsigned char *) mbedtls_test_srv_crt_ec,
                                     mbedtls_test_srv_crt_ec_len ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  !  x509_crt_parse2 returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  !  x509_crt_parse2 returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
         if( ( ret = mbedtls_pk_parse_key( &pkey2,
                                   (const unsigned char *) mbedtls_test_srv_key_ec,
                                   mbedtls_test_srv_key_ec_len, NULL, 0 ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  !  pk_parse_key2 returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  !  pk_parse_key2 returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
         key_cert_init2 = 2;
@@ -2303,8 +2389,10 @@
 #if !defined(_WIN32)
     if( received_sigterm )
     {
-        mbedtls_printf( " interrupted by SIGTERM\n" );
-        ret = 0;
+        mbedtls_printf( " interrupted by SIGTERM (not in net_accept())\n" );
+        if( ret == MBEDTLS_ERR_NET_INVALID_CONTEXT )
+            ret = 0;
+
         goto exit;
     }
 #endif
@@ -2340,8 +2428,10 @@
 #if !defined(_WIN32)
         if( received_sigterm )
         {
-            mbedtls_printf( " interrupted by signal\n" );
-            ret = 0;
+            mbedtls_printf( " interrupted by SIGTERM (in net_accept())\n" );
+            if( ret == MBEDTLS_ERR_NET_ACCEPT_FAILED )
+                ret = 0;
+
             goto exit;
         }
 #endif
@@ -2368,8 +2458,8 @@
         if( ( ret = mbedtls_ssl_set_client_transport_id( &ssl,
                         client_ip, cliip_len ) ) != 0 )
         {
-            mbedtls_printf( " failed\n  ! "
-                    "mbedtls_ssl_set_client_transport_id() returned -0x%x\n\n", -ret );
+            mbedtls_printf( " failed\n  ! mbedtls_ssl_set_client_transport_id() returned -0x%x\n\n",
+                            -ret );
             goto exit;
         }
     }
@@ -2397,9 +2487,8 @@
     mbedtls_printf( "  . Performing the SSL/TLS handshake..." );
     fflush( stdout );
 
-    do
+    while( ( ret = mbedtls_ssl_handshake( &ssl ) ) != 0 )
     {
-        ret = mbedtls_ssl_handshake( &ssl );
 #if defined(MBEDTLS_SSL_ASYNC_PRIVATE_C)
         if( ret == MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS &&
             ssl_async_keys.inject_error == SSL_ASYNC_INJECT_ERROR_CANCEL )
@@ -2408,10 +2497,24 @@
             break;
         }
 #endif /* MBEDTLS_SSL_ASYNC_PRIVATE_C */
+
+        if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
+            ret != MBEDTLS_ERR_SSL_WANT_WRITE &&
+            ret != MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS )
+            break;
+
+        /* For event-driven IO, wait for socket to become available */
+        if( opt.event == 1 /* level triggered IO */ )
+        {
+#if defined(MBEDTLS_TIMING_C)
+            ret = idle( &client_fd, &timer, ret );
+#else
+            ret = idle( &client_fd, ret );
+#endif
+            if( ret != 0 )
+                goto reset;
+        }
     }
-    while( ret == MBEDTLS_ERR_SSL_WANT_READ ||
-           ret == MBEDTLS_ERR_SSL_WANT_WRITE ||
-           ret == MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS );
 
     if( ret == MBEDTLS_ERR_SSL_HELLO_VERIFY_REQUIRED )
     {
@@ -2523,7 +2626,18 @@
             if( ret == MBEDTLS_ERR_SSL_WANT_READ ||
                 ret == MBEDTLS_ERR_SSL_WANT_WRITE ||
                 ret == MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS )
+            {
+                if( opt.event == 1 /* level triggered IO */ )
+                {
+#if defined(MBEDTLS_TIMING_C)
+                    idle( &client_fd, &timer, ret );
+#else
+                    idle( &client_fd, ret );
+#endif
+                }
+
                 continue;
+            }
 
             if( ret <= 0 )
             {
@@ -2611,7 +2725,37 @@
         len = sizeof( buf ) - 1;
         memset( buf, 0, sizeof( buf ) );
 
-        do ret = mbedtls_ssl_read( &ssl, buf, len );
+        do
+        {
+            /* Without the call to `mbedtls_ssl_check_pending`, it might
+             * happen that the client sends application data in the same
+             * datagram as the Finished message concluding the handshake.
+             * In this case, the application data would be ready to be
+             * processed while the underlying transport wouldn't signal
+             * any further incoming data.
+             *
+             * See the test 'Event-driven I/O: session-id resume, UDP packing'
+             * in tests/ssl-opt.sh.
+             */
+
+            /* For event-driven IO, wait for socket to become available */
+            if( mbedtls_ssl_check_pending( &ssl ) == 0 &&
+                opt.event == 1 /* level triggered IO */ )
+            {
+#if defined(MBEDTLS_TIMING_C)
+                idle( &client_fd, &timer, MBEDTLS_ERR_SSL_WANT_READ );
+#else
+                idle( &client_fd, MBEDTLS_ERR_SSL_WANT_READ );
+#endif
+            }
+
+            ret = mbedtls_ssl_read( &ssl, buf, len );
+
+            /* Note that even if `mbedtls_ssl_check_pending` returns true,
+             * it can happen that the subsequent call to `mbedtls_ssl_read`
+             * returns `MBEDTLS_ERR_SSL_WANT_READ`, because the pending messages
+             * might be discarded (e.g. because they are retransmissions). */
+        }
         while( ret == MBEDTLS_ERR_SSL_WANT_READ ||
                ret == MBEDTLS_ERR_SSL_WANT_WRITE ||
                ret == MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS );
@@ -2656,6 +2800,16 @@
                 mbedtls_printf( " failed\n  ! mbedtls_ssl_renegotiate returned %d\n\n", ret );
                 goto reset;
             }
+
+            /* For event-driven IO, wait for socket to become available */
+            if( opt.event == 1 /* level triggered IO */ )
+            {
+#if defined(MBEDTLS_TIMING_C)
+                idle( &client_fd, &timer, ret );
+#else
+                idle( &client_fd, ret );
+#endif
+            }
         }
 
         mbedtls_printf( " ok\n" );
@@ -2691,15 +2845,40 @@
                     mbedtls_printf( " failed\n  ! mbedtls_ssl_write returned %d\n\n", ret );
                     goto reset;
                 }
+
+                /* For event-driven IO, wait for socket to become available */
+                if( opt.event == 1 /* level triggered IO */ )
+                {
+#if defined(MBEDTLS_TIMING_C)
+                    idle( &client_fd, &timer, ret );
+#else
+                    idle( &client_fd, ret );
+#endif
+                }
             }
         }
     }
     else /* Not stream, so datagram */
     {
-        do ret = mbedtls_ssl_write( &ssl, buf, len );
-        while( ret == MBEDTLS_ERR_SSL_WANT_READ ||
-               ret == MBEDTLS_ERR_SSL_WANT_WRITE ||
-               ret == MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS );
+        while( 1 )
+        {
+            ret = mbedtls_ssl_write( &ssl, buf, len );
+
+            if( ret != MBEDTLS_ERR_SSL_WANT_READ &&
+                ret != MBEDTLS_ERR_SSL_WANT_WRITE &&
+                ret != MBEDTLS_ERR_SSL_ASYNC_IN_PROGRESS )
+                break;
+
+            /* For event-driven IO, wait for socket to become available */
+            if( opt.event == 1 /* level triggered IO */ )
+            {
+#if defined(MBEDTLS_TIMING_C)
+                idle( &client_fd, &timer, ret );
+#else
+                idle( &client_fd, ret );
+#endif
+            }
+        }
 
         if( ret < 0 )
         {
diff --git a/programs/test/benchmark.c b/programs/test/benchmark.c
index a2677af..cecf3e3 100644
--- a/programs/test/benchmark.c
+++ b/programs/test/benchmark.c
@@ -131,15 +131,6 @@
                      ( mbedtls_timing_hardclock() - tsc ) / ( jj * BUFSIZE ) );         \
 } while( 0 )
 
-#if defined(MBEDTLS_ERROR_C)
-#define PRINT_ERROR                                                     \
-        mbedtls_strerror( ret, ( char * )tmp, sizeof( tmp ) );          \
-        mbedtls_printf( "FAILED: %s\n", tmp );
-#else
-#define PRINT_ERROR                                                     \
-        mbedtls_printf( "FAILED: -0x%04x\n", -ret );
-#endif
-
 #if defined(MBEDTLS_MEMORY_BUFFER_ALLOC_C) && defined(MBEDTLS_MEMORY_DEBUG)
 
 #define MEMORY_MEASURE_INIT                                             \
@@ -327,32 +318,32 @@
 
 #if defined(MBEDTLS_MD4_C)
     if( todo.md4 )
-        TIME_AND_TSC( "MD4", mbedtls_md4( buf, BUFSIZE, tmp ) );
+        TIME_AND_TSC( "MD4", mbedtls_md4_ret( buf, BUFSIZE, tmp ) );
 #endif
 
 #if defined(MBEDTLS_MD5_C)
     if( todo.md5 )
-        TIME_AND_TSC( "MD5", mbedtls_md5( buf, BUFSIZE, tmp ) );
+        TIME_AND_TSC( "MD5", mbedtls_md5_ret( buf, BUFSIZE, tmp ) );
 #endif
 
 #if defined(MBEDTLS_RIPEMD160_C)
     if( todo.ripemd160 )
-        TIME_AND_TSC( "RIPEMD160", mbedtls_ripemd160( buf, BUFSIZE, tmp ) );
+        TIME_AND_TSC( "RIPEMD160", mbedtls_ripemd160_ret( buf, BUFSIZE, tmp ) );
 #endif
 
 #if defined(MBEDTLS_SHA1_C)
     if( todo.sha1 )
-        TIME_AND_TSC( "SHA-1", mbedtls_sha1( buf, BUFSIZE, tmp ) );
+        TIME_AND_TSC( "SHA-1", mbedtls_sha1_ret( buf, BUFSIZE, tmp ) );
 #endif
 
 #if defined(MBEDTLS_SHA256_C)
     if( todo.sha256 )
-        TIME_AND_TSC( "SHA-256", mbedtls_sha256( buf, BUFSIZE, tmp, 0 ) );
+        TIME_AND_TSC( "SHA-256", mbedtls_sha256_ret( buf, BUFSIZE, tmp, 0 ) );
 #endif
 
 #if defined(MBEDTLS_SHA512_C)
     if( todo.sha512 )
-        TIME_AND_TSC( "SHA-512", mbedtls_sha512( buf, BUFSIZE, tmp, 0 ) );
+        TIME_AND_TSC( "SHA-512", mbedtls_sha512_ret( buf, BUFSIZE, tmp, 0 ) );
 #endif
 
 #if defined(MBEDTLS_ARC4_C)
@@ -667,14 +658,22 @@
     if( todo.dhm )
     {
         int dhm_sizes[] = { 2048, 3072 };
-        const char *dhm_P[] = {
-            MBEDTLS_DHM_RFC3526_MODP_2048_P,
-            MBEDTLS_DHM_RFC3526_MODP_3072_P,
-        };
-        const char *dhm_G[] = {
-            MBEDTLS_DHM_RFC3526_MODP_2048_G,
-            MBEDTLS_DHM_RFC3526_MODP_3072_G,
-        };
+        static const unsigned char dhm_P_2048[] =
+            MBEDTLS_DHM_RFC3526_MODP_2048_P_BIN;
+        static const unsigned char dhm_P_3072[] =
+            MBEDTLS_DHM_RFC3526_MODP_3072_P_BIN;
+        static const unsigned char dhm_G_2048[] =
+            MBEDTLS_DHM_RFC3526_MODP_2048_G_BIN;
+        static const unsigned char dhm_G_3072[] =
+            MBEDTLS_DHM_RFC3526_MODP_3072_G_BIN;
+
+        const unsigned char *dhm_P[] = { dhm_P_2048, dhm_P_3072 };
+        const size_t dhm_P_size[] = { sizeof( dhm_P_2048 ),
+                                      sizeof( dhm_P_3072 ) };
+
+        const unsigned char *dhm_G[] = { dhm_G_2048, dhm_G_3072 };
+        const size_t dhm_G_size[] = { sizeof( dhm_G_2048 ),
+                                      sizeof( dhm_G_3072 ) };
 
         mbedtls_dhm_context dhm;
         size_t olen;
@@ -682,8 +681,10 @@
         {
             mbedtls_dhm_init( &dhm );
 
-            if( mbedtls_mpi_read_string( &dhm.P, 16, dhm_P[i] ) != 0 ||
-                mbedtls_mpi_read_string( &dhm.G, 16, dhm_G[i] ) != 0 )
+            if( mbedtls_mpi_read_binary( &dhm.P, dhm_P[i],
+                                         dhm_P_size[i] ) != 0 ||
+                mbedtls_mpi_read_binary( &dhm.G, dhm_G[i],
+                                         dhm_G_size[i] ) != 0 )
             {
                 mbedtls_exit( 1 );
             }
@@ -765,9 +766,16 @@
     if( todo.ecdh )
     {
         mbedtls_ecdh_context ecdh;
-#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
         mbedtls_mpi z;
+        const mbedtls_ecp_curve_info montgomery_curve_list[] = {
+#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
+            { MBEDTLS_ECP_DP_CURVE25519, 0, 0, "Curve25519" },
 #endif
+#if defined(MBEDTLS_ECP_DP_CURVE448_ENABLED)
+            { MBEDTLS_ECP_DP_CURVE448, 0, 0, "Curve448" },
+#endif
+            { MBEDTLS_ECP_DP_NONE, 0, 0, 0 }
+        };
         const mbedtls_ecp_curve_info *curve_info;
         size_t olen;
 
@@ -796,27 +804,32 @@
             mbedtls_ecdh_free( &ecdh );
         }
 
-        /* Curve25519 needs to be handled separately */
-#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
-        mbedtls_ecdh_init( &ecdh );
-        mbedtls_mpi_init( &z );
-
-        if( mbedtls_ecp_group_load( &ecdh.grp, MBEDTLS_ECP_DP_CURVE25519 ) != 0 ||
-            mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Qp, myrand, NULL ) != 0 )
+        /* Montgomery curves need to be handled separately */
+        for ( curve_info = montgomery_curve_list;
+              curve_info->grp_id != MBEDTLS_ECP_DP_NONE;
+              curve_info++ )
         {
-            mbedtls_exit( 1 );
+            mbedtls_ecdh_init( &ecdh );
+            mbedtls_mpi_init( &z );
+
+            if( mbedtls_ecp_group_load( &ecdh.grp, curve_info->grp_id ) != 0 ||
+                mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Qp, myrand, NULL ) != 0 )
+            {
+                mbedtls_exit( 1 );
+            }
+
+            mbedtls_snprintf( title, sizeof(title), "ECDHE-%s",
+                              curve_info->name );
+            TIME_PUBLIC(  title, "handshake",
+                    ret |= mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Q,
+                                            myrand, NULL );
+                    ret |= mbedtls_ecdh_compute_shared( &ecdh.grp, &z, &ecdh.Qp, &ecdh.d,
+                                                myrand, NULL ) );
+
+            mbedtls_ecdh_free( &ecdh );
+            mbedtls_mpi_free( &z );
         }
 
-        TIME_PUBLIC(  "ECDHE-Curve25519", "handshake",
-                ret |= mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Q,
-                                        myrand, NULL );
-                ret |= mbedtls_ecdh_compute_shared( &ecdh.grp, &z, &ecdh.Qp, &ecdh.d,
-                                            myrand, NULL ) );
-
-        mbedtls_ecdh_free( &ecdh );
-        mbedtls_mpi_free( &z );
-#endif
-
         for( curve_info = mbedtls_ecp_curve_list();
              curve_info->grp_id != MBEDTLS_ECP_DP_NONE;
              curve_info++ )
@@ -842,26 +855,31 @@
             mbedtls_ecdh_free( &ecdh );
         }
 
-        /* Curve25519 needs to be handled separately */
-#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
-        mbedtls_ecdh_init( &ecdh );
-        mbedtls_mpi_init( &z );
-
-        if( mbedtls_ecp_group_load( &ecdh.grp, MBEDTLS_ECP_DP_CURVE25519 ) != 0 ||
-            mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Qp,
-                             myrand, NULL ) != 0 ||
-            mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Q, myrand, NULL ) != 0 )
+        /* Montgomery curves need to be handled separately */
+        for ( curve_info = montgomery_curve_list;
+              curve_info->grp_id != MBEDTLS_ECP_DP_NONE;
+              curve_info++)
         {
-            mbedtls_exit( 1 );
+            mbedtls_ecdh_init( &ecdh );
+            mbedtls_mpi_init( &z );
+
+            if( mbedtls_ecp_group_load( &ecdh.grp, curve_info->grp_id ) != 0 ||
+                mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Qp,
+                                 myrand, NULL ) != 0 ||
+                mbedtls_ecdh_gen_public( &ecdh.grp, &ecdh.d, &ecdh.Q, myrand, NULL ) != 0 )
+            {
+                mbedtls_exit( 1 );
+            }
+
+            mbedtls_snprintf( title, sizeof(title), "ECDH-%s",
+                              curve_info->name );
+            TIME_PUBLIC(  title, "handshake",
+                    ret |= mbedtls_ecdh_compute_shared( &ecdh.grp, &z, &ecdh.Qp, &ecdh.d,
+                                                myrand, NULL ) );
+
+            mbedtls_ecdh_free( &ecdh );
+            mbedtls_mpi_free( &z );
         }
-
-        TIME_PUBLIC(  "ECDH-Curve25519", "handshake",
-                ret |= mbedtls_ecdh_compute_shared( &ecdh.grp, &z, &ecdh.Qp, &ecdh.d,
-                                            myrand, NULL ) );
-
-        mbedtls_ecdh_free( &ecdh );
-        mbedtls_mpi_free( &z );
-#endif
     }
 #endif
 
diff --git a/programs/test/udp_proxy.c b/programs/test/udp_proxy.c
index 20624d2..5797f3d 100644
--- a/programs/test/udp_proxy.c
+++ b/programs/test/udp_proxy.c
@@ -53,6 +53,7 @@
 #include "mbedtls/net_sockets.h"
 #include "mbedtls/error.h"
 #include "mbedtls/ssl.h"
+#include "mbedtls/timing.h"
 
 #include <string.h>
 
@@ -74,17 +75,21 @@
 #include <unistd.h>
 #endif /* ( _WIN32 || _WIN32_WCE ) && !EFIX64 && !EFI32 */
 
-/* For gettimeofday() */
-#if !defined(_WIN32)
-#include <sys/time.h>
-#endif
-
 #define MAX_MSG_SIZE            16384 + 2048 /* max record/datagram size */
 
 #define DFL_SERVER_ADDR         "localhost"
 #define DFL_SERVER_PORT         "4433"
 #define DFL_LISTEN_ADDR         "localhost"
 #define DFL_LISTEN_PORT         "5556"
+#define DFL_PACK                0
+
+#if defined(MBEDTLS_TIMING_C)
+#define USAGE_PACK                                                          \
+    "    pack=%%d             default: 0     (don't pack)\n"                \
+    "                         options: t > 0 (pack for t milliseconds)\n"
+#else
+#define USAGE_PACK
+#endif
 
 #define USAGE                                                               \
     "\n usage: udp_proxy param=<>...\n"                                     \
@@ -105,9 +110,10 @@
     "                        drop packets larger than N bytes\n"            \
     "    bad_ad=0/1          default: 0 (don't add bad ApplicationData)\n"  \
     "    protect_hvr=0/1     default: 0 (don't protect HelloVerifyRequest)\n" \
-    "    protect_len=%%d     default: (don't protect packets of this size)\n" \
+    "    protect_len=%%d      default: (don't protect packets of this size)\n" \
     "\n"                                                                    \
     "    seed=%%d             default: (use current time)\n"                \
+    USAGE_PACK                                                              \
     "\n"
 
 /*
@@ -128,7 +134,8 @@
     int bad_ad;                 /* inject corrupted ApplicationData record  */
     int protect_hvr;            /* never drop or delay HelloVerifyRequest   */
     int protect_len;            /* never drop/delay packet of the given size*/
-
+    unsigned pack;              /* merge packets into single datagram for
+                                 * at most \c merge milliseconds if > 0     */
     unsigned int seed;          /* seed for "random" events                 */
 } opt;
 
@@ -152,6 +159,7 @@
     opt.server_port    = DFL_SERVER_PORT;
     opt.listen_addr    = DFL_LISTEN_ADDR;
     opt.listen_port    = DFL_LISTEN_PORT;
+    opt.pack           = DFL_PACK;
     /* Other members default to 0 */
 
     for( i = 1; i < argc; i++ )
@@ -193,6 +201,15 @@
             if( opt.drop < 0 || opt.drop > 20 || opt.drop == 1 )
                 exit_usage( p, q );
         }
+        else if( strcmp( p, "pack" ) == 0 )
+        {
+#if defined(MBEDTLS_TIMING_C)
+            opt.pack = (unsigned) atoi( q );
+#else
+            mbedtls_printf( " option pack only defined if MBEDTLS_TIMING_C is enabled\n" );
+            exit( 1 );
+#endif
+        }
         else if( strcmp( p, "mtu" ) == 0 )
         {
             opt.mtu = atoi( q );
@@ -267,25 +284,122 @@
     }
 }
 
+#if defined(MBEDTLS_TIMING_C)
 /* Return elapsed time in milliseconds since the first call */
-static unsigned long ellapsed_time( void )
+static unsigned ellapsed_time( void )
 {
-#if defined(_WIN32)
-    return( 0 );
-#else
-    static struct timeval ref = { 0, 0 };
-    struct timeval now;
+    static int initialized = 0;
+    static struct mbedtls_timing_hr_time hires;
 
-    if( ref.tv_sec == 0 && ref.tv_usec == 0 )
+    if( initialized == 0 )
     {
-        gettimeofday( &ref, NULL );
+        (void) mbedtls_timing_get_timer( &hires, 1 );
+        initialized = 1;
         return( 0 );
     }
 
-    gettimeofday( &now, NULL );
-    return( 1000 * ( now.tv_sec  - ref.tv_sec )
-                 + ( now.tv_usec - ref.tv_usec ) / 1000 );
-#endif
+    return( mbedtls_timing_get_timer( &hires, 0 ) );
+}
+
+typedef struct
+{
+    mbedtls_net_context *ctx;
+
+    const char *description;
+
+    unsigned packet_lifetime;
+    unsigned num_datagrams;
+
+    unsigned char data[MAX_MSG_SIZE];
+    size_t len;
+
+} ctx_buffer;
+
+static ctx_buffer outbuf[2];
+
+static int ctx_buffer_flush( ctx_buffer *buf )
+{
+    int ret;
+
+    mbedtls_printf( "  %05u flush    %s: %u bytes, %u datagrams, last %u ms\n",
+                    ellapsed_time(), buf->description,
+                    (unsigned) buf->len, buf->num_datagrams,
+                    ellapsed_time() - buf->packet_lifetime );
+
+    ret = mbedtls_net_send( buf->ctx, buf->data, buf->len );
+
+    buf->len           = 0;
+    buf->num_datagrams = 0;
+
+    return( ret );
+}
+
+static unsigned ctx_buffer_time_remaining( ctx_buffer *buf )
+{
+    unsigned const cur_time = ellapsed_time();
+
+    if( buf->num_datagrams == 0 )
+        return( (unsigned) -1 );
+
+    if( cur_time - buf->packet_lifetime >= opt.pack )
+        return( 0 );
+
+    return( opt.pack - ( cur_time - buf->packet_lifetime ) );
+}
+
+static int ctx_buffer_append( ctx_buffer *buf,
+                              const unsigned char * data,
+                              size_t len )
+{
+    int ret;
+
+    if( len > (size_t) INT_MAX )
+        return( -1 );
+
+    if( len > sizeof( buf->data ) )
+    {
+        mbedtls_printf( "  ! buffer size %u too large (max %u)\n",
+                        (unsigned) len, (unsigned) sizeof( buf->data ) );
+        return( -1 );
+    }
+
+    if( sizeof( buf->data ) - buf->len < len )
+    {
+        if( ( ret = ctx_buffer_flush( buf ) ) <= 0 )
+            return( ret );
+    }
+
+    memcpy( buf->data + buf->len, data, len );
+
+    buf->len += len;
+    if( ++buf->num_datagrams == 1 )
+        buf->packet_lifetime = ellapsed_time();
+
+    return( (int) len );
+}
+#endif /* MBEDTLS_TIMING_C */
+
+static int dispatch_data( mbedtls_net_context *ctx,
+                          const unsigned char * data,
+                          size_t len )
+{
+#if defined(MBEDTLS_TIMING_C)
+    ctx_buffer *buf = NULL;
+    if( opt.pack > 0 )
+    {
+        if( outbuf[0].ctx == ctx )
+            buf = &outbuf[0];
+        else if( outbuf[1].ctx == ctx )
+            buf = &outbuf[1];
+
+        if( buf == NULL )
+            return( -1 );
+
+        return( ctx_buffer_append( buf, data, len ) );
+    }
+#endif /* MBEDTLS_TIMING_C */
+
+    return( mbedtls_net_send( ctx, data, len ) );
 }
 
 typedef struct
@@ -300,12 +414,22 @@
 /* Print packet. Outgoing packets come with a reason (forward, dupl, etc.) */
 void print_packet( const packet *p, const char *why )
 {
+#if defined(MBEDTLS_TIMING_C)
     if( why == NULL )
-        mbedtls_printf( "  %05lu %s %s (%u bytes)\n",
+        mbedtls_printf( "  %05u dispatch %s %s (%u bytes)\n",
                 ellapsed_time(), p->way, p->type, p->len );
     else
-        mbedtls_printf( "        %s %s (%u bytes): %s\n",
+        mbedtls_printf( "  %05u dispatch %s %s (%u bytes): %s\n",
+                ellapsed_time(), p->way, p->type, p->len, why );
+#else
+    if( why == NULL )
+        mbedtls_printf( "        dispatch %s %s (%u bytes)\n",
+                p->way, p->type, p->len );
+    else
+        mbedtls_printf( "        dispatch %s %s (%u bytes): %s\n",
                 p->way, p->type, p->len, why );
+#endif
+
     fflush( stdout );
 }
 
@@ -320,20 +444,28 @@
     {
         unsigned char buf[MAX_MSG_SIZE];
         memcpy( buf, p->buf, p->len );
-        ++buf[p->len - 1];
 
-        print_packet( p, "corrupted" );
-        if( ( ret = mbedtls_net_send( dst, buf, p->len ) ) <= 0 )
+        if( p->len <= 13 )
         {
-            mbedtls_printf( "  ! mbedtls_net_send returned %d\n", ret );
+            mbedtls_printf( "  ! can't corrupt empty AD record" );
+        }
+        else
+        {
+            ++buf[13];
+            print_packet( p, "corrupted" );
+        }
+
+        if( ( ret = dispatch_data( dst, buf, p->len ) ) <= 0 )
+        {
+            mbedtls_printf( "  ! dispatch returned %d\n", ret );
             return( ret );
         }
     }
 
     print_packet( p, why );
-    if( ( ret = mbedtls_net_send( dst, p->buf, p->len ) ) <= 0 )
+    if( ( ret = dispatch_data( dst, p->buf, p->len ) ) <= 0 )
     {
-        mbedtls_printf( "  ! mbedtls_net_send returned %d\n", ret );
+        mbedtls_printf( "  ! dispatch returned %d\n", ret );
         return( ret );
     }
 
@@ -344,9 +476,9 @@
     {
         print_packet( p, "duplicated" );
 
-        if( ( ret = mbedtls_net_send( dst, p->buf, p->len ) ) <= 0 )
+        if( ( ret = dispatch_data( dst, p->buf, p->len ) ) <= 0 )
         {
-            mbedtls_printf( "  ! mbedtls_net_send returned %d\n", ret );
+            mbedtls_printf( "  ! dispatch returned %d\n", ret );
             return( ret );
         }
     }
@@ -472,6 +604,12 @@
 
     mbedtls_net_context listen_fd, client_fd, server_fd;
 
+#if defined( MBEDTLS_TIMING_C )
+    struct timeval tm;
+#endif
+
+    struct timeval *tm_ptr = NULL;
+
     int nb_fds;
     fd_set read_fds;
 
@@ -560,14 +698,65 @@
         nb_fds = listen_fd.fd;
     ++nb_fds;
 
+#if defined(MBEDTLS_TIMING_C)
+    if( opt.pack > 0 )
+    {
+        outbuf[0].ctx = &server_fd;
+        outbuf[0].description = "S <- C";
+        outbuf[0].num_datagrams = 0;
+        outbuf[0].len = 0;
+
+        outbuf[1].ctx = &client_fd;
+        outbuf[1].description = "S -> C";
+        outbuf[1].num_datagrams = 0;
+        outbuf[1].len = 0;
+    }
+#endif /* MBEDTLS_TIMING_C */
+
     while( 1 )
     {
+#if defined(MBEDTLS_TIMING_C)
+        if( opt.pack > 0 )
+        {
+            unsigned max_wait_server, max_wait_client, max_wait;
+            max_wait_server = ctx_buffer_time_remaining( &outbuf[0] );
+            max_wait_client = ctx_buffer_time_remaining( &outbuf[1] );
+
+            max_wait = (unsigned) -1;
+
+            if( max_wait_server == 0 )
+                ctx_buffer_flush( &outbuf[0] );
+            else
+                max_wait = max_wait_server;
+
+            if( max_wait_client == 0 )
+                ctx_buffer_flush( &outbuf[1] );
+            else
+            {
+                if( max_wait_client < max_wait )
+                    max_wait = max_wait_client;
+            }
+
+            if( max_wait != (unsigned) -1 )
+            {
+                tm.tv_sec  = max_wait / 1000;
+                tm.tv_usec = ( max_wait % 1000 ) * 1000;
+
+                tm_ptr = &tm;
+            }
+            else
+            {
+                tm_ptr = NULL;
+            }
+        }
+#endif /* MBEDTLS_TIMING_C */
+
         FD_ZERO( &read_fds );
         FD_SET( server_fd.fd, &read_fds );
         FD_SET( client_fd.fd, &read_fds );
         FD_SET( listen_fd.fd, &read_fds );
 
-        if( ( ret = select( nb_fds, &read_fds, NULL, NULL, NULL ) ) <= 0 )
+        if( ( ret = select( nb_fds, &read_fds, NULL, NULL, tm_ptr ) ) < 0 )
         {
             perror( "select" );
             goto exit;
@@ -589,6 +778,7 @@
                                         &client_fd, &server_fd ) ) != 0 )
                 goto accept;
         }
+
     }
 
 exit:
diff --git a/programs/test/udp_proxy_wrapper.sh b/programs/test/udp_proxy_wrapper.sh
new file mode 100755
index 0000000..29033d5
--- /dev/null
+++ b/programs/test/udp_proxy_wrapper.sh
@@ -0,0 +1,117 @@
+#!/bin/sh
+# -*-sh-basic-offset: 4-*-
+# Usage: udp_proxy_wrapper.sh [PROXY_PARAM...] -- [SERVER_PARAM...]
+
+set -u
+
+MBEDTLS_BASE="$(dirname -- "$0")/../.."
+TPXY_BIN="$MBEDTLS_BASE/programs/test/udp_proxy"
+SRV_BIN="$MBEDTLS_BASE/programs/ssl/ssl_server2"
+
+: ${VERBOSE:=0}
+
+stop_proxy() {
+    if [ -n "${tpxy_pid:-}" ]; then
+        echo
+        echo "  * Killing proxy (pid $tpxy_pid) ..."
+        kill $tpxy_pid
+    fi
+}
+
+stop_server() {
+    if [ -n "${srv_pid:-}" ]; then
+        echo
+        echo "  * Killing server (pid $srv_pid) ..."
+        kill $srv_pid >/dev/null 2>/dev/null
+    fi
+}
+
+cleanup() {
+    stop_server
+    stop_proxy
+    exit 129
+}
+
+trap cleanup INT TERM HUP
+
+# Extract the proxy parameters
+tpxy_cmd_snippet='"$TPXY_BIN"'
+while [ $# -ne 0 ] && [ "$1" != "--" ]; do
+    tail="$1" quoted=""
+    while [ -n "$tail" ]; do
+        case "$tail" in
+            *\'*) quoted="${quoted}${tail%%\'*}'\\''" tail="${tail#*\'}";;
+            *) quoted="${quoted}${tail}"; tail=; false;;
+        esac
+    done
+    tpxy_cmd_snippet="$tpxy_cmd_snippet '$quoted'"
+    shift
+done
+unset tail quoted
+if [ $# -eq 0 ]; then
+    echo "  * No server arguments (must be preceded by \" -- \") - exit"
+    exit 3
+fi
+shift
+
+dtls_enabled=
+ipv6_in_use=
+server_port_orig=
+server_addr_orig=
+for param; do
+    case "$param" in
+        server_port=*) server_port_orig="${param#*=}";;
+        server_addr=*:*) server_addr_orig="${param#*=}"; ipv6_in_use=1;;
+        server_addr=*) server_addr_orig="${param#*=}";;
+        dtls=[!0]*) dtls_enabled=1;;
+    esac
+done
+
+if [ -z "$dtls_enabled" ] || [ -n "$ipv6_in_use" ]; then
+    echo >&2 "$0: Couldn't find DTLS enabling, or IPv6 is in use - immediate fallback to server application..."
+    if [ $VERBOSE -gt 0 ]; then
+        echo "[ $SRV_BIN $* ]"
+    fi
+    exec "$SRV_BIN" "$@"
+fi
+
+if [ -z "$server_port_orig" ]; then
+    server_port_orig=4433
+fi
+echo "  * Server port:       $server_port_orig"
+tpxy_cmd_snippet="$tpxy_cmd_snippet \"listen_port=\$server_port_orig\""
+tpxy_cmd_snippet="$tpxy_cmd_snippet \"server_port=\$server_port\""
+
+if [ -n "$server_addr_orig" ]; then
+    echo "  * Server address:    $server_addr_orig"
+    tpxy_cmd_snippet="$tpxy_cmd_snippet \"server_addr=\$server_addr_orig\""
+    tpxy_cmd_snippet="$tpxy_cmd_snippet \"listen_addr=\$server_addr_orig\""
+fi
+
+server_port=$(( server_port_orig + 1 ))
+set -- "$@" "server_port=$server_port"
+echo "  * Intermediate port: $server_port"
+
+echo "  * Start proxy in background ..."
+if [ $VERBOSE -gt 0 ]; then
+    echo "[ $tpxy_cmd_snippet ]"
+fi
+eval exec "$tpxy_cmd_snippet" >/dev/null 2>&1 &
+tpxy_pid=$!
+
+if [ $VERBOSE -gt 0 ]; then
+    echo "  * Proxy ID:          $TPXY_PID"
+fi
+
+echo "  * Starting server ..."
+if [ $VERBOSE -gt 0 ]; then
+    echo "[ $SRV_BIN $* ]"
+fi
+
+exec "$SRV_BIN" "$@" >&2 &
+srv_pid=$!
+
+wait $srv_pid
+
+stop_proxy
+return 0