From a51c9192b178c7ad389de1e8d63760cb2ceea02c Mon Sep 17 00:00:00 2001
From: merlokk <807634+merlokk@users.noreply.github.com>
Date: Thu, 4 Jul 2019 16:27:33 +0300
Subject: [PATCH] add apdu_decode

---
 fido2/apdu.c | 119 +++++++++++++++++++++++++++++++++++++++++++++++++++
 fido2/apdu.h |  26 +++++++++++
 2 files changed, 145 insertions(+)
 create mode 100644 fido2/apdu.c

diff --git a/fido2/apdu.c b/fido2/apdu.c
new file mode 100644
index 0000000..b2516ea
--- /dev/null
+++ b/fido2/apdu.c
@@ -0,0 +1,119 @@
+// Copyright 2019 SoloKeys Developers
+//
+// Licensed under the Apache License, Version 2.0, <LICENSE-APACHE or
+// http://apache.org/licenses/LICENSE-2.0> or the MIT license <LICENSE-MIT or
+// http://opensource.org/licenses/MIT>, at your option. This file may not be
+// copied, modified, or distributed except according to those terms.
+
+// iso7816:2013. 5.3.2 Decoding conventions for command bodies
+
+#include "apdu.h"
+
+int apdu_decode(uint8_t *data, size_t len, APDU_STRUCT *apdu) 
+{
+    EXT_APDU_HEADER *hapdu = (EXT_APDU_HEADER *)data;
+    
+    apdu->cla = hapdu->cla;
+    apdu->ins = hapdu->ins;
+    apdu->p1 = hapdu->p1;
+    apdu->p2 = hapdu->p2;
+    
+    apdu->lc = 0;
+    apdu->data = NULL;
+    apdu->le = 0;
+    apdu->extended_apdu = false;
+    apdu->case_type = 0x00;
+    
+    uint8_t b0 = hapdu->lc[0];
+    
+    // case 1
+    if (len == 4)
+    {
+        apdu->case_type = 0x01;
+    }
+    
+     // case 2S (Le)
+    if (len == 5 && b0 != 0)
+    {
+        apdu->case_type = 0x02;
+        apdu->le = b0;
+        if (!apdu->le)
+            apdu->le = 0x100;
+    }
+   
+    // case 3S (Lc + data)
+    if (len == 5U + b0 && b0 != 0)
+    {
+        apdu->case_type = 0x13;
+        apdu->lc = (hapdu->lc[1] << 8) + hapdu->lc[2];
+    }
+    
+    // case 4S (Lc + data + Le)
+    if (len == 5U + b0 + 1U && b0 != 0)
+    {
+        apdu->case_type = 0x04;
+        apdu->lc = b0;
+        apdu->le = data[len - 1];
+        if (!apdu->le)
+            apdu->le = 0x100;
+    }
+    
+    // extended length apdu
+    if (len >= 7 && b0 == 0)
+    {
+        uint16_t extlen = (hapdu->lc[1] << 8) + hapdu->lc[2];
+        
+         // case 2E (Le) - extended
+        if (len == 7)
+        {
+            apdu->case_type = 0x12;
+            apdu->extended_apdu = true;
+            apdu->le = extlen;
+            if (!apdu->le)
+                apdu->le = 0x10000;
+        }
+        
+       // case 3E (Lc + data) - extended
+       if (len == 7U + extlen)
+        {
+            apdu->case_type = 0x13;
+            apdu->lc = extlen;
+        }
+
+       // case 4E (Lc + data + Le) - extended 2-byte Le
+       if (len == 7U + extlen + 2U)
+        {
+            apdu->case_type = 0x14;
+            apdu->lc = extlen;
+            apdu->le = (data[len - 2] << 8) + data[len - 1];
+        if (!apdu->le)
+            apdu->le = 0x10000;
+        }
+
+       // case 4E (Lc + data + Le) - extended 3-byte Le
+       if (len == 7U + extlen + 3U && data[len - 3] == 0)
+        {
+            apdu->case_type = 0x24;
+            apdu->lc = extlen;
+            apdu->le = (data[len - 2] << 8) + data[len - 1];
+        if (!apdu->le)
+            apdu->le = 0x10000;
+        }
+    }
+    
+    if (!apdu->case_type)
+        return 1;
+    
+    if (apdu->lc)
+    {
+        if (apdu->extended_apdu)
+        {
+            apdu->data = data + 5;
+        } else {
+            apdu->data = data + 7;
+        }
+        
+    }   
+    
+    return 0;
+}
diff --git a/fido2/apdu.h b/fido2/apdu.h
index d9687c7..ff3940c 100644
--- a/fido2/apdu.h
+++ b/fido2/apdu.h
@@ -2,6 +2,8 @@
 #define _APDU_H_
 
 #include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
 
 typedef struct
 {
@@ -12,6 +14,30 @@ typedef struct
     uint8_t lc;
 } __attribute__((packed)) APDU_HEADER;
 
+typedef struct
+{
+    uint8_t cla;
+    uint8_t ins;
+    uint8_t p1;
+    uint8_t p2;
+    uint8_t lc[3];
+} __attribute__((packed)) EXT_APDU_HEADER;
+
+typedef struct
+{
+    uint8_t cla;
+    uint8_t ins;
+    uint8_t p1;
+    uint8_t p2;
+    uint16_t lc;
+    uint8_t *data;
+    uint32_t le;
+    bool extended_apdu;
+    uint8_t case_type;
+} __attribute__((packed)) APDU_STRUCT;
+
+extern int apdu_decode(uint8_t *data, size_t len, APDU_STRUCT *apdu);
+
 #define APDU_FIDO_U2F_REGISTER        0x01
 #define APDU_FIDO_U2F_AUTHENTICATE    0x02
 #define APDU_FIDO_U2F_VERSION         0x03