From 906c1f1aeef2c2969f1e5c42ea954e9572cb26c4 Mon Sep 17 00:00:00 2001
From: Pavel Rojtberg <rojtberg@gmail.com>
Date: Mon, 28 Sep 2015 01:45:02 +0200
Subject: [PATCH] Input: xpad: re-submit pending ff and led requests

Store pending brightness and FF effect in the driver structure and
replace it with the latest requests until the device is ready to process
next request. Alternate serving LED vs FF requests to make sure one does
not starve another. See [1] for discussion. Inspired by patch of Sarah
Bessmer [2].

[1]: http://www.spinics.net/lists/linux-input/msg40708.html
[2]: http://www.spinics.net/lists/linux-input/msg31450.html
---
 xpad.c | 66 +++++++++++++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 56 insertions(+), 10 deletions(-)

diff --git a/xpad.c b/xpad.c
index 8fbe7b0..cf3338c 100644
--- a/xpad.c
+++ b/xpad.c
@@ -335,6 +335,12 @@ struct usb_xpad {
 	dma_addr_t odata_dma;
 	spinlock_t odata_lock;
 
+	unsigned char rum_odata[XPAD_PKT_LEN]; /* cache for rumble data */
+	unsigned char led_odata[XPAD_PKT_LEN]; /* cache for led data */
+	unsigned pend_rum;               /* length of cached rumble data */
+	unsigned pend_led;               /* length of cached led data */
+	int force_led;                   /* force send led cache next */
+
 #if defined(CONFIG_JOYSTICK_XPAD_LEDS)
 	struct xpad_led *led;
 #endif
@@ -704,14 +710,35 @@ static void xpad_irq_out(struct urb *urb)
 	struct usb_xpad *xpad = urb->context;
 	struct device *dev = &xpad->intf->dev;
 	int retval, status;
+	unsigned long flags;
 
 	status = urb->status;
 
 	switch (status) {
 	case 0:
 		/* success */
-		xpad->irq_out_active = 0;
-		return;
+		if(!xpad->pend_led && !xpad->pend_rum) {
+			xpad->irq_out_active = 0;
+			return;
+		}
+
+		spin_lock_irqsave(&xpad->odata_lock, flags);
+
+		if(xpad->pend_led && (!xpad->pend_rum || xpad->force_led)) {
+			xpad->irq_out->transfer_buffer_length = xpad->pend_led;
+			memcpy(xpad->odata, xpad->led_odata, xpad->pend_led);
+			xpad->pend_led = 0;
+			xpad->force_led = 0;
+			dev_dbg(dev, "%s - sending pending led\n", __func__);
+			break;
+		}
+
+		xpad->irq_out->transfer_buffer_length = xpad->pend_rum;
+		memcpy(xpad->odata, xpad->rum_odata, xpad->pend_rum);
+		xpad->pend_rum = 0;
+		xpad->force_led = 1;
+		dev_dbg(dev, "%s - sending pending rumble\n", __func__);
+		break;
 
 	case -ECONNRESET:
 	case -ENOENT:
@@ -725,11 +752,13 @@ static void xpad_irq_out(struct urb *urb)
 	default:
 		dev_dbg(dev, "%s - nonzero urb status received: %d\n",
 			__func__, status);
-		goto exit;
+
+		spin_lock_irqsave(&xpad->odata_lock, flags);
+		break;
 	}
 
-exit:
 	retval = usb_submit_urb(urb, GFP_ATOMIC);
+	spin_unlock_irqrestore(&xpad->odata_lock, flags);
 	if (retval)
 		dev_err(dev, "%s - usb_submit_urb failed with result %d\n",
 			__func__, retval);
@@ -752,6 +781,9 @@ static int xpad_init_output(struct usb_interface *intf, struct usb_xpad *xpad)
 	}
 
 	spin_lock_init(&xpad->odata_lock);
+	xpad->pend_led = 0;
+	xpad->pend_rum = 0;
+	xpad->force_led = 0;
 
 	xpad->irq_out = usb_alloc_urb(0, GFP_KERNEL);
 	if (!xpad->irq_out) {
@@ -876,9 +908,17 @@ static int xpad_play_effect(struct input_dev *dev, void *data, struct ff_effect
 		retval = usb_submit_urb(xpad->irq_out, GFP_ATOMIC);
 		xpad->irq_out_active = 1;
 	} else {
-		retval = -EIO;
-		dev_dbg(&xpad->dev->dev, "%s - dropped, irq_out is active\n",
-				__func__);
+		retval = 0;
+
+		if(xpad->pend_rum) {
+			dev_dbg(&xpad->dev->dev,
+				"%s - overwriting pending\n", __func__);
+
+			retval = -EIO;
+		}
+
+		xpad->pend_rum = xpad->irq_out->transfer_buffer_length;
+		memcpy(xpad->rum_odata, xpad->odata, xpad->pend_rum);
 	}
 
 	spin_unlock_irqrestore(&xpad->odata_lock, flags);
@@ -964,9 +1004,15 @@ static void xpad_send_led_command(struct usb_xpad *xpad, int command)
 	if (!xpad->irq_out_active) {
 		usb_submit_urb(xpad->irq_out, GFP_KERNEL);
 		xpad->irq_out_active = 1;
-	} else
-		dev_dbg(&xpad->dev->dev, "%s - dropped, irq_out is active\n",
-				__func__);
+	} else {
+		if(xpad->pend_led) {
+			dev_dbg(&xpad->dev->dev,
+				"%s - overwriting pending\n", __func__);
+		}
+
+		xpad->pend_led = xpad->irq_out->transfer_buffer_length;
+		memcpy(xpad->led_odata, xpad->odata, xpad->pend_led);
+	}
 
 	spin_unlock_irqrestore(&xpad->odata_lock, flags);
 }