Mbed TLS Benchmark example on Mbed OS

Diff: main.cpp

Revision:

78:5f419e0f96dd

Parent:

76:68ac2a548d58

Child:

80:0f7a38bbcebe

--- a/main.cpp	Mon Oct 15 15:15:18 2018 +0100
+++ b/main.cpp	Thu Oct 18 12:45:07 2018 +0100
@@ -18,20 +18,14 @@
  */
 
 #include "mbed.h"
-#if DEBUG_LEVEL > 0
-#include "mbedtls/debug.h"
-#endif
 
 #if !defined(MBEDTLS_CONFIG_FILE)
 #include "mbedtls/config.h"
 #else
 #include MBEDTLS_CONFIG_FILE
-#endif
+#endif /* MBEDTLS_CONFIG_FILE */
 
 #include "mbedtls/platform.h"
-
-#include <string.h>
-
 #include "mbedtls/md4.h"
 #include "mbedtls/md5.h"
 #include "mbedtls/ripemd160.h"
@@ -56,10 +50,6 @@
 #include "mbedtls/ecdh.h"
 #include "mbedtls/error.h"
 
-#if defined(MBEDTLS_MEMORY_BUFFER_ALLOC_C)
-#include "mbedtls/memory_buffer_alloc.h"
-#endif
-
 #define RSA_PRIVATE_KEY_2048                                          \
 "-----BEGIN RSA PRIVATE KEY-----\n"                                   \
 "MIIEogIBAAKCAQEA2dwVr+IMGEtA2/MCP6fA5eb/6B18Bq6e7gw8brNPkm3E6LyR\n"  \
@@ -142,892 +132,1357 @@
 "rmpXSt07GAxnG6j9jssA95E4rc1zO0CVKG5bvjVTxwi/sT0/VVX7VsJM4uTAQg==\n"  \
 "-----END RSA PRIVATE KEY-----\n"
 
-/*
- * Uncomment this line to enable ECDSA benchmark.
- */
-#define ENABLE_ECDSA
-
-/*
- * For heap usage estimates, we need an estimate of the overhead per allocated
- * block. ptmalloc2/3 (used in gnu libc for instance) uses 2 size_t per block,
- * so use that as our baseline.
- */
-#define MEM_BLOCK_OVERHEAD  ( 2 * sizeof( size_t ) )
-
-/*
- * Size to use for the malloc buffer if MEMORY_BUFFER_ALLOC_C is defined.
- */
-#define HEAP_SIZE       (1u << 16)  // 64k
-
 #define BUFSIZE         1024
 #define HEADER_FORMAT   "  %-24s :  "
 #define TITLE_LEN       25
 
-#define OPTIONS                                                         \
-    "md4, md5, ripemd160, sha1, sha256, sha512,\n"                      \
-    "arc4, camellia, blowfish,\n"                                       \
-    "des3, des, aes_cmac, des3_cmac, aes_cbc, \n"                       \
-    "aes_ctr, aes_gcm, aes_ccm,\n"                                      \
-    "havege, ctr_drbg, hmac_drbg,\n"                                    \
-    "rsa, dhm, ecdsa, ecdh.\n"
-
 #if defined(MBEDTLS_ERROR_C)
-#define PRINT_ERROR                                            \
-        mbedtls_strerror( ret, ( char * )tmp, sizeof( tmp ) ); \
-        mbedtls_printf( "FAILED: %s\n", tmp );
+#define PRINT_ERROR(RET, CODE)                              \
+    mbedtls_strerror(RET, err_buf, sizeof(err_buf));        \
+    mbedtls_printf("%s returned -0x%04X\n", CODE, -RET);    \
+    mbedtls_printf("  !  %s\n", err_buf);
 #else
-#define PRINT_ERROR                                    \
-        mbedtls_printf( "FAILED: -0x%04x\n", -ret );
-#endif
-
-static volatile int alarmed;
-static void alarm() { alarmed = 1; }
-
-#define TIME_AND_TSC( TITLE, CODE )                                            \
-do {                                                                           \
-    unsigned long i;                                                           \
-    Timeout t;                                                                 \
-                                                                               \
-    mbedtls_printf( HEADER_FORMAT, TITLE );                                    \
-                                                                               \
-    for( i = 1, alarmed = 0, t.attach( alarm, 1.0 ); !alarmed; i++ )           \
-    {                                                                          \
-        CODE;                                                                  \
-    }                                                                          \
-                                                                               \
-    mbedtls_printf( "%9lu KB/s\n", i * BUFSIZE / 1024 );                       \
-} while( 0 )
-
-#if defined(MBEDTLS_MEMORY_BUFFER_ALLOC_C) && defined(MBEDTLS_MEMORY_DEBUG)
-
-#define MEMORY_MEASURE_INIT                                        \
-    size_t max_used, max_blocks, max_bytes;                        \
-    size_t prv_used, prv_blocks;                                   \
-    mbedtls_memory_buffer_alloc_cur_get( &prv_used, &prv_blocks ); \
-    mbedtls_memory_buffer_alloc_max_reset( );
-
-#define MEMORY_MEASURE_PRINT( title_len )                          \
-    mbedtls_memory_buffer_alloc_max_get( &max_used, &max_blocks ); \
-    for( i = 12 - title_len; i != 0; i-- ) mbedtls_printf( " " );  \
-    max_used -= prv_used;                                          \
-    max_blocks -= prv_blocks;                                      \
-    max_bytes = max_used + MEM_BLOCK_OVERHEAD * max_blocks;        \
-    mbedtls_printf( "%6u heap bytes", (unsigned) max_bytes );
-
-#else
-#define MEMORY_MEASURE_INIT
-#define MEMORY_MEASURE_PRINT( title_len )
-#endif
+#define PRINT_ERROR(RET, CODE)                              \
+    mbedtls_printf("%s returned -0x%04X\n", CODE, -RET);
+#endif /* MBEDTLS_ERROR_C */
 
