1 files changed, 177 insertions, 183 deletions

diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index 9fbb8d31575a..cab5a4c5baf5 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -115,6 +115,10 @@ module_param_named(nopnp, i8042_nopnp, bool, 0);
 MODULE_PARM_DESC(nopnp, "Do not use PNP to detect controller settings");
 #endif
 
+static bool i8042_forcenorestore;
+module_param_named(forcenorestore, i8042_forcenorestore, bool, 0);
+MODULE_PARM_DESC(forcenorestore, "Force no restore on s3 resume, copying s2idle behaviour");
+
 #define DEBUG
 #ifdef DEBUG
 static bool i8042_debug;
@@ -174,9 +178,9 @@ static unsigned char i8042_suppress_kbd_ack;
 static struct platform_device *i8042_platform_device;
 static struct notifier_block i8042_kbd_bind_notifier_block;
 
-static irqreturn_t i8042_interrupt(int irq, void *dev_id);
-static bool (*i8042_platform_filter)(unsigned char data, unsigned char str,
-				     struct serio *serio);
+static bool i8042_handle_data(int irq);
+static i8042_filter_t i8042_platform_filter;
+static void *i8042_platform_filter_context;
 
 void i8042_lock_chip(void)
 {
@@ -190,45 +194,29 @@ void i8042_unlock_chip(void)
 }
 EXPORT_SYMBOL(i8042_unlock_chip);
 
-int i8042_install_filter(bool (*filter)(unsigned char data, unsigned char str,
-					struct serio *serio))
+int i8042_install_filter(i8042_filter_t filter, void *context)
 {
-	unsigned long flags;
-	int ret = 0;
-
-	spin_lock_irqsave(&i8042_lock, flags);
+	guard(spinlock_irqsave)(&i8042_lock);
 
-	if (i8042_platform_filter) {
-		ret = -EBUSY;
-		goto out;
-	}
+	if (i8042_platform_filter)
+		return -EBUSY;
 
 	i8042_platform_filter = filter;
-
-out:
-	spin_unlock_irqrestore(&i8042_lock, flags);
-	return ret;
+	i8042_platform_filter_context = context;
+	return 0;
 }
 EXPORT_SYMBOL(i8042_install_filter);
 
-int i8042_remove_filter(bool (*filter)(unsigned char data, unsigned char str,
-				       struct serio *port))
+int i8042_remove_filter(i8042_filter_t filter)
 {
-	unsigned long flags;
-	int ret = 0;
+	guard(spinlock_irqsave)(&i8042_lock);
 
-	spin_lock_irqsave(&i8042_lock, flags);
-
-	if (i8042_platform_filter != filter) {
-		ret = -EINVAL;
-		goto out;
-	}
+	if (i8042_platform_filter != filter)
+		return -EINVAL;
 
 	i8042_platform_filter = NULL;
-
-out:
-	spin_unlock_irqrestore(&i8042_lock, flags);
-	return ret;
+	i8042_platform_filter_context = NULL;
+	return 0;
 }
 EXPORT_SYMBOL(i8042_remove_filter);
 
@@ -267,28 +255,22 @@ static int i8042_wait_write(void)
 
 static int i8042_flush(void)
 {
-	unsigned long flags;
 	unsigned char data, str;
 	int count = 0;
-	int retval = 0;
 
-	spin_lock_irqsave(&i8042_lock, flags);
+	guard(spinlock_irqsave)(&i8042_lock);
 
 	while ((str = i8042_read_status()) & I8042_STR_OBF) {
-		if (count++ < I8042_BUFFER_SIZE) {
-			udelay(50);
-			data = i8042_read_data();
-			dbg("%02x <- i8042 (flush, %s)\n",
-			    data, str & I8042_STR_AUXDATA ? "aux" : "kbd");
-		} else {
-			retval = -EIO;
-			break;
-		}
-	}
+		if (count++ >= I8042_BUFFER_SIZE)
+			return -EIO;
 
-	spin_unlock_irqrestore(&i8042_lock, flags);
+		udelay(50);
+		data = i8042_read_data();
+		dbg("%02x <- i8042 (flush, %s)\n",
+		    data, str & I8042_STR_AUXDATA ? "aux" : "kbd");
+	}
 
