1 files changed, 49 insertions, 31 deletions

diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index f0b1060a7fee..adf7f05c643a 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -2049,14 +2049,11 @@ static int alps_hw_init_v1_v2(struct psmouse *psmouse)
 	return 0;
 }
 
-static int alps_hw_init_v6(struct psmouse *psmouse)
+/* Must be in passthrough mode when calling this function */
+static int alps_trackstick_enter_extended_mode_v3_v6(struct psmouse *psmouse)
 {
 	unsigned char param[2] = {0xC8, 0x14};
 
-	/* Enter passthrough mode to let trackpoint enter 6byte raw mode */
-	if (alps_passthrough_mode_v2(psmouse, true))
-		return -1;
-
 	if (ps2_command(&psmouse->ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
 	    ps2_command(&psmouse->ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
 	    ps2_command(&psmouse->ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
@@ -2064,9 +2061,25 @@ static int alps_hw_init_v6(struct psmouse *psmouse)
 	    ps2_command(&psmouse->ps2dev, &param[1], PSMOUSE_CMD_SETRATE))
 		return -1;
 
+	return 0;
+}
+
+static int alps_hw_init_v6(struct psmouse *psmouse)
+{
+	int ret;
+
+	/* Enter passthrough mode to let trackpoint enter 6byte raw mode */
+	if (alps_passthrough_mode_v2(psmouse, true))
+		return -1;
+
+	ret = alps_trackstick_enter_extended_mode_v3_v6(psmouse);
+
 	if (alps_passthrough_mode_v2(psmouse, false))
 		return -1;
 
+	if (ret)
+		return ret;
+
 	if (alps_absolute_mode_v6(psmouse)) {
 		psmouse_err(psmouse, "Failed to enable absolute mode\n");
 		return -1;
@@ -2140,10 +2153,18 @@ error:
 
 static int alps_setup_trackstick_v3(struct psmouse *psmouse, int reg_base)
 {
-	struct ps2dev *ps2dev = &psmouse->ps2dev;
 	int ret = 0;
+	int reg_val;
 	unsigned char param[4];
 
+	/*
+	 * We need to configure trackstick to report data for touchpad in
+	 * extended format. And also we need to tell touchpad to expect data
+	 * from trackstick in extended format. Without this configuration
+	 * trackstick packets sent from touchpad are in basic format which is
+	 * different from what we expect.
+	 */
+
 	if (alps_passthrough_mode_v3(psmouse, reg_base, true))
 		return -EIO;
 
@@ -2161,39 +2182,36 @@ static int alps_setup_trackstick_v3(struct psmouse *psmouse, int reg_base)
 		ret = -ENODEV;
 	} else {
 		psmouse_dbg(psmouse, "trackstick E7 report: %3ph\n", param);
-
-		/*
-		 * Not sure what this does, but it is absolutely
-		 * essential. Without it, the touchpad does not
-		 * work at all and the trackstick just emits normal
-		 * PS/2 packets.
-		 */
-		if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
-		    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
-		    ps2_command(ps2dev, NULL, PSMOUSE_CMD_SETSCALE11) ||
-		    alps_command_mode_send_nibble(psmouse, 0x9) ||
-		    alps_command_mode_send_nibble(psmouse, 0x4)) {
-			psmouse_err(psmouse,
-				    "Error sending magic E6 sequence\n");
+		if (alps_trackstick_enter_extended_mode_v3_v6(psmouse)) {
+			psmouse_err(psmouse, "Failed to enter into trackstick extended mode\n");
 			ret = -EIO;
-			goto error;
 		}
+	}
+
+	if (alps_passthrough_mode_v3(psmouse, reg_base, false))
+		return -EIO;
+
+	if (ret)
+		return ret;
 
+	if (alps_enter_command_mode(psmouse))
+		return -EIO;
+
+	reg_val = alps_command_mode_read_reg(psmouse, reg_base + 0x08);
+	if (reg_val == -1) {
+		ret = -EIO;
+	} else {
 		/*
-		 * This ensures the trackstick packets are in the format
-		 * supported by this driver. If bit 1 isn't set the packet
-		 * format is different.
+		 * Tell touchpad that trackstick is now in extended mode.
+		 * If bit 1 isn't set the packet format is different.
 		 */
-		if (alps_enter_command_mode(psmouse) ||
-		    alps_command_mode_write_reg(psmouse,
-						reg_base + 0x08, 0x82) ||
-		    alps_exit_command_mode(psmouse))
+		reg_val |= BIT(1);
+		if (__alps_command_mode_write_reg(psmouse, reg_val))
 			ret = -EIO;
 	}
 
-error:
-	if (alps_passthrough_mode_v3(psmouse, reg_base, false))
-		ret = -EIO;
+	if (alps_exit_command_mode(psmouse))
+		return -EIO;
 
 	return ret;
 }