Cleanups; move the mode switching to a separate function

[Andres Salomon]

Commit:     a4eb5a153fa345c350ab345003e205c6f8511e24
Parent:     113d3f2084899a0f7038d7b629da3c015b9ecb14
commit a4eb5a153fa345c350ab345003e205c6f8511e24
Author:     Andres Salomon <dilinger at debian.org>
AuthorDate: Wed Dec 27 15:52:51 2006 -0500
Commit:     Andres Salomon <dilinger at debian.org>
CommitDate: Wed Dec 27 15:52:51 2006 -0500

    Cleanups; move the mode switching to a separate function
    
    The mode switching code now sits in its own function.
    
    Signed-off-by: Andres Salomon <dilinger at debian.org>
---
 drivers/input/mouse/olpc.c |  129 +++++++++++++++++++-------------------------
 1 files changed, 56 insertions(+), 73 deletions(-)

diff --git a/drivers/input/mouse/olpc.c b/drivers/input/mouse/olpc.c
index a2f3af6..4e064a2 100644
--- a/drivers/input/mouse/olpc.c
+++ b/drivers/input/mouse/olpc.c
@@ -238,42 +238,72 @@ static int olpc_find_mode (struct psmous
 	return priv->initial;
 }
 
-static int olpc_absolute_mode(struct psmouse *psmouse, int mode)
+/*
+ * Touchpad should be disabled before calling this!
+ */
+static int olpc_new_mode(struct psmouse *psmouse, int mode)
 {
 	struct ps2dev *ps2dev = &psmouse->ps2dev;
+	struct olpc_data *priv = psmouse->private;
 	unsigned char param;
+	int ret;
 
-	/* Switch to 'Advanced mode.', four disables in a row. */
-	if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE) ||
-			ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE) ||
-			ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE) ||
-			ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE))
-		return -1;
+	if (tpdebug)
+		printk(KERN_WARNING __FILE__ ": Switching to %d. [%lu]\n", mode, jiffies);
 
-	if (ps2_command(ps2dev, &param, 0x01F2))
-		return -3;
-	if (ps2_command(ps2dev, &param, 0x01F2))
-		return -4;
-	if (ps2_command(ps2dev, &param, 0x01F2))
-		return -5;
+	if ((ret = ps2_command(ps2dev, &param, 0x01F2)))
+		goto failed;
+	if ((ret = ps2_command(ps2dev, &param, 0x01F2)))
+		goto failed;
+	if ((ret = ps2_command(ps2dev, &param, 0x01F2)))
+		goto failed;
 
-	switch(mode) {
+	switch (mode) {
 	default:
-		printk(KERN_WARNING __FILE__ ": Invalid tpmode %d. Default to GS mode\n", tpmode);
-	case OLPC_GS: /* GS only */
-		if (ps2_command(ps2dev, NULL, 0xE6))
-			return -8;
+		printk(KERN_WARNING __FILE__ ": Invalid mode %d. Defaulting to OLPC_GS.\n", mode);
+	case OLPC_GS:
+		ret = ps2_command(ps2dev, NULL, 0xE6);
 		break;
-	case OLPC_PT: /* PT only */
-		if (ps2_command(ps2dev, NULL, 0xE7))
-			return -9;
+	case OLPC_PT:
+		ret = ps2_command(ps2dev, NULL, 0xE7);
 		break;
 	}
+	if (ret)
+		goto failed;
+
+	/* XXX: This is a bit hacky, make sure this isn't screwing stuff up. */
+	psmouse->pktcnt = psmouse->out_of_sync = 0;
+	psmouse->last = jiffies;
+	psmouse->state = PSMOUSE_ACTIVATED;
+
+	if ((ret = ps2_command(ps2dev, NULL, PSMOUSE_CMD_ENABLE)))
+		goto failed;
+
+	priv->current_mode = mode;
+	priv->pending_mode = 0;
+	if (tpdebug)
+		printk(KERN_WARNING __FILE__ ": Switched to mode %d successful.\n", mode);
+
+failed:
+	if (ret) {
+		/* XXX: if this ever fails, we need to do a full reset! */
+		printk(KERN_WARNING __FILE__ ": Mode switch to %d failed. (%d) [%lu]\n", mode, ret, jiffies);
+	}
+	return ret;
+}
 
