3 files changed, 139 insertions, 12 deletions

diff --git a/payloads/libpayload/drivers/usb/dwc2.c b/payloads/libpayload/drivers/usb/dwc2.c
index faf1f46282..fa0fd34e3c 100644
--- a/payloads/libpayload/drivers/usb/dwc2.c
+++ b/payloads/libpayload/drivers/usb/dwc2.c
@@ -37,7 +37,8 @@ static void dwc2_reinit(hci_t *controller)
 	grxfsiz_t grxfsiz = { .d32 = 0 };
 	ghwcfg3_t hwcfg3 = { .d32 = 0 };
 	hcintmsk_t hcintmsk = { .d32 = 0 };
-	gnptxfsiz_t gnptxfsiz = { .d32 = 0 };
+	gtxfsiz_t gnptxfsiz = { .d32 = 0 };
+	gtxfsiz_t hptxfsiz = { .d32 = 0 };
 
 	const int timeout = 10000;
 	int i, fifo_blocks, tx_blocks;
@@ -97,15 +98,19 @@ static void dwc2_reinit(hci_t *controller)
 	 * Reserve 2 spaces for the status entries of received packets
 	 * and 2 spaces for bulk and control OUT endpoints. Calculate how
 	 * many blocks can be alloted, assume largest packet size is 512.
+	 * 16 locations reserved for periodic TX .
 	 */
-	fifo_blocks = (hwcfg3.dfifodepth - 4) / (512 / 4);
+	fifo_blocks = (hwcfg3.dfifodepth - 4 - 16) / (512 / 4);
 	tx_blocks = fifo_blocks / 2;
 
 	grxfsiz.rxfdep = (fifo_blocks - tx_blocks) * (512 / 4) + 4;
 	writel(grxfsiz.d32, &reg->core.grxfsiz);
-	gnptxfsiz.nptxfstaddr = grxfsiz.rxfdep;
-	gnptxfsiz.nptxfdep = tx_blocks * (512 / 4);
+	gnptxfsiz.txfstaddr = grxfsiz.rxfdep;
+	gnptxfsiz.txfdep = tx_blocks * (512 / 4);
 	writel(gnptxfsiz.d32, &reg->core.gnptxfsiz);
+	hptxfsiz.txfstaddr = gnptxfsiz.txfstaddr + gnptxfsiz.txfdep;
+	hptxfsiz.txfdep = 16;
+	writel(hptxfsiz.d32, &reg->core.hptxfsiz);
 
 	/* Init host channels */
 	hcintmsk.xfercomp = 1;
