usb: ohci: Add support for interrupt queues

index 2f976d2..5364ced 100644
@@ -1478,6 +1478,31 @@ pkt_print(ohci, NULL, dev, pipe, buffer, transfer_len,
 
 /*-------------------------------------------------------------------------*/
 
+static ohci_dev_t *ohci_get_ohci_dev(ohci_t *ohci, int devnum, int intr)
+{
+	int i;
+
+	if (!intr)
+		return &ohci->ohci_dev;
+
+	/* First see if we already have an ohci_dev for this dev. */
+	for (i = 0; i < NUM_INT_DEVS; i++) {
+		if (ohci->int_dev[i].devnum == devnum)
+			return &ohci->int_dev[i];
+	}
+
+	/* If not then find a free one. */
+	for (i = 0; i < NUM_INT_DEVS; i++) {
+		if (ohci->int_dev[i].devnum == -1) {
+			ohci->int_dev[i].devnum = devnum;
+			return &ohci->int_dev[i];
+		}
+	}
+
+	printf("ohci: Error out of ohci_devs for interrupt endpoints\n");
+	return NULL;
+}
+
 /* common code for handling submit messages - used for all but root hub */
 /* accesses. */
 static int submit_common_msg(ohci_t *ohci, struct usb_device *dev,

@@ -1488,6 +1513,7 @@ static int submit_common_msg(ohci_t *ohci, struct usb_device *dev,
 	int maxsize = usb_maxpacket(dev, pipe);
 	int timeout;
 	urb_priv_t *urb;
+	ohci_dev_t *ohci_dev;
 
 	urb = malloc(sizeof(urb_priv_t));
 	memset(urb, 0, sizeof(urb_priv_t));

@@ -1511,7 +1537,11 @@ static int submit_common_msg(ohci_t *ohci, struct usb_device *dev,
 		return -1;
 	}
 
-	if (sohci_submit_job(ohci, &ohci->ohci_dev, urb, setup) < 0) {
+	ohci_dev = ohci_get_ohci_dev(ohci, dev->devnum, usb_pipeint(pipe));
+	if (!ohci_dev)
+		return -ENOMEM;
+
+	if (sohci_submit_job(ohci, ohci_dev, urb, setup) < 0) {
 		err("sohci_submit_job failed");
 		return -1;
 	}

@@ -1711,8 +1741,11 @@ static int hc_start(ohci_t *ohci)
 {
 	__u32 mask;
 	unsigned int fminterval;
+	int i;
 
 	ohci->disabled = 1;
+	for (i = 0; i < NUM_INT_DEVS; i++)
+		ohci->int_dev[i].devnum = -1;
 
 	/* Tell the controller where the control and bulk lists are
 	 * The lists are empty now. */

index 3f9869b..f1526d4 100644
@@ -367,10 +367,13 @@ typedef struct
 
 #define NUM_TD 64		/* we need more TDs than EDs */
 
+#define NUM_INT_DEVS 8		/* num of ohci_dev structs for int endpoints */
+
 typedef struct ohci_device {
 	ed_t ed[NUM_EDS] __aligned(ED_ALIGNMENT);
 	td_t tds[NUM_TD] __aligned(TD_ALIGNMENT);
 	int ed_cnt;
+	int devnum;
 } ohci_dev_t;
 
 /*

@@ -384,6 +387,7 @@ typedef struct ohci_device {
 typedef struct ohci {
 	/* this allocates EDs for all possible endpoints */
 	struct ohci_device ohci_dev __aligned(TD_ALIGNMENT);
+	struct ohci_device int_dev[NUM_INT_DEVS] __aligned(TD_ALIGNMENT);
 	struct ohci_hcca *hcca;		/* hcca */
 	/*dma_addr_t hcca_dma;*/
 

index 5364ced..17f3ac6 100644
@@ -1505,6 +1505,26 @@ static ohci_dev_t *ohci_get_ohci_dev(ohci_t *ohci, int devnum, int intr)
 
 /* common code for handling submit messages - used for all but root hub */
 /* accesses. */
+static urb_priv_t *ohci_alloc_urb(struct usb_device *dev, unsigned long pipe,
+		void *buffer, int transfer_len, int interval)
+{
+	urb_priv_t *urb;
+
+	urb = calloc(1, sizeof(urb_priv_t));
+	if (!urb) {
+		printf("ohci: Error out of memory allocating urb\n");
+		return NULL;
+	}
+
+	urb->dev = dev;
+	urb->pipe = pipe;
+	urb->transfer_buffer = buffer;
+	urb->transfer_buffer_length = transfer_len;
+	urb->interval = interval;
+
+	return urb;
+}
+
 static int submit_common_msg(ohci_t *ohci, struct usb_device *dev,
 		unsigned long pipe, void *buffer, int transfer_len,
 		struct devrequest *setup, int interval)

@@ -1515,14 +1535,9 @@ static int submit_common_msg(ohci_t *ohci, struct usb_device *dev,
 	urb_priv_t *urb;
 	ohci_dev_t *ohci_dev;
 
-	urb = malloc(sizeof(urb_priv_t));
-	memset(urb, 0, sizeof(urb_priv_t));
-
-	urb->dev = dev;
-	urb->pipe = pipe;
-	urb->transfer_buffer = buffer;
-	urb->transfer_buffer_length = transfer_len;
-	urb->interval = interval;
+	urb = ohci_alloc_urb(dev, pipe, buffer, transfer_len, interval);
+	if (!urb)
+		return -ENOMEM;
 
 #ifdef DEBUG
 	urb->actual_length = 0;

index 17f3ac6..922fc0b 100644
@@ -1622,6 +1622,94 @@ static int submit_common_msg(ohci_t *ohci, struct usb_device *dev,
 	return 0;
 }
 
+#define MAX_INT_QUEUESIZE 8
+
+struct int_queue {
+	int queuesize;
+	int curr_urb;
+	urb_priv_t *urb[MAX_INT_QUEUESIZE];
+};
+
+static struct int_queue *_ohci_create_int_queue(struct usb_device *udev,
+			unsigned long pipe, int queuesize, int elementsize,
+			void *buffer, int interval)
+{
+	ohci_t *ohci = dev_get_priv(usb_get_bus(udev->dev));
+	struct int_queue *queue;
+	ohci_dev_t *ohci_dev;
+	int i;
+
+	if (queuesize > MAX_INT_QUEUESIZE)
+		return NULL;
+
+	ohci_dev = ohci_get_ohci_dev(ohci, udev->devnum, 1);
+	if (!ohci_dev)
+		return NULL;
+
+	queue = malloc(sizeof(*queue));
+	if (!queue) {
+		printf("ohci: Error out of memory allocating int queue\n");
+		return NULL;
+	}
+
+	for (i = 0; i < queuesize; i++) {
+		queue->urb[i] = ohci_alloc_urb(udev, pipe,
+					       buffer + i * elementsize,
+					       elementsize, interval);
+		if (!queue->urb[i])
+			break;
+
+		if (sohci_submit_job(ohci, ohci_dev, queue->urb[i], NULL)) {
+			printf("ohci: Error submitting int queue job\n");
+			urb_free_priv(queue->urb[i]);
+			break;
+		}
+	}
+	if (i == 0) {
+		/* We did not succeed in submitting even 1 urb */
+		free(queue);
+		return NULL;
+	}
+
+	queue->queuesize = i;
+	queue->curr_urb = 0;
+
+	return queue;
+}
+
+static void *_ohci_poll_int_queue(struct usb_device *udev,
+				  struct int_queue *queue)
+{
+	ohci_t *ohci = dev_get_priv(usb_get_bus(udev->dev));
+
+	if (queue->curr_urb == queue->queuesize)
+		return NULL; /* Queue depleted */
+
+	if (hc_interrupt(ohci) < 0)
+		return NULL;
+
+	if (queue->urb[queue->curr_urb]->finished) {
+		void *ret = queue->urb[queue->curr_urb]->transfer_buffer;
+		queue->curr_urb++;
+		return ret;
+	}
+
+	return NULL;
+}
+
+static int _ohci_destroy_int_queue(struct usb_device *dev,
+				   struct int_queue *queue)
+{
+	int i;
+
+	for (i = 0; i < queue->queuesize; i++)
+		urb_free_priv(queue->urb[i]);
+
+	free(queue);
+
+	return 0;
+}
+
 #ifndef CONFIG_DM_USB
 /* submit routines called from usb.c */
 int submit_bulk_msg(struct usb_device *dev, unsigned long pipe, void *buffer,

@@ -1639,6 +1727,24 @@ int submit_int_msg(struct usb_device *dev, unsigned long pipe, void *buffer,
 	return submit_common_msg(&gohci, dev, pipe, buffer, transfer_len, NULL,
 			interval);
 }
+
+struct int_queue *create_int_queue(struct usb_device *dev,
+		unsigned long pipe, int queuesize, int elementsize,
+		void *buffer, int interval)
+{
+	return _ohci_create_int_queue(dev, pipe, queuesize, elementsize,
+				      buffer, interval);
+}
+
+void *poll_int_queue(struct usb_device *dev, struct int_queue *queue)
+{
+	return _ohci_poll_int_queue(dev, queue);
+}
+
+int destroy_int_queue(struct usb_device *dev, struct int_queue *queue)
+{
+	return _ohci_destroy_int_queue(dev, queue);
+}
 #endif
 
 static int _ohci_submit_control_msg(ohci_t *ohci, struct usb_device *dev,

@@ -2072,6 +2178,26 @@ static int ohci_submit_int_msg(struct udevice *dev, struct usb_device *udev,
 				 NULL, interval);
 }
 
+static struct int_queue *ohci_create_int_queue(struct udevice *dev,
+		struct usb_device *udev, unsigned long pipe, int queuesize,
+		int elementsize, void *buffer, int interval)
+{
+	return _ohci_create_int_queue(udev, pipe, queuesize, elementsize,
+				      buffer, interval);
+}
+
+static void *ohci_poll_int_queue(struct udevice *dev, struct usb_device *udev,
+				 struct int_queue *queue)
+{
+	return _ohci_poll_int_queue(udev, queue);
+}
+
+static int ohci_destroy_int_queue(struct udevice *dev, struct usb_device *udev,
+				  struct int_queue *queue)
+{
+	return _ohci_destroy_int_queue(udev, queue);
+}
+
 int ohci_register(struct udevice *dev, struct ohci_regs *regs)
 {
 	struct usb_bus_priv *priv = dev_get_uclass_priv(dev);

@@ -2114,6 +2240,9 @@ struct dm_usb_ops ohci_usb_ops = {
 	.control = ohci_submit_control_msg,
 	.bulk = ohci_submit_bulk_msg,
 	.interrupt = ohci_submit_int_msg,
+	.create_int_queue = ohci_create_int_queue,
+	.poll_int_queue = ohci_poll_int_queue,
+	.destroy_int_queue = ohci_destroy_int_queue,
 };
 
 #endif

index 222e739..2d6b815 100644
@@ -317,7 +317,7 @@ extern int soft_i2c_gpio_scl;
 #define CONFIG_CONSOLE_MUX
 #define CONFIG_PREBOOT
 #define CONFIG_SYS_STDIO_DEREGISTER
-#define CONFIG_SYS_USB_EVENT_POLL
+#define CONFIG_SYS_USB_EVENT_POLL_VIA_INT_QUEUE
 #endif
 
 #if !defined CONFIG_ENV_IS_IN_MMC && \