-#define TIME_PUBLIC( TITLE, TYPE, CODE )            \
-do {                                                \
-    unsigned long ms;                               \
-    int ret = 0;                                    \
-    Timer t;                                        \
-    MEMORY_MEASURE_INIT;                            \
-                                                    \
-    mbedtls_printf( HEADER_FORMAT, TITLE );         \
-    fflush( stdout );                               \
-                                                    \
-    t.start();                                      \
-    CODE;                                           \
-    t.stop();                                       \
-    ms = t.read_ms();                               \
-                                                    \
-    if( ret != 0 )                                  \
-    {                                               \
-        PRINT_ERROR;                                \
-    }                                               \
-    else                                            \
-    {                                               \
-        mbedtls_printf( "%6lu ms/" TYPE, ms );      \
-        MEMORY_MEASURE_PRINT( sizeof( TYPE ) + 1 ); \
-        mbedtls_printf( "\n" );                     \
-    }                                               \
-} while( 0 )
+#define BENCHMARK_FUNC_CALL(TITLE, CODE)                                    \
+do {                                                                        \
+    unsigned long i;                                                        \
+    Timeout t;                                                              \
+                                                                            \
+    mbedtls_printf(HEADER_FORMAT, TITLE);                                   \
+    fflush(stdout);                                                         \
+                                                                            \
+    for (i = 1, alarmed = 0, t.attach(alarm, 1.0); !alarmed; i++)           \
+    {                                                                       \
+        ret = CODE;                                                         \
+        if (ret == MBEDTLS_ERR_AES_FEATURE_UNAVAILABLE) {                   \
+            mbedtls_printf("Feature unavailable\n");                        \
+            break;                                                          \
+        } else if (ret != 0) {                                              \
+            PRINT_ERROR(ret, #CODE);                                        \
+            goto exit;                                                      \
+        }                                                                   \
+    }                                                                       \
+                                                                            \
+    if (ret == 0) {                                                         \
+        mbedtls_printf("%9lu KB/s\n", i * BUFSIZE / 1024);                  \
+    }                                                                       \
+} while(0)
 
-static int myrand( void *rng_state, unsigned char *output, size_t len )
+#define BENCHMARK_PUBLIC(TITLE, TYPE, CODE)             \
+do {                                                    \
+    unsigned long ms;                                   \
+    Timer t;                                            \
+                                                        \
+    mbedtls_printf(HEADER_FORMAT, TITLE);               \
+    fflush(stdout);                                     \
+                                                        \
+    t.start();                                          \
+    CODE;                                               \
+    t.stop();                                           \
+    ms = t.read_ms();                                   \
+                                                        \
+    if (ret != 0) {                                     \
+        PRINT_ERROR(ret, "Public function");            \
+        goto exit;                                      \
+    } else {                                            \
+        mbedtls_printf("%6lu ms/" TYPE, ms);            \
+        mbedtls_printf("\n");                           \
+    }                                                   \
+} while(0)
+
+/* Clear some memory that was used to prepare the context */
+#if defined(MBEDTLS_ECP_C)
+void ecp_clear_precomputed(mbedtls_ecp_group *grp)
 {
-    size_t use_len;
-    int rnd;
-
-    if( rng_state != NULL )
-        rng_state  = NULL;
-
-    while( len > 0 )
-    {
-        use_len = len;
-        if( use_len > sizeof(int) )
-            use_len = sizeof(int);
-
-        rnd = rand();
-        memcpy( output, &rnd, use_len );
-        output += use_len;
-        len -= use_len;
-    }
-
-    return( 0 );
-}
-
-/*
- * Clear some memory that was used to prepare the context
- */
-#if defined(MBEDTLS_ECP_C)
-void ecp_clear_precomputed( mbedtls_ecp_group *grp )
-{
-    if( grp->T != NULL )
-    {
+    if (grp->T != NULL) {
         size_t i;
-        for( i = 0; i < grp->T_size; i++ )
-            mbedtls_ecp_point_free( &grp->T[i] );
-        mbedtls_free( grp->T );
+        for (i = 0; i < grp->T_size; i++) {
+            mbedtls_ecp_point_free(&grp->T[i]);
+        }
+        mbedtls_free(grp->T);
     }
     grp->T = NULL;
     grp->T_size = 0;
 }
 #else
-#define ecp_clear_precomputed( g )
-#endif
+#define ecp_clear_precomputed(g)
+#endif /* MBEDTLS_ECP_C */
 
-unsigned char buf[BUFSIZE];
+static unsigned char buf[BUFSIZE];
+/*
+ * Buffer used to hold various data such as IV, signatures, keys, etc. ECDSA
+ * seems to be the benchmark that uses the most memory from this buffer as it
+ * is holds the output signature
+ */
+static unsigned char tmp[150];
+/* The longest error message has 134 characters (including \0) */
+static char err_buf[134];
+static char title[TITLE_LEN];
+
+static volatile int alarmed;
+
+static void alarm()
+{
+    alarmed = 1;
+}
 
-typedef struct {
-    char md4, md5, ripemd160, sha1, sha256, sha512,
-         arc4, des3, des, aes_cbc, aes_ctr, aes_gcm, aes_ccm,
-         aes_cmac, des3_cmac, camellia, blowfish,
-         havege, ctr_drbg, hmac_drbg,
-         rsa, dhm, ecdsa, ecdh;
-} todo_list;
+static int myrand(void *rng_state, unsigned char *output, size_t len)
+{
+    size_t use_len;
+    int rnd;
+
+    if (rng_state != NULL) {
+        rng_state  = NULL;
+    }
 
-static int test_md( const todo_list * todo, mbedtls_platform_context* ctx )
-{
-    unsigned char tmp[200];
-    // The call below is used to avoid the "unused parameter" warning.
-    // The context itself can be used by cryptographic calls which require it.
-    // Please refer to https://github.com/ARMmbed/mbedtls/issues/1200 for more information.
-    (void)ctx;
-    memset( tmp, 0xBB, sizeof( tmp ) );
+    while (len > 0) {
+        use_len = len;
+        if (use_len > sizeof(int)) {
+            use_len = sizeof(int);
+        }
+
+        rnd = rand();
+        memcpy(output, &rnd, use_len);
+        output += use_len;
+        len -= use_len;
+    }
+
+    return 0;
+}
 
 #if defined(MBEDTLS_MD4_C)
-    if( todo->md4 )
-        TIME_AND_TSC( "MD4", mbedtls_md4( buf, BUFSIZE, tmp ) );
-#endif
+MBED_NOINLINE static int benchmark_md4()
+{
+    int ret;
+
+    BENCHMARK_FUNC_CALL("MD4", mbedtls_md4_ret(buf, BUFSIZE, tmp));
+
+    ret = 0;
+
+exit:
+
+    return ret;
+}
+#endif /* MBEDTLS_MD4_C */
 
 #if defined(MBEDTLS_MD5_C)
-    if( todo->md5 )
-        TIME_AND_TSC( "MD5", mbedtls_md5( buf, BUFSIZE, tmp ) );
-#endif
+MBED_NOINLINE static int benchmark_md5()
+{
+    int ret;
+
+    BENCHMARK_FUNC_CALL("MD5", mbedtls_md5_ret(buf, BUFSIZE, tmp));
+
+    ret = 0;
+
+exit:
+
+    return ret;
+}
+#endif /* MBEDTLS_MD5_C */
 
 #if defined(MBEDTLS_RIPEMD160_C)
-    if( todo->ripemd160 )
-        TIME_AND_TSC( "RIPEMD160", mbedtls_ripemd160( buf, BUFSIZE, tmp ) );
-#endif
+MBED_NOINLINE static int benchmark_ripemd160()
+{
+    int ret;
+
+    BENCHMARK_FUNC_CALL("RIPEMD160", mbedtls_ripemd160_ret(buf, BUFSIZE, tmp));
+
+    ret = 0;
+
+exit:
+
+    return ret;
+}
+#endif /* MBEDTLS_RIPEMD160_C */
 
 #if defined(MBEDTLS_SHA1_C)
-    if( todo->sha1 )
-        TIME_AND_TSC( "SHA-1", mbedtls_sha1( buf, BUFSIZE, tmp ) );
-#endif
+MBED_NOINLINE static int benchmark_sha1()
+{
+    int ret;
+
+    BENCHMARK_FUNC_CALL("SHA-1", mbedtls_sha1_ret(buf, BUFSIZE, tmp));
+
+    ret = 0;
+
+exit:
+
+    return ret;
+}
+#endif /* MBEDTLS_SHA1_C */
 
 #if defined(MBEDTLS_SHA256_C)
-    if( todo->sha256 )
-        TIME_AND_TSC( "SHA-256", mbedtls_sha256( buf, BUFSIZE, tmp, 0 ) );
-#endif
+MBED_NOINLINE static int benchmark_sha256()
+{
+    int ret;
+
+    BENCHMARK_FUNC_CALL("SHA-256", mbedtls_sha256_ret(buf, BUFSIZE, tmp, 0));
+
+    ret = 0;
+
+exit:
+
+    return ret;
+}
+#endif /* MBEDTLS_SHA256_C */
 
 #if defined(MBEDTLS_SHA512_C)
-    if( todo->sha512 )
-        TIME_AND_TSC( "SHA-512", mbedtls_sha512( buf, BUFSIZE, tmp, 0 ) );
-#endif
-    return ( 0 );
-}
-
-static int test_crypt( const todo_list * todo, mbedtls_platform_context* ctx )
+MBED_NOINLINE static int benchmark_sha512()
 {
-    unsigned char tmp[200];
-    char title[TITLE_LEN];
-    // The call below is used to avoid the "unused parameter" warning.
-    // The context itself can be used by cryptographic calls which require it.
-    // Please refer to https://github.com/ARMmbed/mbedtls/issues/1200 for more information.
-    (void)ctx;
-    memset( tmp, 0xBB, sizeof( tmp ) );
+    int ret;
+
+    BENCHMARK_FUNC_CALL("SHA-512", mbedtls_sha512_ret(buf, BUFSIZE, tmp, 0));
+
+    ret = 0;
+
+exit:
+
+    return ret;
+}
+#endif /* MBEDTLS_SHA512_C */
+
 
 #if defined(MBEDTLS_ARC4_C)
-    if( todo->arc4 )
-    {
-        mbedtls_arc4_context arc4;
-        mbedtls_arc4_init( &arc4 );
-        mbedtls_arc4_setup( &arc4, tmp, 32 );
-        TIME_AND_TSC( "ARC4", mbedtls_arc4_crypt( &arc4, BUFSIZE, buf, buf ) );
-        mbedtls_arc4_free( &arc4 );
+MBED_NOINLINE static int benchmark_arc4()
+{
+    int ret = 0;
+    mbedtls_arc4_context arc4;
+
+    mbedtls_arc4_init(&arc4);
+
+    mbedtls_arc4_setup(&arc4, tmp, 32);
+    BENCHMARK_FUNC_CALL("ARC4",
+                        mbedtls_arc4_crypt(&arc4, BUFSIZE, buf, buf));
+
+    ret = 0;
+
+exit:
+    mbedtls_arc4_free(&arc4);
+
+    return ret;
+}
+#endif /* MBEDTLS_ARC4_C */
+
+#if defined(MBEDTLS_DES_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+MBED_NOINLINE static int benchmark_des3()
+{
+    int ret = 0;
+    mbedtls_des3_context des3;
+
+    mbedtls_des3_init(&des3);
+
+    if ((ret = mbedtls_des3_set3key_enc(&des3, tmp)) != 0) {
+        PRINT_ERROR(ret, "mbedtls_des3_set3key_enc()");
+        goto exit;
     }
-#endif
+    BENCHMARK_FUNC_CALL("3DES",
+                        mbedtls_des3_crypt_cbc(&des3, MBEDTLS_DES_ENCRYPT,
+                                BUFSIZE, tmp, buf, buf));
+
+    ret = 0;
+
+exit:
+    mbedtls_des3_free(&des3);
+
+    return ret;
+}
+#endif /* MBEDTLS_DES_C && MBEDTLS_CIPHER_MODE_CBC */
 
 #if defined(MBEDTLS_DES_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
-    if( todo->des3 )
-    {
-        mbedtls_des3_context des3;
-        mbedtls_des3_init( &des3 );
-        mbedtls_des3_set3key_enc( &des3, tmp );
-        TIME_AND_TSC( "3DES",
-                mbedtls_des3_crypt_cbc( &des3, MBEDTLS_DES_ENCRYPT, BUFSIZE, tmp, buf, buf ) );
-        mbedtls_des3_free( &des3 );
+MBED_NOINLINE static int benchmark_des()
+{
+    int ret = 0;
+    mbedtls_des_context des;
+
+    mbedtls_des_init(&des);
+
+    if ((ret = mbedtls_des_setkey_enc(&des, tmp)) != 0) {
+        PRINT_ERROR(ret, "mbedtls_des_setkey_enc()");
+        goto exit;
+    }
+    BENCHMARK_FUNC_CALL("DES",
+                        mbedtls_des_crypt_cbc(&des, MBEDTLS_DES_ENCRYPT,
+                                BUFSIZE, tmp, buf, buf));
+
+    ret = 0;
+
+exit:
+    mbedtls_des_free(&des);
+
+    return ret;
+}
+#endif /* MBEDTLS_DES_C && MBEDTLS_CIPHER_MODE_CBC */
+
+#if defined(MBEDTLS_DES_C) && defined(MBEDTLS_CIPHER_MODE_CBC) && \
+    defined(MBEDTLS_CMAC_C)
+MBED_NOINLINE static int benchmark_des3_cmac()
+{
+    int ret = 0;
+    unsigned char output[8];
+    const mbedtls_cipher_info_t *cipher_info;
+
+    memset(buf, 0, sizeof(buf));
+    memset(tmp, 0, sizeof(tmp));
+
+    cipher_info = mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_DES_EDE3_ECB);
+    if (cipher_info == NULL) {
+        mbedtls_printf("mbedtls_cipher_info_from_type() returned NULL\n");
+        return -1;
+    }
+
+    BENCHMARK_FUNC_CALL("3DES-CMAC",
+                        mbedtls_cipher_cmac(cipher_info, tmp, 192, buf,
+                                            BUFSIZE, output));
+
+    ret = 0;
+
+exit:
+    return ret;
+}
+#endif /* MBEDTLS_DES_C && MBEDTLS_CIPHER_MODE_CBC && MBEDTLS_CMAC_C */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+MBED_NOINLINE static int benchmark_aes_cbc()
+{
+    int ret = 0;
+    int keysize;
+    mbedtls_aes_context aes;
+
+    mbedtls_aes_init(&aes);
+
+    for (keysize = 128; keysize <= 256; keysize += 64) {
+        ret = mbedtls_snprintf(title, sizeof(title), "AES-CBC-%d", keysize);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
+
+        memset(buf, 0, sizeof(buf));
+        memset(tmp, 0, sizeof(tmp));
+
+        ret = mbedtls_aes_setkey_enc(&aes, tmp, keysize);
+        if (ret == MBEDTLS_ERR_AES_FEATURE_UNAVAILABLE) {
+            /* Do not consider this as a failure */
+            mbedtls_printf(HEADER_FORMAT "Feature unavailable\n", title);
+            continue;
+        } else if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_aes_setkey_enc()");
+            goto exit;
+        }
+
+        BENCHMARK_FUNC_CALL(title,
+                            mbedtls_aes_crypt_cbc(&aes,
+                                    MBEDTLS_AES_ENCRYPT, BUFSIZE,
+                                    tmp, buf, buf));
+    }
+
+    ret = 0;
+
+exit:
+    mbedtls_aes_free(&aes);
+
+    return ret;
+}
+#endif /* MBEDTLS_AES_C && MBEDTLS_CIPHER_MODE_CBC */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CIPHER_MODE_CTR)
+MBED_NOINLINE static int benchmark_aes_ctr()
+{
+    int ret = 0;
+    int keysize;
+    size_t nc_offset = 0;
+    unsigned char stream_block[16];
+    mbedtls_aes_context aes;
+
+    mbedtls_aes_init(&aes);
+
+    for (keysize = 128; keysize <= 256; keysize += 64) {
+        ret = mbedtls_snprintf(title, sizeof(title), "AES-CTR-%d", keysize);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
+
+        memset(buf, 0, sizeof(buf));
+        memset(tmp, 0, sizeof(tmp));
+
+        ret = mbedtls_aes_setkey_enc(&aes, tmp, keysize);
+        if (ret == MBEDTLS_ERR_AES_FEATURE_UNAVAILABLE) {
+            /* Do not consider this as a failure */
+            mbedtls_printf(HEADER_FORMAT "Feature unavailable\n", title);
+            continue;
+        } else if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_aes_setkey_enc()");
+            goto exit;
+        }
+
+        BENCHMARK_FUNC_CALL(title,
+                            mbedtls_aes_crypt_ctr(&aes, BUFSIZE, &nc_offset,
+                                    tmp, stream_block, buf,
+                                    buf));
     }
 
-    if( todo->des )
-    {
-        mbedtls_des_context des;
-        mbedtls_des_init( &des );
-        mbedtls_des_setkey_enc( &des, tmp );
-        TIME_AND_TSC( "DES",
-                mbedtls_des_crypt_cbc( &des, MBEDTLS_DES_ENCRYPT, BUFSIZE, tmp, buf, buf ) );
-        mbedtls_des_free( &des );
-    }
-#if defined(MBEDTLS_CMAC_C)
-    if( todo->des3_cmac )
-    {
-        unsigned char output[8];
-        const mbedtls_cipher_info_t *cipher_info;
+    ret = 0;
+
+exit:
+    mbedtls_aes_free(&aes);
+
+    return ret;
+}
+#endif /* MBEDTLS_AES_C && MBEDTLS_CIPHER_MODE_CTR */
 
-        memset( buf, 0, sizeof( buf ) );
-        memset( tmp, 0, sizeof( tmp ) );
-
-        cipher_info = mbedtls_cipher_info_from_type( MBEDTLS_CIPHER_DES_EDE3_ECB );
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_GCM_C)
+MBED_NOINLINE static int benchmark_aes_gcm()
+{
+    int ret = 0;
+    int keysize;
+    mbedtls_gcm_context gcm;
 
-        TIME_AND_TSC( "3DES-CMAC",
-                      mbedtls_cipher_cmac( cipher_info, tmp, 192, buf,
-                      BUFSIZE, output ) );
-    }
-#endif /* MBEDTLS_CMAC_C */
-#endif
+    mbedtls_gcm_init(&gcm);
 
-#if defined(MBEDTLS_AES_C)
-#if defined(MBEDTLS_CIPHER_MODE_CBC)
-    if( todo->aes_cbc )
-    {
-        int keysize;
-        mbedtls_aes_context aes;
-        mbedtls_aes_init( &aes );
-        for( keysize = 128; keysize <= 256; keysize += 64 )
-        {
-            mbedtls_snprintf( title, sizeof( title ), "AES-CBC-%d", keysize );
-
-            memset( buf, 0, sizeof( buf ) );
-            memset( tmp, 0, sizeof( tmp ) );
-            mbedtls_aes_setkey_enc( &aes, tmp, keysize );
-
-            TIME_AND_TSC( title,
-                mbedtls_aes_crypt_cbc( &aes, MBEDTLS_AES_ENCRYPT, BUFSIZE, tmp, buf, buf ) );
+    for (keysize = 128; keysize <= 256; keysize += 64) {
+        ret = mbedtls_snprintf(title, sizeof(title), "AES-GCM-%d", keysize);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
         }
-        mbedtls_aes_free( &aes );
-    }
-#endif
+
+        memset(buf, 0, sizeof(buf));
+        memset(tmp, 0, sizeof(tmp));
 
-#if defined(MBEDTLS_CIPHER_MODE_CTR)
-    if( todo->aes_ctr )
-    {
-        int keysize;
-        size_t nc_offset = 0;
-        unsigned char stream_block[16];
-        mbedtls_aes_context aes;
-        mbedtls_aes_init( &aes );
-        for( keysize = 128; keysize <= 256; keysize += 64 )
-        {
-            mbedtls_snprintf( title, sizeof( title ), "AES-CTR-%d", keysize );
+        ret = mbedtls_gcm_setkey(&gcm, MBEDTLS_CIPHER_ID_AES, tmp, keysize);
+        if (ret == MBEDTLS_ERR_AES_FEATURE_UNAVAILABLE) {
+            /* Do not consider this as a failure */
+            mbedtls_printf(HEADER_FORMAT "Feature unavailable\n", title);
+            continue;
+        } else if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_gcm_setkey()");
+            goto exit;
+        }
 
-            memset( buf, 0, sizeof( buf ) );
-            memset( tmp, 0, sizeof( tmp ) );
-            mbedtls_aes_setkey_enc( &aes, tmp, keysize );
-
-            TIME_AND_TSC( title,
-                mbedtls_aes_crypt_ctr( &aes, BUFSIZE, &nc_offset, tmp, stream_block, buf, buf ) );
-        }
-        mbedtls_aes_free( &aes );
+        BENCHMARK_FUNC_CALL(title,
+                            mbedtls_gcm_crypt_and_tag(&gcm,
+                                    MBEDTLS_GCM_ENCRYPT,
+                                    BUFSIZE, tmp, 12, NULL,
+                                    0, buf, buf, 16, tmp));
     }
-#endif
 
-#if defined(MBEDTLS_GCM_C)
-    if( todo->aes_gcm )
-    {
-        int keysize;
-        mbedtls_gcm_context gcm;
-
-        mbedtls_gcm_init( &gcm );
-        for( keysize = 128; keysize <= 256; keysize += 64 )
-        {
-            mbedtls_snprintf( title, sizeof( title ), "AES-GCM-%d", keysize );
+    ret = 0;
 
-            memset( buf, 0, sizeof( buf ) );
-            memset( tmp, 0, sizeof( tmp ) );
-            mbedtls_gcm_setkey( &gcm, MBEDTLS_CIPHER_ID_AES, tmp, keysize );
-
-            TIME_AND_TSC( title,
-                    mbedtls_gcm_crypt_and_tag( &gcm, MBEDTLS_GCM_ENCRYPT, BUFSIZE, tmp,
-                        12, NULL, 0, buf, buf, 16, tmp ) );
+exit:
+    mbedtls_gcm_free(&gcm);
 
-            mbedtls_gcm_free( &gcm );
-        }
-    }
-#endif
-#if defined(MBEDTLS_CCM_C)
-    if( todo->aes_ccm )
-    {
-        int keysize;
-        mbedtls_ccm_context ccm;
+    return ret;
+}
+#endif /* MBEDTLS_AES_C && MBEDTLS_GCM_C */
 
-        mbedtls_ccm_init( &ccm );
-        for( keysize = 128; keysize <= 256; keysize += 64 )
-        {
-            mbedtls_snprintf( title, sizeof( title ), "AES-CCM-%d", keysize );
-
-            memset( buf, 0, sizeof( buf ) );
-            memset( tmp, 0, sizeof( tmp ) );
-            mbedtls_ccm_setkey( &ccm, MBEDTLS_CIPHER_ID_AES, tmp, keysize );
-
-            TIME_AND_TSC( title,
-                    mbedtls_ccm_encrypt_and_tag( &ccm, BUFSIZE, tmp,
-                        12, NULL, 0, buf, buf, tmp, 16 ) );
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CCM_C)
+MBED_NOINLINE static int benchmark_aes_ccm()
+{
+    int ret = 0;
+    int keysize;
+    mbedtls_ccm_context ccm;
 
-            mbedtls_ccm_free( &ccm );
-        }
-    }
-#endif
-#if defined(MBEDTLS_CMAC_C)
-    if( todo->aes_cmac )
-    {
-        unsigned char output[16];
-        const mbedtls_cipher_info_t *cipher_info;
-        mbedtls_cipher_type_t cipher_type;
-        int keysize;
+    mbedtls_ccm_init(&ccm);
 
-        cipher_type = MBEDTLS_CIPHER_AES_128_ECB;
-        for( keysize = 128; keysize <= 256; keysize += 64 )
-        {
-            mbedtls_snprintf( title, sizeof( title ), "AES-CMAC-%d", keysize );
-
-            memset( buf, 0, sizeof( buf ) );
-            memset( tmp, 0, sizeof( tmp ) );
-
-            cipher_info = mbedtls_cipher_info_from_type( cipher_type );
-
-            TIME_AND_TSC( title,
-                          mbedtls_cipher_cmac( cipher_info, tmp, keysize,
-                                               buf, BUFSIZE, output ) );
-            cipher_type = (mbedtls_cipher_type_t)( cipher_type + 1 );
+    for (keysize = 128; keysize <= 256; keysize += 64) {
+        ret = mbedtls_snprintf(title, sizeof(title), "AES-CCM-%d", keysize);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
         }
 
-        memset( buf, 0, sizeof( buf ) );
-        memset( tmp, 0, sizeof( tmp ) );
-        TIME_AND_TSC( "AES-CMAC-PRF-128",
-                      mbedtls_aes_cmac_prf_128( tmp, 16, buf, BUFSIZE,
-                                                output ) );
+        memset(buf, 0, sizeof(buf));
+        memset(tmp, 0, sizeof(tmp));
+
+        ret = mbedtls_ccm_setkey(&ccm, MBEDTLS_CIPHER_ID_AES, tmp, keysize);
+        if (ret == MBEDTLS_ERR_AES_FEATURE_UNAVAILABLE) {
+            /* Do not consider this as a failure */
+            mbedtls_printf(HEADER_FORMAT "Feature unavailable\n", title);
+            continue;
+        } else if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_gcm_setkey()");
+            goto exit;
+        }
+
+        BENCHMARK_FUNC_CALL(title,
+                            mbedtls_ccm_encrypt_and_tag(&ccm, BUFSIZE, tmp, 12,
+                                    NULL, 0, buf, buf, tmp,
+                                    16));
     }
