Iterate in 16-byte chunks

Signed-off-by: Dave Rodgman <dave.rodgman@arm.com>
diff --git a/library/aes.c b/library/aes.c
index f4b9739..ced8a32 100644
--- a/library/aes.c
+++ b/library/aes.c
@@ -1441,36 +1441,42 @@
                           const unsigned char *input,
                           unsigned char *output)
 {
-    int c, i;
     int ret = MBEDTLS_ERR_ERROR_CORRUPTION_DETECTED;
-    size_t n;
 
-    n = *nc_off;
+    size_t offset = *nc_off;
 
-    if (n > 0x0F) {
+    if (offset > 0x0F) {
         return MBEDTLS_ERR_AES_BAD_INPUT_DATA;
     }
 
-    while (length--) {
-        if (n == 0) {
+    for (size_t i = 0; i < length;) {
+        size_t n = 16;
+        if (offset == 0) {
             ret = mbedtls_aes_crypt_ecb(ctx, MBEDTLS_AES_ENCRYPT, nonce_counter, stream_block);
             if (ret != 0) {
                 goto exit;
             }
-
-            for (i = 16; i > 0; i--) {
-                if (++nonce_counter[i - 1] != 0) {
+            for (int j = 16; j > 0; j--) {
+                if (++nonce_counter[j - 1] != 0) {
                     break;
                 }
             }
+        } else {
+            n -= offset;
         }
-        c = *input++;
-        *output++ = (unsigned char) (c ^ stream_block[n]);
 
-        n = (n + 1) & 0x0F;
+        if (n > (length - i)) {
+            n = (length - i);
+        }
+        mbedtls_xor(&output[i], &input[i], &stream_block[offset], n);
+        // offset might be non-zero for the last block, but in that case, we don't use it again
+        offset = 0;
+        i += n;
     }
 
-    *nc_off = n;
+    // capture offset for future resumption
+    *nc_off = (*nc_off + length) % 16;
+
     ret = 0;
 
 exit: