xhci: implement xhci_fixup_endpoint for interval adjustments

Must be called in a non-atomic context, after the endpoint
has been registered with the hardware via xhci_add_endpoint
and before the first URB is submitted for the endpoint.

Signed-off-by: Jonathan Bell <jonathan@raspberrypi.org>

drivers/usb/host/xhci.c

@@ -1597,6 +1597,109 @@ static int xhci_check_maxpacket(struct xhci_hcd *xhci, unsigned int slot_id,
 	return ret;
 }
 
+/*
+ * RPI: Fixup endpoint intervals when requested
+ * - Check interval versus the (cached) endpoint context
+ * - set the endpoint interval to the new value
+ * - force an endpoint configure command
+ * XXX: bandwidth is not recalculated. We should probably do that.
+ */
+
+static unsigned int xhci_get_endpoint_flag_from_index(unsigned int ep_index)
+{
+	return 1 << (ep_index + 1);
+}
+
+static void xhci_fixup_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
+				struct usb_host_endpoint *ep, int interval)
+{
+	struct xhci_hcd *xhci;
+	struct xhci_ep_ctx *ep_ctx_out, *ep_ctx_in;
+	struct xhci_command *command;
+	struct xhci_input_control_ctx *ctrl_ctx;
+	struct xhci_virt_device *vdev;
+	int xhci_interval;
+	int ret;
+	int ep_index;
+	unsigned long flags;
+	u32 ep_info_tmp;
+
+	xhci = hcd_to_xhci(hcd);
+	ep_index = xhci_get_endpoint_index(&ep->desc);
+
+	/* FS/LS interval translations */
+	if ((udev->speed == USB_SPEED_FULL ||
+	     udev->speed == USB_SPEED_LOW))
+		interval *= 8;
+
+	mutex_lock(&xhci->mutex);
+
+	spin_lock_irqsave(&xhci->lock, flags);
+
+	vdev = xhci->devs[udev->slot_id];
+	/* Get context-derived endpoint interval */
+	ep_ctx_out = xhci_get_ep_ctx(xhci, vdev->out_ctx, ep_index);
+	ep_ctx_in = xhci_get_ep_ctx(xhci, vdev->in_ctx, ep_index);
+	xhci_interval = EP_INTERVAL_TO_UFRAMES(le32_to_cpu(ep_ctx_out->ep_info));
+
+	if (interval == xhci_interval) {
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		mutex_unlock(&xhci->mutex);
+		return;
+	}
+
+	xhci_dbg(xhci, "Fixup interval=%d xhci_interval=%d\n",
+		 interval, xhci_interval);
+	command = xhci_alloc_command_with_ctx(xhci, true, GFP_ATOMIC);
+	if (!command) {
+		/* Failure here is benign, poll at the original rate */
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		mutex_unlock(&xhci->mutex);
+		return;
+	}
+
+	/* xHCI uses exponents for intervals... */
+	xhci_interval = fls(interval) - 1;
+	xhci_interval = clamp_val(xhci_interval, 3, 10);
+	ep_info_tmp = le32_to_cpu(ep_ctx_out->ep_info);
+	ep_info_tmp &= ~EP_INTERVAL(255);
+	ep_info_tmp |= EP_INTERVAL(xhci_interval);
+
+	/* Keep the endpoint context up-to-date while issuing the command. */
+	xhci_endpoint_copy(xhci, vdev->in_ctx,
+			   vdev->out_ctx, ep_index);
+	ep_ctx_in->ep_info = cpu_to_le32(ep_info_tmp);
+
+	/*
+	 * We need to drop the lock, so take an explicit copy
+	 * of the ep context.
+	 */
+	xhci_endpoint_copy(xhci, command->in_ctx, vdev->in_ctx, ep_index);
+
+	ctrl_ctx = xhci_get_input_control_ctx(command->in_ctx);
+	if (!ctrl_ctx) {
+		xhci_warn(xhci,
+			  "%s: Could not get input context, bad type.\n",
+			  __func__);
+		spin_unlock_irqrestore(&xhci->lock, flags);
+		xhci_free_command(xhci, command);
+		mutex_unlock(&xhci->mutex);
+		return;
+	}
+	ctrl_ctx->add_flags = xhci_get_endpoint_flag_from_index(ep_index);
+	ctrl_ctx->drop_flags = 0;
+
+	spin_unlock_irqrestore(&xhci->lock, flags);
+
+	ret = xhci_configure_endpoint(xhci, udev, command,
+				      false, false);
+	if (ret)
+		xhci_warn(xhci, "%s: Configure endpoint failed: %d\n",
+			  __func__, ret);
+	xhci_free_command(xhci, command);
+	mutex_unlock(&xhci->mutex);
+}
+
 /*
  * non-error returns are a promise to giveback() the urb later
  * we drop ownership so next owner (or urb unlink) can get it
@@ -5421,6 +5524,7 @@ static const struct hc_driver xhci_hc_driver = {
 	.endpoint_reset =	xhci_endpoint_reset,
 	.check_bandwidth =	xhci_check_bandwidth,
 	.reset_bandwidth =	xhci_reset_bandwidth,
+	.fixup_endpoint =	xhci_fixup_endpoint,
 	.address_device =	xhci_address_device,
 	.enable_device =	xhci_enable_device,
 	.update_hub_device =	xhci_update_hub_device,