diff --git a/configs/baremetal.h b/configs/baremetal.h
index e497c35..80ed74c 100644
--- a/configs/baremetal.h
+++ b/configs/baremetal.h
@@ -159,6 +159,7 @@
 
 /* Fault Injection Countermeasures */
 #define MBEDTLS_FI_COUNTERMEASURES
+#define MBEDTLS_CCM_SHUFFLING_MASKING
 
 #if defined(MBEDTLS_USER_CONFIG_FILE)
 #include MBEDTLS_USER_CONFIG_FILE
diff --git a/include/mbedtls/config.h b/include/mbedtls/config.h
index 1fc451e..43a9830 100644
--- a/include/mbedtls/config.h
+++ b/include/mbedtls/config.h
@@ -688,6 +688,15 @@
 //#define MBEDTLS_FI_COUNTERMEASURES
 
 /**
+ * \def MBEDTLS_CCM_SHUFFLING_MASKING
+ *
+ * Add shuffling and masking to the CCM module as an additional security
+ * measure.
+ *
+ */
+//#define MBEDTLS_CCM_SHUFFLING_MASKING
+
+/**
  * \def MBEDTLS_CAMELLIA_SMALL_MEMORY
  *
  * Use less ROM for the Camellia implementation (saves about 768 bytes).
diff --git a/library/ccm.c b/library/ccm.c
index aa15af2..5ba869a 100644
--- a/library/ccm.c
+++ b/library/ccm.c
@@ -114,6 +114,7 @@
     mbedtls_platform_zeroize( ctx, sizeof( mbedtls_ccm_context ) );
 }
 
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
 /* Durstenfeld's version of Fisher-Yates shuffle */
 static void mbedtls_generate_permutation( unsigned char* table, size_t  size )
 {
@@ -148,6 +149,7 @@
         table[i] = mbedtls_platform_random_in_range( 256 );
     }
 }
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
 
 /*
  * Macros for common operations.
@@ -159,6 +161,7 @@
  * (Always using b as the source helps the compiler optimise a bit better.)
  * Initial b masking happens outside of this macro due to various sources of it.
  */
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
 #define UPDATE_CBC_MAC                                                      \
     for( i = 0; i < 16; i++ )                                               \
     {                                                                       \
@@ -168,14 +171,25 @@
                                                                             \
     if( ( ret = mbedtls_cipher_update( &ctx->cipher_ctx, y, 16, y, &olen ) ) != 0 ) \
         return( ret );
+#else
+#define UPDATE_CBC_MAC                                                      \
+    for( i = 0; i < 16; i++ )                                               \
+        y[i] ^= b[i];                                                       \
+                                                                            \
+    if( ( ret = mbedtls_cipher_update( &ctx->cipher_ctx, y, 16, y, &olen ) ) != 0 ) \
+        return( ret );
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
 
 /*
  * Copy src to dst starting at a random offset, while masking the whole dst buffer.
  */
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
 #define COPY_MASK( dst, src, mask, len_src, len_dst )                   \
     do                                                                  \
     {                                                                   \
         unsigned j, offset = mbedtls_platform_random_in_range( 256 );   \
+        mbedtls_generate_masks( mask_table, 16 );                       \
+        mbedtls_generate_permutation( perm_table, 16 );                 \
         for( i = 0; i < len_src; i++ )                                  \
         {                                                               \
             j = (i + offset) % len_src;                                 \
@@ -184,11 +198,14 @@
         for( ; i < len_dst; i++ )                                       \
             (dst)[i] ^= (mask)[i];                                      \
     } while( 0 )
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
+
 /*
  * Encrypt or decrypt a partial block with CTR
  * Warning: using b for temporary storage! src and dst must not be b!
  * This avoids allocating one more 16 bytes buffer while allowing src == dst.
  */
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
 #define CTR_CRYPT( dst, src, len )                                      \
     do                                                                  \
     {                                                                   \
@@ -200,13 +217,27 @@
             return( ret );                                              \
         }                                                               \
                                                                         \
-        for( i = 0; i < len; i++ )                                      \
+        for( i = 0; i < (len); i++ )                                    \
         {                                                               \
             (dst)[perm_table[i]] = (src)[perm_table[i]] ^ mask_table[perm_table[i]];\
             (dst)[perm_table[i]] ^= b[perm_table[i]];                   \
             (dst)[perm_table[i]] ^= mask_table[perm_table[i]];          \
         }                                                               \
     } while( 0 )
+#else
+#define CTR_CRYPT( dst, src, len )                                      \
+    do                                                                  \
+    {                                                                   \
+        if( ( ret = mbedtls_cipher_update( &ctx->cipher_ctx, ctr,       \
+                                           16, b, &olen ) ) != 0 )      \
+        {                                                               \
+            return( ret );                                              \
+        }                                                               \
+                                                                        \
+        for( i = 0; i < (len); i++ )                                    \
+            (dst)[i] = (src)[i] ^ b[i];                                 \
+    } while( 0 )
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
 
 /*
  * Authenticated encryption or decryption
@@ -224,11 +255,13 @@
     unsigned char b[16];
     unsigned char y[16];
     unsigned char ctr[16];
-    unsigned char perm_table[16];
-    unsigned char mask_table[16];
     const unsigned char *src;
     unsigned char *dst;
-
+    volatile size_t flow_ctrl = 0;
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
+    unsigned char perm_table[16];
+    unsigned char mask_table[16];
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
     /*
      * Check length requirements: SP800-38C A.1
      * Additional requirement: a < 2^16 - 2^8 to simplify the code.
@@ -251,6 +284,7 @@
     mbedtls_platform_zeroize( ctr, 16 );
 
     q = (uint_fast8_t) (16 - 1 - iv_len);
+    flow_ctrl++; /* 1 */
 
     /*
      * First block B_0:
@@ -264,6 +298,7 @@
      * 5 .. 3   (t - 2) / 2
      * 2 .. 0   q - 1
      */
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
     mbedtls_generate_masks( mask_table, 16 );
     mbedtls_generate_permutation( perm_table, 16 );
     b[0] = (unsigned char) ( ( ( add_len > 0 ) << 6 ) |
@@ -271,17 +306,31 @@
                            ( q - 1 ) ) ^ mask_table[0];
 
     for( i = 0; i < iv_len; i++ )
+    {
         b[i+1] = iv[i] ^ mask_table[i+1];
+        flow_ctrl++; /* iv_len + 1 */
+    }
     for( i = 0, len_left = length; i < q; i++, len_left >>= 8 )
         b[15-i] = (unsigned char)( ( len_left & 0xFF ) ) ^ mask_table[15-i];
+#else
+    b[0] = 0;
+    b[0] |= ( add_len > 0 ) << 6;
+    b[0] |= ( ( tag_len - 2 ) / 2 ) << 3;
+    b[0] |= q - 1;
+
+    mbedtls_platform_memcpy( b + 1, iv, iv_len );
+    flow_ctrl += iv_len; /* iv_len + 1 */
+    for( i = 0, len_left = length; i < q; i++, len_left >>= 8 )
+        b[15-i] = (unsigned char)( len_left & 0xFF );
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
 
     if( len_left > 0 )
         return( MBEDTLS_ERR_CCM_BAD_INPUT );
 
-
     /* Start CBC-MAC with first block */
     memset( y, 0, 16 );
     UPDATE_CBC_MAC;
+    flow_ctrl++; /* iv_len + 2 */
 
     /*
      * If there is additional data, update CBC-MAC with
@@ -292,30 +341,37 @@
         size_t use_len;
         len_left = add_len;
         src = add;
-
-        mbedtls_generate_masks( mask_table, 16 );
-        mbedtls_generate_permutation( perm_table, 16 );
         mbedtls_platform_memset( b, 0, 16 );
-        b[0] = (unsigned char)( ( ( add_len >> 8 ) & 0xFF ) ^ mask_table[0] );
-        b[1] = (unsigned char)( ( ( add_len      ) & 0xFF ) ^ mask_table[1] );
 
         use_len = len_left < 16 - 2 ? len_left : 16 - 2;
 
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
         COPY_MASK( b+2, src, mask_table+2, use_len, 14 );
+        b[0] = (unsigned char)( ( ( add_len >> 8 ) & 0xFF ) ^ mask_table[0] );
+        b[1] = (unsigned char)( ( ( add_len      ) & 0xFF ) ^ mask_table[1] );
+#else
+        b[0] = (unsigned char)( ( add_len >> 8 ) & 0xFF );
+        b[1] = (unsigned char)( ( add_len      ) & 0xFF );
+
+        mbedtls_platform_memcpy( b + 2, src, use_len );
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
 
         len_left -= use_len;
         src += use_len;
 
         UPDATE_CBC_MAC;
+        flow_ctrl++; /* iv_len + 2 + ( add_len? 1 : 0 ) */
 
         while( len_left > 0 )
         {
-            mbedtls_generate_masks( mask_table, 16 );
-            mbedtls_generate_permutation( perm_table, 16 );
             use_len = len_left > 16 ? 16 : len_left;
-
             mbedtls_platform_memset( b, 0, 16 );
-            COPY_MASK( b, src, mask_table, use_len, 16);
+
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
+            COPY_MASK( b, src, mask_table, use_len, 16 );
+#else
+            mbedtls_platform_memcpy( b, src, use_len );
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
             UPDATE_CBC_MAC;
 
             len_left -= use_len;
@@ -337,6 +393,7 @@
     mbedtls_platform_memcpy( ctr + 1, iv, iv_len );
     mbedtls_platform_memset( ctr + 1 + iv_len, 0, q );
     ctr[15] = 1;
+    flow_ctrl++;  /* iv_len + 3 + ( add_len? 1 : 0 ) */
 
     /*
      * Authenticate and {en,de}crypt the message.
@@ -354,22 +411,29 @@
 
         if( mode == CCM_ENCRYPT )
         {
-            mbedtls_generate_masks( mask_table, 16 );
-            mbedtls_generate_permutation( perm_table, 16 );
             mbedtls_platform_memset( b, 0, 16 );
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
             COPY_MASK( b, src, mask_table, use_len, 16 );
+#else
+            mbedtls_platform_memcpy( b, src, use_len );
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
+
             UPDATE_CBC_MAC;
+            flow_ctrl++;  /* iv_len + 3 + ( add_len? 1 : 0 ) + encryptions */
         }
 
         CTR_CRYPT( dst, src, use_len );
 
         if( mode == CCM_DECRYPT )
         {
-            mbedtls_generate_masks( mask_table, 16 );
-            mbedtls_generate_permutation( perm_table, 16 );
             mbedtls_platform_memset( b, 0, 16 );
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
             COPY_MASK( b, dst, mask_table, use_len, 16 );
+#else
+           mbedtls_platform_memcpy( b, dst, use_len );
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
             UPDATE_CBC_MAC;
+            flow_ctrl++; /* iv_len + 3 + ( add_len? 1 : 0 ) + decryptions */
         }
 
         dst += use_len;
@@ -384,6 +448,7 @@
             if( ++ctr[15-i] != 0 )
                 break;
     }
