1 files changed, 33 insertions, 0 deletions
diff --git a/sys/crypto/chacha20/chacha.c b/sys/crypto/chacha20/chacha.c
index c882078c0bbe..c0458dcd5b22 100644
--- a/sys/crypto/chacha20/chacha.c
+++ b/sys/crypto/chacha20/chacha.c
@@ -84,12 +84,32 @@ chacha_keysetup(chacha_ctx *x,const u8 *k,u32 kbits)
 LOCAL void
 chacha_ivsetup(chacha_ctx *x, const u8 *iv, const u8 *counter)
 {
+#ifndef CHACHA_NONCE0_CTR128
   x->input[12] = counter == NULL ? 0 : U8TO32_LITTLE(counter + 0);
   x->input[13] = counter == NULL ? 0 : U8TO32_LITTLE(counter + 4);
   x->input[14] = U8TO32_LITTLE(iv + 0);
   x->input[15] = U8TO32_LITTLE(iv + 4);
+#else
+  // CHACHA_STATELEN
+  (void)iv;
+  x->input[12] = U8TO32_LITTLE(counter + 0);
+  x->input[13] = U8TO32_LITTLE(counter + 4);
+  x->input[14] = U8TO32_LITTLE(counter + 8);
+  x->input[15] = U8TO32_LITTLE(counter + 12);
+#endif
 }
 
+#ifdef CHACHA_NONCE0_CTR128
+LOCAL void
+chacha_ctrsave(const chacha_ctx *x, u8 *counter)
+{
+    U32TO8_LITTLE(counter + 0, x->input[12]);
+    U32TO8_LITTLE(counter + 4, x->input[13]);
+    U32TO8_LITTLE(counter + 8, x->input[14]);
+    U32TO8_LITTLE(counter + 12, x->input[15]);
+}
+#endif
+
 LOCAL void
 chacha_encrypt_bytes(chacha_ctx *x,const u8 *m,u8 *c,u32 bytes)
 {
@@ -192,7 +212,16 @@ chacha_encrypt_bytes(chacha_ctx *x,const u8 *m,u8 *c,u32 bytes)
     j12 = PLUSONE(j12);
     if (!j12) {
       j13 = PLUSONE(j13);
+#ifndef CHACHA_NONCE0_CTR128
       /* stopping at 2^70 bytes per nonce is user's responsibility */
+#else
+      if (!j13) {
+        j14 = PLUSONE(j14);
+        if (!j14) {
+          j15 = PLUSONE(j15);
+        }
+      }
+#endif
     }
 
     U32TO8_LITTLE(c + 0,x0);
@@ -218,6 +247,10 @@ chacha_encrypt_bytes(chacha_ctx *x,const u8 *m,u8 *c,u32 bytes)
       }
       x->input[12] = j12;
       x->input[13] = j13;
+#ifdef CHACHA_NONCE0_CTR128
+      x->input[14] = j14;
+      x->input[15] = j15;
+#endif
       return;
     }
     bytes -= 64;