diff --git a/configs/baremetal.h b/configs/baremetal.h
index 3d0fac5..5294351 100644
--- a/configs/baremetal.h
+++ b/configs/baremetal.h
@@ -41,6 +41,7 @@
 #define MBEDTLS_AES_FEWER_TABLES
 #define MBEDTLS_AES_ONLY_128_BIT_KEY_LENGTH
 #define MBEDTLS_AES_ONLY_ENCRYPT
+#define MBEDTLS_AES_SCA_COUNTERMEASURES
 #define MBEDTLS_CCM_C
 
 /* Asymmetric crypto: Single-curve ECC only. */
diff --git a/configs/baremetal_test.h b/configs/baremetal_test.h
index 7a03777..97c6425 100644
--- a/configs/baremetal_test.h
+++ b/configs/baremetal_test.h
@@ -48,7 +48,4 @@
 
 #undef MBEDTLS_NO_PLATFORM_ENTROPY
 
-#undef  MBEDTLS_ENTROPY_MAX_SOURCES
-#define MBEDTLS_ENTROPY_MAX_SOURCES 3
-
 #endif /* MBEDTLS_BAREMETAL_USER_CONFIG_H */
diff --git a/include/mbedtls/aes.h b/include/mbedtls/aes.h
index 0f430a8..6990be0 100644
--- a/include/mbedtls/aes.h
+++ b/include/mbedtls/aes.h
@@ -172,7 +172,8 @@
  *                 <li>256 bits</li></ul>
  *
  * \return         \c 0 on success.
- * \return         #MBEDTLS_ERR_AES_INVALID_KEY_LENGTH on failure.
+ * \return         #MBEDTLS_ERR_AES_INVALID_KEY_LENGTH or
+ *                 #MBEDTLS_ERR_PLATFORM_FAULT_DETECTED on failure.
  */
 int mbedtls_aes_setkey_enc( mbedtls_aes_context *ctx, const unsigned char *key,
                     unsigned int keybits );
@@ -190,7 +191,8 @@
  *                 <li>256 bits</li></ul>
  *
  * \return         \c 0 on success.
- * \return         #MBEDTLS_ERR_AES_INVALID_KEY_LENGTH on failure.
+ * \return         #MBEDTLS_ERR_AES_INVALID_KEY_LENGTH or
+ *                 #MBEDTLS_ERR_PLATFORM_FAULT_DETECTED on failure.
  */
 int mbedtls_aes_setkey_dec( mbedtls_aes_context *ctx, const unsigned char *key,
                     unsigned int keybits );
@@ -602,6 +604,7 @@
  * \param output    The output (ciphertext) block.
  *
  * \return          \c 0 on success.
+ * \return             #MBEDTLS_ERR_PLATFORM_FAULT_DETECTED in case of error.
  */
 int mbedtls_internal_aes_encrypt( mbedtls_aes_context *ctx,
                                   const unsigned char input[16],
@@ -617,6 +620,7 @@
  * \param output    The output (plaintext) block.
  *
  * \return          \c 0 on success.
+ * \return             #MBEDTLS_ERR_PLATFORM_FAULT_DETECTED in case of error.
  */
 int mbedtls_internal_aes_decrypt( mbedtls_aes_context *ctx,
                                   const unsigned char input[16],
diff --git a/include/mbedtls/config.h b/include/mbedtls/config.h
index dbb84b4..0f65133 100644
--- a/include/mbedtls/config.h
+++ b/include/mbedtls/config.h
@@ -635,6 +635,23 @@
 //#define MBEDTLS_AES_ONLY_ENCRYPT
 
 /**
+ * \def MBEDTLS_AES_SCA_COUNTERMEASURES
+ *
+ * Add countermeasures against possible side-channel-attack to AES calculation.
+ *
+ * Uncommenting this macro adds additional calculation rounds to AES
+ * calculation. Additional rounds are using random data and can occur in any
+ * AES calculation round.
+ *
+ * Tradeoff: Uncommenting this increases ROM footprint by ~100 bytes.
+ * The performance loss is ~50% with 128 bit AES.
+ *
+ * This option is dependent of \c MBEDTLS_ENTROPY_HARDWARE_ALT.
+ *
+ */
+//#define MBEDTLS_AES_SCA_COUNTERMEASURES
+
+/**
  * \def MBEDTLS_CAMELLIA_SMALL_MEMORY
  *
  * Use less ROM for the Camellia implementation (saves about 768 bytes).
diff --git a/include/mbedtls/platform_util.h b/include/mbedtls/platform_util.h
index e20f1c3..fa9f326 100644
--- a/include/mbedtls/platform_util.h
+++ b/include/mbedtls/platform_util.h
@@ -239,11 +239,18 @@
 uint32_t mbedtls_platform_random_in_range( size_t num );
 
 /**
- * \brief       This function does nothing, but can be inserted between
- *              successive reads to a volatile local variable to prevent
- *              compilers from optimizing them away.
+ * \brief       Random delay function.
+ *
+ *              Function implements a random delay by incrementing a local
+ *              variable randomized number of times (busy-looping).
+ *
+ *              Duration of the delay is random as number of variable increments
+ *              is randomized.
+ *
+ * \note        Currently the function is dependent of hardware providing an
+ *              rng with MBEDTLS_ENTROPY_HARDWARE_ALT.
  */
-void mbedtls_platform_enforce_volatile_reads( void );
+void mbedtls_platform_random_delay( void );
 
 #if defined(MBEDTLS_HAVE_TIME_DATE)
 /**
diff --git a/library/aes.c b/library/aes.c
index 243c4ec..c96f29e 100644
--- a/library/aes.c
+++ b/library/aes.c
@@ -85,6 +85,21 @@
 }
 #endif
 
+/*
+ * Data structure for AES round data
+ */
+typedef struct {
+    uint32_t *rk_ptr;       /* Round Key */
+    uint32_t xy_values[8];  /* X0, X1, X2, X3, Y0, Y1, Y2, Y3 */
+} aes_r_data_t;
+
+#if defined(MBEDTLS_AES_SCA_COUNTERMEASURES)
+/* Number of additional AES calculation rounds added for SCA CM */
+#define AES_SCA_CM_ROUNDS  3
+#else /* MBEDTLS_AES_SCA_COUNTERMEASURES */
+#define AES_SCA_CM_ROUNDS  0
+#endif /* MBEDTLS_AES_SCA_COUNTERMEASURES */
+
 #if defined(MBEDTLS_PADLOCK_C) &&                      \
     ( defined(MBEDTLS_HAVE_X86) || defined(MBEDTLS_PADLOCK_ALIGN16) )
 static int aes_padlock_ace = -1;
@@ -497,6 +512,101 @@
 
 #endif /* MBEDTLS_AES_ROM_TABLES */
 
+/**
+ * Randomize positions when to use AES SCA countermeasures.
+ * Each byte indicates one AES round as follows:
+ * first ( tbl_len - 4 ) bytes are reserved for middle AES rounds:
+ *  -4 high bit = table to use 0x10 for SCA CM data, 0 otherwise
+ *  -4 low bits = offset based on order, 4 for even position, 0 otherwise
+ * Last 4 bytes for first(2) and final(2) round calculation
+ *  -4 high bit = table to use, 0x10 for SCA CM data, otherwise real data
+ *  -4 low bits = not used
+ *
+ *  Return  Number of additional AES rounds
+ *
+ * Example of the control bytes:
+ *  Control data when only real data (R) is used:
+ *  | R  | R  | R  | R  | R  | R  | R  | R  | Start   | Final   |
+ *  |0x04|0x00|0x00|0x04|0x00|0x04|0x00|0x04|0x00|0x00|0x00|0x00|
+ *
+ *  Control data with 5 (F) dummy rounds and randomized start and final round:
+ *  | R  | F  | R  | F  | F  | R  | R  | R  | R  | R  | R  | START RF| FINAL FR|
+ *  |0x04|0x10|0x04|0x10|0x10|0x00|0x04|0x00|0x04|0x00|0x04|0x00|0x10|0x10|0x00|
+ */
+static int aes_sca_cm_data_randomize( uint8_t *tbl, uint8_t tbl_len )
+{
+    int i, is_even_pos;
+#if AES_SCA_CM_ROUNDS != 0
+    int is_unique_number;
+    int num;
+#endif
+
+    mbedtls_platform_memset( tbl, 0, tbl_len );
+
+#if AES_SCA_CM_ROUNDS != 0
+    // Randomize SCA CM positions to tbl
+    for( i = 0; i < AES_SCA_CM_ROUNDS; i++ )
+    {
+        is_unique_number = 0;
+        do
+        {
+            is_unique_number++;
+            num = mbedtls_platform_random_in_range( tbl_len - 4 );
+
+            if( is_unique_number > 10 )
+            {
+                // prevent forever loop if random returns constant
+                is_unique_number = 0;
+                tbl[i] = 0x10;    // fake data
+            }
+
+            if( tbl[num] == 0 )
+            {
+                is_unique_number = 0;
+                tbl[num] = 0x10;    // fake data
+            }
+        } while( is_unique_number != 0 );
+    }
+
+    // randomize control data for start and final round
+    for( i = 1; i <= 2; i++ )
+    {
+        num = mbedtls_platform_random_in_range( 0xff );
+        if( ( num % 2 ) == 0 )
+        {
+            tbl[tbl_len - ( i * 2 - 0 )] = 0x10;    // fake data
+            tbl[tbl_len - ( i * 2 - 1 )] = 0x00;    // real data
+        }
+        else
+        {
+            tbl[tbl_len - ( i * 2 - 0 )] = 0x00;    // real data
+            tbl[tbl_len - ( i * 2 - 1 )] = 0x10;    // fake data
+        }
+    }
+#endif /* AES_SCA_CM_ROUNDS != 0 */
+
+    // Fill real AES round data to the remaining places
+    is_even_pos = 1;
+    for( i = 0; i < tbl_len - 4; i++ )
+    {
+        if( tbl[i] == 0 )
+        {
+            if( is_even_pos == 1 )
+            {
+                tbl[i] = 0x04;  // real data, offset 4
+                is_even_pos = 0;
+            }
+            else
+            {
+                tbl[i] = 0x00;  // real data, offset 0
+                is_even_pos = 1;
+            }
+        }
+    }
+
+    return( AES_SCA_CM_ROUNDS );
+}
+
 #if defined(MBEDTLS_AES_FEWER_TABLES)
 
 #define ROTL8(x)  ( (uint32_t)( ( x ) <<  8 ) + (uint32_t)( ( x ) >> 24 ) )
@@ -568,8 +678,12 @@
 int mbedtls_aes_setkey_enc( mbedtls_aes_context *ctx, const unsigned char *key,
                     unsigned int keybits )
 {
-    unsigned int i;
+    unsigned int j = 0;
+    unsigned int flow_ctrl = 0;
+    volatile unsigned int i = 0;
+    volatile int ret = MBEDTLS_ERR_PLATFORM_FAULT_DETECTED;
     uint32_t *RK;
+    uint32_t offset = 0;
 
     AES_VALIDATE_RET( ctx != NULL );
     AES_VALIDATE_RET( key != NULL );
@@ -607,9 +721,19 @@
         return( mbedtls_aesni_setkey_enc( (unsigned char *) ctx->rk, key, keybits ) );
 #endif
 
-    for( i = 0; i < ( keybits >> 5 ); i++ )
+    mbedtls_platform_memset( RK, 0, ( keybits >> 5 ) * 4 );
+    offset = mbedtls_platform_random_in_range( keybits >> 5 );
+
+    for( j = offset; j < ( keybits >> 5 ); j++ )
     {
-        GET_UINT32_LE( RK[i], key, i << 2 );
+        GET_UINT32_LE( RK[j], key, j << 2 );
+        flow_ctrl++;
+    }
+
+    for( j = 0; j < offset; j++ )
+    {
+        GET_UINT32_LE( RK[j], key, j << 2 );
+        flow_ctrl++;
     }
 
     switch( ctx->nr )
@@ -676,7 +800,20 @@
 #endif /* !MBEDTLS_AES_ONLY_128_BIT_KEY_LENGTH */
     }
 
-    return( 0 );
+    ret = 0;
+
+    /* Validate execution path */
+    if( ( flow_ctrl == keybits >> 5 ) && ( ( ctx->nr == 10 && i == 10 )
+#if !defined(MBEDTLS_AES_ONLY_128_BIT_KEY_LENGTH)
+        || ( ctx->nr == 12 && i == 8 )
+        || ( ctx->nr == 14 && i == 7 )
+#endif
+    ) )
+    {
+        return ret;
+    }
+
+    return( MBEDTLS_ERR_PLATFORM_FAULT_DETECTED );
 }
 #endif /* !MBEDTLS_AES_SETKEY_ENC_ALT */
 
@@ -694,7 +831,8 @@
 
     return MBEDTLS_ERR_AES_INVALID_KEY_LENGTH;
 #else /* */
-    int i, j, ret;
+    volatile unsigned int i = 0, j = 0;
+    volatile int ret = MBEDTLS_ERR_PLATFORM_FAULT_DETECTED;
     mbedtls_aes_context cty;
     uint32_t *RK;
     uint32_t *SK;
@@ -725,6 +863,8 @@
     {
         mbedtls_aesni_inverse_key( (unsigned char *) ctx->rk,
                            (const unsigned char *) cty.rk, ctx->nr );
+        i = 0;
+        j = 4;
         goto exit;
     }
 #endif
@@ -755,7 +895,19 @@
 exit:
     mbedtls_aes_free( &cty );
 
-    return( ret );
+    if( ret != 0 )
+    {
+        return( ret );
+    }
+    else if( ( i == 0 ) && ( j == 4 ) )
+    {
+        return( ret );
+    }
+    else
+    {
+        return( MBEDTLS_ERR_PLATFORM_FAULT_DETECTED );
+    }
+
 #endif /* MBEDTLS_AES_ONLY_ENCRYPT */
 }
 
@@ -838,110 +990,174 @@
 
 #endif /* !MBEDTLS_AES_SETKEY_DEC_ALT */
 
-#define AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)                     \
-    do                                                          \
-    {                                                           \
-        (X0) = *RK++ ^ AES_FT0( ( (Y0)       ) & 0xFF ) ^       \
-                       AES_FT1( ( (Y1) >>  8 ) & 0xFF ) ^       \
-                       AES_FT2( ( (Y2) >> 16 ) & 0xFF ) ^       \
-                       AES_FT3( ( (Y3) >> 24 ) & 0xFF );        \
-                                                                \
-        (X1) = *RK++ ^ AES_FT0( ( (Y1)       ) & 0xFF ) ^       \
-                       AES_FT1( ( (Y2) >>  8 ) & 0xFF ) ^       \
-                       AES_FT2( ( (Y3) >> 16 ) & 0xFF ) ^       \
-                       AES_FT3( ( (Y0) >> 24 ) & 0xFF );        \
-                                                                \
-        (X2) = *RK++ ^ AES_FT0( ( (Y2)       ) & 0xFF ) ^       \
-                       AES_FT1( ( (Y3) >>  8 ) & 0xFF ) ^       \
-                       AES_FT2( ( (Y0) >> 16 ) & 0xFF ) ^       \
-                       AES_FT3( ( (Y1) >> 24 ) & 0xFF );        \
-                                                                \
-        (X3) = *RK++ ^ AES_FT0( ( (Y3)       ) & 0xFF ) ^       \
-                       AES_FT1( ( (Y0) >>  8 ) & 0xFF ) ^       \
-                       AES_FT2( ( (Y1) >> 16 ) & 0xFF ) ^       \
-                       AES_FT3( ( (Y2) >> 24 ) & 0xFF );        \
-    } while( 0 )
-
-#define AES_RROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)                 \
-    do                                                      \
-    {                                                       \
-        (X0) = *RK++ ^ AES_RT0( ( (Y0)       ) & 0xFF ) ^   \
-                       AES_RT1( ( (Y3) >>  8 ) & 0xFF ) ^   \
-                       AES_RT2( ( (Y2) >> 16 ) & 0xFF ) ^   \
-                       AES_RT3( ( (Y1) >> 24 ) & 0xFF );    \
-                                                            \
-        (X1) = *RK++ ^ AES_RT0( ( (Y1)       ) & 0xFF ) ^   \
-                       AES_RT1( ( (Y0) >>  8 ) & 0xFF ) ^   \
-                       AES_RT2( ( (Y3) >> 16 ) & 0xFF ) ^   \
-                       AES_RT3( ( (Y2) >> 24 ) & 0xFF );    \
-                                                            \
-        (X2) = *RK++ ^ AES_RT0( ( (Y2)       ) & 0xFF ) ^   \
-                       AES_RT1( ( (Y1) >>  8 ) & 0xFF ) ^   \
-                       AES_RT2( ( (Y0) >> 16 ) & 0xFF ) ^   \
-                       AES_RT3( ( (Y3) >> 24 ) & 0xFF );    \
-                                                            \
-        (X3) = *RK++ ^ AES_RT0( ( (Y3)       ) & 0xFF ) ^   \
-                       AES_RT1( ( (Y2) >>  8 ) & 0xFF ) ^   \
-                       AES_RT2( ( (Y1) >> 16 ) & 0xFF ) ^   \
-                       AES_RT3( ( (Y0) >> 24 ) & 0xFF );    \
-    } while( 0 )
-
 /*
  * AES-ECB block encryption
  */
 #if !defined(MBEDTLS_AES_ENCRYPT_ALT)
