usb: builtin handling of interface alternate settings

The stack reads its own descriptors to figure out which endpoints
are used in which alt settings, and enables/disables them as needed.

The ControlHandler has a callback so it can get notified of alternate
setting changes, which is purely informative (it doesn't have to do anything).

9 files changed, 358 insertions, 147 deletions

diff --git a/embassy-nrf/src/usb.rs b/embassy-nrf/src/usb.rs
index b67201e67..c032b2cc5 100644
--- a/embassy-nrf/src/usb.rs
+++ b/embassy-nrf/src/usb.rs
@@ -194,16 +194,12 @@ impl<'d, T: Instance> driver::Driver<'d> for Driver<'d, T> {
     fn into_bus(self) -> Self::Bus {
         Bus {
             phantom: PhantomData,
-            alloc_in: self.alloc_in,
-            alloc_out: self.alloc_out,
         }
     }
 }
 
 pub struct Bus<'d, T: Instance> {
     phantom: PhantomData<&'d mut T>,
-    alloc_in: Allocator,
-    alloc_out: Allocator,
 }
 
 impl<'d, T: Instance> driver::Bus for Bus<'d, T> {
@@ -264,7 +260,16 @@ impl<'d, T: Instance> driver::Bus for Bus<'d, T> {
             if regs.events_usbreset.read().bits() != 0 {
                 regs.events_usbreset.reset();
                 regs.intenset.write(|w| w.usbreset().set());
-                self.set_configured(false);
+
+                // Disable all endpoints except EP0
+                regs.epinen.write(|w| unsafe { w.bits(0x01) });
+                regs.epouten.write(|w| unsafe { w.bits(0x01) });
+                READY_ENDPOINTS.store(In::mask(0), Ordering::Release);
+                for i in 1..=7 {
+                    In::waker(i).wake();
+                    Out::waker(i).wake();
+                }
+
                 return Poll::Ready(Event::Reset);
             }
 
@@ -297,57 +302,76 @@ impl<'d, T: Instance> driver::Bus for Bus<'d, T> {
     }
 
     #[inline]
-    fn set_configured(&mut self, configured: bool) {
+    fn set_address(&mut self, _addr: u8) {
+        // Nothing to do, the peripheral handles this.
+    }
+
+    fn endpoint_set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool) {
+        Driver::<T>::set_stalled(ep_addr, stalled)
+    }
+
+    fn endpoint_is_stalled(&mut self, ep_addr: EndpointAddress) -> bool {
+        Driver::<T>::is_stalled(ep_addr)
+    }
+
+    fn endpoint_set_enabled(&mut self, ep_addr: EndpointAddress, enabled: bool) {
         let regs = T::regs();
 
-        unsafe {
-            if configured {
-                // TODO: Initialize ISO buffers
+        let i = ep_addr.index();
+        let mask = 1 << i;
 
-                regs.epinen.write(|w| w.bits(self.alloc_in.used.into()));
-                regs.epouten.write(|w| w.bits(self.alloc_out.used.into()));
+        debug!("endpoint_set_enabled {:?} {}", ep_addr, enabled);
 
-                for i in 1..8 {
-                    let out_enabled = self.alloc_out.used & (1 << i) != 0;
+        match ep_addr.direction() {
+            UsbDirection::In => {
+                let mut was_enabled = false;
+                regs.epinen.modify(|r, w| {
+                    let mut bits = r.bits();
+                    was_enabled = (bits & mask) != 0;
+                    if enabled {
+                        bits |= mask
+                    } else {
+                        bits &= !mask
+                    }
+                    unsafe { w.bits(bits) }
+                });
 
+                let ready_mask = In::mask(i);
+                if enabled {
+                    if !was_enabled {
+                        READY_ENDPOINTS.fetch_or(ready_mask, Ordering::AcqRel);
+                    }
+                } else {
+                    READY_ENDPOINTS.fetch_and(!ready_mask, Ordering::AcqRel);
+                }
+
+                In::waker(i).wake();
+            }
+            UsbDirection::Out => {
+                regs.epouten.modify(|r, w| {
+                    let mut bits = r.bits();
+                    if enabled {
+                        bits |= mask
+                    } else {
+                        bits &= !mask
+                    }
+                    unsafe { w.bits(bits) }
+                });
+
+                let ready_mask = Out::mask(i);
+                if enabled {
                     // when first enabled, bulk/interrupt OUT endpoints will *not* receive data (the
                     // peripheral will NAK all incoming packets) until we write a zero to the SIZE
                     // register (see figure 203 of the 52840 manual). To avoid that we write a 0 to the
                     // SIZE register
-                    if out_enabled {
-                        regs.size.epout[i].reset();
-                    }
+                    regs.size.epout[i].reset();
+                } else {
+                    READY_ENDPOINTS.fetch_and(!ready_mask, Ordering::AcqRel);
                 }
 
-                // IN endpoints (low bits) default to ready.
-                // OUT endpoints (high bits) default to NOT ready, they become ready when data comes in.
-                READY_ENDPOINTS.store(0x0000FFFF, Ordering::Release);
-            } else {
-                // Disable all endpoints except EP0
-                regs.epinen.write(|w| w.bits(0x01));
-                regs.epouten.write(|w| w.bits(0x01));
-
-                READY_ENDPOINTS.store(In::mask(0), Ordering::Release);
+                Out::waker(i).wake();
             }
         }
-
-        for i in 1..=7 {
-            In::waker(i).wake();
-            Out::waker(i).wake();
-        }
-    }
-
-    #[inline]
-    fn set_device_address(&mut self, _addr: u8) {
-        // Nothing to do, the peripheral handles this.
-    }
-
-    fn set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool) {
-        Driver::<T>::set_stalled(ep_addr, stalled)
-    }
-
-    fn is_stalled(&mut self, ep_addr: EndpointAddress) -> bool {
-        Driver::<T>::is_stalled(ep_addr)
     }
 
     #[inline]
diff --git a/embassy-usb-hid/src/lib.rs b/embassy-usb-hid/src/lib.rs
index 48e15e86a..b9ba4f1e7 100644
--- a/embassy-usb-hid/src/lib.rs
+++ b/embassy-usb-hid/src/lib.rs
@@ -438,6 +438,14 @@ impl<'d> ControlHandler for Control<'d> {
         self.out_report_offset.store(0, Ordering::Release);
     }
 
+    fn get_descriptor<'a>(&'a mut self, req: Request, buf: &'a mut [u8]) -> InResponse<'a> {
+        match (req.value >> 8) as u8 {
+            HID_DESC_DESCTYPE_HID_REPORT => InResponse::Accepted(self.report_descriptor),
+            HID_DESC_DESCTYPE_HID => InResponse::Accepted(&self.hid_descriptor),
+            _ => InResponse::Rejected,
+        }
+    }
+
     fn control_out(&mut self, req: embassy_usb::control::Request, data: &[u8]) -> OutResponse {
         trace!("HID control_out {:?} {=[u8]:x}", req, data);
         if let RequestType::Class = req.request_type {
@@ -477,13 +485,8 @@ impl<'d> ControlHandler for Control<'d> {
 
     fn control_in<'a>(&'a mut self, req: Request, buf: &'a mut [u8]) -> InResponse<'a> {
         trace!("HID control_in {:?}", req);
-        match (req.request_type, req.request) {
-            (RequestType::Standard, Request::GET_DESCRIPTOR) => match (req.value >> 8) as u8 {
-                HID_DESC_DESCTYPE_HID_REPORT => InResponse::Accepted(self.report_descriptor),
-                HID_DESC_DESCTYPE_HID => InResponse::Accepted(&self.hid_descriptor),
-                _ => InResponse::Rejected,
-            },
-            (RequestType::Class, HID_REQ_GET_REPORT) => {
+        match req.request {
+            HID_REQ_GET_REPORT => {
                 let size = match ReportId::try_from(req.value) {
                     Ok(id) => self.request_handler.and_then(|x| x.get_report(id, buf)),
                     Err(_) => None,
@@ -495,7 +498,7 @@ impl<'d> ControlHandler for Control<'d> {
                     InResponse::Rejected
                 }
             }
-            (RequestType::Class, HID_REQ_GET_IDLE) => {
+            HID_REQ_GET_IDLE => {
                 if let Some(handler) = self.request_handler {
                     let id = req.value as u8;
                     let id = (id != 0).then(|| ReportId::In(id));
@@ -510,7 +513,7 @@ impl<'d> ControlHandler for Control<'d> {
                     InResponse::Rejected
                 }
             }
-            (RequestType::Class, HID_REQ_GET_PROTOCOL) => {
+            HID_REQ_GET_PROTOCOL => {
                 // UNSUPPORTED: Boot Protocol
                 buf[0] = 1;
                 InResponse::Accepted(&buf[0..1])
diff --git a/embassy-usb/src/builder.rs b/embassy-usb/src/builder.rs
index 7e67b4ae6..c0aea9838 100644
--- a/embassy-usb/src/builder.rs
+++ b/embassy-usb/src/builder.rs
@@ -1,5 +1,7 @@
 use heapless::Vec;
 
+use crate::Interface;
+
 use super::control::ControlHandler;
 use super::descriptor::{BosWriter, DescriptorWriter};
 use super::driver::{Driver, Endpoint};
@@ -121,11 +123,10 @@ impl<'a> Config<'a> {
 pub struct Builder<'d, D: Driver<'d>> {
     config: Config<'d>,
     handler: Option<&'d dyn DeviceStateHandler>,
-    interfaces: Vec<(u8, &'d mut dyn ControlHandler), MAX_INTERFACE_COUNT>,
+    interfaces: Vec<Interface<'d>, MAX_INTERFACE_COUNT>,
     control_buf: &'d mut [u8],
 
     driver: D,
-    next_interface_number: u8,
     next_string_index: u8,
 
     device_descriptor: DescriptorWriter<'d>,
@@ -180,7 +181,6 @@ impl<'d, D: Driver<'d>> Builder<'d, D> {
             config,
             interfaces: Vec::new(),
             control_buf,
-            next_interface_number: 0,
             next_string_index: 4,
 
             device_descriptor,
@@ -234,7 +234,7 @@ impl<'d, D: Driver<'d>> Builder<'d, D> {
     ) -> FunctionBuilder<'_, 'd, D> {
         let iface_count_index = if self.config.composite_with_iads {
             self.config_descriptor.iad(
-                InterfaceNumber::new(self.next_interface_number),
+                InterfaceNumber::new(self.interfaces.len() as _),
                 0,
                 class,
                 subclass,
@@ -275,13 +275,15 @@ impl<'a, 'd, D: Driver<'d>> FunctionBuilder<'a, 'd, D> {
             self.builder.config_descriptor.buf[i] += 1;
         }
 
-        let number = self.builder.next_interface_number;
-        self.builder.next_interface_number += 1;
+        let number = self.builder.interfaces.len() as _;
+        let iface = Interface {
+            handler,
+            current_alt_setting: 0,
+            num_alt_settings: 0,
+        };
 
-        if let Some(handler) = handler {
-            if self.builder.interfaces.push((number, handler)).is_err() {
-                panic!("max interface count reached")
-            }
+        if self.builder.interfaces.push(iface).is_err() {
+            panic!("max interface count reached")
         }
 
         InterfaceBuilder {
@@ -318,6 +320,7 @@ impl<'a, 'd, D: Driver<'d>> InterfaceBuilder<'a, 'd, D> {
     ) -> InterfaceAltBuilder<'_, 'd, D> {
         let number = self.next_alt_setting_number;
         self.next_alt_setting_number += 1;
+        self.builder.interfaces[self.interface_number.0 as usize].num_alt_settings += 1;
 
         self.builder.config_descriptor.interface_alt(
             self.interface_number,
diff --git a/embassy-usb/src/control.rs b/embassy-usb/src/control.rs
index 9300d8c4c..0b59451d5 100644
--- a/embassy-usb/src/control.rs
+++ b/embassy-usb/src/control.rs
@@ -2,7 +2,6 @@ use core::mem;
 
 use crate::descriptor::DescriptorWriter;
 use crate::driver::{self, EndpointError};
-use crate::DEFAULT_ALTERNATE_SETTING;
 
 use super::types::*;
 
@@ -150,6 +149,10 @@ pub trait ControlHandler {
     /// Called after a USB reset after the bus reset sequence is complete.
     fn reset(&mut self) {}
 
+    fn set_alternate_setting(&mut self, alternate_setting: u8) {
+        let _ = alternate_setting;
+    }
+
     /// Called when a control request is received with direction HostToDevice.
     ///
     /// # Arguments
@@ -163,8 +166,8 @@ pub trait ControlHandler {
 
     /// Called when a control request is received with direction DeviceToHost.
     ///
-    /// You should write the response to `resp`, then return `InResponse::Accepted(len)`
-    /// with the length of the response.
+    /// You should write the response somewhere (usually to `buf`, but you may use another buffer
+    /// owned by yourself, or a static buffer), then return `InResponse::Accepted(data)`.
     ///
     /// # Arguments
     ///
@@ -174,23 +177,17 @@ pub trait ControlHandler {
         InResponse::Rejected
     }
 
-    fn set_interface(&mut self, alternate_setting: u16) -> OutResponse {
-        if alternate_setting == u16::from(DEFAULT_ALTERNATE_SETTING) {
-            OutResponse::Accepted
-        } else {
-            OutResponse::Rejected
-        }
-    }
-
-    fn get_interface<'a>(&'a mut self, buf: &'a mut [u8]) -> InResponse<'a> {
-        buf[0] = DEFAULT_ALTERNATE_SETTING;
-        InResponse::Accepted(&buf[0..1])
-    }
-
-    fn get_status<'a>(&'a mut self, buf: &'a mut [u8]) -> InResponse {
-        let status: u16 = 0;
-        buf[0..2].copy_from_slice(&status.to_le_bytes());
-        InResponse::Accepted(&buf[0..2])
+    /// Called when a GET DESCRIPTOR control request is received on the interface.
+    ///
+    /// You should write the response somewhere (usually to `buf`, but you may use another buffer
+    /// owned by yourself, or a static buffer), then return `InResponse::Accepted(data)`.
+    ///
+    /// # Arguments
+    ///
+    /// * `req` - The request from the SETUP packet.
+    fn get_descriptor<'a>(&'a mut self, req: Request, buf: &'a mut [u8]) -> InResponse<'a> {
+        let _ = (req, buf);
+        InResponse::Rejected
     }
 }
 
diff --git a/embassy-usb/src/descriptor.rs b/embassy-usb/src/descriptor.rs
index b61dea4b4..dce326780 100644
--- a/embassy-usb/src/descriptor.rs
+++ b/embassy-usb/src/descriptor.rs
@@ -1,5 +1,5 @@
 use super::builder::Config;
-use super::{types::*, CONFIGURATION_VALUE, DEFAULT_ALTERNATE_SETTING};
+use super::{types::*, CONFIGURATION_VALUE};
 
 /// Standard descriptor types
 #[allow(missing_docs)]
@@ -192,7 +192,7 @@ impl<'a> DescriptorWriter<'a> {
         interface_protocol: u8,
         interface_string: Option<StringIndex>,
     ) {
-        if alternate_setting == DEFAULT_ALTERNATE_SETTING {
+        if alternate_setting == 0 {
             match self.num_interfaces_mark {
                 Some(mark) => self.buf[mark] += 1,
                 None => {
diff --git a/embassy-usb/src/descriptor_reader.rs b/embassy-usb/src/descriptor_reader.rs
new file mode 100644
index 000000000..0a12b566c
--- /dev/null
+++ b/embassy-usb/src/descriptor_reader.rs
@@ -0,0 +1,110 @@
+use crate::descriptor::descriptor_type;
+use crate::types::EndpointAddress;
+
+#[derive(Copy, Clone, PartialEq, Eq, Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct ReadError;
+
+pub struct Reader<'a> {
+    data: &'a [u8],
+}
+
+impl<'a> Reader<'a> {
+    pub fn new(data: &'a [u8]) -> Self {
+        Self { data }
+    }
+
+    pub fn eof(&self) -> bool {
+        self.data.is_empty()
+    }
+
+    pub fn read<const N: usize>(&mut self) -> Result<[u8; N], ReadError> {
+        let n = self.data.get(0..N).ok_or(ReadError)?;
+        self.data = &self.data[N..];
+        Ok(n.try_into().unwrap())
+    }
+
+    pub fn read_u8(&mut self) -> Result<u8, ReadError> {
+        Ok(u8::from_le_bytes(self.read()?))
+    }
+    pub fn read_u16(&mut self) -> Result<u16, ReadError> {
+        Ok(u16::from_le_bytes(self.read()?))
+    }
+
+    pub fn read_slice(&mut self, len: usize) -> Result<&'a [u8], ReadError> {
+        let res = self.data.get(0..len).ok_or(ReadError)?;
+        self.data = &self.data[len..];
+        Ok(res)
+    }
+
+    pub fn read_descriptors(&mut self) -> DescriptorIter<'_, 'a> {
+        DescriptorIter { r: self }
+    }
+}
+
+pub struct DescriptorIter<'a, 'b> {
+    r: &'a mut Reader<'b>,
+}
+
+impl<'a, 'b> Iterator for DescriptorIter<'a, 'b> {
+    type Item = Result<(u8, Reader<'a>), ReadError>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        if self.r.eof() {
+            return None;
+        }
+
+        let len = match self.r.read_u8() {
+            Ok(x) => x,
+            Err(e) => return Some(Err(e)),
+        };
+        let type_ = match self.r.read_u8() {
+            Ok(x) => x,
+            Err(e) => return Some(Err(e)),
+        };
+        let data = match self.r.read_slice(len as usize - 2) {
+            Ok(x) => x,
+            Err(e) => return Some(Err(e)),
+        };
+
+        Some(Ok((type_, Reader::new(data))))
+    }
+}
+
+#[derive(Copy, Clone, PartialEq, Eq, Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct EndpointInfo {
+    pub configuration: u8,
+    pub interface: u8,
+    pub interface_alt: u8,
+    pub ep_address: EndpointAddress,
+}
+
+pub fn foreach_endpoint(data: &[u8], mut f: impl FnMut(EndpointInfo)) -> Result<(), ReadError> {
+    let mut ep = EndpointInfo {
+        configuration: 0,
+        interface: 0,
+        interface_alt: 0,
+        ep_address: EndpointAddress::from(0),
+    };
+    for res in Reader::new(data).read_descriptors() {
+        let (kind, mut r) = res?;
+        match kind {
+            descriptor_type::CONFIGURATION => {
+                let _total_length = r.read_u16()?;
+                let _total_length = r.read_u8()?;
+                ep.configuration = r.read_u8()?;
+            }
+            descriptor_type::INTERFACE => {
+                ep.interface = r.read_u8()?;
+                ep.interface_alt = r.read_u8()?;
+            }
+            descriptor_type::ENDPOINT => {
+                ep.ep_address = EndpointAddress::from(r.read_u8()?);
+                f(ep)
+            }
+            _ => {}
+        }
+    }
+    Ok(())
+}
diff --git a/embassy-usb/src/driver.rs b/embassy-usb/src/driver.rs
index cedd349fb..e552dc7b6 100644
--- a/embassy-usb/src/driver.rs
+++ b/embassy-usb/src/driver.rs
@@ -79,17 +79,17 @@ pub trait Bus {
     fn poll<'a>(&'a mut self) -> Self::PollFuture<'a>;
 
     /// Sets the device USB address to `addr`.
-    fn set_device_address(&mut self, addr: u8);
+    fn set_address(&mut self, addr: u8);
 
-    /// Sets the device configured state.
-    fn set_configured(&mut self, configured: bool);
+    /// Enables or disables an endpoint.
+    fn endpoint_set_enabled(&mut self, ep_addr: EndpointAddress, enabled: bool);
 
     /// Sets or clears the STALL condition for an endpoint. If the endpoint is an OUT endpoint, it
     /// should be prepared to receive data again. Only used during control transfers.
-    fn set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool);
+    fn endpoint_set_stalled(&mut self, ep_addr: EndpointAddress, stalled: bool);
 
     /// Gets whether the STALL condition is set for an endpoint. Only used during control transfers.
-    fn is_stalled(&mut self, ep_addr: EndpointAddress) -> bool;
+    fn endpoint_is_stalled(&mut self, ep_addr: EndpointAddress) -> bool;
 
     /// Simulates a disconnect from the USB bus, causing the host to reset and re-enumerate the
     /// device.
diff --git a/embassy-usb/src/lib.rs b/embassy-usb/src/lib.rs
index 7cd00fbaf..acb26c60d 100644
--- a/embassy-usb/src/lib.rs
+++ b/embassy-usb/src/lib.rs
@@ -8,12 +8,15 @@ pub(crate) mod fmt;
 mod builder;
 pub mod control;
 pub mod descriptor;
+mod descriptor_reader;
 pub mod driver;
 pub mod types;
 
 use embassy::util::{select, Either};
 use heapless::Vec;
 
+use crate::descriptor_reader::foreach_endpoint;
+
 use self::control::*;
 use self::descriptor::*;
 use self::driver::{Bus, Driver, Event};
@@ -61,9 +64,6 @@ pub const CONFIGURATION_NONE: u8 = 0;
 /// The bConfiguration value for the single configuration supported by this device.
 pub const CONFIGURATION_VALUE: u8 = 1;
 
-/// The default value for bAlternateSetting for all interfaces.
-pub const DEFAULT_ALTERNATE_SETTING: u8 = 0;
-
 pub const MAX_INTERFACE_COUNT: usize = 4;
 
 /// A handler trait for changes in the device state of the [UsbDevice].
@@ -87,6 +87,12 @@ pub trait DeviceStateHandler {
     fn remote_wakeup_enabled(&self, _enabled: bool) {}
 }
 
+struct Interface<'d> {
+    handler: Option<&'d mut dyn ControlHandler>,
+    current_alt_setting: u8,
+    num_alt_settings: u8,
+}
+
 pub struct UsbDevice<'d, D: Driver<'d>> {
     bus: D::Bus,
     handler: Option<&'d dyn DeviceStateHandler>,
@@ -104,7 +110,7 @@ pub struct UsbDevice<'d, D: Driver<'d>> {
     self_powered: bool,
     pending_address: u8,
 
-    interfaces: Vec<(u8, &'d mut dyn ControlHandler), MAX_INTERFACE_COUNT>,
+    interfaces: Vec<Interface<'d>, MAX_INTERFACE_COUNT>,
 }
 
 impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
@@ -115,7 +121,7 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
         device_descriptor: &'d [u8],
         config_descriptor: &'d [u8],
         bos_descriptor: &'d [u8],
-        interfaces: Vec<(u8, &'d mut dyn ControlHandler), MAX_INTERFACE_COUNT>,
+        interfaces: Vec<Interface<'d>, MAX_INTERFACE_COUNT>,
         control_buf: &'d mut [u8],
     ) -> UsbDevice<'d, D> {
         let control = driver
@@ -247,8 +253,12 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
                 self.remote_wakeup_enabled = false;
                 self.pending_address = 0;
 
-                for (_, h) in self.interfaces.iter_mut() {
-                    h.reset();
+                for iface in self.interfaces.iter_mut() {
+                    iface.current_alt_setting = 0;
+                    if let Some(h) = &mut iface.handler {
+                        h.reset();
+                        h.set_alternate_setting(0);
+                    }
                 }
 
                 if let Some(h) = &self.handler {
@@ -302,7 +312,7 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
                 }
                 (Request::SET_ADDRESS, addr @ 1..=127) => {
                     self.pending_address = addr as u8;
-                    self.bus.set_device_address(self.pending_address);
+                    self.bus.set_address(self.pending_address);
                     self.device_state = UsbDeviceState::Addressed;
                     if let Some(h) = &self.handler {
                         h.addressed(self.pending_address);
@@ -310,59 +320,110 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
                     self.control.accept(stage)
                 }
                 (Request::SET_CONFIGURATION, CONFIGURATION_VALUE_U16) => {
+                    debug!("SET_CONFIGURATION: configured");
                     self.device_state = UsbDeviceState::Configured;
-                    self.bus.set_configured(true);
+
+                    // Enable all endpoints of selected alt settings.
+                    foreach_endpoint(self.config_descriptor, |ep| {
+                        let iface = &self.interfaces[ep.interface as usize];
+                        self.bus.endpoint_set_enabled(
+                            ep.ep_address,
+                            iface.current_alt_setting == ep.interface_alt,
+                        );
+                    })
+                    .unwrap();
+
+                    // Notify handler.
                     if let Some(h) = &self.handler {
                         h.configured(true);
                     }
+
                     self.control.accept(stage)
                 }
                 (Request::SET_CONFIGURATION, CONFIGURATION_NONE_U16) => match self.device_state {
                     UsbDeviceState::Default => self.control.accept(stage),
                     _ => {
+                        debug!("SET_CONFIGURATION: unconfigured");
                         self.device_state = UsbDeviceState::Addressed;
-                        self.bus.set_configured(false);
+
+                        // Disable all endpoints.
+                        foreach_endpoint(self.config_descriptor, |ep| {
+                            self.bus.endpoint_set_enabled(ep.ep_address, false);
+                        })
+                        .unwrap();
+
+                        // Notify handler.
                         if let Some(h) = &self.handler {
                             h.configured(false);
                         }
+
                         self.control.accept(stage)
                     }
                 },
                 _ => self.control.reject(),
             },
+            (RequestType::Standard, Recipient::Interface) => {
+                let iface = match self.interfaces.get_mut(req.index as usize) {
+                    Some(iface) => iface,
+                    None => return self.control.reject(),
+                };
+
+                match req.request {
+                    Request::SET_INTERFACE => {
+                        let new_altsetting = req.value as u8;
+
+                        if new_altsetting >= iface.num_alt_settings {
+                            warn!("SET_INTERFACE: trying to select alt setting out of range.");
+                            return self.control.reject();
+                        }
+
+                        iface.current_alt_setting = new_altsetting;
+
+                        // Enable/disable EPs of this interface as needed.
+                        foreach_endpoint(self.config_descriptor, |ep| {
+                            if ep.interface == req.index as u8 {
+                                self.bus.endpoint_set_enabled(
+                                    ep.ep_address,
+                                    iface.current_alt_setting == ep.interface_alt,
+                                );
+                            }
+                        })
+                        .unwrap();
+
+                        // TODO check it is valid (not out of range)
+                        // TODO actually enable/disable endpoints.
+
+                        if let Some(handler) = &mut iface.handler {
+                            handler.set_alternate_setting(new_altsetting);
+                        }
+                        self.control.accept(stage)
+                    }
+                    _ => self.control.reject(),
+                }
+            }
             (RequestType::Standard, Recipient::Endpoint) => match (req.request, req.value) {
                 (Request::SET_FEATURE, Request::FEATURE_ENDPOINT_HALT) => {
                     let ep_addr = ((req.index as u8) & 0x8f).into();
-                    self.bus.set_stalled(ep_addr, true);
+                    self.bus.endpoint_set_stalled(ep_addr, true);
                     self.control.accept(stage)
                 }
                 (Request::CLEAR_FEATURE, Request::FEATURE_ENDPOINT_HALT) => {
                     let ep_addr = ((req.index as u8) & 0x8f).into();
-                    self.bus.set_stalled(ep_addr, false);
+                    self.bus.endpoint_set_stalled(ep_addr, false);
                     self.control.accept(stage)
                 }
                 _ => self.control.reject(),
             },
-            (_, Recipient::Interface) => {
-                let handler = self
-                    .interfaces
-                    .iter_mut()
-                    .find(|(i, _)| req.index == *i as _)
-                    .map(|(_, h)| h);
-
-                match handler {
-                    Some(handler) => {
-                        let response = match (req.request_type, req.request) {
-                            (RequestType::Standard, Request::SET_INTERFACE) => {
-                                handler.set_interface(req.value)
-                            }
-                            _ => handler.control_out(req, data),
-                        };
-                        match response {
-                            OutResponse::Accepted => self.control.accept(stage),
-                            OutResponse::Rejected => self.control.reject(),
-                        }
-                    }
+            (RequestType::Class, Recipient::Interface) => {
+                let iface = match self.interfaces.get_mut(req.index as usize) {
+                    Some(iface) => iface,
+                    None => return self.control.reject(),
+                };
+                match &mut iface.handler {
+                    Some(handler) => match handler.control_out(req, data) {
+                        OutResponse::Accepted => self.control.accept(stage),
+                        OutResponse::Rejected => self.control.reject(),
+                    },
                     None => self.control.reject(),
                 }
             }
@@ -406,41 +467,54 @@ impl<'d, D: Driver<'d>> UsbDevice<'d, D> {
                 }
                 _ => self.control.reject(),
             },
+            (RequestType::Standard, Recipient::Interface) => {
+                let iface = match self.interfaces.get_mut(req.index as usize) {
+                    Some(iface) => iface,
+                    None => return self.control.reject(),
+                };
+
+                match req.request {
+                    Request::GET_STATUS => {
+                        let status: u16 = 0;
+                        self.control.accept_in(&status.to_le_bytes(), stage).await
+                    }
+                    Request::GET_INTERFACE => {
+                        self.control
+                            .accept_in(&[iface.current_alt_setting], stage)
+                            .await;
+                    }
+                    Request::GET_DESCRIPTOR => match &mut iface.handler {
+                        Some(handler) => match handler.get_descriptor(req, self.control_buf) {
+                            InResponse::Accepted(data) => self.control.accept_in(data, stage).await,
+                            InResponse::Rejected => self.control.reject(),
+                        },
+                        None => self.control.reject(),
+                    },
+                    _ => self.control.reject(),
+                }
+            }
             (RequestType::Standard, Recipient::Endpoint) => match req.request {
                 Request::GET_STATUS => {
                     let ep_addr: EndpointAddress = ((req.index as u8) & 0x8f).into();
                     let mut status: u16 = 0x0000;
-                    if self.bus.is_stalled(ep_addr) {
+                    if self.bus.endpoint_is_stalled(ep_addr) {
                         status |= 0x0001;
                     }
                     self.control.accept_in(&status.to_le_bytes(), stage).await
                 }
                 _ => self.control.reject(),
             },
-            (_, Recipient::Interface) => {
-                let handler = self
-                    .interfaces
-                    .iter_mut()
-                    .find(|(i, _)| req.index == *i as _)
-                    .map(|(_, h)| h);
-
-                match handler {
-                    Some(handler) => {
-                        let response = match (req.request_type, req.request) {
-                            (RequestType::Standard, Request::GET_STATUS) => {
-                                handler.get_status(self.control_buf)
-                            }
-                            (RequestType::Standard, Request::GET_INTERFACE) => {
-                                handler.get_interface(self.control_buf)
-                            }
-                            _ => handler.control_in(req, self.control_buf),
-                        };
-
-                        match response {
-                            InResponse::Accepted(data) => self.control.accept_in(data, stage).await,
-                            InResponse::Rejected => self.control.reject(),
-                        }
-                    }
+            (RequestType::Class, Recipient::Interface) => {
+                let iface = match self.interfaces.get_mut(req.index as usize) {
+                    Some(iface) => iface,
+                    None => return self.control.reject(),
+                };
+
+                match &mut iface.handler {
+                    Some(handler) => match handler.control_in(req, self.control_buf) {
+                        InResponse::Accepted(data) => self.control.accept_in(data, stage).await,
+                        InResponse::Rejected => self.control.reject(),
+                    },
                     None => self.control.reject(),
                 }
             }
diff --git a/embassy-usb/src/types.rs b/embassy-usb/src/types.rs
index 9d00e46cb..b8717ffa9 100644
--- a/embassy-usb/src/types.rs
+++ b/embassy-usb/src/types.rs
@@ -106,7 +106,7 @@ pub struct EndpointInfo {
 /// A handle for a USB interface that contains its number.
 #[derive(Copy, Clone, Eq, PartialEq)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
-pub struct InterfaceNumber(u8);
+pub struct InterfaceNumber(pub(crate) u8);
 
 impl InterfaceNumber {
     pub(crate) fn new(index: u8) -> InterfaceNumber {