diff --git a/config/mbedtls_config.h b/config/mbedtls_config.h
index 709b058..ada3aca 100644
--- a/config/mbedtls_config.h
+++ b/config/mbedtls_config.h
@@ -2112,7 +2112,7 @@
  *
  * Module:  library/chacha20.c
  */
-//#define MBEDTLS_CHACHA20_C
+#define MBEDTLS_CHACHA20_C
 
 /**
  * \def MBEDTLS_CHACHAPOLY_C
@@ -2123,7 +2123,7 @@
  *
  * This module requires: MBEDTLS_CHACHA20_C, MBEDTLS_POLY1305_C
  */
-//#define MBEDTLS_CHACHAPOLY_C
+#define MBEDTLS_CHACHAPOLY_C
 
 /**
  * \def MBEDTLS_CIPHER_C
diff --git a/pico_hsm_sdk_import.cmake b/pico_hsm_sdk_import.cmake
index 7d7ec4c..1e54ee9 100644
--- a/pico_hsm_sdk_import.cmake
+++ b/pico_hsm_sdk_import.cmake
@@ -76,11 +76,6 @@ if (NOT TARGET pico_hsm_sdk)
             ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/sha1.c
             ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/sha256.c
             ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/sha512.c
-            ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/x509write_crt.c
-            ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/x509_create.c
-            ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/pk.c
-            ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/pk_wrap.c
-            ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/pkwrite.c
             )
 
     if (${HSM_DRIVER} STREQUAL "ccid")
@@ -99,6 +94,13 @@ if (NOT TARGET pico_hsm_sdk)
                 ${CMAKE_CURRENT_LIST_DIR}/tinycbor/src/cborencoder.c
                 ${CMAKE_CURRENT_LIST_DIR}/tinycbor/src/cborparser.c
                 ${CMAKE_CURRENT_LIST_DIR}/tinycbor/src/cborparser_dup_string.c
+                ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/x509write_crt.c
+                ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/x509_create.c
+                ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/pk.c
+                ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/pk_wrap.c
+                ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/pkwrite.c
+                ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/pkwrite.c
+                ${CMAKE_CURRENT_LIST_DIR}/mbedtls/library/chachapoly.c
                 )
         target_include_directories(pico_hsm_sdk INTERFACE
                 ${CMAKE_CURRENT_LIST_DIR}/src/usb/hid