@@ -159,9 +164,10 @@ wait_for_complete(endpoint_t *ep, uint32_t ch_num)
 
 		if (hcint.chhltd) {
 			writel(hcint.d32, &reg->host.hchn[ch_num].hcintn);
-
 			if (hcint.xfercomp)
 				return hctsiz.xfersize;
+			else if (hcint.nak || hcint.frmovrun)
+				return hctsiz.xfersize;
 			else if (hcint.xacterr)
 				return -HCSTAT_XFERERR;
 			else if (hcint.bblerr)
@@ -327,6 +333,101 @@ dwc2_control(usbdev_t *dev, direction_t dir, int drlen, void *setup,
 	return ret;
 }
 
+static int
+dwc2_intr(endpoint_t *ep, int size, u8 *src)
+{
+	ep_dir_t data_dir;
+
+	if (ep->direction == IN)
+		data_dir = EPDIR_IN;
+	else if (ep->direction == OUT)
+		data_dir = EPDIR_OUT;
+	else
+		return -1;
+
+	return dwc2_transfer(ep, size, ep->toggle, data_dir, 0, src);
+}
+
+static u32 dwc2_intr_get_timestamp(intr_queue_t *q)
+{
+	hprt_t hprt;
+	hfnum_t hfnum;
+	hci_t *controller = q->endp->dev->controller;
+	dwc_ctrl_t *dwc2 = DWC2_INST(controller);
+	dwc2_reg_t *reg = DWC2_REG(controller);
+
+	hfnum.d32 = readl(&reg->host.hfnum);
+	hprt.d32 = readl(dwc2->hprt0);
+
+	/*
+	 * hfnum.frnum increments when a new SOF is transmitted on
+	 * the USB, and is reset to 0 when it reaches 16'h3FFF
+	 */
+	switch (hprt.prtspd) {
+	case PRTSPD_HIGH:
+		/* 8 micro-frame per ms for high-speed */
+		return hfnum.frnum / 8;
+	case PRTSPD_FULL:
+	case PRTSPD_LOW:
+	default:
+		/* 1 micro-frame per ms for high-speed */
+		return hfnum.frnum / 1;
+	}
+}
+
+/* create and hook-up an intr queue into device schedule */
+static void *
+dwc2_create_intr_queue(endpoint_t *ep, const int reqsize,
+		       const int reqcount, const int reqtiming)
+{
+	intr_queue_t *q = (intr_queue_t *)xzalloc(sizeof(intr_queue_t));
+
+	q->data = dma_memalign(4, reqsize);
+	q->endp = ep;
+	q->reqsize = reqsize;
+	q->reqtiming = reqtiming;
+
+	return q;
+}
+
+static void
+dwc2_destroy_intr_queue(endpoint_t *ep, void *_q)
+{
+	intr_queue_t *q = (intr_queue_t *)_q;
+
+	free(q->data);
+	free(q);
+}
+
+/*
+ * read one intr-packet from queue, if available. extend the queue for
+ * new input. Return NULL if nothing new available.
+ * Recommended use: while (data=poll_intr_queue(q)) process(data);
+ */
+static u8 *
+dwc2_poll_intr_queue(void *_q)
+{
+	intr_queue_t *q = (intr_queue_t *)_q;
+	int ret = 0;
+	u32 timestamp = dwc2_intr_get_timestamp(q);
+
+	/*
+	 * If hfnum.frnum run overflow it will schedule
+	 * an interrupt transfer immediately
+	 */
+	if (timestamp - q->timestamp < q->reqtiming)
+		return NULL;
+
+	q->timestamp = timestamp;
+
+	ret = dwc2_intr(q->endp, q->reqsize, q->data);
+
+	if (ret > 0)
+		return q->data;
+	else
+		return NULL;
+}
+
 hci_t *dwc2_init(void *bar)
 {
 	hci_t *controller = new_controller();
@@ -349,9 +450,9 @@ hci_t *dwc2_init(void *bar)
 	controller->set_address = generic_set_address;
 	controller->finish_device_config = NULL;
 	controller->destroy_device = NULL;
-	controller->create_intr_queue = NULL;
-	controller->destroy_intr_queue = NULL;
-	controller->poll_intr_queue = NULL;
+	controller->create_intr_queue = dwc2_create_intr_queue;
+	controller->destroy_intr_queue = dwc2_destroy_intr_queue;
+	controller->poll_intr_queue = dwc2_poll_intr_queue;
 	controller->reg_base = (uintptr_t)bar;
 	init_device_entry(controller, 0);
 
diff --git a/payloads/libpayload/drivers/usb/dwc2_private.h b/payloads/libpayload/drivers/usb/dwc2_private.h
index 7e6621dce8..c1090420ec 100644
--- a/payloads/libpayload/drivers/usb/dwc2_private.h
+++ b/payloads/libpayload/drivers/usb/dwc2_private.h
@@ -24,9 +24,18 @@
 typedef struct dwc_ctrl {
 #define DMA_SIZE (64 * 1024)
 	void *dma_buffer;
-	uint32_t *hprt0;
+	u32 *hprt0;
+	u32 frame;
 } dwc_ctrl_t;
 
+typedef struct {
+	u8 *data;
+	endpoint_t *endp;
+	int reqsize;
+	u32 reqtiming;
+	u32 timestamp;
+} intr_queue_t;
+
 #define DWC2_INST(controller) ((dwc_ctrl_t *)((controller)->instance))
 #define DWC2_REG(controller) ((dwc2_reg_t *)((controller)->reg_base))
 
diff --git a/payloads/libpayload/include/usb/dwc2_registers.h b/payloads/libpayload/include/usb/dwc2_registers.h
index b6ae2ed7ad..0e46985d4b 100644
--- a/payloads/libpayload/include/usb/dwc2_registers.h
+++ b/payloads/libpayload/include/usb/dwc2_registers.h
@@ -364,10 +364,10 @@ typedef union {
 	uint32_t d32;
 	/* register bits */
 	struct {
-		unsigned nptxfstaddr:16;
-		unsigned nptxfdep:16;
+		unsigned txfstaddr:16;
+		unsigned txfdep:16;
 	};
-} gnptxfsiz_t;
+} gtxfsiz_t;
 
 /**
  * This union represents the bit fields of the Core Receive FIFO Size
@@ -512,6 +512,23 @@ typedef union {
 } hcfg_t;
 
 /**
+ * This union represents the bit fields in the Host Frame Number/Frame Time
+ * Remaining Register
+ */
+typedef union {
+	/* raw register data */
+	uint32_t d32;
+
+	/* register bits */
+	struct {
+		/** Frame Number */
+		unsigned frnum:16;
+		/** Frame Time Remaining */
+		unsigned frrem:16;
+	};
+} hfnum_t;
+
+/**
  * This union represents the bit fields in the Host Port Control and status
  * Register.
  */