abcio: handle INP switch to default better; functions to set INP

fw/abcio.c

@@ -40,14 +40,14 @@ IRQHANDLER(abc)
 
 	switch (addr) {
 	case 0:
+	    dev->out_data[addr] = data;
 	    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;
+		if (!dev->out_cnt)
+		    break;	/* No callback */
 	    }
-	    dev->out_data[addr] = data;
 	    if (callback & 1)
 		dev->callback_out(dev, data);
 	    break;
@@ -78,11 +78,10 @@ IRQHANDLER(abc)
     }
 
     if (what & 0x100) {
-	if (dev->inp_cnt--) {
-	    ABC_INP0_DATA = dev->inp_data[0] = *dev->inp_buf++;
+	if (--dev->inp_cnt) {
 	    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 */
+	    ABC_INP0_DATA = dev->inp_data[0] = *dev->inp_buf++;
 	} else {
 	    ABC_INP0_DATA = dev->inp_data[0] = dev->inp_data_def;
 	    if (callback & 0x100)
@@ -119,13 +118,38 @@ 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;
+    dev->inp_cnt = len;
     ABC_INP0_DATA = dev->inp_data[0] = *dev->inp_buf++;
     ABC_INP1_DATA = dev->inp_data[1] = status;
 
     unmask_irq(ABC_IRQ);
 }
 
+void abc_set_inp_default(struct abc_dev *dev, uint8_t val)
+{
+    mask_irq(ABC_IRQ);
+
+    dev->inp_data_def = val;
+    if (!dev->inp_cnt) {
+	dev->inp_data[0] = val;
+	if (dev == selected_dev)
+	    ABC_INP0_DATA = val;
+    }
+
+    unmask_irq(ABC_IRQ);
+}
+
+void abc_set_inp_status(struct abc_dev *dev, uint8_t val)
+{
+    mask_irq(ABC_IRQ);
+
+    dev->inp_data[1] = val;
+    if (dev == selected_dev)
+	ABC_INP1_DATA = val;
+
+    unmask_irq(ABC_IRQ);
+}
+
 void abc_register(struct abc_dev *dev, unsigned int devsel)
 {
     if (devsel > 63)