2 files changed, 223 insertions, 8 deletions

diff --git a/payloads/libpayload/drivers/usb/ohci.c b/payloads/libpayload/drivers/usb/ohci.c
index 6d98cc1100..0f4886fc63 100644
--- a/payloads/libpayload/drivers/usb/ohci.c
+++ b/payloads/libpayload/drivers/usb/ohci.c
@@ -144,6 +144,8 @@ ohci_init (pcidev_t addr)
 	OHCI_INST (controller)->periodic_ed = periodic_ed;
 
 	OHCI_INST (controller)->opreg->HcHCCA = virt_to_phys(OHCI_INST (controller)->hcca);
+	/* Make sure periodic schedule is enabled. */
+	OHCI_INST (controller)->opreg->HcControl |= PeriodicListEnable;
 	OHCI_INST (controller)->opreg->HcControl &= ~IsochronousEnable; // unused by this driver
 	// disable everything, contrary to what OHCI spec says in 5.1.1.4, as we don't need IRQs
 	OHCI_INST (controller)->opreg->HcInterruptEnable = 1<<31;
@@ -477,33 +479,215 @@ ohci_bulk (endpoint_t *ep, int dalen, u8 *data, int finalize)
 	return failure;
 }
 
+
+struct _intr_queue;
+
+struct _intrq_td {
+	volatile td_t		td;
+	u8			*data;
+	struct _intrq_td	*next;
+	struct _intr_queue	*intrq;
+};
+
+struct _intr_queue {
+	volatile ed_t		ed;
+	struct _intrq_td	*head;
+	struct _intrq_td	*tail;
+	u8			*data;
+	int			reqsize;
+	endpoint_t		*endp;
+	unsigned int		remaining_tds;
+};
+
+typedef struct _intrq_td intrq_td_t;
+typedef struct _intr_queue intr_queue_t;
+
+#define INTRQ_TD_FROM_TD(x) ((intrq_td_t *)x)
+
+static void
+ohci_fill_intrq_td(intrq_td_t *const td, intr_queue_t *const intrq,
+		   u8 *const data)
+{
+	memset(td, 0, sizeof(*td));
+	td->td.config = TD_QUEUETYPE_INTR |
+		(intrq->endp->direction == IN
+			? TD_DIRECTION_IN : TD_DIRECTION_OUT) |
+		TD_DELAY_INTERRUPT_ZERO |
+		TD_TOGGLE_FROM_ED |
+		TD_CC_NOACCESS;
+	td->td.current_buffer_pointer = virt_to_phys(data);
+	td->td.buffer_end = td->td.current_buffer_pointer + intrq->reqsize - 1;
+	td->intrq = intrq;
+	td->data = data;
+}
+
 /* create and hook-up an intr queue into device schedule */
