diff --git a/programs/test/benchmark.c b/programs/test/benchmark.c
index 8ab32f6..86b8425 100644
--- a/programs/test/benchmark.c
+++ b/programs/test/benchmark.c
@@ -1,7 +1,7 @@
 /*
  *  Benchmark demonstration program
  *
- *  Copyright (C) 2006-2015, ARM Limited, All Rights Reserved
+ *  Copyright (C) 2006-2016, ARM Limited, All Rights Reserved
  *  SPDX-License-Identifier: Apache-2.0
  *
  *  Licensed under the Apache License, Version 2.0 (the "License"); you may
@@ -61,6 +61,7 @@
 #include "mbedtls/camellia.h"
 #include "mbedtls/gcm.h"
 #include "mbedtls/ccm.h"
+#include "mbedtls/cmac.h"
 #include "mbedtls/havege.h"
 #include "mbedtls/ctr_drbg.h"
 #include "mbedtls/hmac_drbg.h"
@@ -92,7 +93,8 @@
 
 #define OPTIONS                                                         \
     "md4, md5, ripemd160, sha1, sha256, sha512,\n"                      \
-    "arc4, des3, des, aes_cbc, aes_gcm, aes_ccm, camellia, blowfish,\n" \
+    "arc4, des3, des, camellia, blowfish,\n"                             \
+    "aes_cbc, aes_gcm, aes_ccm, aes_cmac, des3_cmac,\n"                 \
     "havege, ctr_drbg, hmac_drbg\n"                                     \
     "rsa, dhm, ecdsa, ecdh.\n"
 
@@ -234,7 +236,9 @@
 
 typedef struct {
     char md4, md5, ripemd160, sha1, sha256, sha512,
-         arc4, des3, des, aes_cbc, aes_gcm, aes_ccm, camellia, blowfish,
+         arc4, des3, des,
+         aes_cbc, aes_gcm, aes_ccm, aes_cmac, des3_cmac,
+         camellia, blowfish,
          havege, ctr_drbg, hmac_drbg,
          rsa, dhm, ecdsa, ecdh;
 } todo_list;
@@ -283,6 +287,10 @@
                 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 )
@@ -358,7 +366,8 @@
     }
 #endif
 
-#if defined(MBEDTLS_DES_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
+#if defined(MBEDTLS_DES_C)
+#if defined(MBEDTLS_CIPHER_MODE_CBC)
     if( todo.des3 )
     {
         mbedtls_des3_context des3;
@@ -378,7 +387,25 @@
                 mbedtls_des_crypt_cbc( &des, MBEDTLS_DES_ENCRYPT, BUFSIZE, tmp, buf, buf ) );
         mbedtls_des_free( &des );
     }
-#endif
+
+#endif /* MBEDTLS_CIPHER_MODE_CBC */
+#if defined(MBEDTLS_CMAC_C)
+    if( todo.des3_cmac )
+    {
+        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 );
+
+        TIME_AND_TSC( "3DES-CMAC",
+                        mbedtls_cipher_cmac( cipher_info, tmp, 192, buf,
+                        BUFSIZE, output ) );
+    }
+#endif /* MBEDTLS_CMAC_C */
+#endif /* MBEDTLS_DES_C */
 
 #if defined(MBEDTLS_AES_C)
 #if defined(MBEDTLS_CIPHER_MODE_CBC)
@@ -447,7 +474,37 @@
         }
     }
 #endif
-#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;
+
+        for( keysize = 128, cipher_type = MBEDTLS_CIPHER_AES_128_ECB;
+             keysize <= 256;
+             keysize += 64, cipher_type++ )
+        {
+            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 ) );
+        }
+
+        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 ) );
+    }
+#endif /* MBEDTLS_CMAC_C */
+#endif /* MBEDTLS_AES_C */
 
 #if defined(MBEDTLS_CAMELLIA_C) && defined(MBEDTLS_CIPHER_MODE_CBC)
     if( todo.camellia )