diff --git a/library/cipher.c b/library/cipher.c
index a5cd61c..68d0c10 100644
--- a/library/cipher.c
+++ b/library/cipher.c
@@ -46,6 +46,10 @@
 #include "mbedtls/ccm.h"
 #endif
 
+#if defined(MBEDTLS_CHACHA20_C)
+#include "mbedtls/chacha20.h"
+#endif
+
 #if defined(MBEDTLS_CMAC_C)
 #include "mbedtls/cmac.h"
 #endif
@@ -231,6 +235,18 @@
             return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
     }
 
+#if defined(MBEDTLS_CHACHA20_C)
+    if ( ctx->cipher_info->type == MBEDTLS_CIPHER_CHACHA20 )
+    {
+        if ( 0 != mbedtls_chacha20_starts( (mbedtls_chacha20_context*)ctx->cipher_ctx,
+                                           iv,
+                                           0U ) ) /* Initial counter value */
+        {
+            return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
+        }
+    }
+#endif
+
     memcpy( ctx->iv, iv, actual_iv_size );
     ctx->iv_size = actual_iv_size;
 
@@ -314,6 +330,16 @@
         return( MBEDTLS_ERR_CIPHER_BAD_INPUT_DATA );
     }
 
+
+#if defined(MBEDTLS_CHACHA20_C)
+    if ( ctx->cipher_info->type == MBEDTLS_CIPHER_CHACHA20 )
+    {
+        *olen = ilen;
+        return mbedtls_chacha20_update( (mbedtls_chacha20_context*) ctx->cipher_ctx,
+                                         ilen, input, output );
+    }
+#endif
+
 #if defined(MBEDTLS_CIPHER_MODE_CBC)
     if( ctx->cipher_info->mode == MBEDTLS_MODE_CBC )
     {
@@ -646,6 +672,11 @@
         return( 0 );
     }
 
+    if ( MBEDTLS_CIPHER_CHACHA20 == ctx->cipher_info->type )
+    {
+        return( 0 );
+    }
+
     if( MBEDTLS_MODE_ECB == ctx->cipher_info->mode )
     {
         if( ctx->unprocessed_len != 0 )