[SLOF] [PATCH v2 5/5] usb: Set XHCI slot speed according to port status

[Thomas Huth]

So far, the code was always assuming SuperSpeed for all devices,
which seemed to work OK with QEMU ... but let's better play safe
instead and use the speed from the port status instead.

Signed-off-by: Thomas Huth <thuth at redhat.com>
---
 lib/libusb/usb-hub.c  | 15 ++++++++++++++-
 lib/libusb/usb-xhci.c | 12 +++++++-----
 lib/libusb/usb-xhci.h |  2 +-
 3 files changed, 22 insertions(+), 7 deletions(-)

diff --git a/lib/libusb/usb-hub.c b/lib/libusb/usb-hub.c
index 5f56630..58e552f 100644
--- a/lib/libusb/usb-hub.c
+++ b/lib/libusb/usb-hub.c
@@ -39,6 +39,7 @@ struct usb_hub_ps {
 #define HUB_PS_RESET                 (1 << 4)
 #define HUB_PS_POWER                 (1 << 8)
 #define HUB_PS_LOW_SPEED             (1 << 9)
+#define HUB_PS_HIGH_SPEED            (1 << 10)
 
 #define HUB_PF_CONNECTION        0
 #define HUB_PF_ENABLE            1
@@ -157,6 +158,17 @@ static bool usb_hub_init_dev(struct usb_dev *hub_dev, int port)
 	struct usb_dev *newdev;
 
 	if (hub_dev->hcidev->type == USB_XHCI) {
+		struct usb_hub_ps ps;
+		int slotspeed;
+
+		hub_get_port_status(hub_dev, port, &ps, sizeof(ps));
+		if (le16_to_cpu(ps.wPortStatus) & HUB_PS_LOW_SPEED)
+			slotspeed = SLOT_SPEED_LS;
+		else if (le16_to_cpu(ps.wPortStatus) & HUB_PS_HIGH_SPEED)
+			slotspeed = SLOT_SPEED_HS;
+		else
+			slotspeed = SLOT_SPEED_FS;
+
 		/*
 		 * USB3 devices need special setup (e.g. with assigning
 		 * a slot ID and route string), which will all be done
@@ -164,7 +176,8 @@ static bool usb_hub_init_dev(struct usb_dev *hub_dev, int port)
 		 * usb_setup_new_device() and usb_slof_populate_new_device()
 		 * internally, so we can return immediately after this step.
 		 */
-		return usb3_dev_init(hub_dev->hcidev->priv, hub_dev, port);
+		return usb3_dev_init(hub_dev->hcidev->priv, hub_dev, port,
+				     slotspeed);
 	}
 
 	newdev = usb_devpool_get();
diff --git a/lib/libusb/usb-xhci.c b/lib/libusb/usb-xhci.c
index 0b45c26..a359fc7 100644
--- a/lib/libusb/usb-xhci.c
+++ b/lib/libusb/usb-xhci.c
@@ -529,7 +529,7 @@ static uint32_t usb_control_max_packet(uint32_t speed)
 }
 
 static bool xhci_alloc_dev(struct xhci_hcd *xhcd, struct usb_dev *hub,
-			   uint32_t slot_id, uint32_t port)
+			   uint32_t slot_id, uint32_t port, uint32_t slotspeed)
 {
 	struct usb_dev *dev;
 	struct xhci_dev *xdev;
@@ -572,7 +572,7 @@ static bool xhci_alloc_dev(struct xhci_hcd *xhcd, struct usb_dev *hub,
 		rootport = dev->port;
 	}
 	val >>= 4;			/* Remove root hub ID from the string */
-	val |= LAST_CONTEXT(1) | SLOT_SPEED_SS;  /* FIXME speed, read from PS */
+	val |= LAST_CONTEXT(1) | slotspeed;
 	slot->field1 = cpu_to_le32(val);
 	slot->field2 = cpu_to_le32(ROOT_HUB_PORT(rootport));
 
@@ -662,7 +662,8 @@ static void xhci_free_dev(struct xhci_dev *xdev)
 	xhci_free_ctx(&xdev->out_ctx, XHCI_CTX_BUF_SIZE);
 }
 
-bool usb3_dev_init(struct xhci_hcd *xhcd, struct usb_dev *hub, uint32_t port)
+bool usb3_dev_init(struct xhci_hcd *xhcd, struct usb_dev *hub, uint32_t port,
+		   uint32_t slotspeed)
 {
 	/* Device enable slot */
 	xhci_send_enable_slot(xhcd, port);
@@ -671,7 +672,7 @@ bool usb3_dev_init(struct xhci_hcd *xhcd, struct usb_dev *hub, uint32_t port)
 		return false;
 	}
 	dprintf("SLOT ID: %d\n", xhcd->slot_id);
-	if (!xhci_alloc_dev(xhcd, hub, xhcd->slot_id, port)) {
+	if (!xhci_alloc_dev(xhcd, hub, xhcd->slot_id, port, slotspeed)) {
 		dprintf("Unable to allocate device\n");
 		return false;
 	}
@@ -749,7 +750,8 @@ static int xhci_port_scan(struct xhci_hcd *xhcd,
 				dprintf("Port reset complete %d\n", i);
 			}
 			print_port_status(prs);
-			if (!usb3_dev_init(xhcd, NULL, (i - (port_off - 1)))) {
+			if (!usb3_dev_init(xhcd, NULL, i - (port_off - 1),
+					   ((portsc >> 10) & 0xf) << 20)) {
 				dprintf("USB device initialization failed\n");
 			}
 		}
diff --git a/lib/libusb/usb-xhci.h b/lib/libusb/usb-xhci.h
index 0fd2d8e..02b382b 100644
--- a/lib/libusb/usb-xhci.h
+++ b/lib/libusb/usb-xhci.h
@@ -373,6 +373,6 @@ struct xhci_pipe {
 };
 
 extern bool usb3_dev_init(struct xhci_hcd *xhcd, struct usb_dev *hub,
-			  uint32_t port);
+			  uint32_t port, uint32_t slotspeed);
 
 #endif	/* USB_XHCI_H */
-- 
1.8.3.1