-#endif /* MBEDTLS_CMAC_C */
-#endif
+
+    ret = 0;
+
+exit:
+    mbedtls_ccm_free(&ccm);
+
+    return ret;
+}
+#endif /* MBEDTLS_AES_C && MBEDTLS_CCM_C */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CMAC_C)
+MBED_NOINLINE static int benchmark_aes_cmac()
+{
+    int ret = 0;
+    unsigned char output[16];
+    const mbedtls_cipher_info_t *cipher_info;
+    mbedtls_cipher_type_t cipher_type;
+    int keysize;
+
+    cipher_type = MBEDTLS_CIPHER_AES_128_ECB;
+    for (keysize = 128; keysize <= 256; keysize += 64) {
+        ret = mbedtls_snprintf(title, sizeof(title), "AES-CMAC-%d", keysize);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
+
+        memset(buf, 0, sizeof(buf));
+        memset(tmp, 0, sizeof(tmp));
+
+        cipher_info = mbedtls_cipher_info_from_type(cipher_type);
+        if (cipher_info == NULL) {
+            mbedtls_printf("mbedtls_cipher_info_from_type() returned NULL\n");
+            goto exit;
+        }
+
+        BENCHMARK_FUNC_CALL(title,
+                            mbedtls_cipher_cmac(cipher_info, tmp, keysize,
+                                                buf, BUFSIZE, output));
+        cipher_type = (mbedtls_cipher_type_t)(cipher_type + 1);
+    }
+
+    memset(buf, 0, sizeof(buf));
+    memset(tmp, 0, sizeof(tmp));
+
+    BENCHMARK_FUNC_CALL("AES-CMAC-PRF-128",
+                        mbedtls_aes_cmac_prf_128(tmp, 16, buf, BUFSIZE,
+                                output));
+
+    ret = 0;
+
+exit:
+
+    return ret;
+}
+#endif /* MBEDTLS_AES_C && MBEDTLS_CMAC_C */
 
 #if defined(MBEDTLS_CAMELLIA_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
