devices: compile usb module and update current code

This patch did the following:
    start compile usb module
    fix register macro
    update error handling
    reformat code
    update xhci reg setup to support usb3 and usb2

BUG=chromium:831850
TEST=local build
CQ-DEPEND=CL:1510813
Change-Id: I851cf02d01ae6e988b2628552cf57c1f43aa86c8
Reviewed-on: https://chromium-review.googlesource.com/1510814
Commit-Ready: Jingkui Wang <jkwang@google.com>
Tested-by: kokoro <noreply+kokoro@google.com>
Tested-by: Zach Reizner <zachr@chromium.org>
Reviewed-by: Jingkui Wang <jkwang@google.com>

8 files changed, 479 insertions, 348 deletions

diff --git a/devices/src/lib.rs b/devices/src/lib.rs
index bf631c5..f5ca655 100644
--- a/devices/src/lib.rs
+++ b/devices/src/lib.rs
@@ -27,6 +27,9 @@ extern crate vhost;
 extern crate virtio_sys;
 extern crate vm_control;
 
+#[macro_use]
+mod register_space;
+
 mod bus;
 mod cmos;
 mod i8042;
@@ -34,8 +37,8 @@ mod pci;
 mod pit;
 pub mod pl030;
 mod proxy;
-mod register_space;
 mod serial;
+mod usb;
 mod utils;
 pub mod virtio;
 
