From 0dbecb0131904f74e0198f82f7e0a17f5cbd5976 Mon Sep 17 00:00:00 2001
From: Antonio Huete Jimenez <tuxillo@quantumachine.net>
Date: Mon, 11 Oct 2010 12:22:12 +0200
Subject: [PATCH] psm - Reduce jitter

---
 sys/dev/misc/psm/psm.c |  369 +++++++++++++++++++++++++++++++++--------------
 1 files changed, 259 insertions(+), 110 deletions(-)

diff --git a/sys/dev/misc/psm/psm.c b/sys/dev/misc/psm/psm.c
index 8fa8054..bee0278 100644
--- a/sys/dev/misc/psm/psm.c
+++ b/sys/dev/misc/psm/psm.c
@@ -75,6 +75,7 @@
 #include <sys/syslog.h>
 #include <sys/malloc.h>
 #include <sys/rman.h>
+#include <sys/sysctl.h>
 #include <sys/thread2.h>
 #include <sys/time.h>
 #include <sys/uio.h>
@@ -148,6 +149,16 @@ typedef struct ringbuf {
     unsigned char buf[PSM_BUFSIZE];
 } ringbuf_t;
 
+/* data buffer */
+typedef struct packetbuf {
+    unsigned char ipacket[16]; /* interim input buffer */
+    int           inputbytes;  /* # of bytes in the input buffer */
+} packetbuf_t;
+
+#ifndef PSM_PACKETQUEUE
+#define PSM_PACKETQUEUE        128
+#endif
+
 /* driver control block */
 struct psm_softc {		/* Driver status information */
     int		  unit;
@@ -163,15 +174,22 @@ struct psm_softc {		/* Driver status information */
     mousemode_t   dflt_mode;	/* default operation mode */
     mousestatus_t status;	/* accumulated mouse movement */
     ringbuf_t     queue;	/* mouse status queue */
-    unsigned char ipacket[16];	/* interim input buffer */
+    packetbuf_t   pqueue[PSM_PACKETQUEUE];   /* mouse data queue */
+    int           pqueue_start; /* start of data in queue */
+    int           pqueue_end;   /* end of data in queue */
     int           inputbytes;	/* # of bytes in the input buffer */
     int           button;	/* the latest button state */
     int		  xold;	/* previous absolute X position */
     int		  yold;	/* previous absolute Y position */
-    int		  syncerrors;
+    int		  syncerrors; /* XXX: KILL ME! */
     struct timeval inputtimeout;
+    struct timeval lastsoftintr;        /* time of last soft interrupt */
+    struct timeval lastinputerr;        /* time last sync error happened */
     int		  watchdog;	/* watchdog timer flag */
     struct callout callout;	/* watchdog timer call out */
+    struct callout softcallout;  /* buffer timer call out */
+    int           lasterr;
+    int           cmdcount;
 };
 devclass_t psm_devclass;
 #define PSM_SOFTC(unit)	((struct psm_softc*)devclass_get_softc(psm_devclass, unit))
@@ -180,6 +198,7 @@ devclass_t psm_devclass;
 #define PSM_VALID		0x80
 #define PSM_OPEN		1	/* Device is open */
 #define PSM_ASLP		2	/* Waiting for mouse data */
+#define PSM_SOFTARMED           4       /* Software interrupt armed */
 
 /* driver configuration flags (config) */
 #define PSM_CONFIG_RESOLUTION	0x000f	/* resolution */
@@ -258,8 +277,12 @@ static int doinitialize (struct psm_softc *, mousemode_t *);
 static int doopen (struct psm_softc *, int);
 static int reinitialize (struct psm_softc *, int);
 static char *model_name (int);
+static void psmsoftintr(void *);
 static void psmintr (void *);
 static void psmtimeout (void *);
+static int timeelapsed(const struct timeval *, int, int, const struct timeval *);
+static void dropqueue(struct psm_softc *);
+static void flushpackets(struct psm_softc *);
 static void psmfilter_detach(struct knote *);
 static int psmfilter(struct knote *, long);
 
@@ -278,7 +301,8 @@ static probefunc_t enable_4dmouse;
 static probefunc_t enable_4dplus;
 static probefunc_t enable_mmanplus;
 static probefunc_t enable_versapad;
-static int tame_mouse (struct psm_softc *, mousestatus_t *, unsigned char *);
+static int tame_mouse (struct psm_softc *, packetbuf_t *, mousestatus_t *,
+    unsigned char *);
 
 static struct {
     int                 model;
@@ -832,7 +856,7 @@ reinitialize(struct psm_softc *sc, int doinit)
 	disable_aux_dev(sc->kbdc);	/* this may fail; but never mind... */
 	empty_aux_buffer(sc->kbdc, 10);
     }
-    sc->inputbytes = 0;
+    flushpackets(sc);
     sc->syncerrors = 0;
 
     /* try to detect the aux device; are you still there? */
@@ -1225,6 +1249,7 @@ psmattach(device_t dev)
     /* Setup initial state */
     sc->state = PSM_VALID;
     callout_init(&sc->callout);
+    callout_init(&sc->softcallout);
 
     /* Setup our interrupt handler */
     rid = 0;
@@ -1324,10 +1349,11 @@ psmopen(struct dev_open_args *ap)
     sc->status.dy = 0;
     sc->status.dz = 0;
     sc->button = 0;
+    sc->pqueue_start = 0;
+    sc->pqueue_end = 0;
 
     /* empty input buffer */
-    bzero(sc->ipacket, sizeof(sc->ipacket));
-    sc->inputbytes = 0;
+    flushpackets(sc);
     sc->syncerrors = 0;
 
     /* don't let timeout routines in the keyboard driver to poll the kbdc */
@@ -1456,7 +1482,8 @@ psmclose(struct dev_close_args *ap)
 }
 
 static int