-    if( todo->camellia )
-    {
-        int keysize;
-        mbedtls_camellia_context camellia;
-        mbedtls_camellia_init( &camellia );
-        for( keysize = 128; keysize <= 256; keysize += 64 )
-        {
-            mbedtls_snprintf( title, sizeof( title ), "CAMELLIA-CBC-%d", keysize );
+MBED_NOINLINE static int benchmark_camellia()
+{
+    int ret = 0;
+    int keysize;
+    mbedtls_camellia_context camellia;
+
+    mbedtls_camellia_init(&camellia);
+
+    for (keysize = 128; keysize <= 256; keysize += 64) {
+        ret = mbedtls_snprintf(title, sizeof(title), "CAMELLIA-CBC-%d",
+                               keysize);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
+
+        memset(buf, 0, sizeof(buf));
+        memset(tmp, 0, sizeof(tmp));
 
-            memset( buf, 0, sizeof( buf ) );
-            memset( tmp, 0, sizeof( tmp ) );
-            mbedtls_camellia_setkey_enc( &camellia, tmp, keysize );
+        ret = mbedtls_camellia_setkey_enc(&camellia, tmp, keysize);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_camellia_setkey_enc()");
+            goto exit;
+        }
 
-            TIME_AND_TSC( title,
-                    mbedtls_camellia_crypt_cbc( &camellia, MBEDTLS_CAMELLIA_ENCRYPT,
-                        BUFSIZE, tmp, buf, buf ) );
-        }
-        mbedtls_camellia_free( &camellia );
+        BENCHMARK_FUNC_CALL(title,
+                            mbedtls_camellia_crypt_cbc(&camellia,
+                                    MBEDTLS_CAMELLIA_ENCRYPT,
+                                    BUFSIZE, tmp, buf, buf));
     }
-#endif
+
+    ret = 0;
+
+exit:
+    mbedtls_camellia_free(&camellia);
+
+    return ret;
+}
+#endif /* MBEDTLS_CAMELLIA_C && MBEDTLS_CIPHER_MODE_CBC */
 
 #if defined(MBEDTLS_BLOWFISH_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
-    if( todo->blowfish )
-    {
-        int keysize;
-        mbedtls_blowfish_context blowfish;
-        mbedtls_blowfish_init( &blowfish );
+MBED_NOINLINE static int benchmark_blowfish()
+{
+    int ret = 0;
+    int keysize;
+    mbedtls_blowfish_context *blowfish;
+
+    blowfish = (mbedtls_blowfish_context *)mbedtls_calloc(1,
+               sizeof(mbedtls_blowfish_context));
+    if (blowfish == NULL) {
+        mbedtls_printf("Failed to allocate mbedtls_blowfish_context\n");
+        return -1;
+    }
+
+    mbedtls_blowfish_init(blowfish);
 
-        for( keysize = 128; keysize <= 256; keysize += 64 )
-        {
-            mbedtls_snprintf( title, sizeof( title ), "BLOWFISH-CBC-%d", keysize );
+    for (keysize = 128; keysize <= 256; keysize += 64) {
+        mbedtls_snprintf(title, sizeof(title), "BLOWFISH-CBC-%d", keysize);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
 
-            memset( buf, 0, sizeof( buf ) );
-            memset( tmp, 0, sizeof( tmp ) );
-            mbedtls_blowfish_setkey( &blowfish, tmp, keysize );
+        memset(buf, 0, sizeof(buf));
+        memset(tmp, 0, sizeof(tmp));
 
-            TIME_AND_TSC( title,
-                    mbedtls_blowfish_crypt_cbc( &blowfish, MBEDTLS_BLOWFISH_ENCRYPT, BUFSIZE,
-                        tmp, buf, buf ) );
+        if ((ret = mbedtls_blowfish_setkey(blowfish, tmp, keysize)) != 0) {
+            PRINT_ERROR(ret, "mbedtls_blowfish_setkey()");
+            goto exit;
         }
 
-        mbedtls_blowfish_free( &blowfish );
+        BENCHMARK_FUNC_CALL(title,
+                            mbedtls_blowfish_crypt_cbc(blowfish,
+                                    MBEDTLS_BLOWFISH_ENCRYPT,
+                                    BUFSIZE,
+                                    tmp, buf, buf));
     }
-#endif
-
-    return ( 0 );
-}
 
-static int test_rng( const todo_list * todo, mbedtls_platform_context* ctx )
-{
-    unsigned char tmp[200];
-    // The call below is used to avoid the "unused parameter" warning.
-    // The context itself can be used by cryptographic calls which require it.
-    // Please refer to https://github.com/ARMmbed/mbedtls/issues/1200 for more information.
-    (void)ctx;
-    memset( tmp, 0xBB, sizeof( tmp ) );
+    ret = 0;
+
+exit:
+    mbedtls_blowfish_free(blowfish);
+    mbedtls_free(blowfish);
+
+    return ret;
+}
+#endif /* MBEDTLS_BLOWFISH_C && MBEDTLS_CIPHER_MODE_CBC */
 
 #if defined(MBEDTLS_HAVEGE_C)
-    if( todo->havege )
-    {
-        mbedtls_havege_state hs;
-        mbedtls_havege_init( &hs );
-        TIME_AND_TSC( "HAVEGE", mbedtls_havege_random( &hs, buf, BUFSIZE ) );
-        mbedtls_havege_free( &hs );
-    }
-#endif
+MBED_NOINLINE static int benchmark_havege()
+{
+    int ret = 0;
+    mbedtls_havege_state hs;
+
+    mbedtls_havege_init(&hs);
+
+    BENCHMARK_FUNC_CALL("HAVEGE", mbedtls_havege_random(&hs, buf, BUFSIZE));
+
+    ret = 0;
+
+exit:
+    mbedtls_havege_free(&hs);
+
+    return ret;
+}
+#endif /* MBEDTLS_HAVEGE_C */
 
 #if defined(MBEDTLS_CTR_DRBG_C)
-    if( todo->ctr_drbg )
-    {
-        mbedtls_ctr_drbg_context ctr_drbg;
+MBED_NOINLINE static int benchmark_ctr_drbg()
+{
+    int ret = 0;
+    const char *nopr_title = "CTR_DRBG (NOPR)";
+    const char *pr_title = "CTR_DRBG (PR)";
+    mbedtls_ctr_drbg_context ctr_drbg;
+
+    mbedtls_ctr_drbg_init(&ctr_drbg);
 
-        mbedtls_ctr_drbg_init( &ctr_drbg );
+    ret = mbedtls_ctr_drbg_seed(&ctr_drbg, myrand, NULL, NULL, 0);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_ctr_drbg_seed()");
+        goto exit;
+    }
+
+    BENCHMARK_FUNC_CALL(nopr_title,
+                        mbedtls_ctr_drbg_random(&ctr_drbg, buf, BUFSIZE));
 
-        if( mbedtls_ctr_drbg_seed( &ctr_drbg, myrand, NULL, NULL, 0 ) != 0 )
-            return(1);
-        TIME_AND_TSC( "CTR_DRBG (NOPR)",
-                if( mbedtls_ctr_drbg_random( &ctr_drbg, buf, BUFSIZE ) != 0 )
-                    return(1) );
+    ret = mbedtls_ctr_drbg_seed(&ctr_drbg, myrand, NULL, NULL, 0);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_ctr_drbg_seed()");
+        goto exit;
+    }
 
-        if( mbedtls_ctr_drbg_seed( &ctr_drbg, myrand, NULL, NULL, 0 ) != 0 )
-            return(1);
-        mbedtls_ctr_drbg_set_prediction_resistance( &ctr_drbg, MBEDTLS_CTR_DRBG_PR_ON );
-        TIME_AND_TSC( "CTR_DRBG (PR)",
-                if( mbedtls_ctr_drbg_random( &ctr_drbg, buf, BUFSIZE ) != 0 )
-                    return(1) );
-        mbedtls_ctr_drbg_free( &ctr_drbg );
-    }
-#endif
+    mbedtls_ctr_drbg_set_prediction_resistance(&ctr_drbg,
+            MBEDTLS_CTR_DRBG_PR_ON);
+    BENCHMARK_FUNC_CALL(pr_title,
+                        mbedtls_ctr_drbg_random(&ctr_drbg, buf, BUFSIZE));
+
+    ret = 0;
+
+exit:
+    mbedtls_ctr_drbg_free(&ctr_drbg);
+
+    return ret;
+}
+#endif /* MBEDTLS_CTR_DRBG_C */
 
 #if defined(MBEDTLS_HMAC_DRBG_C)