-static void*
-ohci_create_intr_queue (endpoint_t *ep, int reqsize, int reqcount, int reqtiming)
+static void *
+ohci_create_intr_queue(endpoint_t *const ep, const int reqsize,
+		       const int reqcount, const int reqtiming)
 {
-	return NULL;
+	int i;
+	intrq_td_t *first_td = NULL, *last_td = NULL;
+
+	if (reqsize > 4096)
+		return NULL;
+
+	intr_queue_t *const intrq =
+		(intr_queue_t *)memalign(sizeof(intrq->ed), sizeof(*intrq));
+	memset(intrq, 0, sizeof(*intrq));
+	intrq->data = (u8 *)malloc(reqcount * reqsize);
+	intrq->reqsize = reqsize;
+	intrq->endp = ep;
+
+	/* Create #reqcount TDs. */
+	u8 *cur_data = intrq->data;
+	for (i = 0; i < reqcount; ++i) {
+		intrq_td_t *const td = memalign(sizeof(td->td), sizeof(*td));
+		++intrq->remaining_tds;
+		ohci_fill_intrq_td(td, intrq, cur_data);
+		cur_data += reqsize;
+		if (!first_td)
+			first_td = td;
+		else
+			last_td->td.next_td = virt_to_phys(&td->td);
+		last_td = td;
+	}
+
+	/* Create last, dummy TD. */
+	intrq_td_t *dummy_td = memalign(sizeof(dummy_td->td), sizeof(*dummy_td));
+	memset(dummy_td, 0, sizeof(*dummy_td));
+	dummy_td->intrq = intrq;
+	if (last_td)
+		last_td->td.next_td = virt_to_phys(&dummy_td->td);
+	last_td = dummy_td;
+
+	/* Initialize ED. */
+	intrq->ed.config =  (ep->dev->address << ED_FUNC_SHIFT) |
+		((ep->endpoint & 0xf) << ED_EP_SHIFT) |
+		(((ep->direction == IN) ? OHCI_IN : OHCI_OUT) << ED_DIR_SHIFT) |
+		(ep->dev->speed ? ED_LOWSPEED : 0) |
+		(ep->maxpacketsize << ED_MPS_SHIFT);
+	intrq->ed.tail_pointer = virt_to_phys(last_td);
+	intrq->ed.head_pointer = virt_to_phys(first_td) |
+		(ep->toggle ? ED_TOGGLE : 0);
+
+	/* Insert ED into periodic table. */
+	int nothing_placed	= 1;
+	ohci_t *const ohci	= OHCI_INST(ep->dev->controller);
+	u32 *const intr_table	= ohci->hcca->HccaInterruptTable;
+	const u32 dummy_ptr	= virt_to_phys(ohci->periodic_ed);
+	for (i = 0; i < 32; i += reqtiming) {
+		/* Advance to the next free position. */
+		while ((i < 32) && (intr_table[i] != dummy_ptr)) ++i;
+		if (i < 32) {
+			intr_table[i] = virt_to_phys(&intrq->ed);
+			nothing_placed = 0;
+		}
+	}
+	if (nothing_placed) {
+		printf("Error: Failed to place ohci interrupt endpoint "
+			"descriptor into periodic table: no space left\n");
+		ohci_destroy_intr_queue(ep, intrq);
+		return NULL;
+	}
+
+	return intrq;
 }
 
 /* remove queue from device schedule, dropping all data that came in */
 static void
-ohci_destroy_intr_queue (endpoint_t *ep, void *q_)
+ohci_destroy_intr_queue(endpoint_t *const ep, void *const q_)
 {
+	intr_queue_t *const intrq = (intr_queue_t *)q_;
+
+	int i;
+
+	/* Remove interrupt queue from periodic table. */
+	ohci_t *const ohci	= OHCI_INST(ep->dev->controller);
+	u32 *const intr_table	= ohci->hcca->HccaInterruptTable;
+	for (i=0; i < 32; ++i) {
+		if (intr_table[i] == virt_to_phys(intrq))
+			intr_table[i] = virt_to_phys(ohci->periodic_ed);
+	}
+	/* Wait for frame to finish. */
+	mdelay(1);
+
+	/* Free unprocessed TDs. */
+	while ((intrq->ed.head_pointer & ~0x3) != intrq->ed.tail_pointer) {
+		td_t *const cur_td =
+			(td_t *)phys_to_virt(intrq->ed.head_pointer & ~0x3);
+		intrq->ed.head_pointer = cur_td->next_td;
+		free(INTRQ_TD_FROM_TD(cur_td));
+		--intrq->remaining_tds;
+	}
+	/* Free final, dummy TD. */
+	free(phys_to_virt(intrq->ed.head_pointer & ~0x3));
+	/* Free data buffer. */
+	free(intrq->data);
+
+	/* Process done queue and free processed TDs. */
+	ohci_process_done_queue(ohci, 1);
+	while (intrq->head) {
+		intrq_td_t *const cur_td = intrq->head;
+		intrq->head = intrq->head->next;
+		free(cur_td);
+		--intrq->remaining_tds;
+	}
+	if (intrq->remaining_tds) {
+		printf("error: ohci_destroy_intr_queue(): "
+			"freed all but %d TDs.\n", intrq->remaining_tds);
+	}
+
+	free(intrq);
+
+	/* Save data toggle. */
+	ep->toggle = intrq->ed.head_pointer & ED_TOGGLE;
 }
 
 /* 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*
-ohci_poll_intr_queue (void *q_)
+static u8 *
+ohci_poll_intr_queue(void *const q_)
 {
-	return NULL;
+	intr_queue_t *const intrq = (intr_queue_t *)q_;
+
+	u8 *data = NULL;
+
+	/* Process done queue first, then check if we have work to do. */
+	ohci_process_done_queue(OHCI_INST(intrq->endp->dev->controller), 0);
+
+	if (intrq->head) {
+		/* Save pointer to processed TD and advance. */
+		intrq_td_t *const cur_td = intrq->head;
+		intrq->head = cur_td->next;
+
+		/* Return data buffer of this TD. */
+		data = cur_td->data;
+
+		/* Requeue this TD (i.e. copy to dummy and requeue as dummy). */
+		intrq_td_t *const dummy_td =
+			INTRQ_TD_FROM_TD(phys_to_virt(intrq->ed.tail_pointer));
+		ohci_fill_intrq_td(dummy_td, intrq, cur_td->data);
+		/* Reset all but intrq pointer (i.e. init as dummy). */
+		memset(cur_td, 0, sizeof(*cur_td));
+		cur_td->intrq = intrq;
+		/* Insert into interrupt queue as dummy. */
+		dummy_td->td.next_td = virt_to_phys(&cur_td->td);
+		intrq->ed.tail_pointer = virt_to_phys(&cur_td->td);
+	}
+
+	return data;
 }
 
 static void
 ohci_process_done_queue(ohci_t *const ohci, const int spew_debug)
 {
-	int i;
+	int i, j;
+
+	/* Temporary queue of interrupt queue TDs (to reverse order). */
+	intrq_td_t *temp_tdq = NULL;
 
 	/* Check if done head has been written. */
 	if (!(ohci->opreg->HcInterruptStatus & WritebackDoneHead))
@@ -527,6 +711,11 @@ ohci_process_done_queue(ohci_t *const ohci, const int spew_debug)
 			/* Free processed async TDs. */
 			free((void *)done_td);
 			break;
+		case TD_QUEUETYPE_INTR:
+			/* Save done TD if it comes from an interrupt queue. */
+			INTRQ_TD_FROM_TD(done_td)->next = temp_tdq;
+			temp_tdq = INTRQ_TD_FROM_TD(done_td);
+			break;
 		default:
 			break;
 		}
@@ -534,5 +723,30 @@ ohci_process_done_queue(ohci_t *const ohci, const int spew_debug)
 	}
 	if (spew_debug)
 		debug("Processed %d done TDs.\n", i);
+
+	j = 0;
+	/* Process interrupt queue TDs in right order. */
+	while (temp_tdq) {
+		/* Save pointer of current TD and advance. */
+		intrq_td_t *const cur_td = temp_tdq;
+		temp_tdq = temp_tdq->next;
+
+		/* The interrupt queue for the current TD. */
+		intr_queue_t *const intrq = cur_td->intrq;
+		/* Append to interrupt queue. */
+		if (!intrq->head) {
+			/* First element. */
+			intrq->head = intrq->tail = cur_td;
+		} else {
+			/* Insert at tail. */
+			intrq->tail->next = cur_td;
+			intrq->tail = cur_td;
+		}
+		/* It's always the last element. */
+		cur_td->next = NULL;
+		++j;
+	}
+	if (spew_debug)
+		debug("processed %d done tds, %d intr tds thereof.\n", i, j);
 }
 
diff --git a/payloads/libpayload/drivers/usb/ohci_private.h b/payloads/libpayload/drivers/usb/ohci_private.h
index 3826db08ee..a32203c582 100644
--- a/payloads/libpayload/drivers/usb/ohci_private.h
+++ b/payloads/libpayload/drivers/usb/ohci_private.h
@@ -231,6 +231,7 @@
 #define TD_QUEUETYPE_SHIFT	0
 #define TD_QUEUETYPE_MASK	MASK(TD_QUEUETYPE_SHIFT, 2)
 #define TD_QUEUETYPE_ASYNC	(0 << TD_QUEUETYPE_SHIFT)
+#define TD_QUEUETYPE_INTR	(1 << TD_QUEUETYPE_SHIFT)
 
 #define TD_DIRECTION_SHIFT 19
 #define TD_DIRECTION_MASK MASK(TD_DIRECTION_SHIFT, 2)