From 3c903beeae727156bfadde5cd62f3b09d9486fab Mon Sep 17 00:00:00 2001 From: Nicolas Stalder Date: Sun, 3 May 2020 15:52:33 +0200 Subject: [PATCH] Stolen APDU length parsing --- src/constants.rs | 1 - src/pipe.rs | 14 ++-- src/types.rs | 10 ++- src/types/apdu.rs | 193 ++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 211 insertions(+), 7 deletions(-) create mode 100644 src/types/apdu.rs diff --git a/src/constants.rs b/src/constants.rs index d35dbfc..46ccc67 100644 --- a/src/constants.rs +++ b/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 = >::Output; -pub type MessageBuffer = Bytes; 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; diff --git a/src/pipe.rs b/src/pipe.rs index 15eabb2..0a1b175 100644 --- a/src/pipe.rs +++ b/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(); diff --git a/src/types.rs b/src/types.rs index 1151181..efee4dc 100644 --- a/src/types.rs +++ b/src/types.rs @@ -4,6 +4,14 @@ use cortex_m_semihosting::hprintln; use crate::constants::*; +use heapless_bytes::{ + Bytes, + consts, +}; + +pub type MessageBuffer = Bytes; +pub mod apdu; +pub use apdu::*; pub type RawPacket = heapless_bytes::Bytes; @@ -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] } } diff --git a/src/types/apdu.rs b/src/types/apdu.rs new file mode 100644 index 0000000..6301096 --- /dev/null +++ b/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 { + 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 { +// pub trait Apdu<'a>: core::ops::Deref { +// }