se05x/
t1.rs

1// Copyright (C) 2023 Nitrokey GmbH
2// SPDX-License-Identifier: LGPL-3.0-only
3
4use hex_literal::hex;
5use iso7816::command::writer::IntoWriter;
6use iso7816::command::Writer;
7
8pub type Crc = crc16::State<crc16::X_25>;
9
10use core::fmt::{self, Debug};
11use core::ops::Not;
12
13use crate::embedded_hal::{
14    i2c::{Read, Write, WriteRead},
15    Delay,
16};
17use crate::macros::enum_u8;
18
19mod i2cimpl;
20
21#[derive(Debug, Clone, Copy, PartialEq, Eq)]
22pub struct Atr<'a> {
23    /// Protocol version only `01` is supported
24    pub pver: u8,
25    /// VendorID,
26    pub vid: &'a [u8; 5],
27    /// Block waiting time
28    pub bwt: u16,
29    /// Maximum Information Field Size of the SE
30    pub ifsc: u16,
31    /// Maximum I2C clock frequency (kHz)
32    pub plid: u8,
33    /// Maximal I2C Clock Frequency
34    pub mcf: u16,
35    pub config: u8,
36    /// Minimum polling time (ms)
37    pub mpot: u8,
38    /// Secure element guard time (microseconds)
39    pub segt: u16,
40    /// Wake-up time (microseconds)
41    pub wut: u16,
42    pub historical_bytes: &'a [u8],
43}
44
45impl Default for Atr<'_> {
46    fn default() -> Self {
47        Self {
48            pver: 1,
49            vid: &hex!("FFFFFFFFFF"),
50            bwt: 0,
51            ifsc: MAX_FRAME_DATA_LEN as _,
52            plid: 0,
53            mcf: 0,
54            config: 0,
55            mpot: 1,
56            segt: SEGT_US as _,
57            wut: 0,
58            historical_bytes: &[],
59        }
60    }
61}
62
63impl<'a> Atr<'a> {
64    /// If fails to parse, returns default values
65    pub fn parse(data: &'a [u8]) -> Result<Self, Error> {
66        // let atr = hex!("00a0000003960403e800fe020b03e80801000000006400000a4a434f5034204154504f");
67        debug!("Parsing atr: {data:02x?}");
68        if data.len() < 7 {
69            error!("ATR Error 1");
70            return Err(Error::Line(line!()));
71        }
72        let pver = data[0];
73        let vid: &[u8; 5] = (&data[1..][..5]).try_into().unwrap();
74        let dllp_len = data[6];
75
76        let rem = &data[7..];
77
78        if rem.len() < dllp_len as usize || dllp_len < 2 {
79            error!("ATR Error 2");
80            return Err(Error::Line(line!()));
81        }
82        let (dllp, rem) = rem.split_at(dllp_len as usize);
83
84        let [bwt1, bwt2, ifsc1, ifsc2, ..] = dllp else {
85            error!("ATR Error 3");
86            return Err(Error::Line(line!()));
87        };
88        let bwt = u16::from_be_bytes([*bwt1, *bwt2]);
89        let ifsc = u16::from_be_bytes([*ifsc1, *ifsc2]);
90
91        if rem.len() < 2 {
92            error!("ATR Error 4");
93            return Err(Error::Line(line!()));
94        }
95
96        let plid = rem[0];
97        let plp_len = rem[1];
98        let rem = &rem[2..];
99        if rem.len() < plp_len as usize {
100            error!("ATR Error 6");
101            return Err(Error::Line(line!()));
102        }
103        let (plp, rem) = rem.split_at(plp_len as usize);
104        let [mcf1, mcf2, config, mpot, _rfu1, _rfu2, _rfu3, segt1, segt2, wut1, wut2, ..] = plp
105        else {
106            error!("ATR Error 7");
107            return Err(Error::Line(line!()));
108        };
109        let mcf = u16::from_be_bytes([*mcf1, *mcf2]);
110        let segt = u16::from_be_bytes([*segt1, *segt2]);
111        let wut = u16::from_be_bytes([*wut1, *wut2]);
112
113        if rem.is_empty() {
114            error!("ATR Error 8");
115            return Err(Error::Line(line!()));
116        }
117        let hb_len = rem[0];
118        let rem = &rem[1..];
119        if rem.len() < hb_len as usize {
120            error!("ATR Error 9");
121            return Err(Error::Line(line!()));
122        }
123
124        let historical_bytes = &rem[..hb_len as usize];
125
126        Ok(Self {
127            pver,
128            vid,
129            bwt,
130            ifsc,
131            plid,
132            mcf,
133            config: *config,
134            mpot: *mpot,
135            segt,
136            wut,
137            historical_bytes,
138        })
139    }
140}
141
142#[derive(PartialEq, Eq, Clone, Copy, Debug)]
143pub struct Seq(bool);
144
145impl Seq {
146    pub const ZERO: Self = Seq(false);
147    pub const ONE: Self = Seq(true);
148}
149
150impl Not for Seq {
151    type Output = Self;
152    fn not(self) -> Self::Output {
153        Seq(!self.0)
154    }
155}
156
157#[derive(PartialEq, Eq, Clone, Copy, Debug)]
158pub enum Pcb {
159    I(Seq, bool),        // seq, multi
160    S(SBlock),           // code, response?
161    R(Seq, RBlockError), // seq, err
162}
163
164enum_u8!(
165    #[rustfmt::skip]
166    #[derive(PartialEq, Eq, Clone, Copy, Debug)]
167    pub enum SBlock {
168        ResyncRequest =              0b11000000,
169        ResyncResponse =             0b11100000,
170        IfsRequest =                 0b11000001,
171        IfsResponse =                0b11100001,
172        AbortRequest =               0b11000010,
173        AbortResponse =              0b11100010,
174        WtxRequest =                 0b11000011,
175        WtxResponse =                0b11100011,
176        InterfaceSoftResetRequest =  0b11001111,
177        InterfaceSoftResetResponse = 0b11101111,
178        EndOfApduSessionRequest =    0b11000101,
179        EndOfApduSessionResponse =   0b11100101,
180        SeChipResetRequest =         0b11000110,
181        SeChipResetResponse =        0b11100110,
182        GetAtrRequest =              0b11000111,
183        GetAtrResponse =             0b11100111,
184    }
185);
186
187enum_u8!(
188    #[derive(PartialEq, Eq, Clone, Copy, Debug)]
189    pub enum RBlockError {
190        #![mask(0b11)]
191        NoError = 0b00,
192        CrcError = 0b01,
193        OtherError = 0b10,
194    }
195);
196
197/// PCB Mask
198const I_BLOCK_PCB_MASK: u8 = 0b10011111;
199/// PCB template
200const I_BLOCK_PCB: u8 = 0b00000000;
201/// SEQ mask
202const I_BLOCK_SEQ: u8 = 0b01000000;
203/// MORE mask
204const I_BLOCK_MOR: u8 = 0b00100000;
205
206/// PCB template
207const R_BLOCK_PCB: u8 = 0b10000000;
208/// PCB template
209const R_BLOCK_PCB_MASK: u8 = 0b11101100;
210/// SEQ mask
211const R_BLOCK_SEQ: u8 = 0b00010000;
212/// CRC mask
213const R_BLOCK_ERROR_MASK: u8 = 0b00000011;
214
215impl Pcb {
216    pub fn to_byte(self) -> u8 {
217        match self {
218            Self::I(seq, multi) => Self::i_pcb(seq, multi),
219            Self::S(block) => Self::s_pcb(block),
220            Self::R(seq, err) => Self::r_pcb(seq, err),
221        }
222    }
223
224    pub fn i_pcb(seq: Seq, multi: bool) -> u8 {
225        let mut pcb = I_BLOCK_PCB;
226        if multi {
227            pcb |= I_BLOCK_MOR;
228        }
229
230        if seq == Seq::ONE {
231            pcb |= I_BLOCK_SEQ;
232        }
233        pcb
234    }
235    pub fn s_pcb(block: SBlock) -> u8 {
236        block.into()
237    }
238    pub fn r_pcb(seq: Seq, error: RBlockError) -> u8 {
239        let mut pcb = R_BLOCK_PCB;
240        if seq == Seq::ONE {
241            pcb |= R_BLOCK_SEQ;
242        }
243
244        pcb |= error as u8;
245        pcb
246    }
247
248    pub fn parse(value: u8) -> Result<Self, Error> {
249        if value & I_BLOCK_PCB_MASK == I_BLOCK_PCB {
250            let seq = if value & I_BLOCK_SEQ == 0 {
251                Seq::ZERO
252            } else {
253                Seq::ONE
254            };
255
256            let more = (value & I_BLOCK_MOR) != 0;
257            return Ok(Self::I(seq, more));
258        }
259
260        if value & R_BLOCK_PCB_MASK == R_BLOCK_PCB {
261            let seq = if value & R_BLOCK_SEQ == 0 {
262                Seq::ZERO
263            } else {
264                Seq::ONE
265            };
266            let error = (value & R_BLOCK_ERROR_MASK)
267                .try_into()
268                .map_err(|_| Error::BadPcb)?;
269            return Ok(Self::R(seq, error));
270        }
271
272        if let Ok(sblock) = value.try_into() {
273            return Ok(Self::S(sblock));
274        }
275
276        Err(Error::BadPcb)
277    }
278}
279
280pub trait I2CErrorNack: Debug {
281    fn is_address_nack(&self) -> bool;
282    fn is_data_nack(&self) -> bool;
283}
284pub trait I2CForT1:
285    Read<u8, Error = <Self as I2CForT1>::Error>
286    + Write<u8, Error = <Self as I2CForT1>::Error>
287    + WriteRead<u8, Error = <Self as I2CForT1>::Error>
288{
289    type Error: I2CErrorNack;
290}
291
292impl<T> I2CForT1 for T
293where
294    T: Read<u8>
295        + Write<u8, Error = <T as Read<u8>>::Error>
296        + WriteRead<u8, Error = <T as Read<u8>>::Error>,
297    <T as Read<u8>>::Error: I2CErrorNack,
298{
299    type Error = <T as Read<u8>>::Error;
300}
301
302#[derive(Debug, Clone, Copy, PartialEq, Eq)]
303pub enum Error {
304    Unknown,
305    AddressNack,
306    DataNack,
307    BadCrc,
308    BadPcb,
309    BadAddress,
310    ReceptionBuffer,
311    Line(u32),
312    Timeout,
313}
314
315impl fmt::Display for Error {
316    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
317        match self {
318            Self::Unknown => f.write_str("Unknown T=1 error"),
319            Self::AddressNack => f.write_str("NACK on from the device address"),
320            Self::DataNack => f.write_str("Nack for data write"),
321            Self::BadCrc => f.write_str("CRC error"),
322            Self::BadPcb => f.write_str("Received invalid PCB"),
323            Self::BadAddress => f.write_str("Bad address"),
324            Self::ReceptionBuffer => f.write_str("Reception buffer is too small"),
325            Self::Timeout => f.write_str("Read timed out"),
326            Self::Line(l) => write!(f, "Error comming from line: {l}"),
327        }
328    }
329}
330
331impl iso7816::command::writer::Error for Error {
332    fn failed_serialization(_cause: &'static str) -> Self {
333        error!("Failed serializaiton: {}", _cause);
334        Self::Line(line!())
335    }
336}
337
338pub struct T1oI2C<Twi, D> {
339    twi: Twi,
340    se_address: u8,
341    nad_hd2se: u8,
342    nad_se2hd: u8,
343    iseq_snd: Seq,
344    iseq_rcv: Seq,
345    /// Waiting time between attempts to read
346    ///
347    /// Microseconds
348    mpot: u32,
349    /// Retry count for attempts to write data to the se
350    pub retry_count: u32,
351    delay: D,
352    segt: u32,
353    /// Block waiting time
354    /// Maximum time the se05x can take to respond
355    ///
356    /// Microseconds
357    bwt: u32,
358}
359
360// const TWI_RETRIES: usize = 128;
361// const TWI_RETRY_DELAY_MS: u32 = 2;
362// const TWI_RETRY_DELAY_US: u32 = TWI_RETRY_DELAY_MS * 1000;
363/// SEGT value in microseconds
364/// Minimun time between reading attempts
365const SEGT_US: u32 = 10;
366const BWT_US: u32 = 100_000;
367
368/// See table 4 of UM1225
369const NAD_HD_TO_SE: u8 = 0x5A;
370/// See table 4 of UM1225
371const NAD_SE_TO_HD: u8 = 0xA5;
372
373#[derive(Debug, Clone, Copy, PartialEq, Eq)]
374pub enum DataReceived {
375    /// Received one or more IBlocks
376    ///
377    /// Returns the size of data written to the reception buffer
378    IBlocks(usize),
379    SBlock {
380        block: SBlock,
381        /// Any data written prior to receiving the s block
382        i_data: usize,
383        s_data: usize,
384    },
385}
386
387const DEFAULT_RETRY_COUNT: u32 = 1024;
388
389#[cfg(feature = "embedded-hal-v0.2.7")]
390impl<M, N, E> T1oI2C<crate::embedded_hal::Hal027<M>, crate::embedded_hal::Hal027<N>>
391where
392    N: embedded_hal_v0_2_7::blocking::delay::DelayUs<u32>,
393    M: embedded_hal_v0_2_7::blocking::i2c::Write<Error = E>
394        + embedded_hal_v0_2_7::blocking::i2c::Read<Error = E>
395        + embedded_hal_v0_2_7::blocking::i2c::WriteRead<Error = E>,
396    E: I2CErrorNack,
397{
398    pub fn new_hal_027(twi: M, se_address: u8, delay: N) -> Self {
399        Self::new(
400            crate::embedded_hal::Hal027(twi),
401            se_address,
402            crate::embedded_hal::Hal027(delay),
403        )
404    }
405}
406
407#[cfg(feature = "embedded-hal-v1.0")]
408impl<M, N, E> T1oI2C<crate::embedded_hal::Hal10<M>, crate::embedded_hal::Hal10<N>>
409where
410    N: embedded_hal_v1_0::delay::DelayNs,
411    M: embedded_hal_v1_0::i2c::I2c<Error = E>,
412    E: I2CErrorNack,
413{
414    pub fn new_hal_10(twi: M, se_address: u8, delay: N) -> Self {
415        Self::new(
416            crate::embedded_hal::Hal10(twi),
417            se_address,
418            crate::embedded_hal::Hal10(delay),
419        )
420    }
421}
422
423impl<Twi: I2CForT1, D: Delay> T1oI2C<Twi, D> {
424    pub fn new(twi: Twi, se_address: u8, delay: D) -> Self {
425        // Default MPOT value.
426        // TODO: get from ATR
427        const DMPOT_MS: u32 = 1;
428        Self {
429            twi,
430            se_address,
431            // See table 4
432            nad_hd2se: NAD_HD_TO_SE,
433            nad_se2hd: NAD_SE_TO_HD,
434            iseq_snd: Seq::ZERO,
435            iseq_rcv: Seq::ZERO,
436            mpot: DMPOT_MS * 1000,
437            segt: SEGT_US as _,
438            retry_count: DEFAULT_RETRY_COUNT,
439            bwt: BWT_US,
440            delay,
441        }
442    }
443
444    pub fn write(&mut self, data: &[u8]) -> Result<(), Error> {
445        trace!("Writing");
446        match self.twi.write(self.se_address, data) {
447            Ok(_) => Ok(()),
448            Err(err) if err.is_address_nack() => Err(Error::AddressNack),
449            Err(err) if err.is_data_nack() => Err(Error::DataNack),
450            Err(_err) => {
451                warn!("Got error: {:?}", _err);
452                Err(Error::Line(line!()))
453            }
454        }
455    }
456
457    pub fn read(&mut self, buffer: &mut [u8]) -> Result<(), Error> {
458        match self.twi.read(self.se_address, buffer) {
459            Ok(_) => Ok(()),
460            Err(err) if err.is_address_nack() => Err(Error::AddressNack),
461            Err(err) if err.is_data_nack() => Err(Error::DataNack),
462            Err(_err) => {
463                warn!("Got error: {:?}", _err);
464                Err(Error::Line(line!()))
465            }
466        }
467    }
468
469    // Not actually used as discouraged by 3.1.1.1
470    pub fn write_read(&mut self, data: &[u8], buffer: &mut [u8]) -> Result<(), Error> {
471        match self.twi.write_read(self.se_address, data, buffer) {
472            Ok(_) => Ok(()),
473            Err(err) if err.is_address_nack() => Err(Error::AddressNack),
474            Err(err) if err.is_data_nack() => Err(Error::DataNack),
475            Err(_err) => {
476                warn!("Unknown error when writing & reading: {:?}", _err);
477                Err(Error::Line(line!()))
478            }
479        }
480    }
481
482    pub fn receive_data(&mut self, buffer: &mut [u8]) -> Result<DataReceived, Error> {
483        let mut written = 0;
484        let mut retry_count = self.bwt / self.mpot + 1;
485        let mut i = 0;
486        loop {
487            let mut header_buffer = [0; HEADER_LEN];
488            let mut crc_buf = [0; TRAILER_LEN];
489            i += 1;
490            if i == retry_count {
491                break;
492            }
493
494            let read = self.read(&mut header_buffer);
495            match read {
496                Ok(()) => {}
497                Err(Error::AddressNack) => {
498                    self.wait_mpot();
499                    continue;
500                }
501                Err(err) => {
502                    return Err(err);
503                }
504            }
505
506            let [nad, pcb, len] = header_buffer;
507            debug!("Received header: {:02x?}", header_buffer);
508
509            if buffer.len() < written + len as usize {
510                error!("Buffer too small");
511                return Err(Error::ReceptionBuffer);
512            }
513
514            if len as usize > MAX_FRAME_DATA_LEN {
515                error!("Frame too large");
516                return Err(Error::ReceptionBuffer);
517            }
518
519            let mut data_buf = [0; MAX_FRAME_DATA_LEN];
520            let current_buf = &mut buffer[written..][..len as usize];
521            let data_buf = &mut data_buf[..len as _];
522
523            if nad != self.nad_se2hd {
524                error!("Received bad nad: {:02x}", nad);
525                return Err(Error::BadAddress);
526            }
527
528            if len != 0 {
529                self.read(data_buf)?;
530            }
531            self.read(&mut crc_buf)?;
532
533            let pcb = Pcb::parse(pcb).map_err(|_| Error::BadPcb)?;
534
535            let mut crc = Crc::new();
536            crc.update(&header_buffer);
537            crc.update(data_buf);
538            let crc = crc.get().to_le_bytes();
539            if crc_buf != crc {
540                error!("Got bad crc: {:02x?} expected {:02x?}", &data_buf[..2], crc);
541                // TODO: write R-Block with error
542                return Err(Error::BadCrc);
543            }
544
545            let (seq, more) = match pcb {
546                Pcb::S(SBlock::WtxRequest) => {
547                    if len != 1 {
548                        return Err(Error::Line(line!()));
549                    }
550                    let mult = data_buf[0];
551                    debug!("Got WtxRequest, {mult}");
552                    let frame = [
553                        self.nad_hd2se,
554                        Pcb::S(SBlock::WtxResponse).to_byte(),
555                        1,
556                        mult,
557                    ];
558                    let [crc1, crc2] = Crc::calculate(&frame).to_le_bytes();
559                    self.write(&[frame[0], frame[1], frame[2], frame[3], crc1, crc2])?;
560
561                    retry_count = (self.bwt * mult as u32) / self.mpot + 1;
562                    i = 0;
563                    self.delay.delay_us(100_000);
564                    continue;
565                }
566                Pcb::S(block) => {
567                    current_buf.copy_from_slice(data_buf);
568                    return Ok(DataReceived::SBlock {
569                        block,
570                        i_data: written,
571                        s_data: len as usize,
572                    });
573                }
574                Pcb::R(_, _) => {
575                    error!("Got unexpected R-Block in receive");
576                    return Err(Error::Line(line!()));
577                }
578                Pcb::I(seq, more) => (seq, more),
579            };
580            current_buf.copy_from_slice(data_buf);
581            written += len as usize;
582
583            if seq != self.iseq_rcv {
584                warn!("Got bad seq");
585            }
586            self.iseq_rcv = !seq;
587
588            if !more {
589                return Ok(DataReceived::IBlocks(written));
590            }
591            let frame = [
592                self.nad_hd2se,
593                Pcb::R(!seq, RBlockError::NoError).to_byte(),
594                0,
595            ];
596            let [crc1, crc2] = Crc::calculate(&frame).to_le_bytes();
597            self.write(&[frame[0], frame[1], frame[2], crc1, crc2])?;
598        }
599        error!("Waited for btw");
600        Err(Error::Timeout)
601    }
602
603    pub fn resync(&mut self) -> Result<(), Error> {
604        trace!("Resync");
605        let header = [self.nad_hd2se, Pcb::S(SBlock::ResyncRequest).to_byte(), 0];
606        let [crc1, crc2] = Crc::calculate(&header).to_le_bytes();
607        let frame = [header[0], header[1], header[2], crc1, crc2];
608        debug!("Sending: {frame:02x?}");
609        self.write(&frame)?;
610        self.wait_segt();
611        let data = self.receive_data(&mut [])?;
612        if !matches!(
613            data,
614            DataReceived::SBlock {
615                block: SBlock::ResyncResponse,
616                i_data: 0,
617                s_data: 0
618            }
619        ) {
620            error!("Got unexpected error: {data:?}");
621            return Err(Error::BadPcb);
622        }
623        self.iseq_snd = Seq::ZERO;
624        self.iseq_rcv = Seq::ZERO;
625        Ok(())
626    }
627
628    // TODO: find proper length for buffer
629    pub fn interface_soft_reset<'buf>(
630        &mut self,
631        buffer: &'buf mut [u8; 64],
632    ) -> Result<Atr<'buf>, Error> {
633        trace!("Interface Soft Reset");
634        let header = [
635            self.nad_hd2se,
636            Pcb::S(SBlock::InterfaceSoftResetRequest).to_byte(),
637            0,
638        ];
639        let [crc1, crc2] = Crc::calculate(&header).to_le_bytes();
640        self.write(&[header[0], header[1], header[2], crc1, crc2])?;
641        self.wait_segt();
642        let data = self.receive_data(buffer)?;
643        let received = if let DataReceived::SBlock {
644            block: SBlock::InterfaceSoftResetResponse,
645            i_data: 0,
646            s_data,
647        } = data
648        {
649            s_data
650        } else {
651            error!("Got unexpected error: {data:?}");
652            return Err(Error::BadPcb);
653        };
654        let atr = Atr::parse(&buffer[..received]);
655        if let Ok(atr) = &atr {
656            let mpot: u32 = atr.mpot.into();
657            self.mpot = 1000 * mpot;
658            self.segt = atr.segt.into();
659            self.bwt = (atr.bwt as u32) * 1000;
660        };
661        self.iseq_snd = Seq::ZERO;
662        self.iseq_rcv = Seq::ZERO;
663        debug_now!("Got atr: {atr:?}");
664        Ok(atr.unwrap_or_default())
665    }
666
667    pub fn wait_segt(&mut self) {
668        self.delay.delay_us(self.segt)
669    }
670
671    pub fn wait_mpot(&mut self) {
672        self.delay.delay_us(self.mpot)
673    }
674}
675
676/// UM1225 2.1.1
677const MAX_FRAME_DATA_LEN: usize = 0xFE;
678const HEADER_LEN: usize = 3;
679const TRAILER_LEN: usize = 2;
680const MAX_FRAME_LEN: usize = MAX_FRAME_DATA_LEN + HEADER_LEN + TRAILER_LEN;
681
682pub struct FrameSender<'writer, Twi, D> {
683    writer: &'writer mut T1oI2C<Twi, D>,
684    /// Total amount of application data that will be written
685    data: usize,
686    /// Amount of application data already written, includes data currently in `current_frame_buffer`
687    written: usize,
688    sent: usize,
689    current_frame_buffer: [u8; MAX_FRAME_LEN],
690}
691
692impl<'writer, Twi: I2CForT1, D: Delay> FrameSender<'writer, Twi, D> {
693    fn current_offset(&self) -> usize {
694        debug_assert!(self.written - self.sent <= MAX_FRAME_LEN);
695        self.written - self.sent
696    }
697
698    pub fn new(writer: &'writer mut T1oI2C<Twi, D>, data: usize) -> Self {
699        Self {
700            writer,
701            data,
702            written: 0,
703            sent: 0,
704            current_frame_buffer: [0; MAX_FRAME_LEN],
705        }
706    }
707
708    pub fn write_data(&mut self, data: &[u8]) -> Result<usize, Error> {
709        // Prevent false positive when delog is disabled
710        #[allow(clippy::if_same_then_else)]
711        if data.len() < 10 {
712            debug!("Writing data: {:02x?}", data);
713        } else {
714            debug!("Writing {} bytes", data.len());
715        }
716
717        if data.is_empty() {
718            return Ok(0);
719        }
720        if data.len() + self.written > self.data {
721            error!("Writing more data than expected");
722            return Err(Error::Line(line!()));
723        }
724
725        let current_offset = self.current_offset();
726        let available_in_frame = MAX_FRAME_DATA_LEN - current_offset;
727        let chunk_len = available_in_frame.min(data.len());
728        let chunk = &data[..chunk_len];
729        self.written += chunk_len;
730        self.current_frame_buffer[HEADER_LEN + current_offset..][..chunk_len]
731            .copy_from_slice(chunk);
732
733        // frame is full, must flush
734        let full_frame = chunk_len == available_in_frame;
735        // fully written, send remaining buffered data
736        let final_data = self.written == self.data;
737
738        if full_frame || final_data {
739            self.send_current_frame()?;
740        }
741
742        Ok(chunk_len)
743    }
744
745    pub fn send_current_frame(&mut self) -> Result<(), Error> {
746        let data_len = self.current_offset();
747        let is_last = self.written == self.data;
748        let pcb = Pcb::I(self.writer.iseq_snd, !is_last).to_byte();
749
750        self.writer.iseq_snd = !self.writer.iseq_snd;
751
752        let header = [self.writer.nad_hd2se, pcb, data_len as u8];
753        self.current_frame_buffer[0..HEADER_LEN].copy_from_slice(&header);
754        let trailer =
755            Crc::calculate(&self.current_frame_buffer[..HEADER_LEN + data_len]).to_le_bytes();
756        self.current_frame_buffer[HEADER_LEN + data_len..][..TRAILER_LEN].copy_from_slice(&trailer);
757        trace!(
758            "Sending:\n\tHeader: {:02x?}\n\tData: {:02x?}\n\tTrailer: {:02x?}",
759            &self.current_frame_buffer[..HEADER_LEN],
760            &self.current_frame_buffer[HEADER_LEN..][..data_len],
761            &self.current_frame_buffer[HEADER_LEN + data_len..][..TRAILER_LEN],
762        );
763
764        let mut wrote_success = false;
765        for _ in 0..self.writer.retry_count {
766            match self
767                .writer
768                .write(&self.current_frame_buffer[..data_len + HEADER_LEN + TRAILER_LEN])
769            {
770                Ok(()) => {
771                    wrote_success = true;
772                    break;
773                }
774                // Err(Error::DataNack) => {
775                //     self.writer.wait_segt();
776                //     continue;
777                // }
778                Err(Error::AddressNack) => {
779                    self.writer.wait_segt();
780                    continue;
781                }
782                Err(e) => return Err(e),
783            }
784        }
785
786        if !wrote_success {
787            debug_now!(
788                "Failed to send data after {} tries",
789                self.writer.retry_count
790            );
791            return Err(Error::Timeout);
792        }
793
794        self.sent += data_len;
795
796        if is_last {
797            // No R-BLOCK expected for non chained I block
798            return Ok(());
799        }
800
801        let mut resp_buf = [0u8; 5];
802        self.writer.wait_segt();
803        self.writer.read(&mut resp_buf)?;
804        debug!("Got R-Block: {:02x?}", resp_buf);
805        let [nad, pcb, len, crc1, crc2] = resp_buf;
806
807        if nad != self.writer.nad_se2hd {
808            error!("Received bad nad: {:02x}", nad);
809            return Err(Error::BadAddress);
810        }
811
812        let pcb = Pcb::parse(pcb);
813
814        match pcb {
815            Ok(Pcb::R(seq, RBlockError::NoError)) if seq == self.writer.iseq_snd => {}
816            Ok(Pcb::R(_, RBlockError::NoError)) => {
817                warn!("Got incorrect expected sequence");
818            }
819            Ok(Pcb::R(_, RBlockError::CrcError)) => {
820                error!("Got CrcError");
821                return Err(Error::BadCrc);
822            }
823            _ => {
824                error!("Got bad PCB: {pcb:?}");
825                return Err(Error::BadPcb);
826            }
827        }
828
829        if len != 0 {
830            error!("Received R-block with bad len: {}", len);
831            return Err(Error::BadAddress);
832        }
833
834        let crc = Crc::calculate(&resp_buf[0..HEADER_LEN]).to_le_bytes();
835        if [crc1, crc2] != crc {
836            error!(
837                "Got bad crc. Got {:02x?}, expected {:02x?}",
838                [crc1, crc2],
839                crc
840            );
841            return Err(Error::BadCrc);
842        }
843
844        Ok(())
845    }
846}
847
848impl<Twi: I2CForT1, D: Delay> Writer for FrameSender<'_, Twi, D> {
849    type Error = Error;
850    fn write(&mut self, data: &[u8]) -> Result<usize, Self::Error> {
851        self.write_data(data)
852    }
853}
854
855impl<'writer, Twi: I2CForT1, D: Delay> IntoWriter for &'writer mut T1oI2C<Twi, D> {
856    type Writer = FrameSender<'writer, Twi, D>;
857    fn into_writer(self, to_write: usize) -> Result<Self::Writer, <Self::Writer as Writer>::Error> {
858        Ok(FrameSender::new(self, to_write))
859    }
860}
861
862#[cfg(test)]
863mod tests {
864    use super::*;
865
866    fn assert_round_trip(value: u8, pcb: Pcb) {
867        assert_eq!(
868            value,
869            pcb.to_byte(),
870            "Expected 0b{value:08b}, got 0b{:08b}",
871            pcb.to_byte()
872        );
873        assert_eq!(pcb, Pcb::parse(value).unwrap());
874    }
875
876    #[test]
877    fn i_pcb() {
878        assert_round_trip(0b01100000, Pcb::I(Seq::ONE, true));
879        assert_round_trip(0b01000000, Pcb::I(Seq::ONE, false));
880        assert_round_trip(0b00100000, Pcb::I(Seq::ZERO, true));
881        assert_round_trip(0b00000000, Pcb::I(Seq::ZERO, false));
882    }
883
884    #[test]
885    fn r_pcb() {
886        assert_round_trip(0b10010000, Pcb::R(Seq::ONE, RBlockError::NoError));
887        assert_round_trip(0b10000000, Pcb::R(Seq::ZERO, RBlockError::NoError));
888
889        assert_round_trip(0b10010001, Pcb::R(Seq::ONE, RBlockError::CrcError));
890        assert_round_trip(0b10000001, Pcb::R(Seq::ZERO, RBlockError::CrcError));
891
892        assert_round_trip(0b10010010, Pcb::R(Seq::ONE, RBlockError::OtherError));
893        assert_round_trip(0b10000010, Pcb::R(Seq::ZERO, RBlockError::OtherError));
894    }
895
896    #[test]
897    fn atr() {
898        let atr: [u8; 0x23] = hex!(
899                "00" // protocol version
900                "a000000396" // vendor id
901                "04" // DLLP length
902                    "03e8" // BWT = 03E8 = 1s
903                    "00fe" // IFSC = 00FE = default
904                "02" //PLID
905                "0b"// PLP length
906                    "03e8" // Max frequency: 1MHz
907                    "08" // Config: HS mode supported
908                    "01" // MPOT  = 1 ms
909                    "000000" // RFU
910                    "0064" // SEGT = 100ms
911                    "0000" // WUT = 0ms
912                "0a" // len of historical bytes
913                    "4a434f5034204154504f"
914        );
915        assert_eq!(
916            Atr::parse(&atr).unwrap(),
917            Atr {
918                pver: 0,
919                vid: &hex!("a000000396"),
920                bwt: 1000,
921                ifsc: 0xFE,
922                plid: 2,
923                mcf: 1000,
924                config: 0x08,
925                mpot: 1,
926                segt: 100,
927                wut: 0,
928                historical_bytes: &hex!("4a434f5034204154504f")
929            }
930        );
931    }
932}
se05x/t1.rs

se05x/
t1.rs
