added decryption example

1 files changed, 46 insertions, 12 deletions

diff --git a/usb-crypto/usb-crypto.cpp b/usb-crypto/usb-crypto.cpp
index 777b939..0f7a9be 100644
--- a/usb-crypto/usb-crypto.cpp
+++ b/usb-crypto/usb-crypto.cpp
@@ -44,8 +44,11 @@ void print_hex_dump(const uint8_t* data, size_t len) {
   printf("\r\n");
 }
 
-void encrypt() {
+void run_crypto()
+{
+  printf("running encrypt/decrypt test using ChaCha20Poly1305:\r\n");
 
+  printf("\r\nencrypting...\r\n");
   uint8_t hdr[] = "hello world!";
   size_t hdr_len = sizeof(hdr)-1;
   uint8_t body[] = "this is a secret message.";
@@ -59,6 +62,12 @@ void encrypt() {
   uint8_t iv[] = {0x6f, 0xac, 0x1c, 0x6a, 0x94, 0xa5, 0x78, 0x87,
                   0x61, 0xcf, 0x9e, 0xcd};
 
+  uint8_t buf[256];
+  uint8_t tag[16];
+  memset(buf, 0, sizeof(buf));
+  memset(tag, 0, sizeof(tag));
+
+  led_on();
   cipher.clear();
   if(!cipher.setKey(key, sizeof(key))) {
     printf("failed to set key\r\n");
@@ -68,34 +77,59 @@ void encrypt() {
     printf("failed to set iv\r\n");
     return;
   }
+  led_off();
 
-  uint8_t buf[256];
-  uint8_t tag[16];
-  memset(buf, 0, sizeof(buf));
-  memset(tag, 0, sizeof(tag));
-
+  led_on();
   cipher.addAuthData(hdr, hdr_len);
   cipher.encrypt(buf, body, body_len);
   cipher.computeTag(tag, sizeof(tag));
+  led_off();
 
   printf("encrypted data (%d bytes):\r\n", body_len);
   print_hex_dump(buf, body_len);
   printf("\r\n");
   printf("auth tag (%d bytes):\r\n", sizeof(tag));
   print_hex_dump(tag, sizeof(tag));
+
+  printf("\r\ndecrypting...\r\n");
+  memcpy(body, buf, body_len);
+  memset(buf, 0, sizeof(buf));
+
+  led_on();
+  cipher.clear();
+  if(!cipher.setKey(key, sizeof(key))) {
+    printf("failed to set key\r\n");
+    return;
+  }
+  if(!cipher.setIV(iv, sizeof(iv))) {
+    printf("failed to set iv\r\n");
+    return;
+  }
+  led_off();
+
+  led_on();
+  cipher.addAuthData(hdr, hdr_len);
+  cipher.decrypt(buf, body, body_len);
+  bool ct = cipher.checkTag(tag, sizeof(tag));
+  led_off();
+
+  if(!ct) {
+    printf("auth tag mismatch!\r\n");
+    return;
+  } else {
+    printf("auth tag correct!\r\n");
+  }
+  printf("decrypted body: '%s'\r\n", buf);
 }
 
 void handle_cmd(uint8_t cmd)
 {
   switch(cmd) {
-  case 'e': encrypt(); return;
-  case '0': led_off(); break;
-  case '1': led_on(); break;
-  case 't': led_toggle(); break;
+  case 'c': run_crypto(); break;
   case 'r': reset2bootloader(); break;
-  default: printf("error\r\n"); return;
+  default: printf("unknown command"); break;
   }
-  printf("ok\r\n");
+  printf("\r\n");
 }
 
 int main(void)