+
+static uint32_t *aes_fround( uint32_t *R,
+    uint32_t *X0, uint32_t *X1, uint32_t *X2, uint32_t *X3,
+    uint32_t Y0, uint32_t Y1, uint32_t Y2, uint32_t Y3 )
+{
+    *X0 = *R++ ^ AES_FT0( ( Y0       ) & 0xFF ) ^
+                 AES_FT1( ( Y1 >>  8 ) & 0xFF ) ^
+                 AES_FT2( ( Y2 >> 16 ) & 0xFF ) ^
+                 AES_FT3( ( Y3 >> 24 ) & 0xFF );
+
+    *X1 = *R++ ^ AES_FT0( ( Y1       ) & 0xFF ) ^
+                 AES_FT1( ( Y2 >>  8 ) & 0xFF ) ^
+                 AES_FT2( ( Y3 >> 16 ) & 0xFF ) ^
+                 AES_FT3( ( Y0 >> 24 ) & 0xFF );
+
+    *X2 = *R++ ^ AES_FT0( ( Y2       ) & 0xFF ) ^
+                 AES_FT1( ( Y3 >>  8 ) & 0xFF ) ^
+                 AES_FT2( ( Y0 >> 16 ) & 0xFF ) ^
+                 AES_FT3( ( Y1 >> 24 ) & 0xFF );
+
+    *X3 = *R++ ^ AES_FT0( ( Y3       ) & 0xFF ) ^
+                 AES_FT1( ( Y0 >>  8 ) & 0xFF ) ^
+                 AES_FT2( ( Y1 >> 16 ) & 0xFF ) ^
+                 AES_FT3( ( Y2 >> 24 ) & 0xFF );
+
+    return R;
+}
+
+static void aes_fround_final( uint32_t *R,
+    uint32_t *X0, uint32_t *X1, uint32_t *X2, uint32_t *X3,
+    uint32_t Y0, uint32_t Y1, uint32_t Y2, uint32_t Y3 )
+{
+    *X0 = *R++ ^ ( (uint32_t) FSb[ ( (Y0)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) FSb[ ( (Y1) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) FSb[ ( (Y2) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) FSb[ ( (Y3) >> 24 ) & 0xFF ] << 24 );
+
+    *X1 = *R++ ^ ( (uint32_t) FSb[ ( (Y1)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) FSb[ ( (Y2) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) FSb[ ( (Y3) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) FSb[ ( (Y0) >> 24 ) & 0xFF ] << 24 );
+
+    *X2 = *R++ ^ ( (uint32_t) FSb[ ( (Y2)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) FSb[ ( (Y3) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) FSb[ ( (Y0) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) FSb[ ( (Y1) >> 24 ) & 0xFF ] << 24 );
+
+    *X3 = *R++ ^ ( (uint32_t) FSb[ ( (Y3)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) FSb[ ( (Y0) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) FSb[ ( (Y1) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) FSb[ ( (Y2) >> 24 ) & 0xFF ] << 24 );
+}
+
 int mbedtls_internal_aes_encrypt( mbedtls_aes_context *ctx,
                                   const unsigned char input[16],
                                   unsigned char output[16] )
 {
-    int i;
-    uint32_t *RK, X0, X1, X2, X3, Y0, Y1, Y2, Y3;
+    int i, j, offset, start_fin_loops = 1;
+    aes_r_data_t aes_data_real;         // real data
+#if AES_SCA_CM_ROUNDS != 0
+    aes_r_data_t aes_data_fake;         // fake data
+#endif /* AES_SCA_CM_ROUNDS != 0 */
+    aes_r_data_t *aes_data_ptr;         // pointer to aes_data_real or aes_data_fake
+    aes_r_data_t *aes_data_table[2];    // pointers to real and fake data
+    int round_ctrl_table_len = ctx->nr - 1 + AES_SCA_CM_ROUNDS + 2 + 2;
+    volatile int flow_control;
+    // control bytes for AES rounds, reserve based on max ctx->nr
+    uint8_t round_ctrl_table[ 14 - 1 + AES_SCA_CM_ROUNDS + 2 + 2];
 
-    RK = ctx->rk;
+    aes_data_real.rk_ptr = ctx->rk;
+    aes_data_table[0] = &aes_data_real;
 
-    GET_UINT32_LE( X0, input,  0 ); X0 ^= *RK++;
-    GET_UINT32_LE( X1, input,  4 ); X1 ^= *RK++;
-    GET_UINT32_LE( X2, input,  8 ); X2 ^= *RK++;
-    GET_UINT32_LE( X3, input, 12 ); X3 ^= *RK++;
+#if AES_SCA_CM_ROUNDS != 0
+    aes_data_table[1] = &aes_data_fake;
+    aes_data_fake.rk_ptr = ctx->rk;
+    start_fin_loops = 2;
+    for( i = 0; i < 4; i++ )
+        aes_data_fake.xy_values[i] = mbedtls_platform_random_in_range( 0xffffffff );
+#endif
 
-    for( i = ( ctx->nr >> 1 ) - 1; i > 0; i-- )
+    // Get randomized AES calculation control bytes
+    flow_control = aes_sca_cm_data_randomize( round_ctrl_table,
+        round_ctrl_table_len );
+
+    mbedtls_platform_memset( aes_data_real.xy_values, 0, 16 );
+    offset = mbedtls_platform_random_in_range( 4 );
+
+    for( i = offset; i < 4; i++ )
     {
-        AES_FROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 );
-        AES_FROUND( X0, X1, X2, X3, Y0, Y1, Y2, Y3 );
+        GET_UINT32_LE( aes_data_real.xy_values[i], input,  ( i * 4 ) );
     }
 
-    AES_FROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 );
+    for( i = 0; i < offset; i++ )
+    {
+        GET_UINT32_LE( aes_data_real.xy_values[i], input,  ( i * 4 ) );
+    }
 
-    X0 = *RK++ ^ \
-            ( (uint32_t) FSb[ ( Y0       ) & 0xFF ]       ) ^
-            ( (uint32_t) FSb[ ( Y1 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) FSb[ ( Y2 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) FSb[ ( Y3 >> 24 ) & 0xFF ] << 24 );
+    for( i = 0; i < 4; i++ )
+    {
+        for( j = 0; j < start_fin_loops; j++ )
+        {
+            aes_data_ptr =
+                aes_data_table[round_ctrl_table[ round_ctrl_table_len - 2 + j ] >> 4];
+            aes_data_ptr->xy_values[i] ^= *aes_data_ptr->rk_ptr++;
+            flow_control++;
+        }
+    }
 
-    X1 = *RK++ ^ \
-            ( (uint32_t) FSb[ ( Y1       ) & 0xFF ]       ) ^
-            ( (uint32_t) FSb[ ( Y2 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) FSb[ ( Y3 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) FSb[ ( Y0 >> 24 ) & 0xFF ] << 24 );
+    for( i = 0; i < ( ctx->nr - 1 + AES_SCA_CM_ROUNDS ); i++ )
+    {
+        // Read AES control data
+        aes_data_ptr = aes_data_table[round_ctrl_table[i] >> 4];
+        offset = round_ctrl_table[i] & 0x0f;
 
-    X2 = *RK++ ^ \
-            ( (uint32_t) FSb[ ( Y2       ) & 0xFF ]       ) ^
-            ( (uint32_t) FSb[ ( Y3 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) FSb[ ( Y0 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) FSb[ ( Y1 >> 24 ) & 0xFF ] << 24 );
+        aes_data_ptr->rk_ptr = aes_fround( aes_data_ptr->rk_ptr,
+            &aes_data_ptr->xy_values[0 + offset],
+            &aes_data_ptr->xy_values[1 + offset],
+            &aes_data_ptr->xy_values[2 + offset],
+            &aes_data_ptr->xy_values[3 + offset],
+            aes_data_ptr->xy_values[4 - offset],
+            aes_data_ptr->xy_values[5 - offset],
+            aes_data_ptr->xy_values[6 - offset],
+            aes_data_ptr->xy_values[7 - offset] );
+        flow_control++;
+    }
 
-    X3 = *RK++ ^ \
-            ( (uint32_t) FSb[ ( Y3       ) & 0xFF ]       ) ^
-            ( (uint32_t) FSb[ ( Y0 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) FSb[ ( Y1 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) FSb[ ( Y2 >> 24 ) & 0xFF ] << 24 );
+    for( j = 0; j < start_fin_loops; j++ )
+    {
+        aes_data_ptr = aes_data_table[round_ctrl_table[ i + j ] >> 4];
+        aes_fround_final( aes_data_ptr->rk_ptr,
+            &aes_data_ptr->xy_values[0],
+            &aes_data_ptr->xy_values[1],
+            &aes_data_ptr->xy_values[2],
+            &aes_data_ptr->xy_values[3],
+            aes_data_ptr->xy_values[4],
+            aes_data_ptr->xy_values[5],
+            aes_data_ptr->xy_values[6],
+            aes_data_ptr->xy_values[7] );
+        flow_control++;
+    }
 
-    PUT_UINT32_LE( X0, output,  0 );
-    PUT_UINT32_LE( X1, output,  4 );
-    PUT_UINT32_LE( X2, output,  8 );
-    PUT_UINT32_LE( X3, output, 12 );
+    mbedtls_platform_memset( output, 0, 16 );
+    offset = mbedtls_platform_random_in_range( 4 );
 
-    return( 0 );
+    for( i = offset; i < 4; i++ )
+    {
+        PUT_UINT32_LE( aes_data_real.xy_values[i], output,  ( i * 4 ) );
+        flow_control++;
+    }
+
+    for( i = 0; i < offset; i++ )
+    {
+        PUT_UINT32_LE( aes_data_real.xy_values[i], output,  ( i * 4 ) );
+        flow_control++;
+    }
+
+    if( flow_control == ( AES_SCA_CM_ROUNDS + ( 4 * start_fin_loops ) +
+        ctx->nr - 1 + AES_SCA_CM_ROUNDS + start_fin_loops + 4 )  )
+    {
+        /* Validate control path due possible fault injection */
+        return 0;
+    }
+
+    return( MBEDTLS_ERR_PLATFORM_FAULT_DETECTED );
 }
 #endif /* !MBEDTLS_AES_ENCRYPT_ALT */
 
@@ -960,58 +1176,148 @@
 
 #if !defined(MBEDTLS_AES_DECRYPT_ALT)
 #if !defined(MBEDTLS_AES_ONLY_ENCRYPT)
+
+static uint32_t *aes_rround( uint32_t *R,
+    uint32_t *X0, uint32_t *X1, uint32_t *X2, uint32_t *X3,
+    uint32_t Y0, uint32_t Y1, uint32_t Y2, uint32_t Y3 )
+{
+    *X0 = *R++ ^ AES_RT0( ( Y0       ) & 0xFF ) ^
+                 AES_RT1( ( Y3 >>  8 ) & 0xFF ) ^
+                 AES_RT2( ( Y2 >> 16 ) & 0xFF ) ^
+                 AES_RT3( ( Y1 >> 24 ) & 0xFF );
+
+    *X1 = *R++ ^ AES_RT0( ( Y1       ) & 0xFF ) ^
+                 AES_RT1( ( Y0 >>  8 ) & 0xFF ) ^
+                 AES_RT2( ( Y3 >> 16 ) & 0xFF ) ^
+                 AES_RT3( ( Y2 >> 24 ) & 0xFF );
+
+    *X2 = *R++ ^ AES_RT0( ( Y2       ) & 0xFF ) ^
+                 AES_RT1( ( Y1 >>  8 ) & 0xFF ) ^
+                 AES_RT2( ( Y0 >> 16 ) & 0xFF ) ^
+                 AES_RT3( ( Y3 >> 24 ) & 0xFF );
+
+    *X3 = *R++ ^ AES_RT0( ( Y3       ) & 0xFF ) ^
+                 AES_RT1( ( Y2 >>  8 ) & 0xFF ) ^
+                 AES_RT2( ( Y1 >> 16 ) & 0xFF ) ^
+                 AES_RT3( ( Y0 >> 24 ) & 0xFF );
+    return R;
+}
+
+static void aes_rround_final( uint32_t *R,
+    uint32_t *X0, uint32_t *X1, uint32_t *X2, uint32_t *X3,
+    uint32_t Y0, uint32_t Y1, uint32_t Y2, uint32_t Y3 )
+{
+    *X0 = *R++ ^ ( (uint32_t) RSb[ ( (Y0)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) RSb[ ( (Y3) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) RSb[ ( (Y2) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) RSb[ ( (Y1) >> 24 ) & 0xFF ] << 24 );
+
+    *X1 = *R++ ^ ( (uint32_t) RSb[ ( (Y1)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) RSb[ ( (Y0) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) RSb[ ( (Y3) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) RSb[ ( (Y2) >> 24 ) & 0xFF ] << 24 );
+
+    *X2 = *R++ ^ ( (uint32_t) RSb[ ( (Y2)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) RSb[ ( (Y1) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) RSb[ ( (Y0) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) RSb[ ( (Y3) >> 24 ) & 0xFF ] << 24 );
+
+    *X3 = *R++ ^ ( (uint32_t) RSb[ ( (Y3)       ) & 0xFF ]       ) ^
+                 ( (uint32_t) RSb[ ( (Y2) >>  8 ) & 0xFF ] <<  8 ) ^
+                 ( (uint32_t) RSb[ ( (Y1) >> 16 ) & 0xFF ] << 16 ) ^
+                 ( (uint32_t) RSb[ ( (Y0) >> 24 ) & 0xFF ] << 24 );
+}
+
 int mbedtls_internal_aes_decrypt( mbedtls_aes_context *ctx,
                                   const unsigned char input[16],
                                   unsigned char output[16] )
 {
-    int i;
-    uint32_t *RK, X0, X1, X2, X3, Y0, Y1, Y2, Y3;
+    int i, j, offset, start_fin_loops = 1;
+    aes_r_data_t aes_data_real;         // real data
+#if AES_SCA_CM_ROUNDS != 0
+    aes_r_data_t aes_data_fake;         // fake data
+#endif /* AES_SCA_CM_ROUNDS != 0 */
+    aes_r_data_t *aes_data_ptr;         // pointer to aes_data_real or aes_data_fake
+    aes_r_data_t *aes_data_table[2];    // pointers to real and fake data
+    int round_ctrl_table_len = ctx->nr - 1 + AES_SCA_CM_ROUNDS + 2 + 2;
+    // control bytes for AES rounds, reserve based on max ctx->nr
+    volatile int flow_control;
+    uint8_t round_ctrl_table[ 14 - 1 + AES_SCA_CM_ROUNDS + 2 + 2 ];
 
-    RK = ctx->rk;
+    aes_data_real.rk_ptr = ctx->rk;
+    aes_data_table[0] = &aes_data_real;
 
-    GET_UINT32_LE( X0, input,  0 ); X0 ^= *RK++;
-    GET_UINT32_LE( X1, input,  4 ); X1 ^= *RK++;
-    GET_UINT32_LE( X2, input,  8 ); X2 ^= *RK++;
-    GET_UINT32_LE( X3, input, 12 ); X3 ^= *RK++;
+#if AES_SCA_CM_ROUNDS != 0
+    aes_data_table[1] = &aes_data_fake;
+    aes_data_fake.rk_ptr = ctx->rk;
+    start_fin_loops = 2;
+    for( i = 0; i < 4; i++ )
+        aes_data_fake.xy_values[i] = mbedtls_platform_random_in_range( 0xffffffff );
+#endif
 
-    for( i = ( ctx->nr >> 1 ) - 1; i > 0; i-- )
+    // Get randomized AES calculation control bytes
+    flow_control = aes_sca_cm_data_randomize( round_ctrl_table,
+        round_ctrl_table_len );
+
+    for( i = 0; i < 4; i++ )
     {
-        AES_RROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 );
-        AES_RROUND( X0, X1, X2, X3, Y0, Y1, Y2, Y3 );
+        GET_UINT32_LE( aes_data_real.xy_values[i], input,  ( i * 4 ) );
+        for( j = 0; j < start_fin_loops; j++ )
+        {
+            aes_data_ptr =
+                aes_data_table[round_ctrl_table[ round_ctrl_table_len - 4 + j ] >> 4];
+            aes_data_ptr->xy_values[i] ^= *aes_data_ptr->rk_ptr++;
+            flow_control++;
+        }
     }
 
-    AES_RROUND( Y0, Y1, Y2, Y3, X0, X1, X2, X3 );
+    for( i = 0; i < ( ctx->nr - 1 + AES_SCA_CM_ROUNDS ); i++ )
+    {
+        // Read AES control data
+        aes_data_ptr = aes_data_table[round_ctrl_table[i] >> 4];
+        offset = round_ctrl_table[i] & 0x0f;
 
-    X0 = *RK++ ^ \
-            ( (uint32_t) RSb[ ( Y0       ) & 0xFF ]       ) ^
-            ( (uint32_t) RSb[ ( Y3 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) RSb[ ( Y2 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) RSb[ ( Y1 >> 24 ) & 0xFF ] << 24 );
+        aes_data_ptr->rk_ptr = aes_rround( aes_data_ptr->rk_ptr,
+            &aes_data_ptr->xy_values[0 + offset],
+            &aes_data_ptr->xy_values[1 + offset],
+            &aes_data_ptr->xy_values[2 + offset],
+            &aes_data_ptr->xy_values[3 + offset],
+            aes_data_ptr->xy_values[4 - offset],
+            aes_data_ptr->xy_values[5 - offset],
+            aes_data_ptr->xy_values[6 - offset],
+            aes_data_ptr->xy_values[7 - offset] );
+        flow_control++;
+    }
 
-    X1 = *RK++ ^ \
-            ( (uint32_t) RSb[ ( Y1       ) & 0xFF ]       ) ^
-            ( (uint32_t) RSb[ ( Y0 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) RSb[ ( Y3 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) RSb[ ( Y2 >> 24 ) & 0xFF ] << 24 );
+    for( j = 0; j < start_fin_loops; j++ )
+    {
+        aes_data_ptr = aes_data_table[round_ctrl_table[ i + j ] >> 4];
+        aes_rround_final( aes_data_ptr->rk_ptr,
+            &aes_data_ptr->xy_values[0],
+            &aes_data_ptr->xy_values[1],
+            &aes_data_ptr->xy_values[2],
+            &aes_data_ptr->xy_values[3],
+            aes_data_ptr->xy_values[4],
+            aes_data_ptr->xy_values[5],
+            aes_data_ptr->xy_values[6],
+            aes_data_ptr->xy_values[7] );
+        flow_control++;
+    }
 
-    X2 = *RK++ ^ \
-            ( (uint32_t) RSb[ ( Y2       ) & 0xFF ]       ) ^
-            ( (uint32_t) RSb[ ( Y1 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) RSb[ ( Y0 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) RSb[ ( Y3 >> 24 ) & 0xFF ] << 24 );
+    for( i = 0; i < 4; i++ )
+    {
+        PUT_UINT32_LE( aes_data_real.xy_values[i], output,  ( i * 4 ) );
+        flow_control++;
+    }
 
-    X3 = *RK++ ^ \
-            ( (uint32_t) RSb[ ( Y3       ) & 0xFF ]       ) ^
-            ( (uint32_t) RSb[ ( Y2 >>  8 ) & 0xFF ] <<  8 ) ^
-            ( (uint32_t) RSb[ ( Y1 >> 16 ) & 0xFF ] << 16 ) ^
-            ( (uint32_t) RSb[ ( Y0 >> 24 ) & 0xFF ] << 24 );
+    if( flow_control == ( AES_SCA_CM_ROUNDS + ( 4 * start_fin_loops ) +
+        ctx->nr - 1 + AES_SCA_CM_ROUNDS + start_fin_loops + 4 )  )
+    {
+        /* Validate control path due possible fault injection */
+        return 0;
+    }
 
-    PUT_UINT32_LE( X0, output,  0 );
-    PUT_UINT32_LE( X1, output,  4 );
-    PUT_UINT32_LE( X2, output,  8 );
-    PUT_UINT32_LE( X3, output, 12 );
-
-    return( 0 );
+    return( MBEDTLS_ERR_PLATFORM_FAULT_DETECTED );
 }
 #endif /* !MBEDTLS_AES_ONLY_ENCRYPT */
 #endif /* !MBEDTLS_AES_DECRYPT_ALT */
@@ -1062,6 +1368,7 @@
         //
     }
 #endif
+
 #if defined(MBEDTLS_AES_ONLY_ENCRYPT)
     return( mbedtls_internal_aes_encrypt( ctx, input, output ) );
 #else /* MBEDTLS_AES_ONLY_ENCRYPT */
@@ -1480,7 +1787,7 @@
 
     n = *nc_off;
 
-    if ( n > 0x0F )
+    if( n > 0x0F )
         return( MBEDTLS_ERR_AES_BAD_INPUT_DATA );
 
     while( length-- )
diff --git a/library/entropy.c b/library/entropy.c
index b4d1f29..6656ee8 100644
--- a/library/entropy.c
+++ b/library/entropy.c
@@ -273,7 +273,7 @@
         volatile int strong_fi = ctx->source[i].strong;
         if( strong_fi == MBEDTLS_ENTROPY_SOURCE_STRONG )
         {
-            mbedtls_platform_enforce_volatile_reads();
+            mbedtls_platform_random_delay();
 
             if( strong_fi == MBEDTLS_ENTROPY_SOURCE_STRONG )
                 have_one_strong_fi = MBEDTLS_ENTROPY_SOURCE_STRONG;
@@ -305,7 +305,7 @@
 
     if( have_one_strong_fi == MBEDTLS_ENTROPY_SOURCE_STRONG )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( have_one_strong_fi == MBEDTLS_ENTROPY_SOURCE_STRONG )
         {
             return( ret );
diff --git a/library/pk.c b/library/pk.c
index 27276a8..caa5e17 100644
--- a/library/pk.c
+++ b/library/pk.c
@@ -597,7 +597,7 @@
 
     if( ret_fi == UECC_SUCCESS )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ret_fi == UECC_SUCCESS )
             return( 0 );
         else
@@ -1553,7 +1553,7 @@
 
     if( verify_ret == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( verify_ret == 0 )
         {
             return( verify_ret );
diff --git a/library/platform_util.c b/library/platform_util.c
index 16867aa..2c22b3c 100644
--- a/library/platform_util.c
+++ b/library/platform_util.c
@@ -45,6 +45,9 @@
 #include <stddef.h>
 #include <string.h>
 
+/* Max number of loops for mbedtls_platform_random_delay */
+#define MAX_RAND_DELAY  100
+
 #if !defined(MBEDTLS_PLATFORM_ZEROIZE_ALT)
 /*
  * This implementation should never be optimized out by the compiler
@@ -165,13 +168,32 @@
 #endif
 }
 
-/* Some compilers (armcc 5 for example) optimize away successive reads from a
- * volatile local variable (which we use as a counter-measure to fault
- * injection attacks), unless there is a call to an external function between
- * them. This functions doesn't need to do anything, it just needs to be
- * in another compilation unit. So here's a function that does nothing. */
-void mbedtls_platform_enforce_volatile_reads( void )
+void mbedtls_platform_random_delay( void )
 {
+#if !defined(MBEDTLS_ENTROPY_HARDWARE_ALT)
+    return;
+#else
+    size_t rn_1, rn_2, rn_3;
+    volatile size_t i = 0;
+    uint8_t shift;
+
+    rn_1 = mbedtls_platform_random_in_range( MAX_RAND_DELAY );
+    rn_2 = mbedtls_platform_random_in_range( 0xffffffff ) + 1;
+    rn_3 = mbedtls_platform_random_in_range( 0xffffffff ) + 1;
+
+    do
+    {
+        i++;
+        shift = rn_2 & 0x07;
+        if ( i % 2 )
+            rn_2 = (uint32_t)( rn_2 >> shift | rn_2 << ( 32 - shift ) );
+        else
+            rn_3 = (uint32_t)( rn_3 << shift | rn_3 >> ( 32 - shift ) );
+        rn_2 ^= rn_3;
+    } while( i < rn_1 || rn_2 == 0 || rn_3 == 0 );
+
+    return;
+#endif /* !MBEDTLS_ENTROPY_HARDWARE_ALT */
 }
 
 #if defined(MBEDTLS_HAVE_TIME_DATE) && !defined(MBEDTLS_PLATFORM_GMTIME_R_ALT)
diff --git a/library/sha256.c b/library/sha256.c
index 785e28b..4dcec89 100644
--- a/library/sha256.c
+++ b/library/sha256.c
@@ -196,11 +196,23 @@
         A[i] = ctx->state[i];
 
 #if defined(MBEDTLS_SHA256_SMALLER)
+    {
+        uint32_t offset = mbedtls_platform_random_in_range(16);
+        mbedtls_platform_memset( W, 0, sizeof( W ) );
+
+        for( i = offset; i < 16; i++ )
+        {
+            W[i] = (uint32_t)mbedtls_platform_get_uint32_be( &data[4 * i] );
+        }
+        for( i = 0; i < offset; i++ )
+        {
+            W[i] = (uint32_t)mbedtls_platform_get_uint32_be( &data[4 * i] );
+        }
+    }
+
     for( i = 0; i < 64; i++ )
     {
-        if( i < 16 )
-            W[i] = (uint32_t)mbedtls_platform_get_uint32_be( &data[4 * i] );
-        else
+        if( i >= 16 )
             R( i );
 
         P( A[0], A[1], A[2], A[3], A[4], A[5], A[6], A[7], W[i], K[i] );
@@ -323,6 +335,7 @@
     int ret, s_pos, o_pos;
     uint32_t used;
     uint32_t high, low;
+    uint32_t offset = 0;
 
     SHA256_VALIDATE_RET( ctx != NULL );
     SHA256_VALIDATE_RET( (unsigned char *)output != NULL );
@@ -366,11 +379,15 @@
     /*
      * Output final state
      */
+    offset = mbedtls_platform_random_in_range(7);
 
-    for( s_pos = 0, o_pos = 0; s_pos < 7; s_pos++, o_pos += 4 )
+    mbedtls_platform_memset( output, 0, 32 );
+
+    for( s_pos = offset, o_pos = offset * 4; s_pos < 7;
+            s_pos++, o_pos += 4 )
     {
         (void)mbedtls_platform_put_uint32_be( &output[o_pos],
-                                              ctx->state[s_pos] );
+                                            ctx->state[s_pos] );
     }
 
 #if !defined(MBEDTLS_SHA256_NO_SHA224)
@@ -378,6 +395,11 @@
 #endif
         (void)mbedtls_platform_put_uint32_be( &output[28], ctx->state[7] );
 
+    for( s_pos = 0, o_pos = 0; s_pos < (int)offset; s_pos++, o_pos += 4 )
+    {
+        (void)mbedtls_platform_put_uint32_be( &output[o_pos],
+                                            ctx->state[s_pos] );
+    }
     return( 0 );
 }
 
diff --git a/library/ssl_cli.c b/library/ssl_cli.c
index 479554d..3c59923 100644
--- a/library/ssl_cli.c
+++ b/library/ssl_cli.c
@@ -717,7 +717,7 @@
             ( mbedtls_ssl_conf_get_prng( ssl->conf ), p, 28 );
     if( ret == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ret == 0 )
         {
             ssl->handshake->hello_random_set = MBEDTLS_SSL_FI_FLAG_SET;
@@ -2369,7 +2369,7 @@
 
     if( ret == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ret == 0 )
         {
             ssl->handshake->premaster_generated = MBEDTLS_SSL_FI_FLAG_SET;
@@ -2442,7 +2442,7 @@
 
     if( ret == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ret == 0 )
         {
             ssl->handshake->premaster_generated = MBEDTLS_SSL_FI_FLAG_SET;
@@ -3071,7 +3071,7 @@
 
         if( ret == 0 )
         {
-            mbedtls_platform_enforce_volatile_reads();
+            mbedtls_platform_random_delay();
 
             if( ret == 0 )
             {
diff --git a/library/ssl_srv.c b/library/ssl_srv.c
index 92d1da0..bab8f00 100644
--- a/library/ssl_srv.c
+++ b/library/ssl_srv.c
@@ -4064,7 +4064,7 @@
 
     if( pmscounter == ssl->handshake->pmslen )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( pmscounter == ssl->handshake->pmslen )
         {
             ssl->handshake->premaster_generated = MBEDTLS_SSL_FI_FLAG_SET;
@@ -4651,7 +4651,7 @@
 
     if( ret == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
 
         if( ret == 0 )
         {
diff --git a/library/ssl_tls.c b/library/ssl_tls.c
index 19bdc90..1d5503b 100644
--- a/library/ssl_tls.c
+++ b/library/ssl_tls.c
@@ -791,8 +791,8 @@
         return( MBEDTLS_ERR_SSL_BAD_INPUT_DATA );
 
     nb = strlen( label );
-    mbedtls_platform_memcpy( tmp + md_len, label, nb );
-    mbedtls_platform_memcpy( tmp + md_len + nb, random, rlen );
+    (void)mbedtls_platform_memcpy( tmp + md_len, label, nb );
+    (void)mbedtls_platform_memcpy( tmp + md_len + nb, random, rlen );
     nb += rlen;
 
     /*
@@ -801,19 +801,28 @@
     if ( ( ret = mbedtls_md_setup( &md_ctx, md_info, 1 ) ) != 0 )
         return( ret );
 
-    mbedtls_md_hmac_starts( &md_ctx, secret, slen );
-    mbedtls_md_hmac_update( &md_ctx, tmp + md_len, nb );
-    mbedtls_md_hmac_finish( &md_ctx, tmp );
+    if ( ( ret = mbedtls_md_hmac_starts( &md_ctx, secret, slen ) ) != 0 )
+        return( ret );
+    if ( ( ret = mbedtls_md_hmac_update( &md_ctx, tmp + md_len, nb ) ) != 0 )
+        return( ret );
+    if ( ( ret = mbedtls_md_hmac_finish( &md_ctx, tmp ) ) != 0 )
+        return( ret );
 
     for( i = 0; i < dlen; i += md_len )
     {
-        mbedtls_md_hmac_reset ( &md_ctx );
-        mbedtls_md_hmac_update( &md_ctx, tmp, md_len + nb );
-        mbedtls_md_hmac_finish( &md_ctx, h_i );
+        if ( ( ret = mbedtls_md_hmac_reset ( &md_ctx ) ) != 0 )
+            return( ret );
+        if ( ( ret = mbedtls_md_hmac_update( &md_ctx, tmp, md_len + nb ) ) != 0 )
+            return( ret );
+        if ( ( ret = mbedtls_md_hmac_finish( &md_ctx, h_i ) ) != 0 )
+            return( ret );
 
-        mbedtls_md_hmac_reset ( &md_ctx );
-        mbedtls_md_hmac_update( &md_ctx, tmp, md_len );
-        mbedtls_md_hmac_finish( &md_ctx, tmp );
+        if ( ( ret = mbedtls_md_hmac_reset ( &md_ctx ) ) != 0 )
+            return( ret );
+        if ( ( ret = mbedtls_md_hmac_update( &md_ctx, tmp, md_len ) ) != 0 )
+            return( ret );
+        if ( ( ret = mbedtls_md_hmac_finish( &md_ctx, tmp ) ) != 0 )
+            return( ret );
 
         k = ( i + md_len > dlen ) ? dlen % md_len : md_len;
 
@@ -823,8 +832,8 @@
 
     mbedtls_md_free( &md_ctx );
 
-    mbedtls_platform_zeroize( tmp, sizeof( tmp ) );
-    mbedtls_platform_zeroize( h_i, sizeof( h_i ) );
+    (void)mbedtls_platform_zeroize( tmp, sizeof( tmp ) );
+    (void)mbedtls_platform_zeroize( h_i, sizeof( h_i ) );
 
     return( 0 );
 }
@@ -1929,7 +1938,7 @@
                   ssl );
     if( ret == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ret == 0 )
         {
             ssl->handshake->key_derivation_done = MBEDTLS_SSL_FI_FLAG_SET;
@@ -2011,7 +2020,7 @@
                                       mbedtls_ssl_conf_get_prng( ssl->conf ) );
         if( ret == 0 )
         {
-            mbedtls_platform_enforce_volatile_reads();
+            mbedtls_platform_random_delay();
             if( ret == 0 )
             {
                 ssl->handshake->premaster_generated = MBEDTLS_SSL_FI_FLAG_SET;
@@ -2054,7 +2063,7 @@
                                        mbedtls_ssl_conf_get_prng( ssl->conf ) );
         if( ret == 0 )
         {
-            mbedtls_platform_enforce_volatile_reads();
+            mbedtls_platform_random_delay();
             if( ret == 0 )
             {
                 ssl->handshake->premaster_generated = MBEDTLS_SSL_FI_FLAG_SET;
@@ -2085,7 +2094,7 @@
             mbedtls_ssl_suite_get_key_exchange( ciphersuite_info ) );
         if( ret == 0 )
         {
-            mbedtls_platform_enforce_volatile_reads();
+            mbedtls_platform_random_delay();
             if( ret == 0 )
             {
                 ssl->handshake->premaster_generated = MBEDTLS_SSL_FI_FLAG_SET;
@@ -2114,7 +2123,7 @@
                 mbedtls_ssl_conf_get_prng( ssl->conf ) );
         if( ret == 0 )
         {
-            mbedtls_platform_enforce_volatile_reads();
+            mbedtls_platform_random_delay();
             if( ret == 0 )
             {
                 ssl->handshake->premaster_generated = MBEDTLS_SSL_FI_FLAG_SET;
@@ -7346,7 +7355,7 @@
 
     if( verify_ret == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( verify_ret == 0 )
         {
             flow_counter++;
@@ -7436,7 +7445,7 @@
         ( verify_ret == MBEDTLS_ERR_X509_CERT_VERIFY_FAILED ||
           verify_ret == MBEDTLS_ERR_SSL_BAD_HS_CERTIFICATE ) )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( authmode == MBEDTLS_SSL_VERIFY_OPTIONAL &&
             ( verify_ret == MBEDTLS_ERR_X509_CERT_VERIFY_FAILED ||
               verify_ret == MBEDTLS_ERR_SSL_BAD_HS_CERTIFICATE ) )
@@ -7502,7 +7511,7 @@
         flow_counter == 4 )
 #endif
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( verify_ret == 0 &&
 #if defined(MBEDTLS_ECP_C) || defined(MBEDTLS_USE_TINYCRYPT)
             flow_counter == 5 )
@@ -7989,7 +7998,7 @@
         1 )
 #endif
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( authmode == MBEDTLS_SSL_VERIFY_NONE ||
             authmode == MBEDTLS_SSL_VERIFY_OPTIONAL ||
 #if defined(MBEDTLS_KEY_EXCHANGE__WITH_CERT__ENABLED)
@@ -8010,7 +8019,7 @@
 #if !defined(MBEDTLS_SSL_NO_SESSION_RESUMPTION)
     if( ssl->handshake->resume == MBEDTLS_SSL_FI_FLAG_SET )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ssl->handshake->resume == MBEDTLS_SSL_FI_FLAG_SET )
         {
             /* When doing session resume, no premaster or peer authentication */
@@ -8027,7 +8036,7 @@
 
     if( ssl->handshake->peer_authenticated == MBEDTLS_SSL_FI_FLAG_SET )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ssl->handshake->peer_authenticated == MBEDTLS_SSL_FI_FLAG_SET )
         {
             ret = 0;
@@ -8048,7 +8057,7 @@
         ssl->handshake->key_derivation_done == MBEDTLS_SSL_FI_FLAG_SET &&
         ssl->handshake->premaster_generated == MBEDTLS_SSL_FI_FLAG_SET )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( ssl->handshake->hello_random_set == MBEDTLS_SSL_FI_FLAG_SET &&
             ssl->handshake->key_derivation_done == MBEDTLS_SSL_FI_FLAG_SET &&
             ssl->handshake->premaster_generated == MBEDTLS_SSL_FI_FLAG_SET )
@@ -11119,6 +11128,9 @@
     mbedtls_platform_memcpy( buf, ssl->in_offt, n );
     ssl->in_msglen -= n;
 
+    // clear incoming data after it's copied to buffer
+    mbedtls_platform_memset(ssl->in_offt, 0, n);
+
     if( ssl->in_msglen == 0 )
     {
         /* all bytes consumed */
diff --git a/library/version_features.c b/library/version_features.c
index 7a97d38..6882830 100644
--- a/library/version_features.c
+++ b/library/version_features.c
@@ -270,6 +270,9 @@
 #if defined(MBEDTLS_AES_ONLY_ENCRYPT)
     "MBEDTLS_AES_ONLY_ENCRYPT",
 #endif /* MBEDTLS_AES_ONLY_ENCRYPT */
+#if defined(MBEDTLS_AES_SCA_COUNTERMEASURES)
+    "MBEDTLS_AES_SCA_COUNTERMEASURES",
+#endif /* MBEDTLS_AES_SCA_COUNTERMEASURES */
 #if defined(MBEDTLS_CAMELLIA_SMALL_MEMORY)
     "MBEDTLS_CAMELLIA_SMALL_MEMORY",
 #endif /* MBEDTLS_CAMELLIA_SMALL_MEMORY */
diff --git a/library/x509_crt.c b/library/x509_crt.c
index fd3fa1a..af8f1d6 100644
--- a/library/x509_crt.c
+++ b/library/x509_crt.c
@@ -3043,7 +3043,7 @@
 
         if( ret_fi == 0 )
         {
-            mbedtls_platform_enforce_volatile_reads();
+            mbedtls_platform_random_delay();
             if( ret_fi == 0 )
                 signature_is_good = X509_SIGNATURE_IS_GOOD;
         }
@@ -3549,7 +3549,7 @@
         if( signature_is_good_fi != X509_SIGNATURE_IS_GOOD )
             *flags |= MBEDTLS_X509_BADCERT_NOT_TRUSTED | X509_BADCERT_FI_EXTRA;
 
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( signature_is_good_fi != X509_SIGNATURE_IS_GOOD )
             *flags |= MBEDTLS_X509_BADCERT_NOT_TRUSTED | X509_BADCERT_FI_EXTRA;
 
@@ -3861,7 +3861,7 @@
     flags_fi = *flags;
     if( flags_fi == 0 )
     {
-        mbedtls_platform_enforce_volatile_reads();
+        mbedtls_platform_random_delay();
         if( flags_fi == 0 )
             return( 0 );
     }
diff --git a/programs/ssl/query_config.c b/programs/ssl/query_config.c
index fd2bc90..315065b 100644
--- a/programs/ssl/query_config.c
+++ b/programs/ssl/query_config.c
@@ -762,6 +762,14 @@
     }
 #endif /* MBEDTLS_AES_ONLY_ENCRYPT */
 
+#if defined(MBEDTLS_AES_SCA_COUNTERMEASURES)
+    if( strcmp( "MBEDTLS_AES_SCA_COUNTERMEASURES", config ) == 0 )
+    {
+        MACRO_EXPANSION_TO_STR( MBEDTLS_AES_SCA_COUNTERMEASURES );
+        return( 0 );
+    }
+#endif /* MBEDTLS_AES_SCA_COUNTERMEASURES */
+
 #if defined(MBEDTLS_CAMELLIA_SMALL_MEMORY)
     if( strcmp( "MBEDTLS_CAMELLIA_SMALL_MEMORY", config ) == 0 )
     {
diff --git a/scripts/config.pl b/scripts/config.pl
index ff522b6..2519623 100755
--- a/scripts/config.pl
+++ b/scripts/config.pl
@@ -53,6 +53,7 @@
 #   MBEDTLS_NO_64BIT_MULTIPLICATION
 #   MBEDTLS_AES_ONLY_128_BIT_KEY_LENGTH
 #   MBEDTLS_AES_ONLY_ENCRYPT
+#   MBEDTLS_AES_SCA_COUNTERMEASURES
 #   and any symbol beginning _ALT
 #
 
@@ -130,6 +131,7 @@
 MBEDTLS_USE_TINYCRYPT
 MBEDTLS_AES_ONLY_128_BIT_KEY_LENGTH
 MBEDTLS_AES_ONLY_ENCRYPT
+MBEDTLS_AES_SCA_COUNTERMEASURES
 _ALT\s*$
 );
 
diff --git a/tests/scripts/all.sh b/tests/scripts/all.sh
index 42ef32d..0ea4ae8 100755
--- a/tests/scripts/all.sh
+++ b/tests/scripts/all.sh
@@ -1294,6 +1294,16 @@
     make test
 }
 
+component_test_aes_sca_countermeasures () {
+    msg "build: default config + MBEDTLS_AES_SCA_COUNTERMEASURES + MBEDTLS_ENTROPY_HARDWARE_ALT + !MBEDTLS_AESNI_C"
+    scripts/config.pl set MBEDTLS_AES_SCA_COUNTERMEASURES
+    scripts/config.pl set MBEDTLS_ENTROPY_HARDWARE_ALT
+    scripts/config.pl unset MBEDTLS_AESNI_C
+
+    msg "test: AES SCA countermeasures"
+    make test
+}
+
 component_test_make_shared () {
     msg "build/test: make shared" # ~ 40s
     make SHARED=1 all check
diff --git a/tests/suites/helpers.function b/tests/suites/helpers.function
index 43426f5..4b53da7 100644
--- a/tests/suites/helpers.function
+++ b/tests/suites/helpers.function
@@ -32,6 +32,8 @@
 #include "tinycrypt/ecc.h"
 #endif /* MBEDTLS_USE_TINYCRYPT */
 
+#include "mbedtls/entropy.h"
+
 #ifdef _MSC_VER
 #include <basetsd.h>
 typedef UINT8 uint8_t;
@@ -300,6 +302,10 @@
 #define ENTROPY_HAVE_STRONG
 #endif
 
+/* Set flag if number of entropy sources is more than 2 */
+#if ( MBEDTLS_ENTROPY_MAX_SOURCES > 2 )
+#define ENTROPY_SOURCE_COUNT_MORE_THAN_TWO
+#endif
 
 /*----------------------------------------------------------------------------*/
 /* Helper Functions */
diff --git a/tests/suites/test_suite_entropy.function b/tests/suites/test_suite_entropy.function
index cf19732..0442973 100644
--- a/tests/suites/test_suite_entropy.function
+++ b/tests/suites/test_suite_entropy.function
@@ -193,7 +193,7 @@
 }
 /* END_CASE */
 
-/* BEGIN_CASE */
+/* BEGIN_CASE depends_on:ENTROPY_SOURCE_COUNT_MORE_THAN_TWO */
 void entropy_source_fail( char * path )
 {
     mbedtls_entropy_context ctx;
@@ -225,7 +225,7 @@
 }
 /* END_CASE */
 
-/* BEGIN_CASE depends_on:ENTROPY_HAVE_STRONG */
+/* BEGIN_CASE depends_on:ENTROPY_HAVE_STRONG:ENTROPY_SOURCE_COUNT_MORE_THAN_TWO */
 void entropy_threshold( int threshold, int chunk_size, int result )
 {
     mbedtls_entropy_context ctx;
diff --git a/tinycrypt/ecc.c b/tinycrypt/ecc.c
index a0333b0..c6c722a 100644
--- a/tinycrypt/ecc.c
+++ b/tinycrypt/ecc.c
@@ -168,7 +168,7 @@
 	}
 
 	/* i should be 32 */
-	mbedtls_platform_enforce_volatile_reads();
+	mbedtls_platform_random_delay();
 	diff |= (unsigned char) i ^ 32;
 
 	return diff;
@@ -296,7 +296,7 @@
 	}
 
 	/* i should be -1 now */
-	mbedtls_platform_enforce_volatile_reads();
+	mbedtls_platform_random_delay();
 	diff |= i ^ -1;
 
 	return diff;
@@ -1015,13 +1015,10 @@
 static uECC_word_t regularize_k(const uECC_word_t * const k, uECC_word_t *k0,
 			 uECC_word_t *k1)
 {
-
-	wordcount_t num_n_words = NUM_ECC_WORDS;
 	bitcount_t num_n_bits = NUM_ECC_BITS;
 
 	uECC_word_t carry = uECC_vli_add(k0, k, curve_n) ||
-			     (num_n_bits < ((bitcount_t)num_n_words * uECC_WORD_SIZE * 8) &&
-			     uECC_vli_testBit(k0, num_n_bits));
+			     uECC_vli_testBit(k0, num_n_bits);
 
 	uECC_vli_add(k1, k0, curve_n);
 
@@ -1046,7 +1043,7 @@
 	if (problem != 0) {
 		return UECC_FAULT_DETECTED;
 	}
-	mbedtls_platform_enforce_volatile_reads();
+	mbedtls_platform_random_delay();
 	if (problem != 0) {
 		return UECC_FAULT_DETECTED;
 	}
@@ -1058,7 +1055,7 @@
 		/* invalid input, can happen without fault */
 		return UECC_FAILURE;
 	}
-	mbedtls_platform_enforce_volatile_reads();
+	mbedtls_platform_random_delay();
 	if (problem != 0) {
 		/* failure on second check means fault, though */
 		return UECC_FAULT_DETECTED;
@@ -1088,7 +1085,7 @@
 		r = UECC_FAULT_DETECTED;
 		goto clear_and_out;
 	}
-	mbedtls_platform_enforce_volatile_reads();
+	mbedtls_platform_random_delay();
 	if (problem != 0) {
 		r = UECC_FAULT_DETECTED;
 		goto clear_and_out;
@@ -1101,7 +1098,7 @@
 		r = UECC_FAULT_DETECTED;
 		goto clear_and_out;
 	}
-	mbedtls_platform_enforce_volatile_reads();
+	mbedtls_platform_random_delay();
 	if (problem != 0) {
 		r = UECC_FAULT_DETECTED;
 		goto clear_and_out;
@@ -1198,7 +1195,7 @@
 	/* Make sure that y^2 == x^3 + ax + b */
 	diff = uECC_vli_equal(tmp1, tmp2);
 	if (diff == 0) {
-		mbedtls_platform_enforce_volatile_reads();
+	    mbedtls_platform_random_delay();
 		if (diff == 0) {
 			return 0;
 		}
diff --git a/tinycrypt/ecc_dsa.c b/tinycrypt/ecc_dsa.c
index ca071f8..660c5e9 100644
--- a/tinycrypt/ecc_dsa.c
+++ b/tinycrypt/ecc_dsa.c
@@ -72,10 +72,6 @@
 		     unsigned bits_size)
 {
 	unsigned num_n_bytes = BITS_TO_BYTES(NUM_ECC_BITS);
-	unsigned num_n_words = BITS_TO_WORDS(NUM_ECC_BITS);
-	int shift;
-	uECC_word_t carry;
-	uECC_word_t *ptr;
 
 	if (bits_size > num_n_bytes) {
 		bits_size = num_n_bytes;
@@ -83,22 +79,6 @@
 
 	uECC_vli_clear(native);
 	uECC_vli_bytesToNative(native, bits, bits_size);
-	if (bits_size * 8 <= (unsigned)NUM_ECC_BITS) {
-		return;
-	}
-	shift = bits_size * 8 - NUM_ECC_BITS;
-	carry = 0;
-	ptr = native + num_n_words;
-	while (ptr-- > native) {
-		uECC_word_t temp = *ptr;
-		*ptr = (temp >> shift) | carry;
-		carry = temp << (uECC_WORD_BITS - shift);
-	}
-
-	/* Reduce mod curve_n */
-	if (uECC_vli_cmp_unsafe(curve_n, native) != 1) {
-		uECC_vli_sub(native, native, curve_n);
-	}
 }
 
 int uECC_sign_with_k(const uint8_t *private_key, const uint8_t *message_hash,
@@ -299,7 +279,7 @@
 	/* Accept only if v == r. */
 	diff = uECC_vli_equal(rx, r);
 	if (diff == 0) {
-		mbedtls_platform_enforce_volatile_reads();
+	    mbedtls_platform_random_delay();
 		if (diff == 0) {
 			return UECC_SUCCESS;
 		}