diff --git a/devices/src/register_space/register.rs b/devices/src/register_space/register.rs
index 4c02991..abe9bb6 100644
--- a/devices/src/register_space/register.rs
+++ b/devices/src/register_space/register.rs
@@ -172,7 +172,7 @@ where
 #[macro_export]
 macro_rules! static_register {
     (ty: $ty:ty,offset: $offset:expr,value: $value:expr,) => {{
-        use super::*;
+        use register_space::*;
         static REG_SPEC: StaticRegisterSpec<$ty> = StaticRegisterSpec::<$ty> {
             offset: $offset,
             value: $value,
@@ -356,7 +356,7 @@ macro_rules! register {
         guest_writeable_mask: $mask:expr,
         guest_write_1_to_clear_mask: $w1tcm:expr,
     ) => {{
-        use super::*;
+        use register_space::*;
         let spec: RegisterSpec<$ty> = RegisterSpec::<$ty> {
             name: String::from($name),
             offset: $offset,
@@ -367,7 +367,7 @@ macro_rules! register {
         Register::<$ty>::new(spec, $rv)
     }};
     (name: $name:tt, ty: $ty:ty,offset: $offset:expr,reset_value: $rv:expr,) => {{
-        use super::*;
+        use register_space::*;
         let spec: RegisterSpec<$ty> = RegisterSpec::<$ty> {
             name: String::from($name),
             offset: $offset,
@@ -392,7 +392,7 @@ macro_rules! register_array {
         $gwm:expr,guest_write_1_to_clear_mask:
         $gw1tcm:expr,
     ) => {{
-        use super::*;
+        use register_space::*;
         let mut v: Vec<Register<$ty>> = Vec::new();
         for i in 0..$cnt {
             let offset = $base_offset + ($stride * i) as RegisterOffset;
diff --git a/devices/src/usb/log.rs b/devices/src/usb/log.rs
index b086d08..080457c 100644
--- a/devices/src/usb/log.rs
+++ b/devices/src/usb/log.rs
@@ -8,6 +8,5 @@ macro_rules! usb_debug {
     ($($args:tt)+) => {
         // Uncomment the following line to enable logging.
         // debug!($($args)*)
-    }
+    };
 }
-
diff --git a/devices/src/usb/xhci/event_ring.rs b/devices/src/usb/xhci/event_ring.rs
index 0b4048b..836650f 100644
--- a/devices/src/usb/xhci/event_ring.rs
+++ b/devices/src/usb/xhci/event_ring.rs
@@ -4,22 +4,42 @@
 
 use data_model::DataInit;
 use std;
+use std::fmt::{self, Display};
 use std::mem::size_of;
 use std::sync::atomic::{fence, Ordering};
-use sys_util::{GuestAddress, GuestMemory};
+use sys_util::{GuestAddress, GuestMemory, GuestMemoryError};
 
 use super::xhci_abi::*;
 
-#[derive(Debug, PartialEq)]
+#[derive(Debug)]
 pub enum Error {
-    Uninitialized,       // The event ring is uninitialized.
-    InvalidMemoryAccess, // Event ring want to do invalid memory access.
-    InconsistentState,   // Event ring is in a bad state.
-    EventRingFull,       // Event ring is full.
+    Uninitialized,
+    EventRingFull,
+    BadEnqueuePointer(GuestAddress),
+    BadSegTableIndex(u16),
+    BadSegTableAddress(GuestAddress),
+    MemoryRead(GuestMemoryError),
+    MemoryWrite(GuestMemoryError),
 }
 
 type Result<T> = std::result::Result<T, Error>;
 
+impl Display for Error {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        use self::Error::*;
+
+        match self {
+            Uninitialized => write!(f, "event ring is uninitialized"),
+            EventRingFull => write!(f, "event ring is full"),
+            BadEnqueuePointer(addr) => write!(f, "event ring has a bad enqueue pointer: {}", addr),
+            BadSegTableIndex(i) => write!(f, "event ring has a bad seg table index: {}", i),
+            BadSegTableAddress(addr) => write!(f, "event ring has a bad seg table addr: {}", addr),
+            MemoryRead(e) => write!(f, "event ring cannot read from guest memory: {}", e),
+            MemoryWrite(e) => write!(f, "event ring cannot write to guest memory: {}", e),
+        }
+    }
+}
+
 /// Event rings are segmented circular buffers used to pass event TRBs from the xHCI device back to
 /// the guest.  Each event ring is associated with a single interrupter.  See section 4.9.4 of the
 /// xHCI specification for more details.
@@ -37,7 +57,7 @@ pub struct EventRing {
 }
 
 impl EventRing {
-    /// Create an empty, uninited event ring.
+    /// Create an empty, uninitialized event ring.
     pub fn new(mem: GuestMemory) -> Self {
         EventRing {
             mem,
@@ -47,7 +67,7 @@ impl EventRing {
             enqueue_pointer: GuestAddress(0),
             dequeue_pointer: GuestAddress(0),
             trb_count: 0,
-            // As specified in xHCI spec 4.9.4, cycle state should be initilized to 1.
+            // As specified in xHCI spec 4.9.4, cycle state should be initialized to 1.
             producer_cycle_state: true,
         }
     }
@@ -63,7 +83,7 @@ impl EventRing {
         trb.set_cycle_bit(!self.producer_cycle_state);
         self.mem
             .write_obj_at_addr(trb, self.enqueue_pointer)
-            .expect("Fail to write Guest Memory");
+            .map_err(Error::MemoryWrite)?;
 
         // Updating the cycle state bit should always happen after updating other parts.
         fence(Ordering::SeqCst);
@@ -73,15 +93,15 @@ impl EventRing {
             // Offset of cycle state byte.
             const CYCLE_STATE_OFFSET: usize = 12usize;
             let data = trb.as_slice();
-            // Trb contains 4 dwrods, the last one contains cycle bit.
+            // Trb contains 4 dwords, the last one contains cycle bit.
             let cycle_bit_dword = &data[CYCLE_STATE_OFFSET..];
             let address = self.enqueue_pointer;
             let address = address
                 .checked_add(CYCLE_STATE_OFFSET as u64)
-                .expect("unexpected address in event ring");
+                .ok_or(Error::BadEnqueuePointer(self.enqueue_pointer.clone()))?;
             self.mem
-                .write_at_addr(cycle_bit_dword, address)
-                .expect("Fail to write Guest Memory");
+                .write_all_at_addr(cycle_bit_dword, address)
+                .map_err(Error::MemoryWrite)?;
         }
 
         usb_debug!(
@@ -92,7 +112,7 @@ impl EventRing {
         );
         self.enqueue_pointer = match self.enqueue_pointer.checked_add(size_of::<Trb>() as u64) {
             Some(addr) => addr,
-            None => return Err(Error::InconsistentState),
+            None => return Err(Error::BadEnqueuePointer(self.enqueue_pointer.clone())),
         };
         self.trb_count -= 1;
         if self.trb_count == 0 {
@@ -107,17 +127,17 @@ impl EventRing {
     }
 
     /// Set segment table size.
-    pub fn set_seg_table_size(&mut self, size: u16) {
+    pub fn set_seg_table_size(&mut self, size: u16) -> Result<()> {
         usb_debug!("event ring seg table size is set to {}", size);
         self.segment_table_size = size;
-        self.try_reconfigure_event_ring();
+        self.try_reconfigure_event_ring()
     }
 
     /// Set segment table base addr.
-    pub fn set_seg_table_base_addr(&mut self, addr: GuestAddress) {
+    pub fn set_seg_table_base_addr(&mut self, addr: GuestAddress) -> Result<()> {
         usb_debug!("event ring seg table base addr is set to {:#x}", addr.0);
         self.segment_table_base_address = addr;
-        self.try_reconfigure_event_ring();
+        self.try_reconfigure_event_ring()
     }
 
     /// Set dequeue pointer.
@@ -139,8 +159,7 @@ impl EventRing {
     /// Event ring is considered full when there is only space for one last TRB. In this case, xHC
     /// should write an error Trb and do a bunch of handlings. See spec, figure 4-12 for more
     /// details.
-    /// For now, we just check event ring full and panic (as it's unlikely to happen).
-    /// TODO(jkwang) Handle event ring full.
+    /// For now, we just check event ring full and fail (as it's unlikely to happen).
     pub fn is_full(&self) -> Result<bool> {
         if self.trb_count == 1 {
             // erst == event ring segment table
@@ -153,12 +172,11 @@ impl EventRing {
     }
 
     /// Try to init event ring. Will fail if seg table size/address are invalid.
-    fn try_reconfigure_event_ring(&mut self) {
+    fn try_reconfigure_event_ring(&mut self) -> Result<()> {
         if self.segment_table_size == 0 || self.segment_table_base_address.0 == 0 {
-            return;
+            return Ok(());
         }
         self.load_current_seg_table_entry()
-            .expect("Unable to init event ring");
     }
 
     // Check if this event ring is inited.
@@ -186,17 +204,19 @@ impl EventRing {
         // TODO(jkwang) We can refactor GuestMemory to allow in-place memory operation.
         self.mem
             .read_obj_from_addr(seg_table_addr)
-            .map_err(|_e| Error::InvalidMemoryAccess)
+            .map_err(Error::MemoryRead)
     }
 
     // Get seg table addr at index.
     fn get_seg_table_addr(&self, index: u16) -> Result<GuestAddress> {
         if index > self.segment_table_size {
-            return Err(Error::InvalidMemoryAccess);
+            return Err(Error::BadSegTableIndex(index));
         }
         self.segment_table_base_address
             .checked_add(((size_of::<EventRingSegmentTableEntry>() as u16) * index) as u64)
-            .ok_or(Error::InvalidMemoryAccess)
+            .ok_or(Error::BadSegTableAddress(
+                self.segment_table_base_address.clone(),
+            ))
     }
 }
 
@@ -210,9 +230,12 @@ mod test {
         let gm = GuestMemory::new(&vec![(GuestAddress(0), 0x1000)]).unwrap();
         let mut er = EventRing::new(gm.clone());
         let trb = Trb::new();
-        assert_eq!(er.add_event(trb), Err(Error::Uninitialized));
+        match er.add_event(trb).err().unwrap() {
+            Error::Uninitialized => {}
+            _ => panic!("unexpected error"),
+        }
         assert_eq!(er.is_empty(), true);
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.is_full().unwrap(), false);
     }
 
     #[test]
@@ -232,14 +255,16 @@ mod test {
         gm.write_obj_at_addr(
             st_entries[1],
             GuestAddress(0x8 + size_of::<EventRingSegmentTableEntry>() as u64),
-        ).unwrap();
+        )
+        .unwrap();
         gm.write_obj_at_addr(
             st_entries[2],
             GuestAddress(0x8 + 2 * size_of::<EventRingSegmentTableEntry>() as u64),
-        ).unwrap();
+        )
+        .unwrap();
         // Init event ring. Must init after segment tables writting.
-        er.set_seg_table_size(3);
-        er.set_seg_table_base_addr(GuestAddress(0x8));
+        er.set_seg_table_size(3).unwrap();
+        er.set_seg_table_base_addr(GuestAddress(0x8)).unwrap();
         er.set_dequeue_pointer(GuestAddress(0x100));
 
         let mut trb = Trb::new();
@@ -247,17 +272,17 @@ mod test {
         // Fill first table.
         trb.set_control(1);
         assert_eq!(er.is_empty(), true);
-        assert_eq!(er.is_full(), Ok(false));
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.is_full().unwrap(), false);
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm.read_obj_from_addr(GuestAddress(0x100)).unwrap();
         assert_eq!(t.get_control(), 1);
         assert_eq!(t.get_cycle(), 1);
 
         trb.set_control(2);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x100 + trb_size))
@@ -266,8 +291,8 @@ mod test {
         assert_eq!(t.get_cycle(), 1);
 
         trb.set_control(3);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x100 + 2 * trb_size))
@@ -277,16 +302,16 @@ mod test {
 
         // Fill second table.
         trb.set_control(4);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm.read_obj_from_addr(GuestAddress(0x200)).unwrap();
         assert_eq!(t.get_control(), 4);
         assert_eq!(t.get_cycle(), 1);
 
         trb.set_control(5);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x200 + trb_size))
@@ -295,8 +320,8 @@ mod test {
         assert_eq!(t.get_cycle(), 1);
 
         trb.set_control(6);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x200 + 2 * trb_size as u64))
@@ -306,17 +331,17 @@ mod test {
 
         // Fill third table.
         trb.set_control(7);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm.read_obj_from_addr(GuestAddress(0x300)).unwrap();
         assert_eq!(t.get_control(), 7);
         assert_eq!(t.get_cycle(), 1);
 
         trb.set_control(8);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
         // There is only one last trb. Considered full.
-        assert_eq!(er.is_full(), Ok(true));
+        assert_eq!(er.is_full().unwrap(), true);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x300 + trb_size))
@@ -325,18 +350,21 @@ mod test {
         assert_eq!(t.get_cycle(), 1);
 
         // Add the last trb will result in error.
-        assert_eq!(er.add_event(trb.clone()), Err(Error::EventRingFull));
+        match er.add_event(trb.clone()) {
+            Err(Error::EventRingFull) => {}
+            _ => panic!("er should be full"),
+        };
 
         // Dequeue one trb.
         er.set_dequeue_pointer(GuestAddress(0x100 + trb_size));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
 
         // Fill the last trb of the third table.
         trb.set_control(9);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
         // There is only one last trb. Considered full.
-        assert_eq!(er.is_full(), Ok(true));
+        assert_eq!(er.is_full().unwrap(), true);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x300 + trb_size))
@@ -345,17 +373,20 @@ mod test {
         assert_eq!(t.get_cycle(), 1);
 
         // Add the last trb will result in error.
-        assert_eq!(er.add_event(trb.clone()), Err(Error::EventRingFull));
+        match er.add_event(trb.clone()) {
+            Err(Error::EventRingFull) => {}
+            _ => panic!("er should be full"),
+        };
 
         // Dequeue until empty.
         er.set_dequeue_pointer(GuestAddress(0x100));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), true);
 
         // Fill first table again.
         trb.set_control(10);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm.read_obj_from_addr(GuestAddress(0x100)).unwrap();
         assert_eq!(t.get_control(), 10);
@@ -363,8 +394,8 @@ mod test {
         assert_eq!(t.get_cycle(), 0);
 
         trb.set_control(11);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x100 + trb_size))
@@ -373,8 +404,8 @@ mod test {
         assert_eq!(t.get_cycle(), 0);
 
         trb.set_control(12);
-        assert_eq!(er.add_event(trb.clone()), Ok(()));
-        assert_eq!(er.is_full(), Ok(false));
+        assert_eq!(er.add_event(trb.clone()).unwrap(), ());
+        assert_eq!(er.is_full().unwrap(), false);
         assert_eq!(er.is_empty(), false);
         let t: Trb = gm
             .read_obj_from_addr(GuestAddress(0x100 + 2 * trb_size))
diff --git a/devices/src/usb/xhci/mod.rs b/devices/src/usb/xhci/mod.rs
index 5429ec6..4fac7a7 100644
--- a/devices/src/usb/xhci/mod.rs
+++ b/devices/src/usb/xhci/mod.rs
@@ -2,14 +2,7 @@
 // Use of this source code is governed by a BSD-style license that can be
 // found in the LICENSE file.
 
-#[macro_use]
-mod mmio_register;
-mod mmio_space;
-
 mod event_ring;
-#[allow(dead_code)]
 mod xhci_abi;
-#[allow(dead_code)]
 mod xhci_abi_schema;
-#[allow(dead_code)]
 mod xhci_regs;
diff --git a/devices/src/usb/xhci/xhci_abi.rs b/devices/src/usb/xhci/xhci_abi.rs
index 16ee663..b7a96e8 100644
--- a/devices/src/usb/xhci/xhci_abi.rs
+++ b/devices/src/usb/xhci/xhci_abi.rs
@@ -4,7 +4,7 @@
 
 pub use super::xhci_abi_schema::*;
 use data_model::DataInit;
-use std;
+use std::fmt::{self, Display};
 
 unsafe impl DataInit for Trb {}
 unsafe impl DataInit for NormalTrb {}
@@ -140,137 +140,164 @@ impl TypedTrb for PortStatusChangeEventTrb {
     const TY: TrbType = TrbType::PortStatusChangeEvent;
 }
 
+#[derive(Debug, PartialEq)]
+pub enum Error {
+    UnknownTrbType(u8),
+    UnknownCompletionCode(u8),
+    UnknownDeviceSlotState(u8),
+    UnknownEndpointState(u8),
+    CannotCastTrb,
+}
+
+type Result<T> = std::result::Result<T, Error>;
+
+impl Display for Error {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        use self::Error::*;
+
+        match self {
+            UnknownTrbType(v) => write!(f, "we got an unknown trb type value: {}", v),
+            UnknownCompletionCode(v) => write!(f, "we got an unknown trb completion code: {}", v),
+            UnknownDeviceSlotState(v) => write!(f, "we got and unknown device slot state: {}", v),
+            UnknownEndpointState(v) => write!(f, "we got and unknown endpoint state: {}", v),
+            CannotCastTrb => write!(f, "cannot cast trb from raw memory"),
+        }
+    }
+}
+
 /// All trb structs have the same size. One trb could be safely casted to another, though the
 /// values might be invalid.
 pub unsafe trait TrbCast: DataInit + TypedTrb {
-    fn cast<T: TrbCast>(&self) -> &T {
-        T::from_slice(self.as_slice()).unwrap()
+    fn cast<T: TrbCast>(&self) -> Result<&T> {
+        T::from_slice(self.as_slice()).ok_or(Error::CannotCastTrb)
     }
 
-    fn cast_mut<T: TrbCast>(&mut self) -> &mut T {
-        T::from_mut_slice(self.as_mut_slice()).unwrap()
+    fn cast_mut<T: TrbCast>(&mut self) -> Result<&mut T> {
+        T::from_mut_slice(self.as_mut_slice()).ok_or(Error::CannotCastTrb)
     }
 
-    fn checked_cast<T: TrbCast>(&self) -> Option<&T> {
+    fn checked_cast<T: TrbCast>(&self) -> Result<&T> {
         if Trb::from_slice(self.as_slice())
-            .unwrap()
-            .trb_type()
-            .unwrap()
+            .ok_or(Error::CannotCastTrb)?
+            .trb_type()?
             != T::TY
         {
-            return None;
+            return Err(Error::CannotCastTrb);
         }
-        Some(T::from_slice(self.as_slice()).unwrap())
+        T::from_slice(self.as_slice()).ok_or(Error::CannotCastTrb)
     }
 
-    fn checked_mut_cast<T: TrbCast>(&mut self) -> Option<&mut T> {
+    fn checked_mut_cast<T: TrbCast>(&mut self) -> Result<&mut T> {
         if Trb::from_slice(self.as_slice())
-            .unwrap()
-            .trb_type()
-            .unwrap()
+            .ok_or(Error::CannotCastTrb)?
+            .trb_type()?
             != T::TY
         {
-            return None;
+            return Err(Error::CannotCastTrb);
         }
-        Some(T::from_mut_slice(self.as_mut_slice()).unwrap())
+        T::from_mut_slice(self.as_mut_slice()).ok_or(Error::CannotCastTrb)
     }
 }
 
 impl Trb {
-    /// Get trb type.
-    pub fn trb_type(&self) -> Option<TrbType> {
-        TrbType::from_raw(self.get_trb_type())
-    }
-
-    /// Get debug string, the string will be printed with correct trb type and
-    /// fields.
-    pub fn debug_str(&self) -> String {
-        let trb_type = match self.trb_type() {
-            None => return format!("unexpected trb type: {:?}", self),
-            Some(t) => t,
-        };
-        match trb_type {
-            TrbType::Reserved => format!("reserved trb type"),
+    fn fmt_helper(&self, f: &mut fmt::Formatter) -> Result<fmt::Result> {
+        match self.trb_type()? {
+            TrbType::Reserved => Ok(write!(f, "reserved trb type")),
             TrbType::Normal => {
-                let t = self.cast::<NormalTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<NormalTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::SetupStage => {
-                let t = self.cast::<SetupStageTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<SetupStageTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::DataStage => {
-                let t = self.cast::<DataStageTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<DataStageTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::StatusStage => {
-                let t = self.cast::<StatusStageTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<StatusStageTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::Isoch => {
-                let t = self.cast::<IsochTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<IsochTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::Link => {
-                let t = self.cast::<LinkTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<LinkTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::EventData => {
-                let t = self.cast::<EventDataTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<EventDataTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::Noop => {
-                let t = self.cast::<NoopTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<NoopTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
-            TrbType::EnableSlotCommand => format!("trb: enable slot command {:?}", self),
+            TrbType::EnableSlotCommand => Ok(write!(f, "trb: enable slot command {:?}", self)),
             TrbType::DisableSlotCommand => {
-                let t = self.cast::<DisableSlotCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<DisableSlotCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::AddressDeviceCommand => {
-                let t = self.cast::<AddressDeviceCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<AddressDeviceCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::ConfigureEndpointCommand => {
-                let t = self.cast::<ConfigureEndpointCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<ConfigureEndpointCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::EvaluateContextCommand => {
-                let t = self.cast::<EvaluateContextCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<EvaluateContextCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::ResetEndpointCommand => {
-                let t = self.cast::<ResetEndpointCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<ResetEndpointCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::StopEndpointCommand => {
-                let t = self.cast::<StopEndpointCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<StopEndpointCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::SetTRDequeuePointerCommand => {
-                let t = self.cast::<SetTRDequeuePointerCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<SetTRDequeuePointerCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::ResetDeviceCommand => {
-                let t = self.cast::<ResetDeviceCommandTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<ResetDeviceCommandTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
-            TrbType::NoopCommand => format!("trb: noop command {:?}", self),
+            TrbType::NoopCommand => Ok(write!(f, "trb: noop command {:?}", self)),
             TrbType::TransferEvent => {
-                let t = self.cast::<TransferEventTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<TransferEventTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::CommandCompletionEvent => {
-                let t = self.cast::<CommandCompletionEventTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<CommandCompletionEventTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
             TrbType::PortStatusChangeEvent => {
-                let t = self.cast::<PortStatusChangeEventTrb>();
-                format!("trb: {:?}", t)
+                let t = self.cast::<PortStatusChangeEventTrb>()?;
+                Ok(write!(f, "trb: {:?}", t))
             }
         }
     }
+}
+impl Display for Trb {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        match self.fmt_helper(f) {
+            Ok(f) => f,
+            Err(e) => write!(f, "fail to format trb {}", e),
+        }
+    }
+}
+
+impl Trb {
+    /// Get trb type.
+    pub fn trb_type(&self) -> Result<TrbType> {
+        TrbType::from_raw(self.get_trb_type())
+    }
 
     /// Get cycle bit.
     pub fn get_cycle_bit(&self) -> bool {
@@ -286,17 +313,17 @@ impl Trb {
     }
 
     /// Get chain bit.
-    pub fn get_chain_bit(&self) -> bool {
-        match self.trb_type() {
-            Some(TrbType::Normal) => self.cast::<NormalTrb>().get_chain() != 0,
-            Some(TrbType::DataStage) => self.cast::<DataStageTrb>().get_chain() != 0,
-            Some(TrbType::StatusStage) => self.cast::<StatusStageTrb>().get_chain() != 0,
-            Some(TrbType::Isoch) => self.cast::<IsochTrb>().get_chain() != 0,
-            Some(TrbType::Noop) => self.cast::<NoopTrb>().get_chain() != 0,
-            Some(TrbType::Link) => self.cast::<LinkTrb>().get_chain() != 0,
-            Some(TrbType::EventData) => self.cast::<EventDataTrb>().get_chain() != 0,
+    pub fn get_chain_bit(&self) -> Result<bool> {
+        Ok(match self.trb_type() {
+            Ok(TrbType::Normal) => self.cast::<NormalTrb>()?.get_chain() != 0,
+            Ok(TrbType::DataStage) => self.cast::<DataStageTrb>()?.get_chain() != 0,
+            Ok(TrbType::StatusStage) => self.cast::<StatusStageTrb>()?.get_chain() != 0,
+            Ok(TrbType::Isoch) => self.cast::<IsochTrb>()?.get_chain() != 0,
+            Ok(TrbType::Noop) => self.cast::<NoopTrb>()?.get_chain() != 0,
+            Ok(TrbType::Link) => self.cast::<LinkTrb>()?.get_chain() != 0,
+            Ok(TrbType::EventData) => self.cast::<EventDataTrb>()?.get_chain() != 0,
             _ => false,
-        }
+        })
     }
 
     /// Get interrupt target.
@@ -306,8 +333,8 @@ impl Trb {
     }
 
     /// Only some of trb types could appear in transfer ring.
-    pub fn can_be_in_transfer_ring(&self) -> bool {
-        match self.trb_type().unwrap() {
+    pub fn can_be_in_transfer_ring(&self) -> Result<bool> {
+        match self.trb_type()? {
             TrbType::Normal
             | TrbType::SetupStage
             | TrbType::DataStage
@@ -315,19 +342,19 @@ impl Trb {
             | TrbType::Isoch
             | TrbType::Link
             | TrbType::EventData
-            | TrbType::Noop => true,
-            _ => false,
+            | TrbType::Noop => Ok(true),
+            _ => Ok(false),
         }
     }
 
     /// Length of this transfer.
-    pub fn transfer_length(&self) -> u32 {
+    pub fn transfer_length(&self) -> Result<u32> {
         const STATUS_TRANSFER_LENGTH_MASK: u32 = 0x1ffff;
-        match self.trb_type().unwrap() {
+        match self.trb_type()? {
             TrbType::Normal | TrbType::SetupStage | TrbType::DataStage | TrbType::Isoch => {
-                self.get_status() & STATUS_TRANSFER_LENGTH_MASK
+                Ok(self.get_status() & STATUS_TRANSFER_LENGTH_MASK)
             }
-            _ => 0,
+            _ => Ok(0),
         }
     }
 
@@ -338,20 +365,47 @@ impl Trb {
     }
 
     /// Returns true if this trb is immediate data.
-    pub fn immediate_data(&self) -> bool {
+    pub fn immediate_data(&self) -> Result<bool> {
         const FLAGS_IMMEDIATE_DATA_MASK: u16 = 0x20;
-        match self.trb_type().unwrap() {
+        match self.trb_type()? {
             TrbType::Normal | TrbType::SetupStage | TrbType::DataStage | TrbType::Isoch => {
-                (self.get_flags() & FLAGS_IMMEDIATE_DATA_MASK) != 0
+                Ok((self.get_flags() & FLAGS_IMMEDIATE_DATA_MASK) != 0)
             }
-            _ => false,
+            _ => Ok(false),
         }
     }
 }
 
+impl LinkTrb {
+    /// Get cycle.
+    pub fn get_cycle_bit(&self) -> bool {
+        self.get_cycle() != 0
+    }
+
+    /// Get toggle cycle.
+    pub fn get_toggle_cycle_bit(&self) -> bool {
+        self.get_toggle_cycle() != 0
+    }
+
+    /// set chain status.
+    pub fn set_chain_bit(&mut self, v: bool) {
+        self.set_chain(v as u8);
+    }
+
+    /// Get chain status.
+    pub fn get_chain_bit(&self) -> bool {
+        self.get_chain() != 0
+    }
+
+    /// Get interrupt on completion.
+    pub fn interrupt_on_completion(&self) -> bool {
+        self.get_interrupt_on_completion() != 0
+    }
+}
+
 /// Trait for enum that could be converted from raw u8.
-pub trait PrimitiveEnum: Sized {
-    fn from_raw(val: u8) -> Option<Self>;
+pub trait PrimitiveTrbEnum: Sized {
+    fn from_raw(val: u8) -> Result<Self>;
 }
 
 /// All kinds of trb.
@@ -381,32 +435,32 @@ pub enum TrbType {
     PortStatusChangeEvent = 34,
 }
 
-impl PrimitiveEnum for TrbType {
-    fn from_raw(val: u8) -> Option<Self> {
+impl PrimitiveTrbEnum for TrbType {
+    fn from_raw(val: u8) -> Result<Self> {
         match val {
-            0 => Some(TrbType::Reserved),
-            1 => Some(TrbType::Normal),
-            2 => Some(TrbType::SetupStage),
-            3 => Some(TrbType::DataStage),
-            4 => Some(TrbType::StatusStage),
-            5 => Some(TrbType::Isoch),
-            6 => Some(TrbType::Link),
-            7 => Some(TrbType::EventData),
-            8 => Some(TrbType::Noop),
-            9 => Some(TrbType::EnableSlotCommand),
-            10 => Some(TrbType::DisableSlotCommand),
-            11 => Some(TrbType::AddressDeviceCommand),
-            12 => Some(TrbType::ConfigureEndpointCommand),
-            13 => Some(TrbType::EvaluateContextCommand),
-            14 => Some(TrbType::ResetEndpointCommand),
-            15 => Some(TrbType::StopEndpointCommand),
-            16 => Some(TrbType::SetTRDequeuePointerCommand),
-            17 => Some(TrbType::ResetDeviceCommand),
-            23 => Some(TrbType::NoopCommand),
-            32 => Some(TrbType::TransferEvent),
-            33 => Some(TrbType::CommandCompletionEvent),
-            34 => Some(TrbType::PortStatusChangeEvent),
-            _ => None,
+            0 => Ok(TrbType::Reserved),
+            1 => Ok(TrbType::Normal),
+            2 => Ok(TrbType::SetupStage),
+            3 => Ok(TrbType::DataStage),
+            4 => Ok(TrbType::StatusStage),
+            5 => Ok(TrbType::Isoch),
+            6 => Ok(TrbType::Link),
+            7 => Ok(TrbType::EventData),
+            8 => Ok(TrbType::Noop),
+            9 => Ok(TrbType::EnableSlotCommand),
+            10 => Ok(TrbType::DisableSlotCommand),
+            11 => Ok(TrbType::AddressDeviceCommand),
+            12 => Ok(TrbType::ConfigureEndpointCommand),
+            13 => Ok(TrbType::EvaluateContextCommand),
+            14 => Ok(TrbType::ResetEndpointCommand),
+            15 => Ok(TrbType::StopEndpointCommand),
+            16 => Ok(TrbType::SetTRDequeuePointerCommand),
+            17 => Ok(TrbType::ResetDeviceCommand),
+            23 => Ok(TrbType::NoopCommand),
+            32 => Ok(TrbType::TransferEvent),
+            33 => Ok(TrbType::CommandCompletionEvent),
+            34 => Ok(TrbType::PortStatusChangeEvent),
+            v => Err(Error::UnknownTrbType(v)),
         }
     }
 }
@@ -422,17 +476,17 @@ pub enum TrbCompletionCode {
     ContextStateError = 19,
 }
 
-impl PrimitiveEnum for TrbCompletionCode {
-    fn from_raw(val: u8) -> Option<Self> {
+impl PrimitiveTrbEnum for TrbCompletionCode {
+    fn from_raw(val: u8) -> Result<Self> {
         match val {
-            1 => Some(TrbCompletionCode::Success),
-            4 => Some(TrbCompletionCode::TransactionError),
-            5 => Some(TrbCompletionCode::TrbError),
-            9 => Some(TrbCompletionCode::NoSlotsAvailableError),
-            11 => Some(TrbCompletionCode::SlotNotEnabledError),
-            13 => Some(TrbCompletionCode::ShortPacket),
-            19 => Some(TrbCompletionCode::ContextStateError),
-            _ => None,
+            1 => Ok(TrbCompletionCode::Success),
+            4 => Ok(TrbCompletionCode::TransactionError),
+            5 => Ok(TrbCompletionCode::TrbError),
+            9 => Ok(TrbCompletionCode::NoSlotsAvailableError),
+            11 => Ok(TrbCompletionCode::SlotNotEnabledError),
+            13 => Ok(TrbCompletionCode::ShortPacket),
+            19 => Ok(TrbCompletionCode::ContextStateError),
+            v => Err(Error::UnknownCompletionCode(v)),
         }
     }
 }
@@ -448,21 +502,21 @@ pub enum DeviceSlotState {
     Configured = 3,
 }
 
-impl PrimitiveEnum for DeviceSlotState {
-    fn from_raw(val: u8) -> Option<Self> {
+impl PrimitiveTrbEnum for DeviceSlotState {
+    fn from_raw(val: u8) -> Result<Self> {
         match val {
-            0 => Some(DeviceSlotState::DisabledOrEnabled),
-            1 => Some(DeviceSlotState::Default),
-            2 => Some(DeviceSlotState::Addressed),
-            3 => Some(DeviceSlotState::Configured),
-            _ => None,
+            0 => Ok(DeviceSlotState::DisabledOrEnabled),
+            1 => Ok(DeviceSlotState::Default),
+            2 => Ok(DeviceSlotState::Addressed),
+            3 => Ok(DeviceSlotState::Configured),
+            v => Err(Error::UnknownDeviceSlotState(v)),
         }
     }
 }
 
 impl SlotContext {
     /// Set slot context state.
-    pub fn state(&self) -> Option<DeviceSlotState> {
+    pub fn state(&self) -> Result<DeviceSlotState> {
         DeviceSlotState::from_raw(self.get_slot_state())
     }
 
@@ -478,19 +532,19 @@ pub enum EndpointState {
     Running = 1,
 }
 
-impl PrimitiveEnum for EndpointState {
-    fn from_raw(val: u8) -> Option<Self> {
+impl PrimitiveTrbEnum for EndpointState {
+    fn from_raw(val: u8) -> Result<Self> {
         match val {
-            0 => Some(EndpointState::Disabled),
-            1 => Some(EndpointState::Running),
-            _ => None,
+            0 => Ok(EndpointState::Disabled),
+            1 => Ok(EndpointState::Running),
+            v => Err(Error::UnknownEndpointState(v)),
         }
     }
 }
 
 impl EndpointContext {
     /// Get endpoint context state.
-    pub fn state(&self) -> Option<EndpointState> {
+    pub fn state(&self) -> Result<EndpointState> {
         EndpointState::from_raw(self.get_endpoint_state())
     }
 
diff --git a/devices/src/usb/xhci/xhci_abi_schema.rs b/devices/src/usb/xhci/xhci_abi_schema.rs
index 8174ce8..91ca7c1 100644
--- a/devices/src/usb/xhci/xhci_abi_schema.rs
+++ b/devices/src/usb/xhci/xhci_abi_schema.rs
@@ -484,7 +484,7 @@ pub struct DeviceContext {
 /// POD struct associates a TRB with its address in guest memory.  This is
 /// useful because transfer and command completion event TRBs must contain
 /// pointers to the original TRB that generated the event.
-#[derive(Clone, Copy)]
+#[derive(Clone, Copy, Debug, PartialEq)]
 pub struct AddressedTrb {
     pub trb: Trb,
     pub gpa: u64,
diff --git a/devices/src/usb/xhci/xhci_regs.rs b/devices/src/usb/xhci/xhci_regs.rs
index c7881c7..a5ea3b2 100644
--- a/devices/src/usb/xhci/xhci_regs.rs
+++ b/devices/src/usb/xhci/xhci_regs.rs
@@ -2,14 +2,25 @@
 // Use of this source code is governed by a BSD-style license that can be
 // found in the LICENSE file.
 
-use register_space::RegisterSpace;
+use register_space::{Register, RegisterSpace};
 
 /// Max interrupter number.
 pub const MAX_INTERRUPTER: u8 = 1;
 /// For port configuration, see register HCSPARAMS1, spcap1.3 and spcap2.3.
-pub const MAX_SLOTS: u8 = 8;
-/// Max port number.
-pub const MAX_PORTS: u8 = 8;
+pub const MAX_SLOTS: u8 = 16;
+
+/// Usb 2 ports start from port number 0.
+pub const USB2_PORTS_START: u8 = 0;
+/// Last usb 2 ports is 7.
+pub const USB2_PORTS_END: u8 = 8;
+/// Usb 3 ports start from port number 8.
+pub const USB3_PORTS_START: u8 = 8;
+/// Last usb 3 port is 15.
+pub const USB3_PORTS_END: u8 = 16;
+
+/// Max port number. Review the following before changing this:
+///     HCSPARAMS1, portsc, spcap1.3 and spcap2.3.
+pub const MAX_PORTS: u8 = USB3_PORTS_END;
 
 /// Cap register length.
 pub const XHCI_CAPLENGTH: u8 = 0x20;
@@ -135,173 +146,173 @@ pub struct XhciRegs {
 
 /// This function returns mmio space definition for xhci. See Xhci spec chapter 5
 /// for details.
-pub fn init_xhci_mmio_space_and_regs() -> (MMIOSpace, XhciRegs) {
-    let mut mmio = MMIOSpace::new();
+pub fn init_xhci_mmio_space_and_regs() -> (RegisterSpace, XhciRegs) {
+    let mut mmio = RegisterSpace::new();
 
     /* Host Controller Capability Registers */
     mmio.add_register(
         // CAPLENGTH
         static_register!(
-            ty: u8,
-            offset: 0x00,
-            value: XHCI_CAPLENGTH, // Operation register start at offset 0x20
-            ),
+        ty: u8,
+        offset: 0x00,
+        value: XHCI_CAPLENGTH, // Operation register start at offset 0x20
+        ),
     );
     mmio.add_register(
         // HCIVERSION
         static_register!(
-            ty: u16,
-            offset: 0x02,
-            value: 0x0110,// Revision 1.1
-            ),
+        ty: u16,
+        offset: 0x02,
+        value: 0x0110,// Revision 1.1
+        ),
     );
     mmio.add_register(
         // HCSPARAMS1
         static_register!(
-            ty: u32,
-            offset: 0x04,
-            value: 0x08000108, // max_slots = 8, max_interrupters = 1, max_ports = 8
-            ),
+        ty: u32,
+        offset: 0x04,
+        value: 0x10000110, // max_slots = 16, max_interrupters = 1, max_ports = 16
+        ),
     );
 
     mmio.add_register(
         // HCSPARAMS2
         static_register!(
-            ty: u32,
-            offset: 0x08,
-            // Maximum number of event ring segment table entries = 32k
-            // No scratchpad buffers.
-            value: 0xf0,
-            ),
+        ty: u32,
+        offset: 0x08,
+        // Maximum number of event ring segment table entries = 32k
+        // No scratchpad buffers.
+        value: 0xf0,
+        ),
     );
 
     mmio.add_register(
         // HCSPARAM3
         static_register!(
-            ty: u32,
-            offset: 0x0c,
-
-            // Exit latencies for U1 (standby with fast exit) and U2 (standby with
-            // slower exit) power states. We use the max values:
-            // - U1 to U0: < 10 us
-            // - U2 to U1: < 2047 us
-            value: 0x07FF000A,
-            ),
+        ty: u32,
+        offset: 0x0c,
+
+        // Exit latencies for U1 (standby with fast exit) and U2 (standby with
+        // slower exit) power states. We use the max values:
+        // - U1 to U0: < 10 us
+        // - U2 to U1: < 2047 us
+        value: 0x07FF000A,
+        ),
     );
 
     mmio.add_register(
         // HCCPARAMS1
         static_register!(
-            ty: u32,
-            offset: 0x10,
-            // Supports 64 bit addressing
-            // Max primary stream array size = 0 (streams not supported).
-            // Extended capabilities pointer = 0xC000 offset from base.
-            value: 0x30000501,
-            ),
+        ty: u32,
+        offset: 0x10,
+        // Supports 64 bit addressing
+        // Max primary stream array size = 0 (streams not supported).
+        // Extended capabilities pointer = 0xC000 offset from base.
+        value: 0x30000501,
+        ),
     );
     mmio.add_register(
         // DBOFF
         static_register!(
-            ty: u32,
-            offset: 0x14,
-            value: XHCI_DBOFF, // Doorbell array offset 0x2000 from base.
-            ),
+        ty: u32,
+        offset: 0x14,
+        value: XHCI_DBOFF, // Doorbell array offset 0x2000 from base.
+        ),
     );
 
     mmio.add_register(
         // RTSOFF
         static_register!(
-            ty: u32,
-            offset: 0x18,
-            value: XHCI_RTSOFF, // Runtime registers offset 0x3000 from base.
-            ),
+        ty: u32,
+        offset: 0x18,
+        value: XHCI_RTSOFF, // Runtime registers offset 0x3000 from base.
+        ),
     );
 
     mmio.add_register(
         // HCCPARAMS2
         static_register!(
-            ty: u32,
-            offset: 0x1c,
-            value: 0,
-            ),
+        ty: u32,
+        offset: 0x1c,
+        value: 0,
+        ),
     );
     /* End of Host Controller Capability Registers */
 
     /* Host Controller Operational Registers */
     let usbcmd = register!(
-            name: "usbcmd",
-            ty: u32,
-            offset: 0x20,
-            reset_value: 0,
-            guest_writeable_mask: 0x00002F0F,
-            guest_write_1_to_clear_mask: 0,
-        );
+        name: "usbcmd",
+        ty: u32,
+        offset: 0x20,
+        reset_value: 0,
+        guest_writeable_mask: 0x00002F0F,
+        guest_write_1_to_clear_mask: 0,
+    );
     mmio.add_register(usbcmd.clone());
 
     let usbsts = register!(
-            name: "usbsts",
-            ty: u32,
-            offset: 0x24,
-            reset_value: 0x00000001,
-            guest_writeable_mask: 0x0000041C,
-            guest_write_1_to_clear_mask: 0x0000041C,
-        );
+        name: "usbsts",
+        ty: u32,
+        offset: 0x24,
+        reset_value: 0x00000001,
+        guest_writeable_mask: 0x0000041C,
+        guest_write_1_to_clear_mask: 0x0000041C,
+    );
     mmio.add_register(usbsts.clone());
 
     mmio.add_register(
         //  Pagesize
         static_register!(
-            ty: u32,
-            offset: 0x28,
-            value: 0x00000001,
-            ),
+        ty: u32,
+        offset: 0x28,
+        value: 0x00000001,
+        ),
     );
 
     let dnctrl = register!(
-            name: "dnctrl",
-            ty: u32,
-            offset: 0x34,
-            reset_value: 0,
-            guest_writeable_mask: 0x0000FFFF,
-            guest_write_1_to_clear_mask: 0,
-        );
+        name: "dnctrl",
+        ty: u32,
+        offset: 0x34,
+        reset_value: 0,
+        guest_writeable_mask: 0x0000FFFF,
+        guest_write_1_to_clear_mask: 0,
+    );
     mmio.add_register(dnctrl.clone());
 
     let crcr = register!(
-            name: "crcr",
-            ty: u64,
-            offset: 0x38,
-            reset_value: 9,
-            guest_writeable_mask: 0xFFFFFFFFFFFFFFC7,
-            guest_write_1_to_clear_mask: 0,
-        );
+        name: "crcr",
+        ty: u64,
+        offset: 0x38,
+        reset_value: 9,
+        guest_writeable_mask: 0xFFFFFFFFFFFFFFC7,
+        guest_write_1_to_clear_mask: 0,
+    );
     mmio.add_register(crcr.clone());
 
     let dcbaap = register!(
-            name: "dcbaap",
-            ty: u64,
-            offset: 0x50,
-            reset_value: 0x0,
-            guest_writeable_mask: 0xFFFFFFFFFFFFFFC0,
-            guest_write_1_to_clear_mask: 0,
-        );
+        name: "dcbaap",
+        ty: u64,
+        offset: 0x50,
+        reset_value: 0x0,
+        guest_writeable_mask: 0xFFFFFFFFFFFFFFC0,
+        guest_write_1_to_clear_mask: 0,
+    );
     mmio.add_register(dcbaap.clone());
 
     let config = register!(
-            name: "config",
-            ty: u64,
-            offset: 0x58,
-            reset_value: 0,
-            guest_writeable_mask: 0x0000003F,
-            guest_write_1_to_clear_mask: 0,
-        );
+        name: "config",
+        ty: u64,
+        offset: 0x58,
+        reset_value: 0,
+        guest_writeable_mask: 0x0000003F,
+        guest_write_1_to_clear_mask: 0,
+    );
     mmio.add_register(config.clone());
 
     let portsc = register_array!(
         name: "portsc",
         ty: u32,
-        cnt: 8, //  Must be equal to max_ports
+        cnt: MAX_PORTS,
         base_offset: 0x420,
         stride: 16,
         reset_value: 0x000002A0,
@@ -313,7 +324,7 @@ pub fn init_xhci_mmio_space_and_regs() -> (MMIOSpace, XhciRegs) {
     mmio.add_register_array(&register_array!(
             name: "portpmsc",
             ty: u32,
-            cnt: 8,
+            cnt: MAX_PORTS,
             base_offset: 0x424,
             stride: 16,
             reset_value: 0,
@@ -324,7 +335,7 @@ pub fn init_xhci_mmio_space_and_regs() -> (MMIOSpace, XhciRegs) {
     mmio.add_register_array(&register_array!(
             name: "portli",
             ty: u32,
-            cnt: 8,
+            cnt: MAX_PORTS,
             base_offset: 0x428,
             stride: 16,
             reset_value: 0,
@@ -335,7 +346,7 @@ pub fn init_xhci_mmio_space_and_regs() -> (MMIOSpace, XhciRegs) {
     mmio.add_register_array(&register_array!(
             name: "porthlpmc",
             ty: u32,
-            cnt: 8,
+            cnt: MAX_PORTS,
             base_offset: 0x42c,
             stride: 16,
             reset_value: 0,
@@ -345,7 +356,7 @@ pub fn init_xhci_mmio_space_and_regs() -> (MMIOSpace, XhciRegs) {
     let doorbells = register_array!(
         name: "doorbell",
         ty: u32,
-        cnt: 9, //  Must be equal to max_ports
+        cnt: MAX_SLOTS + 1, //  Must be equal to max_slots + 1
         base_offset: 0x2000,
         stride: 4,
         reset_value: 0,
@@ -358,10 +369,10 @@ pub fn init_xhci_mmio_space_and_regs() -> (MMIOSpace, XhciRegs) {
     mmio.add_register(
         // mfindex
         static_register!(
-            ty: u32,
-            offset: 0x3000,
-            value: 0, // 4 ports starting at port 5
-            ),
+        ty: u32,
+        offset: 0x3000,
+        value: 0, // 4 ports starting at port 5
+        ),
     );
 
     /* Reg Array for interrupters */
@@ -440,41 +451,81 @@ pub fn init_xhci_mmio_space_and_regs() -> (MMIOSpace, XhciRegs) {
     mmio.add_register(
         // spcap 1.1
         static_register!(
-            ty: u32,
-            offset: 0xc000,
-            // "Supported Protocol" capability.
-            // Not next capability.
-            // USB 2.0. Revision 2.0.
-            value: 0x02000002,
-            ),
+        ty: u32,
+        offset: 0xc000,
+        // "Supported Protocol" capability.
+        // Next capability starts after 0x40 dwords.
+        // USB 2.0. Revision 2.0.
+        value: 0x02004002,
+        ),
     );
     mmio.add_register(
         // spcap 1.2
         static_register!(
-            ty: u32,
-            offset: 0xc004,
-            value: 0x20425355, // Name string = "USB "
-            ),
+        ty: u32,
+        offset: 0xc004,
+        value: 0x20425355, // Name string = "USB "
+        ),
     );
     mmio.add_register(
         // spcap 1.3
         static_register!(
-            ty: u32,
-            offset: 0xc008,
-            value: 0x00000801, // 4 ports starting at port 1.
-            ),
+        ty: u32,
+        offset: 0xc008,
+        value: 0x00000801, // 8 ports starting at port 1. See USB2_PORTS_START and USB2_PORTS_END.
+        ),
     );
 
     mmio.add_register(
         // spcap 1.4
         static_register!(
-            ty: u32,
-            offset: 0xc00c,
-            // The specification says that this shall be set to 0.
-            // Section 7.2.2.1.4.
-            value: 0,
-            ),
+        ty: u32,
+        offset: 0xc00c,
+        // The specification says that this shall be set to 0.
+        // Section 7.2.2.1.4.
+        value: 0,
+        ),
     );
+
+    mmio.add_register(
+        // spcap 2.1
+        static_register!(
+        ty: u32,
+        offset: 0xc100,
+        // "Supported Protocol" capability.
+        // Not next capability.
+        // USB 3.0. Revision 2.0.
+        value: 0x03000002,
+        ),
+    );
+    mmio.add_register(
+        // spcap 2.2
+        static_register!(
+        ty: u32,
+        offset: 0xc104,
+        value: 0x20425355, // Name string = "USB "
+        ),
+    );
+    mmio.add_register(
+        // spcap 2.3
+        static_register!(
+        ty: u32,
+        offset: 0xc108,
+        value: 0x00000809, // 8 ports starting at port 9. See USB3_PORTS_START and USB3_PORTS_END.
+        ),
+    );
+
+    mmio.add_register(
+        // spcap 2.4
+        static_register!(
+        ty: u32,
+        offset: 0xc10c,
+        // The specification says that this shall be set to 0.
+        // Section 7.2.2.1.4.
+        value: 0,
+        ),
+    );
+
     /* End of Host Controller Operational Registers */
 
     (mmio, xhci_regs)