libtomcrypt: ctr_encrypt(): adjust for OP-TEE CE accelerated routines

Add the remaining LTC code from
c54b634 ("core: crypto: cleanup and fix CE accelerated AES CTR").
Most of that commit was made irrelevant after the move of accelerated
code out of core/lib/libtomcrypt/src, except for the bits that touch
ctr_encrypt() in core/lib/libtomcrypt/src/modes/ctr/ctr_encrypt.c.

Re-introduce the needed change. Fixes failures in xtest 4003 when
CFG_CRYPTO_WITH_CE=y.

Signed-off-by: Jerome Forissier <jerome.forissier@linaro.org>
Acked-by: Jens Wiklander <jens.wiklander@linaro.org>
Acked-by: Etienne Carriere <etienne.carriere@linaro.org>
[JW: update commit message]
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

core/lib/libtomcrypt/src/modes/ctr/ctr_encrypt.c

@@ -10,6 +10,26 @@
 
 #ifdef LTC_CTR_MODE
 
+static void s_ctr_increment_counter(symmetric_CTR *ctr)
+{
+	int x;
+
+	if (ctr->mode == CTR_COUNTER_LITTLE_ENDIAN) {
+		for (x = 0; x < ctr->ctrlen; x++) {
+			ctr->ctr[x] = (ctr->ctr[x] + 1) & 0xff;
+			if (ctr->ctr[x])
+				return;
+		}
+	} else {
+		for (x = ctr->blocklen - 1; x >= ctr->ctrlen; x--) {
+			ctr->ctr[x] = (ctr->ctr[x] + 1) & 0xff;
+			if (ctr->ctr[x]) {
+				return;
+			}
+		}
+	}
+}
+
 /**
   CTR encrypt software implementation
   @param pt     Plaintext
@@ -20,31 +40,12 @@
 */
 static int s_ctr_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CTR *ctr)
 {
-   int x, err;
+   int err;
 
    while (len) {
       /* is the pad empty? */
       if (ctr->padlen == ctr->blocklen) {
-         /* increment counter */
-         if (ctr->mode == CTR_COUNTER_LITTLE_ENDIAN) {
-            /* little-endian */
-            for (x = 0; x < ctr->ctrlen; x++) {
-               ctr->ctr[x] = (ctr->ctr[x] + (unsigned char)1) & (unsigned char)255;
-               if (ctr->ctr[x] != (unsigned char)0) {
-                  break;
-               }
-            }
-         } else {
-            /* big-endian */
-            for (x = ctr->blocklen-1; x >= ctr->ctrlen; x--) {
-               ctr->ctr[x] = (ctr->ctr[x] + (unsigned char)1) & (unsigned char)255;
-               if (ctr->ctr[x] != (unsigned char)0) {
-                  break;
-               }
-            }
-         }
-
-         /* encrypt it */
+         /* encrypt counter into pad */
          if ((err = cipher_descriptor[ctr->cipher]->ecb_encrypt(ctr->ctr, ctr->pad, &ctr->key)) != CRYPT_OK) {
             return err;
          }
@@ -65,6 +66,11 @@ static int s_ctr_encrypt(const unsigned char *pt, unsigned char *ct, unsigned lo
 #endif
       *ct++ = *pt++ ^ ctr->pad[ctr->padlen++];
       --len;
+
+      /* done with one full block? if so, set counter for next block. */
+      if (ctr->padlen == ctr->blocklen) {
+         s_ctr_increment_counter(ctr);
+      }
    }
    return CRYPT_OK;
 }
@@ -79,7 +85,8 @@ static int s_ctr_encrypt(const unsigned char *pt, unsigned char *ct, unsigned lo
 */
 int ctr_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CTR *ctr)
 {
-   int err, fr;
+   unsigned long incr;
+   int err;
 
    LTC_ARGCHK(pt != NULL);
    LTC_ARGCHK(ct != NULL);
@@ -101,25 +108,29 @@ int ctr_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, s
    }
 #endif
 
-   /* handle acceleration only if pad is empty, accelerator is present and length is >= a block size */
-   if ((cipher_descriptor[ctr->cipher]->accel_ctr_encrypt != NULL) && (len >= (unsigned long)ctr->blocklen)) {
-     if (ctr->padlen < ctr->blocklen) {
-       fr = ctr->blocklen - ctr->padlen;
-       if ((err = s_ctr_encrypt(pt, ct, fr, ctr)) != CRYPT_OK) {
-          return err;
-       }
-       pt += fr;
-       ct += fr;
-       len -= fr;
-     }
-
-     if (len >= (unsigned long)ctr->blocklen) {
+   if (cipher_descriptor[ctr->cipher]->accel_ctr_encrypt != NULL ) {
+     /* handle acceleration only if not in the middle of a block, accelerator is present and length is >= a block size */
+     if ((ctr->padlen == 0 || ctr->padlen == ctr->blocklen) && len >= (unsigned long)ctr->blocklen) {
        if ((err = cipher_descriptor[ctr->cipher]->accel_ctr_encrypt(pt, ct, len/ctr->blocklen, ctr->ctr, ctr->mode, &ctr->key)) != CRYPT_OK) {
-          return err;
+         return err;
        }
        pt += (len / ctr->blocklen) * ctr->blocklen;
        ct += (len / ctr->blocklen) * ctr->blocklen;
        len %= ctr->blocklen;
+       /* counter was changed by accelerator so mark pad empty (will need updating in s_ctr_encrypt()) */
+       ctr->padlen = ctr->blocklen;
+     }
+
+     /* try to re-synchronize on a block boundary for maximum use of acceleration */
+     incr = ctr->blocklen - ctr->padlen;
+     if (len >= incr + (unsigned long)ctr->blocklen) {
+       if ((err = s_ctr_encrypt(pt, ct, incr, ctr)) != CRYPT_OK) {
+         return err;
+       }
+       pt += incr;
+       ct += incr;
+       len -= incr;
+       return ctr_encrypt(pt, ct, len, ctr);
      }
    }