+    flow_ctrl++;  /* iv_len + 4 + ( add_len? 1 : 0 ) + enc/dec */
 
     /*
      * Authentication: reset counter and crypt/mask internal tag
@@ -394,11 +459,25 @@
     CTR_CRYPT( y, y, 16 );
     mbedtls_platform_memcpy( tag, y, tag_len );
 
+    flow_ctrl++; /* iv_len + 5 + ( add_len? 1 : 0 ) + enc/dec */
+
     mbedtls_platform_zeroize( b, 16 );
     mbedtls_platform_zeroize( y, 16 );
     mbedtls_platform_zeroize( ctr, 16 );
 
-    return( ret );
+    {
+        size_t operations = length / 16;
+        operations += ( length % 16 ? 1 : 0 );
+        operations += ( add_len > 0 ? 1 : 0 );
+        /* See comments above on steps in calculating flow_ctrl */
+        if( flow_ctrl == iv_len + 5 + operations )
+        {
+            mbedtls_platform_random_delay();
+            if( flow_ctrl == iv_len + 5 + operations )
+                return( ret );
+        }
+    }
+    return( MBEDTLS_ERR_PLATFORM_FAULT_DETECTED );
 }
 
 /*
diff --git a/library/sha256.c b/library/sha256.c
index 5214591..c2a20d2 100644
--- a/library/sha256.c
+++ b/library/sha256.c
@@ -281,7 +281,9 @@
 
     if( flow_ctrl == 8 )
     {
-        return( 0 );
+        mbedtls_platform_random_delay();
+        if( flow_ctrl == 8 )
+            return( 0 );
     }
     /* Free the ctx upon suspected FI */
     mbedtls_sha256_free( ctx );
@@ -355,6 +357,7 @@
     /* Re-check ilen_dup to protect from a FI attack */
     if( ilen_dup < 64 )
     {
+        mbedtls_platform_random_delay();
         /* Re-check that the calculated offsets are correct */
         ilen_change = ilen - ilen_dup;
         if( ( input_dup + ilen_change ) == input )
@@ -458,7 +461,9 @@
     /* flow ctrl was incremented twice and then 7 times in two loops */
     if( flow_ctrl == 9 )
     {
-        return( 0 );
+        mbedtls_platform_random_delay();
+        if( flow_ctrl == 9 )
+            return( 0 );
     }
     /* Free the ctx and clear output upon suspected FI */
     mbedtls_sha256_free( ctx );
@@ -509,7 +514,9 @@
 
     if( input_dup == input && ilen_dup == ilen )
     {
-        return( ret );
+        mbedtls_platform_random_delay();
+        if( input_dup == input && ilen_dup == ilen )
+            return( ret );
     }
     mbedtls_platform_memset( output, 0, 32 );
     return( MBEDTLS_ERR_PLATFORM_FAULT_DETECTED );
diff --git a/library/version_features.c b/library/version_features.c
index beccd3f..ea072ac 100644
--- a/library/version_features.c
+++ b/library/version_features.c
@@ -279,6 +279,9 @@
 #if defined(MBEDTLS_FI_COUNTERMEASURES)
     "MBEDTLS_FI_COUNTERMEASURES",
 #endif /* MBEDTLS_FI_COUNTERMEASURES */
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
+    "MBEDTLS_CCM_SHUFFLING_MASKING",
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
 #if defined(MBEDTLS_CAMELLIA_SMALL_MEMORY)
     "MBEDTLS_CAMELLIA_SMALL_MEMORY",
 #endif /* MBEDTLS_CAMELLIA_SMALL_MEMORY */
diff --git a/programs/ssl/query_config.c b/programs/ssl/query_config.c
index 580eaf3..0711703 100644
--- a/programs/ssl/query_config.c
+++ b/programs/ssl/query_config.c
@@ -786,6 +786,14 @@
     }
 #endif /* MBEDTLS_FI_COUNTERMEASURES */
 
+#if defined(MBEDTLS_CCM_SHUFFLING_MASKING)
+    if( strcmp( "MBEDTLS_CCM_SHUFFLING_MASKING", config ) == 0 )
+    {
+        MACRO_EXPANSION_TO_STR( MBEDTLS_CCM_SHUFFLING_MASKING );
+        return( 0 );
+    }
+#endif /* MBEDTLS_CCM_SHUFFLING_MASKING */
+
 #if defined(MBEDTLS_CAMELLIA_SMALL_MEMORY)
     if( strcmp( "MBEDTLS_CAMELLIA_SMALL_MEMORY", config ) == 0 )
     {