--- drivers/input/serio/i8042.c.orig 2021-01-25 03:47:14.000000000 +0300 +++ drivers/input/serio/i8042.c 2021-02-01 02:57:54.556703786 +0300 @@ -159,6 +159,8 @@ struct i8042_port { #define I8042_MUX_PORT_NO 2 #define I8042_NUM_PORTS (I8042_NUM_MUX_PORTS + 2) +#define I8042_QUEUE_SIZE 4 + static struct i8042_port i8042_ports[I8042_NUM_PORTS]; static unsigned char i8042_initial_ctr; @@ -520,33 +522,36 @@ static irqreturn_t i8042_interrupt(int i struct i8042_port *port; struct serio *serio; unsigned long flags; - unsigned char str, data; - unsigned int dfl; - unsigned int port_no; + unsigned char str; bool filtered; + unsigned char q_data[I8042_QUEUE_SIZE]; + unsigned int q_port[I8042_QUEUE_SIZE]; + unsigned int q_dfl[I8042_QUEUE_SIZE]; + unsigned int q_used = 0, q_cycles = 0, q_i; 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; - } + do { + 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(); + q_data[q_used] = 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); + if (i8042_mux_present && (str & I8042_STR_AUXDATA)) { + static unsigned long last_transmit; + static unsigned char last_str; + + q_dfl[q_used] = 0; + if (str & I8042_STR_MUXERR) { + dbg("MUX error, status is %02x, data is %02x\n", + str, q_data[q_used]); /* * When MUXERR condition is signalled the data register can only contain * 0xfd, 0xfe or 0xff if implementation follows the spec. Unfortunately @@ -560,7 +565,7 @@ static irqreturn_t i8042_interrupt(int i * was transmitted (if transmission happened not too long ago). */ - switch (data) { + switch (q_data[q_used]) { default: if (time_before(jiffies, last_transmit + HZ/10)) { str = last_str; @@ -569,37 +574,49 @@ static irqreturn_t i8042_interrupt(int i fallthrough; /* report timeout */ case 0xfc: case 0xfd: - case 0xfe: dfl = SERIO_TIMEOUT; data = 0xfe; break; - case 0xff: dfl = SERIO_PARITY; data = 0xfe; break; + case 0xfe: + q_dfl[q_used] = SERIO_TIMEOUT; + q_data[q_used] = 0xfe; + break; + case 0xff: + q_dfl[q_used] = SERIO_PARITY; + q_data[q_used] = 0xfe; + break; + } } - } - port_no = I8042_MUX_PORT_NO + ((str >> 6) & 3); - last_str = str; - last_transmit = jiffies; - } else { + q_port[q_used] = I8042_MUX_PORT_NO + ((str >> 6) & 3); + last_str = str; + last_transmit = jiffies; + } else { - dfl = ((str & I8042_STR_PARITY) ? SERIO_PARITY : 0) | - ((str & I8042_STR_TIMEOUT && !i8042_notimeout) ? SERIO_TIMEOUT : 0); + q_dfl[q_used] = ((str & I8042_STR_PARITY) ? SERIO_PARITY : 0) | + ((str & I8042_STR_TIMEOUT && !i8042_notimeout) ? SERIO_TIMEOUT : 0); - port_no = (str & I8042_STR_AUXDATA) ? - I8042_AUX_PORT_NO : I8042_KBD_PORT_NO; - } + q_port[q_used] = (str & I8042_STR_AUXDATA) ? + I8042_AUX_PORT_NO : I8042_KBD_PORT_NO; + } + + port = &i8042_ports[q_port[q_used]]; + serio = port->exists ? port->serio : NULL; + + filter_dbg(port->driver_bound, q_data[q_used], "<- i8042 (interrupt, %d, %d%s%s)\n", + q_port[q_used], irq, + q_dfl[q_used] & SERIO_PARITY ? ", bad parity" : "", + q_dfl[q_used] & SERIO_TIMEOUT ? ", timeout" : ""); - port = &i8042_ports[port_no]; - serio = port->exists ? port->serio : NULL; + filtered = i8042_filter(q_data[q_used], str, serio); - 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 (likely(serio && !filtered)) + ++q_used; - filtered = i8042_filter(data, str, serio); + str = i8042_read_status(); + } while ((q_used < I8042_QUEUE_SIZE) && (++q_cycles < 32) && (str & I8042_STR_OBF)); spin_unlock_irqrestore(&i8042_lock, flags); - if (likely(serio && !filtered)) - serio_interrupt(serio, data, dfl); + for (q_i = 0; q_i < q_used; ++q_i) + serio_interrupt(i8042_ports[q_port[q_i]].serio, q_data[q_i], q_dfl[q_i]); out: return IRQ_RETVAL(ret);