diff --git a/library/cipher.c b/library/cipher.c
index 6853fb8..5470dcf 100644
--- a/library/cipher.c
+++ b/library/cipher.c
@@ -879,7 +879,8 @@
     for (i = input_len; i > 0; i--) {
         prev_done = done;
         done |= (input[i-1] != 0);
-        *data_len |= i * (done != prev_done);
+        size_t mask = mbedtls_ct_size_mask(done ^ prev_done);
+        *data_len |= i & mask;
     }
 
     return 0;