diff --git a/programs/aes/crypt_and_hash.c b/programs/aes/crypt_and_hash.c
index a14d20c1040f..adb95e03ca45 100644
--- a/programs/aes/crypt_and_hash.c
+++ b/programs/aes/crypt_and_hash.c
@@ -2,7 +2,7 @@
  *  \brief  Generic file encryption program using generic wrappers for configured
  *          security.
  *
- *  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
@@ -74,7 +74,7 @@ int main( void )
 int main( int argc, char *argv[] )
 {
     int ret = 1, i, n;
-    int mode, lastn;
+    int mode;
     size_t keylen, ilen, olen;
     FILE *fkey, *fin = NULL, *fout = NULL;
 
@@ -264,7 +264,7 @@ int main( int argc, char *argv[] )
     {
         /*
          * Generate the initialization vector as:
-         * IV = SHA-256( filesize || filename )[0..15]
+         * IV = MD( filesize || filename )[0..15]
          */
         for( i = 0; i < 8; i++ )
             buffer[i] = (unsigned char)( filesize >> ( i << 3 ) );
@@ -278,15 +278,6 @@ int main( int argc, char *argv[] )
 
         memcpy( IV, digest, 16 );
 
-        /*
-         * The last four bits in the IV are actually used
-         * to store the file size modulo the AES block size.
-         */
-        lastn = (int)( filesize & 0x0F );
-
-        IV[15] = (unsigned char)
-            ( ( IV[15] & 0xF0 ) | lastn );
-
         /*
          * Append the IV at the beginning of the output.
          */
@@ -393,10 +384,10 @@ int main( int argc, char *argv[] )
          *  The encrypted file must be structured as follows:
          *
          *        00 .. 15              Initialization Vector
-         *        16 .. 31              AES Encrypted Block #1
+         *        16 .. 31              Encrypted Block #1
          *           ..
-         *      N*16 .. (N+1)*16 - 1    AES Encrypted Block #N
-         *  (N+1)*16 .. (N+1)*16 + 32   HMAC-SHA-256(ciphertext)
+         *      N*16 .. (N+1)*16 - 1    Encrypted Block #N
+         *  (N+1)*16 .. (N+1)*16 + n    Hash(ciphertext)
          */
         if( filesize < 16 + mbedtls_md_get_size( md_info ) )
         {
@@ -413,7 +404,8 @@ int main( int argc, char *argv[] )
         /*
          * Check the file size.
          */
-        if( ( ( filesize - mbedtls_md_get_size( md_info ) ) %
+        if( cipher_info->mode != MBEDTLS_MODE_GCM &&
+            ( ( filesize - mbedtls_md_get_size( md_info ) ) %
                 mbedtls_cipher_get_block_size( &cipher_ctx ) ) != 0 )
         {
             mbedtls_fprintf( stderr, "File content not a multiple of the block size (%d).\n",
@@ -436,7 +428,6 @@ int main( int argc, char *argv[] )
         }
 
         memcpy( IV, buffer, 16 );
-        lastn = IV[15] & 0x0F;
 
         /*
          * Hash the IV and the secret key together 8192 times
@@ -481,18 +472,19 @@ int main( int argc, char *argv[] )
          */
         for( offset = 0; offset < filesize; offset += mbedtls_cipher_get_block_size( &cipher_ctx ) )
         {
-            if( fread( buffer, 1, mbedtls_cipher_get_block_size( &cipher_ctx ), fin ) !=
-                (size_t) mbedtls_cipher_get_block_size( &cipher_ctx ) )
+            ilen = ( (unsigned int) filesize - offset > mbedtls_cipher_get_block_size( &cipher_ctx ) ) ?
+                mbedtls_cipher_get_block_size( &cipher_ctx ) : (unsigned int) ( filesize - offset );
+
+            if( fread( buffer, 1, ilen, fin ) != ilen )
             {
                 mbedtls_fprintf( stderr, "fread(%d bytes) failed\n",
                     mbedtls_cipher_get_block_size( &cipher_ctx ) );
                 goto exit;
             }
 
-            mbedtls_md_hmac_update( &md_ctx, buffer, mbedtls_cipher_get_block_size( &cipher_ctx ) );
-            if( mbedtls_cipher_update( &cipher_ctx, buffer,
-                               mbedtls_cipher_get_block_size( &cipher_ctx ),
-                               output, &olen ) != 0 )
+            mbedtls_md_hmac_update( &md_ctx, buffer, ilen );
+            if( mbedtls_cipher_update( &cipher_ctx, buffer, ilen, output,
+                                       &olen ) != 0 )
             {
                 mbedtls_fprintf( stderr, "mbedtls_cipher_update() returned error\n" );
                 goto exit;