diff --git a/programs/test/benchmark.c b/programs/test/benchmark.c
index 0366f9c..98d231a 100644
--- a/programs/test/benchmark.c
+++ b/programs/test/benchmark.c
@@ -59,9 +59,12 @@
 #include "mbedtls/aes.h"
 #include "mbedtls/blowfish.h"
 #include "mbedtls/camellia.h"
+#include "mbedtls/chacha20.h"
 #include "mbedtls/gcm.h"
 #include "mbedtls/ccm.h"
+#include "mbedtls/chachapoly.h"
 #include "mbedtls/cmac.h"
+#include "mbedtls/poly1305.h"
 #include "mbedtls/havege.h"
 #include "mbedtls/ctr_drbg.h"
 #include "mbedtls/hmac_drbg.h"
@@ -93,8 +96,9 @@
 
 #define OPTIONS                                                         \
     "md4, md5, ripemd160, sha1, sha256, sha512,\n"                      \
-    "arc4, des3, des, camellia, blowfish,\n"                            \
+    "arc4, des3, des, camellia, blowfish, chacha20,\n"                  \
     "aes_cbc, aes_gcm, aes_ccm, aes_cmac, aes_xts, des3_cmac,\n"        \
+    "chachapoly, poly1305\n"                                            \
     "havege, ctr_drbg, hmac_drbg\n"                                     \
     "rsa, dhm, ecdsa, ecdh.\n"
 
@@ -228,8 +232,10 @@
 typedef struct {
     char md4, md5, ripemd160, sha1, sha256, sha512,
          arc4, des3, des,
-         aes_cbc, aes_gcm, aes_ccm, aes_cmac, aes_xts, des3_cmac,
          camellia, blowfish,
+         chacha20,
+         aes_cbc, aes_gcm, aes_ccm, aes_cmac, aes_xts, des3_cmac,
+         chachapoly, poly1305,
          havege, ctr_drbg, hmac_drbg,
          rsa, dhm, ecdsa, ecdh;
 } todo_list;
@@ -278,6 +284,8 @@
                 todo.aes_gcm = 1;
             else if( strcmp( argv[i], "aes_ccm" ) == 0 )
                 todo.aes_ccm = 1;
+            else if( strcmp( argv[i], "chachapoly" ) == 0 )
+                todo.chachapoly = 1;
             else if( strcmp( argv[i], "aes_cmac" ) == 0 )
                 todo.aes_cmac = 1;
             else if( strcmp( argv[i], "aes_xts" ) == 0 )
@@ -288,6 +296,10 @@
                 todo.camellia = 1;
             else if( strcmp( argv[i], "blowfish" ) == 0 )
                 todo.blowfish = 1;
+            else if( strcmp( argv[i], "chacha20" ) == 0 )
+                todo.chacha20 = 1;
+            else if( strcmp( argv[i], "poly1305" ) == 0 )
+                todo.poly1305 = 1;
             else if( strcmp( argv[i], "havege" ) == 0 )
                 todo.havege = 1;
             else if( strcmp( argv[i], "ctr_drbg" ) == 0 )
@@ -467,6 +479,27 @@
         }
     }
 #endif
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    if( todo.chachapoly )
+    {
+        mbedtls_chachapoly_context chachapoly;
+
+        mbedtls_chachapoly_init( &chachapoly );
+        memset( buf, 0, sizeof( buf ) );
+        memset( tmp, 0, sizeof( tmp ) );
+
+        mbedtls_snprintf( title, sizeof( title ), "ChaCha20-Poly1305" );
+
+        mbedtls_chachapoly_setkey( &chachapoly, tmp );
+
+        TIME_AND_TSC( title,
+                mbedtls_chachapoly_crypt_and_tag( &chachapoly,
+                    MBEDTLS_CHACHAPOLY_ENCRYPT, BUFSIZE, tmp,
+                    NULL, 0, buf, buf, tmp ) );
+
+        mbedtls_chachapoly_free( &chachapoly );
+    }
+#endif
 #if defined(MBEDTLS_CMAC_C)
     if( todo.aes_cmac )
     {
@@ -545,6 +578,20 @@
     }
 #endif
 
+#if defined(MBEDTLS_CHACHA20_C)
+    if ( todo.chacha20 )
+    {
+        TIME_AND_TSC( "ChaCha20", mbedtls_chacha20_crypt( buf, buf, 0U, BUFSIZE, buf, buf ) );
+    }
+#endif
+
+#if defined(MBEDTLS_POLY1305_C)
+    if ( todo.poly1305 )
+    {
+        TIME_AND_TSC( "Poly1305", mbedtls_poly1305_mac( buf, buf, BUFSIZE, buf ) );
+    }
+#endif
+
 #if defined(MBEDTLS_BLOWFISH_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
     if( todo.blowfish )
     {
diff --git a/programs/test/selftest.c b/programs/test/selftest.c
index 72a3734..13fa98c 100644
--- a/programs/test/selftest.c
+++ b/programs/test/selftest.c
@@ -44,6 +44,9 @@
 #include "mbedtls/des.h"
 #include "mbedtls/aes.h"
 #include "mbedtls/camellia.h"
+#include "mbedtls/chacha20.h"
+#include "mbedtls/poly1305.h"
+#include "mbedtls/chachapoly.h"
 #include "mbedtls/base64.h"
 #include "mbedtls/bignum.h"
 #include "mbedtls/rsa.h"
@@ -207,6 +210,15 @@
 #if defined(MBEDTLS_CMAC_C)
     {"cmac", mbedtls_cmac_self_test},
 #endif
+#if defined(MBEDTLS_CHACHA20_C)
+    {"chacha20", mbedtls_chacha20_self_test},
+#endif
+#if defined(MBEDTLS_POLY1305_C)
+    {"poly1305", mbedtls_poly1305_self_test},
+#endif
+#if defined(MBEDTLS_CHACHAPOLY_C)
+    {"chacha20-poly1305", mbedtls_chachapoly_self_test},
+#endif
 #if defined(MBEDTLS_BASE64_C)
     {"base64", mbedtls_base64_self_test},
 #endif