2 files changed, 95 insertions, 2 deletions

diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 62db51d0185..12e277a26d9 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -96,6 +96,25 @@ static void xhci_ring_free(struct xhci_ring *ring)
 }
 
 /**
+ * Free the scratchpad buffer array and scratchpad buffers
+ *
+ * @ctrl	host controller data structure
+ * @return	none
+ */
+static void xhci_scratchpad_free(struct xhci_ctrl *ctrl)
+{
+	if (!ctrl->scratchpad)
+		return;
+
+	ctrl->dcbaa->dev_context_ptrs[0] = 0;
+
+	free((void *)(uintptr_t)ctrl->scratchpad->sp_array[0]);
+	free(ctrl->scratchpad->sp_array);
+	free(ctrl->scratchpad);
+	ctrl->scratchpad = NULL;
+}
+
+/**
  * frees the "xhci_container_ctx" pointer passed
  *
  * @param ptr	pointer to "xhci_container_ctx" to be freed
@@ -155,6 +174,7 @@ void xhci_cleanup(struct xhci_ctrl *ctrl)
 {
 	xhci_ring_free(ctrl->event_ring);
 	xhci_ring_free(ctrl->cmd_ring);
+	xhci_scratchpad_free(ctrl);
 	xhci_free_virt_devices(ctrl);
 	free(ctrl->erst.entries);
 	free(ctrl->dcbaa);
@@ -320,6 +340,70 @@ struct xhci_ring *xhci_ring_alloc(unsigned int num_segs, bool link_trbs)
 }
 
 /**
+ * Set up the scratchpad buffer array and scratchpad buffers
+ *
+ * @ctrl	host controller data structure
+ * @return	-ENOMEM if buffer allocation fails, 0 on success
+ */
+static int xhci_scratchpad_alloc(struct xhci_ctrl *ctrl)
+{
+	struct xhci_hccr *hccr = ctrl->hccr;
+	struct xhci_hcor *hcor = ctrl->hcor;
+	struct xhci_scratchpad *scratchpad;
+	int num_sp;
+	uint32_t page_size;
+	void *buf;
+	int i;
+
+	num_sp = HCS_MAX_SCRATCHPAD(xhci_readl(&hccr->cr_hcsparams2));
+	if (!num_sp)
+		return 0;
+
+	scratchpad = malloc(sizeof(*scratchpad));
+	if (!scratchpad)
+		goto fail_sp;
+	ctrl->scratchpad = scratchpad;
+
+	scratchpad->sp_array = xhci_malloc(num_sp * sizeof(u64));
+	if (!scratchpad->sp_array)
+		goto fail_sp2;
+	ctrl->dcbaa->dev_context_ptrs[0] =
+		cpu_to_le64((uintptr_t)scratchpad->sp_array);
+
+	page_size = xhci_readl(&hcor->or_pagesize) & 0xffff;
+	for (i = 0; i < 16; i++) {
+		if ((0x1 & page_size) != 0)
+			break;
+		page_size = page_size >> 1;
+	}
+	BUG_ON(i == 16);
+
+	page_size = 1 << (i + 12);
+	buf = memalign(page_size, num_sp * page_size);
+	if (!buf)
+		goto fail_sp3;
+	memset(buf, '\0', num_sp * page_size);
+	xhci_flush_cache((uintptr_t)buf, num_sp * page_size);
+
+	for (i = 0; i < num_sp; i++) {
+		uintptr_t ptr = (uintptr_t)buf + i * page_size;
+		scratchpad->sp_array[i] = cpu_to_le64(ptr);
+	}
+
+	return 0;
+
+fail_sp3:
+	free(scratchpad->sp_array);
+
+fail_sp2:
+	free(scratchpad);
+	ctrl->scratchpad = NULL;
+
+fail_sp:
+	return -ENOMEM;
+}
+
+/**
  * Allocates the Container context
  *
  * @param ctrl	Host controller data structure
@@ -499,6 +583,9 @@ int xhci_mem_init(struct xhci_ctrl *ctrl, struct xhci_hccr *hccr,
 
 	xhci_writeq(&ctrl->ir_set->erst_base, val_64);
 
+	/* set up the scratchpad buffer array and scratchpad buffers */
+	xhci_scratchpad_alloc(ctrl);
+
 	/* initializing the virtual devices to NULL */
 	for (i = 0; i < MAX_HC_SLOTS; ++i)
 		ctrl->devs[i] = NULL;
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 431afd864db..32dd611efac 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -111,9 +111,10 @@ struct xhci_hccr {
 #define HCS_IST(p)		(((p) >> 0) & 0xf)
 /* bits 4:7, max number of Event Ring segments */
 #define HCS_ERST_MAX(p)		(((p) >> 4) & 0xf)
+/* bits 21:25 Hi 5 bits of Scratchpad buffers SW must allocate for the HW */
 /* bit 26 Scratchpad restore - for save/restore HW state - not used yet */
-/* bits 27:31 number of Scratchpad buffers SW must allocate for the HW */
-#define HCS_MAX_SCRATCHPAD(p)   (((p) >> 27) & 0x1f)
+/* bits 27:31 Lo 5 bits of Scratchpad buffers SW must allocate for the HW */
+#define HCS_MAX_SCRATCHPAD(p)	((((p) >> 16) & 0x3e0) | (((p) >> 27) & 0x1f))
 
 /* HCSPARAMS3 - hcs_params3 - bitmasks */
 /* bits 0:7, Max U1 to U0 latency for the roothub ports */
@@ -1037,6 +1038,10 @@ struct xhci_erst {
 	unsigned int		erst_size;
 };
 
+struct xhci_scratchpad {
+	u64 *sp_array;
+};
+
 /*
  * Each segment table entry is 4*32bits long.  1K seems like an ok size:
  * (1K bytes * 8bytes/bit) / (4*32 bits) = 64 segment entries in the table,
@@ -1224,6 +1229,7 @@ struct xhci_ctrl {
 	struct xhci_intr_reg *ir_set;
 	struct xhci_erst erst;
 	struct xhci_erst_entry entry[ERST_NUM_SEGS];
+	struct xhci_scratchpad *scratchpad;
 	struct xhci_virt_device *devs[MAX_HC_SLOTS];
 	int rootdev;
 };