-tame_mouse(struct psm_softc *sc, mousestatus_t *status, unsigned char *buf)
+tame_mouse(struct psm_softc *sc, packetbuf_t *pb, mousestatus_t *status,
+    unsigned char *buf)
 {
     static unsigned char butmapps2[8] = {
         0,
@@ -1510,7 +1537,7 @@ tame_mouse(struct psm_softc *sc, mousestatus_t *status, unsigned char *buf)
         buf[7] = (~status->button >> 3) & 0x7f;
 	return MOUSE_SYS_PACKETSIZE;
     }
-    return sc->inputbytes;
+    return pb->inputbytes;
 }
 
 static int
@@ -1606,12 +1633,35 @@ block_mouse_data(struct psm_softc *sc, int *c)
      */
     empty_aux_buffer(sc->kbdc, 0);	/* flush the queue */
     read_aux_data_no_wait(sc->kbdc);	/* throw away data if any */
-    sc->inputbytes = 0;
+    flushpackets(sc);
     crit_exit();
 
     return 0;
 }
 
+
+static void
+dropqueue(struct psm_softc *sc)
+{
+
+       sc->queue.count = 0;
+       sc->queue.head = 0;
+       sc->queue.tail = 0;
+       if ((sc->state & PSM_SOFTARMED) != 0) {
+               sc->state &= ~PSM_SOFTARMED;
+               callout_stop(&sc->softcallout);
+       }
+       sc->pqueue_start = sc->pqueue_end;
+}
+
+static void
+flushpackets(struct psm_softc *sc)
+{
+       dropqueue(sc);
+       bzero(&sc->pqueue, sizeof(sc->pqueue));
+}
+
+
 static int
 unblock_mouse_data(struct psm_softc *sc, int c)
 {
@@ -1954,93 +2004,161 @@ psmtimeout(void *arg)
     crit_exit();
 }
 