-    if( todo->hmac_drbg )
-    {
-        mbedtls_hmac_drbg_context hmac_drbg;
-        const mbedtls_md_info_t *md_info;
+MBED_NOINLINE static int benchmark_hmac_drbg()
+{
+    int ret = 0;
+    mbedtls_hmac_drbg_context hmac_drbg;
+    const mbedtls_md_info_t *md_info;
 
-        mbedtls_hmac_drbg_init( &hmac_drbg );
+    mbedtls_hmac_drbg_init(&hmac_drbg);
 
 #if defined(MBEDTLS_SHA1_C)
-        if( ( md_info = mbedtls_md_info_from_type( MBEDTLS_MD_SHA1 ) ) == NULL )
-            return(1);
-
-        if( mbedtls_hmac_drbg_seed( &hmac_drbg, md_info, myrand, NULL, NULL, 0 ) != 0 )
-            return(1);
-        TIME_AND_TSC( "HMAC_DRBG SHA-1 (NOPR)",
-                if( mbedtls_hmac_drbg_random( &hmac_drbg, buf, BUFSIZE ) != 0 )
-                    return(1) );
-        mbedtls_hmac_drbg_free( &hmac_drbg );
+    md_info = mbedtls_md_info_from_type(MBEDTLS_MD_SHA1);
+    if (md_info == NULL) {
+        mbedtls_printf("mbedtls_md_info_from_type() returned NULL\n");
+        ret = -1;
+        goto exit;
+    }
 
-        if( mbedtls_hmac_drbg_seed( &hmac_drbg, md_info, myrand, NULL, NULL, 0 ) != 0 )
-            return(1);
-        mbedtls_hmac_drbg_set_prediction_resistance( &hmac_drbg,
-                                             MBEDTLS_HMAC_DRBG_PR_ON );
-        TIME_AND_TSC( "HMAC_DRBG SHA-1 (PR)",
-                if( mbedtls_hmac_drbg_random( &hmac_drbg, buf, BUFSIZE ) != 0 )
-                    return(1) );
-        mbedtls_hmac_drbg_free( &hmac_drbg );
-#endif
+    ret = mbedtls_hmac_drbg_seed(&hmac_drbg, md_info, myrand, NULL, NULL, 0);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_hmac_drbg_seed()");
+        goto exit;
+    }
+    BENCHMARK_FUNC_CALL("HMAC_DRBG SHA-1 (NOPR)",
+                        mbedtls_hmac_drbg_random(&hmac_drbg, buf, BUFSIZE));
+
+    ret = mbedtls_hmac_drbg_seed(&hmac_drbg, md_info, myrand, NULL, NULL, 0);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_hmac_drbg_seed()");
+        goto exit;
+    }
+    mbedtls_hmac_drbg_set_prediction_resistance(&hmac_drbg,
+            MBEDTLS_HMAC_DRBG_PR_ON);
+    BENCHMARK_FUNC_CALL("HMAC_DRBG SHA-1 (PR)",
+                        mbedtls_hmac_drbg_random(&hmac_drbg, buf, BUFSIZE));
+#endif /* MBEDTLS_SHA1_C */
 
 #if defined(MBEDTLS_SHA256_C)
-        if( ( md_info = mbedtls_md_info_from_type( MBEDTLS_MD_SHA256 ) ) == NULL )
-            return(1);
+    md_info = mbedtls_md_info_from_type(MBEDTLS_MD_SHA256);
+    if (md_info == NULL) {
+        PRINT_ERROR(ret, "mbedtls_md_info_from_type()");
+        goto exit;
+    }
 
-        if( mbedtls_hmac_drbg_seed( &hmac_drbg, md_info, myrand, NULL, NULL, 0 ) != 0 )
-            return(1);
-        TIME_AND_TSC( "HMAC_DRBG SHA-256 (NOPR)",
-                if( mbedtls_hmac_drbg_random( &hmac_drbg, buf, BUFSIZE ) != 0 )
-                    return(1) );
-        mbedtls_hmac_drbg_free( &hmac_drbg );
+    ret = mbedtls_hmac_drbg_seed(&hmac_drbg, md_info, myrand, NULL, NULL, 0);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_hmac_drbg_seed()");
+        goto exit;
+    }
+    BENCHMARK_FUNC_CALL("HMAC_DRBG SHA-256 (NOPR)",
+                        mbedtls_hmac_drbg_random(&hmac_drbg, buf, BUFSIZE));
 
-        if( mbedtls_hmac_drbg_seed( &hmac_drbg, md_info, myrand, NULL, NULL, 0 ) != 0 )
-            return(1);
-        mbedtls_hmac_drbg_set_prediction_resistance( &hmac_drbg,
-                                             MBEDTLS_HMAC_DRBG_PR_ON );
-        TIME_AND_TSC( "HMAC_DRBG SHA-256 (PR)",
-                if( mbedtls_hmac_drbg_random( &hmac_drbg, buf, BUFSIZE ) != 0 )
-                    return(1) );
-        mbedtls_hmac_drbg_free( &hmac_drbg );
-#endif
+    ret = mbedtls_hmac_drbg_seed(&hmac_drbg, md_info, myrand, NULL, NULL, 0);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_hmac_drbg_seed()");
+        goto exit;
     }
-#endif
-    return (0 );
-}
+    mbedtls_hmac_drbg_set_prediction_resistance(&hmac_drbg,
+            MBEDTLS_HMAC_DRBG_PR_ON);
+    BENCHMARK_FUNC_CALL("HMAC_DRBG SHA-256 (PR)",
+                        mbedtls_hmac_drbg_random(&hmac_drbg, buf, BUFSIZE));
+#endif /* MBEDTLS_SHA256_C */
 
-static int test_pk( const todo_list * todo, mbedtls_platform_context* ctx )
-{
-    unsigned char tmp[200];
-    char title[TITLE_LEN];
-    // The call below is used to avoid the "unused parameter" warning.
-    // The context itself can be used by cryptographic calls which require it.
-    // Please refer to https://github.com/ARMmbed/mbedtls/issues/1200 for more information.
-    (void)ctx;
-    memset( tmp, 0xBB, sizeof( tmp ) );
+    ret = 0;
+
+exit:
+    mbedtls_hmac_drbg_free(&hmac_drbg);
+
+    return ret;
+}
+#endif /* MBEDTLS_HMAC_DRBG_C */
 
 #if defined(MBEDTLS_RSA_C) && \
     defined(MBEDTLS_PEM_PARSE_C) && defined(MBEDTLS_PK_PARSE_C)
