Refactor so Endpoint controls pipe characteristics.

Endpoints know their own address, number, max packet size, and data
toggle states. When a pipe is needed, pass in the endpoint its needed
for so that configuration can happen according to that endpoint.

3 files changed, 322 insertions, 145 deletions

diff --git a/usbh/src/device.rs b/usbh/src/device.rs
index 6efc395..aba3919 100644
--- a/usbh/src/device.rs
+++ b/usbh/src/device.rs
@@ -1,9 +1,12 @@
-use super::pipe::{Pipe, PipeErr, PipeTable, NO_DATA_STAGE};
+use super::pipe::{PipeErr, PipeTable, NO_DATA_STAGE};
 use super::usbproto::*;
 
 use core::convert::TryInto;
 use log::{debug, error, info, trace};
 
+// TODO: impl Drop for Device/Endpoint cleanup if any ends up being
+// required.
+
 // FIXME: once again, this doesn't belong here. The issue is that
 // we're using `pipe_for`, which requires it.
 use atsamd_hal::target_device::usb;
@@ -46,12 +49,20 @@ pub(crate) struct DeviceTable {
 // putting in an offset, which can be forgotten, it's better to let
 // something else handle address assignment.
 impl DeviceTable {
-    pub(crate) fn new() -> Self {
-        // TODO: should we create addr 0 here for use during
-        // enumeration? Probably.
-        Self {
-            devices: Default::default(),
-        }
+    pub(crate) fn new<F>(millis: &'static F) -> Self
+    where
+        F: Fn() -> usize + 'static,
+    {
+        let mut devices: [Option<Device>; MAX_DEVICES] = {
+            let mut devs: [core::mem::MaybeUninit<Option<Device>>; MAX_DEVICES] =
+                unsafe { core::mem::MaybeUninit::uninit().assume_init() };
+            for d in &mut devs[..] {
+                unsafe { core::ptr::write(d.as_mut_ptr(), None) }
+            }
+            unsafe { core::mem::transmute(devs) }
+        };
+        devices[0] = Some(Device::new(0, 8, millis));
+        Self { devices: devices }
     }
 
     /// Return the device at address `addr`.
@@ -66,11 +77,15 @@ impl DeviceTable {
     /// Allocate a device with the next available address.
     // TODO: get rid of the millis argument somehow, but the device
     // does need a way of tracking time for Settle reasons.
-    pub(crate) fn next(&mut self, millis: &'static dyn Fn() -> usize) -> Option<&mut Device> {
+    pub(crate) fn next(
+        &mut self,
+        max_packet_size: u8,
+        millis: &'static dyn Fn() -> usize,
+    ) -> Option<&mut Device> {
         for i in 1..self.devices.len() {
             if self.devices[i].is_none() {
                 let a = i.try_into().unwrap();
-                let d = Device::new(a, millis);
+                let d = Device::new(a, max_packet_size, millis);
                 self.devices[i] = Some(d);
                 return self.device_for(a);
             }
@@ -109,16 +124,35 @@ pub struct Device {
     pub addr: u8,
     pub max_packet_size: u8,
     pub endpoints: [Option<Endpoint>; MAX_ENDPOINTS],
+    pub ep0: Endpoint,
 
     state: FSM,
     millis: &'static dyn Fn() -> usize,
 }
 impl Device {
-    fn new(addr: u8, millis: &'static dyn Fn() -> usize) -> Self {
+    // TODO: get max packet size from device descriptor.
+    pub fn new(addr: u8, max_packet_size: u8, millis: &'static dyn Fn() -> usize) -> Self {
+        // Set up endpoints array with 0 as default control endpoint.
+        let endpoints: [Option<Endpoint>; MAX_ENDPOINTS] = {
+            let mut eps: [core::mem::MaybeUninit<Option<Endpoint>>; MAX_ENDPOINTS] =
+                unsafe { core::mem::MaybeUninit::uninit().assume_init() };
+            for ep in &mut eps[..] {
+                unsafe { core::ptr::write(ep.as_mut_ptr(), None) }
+            }
+            unsafe { core::mem::transmute(eps) }
+        };
+
         Self {
             addr: addr,
-            max_packet_size: 8,
-            endpoints: Default::default(),
+            max_packet_size: max_packet_size,
+            endpoints: endpoints,
+            ep0: Endpoint::new(
+                addr,
+                0,
+                TransferType::Control,
+                TransferDirection::In,
+                max_packet_size,
+            ),
 
             state: FSM::AddressSet,
 
@@ -141,11 +175,12 @@ impl Device {
             FSM::WaitForSettle(until) => {
                 if (self.millis)() >= until {
                     // Dunno why we get the device descriptor a second time.
-                    let mut pipe = pipe_table.pipe_for(host, self.addr, 0);
+                    let mut pipe = pipe_table.pipe_for(host, &self.ep0);
 
                     let mut vol_descr =
                         ::vcell::VolatileCell::<DeviceDescriptor>::new(Default::default());
                     pipe.control_transfer(
+                        &mut self.ep0,
                         RequestType::get_descr(),
                         RequestCode::GetDescriptor,
                         WValue::from((0, DescriptorType::Device as u8)),
@@ -163,11 +198,12 @@ impl Device {
 
             FSM::GetConfigDescriptor => {
                 // Get config descriptor with minimal data, to see how much we need to allocate for the full descriptor.
-                let mut pipe = pipe_table.pipe_for(host, self.addr, 0);
+                let mut pipe = pipe_table.pipe_for(host, &self.ep0);
 
                 let mut vol_descr =
                     ::vcell::VolatileCell::<ConfigurationDescriptor>::new(Default::default());
                 pipe.control_transfer(
+                    &mut self.ep0,
                     RequestType::get_descr(),
                     RequestCode::GetDescriptor,
                     WValue::from((0, DescriptorType::Configuration as u8)),
@@ -183,6 +219,7 @@ impl Device {
                 let buf: [u8; 64] = [0; 64];
                 let mut tmp = &buf[..desc.w_total_length as usize];
                 pipe.control_transfer(
+                    &mut self.ep0,
                     RequestType::get_descr(),
                     RequestCode::GetDescriptor,
                     WValue::from((0, DescriptorType::Configuration as u8)),
@@ -195,11 +232,12 @@ impl Device {
             }
 
             FSM::SetConfig => {
-                let mut pipe = pipe_table.pipe_for(host, self.addr, 0);
+                let mut pipe = pipe_table.pipe_for(host, &self.ep0);
 
                 debug!("+++ setting configuration");
                 let conf: u8 = 1;
                 pipe.control_transfer(
+                    &mut self.ep0,
                     RequestType::set(),
                     RequestCode::SetConfiguration,
                     WValue::from((conf, 0)),
@@ -211,6 +249,7 @@ impl Device {
 
                 debug!("+++ setting idle");
                 pipe.control_transfer(
+                    &mut self.ep0,
                     RequestType::from((
                         RequestDirection::HostToDevice,
                         RequestKind::Class,
@@ -227,6 +266,7 @@ impl Device {
                 debug!("+++ setting report");
                 let mut report: u8 = 0;
                 pipe.control_transfer(
+                    &mut self.ep0,
                     RequestType::from((
                         RequestDirection::HostToDevice,
                         RequestKind::Class,
@@ -240,7 +280,23 @@ impl Device {
                 )?;
                 debug!(" -- report set");
 
-                self.state = FSM::GetReport(3)
+                // Stub in some endpoints until we can parse the
+                // configuration descriptor.
+                self.endpoints[1] = Some(Endpoint::new(
+                    self.addr,
+                    1,
+                    TransferType::Interrupt,
+                    TransferDirection::In,
+                    8,
+                ));
+                self.endpoints[2] = Some(Endpoint::new(
+                    self.addr,
+                    2,
+                    TransferType::Interrupt,
+                    TransferDirection::In,
+                    8,
+                ));
+                self.state = FSM::GetReport(2)
             }
 
             FSM::GetReport(0) => self.state = FSM::Steady,
@@ -252,22 +308,12 @@ impl Device {
                 // get some keyboard reports without further setup.
 
                 // EP 1 is boot proto keyboard.
-                let mut pipe = pipe_table.pipe_for(host, self.addr, 1);
-                pipe.regs.cfg.write(|w| unsafe { w.ptype().bits(0x4) });
-                pipe.regs.statusclr.write(|w| unsafe {
-                    // No function for this. FIXME: need to patch the SVD for
-                    // PSTATUSCLR.DTGL at bit0. No? This is in the SVD, but
-                    // not the rust output.
-                    w.bits(1)
-                });
-                self.read_report(&mut pipe, 1);
+                self.read_report(pipe_table, host, 1);
 
                 // EP 2 is consumer control keys.
-                //let mut pipe = pipe_table.pipe_for(host, self.addr, 2);
-                //pipe.regs.cfg.write(|w| unsafe { w.ptype().bits(0x4) });
-                //self.read_report(&mut pipe, 2);
+                self.read_report(pipe_table, host, 2);
 
-                self.state = FSM::GetReport(count - 1)
+                self.state = FSM::GetReport(count)
             }
 
             FSM::Steady => {}
@@ -275,34 +321,73 @@ impl Device {
         Ok(())
     }
 
-    fn read_report(&mut self, pipe: &mut Pipe, id: u8) {
-        let mut buf: core::mem::MaybeUninit<[u8; 8]> = core::mem::MaybeUninit::uninit();
-        match pipe.in_transfer(&mut buf, 15, self.millis) {
-            Ok(bytes_received) => {
-                let tmp = unsafe { &(buf.assume_init())[..bytes_received] };
-                info!("report {}: {:?}", id, tmp);
-            }
+    fn read_report(&mut self, pipe_table: &mut PipeTable, host: &mut usb::HOST, id: u8) {
+        if let Some(ref mut ep) = self.endpoints[id as usize] {
+            let mut pipe = pipe_table.pipe_for(host, ep);
+            let mut buf: core::mem::MaybeUninit<[u8; 8]> = core::mem::MaybeUninit::uninit();
+            match pipe.in_transfer(ep, &mut buf, 15, self.millis) {
+                Ok(bytes_received) => {
+                    let tmp = unsafe { &(buf.assume_init())[..bytes_received] };
+                    info!("report {}: {:?}", id, tmp);
+                }
 
-            Err(e) => info!("error {}: {:?}", id, e),
+                Err(e) => trace!("error {}: {:?}", id, e),
+            }
+        } else {
+            error!("endpoint {} doesn't exist!", id)
         }
     }
 }
 
+// TransferType (INTERRUPT)
+// Direction (IN)
 pub struct Endpoint {
-    num: u8,
-    typ: EndpointType,
+    // This just points back to the address because we need to know it
+    // for all endpoint operations, but we don't want to pass the
+    // address struct (which contains all endpoints) around.
+    pub addr: u8,
+    pub num: u8,
+    pub transfer_type: TransferType,
+    pub direction: TransferDirection,
+    pub in_toggle: bool,
+    pub out_toggle: bool,
+    pub max_packet_size: u8,
 }
-impl Default for Endpoint {
-    fn default() -> Self {
+
+// cf §9.6.6 of USB 2.0
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum TransferDirection {
+    Out,
+    In,
+}
+
+// ibid
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub enum TransferType {
+    Control = 0,
+    Isochronous = 1,
+    Bulk = 2,
+    Interrupt = 3,
+}
+
+impl Endpoint {
+    // TODO: direction is ignored on control endpoints. Try to remove
+    // it from the API in those cases as well.
+    pub fn new(
+        addr: u8,
+        num: u8,
+        transfer_type: TransferType,
+        direction: TransferDirection,
+        max_packet_size: u8,
+    ) -> Self {
         Self {
-            num: 0,
-            typ: EndpointType::Control,
+            addr: addr,
+            num: num,
+            transfer_type: transfer_type,
+            direction: direction,
+            in_toggle: false,
+            out_toggle: false,
+            max_packet_size: max_packet_size,
         }
     }
 }
-
-enum EndpointType {
-    Control,
-    In,
-    Out,
-}
diff --git a/usbh/src/lib.rs b/usbh/src/lib.rs
index 63b28ba..1ed414c 100755
--- a/usbh/src/lib.rs
+++ b/usbh/src/lib.rs
@@ -5,8 +5,8 @@ mod device;
 mod pipe;
 mod usbproto;
 
-use device::DeviceTable;
-use pipe::{PType, PipeErr, PipeTable, NO_DATA_STAGE};
+use device::{Device, DeviceTable};
+use pipe::{PipeErr, PipeTable, NO_DATA_STAGE};
 use rb::{Reader, RingBuffer, Writer};
 use usbproto::*;
 
@@ -57,6 +57,7 @@ enum TaskState {
     Steady(SteadyState),
 }
 
+// Must be at least 100ms. cf §9.1.2 of USB 2.0.
 const SETTLE_DELAY: usize = 205; // Delay in sec/1024
 const NAK_LIMIT: usize = 15;
 
@@ -79,6 +80,7 @@ where
     pipe_table: PipeTable,
 
     devices: DeviceTable,
+    addr0: Device,
 
     // need sof 1kHz pad?
     _sof_pad: gpio::Pa23<gpio::PfG>,
@@ -115,7 +117,8 @@ where
 
             pipe_table: PipeTable::new(),
 
-            devices: DeviceTable::new(),
+            devices: DeviceTable::new(millis),
+            addr0: Device::new(0, 8, millis),
 
             _sof_pad: sof_pin.into_function_g(port),
             _dm_pad: dm_pin.into_function_g(port),
@@ -314,9 +317,6 @@ where
                     trace!("reset was sent");
                     self.usb.host().intflag.write(|w| w.rst().set_bit());
 
-                    // Make sure we always have a control pipe set up.
-                    self.init_pipe0();
-
                     // Seems unneccesary, since SOFE will be set
                     // immediately after reset according to §32.6.3.3.
                     self.usb.host().ctrlb.modify(|_, w| w.sofe().set_bit());
@@ -358,9 +358,12 @@ where
     }
 
     fn configure_dev(&mut self) -> Result<(), PipeErr> {
-        let mut pipe = self.pipe_table.pipe_for(self.usb.host_mut(), 0, 0);
+        let mut pipe = self
+            .pipe_table
+            .pipe_for(self.usb.host_mut(), &self.addr0.ep0);
         let mut vol_descr = ::vcell::VolatileCell::<DeviceDescriptor>::new(Default::default());
         pipe.control_transfer(
+            &mut self.addr0.ep0,
             RequestType::get_descr(),
             RequestCode::GetDescriptor,
             WValue::from((0, DescriptorType::Device as u8)),
@@ -372,13 +375,13 @@ where
         let desc = vol_descr.get();
         trace!(" -- devDesc: {:?}", desc);
 
-        match self.devices.next(self.millis) {
+        match self.devices.next(desc.b_max_packet_size, self.millis) {
             // TODO: new error for being out of devices.
             None => Err(PipeErr::Other("out of devices")),
             Some(device) => {
-                device.max_packet_size = desc.b_max_packet_size;
                 debug!("Setting address to {}.", device.addr);
                 pipe.control_transfer(
+                    &mut self.addr0.ep0,
                     RequestType::set(),
                     RequestCode::SetAddress,
                     WValue::from((device.addr, 0)),
@@ -393,20 +396,6 @@ where
             }
         }
     }
-
-    // Set up a default pipe for the control endpoint 0 on pipe 0.
-    fn init_pipe0(&mut self) {
-        let speed = self.usb.host().status.read().speed().bits();
-        let pipe = self.pipe_table.pipe_for(self.usb.host_mut(), 0, 0);
-        pipe.regs.cfg.write(|w| {
-            unsafe { w.ptype().bits(PType::Control as u8) };
-            w.bk().clear_bit()
-        });
-        pipe.desc.bank0.pcksize.write(|w| match speed {
-            0 => w.size().bytes64(),
-            _ => w.size().bytes8(),
-        });
-    }
 }
 
 pub fn handler(usbp: usize, events: &mut EventWriter) {
diff --git a/usbh/src/pipe.rs b/usbh/src/pipe.rs
index d7b7981..08602a3 100644
--- a/usbh/src/pipe.rs
+++ b/usbh/src/pipe.rs
@@ -12,6 +12,8 @@ use pck_size::PckSize;
 use status_bk::StatusBk;
 use status_pipe::StatusPipe;
 
+use super::device::{Endpoint, TransferType};
+
 use super::usbproto::*;
 
 use atsamd_hal::target_device::usb::{
@@ -60,31 +62,47 @@ pub(crate) struct PipeTable {
 
 impl PipeTable {
     pub(crate) fn new() -> Self {
-        Self {
-            tbl: [PipeDesc::new(); MAX_PIPES],
-        }
+        let tbl = {
+            let mut tbl: [core::mem::MaybeUninit<PipeDesc>; MAX_PIPES] =
+                unsafe { core::mem::MaybeUninit::uninit().assume_init() };
+
+            for e in &mut tbl[..] {
+                unsafe { core::ptr::write(e.as_mut_ptr(), PipeDesc::new()) }
+            }
+
+            unsafe { core::mem::transmute(tbl) }
+        };
+        Self { tbl: tbl }
     }
 
     pub(crate) fn pipe_for<'a, 'b>(
         &'a mut self,
         host: &'b mut usb::HOST,
-        addr: u8,
-        ep: u8,
+        endpoint: &Endpoint,
     ) -> Pipe<'a, 'b> {
         // Just use two pipes for now. 0 is always for control
         // endpoints, 1 for everything else.
         //
         // TODO: cache in-use pipes and return them without init if
         // possible.
-        let i = if ep == 0 { 0 } else { 1 };
+        let i = if endpoint.num == 0 { 0 } else { 1 };
 
         let pregs = PipeRegs::from(host, i);
         let pdesc = &mut self.tbl[i];
 
-        trace!("setting paddr of pipe {} to {}:{}", i, addr, ep);
+        pregs.cfg.write(|w| {
+            let ptype = PType::from(endpoint.transfer_type) as u8;
+            unsafe { w.ptype().bits(ptype) }
+        });
+        trace!(
+            "setting paddr of pipe {} to {}:{}",
+            i,
+            endpoint.addr,
+            endpoint.num
+        );
         pdesc.bank0.ctrl_pipe.write(|w| {
-            w.pdaddr().set_addr(addr);
-            w.pepnum().set_epnum(ep)
+            w.pdaddr().set_addr(endpoint.addr);
+            w.pepnum().set_epnum(endpoint.num)
         });
         Pipe {
             num: i,
@@ -97,12 +115,14 @@ impl PipeTable {
 // TODO: hide regs/desc fields. Needed right now for init_pipe0.
 pub(crate) struct Pipe<'a, 'b> {
     num: usize,
+
     pub(crate) regs: PipeRegs<'b>,
     pub(crate) desc: &'a mut PipeDesc,
 }
 impl Pipe<'_, '_> {
     pub(crate) fn control_transfer<T>(
         &mut self,
+        ep: &mut Endpoint,
         bm_request_type: RequestType,
         b_request: RequestCode,
         w_value: WValue,
@@ -110,9 +130,6 @@ impl Pipe<'_, '_> {
         buf: Option<&mut T>,
         millis: &dyn Fn() -> usize,
     ) -> Result<(), PipeErr> {
-        // Pipe data toggles for control pipes defined in SAMD21 data
-        // sheet §32.6.3.9
-
         let len = match buf {
             None => 0,
             _ => core::mem::size_of::<T>(),
@@ -128,8 +145,8 @@ impl Pipe<'_, '_> {
             w_index: w_index,
             w_length: len as u16,
         };
-        self.dtgl_clear();
         self.send(
+            ep,
             PToken::Setup,
             &DataBuf::from(&mut setup_packet),
             NAK_LIMIT,
@@ -139,17 +156,16 @@ impl Pipe<'_, '_> {
         /*
          * Data stage.
          */
-        self.dtgl_set();
         if let Some(b) = buf {
             // TODO: data stage, has up to 5,000ms (in 500ms
             // per-packet chunks) to complete. cf §9.2.6.4 of USB 2.0.
             match bm_request_type.direction()? {
                 RequestDirection::DeviceToHost => {
-                    self.in_transfer(b, NAK_LIMIT, millis)?;
+                    self.in_transfer(ep, b, NAK_LIMIT, millis)?;
                 }
 
                 RequestDirection::HostToDevice => {
-                    self.out_transfer(b, NAK_LIMIT, millis)?;
+                    self.out_transfer(ep, b, NAK_LIMIT, millis)?;
                 }
             }
         }
@@ -159,7 +175,6 @@ impl Pipe<'_, '_> {
          */
         // TODO: status stage has up to 50ms to complete. cf §9.2.6.4
         // of USB 2.0.
-        self.dtgl_set();
         self.desc.bank0.pcksize.write(|w| {
             unsafe { w.byte_count().bits(0) };
             unsafe { w.multi_packet_size().bits(0) }
@@ -171,12 +186,14 @@ impl Pipe<'_, '_> {
         };
 
         trace!("dispatching status stage");
-        self.dispatch_retries(token, NAK_LIMIT, millis)?;
+        self.dispatch_retries(ep, token, NAK_LIMIT, millis)?;
+
         Ok(())
     }
 
     fn send(
         &mut self,
+        ep: &mut Endpoint,
         token: PToken,
         buf: &DataBuf,
         nak_limit: usize,
@@ -194,41 +211,12 @@ impl Pipe<'_, '_> {
             unsafe { w.multi_packet_size().bits(0) }
         });
 
-        self.dispatch_retries(token, nak_limit, millis)
-    }
-
-    pub(crate) fn out_transfer<T>(
-        &mut self,
-        buf: &mut T,
-        nak_limit: usize,
-        millis: &dyn Fn() -> usize,
-    ) -> Result<usize, PipeErr> {
-        let db: DataBuf = buf.into();
-
-        trace!("p{}: Should OUT for {}b.", self.num, db.len);
-        self.desc.bank0.pcksize.write(|w| {
-            unsafe { w.byte_count().bits(db.len as u16) };
-            unsafe { w.multi_packet_size().bits(0) }
-        });
-
-        let mut bytes_sent = 0;
-        while bytes_sent < db.len {
-            self.desc
-                .bank0
-                .addr
-                .write(|w| unsafe { w.addr().bits(db.ptr as u32 + bytes_sent as u32) });
-            self.dispatch_retries(PToken::Out, nak_limit, millis)?;
-
-            let sent = self.desc.bank0.pcksize.read().byte_count().bits() as usize;
-            bytes_sent += sent;
-            trace!("!! wrote {} of {}", bytes_sent, db.len);
-        }
-
-        Ok(bytes_sent)
+        self.dispatch_retries(ep, token, nak_limit, millis)
     }
 
     pub(crate) fn in_transfer<T>(
         &mut self,
+        ep: &mut Endpoint,
         buf: &mut T,
         nak_limit: usize,
         millis: &dyn Fn() -> usize,
@@ -259,8 +247,10 @@ impl Pipe<'_, '_> {
                 .addr
                 .write(|w| unsafe { w.addr().bits(db.ptr as u32 + bytes_received as u32) });
             self.regs.statusclr.write(|w| w.bk0rdy().set_bit());
+            trace!("--- !!! regs-pre-dispatch !!! ---");
+            self.log_regs();
 
-            self.dispatch_retries(PToken::In, nak_limit, millis)?;
+            self.dispatch_retries(ep, PToken::In, nak_limit, millis)?;
             let recvd = self.desc.bank0.pcksize.read().byte_count().bits() as usize;
             bytes_received += recvd;
             trace!("!! read {} of {}", bytes_received, db.len);
@@ -271,7 +261,6 @@ impl Pipe<'_, '_> {
             // Don't allow writing past the buffer.
             assert!(bytes_received <= db.len);
         }
-        //self.dtgl();
 
         self.regs.statusset.write(|w| w.pfreeze().set_bit());
         if bytes_received < db.len {
@@ -288,11 +277,42 @@ impl Pipe<'_, '_> {
         }
     }
 
-    fn dtgl_set(&mut self) {
+    pub(crate) fn out_transfer<T>(
+        &mut self,
+        ep: &mut Endpoint,
+        buf: &mut T,
+        nak_limit: usize,
+        millis: &dyn Fn() -> usize,
+    ) -> Result<usize, PipeErr> {
+        let db: DataBuf = buf.into();
+
+        trace!("p{}: Should OUT for {}b.", self.num, db.len);
+        self.desc.bank0.pcksize.write(|w| {
+            unsafe { w.byte_count().bits(db.len as u16) };
+            unsafe { w.multi_packet_size().bits(0) }
+        });
+
+        let mut bytes_sent = 0;
+        while bytes_sent < db.len {
+            self.desc
+                .bank0
+                .addr
+                .write(|w| unsafe { w.addr().bits(db.ptr as u32 + bytes_sent as u32) });
+            self.dispatch_retries(ep, PToken::Out, nak_limit, millis)?;
+
+            let sent = self.desc.bank0.pcksize.read().byte_count().bits() as usize;
+            bytes_sent += sent;
+            trace!("!! wrote {} of {}", bytes_sent, db.len);
+        }
+
+        Ok(bytes_sent)
+    }
+
+    pub(crate) fn dtgl_set(&mut self) {
         self.regs.statusset.write(|w| w.dtgl().set_bit());
     }
 
-    fn dtgl_clear(&mut self) {
+    pub(crate) fn dtgl_clear(&mut self) {
         self.regs.statusclr.write(|w| unsafe {
             // No function for this. FIXME: need to patch the SVD for
             // PSTATUSCLR.DTGL at bit0. No? This is in the SVD, but
@@ -301,20 +321,27 @@ impl Pipe<'_, '_> {
         });
     }
 
-    fn dtgl(&mut self) {
+    pub(crate) fn dtgl(&mut self, ep: &mut Endpoint, token: PToken) {
         // TODO: this makes no sense to me, and docs are unclear. If
         // the status bit is set, set it again? if it's clear then
         // clear it? This is what I get for having to work from
         // Arduino sources.
         trace!(
-            "~~~ curbk: {}, dtgl: {}",
-            self.regs.status.read().curbk().bit(),
-            self.regs.status.read().dtgl().bit()
+            "~~~ tok: {:?}, dtgl: {}, i: {}, o: {}",
+            token,
+            self.regs.status.read().dtgl().bit(),
+            ep.in_toggle,
+            ep.out_toggle,
         );
         if self.regs.status.read().dtgl().bit_is_set() {
+            self.dtgl_clear();
+        } else {
             self.dtgl_set();
+        }
+        if token == PToken::In {
+            ep.in_toggle = self.regs.status.read().dtgl().bit_is_set()
         } else {
-            self.dtgl_clear();
+            ep.out_toggle = self.regs.status.read().dtgl().bit_is_set()
         }
     }
 
@@ -323,31 +350,40 @@ impl Pipe<'_, '_> {
     // non-blocking.
     fn dispatch_retries(
         &mut self,
+        ep: &mut Endpoint,
         token: PToken,
         retries: usize,
         millis: &dyn Fn() -> usize,
     ) -> Result<(), PipeErr> {
-        self.dispatch_packet(token);
+        self.dispatch_packet(ep, token);
 
         let until = millis() + USB_TIMEOUT;
         let mut last_err = PipeErr::SWTimeout;
         let mut naks = 0;
         while millis() < until {
-            match self.dispatch_result(token) {
-                Ok(true) => return Ok(()),
+            let res = self.dispatch_result(token);
+            match res {
+                Ok(true) => {
+                    if token == PToken::In {
+                        ep.in_toggle = self.regs.status.read().dtgl().bit_is_set();
+                    } else if token == PToken::Out {
+                        ep.out_toggle = self.regs.status.read().dtgl().bit_is_set();
+                    }
+                    return Ok(());
+                }
                 Ok(false) => continue,
 
                 Err(e) => {
                     last_err = e;
                     match last_err {
-                        PipeErr::DataToggle => self.dtgl(),
+                        PipeErr::DataToggle => self.dtgl(ep, token),
                         PipeErr::Stall => break,
                         _ => {
                             naks += 1;
                             if naks > retries {
                                 break;
                             } else {
-                                self.dispatch_packet(token);
+                                self.dispatch_packet(ep, token);
                             }
                         }
                     }
@@ -358,46 +394,105 @@ impl Pipe<'_, '_> {
         Err(last_err)
     }
 
-    fn dispatch_packet(&mut self, token: PToken) {
+    fn dispatch_packet(&mut self, ep: &mut Endpoint, token: PToken) {
         self.regs
             .cfg
             .modify(|_, w| unsafe { w.ptoken().bits(token as u8) });
-
-        trace!("initial regs");
-        self.log_regs();
+        self.regs.intflag.modify(|_, w| w.trfail().set_bit());
+        self.regs.intflag.modify(|_, w| w.perr().set_bit());
 
         match token {
             PToken::Setup => {
                 self.regs.intflag.write(|w| w.txstp().set_bit());
                 self.regs.statusset.write(|w| w.bk0rdy().set_bit());
+
+                // Toggles should be 1 for host and function's
+                // sequence at end of setup transaction. cf §8.6.1 of
+                // USB 2.0.
+                self.dtgl_clear();
+                ep.in_toggle = true;
+                ep.out_toggle = true;
+            }
+            PToken::In => {
+                self.regs.statusclr.write(|w| w.bk0rdy().set_bit());
+                if ep.in_toggle {
+                    self.dtgl_set();
+                } else {
+                    self.dtgl_clear();
+                }
             }
-            PToken::In => self.regs.statusclr.write(|w| w.bk0rdy().set_bit()),
             PToken::Out => {
                 self.regs.intflag.write(|w| w.trcpt0().set_bit());
                 self.regs.statusset.write(|w| w.bk0rdy().set_bit());
+                if ep.out_toggle {
+                    self.dtgl_set();
+                } else {
+                    self.dtgl_clear();
+                }
             }
             _ => {}
         }
+
+        trace!("initial regs");
+        self.log_regs();
+
         self.regs.statusclr.write(|w| w.pfreeze().set_bit());
     }
 
+    #[cfg(not(feature = "foo"))]
     fn dispatch_result(&mut self, token: PToken) -> Result<bool, PipeErr> {
         if self.is_transfer_complete(token)? {
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
+            Ok(true)
+        } else if self.regs.intflag.read().trfail().bit_is_set() {
+            self.regs.intflag.write(|w| w.trfail().set_bit());
+            trace!("trfail");
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
+            self.log_regs();
+            Err(PipeErr::TransferFail)
+        } else if self.desc.bank0.status_bk.read().errorflow().bit_is_set() {
+            trace!("errorflow");
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
+            self.log_regs();
+            Err(PipeErr::Flow)
+        } else if self.desc.bank0.status_pipe.read().touter().bit_is_set() {
+            trace!("touter");
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
+            self.log_regs();
+            Err(PipeErr::HWTimeout)
+        } else if self.desc.bank0.status_pipe.read().dtgler().bit_is_set() {
+            trace!("dtgler");
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
+            self.log_regs();
+            Err(PipeErr::DataToggle)
+        } else {
+            // Nothing wrong, but not done yet.
+            Ok(false)
+        }
+    }
+
+    #[cfg(feature = "foo")]
+    fn dispatch_result(&mut self, token: PToken) -> Result<bool, PipeErr> {
+        if self.is_transfer_complete(token)? {
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
             Ok(true)
         } else if self.regs.intflag.read().stall().bit_is_set() {
             warn!("stall");
             self.log_regs();
             self.regs.intflag.write(|w| w.stall().set_bit());
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
             Err(PipeErr::Stall)
         } else if self.regs.intflag.read().trfail().bit_is_set() {
             warn!("trfail");
             self.log_regs();
             self.regs.intflag.write(|w| w.trfail().set_bit());
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
             Err(PipeErr::TransferFail)
         } else if self.regs.intflag.read().perr().bit_is_set() {
             warn!("perr");
             self.log_regs();
             self.regs.intflag.write(|w| w.perr().set_bit());
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
             Err(PipeErr::PipeErr)
         } else if self.desc.bank0.status_bk.read().errorflow().bit_is_set() {
             warn!("errorflow");
@@ -406,6 +501,7 @@ impl Pipe<'_, '_> {
                 .bank0
                 .status_bk
                 .write(|w| w.errorflow().clear_bit());
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
             Err(PipeErr::Flow)
         } else if self.desc.bank0.status_pipe.read().touter().bit_is_set() {
             warn!("touter");
@@ -414,6 +510,7 @@ impl Pipe<'_, '_> {
                 .bank0
                 .status_pipe
                 .write(|w| w.touter().clear_bit());
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
             Err(PipeErr::HWTimeout)
         } else if self.desc.bank0.status_pipe.read().dtgler().bit_is_set() {
             warn!("dtgler");
@@ -422,6 +519,7 @@ impl Pipe<'_, '_> {
                 .bank0
                 .status_pipe
                 .write(|w| w.dtgler().clear_bit());
+            self.regs.statusset.write(|w| w.pfreeze().set_bit());
             Err(PipeErr::DataToggle)
         } else {
             // Nothing wrong, but not done yet.
@@ -434,7 +532,6 @@ impl Pipe<'_, '_> {
             PToken::Setup => {
                 if self.regs.intflag.read().txstp().bit_is_set() {
                     self.regs.intflag.write(|w| w.txstp().set_bit());
-                    self.regs.statusset.write(|w| w.pfreeze().set_bit());
                     Ok(true)
                 } else {
                     Ok(false)
@@ -443,7 +540,6 @@ impl Pipe<'_, '_> {
             PToken::In => {
                 if self.regs.intflag.read().trcpt0().bit_is_set() {
                     self.regs.intflag.write(|w| w.trcpt0().set_bit());
-                    self.regs.statusset.write(|w| w.pfreeze().set_bit());
                     Ok(true)
                 } else {
                     Ok(false)
@@ -452,7 +548,6 @@ impl Pipe<'_, '_> {
             PToken::Out => {
                 if self.regs.intflag.read().trcpt0().bit_is_set() {
                     self.regs.intflag.write(|w| w.trcpt0().set_bit());
-                    self.regs.statusset.write(|w| w.pfreeze().set_bit());
                     Ok(true)
                 } else {
                     Ok(false)
@@ -518,6 +613,16 @@ pub(crate) enum PType {
     _Reserved0 = 0x06,
     _Reserved1 = 0x07,
 }
+impl From<TransferType> for PType {
+    fn from(v: TransferType) -> Self {
+        match v {
+            TransferType::Control => Self::Control,
+            TransferType::Isochronous => Self::ISO,
+            TransferType::Bulk => Self::Bulk,
+            TransferType::Interrupt => Self::Interrupt,
+        }
+    }
+}
 
 struct DataBuf<'a> {
     ptr: *mut u8,
@@ -630,7 +735,6 @@ impl<'a> PipeRegs<'a> {
 }
 
 // §32.8.7.1
-#[derive(Clone, Copy, Debug)]
 pub(crate) struct PipeDesc {
     pub bank0: BankDesc,
     pub bank1: BankDesc,
@@ -646,7 +750,6 @@ impl PipeDesc {
     }
 }
 
-#[derive(Clone, Copy, Debug)]
 #[repr(C, packed)]
 // 16 bytes per bank.
 pub(crate) struct BankDesc {