+static int psmhz = 20;
+SYSCTL_INT(_debug, OID_AUTO, psmhz, CTLFLAG_RW, &psmhz, 0, "");
+
+static int psm_soft_timeout = 500000; /* 0.5 sec */
+SYSCTL_INT(_debug, OID_AUTO, psm_soft_timeout, CTLFLAG_RW,
+    &psm_soft_timeout, 0, "");
+
+static int psmerrsecs = 2;
+SYSCTL_INT(_debug, OID_AUTO, psmerrsecs, CTLFLAG_RW, &psmerrsecs, 0, "");
+static int psmerrusecs = 0;
+SYSCTL_INT(_debug, OID_AUTO, psmerrusecs, CTLFLAG_RW, &psmerrusecs, 0, "");
+static int psmsecs = 0;
+SYSCTL_INT(_debug, OID_AUTO, psmsecs, CTLFLAG_RW, &psmsecs, 0, "");
+static int psmusecs = 500000;
+SYSCTL_INT(_debug, OID_AUTO, psmusecs, CTLFLAG_RW, &psmusecs, 0, "");
+
 static void
 psmintr(void *arg)
 {
-    /*
-     * the table to turn PS/2 mouse button bits (MOUSE_PS2_BUTTON?DOWN)
-     * into `mousestatus' button bits (MOUSE_BUTTON?DOWN).
-     */
-    static int butmap[8] = {
-        0, 
-	MOUSE_BUTTON1DOWN, 
-	MOUSE_BUTTON3DOWN, 
-	MOUSE_BUTTON1DOWN | MOUSE_BUTTON3DOWN, 
-	MOUSE_BUTTON2DOWN, 
-	MOUSE_BUTTON1DOWN | MOUSE_BUTTON2DOWN, 
-	MOUSE_BUTTON2DOWN | MOUSE_BUTTON3DOWN,
-        MOUSE_BUTTON1DOWN | MOUSE_BUTTON2DOWN | MOUSE_BUTTON3DOWN
-    };
-    static int butmap_versapad[8] = {
-	0, 
-	MOUSE_BUTTON3DOWN, 
-	0, 
-	MOUSE_BUTTON3DOWN, 
-	MOUSE_BUTTON1DOWN, 
-	MOUSE_BUTTON1DOWN | MOUSE_BUTTON3DOWN, 
-	MOUSE_BUTTON1DOWN,
-	MOUSE_BUTTON1DOWN | MOUSE_BUTTON3DOWN
-    };
     struct psm_softc *sc = arg;
-    mousestatus_t ms;
-    struct timeval tv;
-    int x, y, z;
+    struct timeval now;
     int c;
-    int l;
-    int x0, y0;
+    packetbuf_t *pb;
+    int haderror = 0;
 
     /* read until there is nothing to read */
     while((c = read_aux_data_no_wait(sc->kbdc)) != -1) {
-    
+
+        pb = &sc->pqueue[sc->pqueue_end];
         /* discard the byte if the device is not open */
         if ((sc->state & PSM_OPEN) == 0)
             continue;
-    
-	getmicrouptime(&tv);
-	if ((sc->inputbytes > 0) && timevalcmp(&tv, &sc->inputtimeout, >)) {
+
+	getmicrouptime(&now);
+	if ((pb->inputbytes > 0) && timevalcmp(&now, &sc->inputtimeout, >)) {
 	    log(LOG_DEBUG, "psmintr: delay too long; resetting byte count\n");
-	    sc->inputbytes = 0;
+	    pb->inputbytes = 0;
 	    sc->syncerrors = 0;
 	}
 	sc->inputtimeout.tv_sec = PSM_INPUT_TIMEOUT/1000000;
 	sc->inputtimeout.tv_usec = PSM_INPUT_TIMEOUT%1000000;
-	timevaladd(&sc->inputtimeout, &tv);
+	timevaladd(&sc->inputtimeout, &now);
 
-        sc->ipacket[sc->inputbytes++] = c;
-        if (sc->inputbytes < sc->mode.packetsize) 
+        pb->ipacket[pb->inputbytes++] = c;
+        if (pb->inputbytes < sc->mode.packetsize)
 	    continue;
 
 #if 0
         log(LOG_DEBUG, "psmintr: %02x %02x %02x %02x %02x %02x\n",
-	    sc->ipacket[0], sc->ipacket[1], sc->ipacket[2],
-	    sc->ipacket[3], sc->ipacket[4], sc->ipacket[5]);
+	    pb->ipacket[0], pb->ipacket[1], pb->ipacket[2],
+	    pb->ipacket[3], pb->ipacket[4], pb->ipacket[5]);
 #endif
 
-	c = sc->ipacket[0];
+	c = pb->ipacket[0];
 
 	if ((c & sc->mode.syncmask[0]) != sc->mode.syncmask[1]) {
-            log(LOG_DEBUG, "psmintr: out of sync (%04x != %04x).\n", 
-		c & sc->mode.syncmask[0], sc->mode.syncmask[1]);
+            log(LOG_DEBUG, "psmintr: out of sync (%04x != %04x) %d"
+		" cmds since last error.\n",
+		c & sc->mode.syncmask[0], sc->mode.syncmask[1],
+		sc->cmdcount - sc->lasterr);
+	    haderror = 1;
+	    sc->lasterr = sc->cmdcount;
+	    dropqueue(sc);
 	    ++sc->syncerrors;
+	    sc->lastinputerr = now;
 	    if (sc->syncerrors < sc->mode.packetsize) {
 		log(LOG_DEBUG, "psmintr: discard a byte (%d).\n", sc->syncerrors);
-		--sc->inputbytes;
-		bcopy(&sc->ipacket[1], &sc->ipacket[0], sc->inputbytes);
+		--pb->inputbytes;
+		bcopy(&pb->ipacket[1], &pb->ipacket[0], pb->inputbytes);
 	    } else if (sc->syncerrors == sc->mode.packetsize) {
 		log(LOG_DEBUG, "psmintr: re-enable the mouse.\n");
-		sc->inputbytes = 0;
+		pb->inputbytes = 0;
 		disable_aux_dev(sc->kbdc);
 		enable_aux_dev(sc->kbdc);
 	    } else if (sc->syncerrors < PSM_SYNCERR_THRESHOLD1) {
 		log(LOG_DEBUG, "psmintr: discard a byte (%d).\n", sc->syncerrors);
-		--sc->inputbytes;
-		bcopy(&sc->ipacket[1], &sc->ipacket[0], sc->inputbytes);
+		--pb->inputbytes;
+		bcopy(&pb->ipacket[1], &pb->ipacket[0], pb->inputbytes);
 	    } else if (sc->syncerrors >= PSM_SYNCERR_THRESHOLD1) {
 		log(LOG_DEBUG, "psmintr: reset the mouse.\n");
 		reinitialize(sc, TRUE);
 	    }
 	    continue;
 	}
+	/* if this packet is at all bogus then drop the packet. */
+	if (haderror ||
+	    !timeelapsed(&sc->lastinputerr, psmerrsecs, psmerrusecs, &now)) {
+		pb->inputbytes = 0;
+		haderror = 0;
+		continue;
+	}
+
+	sc->cmdcount++;
+	if (++sc->pqueue_end >= PSM_PACKETQUEUE)
+		sc->pqueue_end = 0;
+	/*
+	 * If we've filled the queue then call the softintr ourselves,
+	 * otherwise schedule the interrupt for later.
+	 */
+	if (!timeelapsed(&sc->lastsoftintr, psmsecs, psmusecs, &now) ||
+	    (sc->pqueue_end == sc->pqueue_start)) {
+		if ((sc->state & PSM_SOFTARMED) != 0) {
+			sc->state &= ~PSM_SOFTARMED;
+			callout_stop(&sc->softcallout);
+		}
+		psmsoftintr(arg);
+	} else if ((sc->state & PSM_SOFTARMED) == 0) {
+		sc->state |= PSM_SOFTARMED;
+		callout_reset(&sc->softcallout, psmhz < 1 ? 1 : (hz/psmhz),
+		    psmsoftintr, arg);
+	}
+    }
+
+}
+
+static void
+psmsoftintr(void *arg)
+{
+    /*
+     * the table to turn PS/2 mouse button bits (MOUSE_PS2_BUTTON?DOWN)
+     * into `mousestatus' button bits (MOUSE_BUTTON?DOWN).
+     */
+    static int butmap[8] = {
+        0,
+	MOUSE_BUTTON1DOWN,
+	MOUSE_BUTTON3DOWN,
+	MOUSE_BUTTON1DOWN | MOUSE_BUTTON3DOWN,
+	MOUSE_BUTTON2DOWN,
+	MOUSE_BUTTON1DOWN | MOUSE_BUTTON2DOWN,
+	MOUSE_BUTTON2DOWN | MOUSE_BUTTON3DOWN,
+        MOUSE_BUTTON1DOWN | MOUSE_BUTTON2DOWN | MOUSE_BUTTON3DOWN
+    };
+    static int butmap_versapad[8] = {
+	0,
+	MOUSE_BUTTON3DOWN,
+	0,
+	MOUSE_BUTTON3DOWN,
+	MOUSE_BUTTON1DOWN,
+	MOUSE_BUTTON1DOWN | MOUSE_BUTTON3DOWN,
+	MOUSE_BUTTON1DOWN,
+	MOUSE_BUTTON1DOWN | MOUSE_BUTTON3DOWN
+    };
+    struct psm_softc *sc = arg;
+    mousestatus_t ms;
+    int x, y, z;
+    int c;
+    int l;
+    int x0, y0;
+    packetbuf_t *pb;
+
+    crit_enter();
+    do {
+
+	pb = &sc->pqueue[sc->pqueue_start];
+	c = pb->ipacket[0];
 
 	/* 
 	 * A kludge for Kensington device! 
@@ -2048,11 +2166,11 @@ psmintr(void *arg)
 	 * a strange place.
 	 */
 	if (sc->hw.model == MOUSE_MODEL_THINK)
-	    sc->ipacket[1] |= (c & MOUSE_PS2_XOVERFLOW) ? 0x80 : 0;
+	    pb->ipacket[1] |= (c & MOUSE_PS2_XOVERFLOW) ? 0x80 : 0;
 
         /* ignore the overflow bits... */
-        x = (c & MOUSE_PS2_XNEG) ?  sc->ipacket[1] - 256 : sc->ipacket[1];
-        y = (c & MOUSE_PS2_YNEG) ?  sc->ipacket[2] - 256 : sc->ipacket[2];
+        x = (c & MOUSE_PS2_XNEG) ?  pb->ipacket[1] - 256 : pb->ipacket[1];
+        y = (c & MOUSE_PS2_YNEG) ?  pb->ipacket[2] - 256 : pb->ipacket[2];
 	z = 0;
         ms.obutton = sc->button;		  /* previous button state */
         ms.button = butmap[c & MOUSE_PS2_BUTTONS];
@@ -2074,18 +2192,18 @@ psmintr(void *arg)
 	     * s: wheel data sign bit
 	     * d2-d0: wheel data
 	     */
-	    z = (sc->ipacket[3] & MOUSE_EXPLORER_ZNEG)
-		? (sc->ipacket[3] & 0x0f) - 16 : (sc->ipacket[3] & 0x0f);
-	    ms.button |= (sc->ipacket[3] & MOUSE_EXPLORER_BUTTON4DOWN)
+	    z = (pb->ipacket[3] & MOUSE_EXPLORER_ZNEG)
+		? (pb->ipacket[3] & 0x0f) - 16 : (pb->ipacket[3] & 0x0f);
+	    ms.button |= (pb->ipacket[3] & MOUSE_EXPLORER_BUTTON4DOWN)
 		? MOUSE_BUTTON4DOWN : 0;
-	    ms.button |= (sc->ipacket[3] & MOUSE_EXPLORER_BUTTON5DOWN)
+	    ms.button |= (pb->ipacket[3] & MOUSE_EXPLORER_BUTTON5DOWN)
 		? MOUSE_BUTTON5DOWN : 0;
 	    break;
 
 	case MOUSE_MODEL_INTELLI:
 	case MOUSE_MODEL_NET:
 	    /* wheel data is in the fourth byte */
-	    z = (char)sc->ipacket[3];
+	    z = (char)pb->ipacket[3];
 	    /* some mice may send 7 when there is no Z movement?! XXX */
 	    if ((z >= 7) || (z <= -7))
 		z = 0;
@@ -2126,23 +2244,23 @@ psmintr(void *arg)
 	     */
 	    if (((c & MOUSE_PS2PLUS_SYNCMASK) == MOUSE_PS2PLUS_SYNC)
 		    && (abs(x) > 191)
-		    && MOUSE_PS2PLUS_CHECKBITS(sc->ipacket)) {
+		    && MOUSE_PS2PLUS_CHECKBITS(pb->ipacket)) {
 		/* the extended data packet encodes button and wheel events */
-		switch (MOUSE_PS2PLUS_PACKET_TYPE(sc->ipacket)) {
+		switch (MOUSE_PS2PLUS_PACKET_TYPE(pb->ipacket)) {
 		case 1:
 		    /* wheel data packet */
 		    x = y = 0;
-		    if (sc->ipacket[2] & 0x80) {
+		    if (pb->ipacket[2] & 0x80) {
 			/* horizontal roller count - ignore it XXX*/
 		    } else {
 			/* vertical roller count */
-			z = (sc->ipacket[2] & MOUSE_PS2PLUS_ZNEG)
-			    ? (sc->ipacket[2] & 0x0f) - 16
-			    : (sc->ipacket[2] & 0x0f);
+			z = (pb->ipacket[2] & MOUSE_PS2PLUS_ZNEG)
+			    ? (pb->ipacket[2] & 0x0f) - 16
+			    : (pb->ipacket[2] & 0x0f);
 		    }
-		    ms.button |= (sc->ipacket[2] & MOUSE_PS2PLUS_BUTTON4DOWN)
+		    ms.button |= (pb->ipacket[2] & MOUSE_PS2PLUS_BUTTON4DOWN)
 			? MOUSE_BUTTON4DOWN : 0;
-		    ms.button |= (sc->ipacket[2] & MOUSE_PS2PLUS_BUTTON5DOWN)
+		    ms.button |= (pb->ipacket[2] & MOUSE_PS2PLUS_BUTTON5DOWN)
 			? MOUSE_BUTTON5DOWN : 0;
 		    break;
 		case 2:
@@ -2153,20 +2271,20 @@ psmintr(void *arg)
 		     */
 		    x = y = 0;
 		    /* horizontal count */
-		    if (sc->ipacket[2] & 0x0f)
-			z = (sc->ipacket[2] & MOUSE_SPOINT_WNEG) ? -2 : 2;
+		    if (pb->ipacket[2] & 0x0f)
+			z = (pb->ipacket[2] & MOUSE_SPOINT_WNEG) ? -2 : 2;
 		    /* vertical count */
-		    if (sc->ipacket[2] & 0xf0)
-			z = (sc->ipacket[2] & MOUSE_SPOINT_ZNEG) ? -1 : 1;
+		    if (pb->ipacket[2] & 0xf0)
+			z = (pb->ipacket[2] & MOUSE_SPOINT_ZNEG) ? -1 : 1;
 #if 0
 		    /* vertical count */
-		    z = (sc->ipacket[2] & MOUSE_SPOINT_ZNEG)
-			? ((sc->ipacket[2] >> 4) & 0x0f) - 16
-			: ((sc->ipacket[2] >> 4) & 0x0f);
+		    z = (pb->ipacket[2] & MOUSE_SPOINT_ZNEG)
+			? ((pb->ipacket[2] >> 4) & 0x0f) - 16
+			: ((pb->ipacket[2] >> 4) & 0x0f);
 		    /* horizontal count */
-		    w = (sc->ipacket[2] & MOUSE_SPOINT_WNEG)
-			? (sc->ipacket[2] & 0x0f) - 16
-			: (sc->ipacket[2] & 0x0f);
+		    w = (pb->ipacket[2] & MOUSE_SPOINT_WNEG)
+			? (pb->ipacket[2] & 0x0f) - 16
+			: (pb->ipacket[2] & 0x0f);
 #endif
 		    break;
 		case 0:
@@ -2178,8 +2296,8 @@ psmintr(void *arg)
 		    if (bootverbose)
 			log(LOG_DEBUG, "psmintr: unknown PS2++ packet type %d: "
 				       "0x%02x 0x%02x 0x%02x\n",
-			    MOUSE_PS2PLUS_PACKET_TYPE(sc->ipacket),
-			    sc->ipacket[0], sc->ipacket[1], sc->ipacket[2]);
+			    MOUSE_PS2PLUS_PACKET_TYPE(pb->ipacket),
+			    pb->ipacket[0], pb->ipacket[1], pb->ipacket[2]);
 		    break;
 		}
 	    } else {
@@ -2195,12 +2313,12 @@ psmintr(void *arg)
 
 	case MOUSE_MODEL_NETSCROLL:
 	    /* three addtional bytes encode buttons and wheel events */
-	    ms.button |= (sc->ipacket[3] & MOUSE_PS2_BUTTON3DOWN)
+	    ms.button |= (pb->ipacket[3] & MOUSE_PS2_BUTTON3DOWN)
 		? MOUSE_BUTTON4DOWN : 0;
-	    ms.button |= (sc->ipacket[3] & MOUSE_PS2_BUTTON1DOWN)
+	    ms.button |= (pb->ipacket[3] & MOUSE_PS2_BUTTON1DOWN)
 		? MOUSE_BUTTON5DOWN : 0;
-	    z = (sc->ipacket[3] & MOUSE_PS2_XNEG) 
-		? sc->ipacket[4] - 256 : sc->ipacket[4];
+	    z = (pb->ipacket[3] & MOUSE_PS2_XNEG)
+		? pb->ipacket[4] - 256 : pb->ipacket[4];
 	    break;
 
 	case MOUSE_MODEL_THINK:
@@ -2234,8 +2352,8 @@ psmintr(void *arg)
 	    ms.button |= (c & MOUSE_PS2VERSA_TAP) ? MOUSE_BUTTON4DOWN : 0;
 	    x = y = 0;
 	    if (c & MOUSE_PS2VERSA_IN_USE) {
-		x0 = sc->ipacket[1] | (((sc->ipacket[4]) & 0x0f) << 8);
-		y0 = sc->ipacket[2] | (((sc->ipacket[4]) & 0xf0) << 4);
+		x0 = pb->ipacket[1] | (((pb->ipacket[4]) & 0x0f) << 8);
+		y0 = pb->ipacket[2] | (((pb->ipacket[4]) & 0xf0) << 4);
 		if (x0 & 0x800)
 		    x0 -= 0x1000;
 		if (y0 & 0x800)
@@ -2275,8 +2393,8 @@ psmintr(void *arg)
 	     * s2: wheel 2 direction
 	     * d2: wheel 2 data
 	     */
-	    x = (sc->ipacket[1] & 0x80) ? sc->ipacket[1] - 256 : sc->ipacket[1];
-	    y = (sc->ipacket[2] & 0x80) ? sc->ipacket[2] - 256 : sc->ipacket[2];
+	    x = (pb->ipacket[1] & 0x80) ? pb->ipacket[1] - 256 : pb->ipacket[1];
+	    y = (pb->ipacket[2] & 0x80) ? pb->ipacket[2] - 256 : pb->ipacket[2];
 	    switch (c & MOUSE_4D_WHEELBITS) {
 	    case 0x10:
 		z = 1;
@@ -2306,11 +2424,11 @@ psmintr(void *arg)
 		 * d1-d0: wheel data
 		 */
 		x = y = 0;
-		if (sc->ipacket[2] & MOUSE_4DPLUS_BUTTON4DOWN)
+		if (pb->ipacket[2] & MOUSE_4DPLUS_BUTTON4DOWN)
 		    ms.button |= MOUSE_BUTTON4DOWN;
-		z = (sc->ipacket[2] & MOUSE_4DPLUS_ZNEG)
-			? ((sc->ipacket[2] & 0x07) - 8)
-			: (sc->ipacket[2] & 0x07) ;
+		z = (pb->ipacket[2] & MOUSE_4DPLUS_ZNEG)
+			? ((pb->ipacket[2] & 0x07) - 8)
+			: (pb->ipacket[2] & 0x07) ;
 	    } else {
 		/* preserve previous button states */
 		ms.button |= ms.obutton & MOUSE_EXTBUTTONS;
@@ -2347,7 +2465,7 @@ psmintr(void *arg)
 	    | (ms.obutton ^ ms.button);
 
 	if (sc->mode.level < PSM_LEVEL_NATIVE)
-	    sc->inputbytes = tame_mouse(sc, &ms, sc->ipacket);
+	    sc->inputbytes = tame_mouse(sc, pb, &ms, pb->ipacket);
 
         sc->status.flags |= ms.flags;
         sc->status.dx += ms.dx;
@@ -2359,23 +2477,29 @@ psmintr(void *arg)
 	sc->watchdog = FALSE;
 
         /* queue data */
-        if (sc->queue.count + sc->inputbytes < sizeof(sc->queue.buf)) {
-	    l = min(sc->inputbytes, sizeof(sc->queue.buf) - sc->queue.tail);
-	    bcopy(&sc->ipacket[0], &sc->queue.buf[sc->queue.tail], l);
-	    if (sc->inputbytes > l)
-	        bcopy(&sc->ipacket[l], &sc->queue.buf[0], sc->inputbytes - l);
-            sc->queue.tail = 
-		(sc->queue.tail + sc->inputbytes) % sizeof(sc->queue.buf);
-            sc->queue.count += sc->inputbytes;
+        if (sc->queue.count + pb->inputbytes < sizeof(sc->queue.buf)) {
+	    l = min(pb->inputbytes, sizeof(sc->queue.buf) - sc->queue.tail);
+	    bcopy(&pb->ipacket[0], &sc->queue.buf[sc->queue.tail], l);
+	    if (pb->inputbytes > l)
+	        bcopy(&pb->ipacket[l], &sc->queue.buf[0], pb->inputbytes - l);
+            sc->queue.tail =
+		(sc->queue.tail + pb->inputbytes) % sizeof(sc->queue.buf);
+            sc->queue.count += pb->inputbytes;
 	}
-        sc->inputbytes = 0;
+        pb->inputbytes = 0;
+
+	if (++sc->pqueue_start >= PSM_PACKETQUEUE)
+		sc->pqueue_start = 0;
+
+    } while(sc->pqueue_start != sc->pqueue_end);
 
-        if (sc->state & PSM_ASLP) {
-            sc->state &= ~PSM_ASLP;
+    if (sc->state & PSM_ASLP) {
+	    sc->state &= ~PSM_ASLP;
             wakeup((caddr_t) sc);
-    	}
-	KNOTE(&sc->rkq.ki_note, 0);
     }
+    KNOTE(&sc->rkq.ki_note, 0);
+    sc->state &= ~PSM_SOFTARMED;
+    crit_exit();
 }
 
 static struct filterops psmfiltops =
@@ -2876,4 +3000,29 @@ psmresume(device_t dev)
     return (err);
 }
 
+/*
+ * Return true if 'now' is earlier than (start + (secs.usecs)).
+ * Now may be NULL and the function will fetch the current time from
+ * getmicrouptime(), or a cached 'now' can be passed in.
+ * All values should be numbers derived from getmicrouptime().
+ */
+static int
+timeelapsed(start, secs, usecs, now)
+	const struct timeval *start, *now;
+	int secs, usecs;
+{
+	struct timeval snow, tv;
+
+	/* if there is no 'now' passed in, the get it as a convience. */
+	if (now == NULL) {
+		getmicrouptime(&snow);
+		now = &snow;
+	}
+
+	tv.tv_sec = secs;
+	tv.tv_usec = usecs;
+	timevaladd(&tv, start);
+	return (timevalcmp(&tv, now, <));
+}
+
 DRIVER_MODULE(psm, atkbdc, psm_driver, psm_devclass, 0, 0);
-- 
1.7.0.5