-    if( todo->rsa )
-    {
-        mbedtls_pk_context pk;
-        mbedtls_rsa_context *rsa;
-        const char *rsa_keys[] = { RSA_PRIVATE_KEY_2048, RSA_PRIVATE_KEY_4096 };
-        size_t i;
+MBED_NOINLINE static int benchmark_rsa()
+{
+    int ret = 0;
+    mbedtls_pk_context pk;
+    mbedtls_rsa_context *rsa;
+    const char *rsa_keys[] = {
+        RSA_PRIVATE_KEY_2048,
+        RSA_PRIVATE_KEY_4096,
+    };
+    size_t i;
 
-        for( i = 0; i < sizeof( rsa_keys ) / sizeof( rsa_keys[0] ); i++ )
-        {
-            mbedtls_pk_init( &pk );
-            mbedtls_pk_parse_key( &pk, (const unsigned char *) rsa_keys[i],
-                                                       strlen( rsa_keys[i] ) + 1, NULL, 0 );
-            rsa = mbedtls_pk_rsa( pk );
+    for (i = 0; i < sizeof(rsa_keys) / sizeof(rsa_keys[0]) && ret == 0; i++) {
+        mbedtls_pk_init(&pk);
+
+        ret = mbedtls_pk_parse_key(&pk, (const unsigned char *)rsa_keys[i],
+                                   strlen(rsa_keys[i]) + 1, NULL, 0);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_pk_parse_key()");
+            goto exit;
+        }
+
+        rsa = mbedtls_pk_rsa(pk);
 
-            mbedtls_snprintf( title, sizeof( title ), "RSA-%d", mbedtls_pk_get_bitlen( &pk ) );
+        ret = mbedtls_snprintf(title, sizeof(title), "RSA-%d",
+                               mbedtls_pk_get_bitlen(&pk));
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
 
-            TIME_PUBLIC( title, " public",
-                    buf[0] = 0;
-                    ret = mbedtls_rsa_public( rsa, buf, buf ) );
+        BENCHMARK_PUBLIC(title, " public",
+                         buf[0] = 0;
+                         ret = mbedtls_rsa_public(rsa, buf, buf));
 
-            TIME_PUBLIC( title, "private",
-                    buf[0] = 0;
-                    ret = mbedtls_rsa_private( rsa, myrand, NULL, buf, buf ) );
+        BENCHMARK_PUBLIC(title, "private",
+                         buf[0] = 0;
+                         ret = mbedtls_rsa_private(rsa, myrand, NULL, buf,
+                                 buf));
 
-            mbedtls_pk_free( &pk );
-        }
+exit:
+        mbedtls_pk_free(&pk);
     }
-#endif
+
+    return ret;
+}
+#endif /* MBEDTLS_RSA_C && MBEDTLS_PEM_PARSE_C && MBEDTLS_PK_PARSE_C */
 
 #if defined(MBEDTLS_DHM_C) && defined(MBEDTLS_BIGNUM_C)
-    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,
-        };
+MBED_NOINLINE static int benchmark_dhm()
+{
+    int ret = 0;
+    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,
+    };
 
-        mbedtls_dhm_context dhm;
-        size_t olen;
-        for( i = 0; (size_t) i < sizeof( dhm_sizes ) / sizeof( dhm_sizes[0] ); i++ )
-        {
-            mbedtls_dhm_init( &dhm );
+    mbedtls_dhm_context dhm;
+    size_t olen;
+    size_t i;
+
+    for (i = 0;
+            i < sizeof(dhm_sizes) / sizeof(dhm_sizes[0]) && ret == 0;
+            i++) {
+        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 )
-            {
-                return( 1 );
-            }
-
-            dhm.len = mbedtls_mpi_size( &dhm.P );
-            mbedtls_dhm_make_public( &dhm, (int) dhm.len, buf, dhm.len, myrand, NULL );
-            if( mbedtls_mpi_copy( &dhm.GY, &dhm.GX ) != 0 )
-                return( 1 );
-
-            mbedtls_snprintf( title, sizeof( title ), "DHE-%d", dhm_sizes[i] );
-            TIME_PUBLIC( title, "handshake",
-                    ret |= mbedtls_dhm_make_public( &dhm, (int) dhm.len, buf, dhm.len,
-                                            myrand, NULL );
-                    ret |= mbedtls_dhm_calc_secret( &dhm, buf, sizeof( buf ), &olen, myrand, NULL ) );
+        ret = mbedtls_mpi_read_string(&dhm.P, 16, dhm_P[i]);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_mpi_read_string()");
+            goto exit;
+        }
+        ret = mbedtls_mpi_read_string(&dhm.G, 16, dhm_G[i]);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_mpi_read_string()");
+            goto exit;
+        }
 
-            mbedtls_snprintf( title, sizeof( title ), "DH-%d", dhm_sizes[i] );
-            TIME_PUBLIC( title, "handshake",
-                    ret |= mbedtls_dhm_calc_secret( &dhm, buf, sizeof( buf ), &olen, myrand, NULL ) );
-
-            mbedtls_dhm_free( &dhm );
+        dhm.len = mbedtls_mpi_size(&dhm.P);
+        ret = mbedtls_dhm_make_public(&dhm, (int) dhm.len, buf, dhm.len,
+                                      myrand, NULL);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_dhm_make_public()");
+            goto exit;
         }
-    }
-#endif
 
-#if defined(MBEDTLS_ECDSA_C) && defined(MBEDTLS_SHA256_C) && defined(ENABLE_ECDSA)
-    if( todo->ecdsa )
-    {
-        mbedtls_ecdsa_context ecdsa;
-        const mbedtls_ecp_curve_info *curve_info;
-        size_t sig_len;
+        ret = mbedtls_mpi_copy(&dhm.GY, &dhm.GX);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_mpi_copy()");
+            goto exit;
+        }
 
-        memset( buf, 0x2A, sizeof( buf ) );
+        ret = mbedtls_snprintf(title, sizeof(title), "DHE-%d", dhm_sizes[i]);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
 
-        for( curve_info = mbedtls_ecp_curve_list();
-             curve_info->grp_id != MBEDTLS_ECP_DP_NONE;
-             curve_info++ )
-        {
-            mbedtls_ecdsa_init( &ecdsa );
+        /*
+         * Benchmarking this requires two function calls that can fail. We
+         * add a check in between them to check for any errors. In normal
+         * operation, the overhead of this check is negligible
+         */
+        BENCHMARK_PUBLIC(title, "handshake",
+                         ret = mbedtls_dhm_make_public(&dhm, (int)dhm.len,
+                                 buf, dhm.len, myrand,
+                                 NULL);
+                         if (ret != 0) {
+                             PRINT_ERROR(ret, "mbedtls_dhm_make_public()");
+                             goto exit;
+                         }
+                         ret = mbedtls_dhm_calc_secret(&dhm, buf, sizeof(buf),
+                                 &olen, myrand, NULL));
 
-            if( mbedtls_ecdsa_genkey( &ecdsa, curve_info->grp_id, myrand, NULL ) != 0 )
-                return( 1 );
-            ecp_clear_precomputed( &ecdsa.grp );
-
-            mbedtls_snprintf( title, sizeof( title ), "ECDSA-%s",
-                                              curve_info->name );
-            TIME_PUBLIC( title, "sign",
-                    ret = mbedtls_ecdsa_write_signature( &ecdsa, MBEDTLS_MD_SHA256, buf, ( curve_info->bit_size + 7 ) / 8,
-                                                tmp, &sig_len, myrand, NULL ) );
-
-            mbedtls_ecdsa_free( &ecdsa );
+        ret = mbedtls_snprintf(title, sizeof(title), "DH-%d", dhm_sizes[i]);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
         }
 
-        for( curve_info = mbedtls_ecp_curve_list();
-             curve_info->grp_id != MBEDTLS_ECP_DP_NONE;
-             curve_info++ )
-        {
-            mbedtls_ecdsa_init( &ecdsa );
+        BENCHMARK_PUBLIC(title, "handshake",
+                         ret = mbedtls_dhm_calc_secret(&dhm, buf, sizeof(buf),
+                                 &olen, myrand, NULL));
+
+exit:
+        mbedtls_dhm_free(&dhm);
+    }
+
+    return ret;
+}
+#endif /* MBEDTLS_DHM_C && MBEDTLS_BIGNUM_C */
 
-            if( mbedtls_ecdsa_genkey( &ecdsa, curve_info->grp_id, myrand, NULL ) != 0 ||
-                mbedtls_ecdsa_write_signature( &ecdsa, MBEDTLS_MD_SHA256, buf, ( curve_info->bit_size + 7 ) / 8,
-                                               tmp, &sig_len, myrand, NULL ) != 0 )
-            {
-                return( 1 );
-            }
-            ecp_clear_precomputed( &ecdsa.grp );
+#if defined(MBEDTLS_ECDSA_C) && defined(MBEDTLS_SHA256_C)
+MBED_NOINLINE static int benchmark_ecdsa()
+{
+    int ret = 0;
+    mbedtls_ecdsa_context ecdsa;
+    const mbedtls_ecp_curve_info *curve_info;
+    size_t sig_len;
+    size_t hash_len;
+
+    memset(buf, 0x2A, sizeof(buf));
 
-            mbedtls_snprintf( title, sizeof( title ), "ECDSA-%s",
-                                              curve_info->name );
-            TIME_PUBLIC( title, "verify",
-                    ret = mbedtls_ecdsa_read_signature( &ecdsa, buf, ( curve_info->bit_size + 7 ) / 8,
-                                                tmp, sig_len ) );
+    for (curve_info = mbedtls_ecp_curve_list();
+            curve_info->grp_id != MBEDTLS_ECP_DP_NONE && ret == 0;
+            curve_info++) {
+        mbedtls_ecdsa_init(&ecdsa);
 
-            mbedtls_ecdsa_free( &ecdsa );
+        ret = mbedtls_ecdsa_genkey(&ecdsa, curve_info->grp_id, myrand, NULL);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecdsa_genkey()");
+            goto exit;
         }
-    }
-#endif
+
+        ecp_clear_precomputed(&ecdsa.grp);
 
