Fix AES-CBC for in-place operation

Signed-off-by: Dave Rodgman <dave.rodgman@arm.com>
diff --git a/library/aes.c b/library/aes.c
index eb3f873..bfcaf35 100644
--- a/library/aes.c
+++ b/library/aes.c
@@ -1071,26 +1071,21 @@
     const unsigned char *ivp = iv;
 
     if (mode == MBEDTLS_AES_DECRYPT) {
-        if (length >= 16) {
-            unsigned char temp2[16];
-            memcpy(temp, input + length - 16, 16);
-
-            while (length > 0) {
-                ret = mbedtls_aes_crypt_ecb(ctx, mode, input, temp2);
-                if (ret != 0) {
-                    goto exit;
-                }
-
-                mbedtls_xor(output, temp2, ivp, 16);
-
-                ivp = input;
-
-                input  += 16;
-                output += 16;
-                length -= 16;
+        unsigned char temp2[16];
+        while (length > 0) {
+            memcpy(temp, input, 16);
+            ret = mbedtls_aes_crypt_ecb(ctx, mode, input, temp2);
+            if (ret != 0) {
+                goto exit;
             }
 
+            mbedtls_xor(output, temp2, iv, 16);
+
             memcpy(iv, temp, 16);
+
+            input  += 16;
+            output += 16;
+            length -= 16;
         }
     } else {
         while (length > 0) {