Stolen APDU length parsing

src/constants.rs

@@ -51,7 +51,6 @@ pub const MAX_IFSD: [u8; 4] = [0xfe, 0x00, 0x00, 0x00];
 // dwMaxCCIDMsgLen 3072 (gnuk: 271)
 // pub const MAX_MSG_LENGTH_TYPE: consts::U3072;
 pub type MAX_MSG_LENGTH_TYPE = <consts::U2048 as core::ops::Add<consts::U1024>>::Output;
-pub type MessageBuffer = Bytes<MAX_MSG_LENGTH_TYPE>;
 pub const MAX_MSG_LENGTH: usize = MAX_MSG_LENGTH_TYPE::USIZE;
 pub const MAX_MSG_LENGTH_LE: [u8; 4] = [0x00, 0x0C, 0x00, 0x00];
 pub const NUM_SLOTS: u8 = 1;

src/pipe.rs

@@ -168,6 +168,10 @@ where
 
     fn fake_poll_app(&mut self) {
         if let State::Processing = self.state {
+
+            let apdu = Apdu::try_from(self.message.as_ref()).unwrap();
+            hprintln!(":: {:?}", &apdu).ok();
+
             // we should have an open XfrBlock allowance
             self.state = State::ReadyToSend;
             // fake some data
@@ -244,11 +248,11 @@ where
             let needs_zlp = packet.len() == PACKET_SIZE;
             match self.write.write(packet) {
                 Ok(n) if n == packet.len() => {
-                    if packet.len() > 8 {
-                        hprintln!("--> sent {:?}... successfully", &packet[..8]).ok();
-                    } else {
-                        hprintln!("--> sent {:?} successfully", packet).ok();
-                    }
+                    // if packet.len() > 8 {
+                    //     hprintln!("--> sent {:?}... successfully", &packet[..8]).ok();
+                    // } else {
+                    //     hprintln!("--> sent {:?} successfully", packet).ok();
+                    // }
 
                     if needs_zlp {
                         hprintln!("sending ZLP").ok();

src/types.rs

@@ -4,6 +4,14 @@ use cortex_m_semihosting::hprintln;
 
 use crate::constants::*;
 
+use heapless_bytes::{
+    Bytes,
+    consts,
+};
+
+pub type MessageBuffer = Bytes<MAX_MSG_LENGTH_TYPE>;
+pub mod apdu;
+pub use apdu::*;
 
 pub type RawPacket = heapless_bytes::Bytes<PACKET_SIZE_TYPE>;
 
@@ -39,7 +47,7 @@ pub trait PacketWithData: Packet {
         let declared_len =
             u32::from_le_bytes(self[1..5].try_into().unwrap()) as usize;
         let len = core::cmp::min(PACKET_SIZE - 10, declared_len);
-        hprintln!("delcared = {}, len = {}", declared_len, len).ok();
+        // hprintln!("delcared = {}, len = {}", declared_len, len).ok();
         &self[10..][..len]
     }
 }

src/types/apdu.rs

@@ -0,0 +1,193 @@
+use super::*;
+
+pub struct Apdu<'a> {
+    lc: usize,
+    le: usize,
+    offset: usize,
+    apdu: &'a [u8]
+}
+
+impl<'a> core::ops::Deref for Apdu<'a> {
+    type Target = &'a [u8];
+
+    #[inline]
+    fn deref(&self) -> &Self::Target {
+        &self.apdu
+    }
+}
+
+impl<'a> core::convert::TryFrom<&'a [u8]> for Apdu<'a> {
+    type Error = ();
+    fn try_from(apdu: &'a [u8]) -> core::result::Result<Self, Self::Error> {
+        let (lc, le, offset) = calculate_lengths(apdu)?;
+        Ok(Self { lc, le, offset, apdu })
+    }
+}
+
+impl Apdu<'_> {
+    #[inline]
+    /// The "class" byte of the APDU
+    pub fn cla(&self) -> u8 {
+        *&self[0]
+
+    }
+
+    #[inline]
+    /// The "instruction" byte of the APDU
+    pub fn ins(&self) -> u8 {
+        *&self[1]
+    }
+
+    #[inline]
+    /// The first "parameter" byte of the APDU
+    pub fn p1(&self) -> u8 {
+        *&self[2]
+    }
+
+    #[inline]
+    /// The second "parameter" byte of the APDU
+    pub fn p2(&self) -> u8 {
+        *&self[3]
+    }
+
+    #[inline]
+    /// The length of the APDU's command data bytes
+    pub fn lc(&self) -> usize {
+        self.lc
+    }
+
+    #[inline]
+    /// The maximum expected length of the response
+    pub fn le(&self) -> usize {
+        self.le
+    }
+
+    #[inline]
+    pub fn data(&self) -> &[u8] {
+        &self.apdu[4 + self.offset..][..self.lc]
+    }
+
+}
+
+impl core::fmt::Debug for Apdu<'_> {
+
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+
+        let mut debug_struct = f.debug_struct("Apdu");
+
+        let mut debug_struct = debug_struct
+            .field("cla", &format_args!("0x{:x}", &self.cla()))
+            .field("ins", &format_args!("0x{:x}", &self.ins()))
+            .field("p1", &format_args!("0x{:x}", &self.p1()))
+            .field("p2", &format_args!("0x{:x}", &self.p2()))
+            .field("lc", &self.lc())
+            .field("le", &self.le())
+        ;
+
+        if self.lc() > 0 {
+            let l = core::cmp::min(self.lc(), 8);
+            debug_struct = if l < 8 {
+                debug_struct
+                    .field("data[..8]", &(&self.data()[..8]))
+            } else {
+                debug_struct
+                    .field("data", &self.data())
+            }
+        };
+
+        debug_struct.finish()
+    }
+}
+
+// http://www.ttfn.net/techno/smartcards/iso7816_4.html#table5
+#[inline]
+fn calculate_lengths(apdu: &[u8]) -> Result<(usize, usize, usize), ()> {
+    hprintln!("parsing {:?}", apdu).ok();
+    // b = body
+    let b = &apdu[4..];
+    let l = b.len();
+    let mut le: usize;
+    let mut lc: usize;
+
+    let mut offset: usize = 0;
+
+    // Case 1
+    if l == 0{
+        lc = 0;
+        le = 0;
+        return Ok((lc, le, offset));
+    }
+
+    // the reference use indexing-from-1
+    let b1 = b[0] as usize;
+
+    // Case 2S
+    if l == 1 {
+        lc = 0;
+        le = if b1 == 0 {
+            256
+        } else {
+            b1 as _
+        };
+        return Ok((lc, le, offset));
+    }
+
+    // Case 3S
+    if l == 1 + b1 && b1 != 0 {
+        // B1 encodes Lc valued from 1 to 255
+        lc = b1;
+        le = 0;
+        return Ok((lc, le, 1));
+    }
+
+    // Case 4S
+    if l == 2 + b1 && b1 != 0 {
+        // B1 encodes Lc valued from 1 to 255
+        // Bl encodes Le from 1 to 256
+        lc = b1;
+        le = b[l - 1] as usize;
+        return Ok((lc, le, 1));
+    }
+
+    // only extended cases left now
+    if b1 != 0 {
+        return Err(())
+    };
+
+    // Case 2E (no data)
+    if l == 3 && b1 == 0 {
+        lc = 0;
+        if b[1] == 0 && b[2] == 0 {
+            le = 65_536;
+        } else {
+            le = u16::from_be_bytes([b[1], b[2]]) as usize;
+        }
+        return Ok((lc, le, 0));
+    }
+
+    lc = u16::from_be_bytes([b[1], b[2]]) as usize;
+
+    // Case 3E
+    if l == 3 + lc {
+        le = 0;
+        return Ok((lc, le, 3));
+    }
+
+    // Case 4E
+    if l == 5 + lc {
+        let pre_le = u16::from_be_bytes([b[l - 2], b[l - 1]]) as usize;
+        if pre_le == 0 {
+            le = 65_6536;
+        } else {
+            le = pre_le;
+        }
+        return Ok((lc, le, 3));
+    }
+
+    Err(())
+}
+
+
+// // pub trait Apdu: core::ops::Deref<Target = RawPacket> {
+// pub trait Apdu<'a>: core::ops::Deref<Target = &'a [u8]> {
+// }