abcio: init function; better handling of select switching

fw/abcio.c

@@ -13,41 +13,44 @@ static struct abc_dev *abc_device[64];
 static struct abc_dev *selected_dev;
 static uint8_t abc_devsel = -1;
 
-static inline void abc_select(struct abc_dev *dev)
+/* Not inlining this makes the code larger...! */
+static inline __attribute__((always_inline))
+void abc_select(struct abc_dev *dev)
 {
     selected_dev = dev;
     if (!dev) {
 	ABC_BUSY_MASK = 0x0082;	/* Only RST# and CS# */
-	ABC_INP_ENABLE = 0;
+	ABC_INP = 0;
     } else {
 	ABC_BUSY_MASK = dev->callback_mask | 0x0082;
-	ABC_INP_ENABLE = 3;
+	ABC_INP = dev->inp_data_w;
     }
 }
 
 IRQHANDLER(abc)
 {
-    uint16_t what = ABC_BUSY_STATUS;
-    bool callback = selected_dev && (what & selected_dev->callback_mask);
+    unsigned int what = ABC_BUSY_STATUS;
+    unsigned int callback =
+	selected_dev ? (what & selected_dev->callback_mask) : 0;
 
     if (what & 0xff) {
-	uint8_t addr = ABC_OUT_ADDR;
-	uint8_t data = ABC_OUT_DATA;
+	unsigned int addr = ABC_OUT_ADDR;
+	unsigned int data = ABC_OUT_DATA;
 
 	switch (addr) {
 	case 0:
-	    if (selected_dev->out_cnt) {
+	    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)
-		    callback = false;
+		if (selected_dev->out_cnt)
+		    callback &= ~1;
 	    }
 
 	    /* fall through */
 	case 2 ... 5:
 	    selected_dev->out_data[addr] = data;
-	    if (callback)
+	    if (callback & (1 << addr))
 		selected_dev->callback_out(selected_dev, data, addr);
 	    break;
 
@@ -73,22 +76,22 @@ IRQHANDLER(abc)
     }
 
     if (what & 0x100) {
-	if (selected_dev->inp_cnt) {
+	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];
-	    if (--selected_dev->inp_cnt)
-		callback = false;
+	    /* Callback is always false here, because we are one byte "ahead" */
+	    callback = false;
 	} else {
 	    ABC_INP0_DATA = selected_dev->inp_data[0];
 	}
-	if (callback)
+	if (callback & 0x100)
 	    selected_dev->callback_inp(selected_dev, 0);
     }
 
     if (what & 0x200) {
 	ABC_INP1_DATA = selected_dev->inp_data[1];
-	if (callback)
+	if (callback & 0x200)
 	    selected_dev->callback_inp(selected_dev, 1);
     }
 
@@ -115,7 +118,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;
+    dev->inp_cnt = len - 1;
     ABC_INP0_DATA = *dev->inp_buf++;
     ABC_INP1_DATA = dev->inp_data[1] = status;
 
@@ -135,3 +138,16 @@ void abc_register(struct abc_dev *dev, unsigned int devsel)
 
     unmask_irq(ABC_IRQ);
 }
+
+void abc_init(void)
+{
+    unsigned int devsel;
+
+    mask_irq(ABC_IRQ);
+
+    devsel = ABC_IOSEL;
+    abc_devsel = (devsel & 0x100) ? -1U : devsel & 0x3f;
+    abc_select(NULL);
+
+    unmask_irq(ABC_IRQ);
+}

fw/abcio.h

@@ -18,8 +18,11 @@ struct abc_dev {
 
     uint8_t out_data[6];	/* [1] is devsel, all 8 bits */
     union {
-	uint8_t inp_data[2];
-	uint16_t inp_data_w;
+	struct {
+	    uint8_t inp_data[2];
+	    uint8_t inp_en;
+	};
+	uint32_t inp_data_w;
     };
 
     void (*callback_out)(struct abc_dev *, uint8_t addr, uint8_t data);