[14/22] usb: Add packet combining functions

diff --git a/hw/usb.h b/hw/usb.h
index 3a6cc84..3c8ba01 100644
--- a/hw/usb.h
+++ b/hw/usb.h
@@ -160,6 +160,7 @@  typedef struct USBBusOps USBBusOps;
 typedef struct USBPort USBPort;
 typedef struct USBDevice USBDevice;
 typedef struct USBPacket USBPacket;
+typedef struct USBCombinedPacket USBCombinedPacket;
 typedef struct USBEndpoint USBEndpoint;
 
 typedef struct USBDesc USBDesc;
@@ -292,6 +293,14 @@  typedef struct USBDeviceClass {
      */
     int (*handle_data)(USBDevice *dev, USBPacket *p);
 
+    /*
+     * Process / cancel combined packets, called from
+     * usb_ep_combine_input_packets() / usb_combined_packet_cancel().
+     * Only called for devices which call these functions themselves.
+     */
+    int (*handle_combined_data)(USBDevice *dev, USBPacket *p);
+    void (*cancel_combined_packet)(USBDevice *dev, USBPacket *p);
+
     void (*set_interface)(USBDevice *dev, int interface,
                           int alt_old, int alt_new);
 
@@ -356,7 +365,15 @@  struct USBPacket {
     int result; /* transfer length or USB_RET_* status code */
     /* Internal use by the USB layer.  */
     USBPacketState state;
+    USBCombinedPacket *combined;
     QTAILQ_ENTRY(USBPacket) queue;
+    QTAILQ_ENTRY(USBPacket) combined_entry;
+};
+
+struct USBCombinedPacket {
+    USBPacket *first;
+    QTAILQ_HEAD(packets_head, USBPacket) packets;
+    QEMUIOVector iov;
 };
 
 void usb_packet_init(USBPacket *p);
@@ -399,6 +416,10 @@  void usb_ep_set_pipeline(USBDevice *dev, int pid, int ep, bool enabled);
 USBPacket *usb_ep_find_packet_by_id(USBDevice *dev, int pid, int ep,
                                     uint64_t id);
 
+void usb_ep_combine_input_packets(USBEndpoint *ep);
+void usb_combined_input_packet_complete(USBDevice *dev, USBPacket *p);
+void usb_combined_packet_cancel(USBDevice *dev, USBPacket *p);
+
 void usb_attach(USBPort *port);
 void usb_detach(USBPort *port);
 void usb_port_reset(USBPort *port);
diff --git a/hw/usb/Makefile.objs b/hw/usb/Makefile.objs
index 4225136..b193321 100644
--- a/hw/usb/Makefile.objs
+++ b/hw/usb/Makefile.objs
@@ -7,7 +7,7 @@  hw-obj-y += libhw.o
 hw-obj-$(CONFIG_SMARTCARD) += dev-smartcard-reader.o
 hw-obj-$(CONFIG_USB_REDIR) += redirect.o
 
-common-obj-y += core.o bus.o desc.o dev-hub.o
+common-obj-y += core.o combined-packet.o bus.o desc.o dev-hub.o
 common-obj-y += host-$(HOST_USB).o dev-bluetooth.o
 common-obj-y += dev-hid.o dev-storage.o dev-wacom.o
 common-obj-y += dev-serial.o dev-network.o dev-audio.o
diff --git a/hw/usb/combined-packet.c b/hw/usb/combined-packet.c
new file mode 100644
index 0000000..b6fcb2c
--- /dev/null
+++ b/hw/usb/combined-packet.c
@@ -0,0 +1,183 @@ 
+/*
+ * QEMU USB packet combining code (for input pipelining)
+ *
+ * Copyright(c) 2012 Red Hat, Inc.
+ *
+ * Red Hat Authors:
+ * Hans de Goede <hdegoede@redhat.com>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or(at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+#include "qemu-common.h"
+#include "hw/usb.h"
+#include "iov.h"
+#include "trace.h"
+
+static void usb_combined_packet_add(USBCombinedPacket *combined, USBPacket *p)
+{
+    qemu_iovec_concat(&combined->iov, &p->iov, 0, p->iov.size);
+    QTAILQ_INSERT_TAIL(&combined->packets, p, combined_entry);
+    p->combined = combined;
+}
+
+static void usb_combined_packet_remove(USBCombinedPacket *combined,
+                                       USBPacket *p)
+{
+    assert(p->combined == combined);
+    p->combined = NULL;
+    QTAILQ_REMOVE(&combined->packets, p, combined_entry);
+}
+
+/* Also handles completion of non combined packets for pipelined input eps */
+void usb_combined_input_packet_complete(USBDevice *dev, USBPacket *p)
+{
+    USBCombinedPacket *combined = p->combined;
+    USBEndpoint *ep = p->ep;
+    USBPacket *next;
+    enum { completing, complete, leftover };
+    int result, state = completing;
+    bool short_not_ok;
+
+    if (combined == NULL) {
+        usb_packet_complete_one(dev, p);
+        goto leave;
+    }
+
+    assert(combined->first == p && p == QTAILQ_FIRST(&combined->packets));
+
+    result = combined->first->result;
+    short_not_ok = QTAILQ_LAST(&combined->packets, packets_head)->short_not_ok;
+
+    QTAILQ_FOREACH_SAFE(p, &combined->packets, combined_entry, next) {
+        if (state == completing) {
+            /* Distribute data over uncombined packets */
+            if (result >= p->iov.size) {
+                p->result = p->iov.size;
+            } else {
+                /* Send short or error packet to complete the transfer */
+                p->result = result;
+                state = complete;
+            }
+            p->short_not_ok = short_not_ok;
+            usb_combined_packet_remove(combined, p);
+            usb_packet_complete_one(dev, p);
+            result -= p->result;
+        } else {
+            /* Remove any leftover packets from the queue */
+            state = leftover;
+            p->result = USB_RET_REMOVE_FROM_QUEUE;
+            dev->port->ops->complete(dev->port, p);
+        }
+    }
+    /* 
+     * If we had leftover packets the hcd driver will have cancelled them
+     * and usb_combined_packet_cancel has already freed combined!
+     */
+    if (state != leftover) {
+        g_free(combined);
+    }
+leave:
+    /* Check if there are packets in the queue waiting for our completion */
+    usb_ep_combine_input_packets(ep);
+}
+
+/* May only be called for combined packets! */
+void usb_combined_packet_cancel(USBDevice *dev, USBPacket *p)
+{
+    USBCombinedPacket *combined = p->combined;
+    USBDeviceClass *klass = USB_DEVICE_GET_CLASS(dev);
+    assert(combined != NULL);
+    assert(klass->cancel_combined_packet != NULL);
+
+    usb_combined_packet_remove(combined, p);
+    if (p == combined->first) {
+        klass->cancel_combined_packet(dev, p);
+    }
+    if (QTAILQ_EMPTY(&combined->packets)) {
+        g_free(combined);
+    }
+}
+
+/*
+ * Large input transfers can get split into multiple input packets, this
+ * function recombines them, removing the short_not_ok checks which all but
+ * the last packet of such splits transfers have, thereby allowing input
+ * transfer pipelining (which we cannot do on short_not_ok transfers)
+ */
+void usb_ep_combine_input_packets(USBEndpoint *ep)
+{
+    USBPacket *p, *u, *next, *prev = NULL, *first = NULL;
+    USBPort *port = ep->dev->port;
+    USBDeviceClass *klass = USB_DEVICE_GET_CLASS(ep->dev);
+    int ret;
+
+    assert(ep->pipeline);
+    assert(ep->pid == USB_TOKEN_IN);
+    assert(klass->handle_combined_data != NULL);
+
+    QTAILQ_FOREACH_SAFE(p, &ep->queue, queue, next) {
+        /* Empty the queue on a halt */
+        if (ep->halted) {
+            p->result = USB_RET_REMOVE_FROM_QUEUE;
+            port->ops->complete(port, p);
+            continue;
+        }
+
+        /* Skip packets already submitted to the device */
+        if (p->state == USB_PACKET_ASYNC) {
+            prev = p;
+            continue;
+        }
+        usb_packet_check_state(p, USB_PACKET_QUEUED);
+
+        /*
+         * If the previous (combined) packet has the short_not_ok flag set
+         * stop, as we must not submit packets to the device after a transfer
+         * ending with short_not_ok packet.
+         */
+        if (prev && prev->short_not_ok) {
+            break;
+        }
+
+        if (first) {
+            if (first->combined == NULL) {
+                USBCombinedPacket *combined = g_new0(USBCombinedPacket, 1);
+
+                combined->first = first;
+                QTAILQ_INIT(&combined->packets);
+                qemu_iovec_init(&combined->iov, 2);
+                usb_combined_packet_add(combined, first);
+            }
+            usb_combined_packet_add(first->combined, p);
+        } else {
+            first = p;
+        }
+
+        /* Is this packet the last one of a (combined) transfer? */
+        if ((p->iov.size % ep->max_packet_size) != 0 || !p->short_not_ok ||
+                next == NULL) {
+            ret = klass->handle_combined_data(ep->dev, first);
+            assert(ret == USB_RET_ASYNC);
+            if (first->combined) {
+                QTAILQ_FOREACH(u, &first->combined->packets, combined_entry) {
+                    usb_packet_set_state(u, USB_PACKET_ASYNC);
+                }
+            } else {
+                usb_packet_set_state(first, USB_PACKET_ASYNC);
+            }
+            first = NULL;
+            prev = p;
+        }
+    }
+}
diff --git a/hw/usb/core.c b/hw/usb/core.c
index 87a513f..99b9fdb 100644
--- a/hw/usb/core.c
+++ b/hw/usb/core.c
@@ -544,6 +544,7 @@  void usb_packet_setup(USBPacket *p, int pid, USBEndpoint *ep, uint64_t id,
     p->parameter = 0;
     p->short_not_ok = short_not_ok;
     p->int_req = int_req;
+    p->combined = NULL;
     qemu_iovec_reset(&p->iov);
     usb_packet_set_state(p, USB_PACKET_SETUP);
 }