diff --git a/configs/config-thread.h b/configs/config-thread.h
index fdfa64aa4..990fe08c6 100644
--- a/configs/config-thread.h
+++ b/configs/config-thread.h
@@ -39,7 +39,6 @@
 
 /* mbed TLS feature support */
 #define MBEDTLS_AES_ROM_TABLES
-#define MBEDTLS_CMAC_C
 #define MBEDTLS_ECP_DP_SECP256R1_ENABLED
 #define MBEDTLS_ECP_NIST_OPTIM
 #define MBEDTLS_KEY_EXCHANGE_ECJPAKE_ENABLED
@@ -58,6 +57,7 @@
 #define MBEDTLS_CCM_C
 #define MBEDTLS_CIPHER_C
 #define MBEDTLS_CTR_DRBG_C
+#define MBEDTLS_CMAC_C
 #define MBEDTLS_ECJPAKE_C
 #define MBEDTLS_ECP_C
 #define MBEDTLS_ENTROPY_C
diff --git a/library/cmac.c b/library/cmac.c
index 920c23d6a..f86e4c5aa 100644
--- a/library/cmac.c
+++ b/library/cmac.c
@@ -39,14 +39,19 @@
 
 #include <string.h>
 
-#if defined(MBEDTLS_SELF_TEST) && defined(MBEDTLS_AES_C)
+
 #if defined(MBEDTLS_PLATFORM_C)
 #include "mbedtls/platform.h"
 #else
+#include <stdlib.h>
+#define mbedtls_calloc     calloc
+#define mbedtls_free       free
+#if defined(MBEDTLS_SELF_TEST) && ( defined(MBEDTLS_AES_C) || defined(MBEDTLS_DES_C) )
 #include <stdio.h>
-#define mbedtls_printf printf
+#define mbedtls_printf     printf
+#endif /* defined(MBEDTLS_SELF_TEST) && ( defined(MBEDTLS_AES_C) || defined(MBEDTLS_DES_C) )*/
 #endif /* MBEDTLS_PLATFORM_C */
-#endif /* MBEDTLS_SELF_TEST && MBEDTLS_AES_C */
+
 
 /* Implementation that should never be optimized out by the compiler */
 static void mbedtls_zeroize( void *v, size_t n ) {
diff --git a/programs/test/selftest.c b/programs/test/selftest.c
index 74eed598e..33f96ea13 100644
--- a/programs/test/selftest.c
+++ b/programs/test/selftest.c
@@ -279,8 +279,11 @@ int main( int argc, char *argv[] )
 #endif
 
 #if defined(MBEDTLS_CMAC_C) && ( defined(MBEDTLS_AES_C) || defined(MBEDTLS_DES_C) )
-    if( ( ret = mbedtls_cmac_self_test( v ) ) != 0 )
-        return( ret );
+    if( ( mbedtls_cmac_self_test( v ) ) != 0 )
+    {
+        suites_failed++;
+    }
+    suites_tested++;
 #endif
 
 #if defined(MBEDTLS_BASE64_C)