-#if defined(MBEDTLS_ECDH_C)
-    if( todo->ecdh )
-    {
-        mbedtls_ecdh_context ecdh;
-#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
-        mbedtls_mpi z;
-#endif
-        const mbedtls_ecp_curve_info *curve_info;
-        size_t olen;
+        ret = mbedtls_snprintf(title, sizeof(title), "ECDSA-%s",
+                               curve_info->name);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
 
-        for( curve_info = mbedtls_ecp_curve_list();
-             curve_info->grp_id != MBEDTLS_ECP_DP_NONE;
-             curve_info++ )
-        {
-            mbedtls_ecdh_init( &ecdh );
+        hash_len = (curve_info->bit_size + 7) / 8;
+        BENCHMARK_PUBLIC(title, "sign",
+                         ret = mbedtls_ecdsa_write_signature(&ecdsa,
+                                 MBEDTLS_MD_SHA256,
+                                 buf, hash_len,
+                                 tmp, &sig_len,
+                                 myrand, NULL));
+
+        mbedtls_ecdsa_free(&ecdsa);
+    }
+
+    for (curve_info = mbedtls_ecp_curve_list();
+            curve_info->grp_id != MBEDTLS_ECP_DP_NONE && ret == 0;
+            curve_info++) {
+        mbedtls_ecdsa_init(&ecdsa);
 
-            if( mbedtls_ecp_group_load( &ecdh.grp, curve_info->grp_id ) != 0 ||
-                mbedtls_ecdh_make_public( &ecdh, &olen, buf, sizeof( buf ),
-                                  myrand, NULL ) != 0 ||
-                mbedtls_ecp_copy( &ecdh.Qp, &ecdh.Q ) != 0 )
-            {
-                return( 1 );
-            }
-            ecp_clear_precomputed( &ecdh.grp );
+        ret = mbedtls_ecdsa_genkey(&ecdsa, curve_info->grp_id, myrand, NULL);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecdsa_genkey()");
+            goto exit;
+        }
 
-            mbedtls_snprintf( title, sizeof( title ), "ECDHE-%s",
-                                              curve_info->name );
-            TIME_PUBLIC( title, "handshake",
-                    ret |= mbedtls_ecdh_make_public( &ecdh, &olen, buf, sizeof( buf ),
-                                             myrand, NULL );
-                    ret |= mbedtls_ecdh_calc_secret( &ecdh, &olen, buf, sizeof( buf ),
-                                             myrand, NULL ) );
-            mbedtls_ecdh_free( &ecdh );
+        hash_len = (curve_info->bit_size + 7) / 8;
+        ret = mbedtls_ecdsa_write_signature(&ecdsa, MBEDTLS_MD_SHA256, buf,
+                                            hash_len, tmp, &sig_len, myrand,
+                                            NULL);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecdsa_write_signature()");
+            goto exit;
+        }
+
+        ecp_clear_precomputed(&ecdsa.grp);
+
+        ret = mbedtls_snprintf(title, sizeof(title), "ECDSA-%s",
+                               curve_info->name);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
         }
 
-        /* Curve25519 needs to be handled separately */
-#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
-        mbedtls_ecdh_init( &ecdh );
-        mbedtls_mpi_init( &z );
+        BENCHMARK_PUBLIC(title, "verify",
+                         ret = mbedtls_ecdsa_read_signature(&ecdsa, buf,
+                                 hash_len, tmp,
+                                 sig_len));
+
+exit:
+        mbedtls_ecdsa_free(&ecdsa);
+    }
+
+    return ret;
+}
+#endif /* MBEDTLS_ECDSA_C && MBEDTLS_SHA2565_C */
+
+#if defined(MBEDTLS_ECDH_C)
+MBED_NOINLINE static int benchmark_ecdh()
+{
+    int ret = 0;
+    mbedtls_ecdh_context ecdh;
+    const mbedtls_ecp_curve_info *curve_info;
+    size_t olen;
 
-        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 )
-        {
-            return( 1 );
+    for (curve_info = mbedtls_ecp_curve_list();
+            curve_info->grp_id != MBEDTLS_ECP_DP_NONE && ret == 0;
+            curve_info++) {
+        mbedtls_ecdh_init(&ecdh);
+
+        ret = mbedtls_ecp_group_load(&ecdh.grp, curve_info->grp_id);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecp_group_load()");
+            goto exit;
+        }
+
+        ret = mbedtls_ecdh_make_public(&ecdh, &olen, buf, sizeof(buf),
+                                       myrand, NULL);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecdh_make_public()");
+            goto exit;
+        }
+
+        ret = mbedtls_ecp_copy(&ecdh.Qp, &ecdh.Q);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecp_copy()");
+            goto exit;
         }
 
-        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++ )
-        {
-            mbedtls_ecdh_init( &ecdh );
+        ecp_clear_precomputed(&ecdh.grp);
 
-            if( mbedtls_ecp_group_load( &ecdh.grp, curve_info->grp_id ) != 0 ||
-                mbedtls_ecdh_make_public( &ecdh, &olen, buf, sizeof( buf ),
-                                  myrand, NULL ) != 0 ||
-                mbedtls_ecp_copy( &ecdh.Qp, &ecdh.Q ) != 0 ||
-                mbedtls_ecdh_make_public( &ecdh, &olen, buf, sizeof( buf ),
-                                  myrand, NULL ) != 0 )
-            {
-                return( 1 );
-            }
-            ecp_clear_precomputed( &ecdh.grp );
-
-            mbedtls_snprintf( title, sizeof( title ), "ECDH-%s",
-                                              curve_info->name );
-            TIME_PUBLIC( title, "handshake",
-                    ret |= mbedtls_ecdh_calc_secret( &ecdh, &olen, buf, sizeof( buf ),
-                                             myrand, NULL ) );
-            mbedtls_ecdh_free( &ecdh );
+        ret = mbedtls_snprintf(title, sizeof(title), "ECDHE-%s",
+                               curve_info->name);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
         }
 
-        /* Curve25519 needs to be handled separately */
-#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
-        mbedtls_ecdh_init( &ecdh );
-        mbedtls_mpi_init( &z );
+        /*
+         * Benchmarking this requires two function calls that can fail. We
+         * add a check in between them to check for any errors. In normal
+         * operation, the overhead of this check is negligible
+         */
+        BENCHMARK_PUBLIC(title, "handshake",
+                         ret = mbedtls_ecdh_make_public(&ecdh, &olen, buf,
+                                 sizeof(buf), myrand,
+                                 NULL);
+                         if (ret != 0) {
+                             PRINT_ERROR(ret, "mbedtls_ecdh_make_public()");
+                             goto exit;
+                         }
+                         ret = mbedtls_ecdh_calc_secret(&ecdh, &olen, buf,
+                                 sizeof(buf), myrand,
+                                 NULL));
+        mbedtls_ecdh_free(&ecdh);
+    }
+
+    for (curve_info = mbedtls_ecp_curve_list();
+            curve_info->grp_id != MBEDTLS_ECP_DP_NONE && ret == 0;
+            curve_info++) {
+        mbedtls_ecdh_init(&ecdh);
 
-        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 )
-        {
-            return( 1 );
+        ret = mbedtls_ecp_group_load(&ecdh.grp, curve_info->grp_id);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecp_group_load()");
+            goto exit;
+        }
+
+        ret = mbedtls_ecdh_make_public(&ecdh, &olen, buf, sizeof(buf), myrand,
+                                       NULL);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecdh_make_public()");
+            goto exit;
+        }
+
+        ret = mbedtls_ecp_copy(&ecdh.Qp, &ecdh.Q);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecp_copy()");
+            goto exit;
+        }
+
+        ret = mbedtls_ecdh_make_public(&ecdh, &olen, buf, sizeof(buf), myrand,
+                                       NULL);
+        if (ret != 0) {
+            PRINT_ERROR(ret, "mbedtls_ecdh_make_public()");
+            goto exit;
         }
 
-        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
-    return ( 0 );
-
-}
+        ecp_clear_precomputed(&ecdh.grp);
 
-static int benchmark( int argc, char *argv[], mbedtls_platform_context* ctx )
-{
-    int i;
-    todo_list todo;
-#if defined(MBEDTLS_MEMORY_BUFFER_ALLOC_C)
-    unsigned char malloc_buf[HEAP_SIZE] = { 0 };
-#endif
+        ret = mbedtls_snprintf(title, sizeof(title), "ECDH-%s",
+                               curve_info->name);
+        if (ret < 0 || static_cast<size_t>(ret) >= sizeof(title)) {
+            mbedtls_printf("Failed to compose title string using "
+                           "mbedtls_snprintf(): %d\n", ret);
+            goto exit;
+        }
+        BENCHMARK_PUBLIC(title, "handshake",
+                         ret = mbedtls_ecdh_calc_secret(&ecdh, &olen, buf,
+                                 sizeof(buf), myrand,
+                                 NULL));
 
-    if( argc <= 1 )
-    {
-        memset( &todo, 1, sizeof( todo ) );
+exit:
+        mbedtls_ecdh_free(&ecdh);
     }
-    else
-    {
-        memset( &todo, 0, sizeof( todo ) );
+
+    return ret;
+}
+#endif /* MBEDTLS_ECDH_C */
 
-        for( i = 1; i < argc; i++ )
-        {
-            if( strcmp( argv[i], "md4" ) == 0 )
-                todo.md4 = 1;
-            else if( strcmp( argv[i], "md5" ) == 0 )
-                todo.md5 = 1;
-            else if( strcmp( argv[i], "ripemd160" ) == 0 )
-                todo.ripemd160 = 1;
-            else if( strcmp( argv[i], "sha1" ) == 0 )
-                todo.sha1 = 1;
-            else if( strcmp( argv[i], "sha256" ) == 0 )
-                todo.sha256 = 1;
-            else if( strcmp( argv[i], "sha512" ) == 0 )
-                todo.sha512 = 1;
-            else if( strcmp( argv[i], "arc4" ) == 0 )
-                todo.arc4 = 1;
-            else if( strcmp( argv[i], "des3" ) == 0 )
-                todo.des3 = 1;
-            else if( strcmp( argv[i], "des" ) == 0 )
-                todo.des = 1;
-            else if( strcmp( argv[i], "aes_cbc" ) == 0 )
-                todo.aes_cbc = 1;
-            else if( strcmp( argv[i], "aes_ctr" ) == 0 )
-                todo.aes_ctr = 1;
-            else if( strcmp( argv[i], "aes_gcm" ) == 0 )
-                todo.aes_gcm = 1;
-            else if( strcmp( argv[i], "aes_ccm" ) == 0 )
-                todo.aes_ccm = 1;
-            else if( strcmp( argv[i], "aes_cmac" ) == 0 )
-                todo.aes_cmac = 1;
-            else if( strcmp( argv[i], "des3_cmac" ) == 0 )
-                todo.des3_cmac = 1;
-            else if( strcmp( argv[i], "camellia" ) == 0 )
-                todo.camellia = 1;
-            else if( strcmp( argv[i], "blowfish" ) == 0 )
-                todo.blowfish = 1;
-            else if( strcmp( argv[i], "havege" ) == 0 )
-                todo.havege = 1;
-            else if( strcmp( argv[i], "ctr_drbg" ) == 0 )
-                todo.ctr_drbg = 1;
-            else if( strcmp( argv[i], "hmac_drbg" ) == 0 )
-                todo.hmac_drbg = 1;
-            else if( strcmp( argv[i], "rsa" ) == 0 )
-                todo.rsa = 1;
-            else if( strcmp( argv[i], "dhm" ) == 0 )
-                todo.dhm = 1;
-            else if( strcmp( argv[i], "ecdsa" ) == 0 )
-                todo.ecdsa = 1;
-            else if( strcmp( argv[i], "ecdh" ) == 0 )
-                todo.ecdh = 1;
-            else
-            {
-                mbedtls_printf( "Unrecognized option: %s\n", argv[i] );
-                mbedtls_printf( "Available options: " OPTIONS );
-            }
-        }
+#if defined(MBEDTLS_ECDH_C) && defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
+/* Curve25519 needs to be handled separately */
+MBED_NOINLINE static int benchmark_ecdh_curve22519()
+{
+    int ret = 0;
+    mbedtls_ecdh_context ecdh;
+    mbedtls_mpi z;
+
+    mbedtls_ecdh_init(&ecdh);
+    mbedtls_mpi_init(&z);
+
+    ret = mbedtls_ecp_group_load(&ecdh.grp, MBEDTLS_ECP_DP_CURVE25519);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_ecp_group_load()");
+        goto exit;
+    }
+
+    ret = mbedtls_ecdh_gen_public(&ecdh.grp, &ecdh.d, &ecdh.Qp, myrand,
+                                  NULL);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_ecdh_gen_public()");
+        goto exit;
     }
 