-	return retval;
+	return 0;
 }
 
 /*
@@ -345,17 +327,12 @@ static int __i8042_command(unsigned char *param, int command)
 
 int i8042_command(unsigned char *param, int command)
 {
-	unsigned long flags;
-	int retval;
-
 	if (!i8042_present)
 		return -1;
 
-	spin_lock_irqsave(&i8042_lock, flags);
-	retval = __i8042_command(param, command);
-	spin_unlock_irqrestore(&i8042_lock, flags);
+	guard(spinlock_irqsave)(&i8042_lock);
 
-	return retval;
+	return __i8042_command(param, command);
 }
 EXPORT_SYMBOL(i8042_command);
 
@@ -365,19 +342,18 @@ EXPORT_SYMBOL(i8042_command);
 
 static int i8042_kbd_write(struct serio *port, unsigned char c)
 {
-	unsigned long flags;
-	int retval = 0;
+	int error;
 
-	spin_lock_irqsave(&i8042_lock, flags);
+	guard(spinlock_irqsave)(&i8042_lock);
 
-	if (!(retval = i8042_wait_write())) {
-		dbg("%02x -> i8042 (kbd-data)\n", c);
-		i8042_write_data(c);
-	}
+	error = i8042_wait_write();
+	if (error)
+		return error;
 
-	spin_unlock_irqrestore(&i8042_lock, flags);
+	dbg("%02x -> i8042 (kbd-data)\n", c);
+	i8042_write_data(c);
 
-	return retval;
+	return 0;
 }
 
 /*
@@ -430,7 +406,7 @@ static void i8042_port_close(struct serio *serio)
 	 * See if there is any data appeared while we were messing with
 	 * port state.
 	 */
