From e364b2a8be667fd3e484fe72e5e5117cecfecab4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sosth=C3=A8ne=20Gu=C3=A9don?= <sosthene@nitrokey.com>
Date: Thu, 15 Jun 2023 11:32:30 +0200
Subject: [PATCH] Import TLV parsing from opcard

---
 src/lib.rs |   1 +
 src/tlv.rs | 170 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 171 insertions(+)
 create mode 100644 src/tlv.rs

diff --git a/src/lib.rs b/src/lib.rs
index 6d96b4e..9267f79 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -20,6 +20,7 @@ pub mod response;
 pub use aid::{Aid, App};
 pub use command::{Command, Instruction};
 pub use response::{Response, Status};
+pub mod tlv;
 
 #[cfg(test)]
 mod tests {
diff --git a/src/tlv.rs b/src/tlv.rs
new file mode 100644
index 0000000..0e2a2f3
--- /dev/null
+++ b/src/tlv.rs
@@ -0,0 +1,170 @@
+//! BER-TLV writer and parser
+
+#[derive(Debug, PartialEq, Eq, PartialOrd, Ord, Clone, Copy)]
+pub struct Tag([u8; 3]);
+
+impl From<u8> for Tag {
+    fn from(value: u8) -> Self {
+        Tag([value, 0, 0])
+    }
+}
+
+impl From<u16> for Tag {
+    fn from(value: u16) -> Self {
+        value.to_be_bytes().into()
+    }
+}
+
+impl From<[u8; 1]> for Tag {
+    fn from([value]: [u8; 1]) -> Self {
+        value.into()
+    }
+}
+
+impl From<[u8; 2]> for Tag {
+    fn from([b1, b2]: [u8; 2]) -> Self {
+        if b1 == 0 {
+            Tag([b2, 0, 0])
+        } else {
+            Tag([b1, b2, 0])
+        }
+    }
+}
+
+impl From<[u8; 3]> for Tag {
+    fn from([b1, b2, b3]: [u8; 3]) -> Self {
+        if b1 == 0 {
+            [b2, b3].into()
+        } else {
+            Tag([b1, b2, 0])
+        }
+    }
+}
+
+impl Tag {
+    pub fn serialize(&self) -> heapless::Vec<u8, 3> {
+        let [b1, b2, b3] = self.0;
+        if b1 == 0 {
+            if b2 == 0 {
+                debug_assert_ne!(b3 & 0b11111, 0b11111, "Invalid encoding for 1 byte tag");
+                heapless::Vec::try_from([b3].as_slice()).unwrap()
+            } else {
+                debug_assert_eq!(
+                    b3 & 0b11111,
+                    0b11111,
+                    "Invalid encoding for first byte of tag"
+                );
+                debug_assert!(
+                    (0x1F..=0x7F).contains(&b3),
+                    "Invalid encoding for first byte of tag"
+                );
+                heapless::Vec::try_from([b2, b3].as_slice()).unwrap()
+            }
+        } else {
+            debug_assert_eq!(
+                b1 & 0b11111,
+                0b11111,
+                "Invalid encoding for first byte of tag"
+            );
+            debug_assert!(b2 > 0x80);
+            debug_assert!((0x00..0x7F).contains(&b3));
+            heapless::Vec::try_from([b1, b2, b3].as_slice()).unwrap()
+        }
+    }
+}
+
+pub fn get_do<'input>(tag_path: &[Tag], data: &'input [u8]) -> Option<&'input [u8]> {
+    let mut to_ret = data;
+    let mut remainder = data;
+    for tag in tag_path {
+        loop {
+            let (cur_tag, cur_value, cur_remainder) = take_do(remainder)?;
+            remainder = cur_remainder;
+            if *tag == cur_tag {
+                to_ret = cur_value;
+                remainder = cur_value;
+                break;
+            }
+        }
+    }
+    Some(to_ret)
+}
+
+/// Returns (tag, data, remainder)
+fn take_do(data: &[u8]) -> Option<(Tag, &[u8], &[u8])> {
+    let (tag, remainder) = take_tag(data)?;
+    let (len, remainder) = take_len(remainder)?;
+    if remainder.len() < len {
+        None
+    } else {
+        let (value, remainder) = remainder.split_at(len);
+        Some((tag, value, remainder))
+    }
+}
+
+// See
+// https://www.emvco.com/wp-content/uploads/2017/05/EMV_v4.3_Book_3_Application_Specification_20120607062110791.pdf
+// Annex B1
+pub fn take_tag(data: &[u8]) -> Option<(Tag, &[u8])> {
+    let b1 = *data.first()?;
+    if (b1 & 0x1f) == 0x1f {
+        let b2 = *data.get(1)?;
+        if (0x00..0x1E).contains(&b2) || b2 == 0x80 {
+            return None;
+        }
+
+        if (0x81..0xFF).contains(&b2) {
+            let b3 = *data.get(2)?;
+            if (0x81..0xFF).contains(&b3) {
+                return None;
+            }
+
+            Some((Tag([b1, b2, b3]), &data[3..]))
+        } else {
+            Some((Tag([b1, b2, 0]), &data[2..]))
+        }
+    } else {
+        Some((Tag([b1, 0, 0]), &data[1..]))
+    }
+}
+
+pub fn take_len(data: &[u8]) -> Option<(usize, &[u8])> {
+    let l1 = *data.first()?;
+    if l1 <= 0x7F {
+        Some((l1 as usize, &data[1..]))
+    } else if l1 == 0x81 {
+        Some((*data.get(1)? as usize, &data[2..]))
+    } else {
+        if l1 != 0x82 {
+            return None;
+        }
+        let l2 = *data.get(1)?;
+        let l3 = *data.get(2)?;
+        let len = u16::from_be_bytes([l2, l3]) as usize;
+        Some((len, &data[3..]))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use hex_literal::hex;
+
+    #[test]
+    fn dos() {
+        assert_eq!(
+            get_do(&[0x02u16].map(Tag::from), &hex!("02 02 1DB9 02 02 1DB9")),
+            Some(hex!("1DB9").as_slice())
+        );
+        assert_eq!(
+            get_do(&[0xA6u16, 0x7F49, 0x86].map(Tag::from), &hex!("A6 26 7F49 23 86 21 04 2525252525252525252525252525252525252525252525252525252525252525")),
+            Some(hex!("04 2525252525252525252525252525252525252525252525252525252525252525").as_slice())
+        );
+
+        // Multiple nested
+        assert_eq!(
+            get_do(&[0xA6u16, 0x7F49, 0x86].map(Tag::from), &hex!("A6 2A 02 02 DEAD 7F49 23 86 21 04 2525252525252525252525252525252525252525252525252525252525252525")),
+            Some(hex!("04 2525252525252525252525252525252525252525252525252525252525252525").as_slice())
+        );
+    }
+}