abcio: don't lose INP(0) queued data during select switch

... need to keep track both of the current INP(0) data and the default
(end-of-queue) one.

fw/abcio.c

@@ -32,6 +32,7 @@ IRQHANDLER(abc)
     unsigned int what = ABC_BUSY_STATUS;
     unsigned int callback =
 	selected_dev ? (what & selected_dev->callback_mask) : 0;
+    struct abc_dev *dev = selected_dev;
 
     if (what & 0xff) {
 	unsigned int addr = ABC_OUT_ADDR;
@@ -39,19 +40,19 @@ IRQHANDLER(abc)
 
 	switch (addr) {
 	case 0:
-	    if (selected_dev->out_cnt--) {
-		*selected_dev->out_buf++ = data;
-		selected_dev->inp_data[1] &= selected_dev->status_first_out_mask;
-		ABC_INP1_DATA = selected_dev->inp_data[1];
-		if (selected_dev->out_cnt)
+	    if (dev->out_cnt--) {
+		*dev->out_buf++ = data;
+		dev->inp_data[1] &= dev->status_first_out_mask;
+		ABC_INP1_DATA = dev->inp_data[1];
+		if (dev->out_cnt)
 		    callback &= ~1;
 	    }
 
 	    /* fall through */
 	case 2 ... 5:
-	    selected_dev->out_data[addr] = data;
+	    dev->out_data[addr] = data;
 	    if (callback & (1 << addr))
-		selected_dev->callback_out(selected_dev, data, addr);
+		dev->callback_out(dev, data, addr);
 	    break;
 
 	case 1:
@@ -76,23 +77,22 @@ IRQHANDLER(abc)
     }
 
     if (what & 0x100) {
-	if (selected_dev->inp_cnt--) {
-	    ABC_INP0_DATA = *selected_dev->inp_buf++;
-	    selected_dev->inp_data[1] &= selected_dev->status_first_inp_mask;
-	    ABC_INP1_DATA = selected_dev->inp_data[1];
-	    /* Callback is always false here, because we are one byte "ahead" */
-	    callback = false;
+	if (dev->inp_cnt--) {
+	    ABC_INP0_DATA = dev->inp_data[0] = *dev->inp_buf++;
+	    dev->inp_data[1] &= dev->status_first_inp_mask;
+	    ABC_INP1_DATA = dev->inp_data[1];
+	    /* No callback, because we are one byte "ahead" of ABC */
 	} else {
-	    ABC_INP0_DATA = selected_dev->inp_data[0];
+	    ABC_INP0_DATA = dev->inp_data[0] = dev->inp_data_def;
+	    if (callback & 0x100)
+		dev->callback_inp(dev, 0);
 	}
-	if (callback & 0x100)
-	    selected_dev->callback_inp(selected_dev, 0);
     }
 
     if (what & 0x200) {
-	ABC_INP1_DATA = selected_dev->inp_data[1];
+	ABC_INP1_DATA = dev->inp_data[1];
 	if (callback & 0x200)
-	    selected_dev->callback_inp(selected_dev, 1);
+	    dev->callback_inp(dev, 1);
     }
 
     ABC_BUSY_STATUS = what;
@@ -119,7 +119,7 @@ void abc_setup_inp_queue(struct abc_dev *dev, const void *buf, size_t len,
     dev->status_first_inp_mask = status_first_mask;
     dev->inp_buf = buf;
     dev->inp_cnt = len - 1;
-    ABC_INP0_DATA = *dev->inp_buf++;
+    ABC_INP0_DATA = dev->inp_data[0] = *dev->inp_buf++;
     ABC_INP1_DATA = dev->inp_data[1] = status;
 
     unmask_irq(ABC_IRQ);

fw/abcio.h

@@ -21,6 +21,7 @@ struct abc_dev {
 	struct {
 	    uint8_t inp_data[2];
 	    uint8_t inp_en;
+	    uint8_t inp_data_def; /* inp_data[0] when no queue active */
 	};
 	uint32_t inp_data_w;
     };