-	i8042_interrupt(0, NULL);
+	i8042_handle_data(0);
 }
 
 /*
@@ -456,9 +432,8 @@ static int i8042_start(struct serio *serio)
 		device_set_wakeup_enable(&serio->dev, true);
 	}
 
-	spin_lock_irq(&i8042_lock);
+	guard(spinlock_irq)(&i8042_lock);
 	port->exists = true;
-	spin_unlock_irq(&i8042_lock);
 
 	return 0;
 }
@@ -472,10 +447,10 @@ static void i8042_stop(struct serio *serio)
 {
 	struct i8042_port *port = serio->port_data;
 
-	spin_lock_irq(&i8042_lock);
-	port->exists = false;
-	port->serio = NULL;
-	spin_unlock_irq(&i8042_lock);
+	scoped_guard(spinlock_irq, &i8042_lock) {
+		port->exists = false;
+		port->serio = NULL;
+	}
 
 	/*
 	 * We need to make sure that interrupt handler finishes using
@@ -505,7 +480,10 @@ static bool i8042_filter(unsigned char data, unsigned char str,
 		}
 	}
 
-	if (i8042_platform_filter && i8042_platform_filter(data, str, serio)) {
+	if (!i8042_platform_filter)
+		return false;
+
+	if (i8042_platform_filter(data, str, serio, i8042_platform_filter_context)) {
 		dbg("Filtered out by platform filter\n");
 		return true;
 	}
@@ -514,44 +492,10 @@ static bool i8042_filter(unsigned char data, unsigned char str,
 }
 
 /*
- * i8042_interrupt() is the most important function in this driver -
- * it handles the interrupts from the i8042, and sends incoming bytes
- * to the upper layers.
- */
-
-static irqreturn_t i8042_interrupt(int irq, void *dev_id)
-{
-	struct i8042_port *port;
-	struct serio *serio;
-	unsigned long flags;
-	unsigned char str, data;
-	unsigned int dfl;
-	unsigned int port_no;
-	bool filtered;
-	int ret = 1;
-
-	spin_lock_irqsave(&i8042_lock, flags);
-
-	str = i8042_read_status();
-	if (unlikely(~str & I8042_STR_OBF)) {
-		spin_unlock_irqrestore(&i8042_lock, flags);
-		if (irq)
-			dbg("Interrupt %d, without any data\n", irq);
-		ret = 0;
-		goto out;
-	}
-
-	data = i8042_read_data();
-
-	if (i8042_mux_present && (str & I8042_STR_AUXDATA)) {
-		static unsigned long last_transmit;
-		static unsigned char last_str;
-
-		dfl = 0;
-		if (str & I8042_STR_MUXERR) {
-			dbg("MUX error, status is %02x, data is %02x\n",
-			    str, data);
-/*
+ * i8042_handle_mux() handles case when data is coming from one of
+ * the multiplexed ports. It would be simple if not for quirks with
+ * handling errors:
+ *
  * When MUXERR condition is signalled the data register can only contain
  * 0xfd, 0xfe or 0xff if implementation follows the spec. Unfortunately
  * it is not always the case. Some KBCs also report 0xfc when there is
@@ -563,50 +507,106 @@ static irqreturn_t i8042_interrupt(int irq, void *dev_id)
  * rest assume that the data came from the same serio last byte
  * was transmitted (if transmission happened not too long ago).
  */
-
-			switch (data) {
-				default:
-					if (time_before(jiffies, last_transmit + HZ/10)) {
-						str = last_str;
-						break;
-					}
-					fallthrough;	/* report timeout */
-				case 0xfc:
-				case 0xfd:
-				case 0xfe: dfl = SERIO_TIMEOUT; data = 0xfe; break;
-				case 0xff: dfl = SERIO_PARITY;  data = 0xfe; break;
+static int i8042_handle_mux(u8 str, u8 *data, unsigned int *dfl)
+{
+	static unsigned long last_transmit;
+	static unsigned long last_port;
+	unsigned int mux_port;
+
+	mux_port = (str >> 6) & 3;
+	*dfl = 0;
+
+	if (str & I8042_STR_MUXERR) {
+		dbg("MUX error, status is %02x, data is %02x\n",
+		    str, *data);
+
+		switch (*data) {
+		default:
+			if (time_before(jiffies, last_transmit + HZ/10)) {
+				mux_port = last_port;
+				break;
 			}
+			fallthrough;	/* report timeout */
+		case 0xfc:
+		case 0xfd:
+		case 0xfe:
+			*dfl = SERIO_TIMEOUT;
+			*data = 0xfe;
+			break;
+		case 0xff:
+			*dfl = SERIO_PARITY;
+			*data = 0xfe;
+			break;
 		}
+	}
 
-		port_no = I8042_MUX_PORT_NO + ((str >> 6) & 3);
-		last_str = str;
-		last_transmit = jiffies;
-	} else {
+	last_port = mux_port;
+	last_transmit = jiffies;
 
-		dfl = ((str & I8042_STR_PARITY) ? SERIO_PARITY : 0) |
-		      ((str & I8042_STR_TIMEOUT && !i8042_notimeout) ? SERIO_TIMEOUT : 0);
+	return I8042_MUX_PORT_NO + mux_port;
+}
 
-		port_no = (str & I8042_STR_AUXDATA) ?
-				I8042_AUX_PORT_NO : I8042_KBD_PORT_NO;
-	}
+/*
+ * i8042_handle_data() is the most important function in this driver -
+ * it reads the data from the i8042, determines its destination serio
+ * port, and sends received byte to the upper layers.
+ *
+ * Returns true if there was data waiting, false otherwise.
+ */
+static bool i8042_handle_data(int irq)
+{
+	struct i8042_port *port;
+	struct serio *serio;
+	unsigned char str, data;
+	unsigned int dfl;
+	unsigned int port_no;
+	bool filtered;
+
+	scoped_guard(spinlock_irqsave, &i8042_lock) {
+		str = i8042_read_status();
+		if (unlikely(~str & I8042_STR_OBF))
+			return false;
 
-	port = &i8042_ports[port_no];
-	serio = port->exists ? port->serio : NULL;
+		data = i8042_read_data();
 
-	filter_dbg(port->driver_bound, data, "<- i8042 (interrupt, %d, %d%s%s)\n",
-		   port_no, irq,
-		   dfl & SERIO_PARITY ? ", bad parity" : "",
-		   dfl & SERIO_TIMEOUT ? ", timeout" : "");
+		if (i8042_mux_present && (str & I8042_STR_AUXDATA)) {
+			port_no = i8042_handle_mux(str, &data, &dfl);
+		} else {
 
-	filtered = i8042_filter(data, str, serio);
+			dfl = (str & I8042_STR_PARITY) ? SERIO_PARITY : 0;
+			if ((str & I8042_STR_TIMEOUT) && !i8042_notimeout)
+				dfl |= SERIO_TIMEOUT;
 
-	spin_unlock_irqrestore(&i8042_lock, flags);
+			port_no = (str & I8042_STR_AUXDATA) ?
+					I8042_AUX_PORT_NO : I8042_KBD_PORT_NO;
+		}
+
+		port = &i8042_ports[port_no];
+		serio = port->exists ? port->serio : NULL;
+
+		filter_dbg(port->driver_bound,
+			   data, "<- i8042 (interrupt, %d, %d%s%s)\n",
+			   port_no, irq,
+			   dfl & SERIO_PARITY ? ", bad parity" : "",
+			   dfl & SERIO_TIMEOUT ? ", timeout" : "");
+
+		filtered = i8042_filter(data, str, serio);
+	}
 
 	if (likely(serio && !filtered))
 		serio_interrupt(serio, data, dfl);
 
- out:
-	return IRQ_RETVAL(ret);
+	return true;
+}
+
+static irqreturn_t i8042_interrupt(int irq, void *dev_id)
+{
+	if (unlikely(!i8042_handle_data(irq))) {
+		dbg("Interrupt %d, without any data\n", irq);
+		return IRQ_NONE;
+	}
+
+	return IRQ_HANDLED;
 }
 
 /*
@@ -749,24 +749,22 @@ static bool i8042_irq_being_tested;
 
 static irqreturn_t i8042_aux_test_irq(int irq, void *dev_id)
 {
-	unsigned long flags;
 	unsigned char str, data;
-	int ret = 0;
 
-	spin_lock_irqsave(&i8042_lock, flags);
+	guard(spinlock_irqsave)(&i8042_lock);
+
 	str = i8042_read_status();
-	if (str & I8042_STR_OBF) {
-		data = i8042_read_data();
-		dbg("%02x <- i8042 (aux_test_irq, %s)\n",
-		    data, str & I8042_STR_AUXDATA ? "aux" : "kbd");
-		if (i8042_irq_being_tested &&
-		    data == 0xa5 && (str & I8042_STR_AUXDATA))
-			complete(&i8042_aux_irq_delivered);
-		ret = 1;
-	}
-	spin_unlock_irqrestore(&i8042_lock, flags);
+	if (!(str & I8042_STR_OBF))
+		return IRQ_NONE;
+
+	data = i8042_read_data();
+	dbg("%02x <- i8042 (aux_test_irq, %s)\n",
+	    data, str & I8042_STR_AUXDATA ? "aux" : "kbd");
+
+	if (i8042_irq_being_tested && data == 0xa5 && (str & I8042_STR_AUXDATA))
+		complete(&i8042_aux_irq_delivered);
 
-	return IRQ_RETVAL(ret);
+	return IRQ_HANDLED;
 }
 
 /*
@@ -807,7 +805,6 @@ static int i8042_check_aux(void)
 	int retval = -1;
 	bool irq_registered = false;
 	bool aux_loop_broken = false;
-	unsigned long flags;
 	unsigned char param;
 
 /*
@@ -891,18 +888,15 @@ static int i8042_check_aux(void)
 	if (i8042_enable_aux_port())
 		goto out;
 
-	spin_lock_irqsave(&i8042_lock, flags);
-
-	init_completion(&i8042_aux_irq_delivered);
-	i8042_irq_being_tested = true;
+	scoped_guard(spinlock_irqsave, &i8042_lock) {
+		init_completion(&i8042_aux_irq_delivered);
+		i8042_irq_being_tested = true;
 
-	param = 0xa5;
-	retval = __i8042_command(&param, I8042_CMD_AUX_LOOP & 0xf0ff);
-
-	spin_unlock_irqrestore(&i8042_lock, flags);
-
-	if (retval)
-		goto out;
+		param = 0xa5;
+		retval = __i8042_command(&param, I8042_CMD_AUX_LOOP & 0xf0ff);
+		if (retval)
+			goto out;
+	}
 
 	if (wait_for_completion_timeout(&i8042_aux_irq_delivered,
 					msecs_to_jiffies(250)) == 0) {
@@ -990,7 +984,6 @@ static int i8042_controller_selftest(void)
 
 static int i8042_controller_init(void)
 {
-	unsigned long flags;
 	int n = 0;
 	unsigned char ctr[2];
 
@@ -1027,14 +1020,14 @@ static int i8042_controller_init(void)
  * Handle keylock.
  */
 
-	spin_lock_irqsave(&i8042_lock, flags);
-	if (~i8042_read_status() & I8042_STR_KEYLOCK) {
-		if (i8042_unlock)
-			i8042_ctr |= I8042_CTR_IGNKEYLOCK;
-		else
-			pr_warn("Warning: Keylock active\n");
+	scoped_guard(spinlock_irqsave, &i8042_lock) {
+		if (~i8042_read_status() & I8042_STR_KEYLOCK) {
+			if (i8042_unlock)
+				i8042_ctr |= I8042_CTR_IGNKEYLOCK;
+			else
+				pr_warn("Warning: Keylock active\n");
+		}
 	}
-	spin_unlock_irqrestore(&i8042_lock, flags);
 
 /*
  * If the chip is configured into nontranslated mode by the BIOS, don't
@@ -1212,13 +1205,14 @@ static int i8042_controller_resume(bool s2r_wants_reset)
 	if (i8042_mux_present) {
 		if (i8042_set_mux_mode(true, NULL) || i8042_enable_mux_ports())
 			pr_warn("failed to resume active multiplexor, mouse won't work\n");
-	} else if (i8042_ports[I8042_AUX_PORT_NO].serio)
+	} else if (i8042_ports[I8042_AUX_PORT_NO].serio) {
 		i8042_enable_aux_port();
+	}
 
 	if (i8042_ports[I8042_KBD_PORT_NO].serio)
 		i8042_enable_kbd_port();
 
-	i8042_interrupt(0, NULL);
+	i8042_handle_data(0);
 
 	return 0;
 }
@@ -1232,7 +1226,7 @@ static int i8042_pm_suspend(struct device *dev)
 {
 	int i;
 
-	if (pm_suspend_via_firmware())
+	if (!i8042_forcenorestore && pm_suspend_via_firmware())
 		i8042_controller_reset(true);
 
 	/* Set up serio interrupts for system wakeup. */
@@ -1248,8 +1242,8 @@ static int i8042_pm_suspend(struct device *dev)
 
 static int i8042_pm_resume_noirq(struct device *dev)
 {
-	if (!pm_resume_via_firmware())
-		i8042_interrupt(0, NULL);
+	if (i8042_forcenorestore || !pm_resume_via_firmware())
+		i8042_handle_data(0);
 
 	return 0;
 }
@@ -1271,7 +1265,7 @@ static int i8042_pm_resume(struct device *dev)
 	 * not restore the controller state to whatever it had been at boot
 	 * time, so we do not need to do anything.
 	 */
-	if (!pm_suspend_via_firmware())
+	if (i8042_forcenorestore || !pm_suspend_via_firmware())
 		return 0;
 
 	/*
@@ -1286,7 +1280,7 @@ static int i8042_pm_resume(struct device *dev)
 
 static int i8042_pm_thaw(struct device *dev)
 {
-	i8042_interrupt(0, NULL);
+	i8042_handle_data(0);
 
 	return 0;
 }
@@ -1329,7 +1323,7 @@ static int i8042_create_kbd_port(void)
 	struct serio *serio;
 	struct i8042_port *port = &i8042_ports[I8042_KBD_PORT_NO];
 
-	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
+	serio = kzalloc(sizeof(*serio), GFP_KERNEL);
 	if (!serio)
 		return -ENOMEM;
 
@@ -1359,7 +1353,7 @@ static int i8042_create_aux_port(int idx)
 	int port_no = idx < 0 ? I8042_AUX_PORT_NO : I8042_MUX_PORT_NO + idx;
 	struct i8042_port *port = &i8042_ports[port_no];
 
-	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
+	serio = kzalloc(sizeof(*serio), GFP_KERNEL);
 	if (!serio)
 		return -ENOMEM;
 
@@ -1599,7 +1593,7 @@ static struct platform_driver i8042_driver = {
 #endif
 	},
 	.probe		= i8042_probe,
-	.remove_new	= i8042_remove,
+	.remove		= i8042_remove,
 	.shutdown	= i8042_shutdown,
 };