-    mbedtls_printf( "\n\n" );
-
-#if defined(MBEDTLS_MEMORY_BUFFER_ALLOC_C)
-    mbedtls_memory_buffer_alloc_init( malloc_buf, sizeof( malloc_buf ) );
-#endif
-    memset( buf, 0xAA, sizeof( buf ) );
+    /*
+     * Benchmarking this requires two function calls that can fail. We
+     * add a check in between them to check for any errors. In normal
+     * operation, the overhead of this check is negligible
+     */
+    BENCHMARK_PUBLIC("ECDHE-Curve25519", "handshake",
+                     ret = mbedtls_ecdh_gen_public(&ecdh.grp, &ecdh.d,
+                             &ecdh.Q, myrand, NULL);
+                     if (ret != 0) {
+                         PRINT_ERROR(ret, "mbedtls_ecdh_make_public()");
+                         goto exit;
+                     }
+                     ret = mbedtls_ecdh_compute_shared(&ecdh.grp, &z,
+                             &ecdh.Qp, &ecdh.d,
+                             myrand, NULL));
 
-    if( test_md( &todo, ctx ) != 0)
-        return ( 1 );
-    if( test_crypt( &todo, ctx ) != 0)
-        return ( 1 );
-    if( test_rng( &todo, ctx ) != 0)
-        return ( 1 );
-    if( test_pk( &todo, ctx ) != 0)
-        return ( 1 );
+    mbedtls_ecdh_free(&ecdh);
+    mbedtls_mpi_free(&z);
+
+    mbedtls_ecdh_init(&ecdh);
+    mbedtls_mpi_init(&z);
+
+    ret = mbedtls_ecp_group_load(&ecdh.grp, MBEDTLS_ECP_DP_CURVE25519);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_ecp_group_load()");
+        goto exit;
+    }
 
-    mbedtls_printf("\nDONE\n");
+    ret = mbedtls_ecdh_gen_public(&ecdh.grp, &ecdh.d, &ecdh.Qp, myrand, NULL);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_ecdh_gen_public()");
+        goto exit;
+    }
 
-#if defined(MBEDTLS_MEMORY_BUFFER_ALLOC_C)
-    mbedtls_memory_buffer_alloc_free();
-#endif
+    ret = mbedtls_ecdh_gen_public(&ecdh.grp, &ecdh.d, &ecdh.Q, myrand, NULL);
+    if (ret != 0) {
+        PRINT_ERROR(ret, "mbedtls_ecdh_gen_public()");
+        goto exit;
+    }
 
-    return( 0 );
+    BENCHMARK_PUBLIC("ECDH-Curve25519", "handshake",
+                     ret = mbedtls_ecdh_compute_shared(&ecdh.grp, &z,
+                             &ecdh.Qp, &ecdh.d,
+                             myrand, NULL));
+
+exit:
+    mbedtls_ecdh_free(&ecdh);
+    mbedtls_mpi_free(&z);
+
+    return ret;
 }
+#endif /* MBEDTLS_ECDH_C && MBEDTLS_ECP_DP_CURVE25519_ENABLED */
 
-int main(void) {
+int main()
+{
     mbedtls_platform_context platform_ctx;
-    int exit_code = MBEDTLS_EXIT_FAILURE;
+    int exit_code = MBEDTLS_EXIT_SUCCESS;
 
-    if((exit_code = mbedtls_platform_setup(&platform_ctx)) != 0) {
-        printf("Platform initialization failed with error %d\n", exit_code);
+    memset(buf, 0xAA, sizeof(buf));
+    memset(tmp, 0xBB, sizeof(tmp));
+
+    if ((exit_code = mbedtls_platform_setup(&platform_ctx)) != 0) {
+        mbedtls_printf("Platform initialization failed with error %d\r\n",
+                       exit_code);
         return MBEDTLS_EXIT_FAILURE;
     }
 
-    exit_code = benchmark(0, NULL, &platform_ctx);
-    if (exit_code != 0) {
-        mbedtls_printf("Benchmark failed with error %d\n", exit_code);
+#if defined(MBEDTLS_MD4_C)
+    if (benchmark_md4() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_MD4_C */
+
+#if defined(MBEDTLS_MD5_C)
+    if (benchmark_md5() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_MD5_C */
+
+#if defined(MBEDTLS_RIPEMD160_C)
+    if (benchmark_ripemd160() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_RIPEMD160_C */
+
+#if defined(MBEDTLS_SHA1_C)
+    if (benchmark_sha1() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_SHA1_C */
+
+#if defined(MBEDTLS_SHA256_C)
+    if (benchmark_sha256() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_SHA256_C */
+
+#if defined(MBEDTLS_SHA256_C)
+    if (benchmark_sha512() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_SHA512_C */
+
+#if defined(MBEDTLS_ARC4_C)
+    if (benchmark_arc4() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_ARC4_C */
+
+#if defined(MBEDTLS_DES_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+    if (benchmark_des3() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_DES_C && MBEDTLS_CIPHER_MODE_CBC */
+
+#if defined(MBEDTLS_DES_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+    if (benchmark_des() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_DES_C && MBEDTLS_CIPHER_MODE_CBC */
+
+#if defined(MBEDTLS_DES_C) && defined(MBEDTLS_CIPHER_MODE_CBC) && \
+    defined(MBEDTLS_CMAC_C)
+    if (benchmark_des3_cmac() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_DES_C && MBEDTLS_CIPHER_MODE_CBC && MBEDTLS_CMAC_C */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+    if (benchmark_aes_cbc() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_AES_C && MBEDTLS_CIPHER_MODE_CBC */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CIPHER_MODE_CTR)
+    if (benchmark_aes_ctr() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_AES_C && MBEDTLS_CIPHER_MODE_CTR */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_GCM_C)
+    if (benchmark_aes_gcm() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_AES_C && MBEDTLS_GCM_C */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CCM_C)
+    if (benchmark_aes_ccm() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_AES_C && MBEDTLS_CCM_C */
+
+#if defined(MBEDTLS_AES_C) && defined(MBEDTLS_CMAC_C)
+    if (benchmark_aes_cmac() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_AES_C && MBEDTLS_CMAC_C */
+
+#if defined(MBEDTLS_CAMELLIA_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+    if (benchmark_camellia() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_CAMELLIA_C && MBEDTLS_CIPHER_MODE_CBC */
+
+#if defined(MBEDTLS_BLOWFISH_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+    if (benchmark_blowfish() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_BLOWFISH_C && MBEDTLS_CIPHER_MODE_CBC */
+
+#if defined(MBEDTLS_HAVEGE_C)
+    if (benchmark_havege() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_HAVEGE_C */
+
+#if defined(MBEDTLS_CTR_DRBG_C)
+    if (benchmark_ctr_drbg() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_CTR_DRBG_C */
+
+#if defined(MBEDTLS_HMAC_DRBG_C)
+    if (benchmark_hmac_drbg() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_HMAC_DRBG_C */
+
+#if defined(MBEDTLS_RSA_C) && \
+    defined(MBEDTLS_PEM_PARSE_C) && defined(MBEDTLS_PK_PARSE_C)
+    if (benchmark_rsa() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_RSA_C && MBEDTLS_PEM_PARSE_C && MBEDTLS_PK_PARSE_C */
+
+#if defined(MBEDTLS_DHM_C) && defined(MBEDTLS_BIGNUM_C)
+    if (benchmark_dhm() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_DHM_C && MBEDTLS_BIGNUM_C */
+
+#if defined(MBEDTLS_ECDSA_C) && defined(MBEDTLS_SHA256_C)
+    if (benchmark_ecdsa() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_ECDSA_C && MBEDTLS_SHA2565_C */
+
+#if defined(MBEDTLS_ECDH_C)
+    if (benchmark_ecdh() != 0) {
         exit_code = MBEDTLS_EXIT_FAILURE;
     }
 
+#if defined(MBEDTLS_ECP_DP_CURVE25519_ENABLED)
+    if (benchmark_ecdh_curve22519() != 0) {
+        exit_code = MBEDTLS_EXIT_FAILURE;
+    }
+#endif /* MBEDTLS_ECP_DP_CURVE25519_ENABLED */
+#endif /* MBEDTLS_ECDH_C */
+
+    mbedtls_printf("DONE\n");
+
     mbedtls_platform_teardown(&platform_ctx);
+
     return exit_code;
 }