-	if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_ENABLE))
-		return -10;
+static int olpc_absolute_mode(struct psmouse *psmouse, int mode)
+{
+	struct ps2dev *ps2dev = &psmouse->ps2dev;
 
-	return 0;
+	/* Switch to 'Advanced mode.', four disables in a row. */
+	if (ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE) ||
+			ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE) ||
+			ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE) ||
+			ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE))
+		return -1;
+	
+	return olpc_new_mode(psmouse, mode);
 }
 
 /*
@@ -321,7 +351,6 @@ static void olpc_mode_switch(void *p)
 	struct olpc_data *priv = psmouse->private;
 	struct ps2dev *ps2dev = &psmouse->ps2dev;
 	int pending_mode, ret;
-	unsigned char param[3];
 
 	if (priv->pending_mode == priv->current_mode) {
 		priv->pending_mode = 0;
@@ -335,7 +364,7 @@ static void olpc_mode_switch(void *p)
 	/* XXX: This is a bit hacky, make sure this isn't screwing stuff up. */
 	psmouse->state = PSMOUSE_INITIALIZING;
 
-	ret = ps2_command(ps2dev, NULL, 0xF5);
+	ret = ps2_command(ps2dev, NULL, PSMOUSE_CMD_DISABLE);
 	if (ret) {
 		/* XXX: if this ever fails, we need to do a full reset! */
 		printk(KERN_WARNING __FILE__ ": Disable failed for switch to %d. (%d) [%lu]\n", priv->pending_mode, ret, jiffies);
@@ -349,54 +378,8 @@ static void olpc_mode_switch(void *p)
 	 * hardware sends back its ACK, it has stopped sending bytes).
 	 */
 	pending_mode = priv->pending_mode;
-	if (tpdebug)
-		printk(KERN_WARNING __FILE__ ": Switching to %d. [%lu]\n", pending_mode, jiffies);
-
-	if (ps2_command(ps2dev, param, 0x01F2)) {
-		ret = 3;
+	if (olpc_new_mode(psmouse, priv->pending_mode))
 		goto bad;
-	}
-	if (ps2_command(ps2dev, param, 0x01F2)) {
-		ret = 4;
-		goto bad;
-	}
-	if (ps2_command(ps2dev, param, 0x01F2)) {
-		ret = 5;
-		goto bad;
-	}
-
-	switch (pending_mode) {
-	default:
-		printk(KERN_WARNING __FILE__ ": Invalid tpmode %d. Default to simultaneous mode\n", tpmode);
-	case OLPC_GS: /* GS only */
-		if (ps2_command(ps2dev, NULL, 0xE6)) {
-			ret = -20;
-			goto bad;
-		}
-		break;
-	case OLPC_PT: /* PT only */
-		if (ps2_command(ps2dev, NULL, 0xE7)) {
-			ret = -30;
-			goto bad;
-		}
-		break;
-	}
-
-	/* XXX: This is a bit hacky, make sure this isn't screwing stuff up. */
-	psmouse->pktcnt = psmouse->out_of_sync = 0;
-	psmouse->last = jiffies;
-	psmouse->state = PSMOUSE_ACTIVATED;
-
-	ret = ps2_command(ps2dev, NULL, 0xF4);
-	if (ret) {
-		printk(KERN_WARNING __FILE__ ": Enable failed for switch to %d. (%d) [%lu]\n", pending_mode, ret, jiffies);
-	}
-	else {
-		priv->current_mode = pending_mode;
-		if (tpdebug)
-			printk(KERN_WARNING __FILE__ ": Switched to %d: %d.\n", pending_mode, ret);
-	}
-	priv->pending_mode = 0;
 	return;
 
 bad: