--- qemu/hw/esp.c	2018/04/24 16:48:52	1.1.1.6
+++ qemu/hw/esp.c	2018/04/24 16:53:45	1.1.1.7
@@ -21,12 +21,10 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
+
 #include "hw.h"
-#include "block.h"
 #include "scsi-disk.h"
-#include "sun4m.h"
-/* FIXME: Only needed for MAX_DISKS, which is probably wrong.  */
-#include "sysemu.h"
+#include "scsi.h"
 
 /* debug ESP card */
 //#define DEBUG_ESP
@@ -43,30 +41,32 @@
 #define DPRINTF(fmt, args...) \
 do { printf("ESP: " fmt , ##args); } while (0)
 #else
-#define DPRINTF(fmt, args...)
+#define DPRINTF(fmt, args...) do {} while (0)
 #endif
 
-#define ESP_MASK 0x3f
+#define ESP_ERROR(fmt, args...) \
+do { printf("ESP ERROR: %s: " fmt, __func__ , ##args); } while (0)
+
 #define ESP_REGS 16
-#define ESP_SIZE (ESP_REGS * 4)
-#define TI_BUFSZ 32
+#define TI_BUFSZ 16
 
 typedef struct ESPState ESPState;
 
 struct ESPState {
+    uint32_t it_shift;
     qemu_irq irq;
     uint8_t rregs[ESP_REGS];
     uint8_t wregs[ESP_REGS];
     int32_t ti_size;
     uint32_t ti_rptr, ti_wptr;
     uint8_t ti_buf[TI_BUFSZ];
-    int sense;
-    int dma;
+    uint32_t sense;
+    uint32_t dma;
     SCSIDevice *scsi_dev[ESP_MAX_DEVS];
     SCSIDevice *current_dev;
     uint8_t cmdbuf[TI_BUFSZ];
-    int cmdlen;
-    int do_cmd;
+    uint32_t cmdlen;
+    uint32_t do_cmd;
 
     /* The amount of data left in the current DMA transfer.  */
     uint32_t dma_left;
@@ -75,6 +75,9 @@ struct ESPState {
     uint32_t dma_counter;
     uint8_t *async_buf;
     uint32_t async_len;
+
+    espdma_memory_read_write dma_memory_read;
+    espdma_memory_read_write dma_memory_write;
     void *dma_opaque;
 };
 
@@ -120,14 +123,16 @@ struct ESPState {
 #define STAT_DI 0x01
 #define STAT_CD 0x02
 #define STAT_ST 0x03
-#define STAT_MI 0x06
-#define STAT_MO 0x07
+#define STAT_MO 0x06
+#define STAT_MI 0x07
 #define STAT_PIO_MASK 0x06
 
 #define STAT_TC 0x10
 #define STAT_PE 0x20
 #define STAT_GE 0x40
-#define STAT_IN 0x80
+#define STAT_INT 0x80
+
+#define BUSID_DID 0x07
 
 #define INTR_FC 0x08
 #define INTR_BS 0x10
@@ -139,25 +144,39 @@ struct ESPState {
 
 #define CFG1_RESREPT 0x40
 
-#define CFG2_MASK 0x15
-
 #define TCHI_FAS100A 0x4
 
-static int get_cmd(ESPState *s, uint8_t *buf)
+static void esp_raise_irq(ESPState *s)
+{
+    if (!(s->rregs[ESP_RSTAT] & STAT_INT)) {
+        s->rregs[ESP_RSTAT] |= STAT_INT;
+        qemu_irq_raise(s->irq);
+    }
+}
+
+static void esp_lower_irq(ESPState *s)
+{
+    if (s->rregs[ESP_RSTAT] & STAT_INT) {
+        s->rregs[ESP_RSTAT] &= ~STAT_INT;
+        qemu_irq_lower(s->irq);
+    }
+}
+
+static uint32_t get_cmd(ESPState *s, uint8_t *buf)
 {
     uint32_t dmalen;
     int target;
 
-    dmalen = s->rregs[ESP_TCLO] | (s->rregs[ESP_TCMID] << 8);
-    target = s->wregs[ESP_WBUSID] & 7;
-    DPRINTF("get_cmd: len %d target %d\n", dmalen, target);
+    target = s->wregs[ESP_WBUSID] & BUSID_DID;
     if (s->dma) {
-        espdma_memory_read(s->dma_opaque, buf, dmalen);
+        dmalen = s->rregs[ESP_TCLO] | (s->rregs[ESP_TCMID] << 8);
+        s->dma_memory_read(s->dma_opaque, buf, dmalen);
     } else {
+        dmalen = s->ti_size;
+        memcpy(buf, s->ti_buf, dmalen);
         buf[0] = 0;
-        memcpy(&buf[1], s->ti_buf, dmalen);
-        dmalen++;
     }
+    DPRINTF("get_cmd: len %d target %d\n", dmalen, target);
 
     s->ti_size = 0;
     s->ti_rptr = 0;
@@ -171,10 +190,10 @@ static int get_cmd(ESPState *s, uint8_t 
 
     if (target >= ESP_MAX_DEVS || !s->scsi_dev[target]) {
         // No such drive
-        s->rregs[ESP_RSTAT] = STAT_IN;
+        s->rregs[ESP_RSTAT] = 0;
         s->rregs[ESP_RINTR] = INTR_DC;
         s->rregs[ESP_RSEQ] = SEQ_0;
-        qemu_irq_raise(s->irq);
+        esp_raise_irq(s);
         return 0;
     }
     s->current_dev = s->scsi_dev[target];
@@ -191,7 +210,7 @@ static void do_cmd(ESPState *s, uint8_t 
     datalen = s->current_dev->send_command(s->current_dev, 0, &buf[1], lun);
     s->ti_size = datalen;
     if (datalen != 0) {
-        s->rregs[ESP_RSTAT] = STAT_IN | STAT_TC;
+        s->rregs[ESP_RSTAT] = STAT_TC;
         s->dma_left = 0;
         s->dma_counter = 0;
         if (datalen > 0) {
@@ -204,7 +223,7 @@ static void do_cmd(ESPState *s, uint8_t 
     }
     s->rregs[ESP_RINTR] = INTR_BS | INTR_FC;
     s->rregs[ESP_RSEQ] = SEQ_CD;
-    qemu_irq_raise(s->irq);
+    esp_raise_irq(s);
 }
 
 static void handle_satn(ESPState *s)
@@ -223,10 +242,10 @@ static void handle_satn_stop(ESPState *s
     if (s->cmdlen) {
         DPRINTF("Set ATN & Stop: cmdlen %d\n", s->cmdlen);
         s->do_cmd = 1;
-        s->rregs[ESP_RSTAT] = STAT_IN | STAT_TC | STAT_CD;
+        s->rregs[ESP_RSTAT] = STAT_TC | STAT_CD;
         s->rregs[ESP_RINTR] = INTR_BS | INTR_FC;
         s->rregs[ESP_RSEQ] = SEQ_CD;
-        qemu_irq_raise(s->irq);
+        esp_raise_irq(s);
     }
 }
 
@@ -236,8 +255,8 @@ static void write_response(ESPState *s)
     s->ti_buf[0] = s->sense;
     s->ti_buf[1] = 0;
     if (s->dma) {
-        espdma_memory_write(s->dma_opaque, s->ti_buf, 2);
-        s->rregs[ESP_RSTAT] = STAT_IN | STAT_TC | STAT_ST;
+        s->dma_memory_write(s->dma_opaque, s->ti_buf, 2);
+        s->rregs[ESP_RSTAT] = STAT_TC | STAT_ST;
         s->rregs[ESP_RINTR] = INTR_BS | INTR_FC;
         s->rregs[ESP_RSEQ] = SEQ_CD;
     } else {
@@ -246,18 +265,18 @@ static void write_response(ESPState *s)
         s->ti_wptr = 0;
         s->rregs[ESP_RFLAGS] = 2;
     }
-    qemu_irq_raise(s->irq);
+    esp_raise_irq(s);
 }
 
 static void esp_dma_done(ESPState *s)
 {
-    s->rregs[ESP_RSTAT] |= STAT_IN | STAT_TC;
+    s->rregs[ESP_RSTAT] |= STAT_TC;
     s->rregs[ESP_RINTR] = INTR_BS;
     s->rregs[ESP_RSEQ] = 0;
     s->rregs[ESP_RFLAGS] = 0;
     s->rregs[ESP_TCLO] = 0;
     s->rregs[ESP_TCMID] = 0;
-    qemu_irq_raise(s->irq);
+    esp_raise_irq(s);
 }
 
 static void esp_do_dma(ESPState *s)
@@ -269,7 +288,7 @@ static void esp_do_dma(ESPState *s)
     len = s->dma_left;
     if (s->do_cmd) {
         DPRINTF("command len %d + %d\n", s->cmdlen, len);
-        espdma_memory_read(s->dma_opaque, &s->cmdbuf[s->cmdlen], len);
+        s->dma_memory_read(s->dma_opaque, &s->cmdbuf[s->cmdlen], len);
         s->ti_size = 0;
         s->cmdlen = 0;
         s->do_cmd = 0;
@@ -284,9 +303,9 @@ static void esp_do_dma(ESPState *s)
         len = s->async_len;
     }
     if (to_device) {
-        espdma_memory_read(s->dma_opaque, s->async_buf, len);
+        s->dma_memory_read(s->dma_opaque, s->async_buf, len);
     } else {
-        espdma_memory_write(s->dma_opaque, s->async_buf, len);
+        s->dma_memory_write(s->dma_opaque, s->async_buf, len);
     }
     s->dma_left -= len;
     s->async_buf += len;
@@ -302,7 +321,7 @@ static void esp_do_dma(ESPState *s)
         } else {
             s->current_dev->read_data(s->current_dev, 0);
             /* If there is still data to be read from the device then
-               complete the DMA operation immeriately.  Otherwise defer
+               complete the DMA operation immediately.  Otherwise defer
                until the scsi layer has completed.  */
             if (s->dma_left == 0 && s->ti_size > 0) {
                 esp_dma_done(s);
@@ -381,6 +400,8 @@ static void esp_reset(void *opaque)
 {
     ESPState *s = opaque;
 
+    esp_lower_irq(s);
+
     memset(s->rregs, 0, ESP_REGS);
     memset(s->wregs, 0, ESP_REGS);
     s->rregs[ESP_TCHI] = TCHI_FAS100A; // Indicate fas100a
@@ -389,6 +410,8 @@ static void esp_reset(void *opaque)
     s->ti_wptr = 0;
     s->dma = 0;
     s->do_cmd = 0;
+
+    s->rregs[ESP_CFG1] = 7;
 }
 
 static void parent_esp_reset(void *opaque, int irq, int level)
@@ -402,20 +425,20 @@ static uint32_t esp_mem_readb(void *opaq
     ESPState *s = opaque;
     uint32_t saddr;
 
-    saddr = (addr & ESP_MASK) >> 2;
+    saddr = addr >> s->it_shift;
     DPRINTF("read reg[%d]: 0x%2.2x\n", saddr, s->rregs[saddr]);
     switch (saddr) {
     case ESP_FIFO:
         if (s->ti_size > 0) {
             s->ti_size--;
             if ((s->rregs[ESP_RSTAT] & STAT_PIO_MASK) == 0) {
-                /* Data in/out.  */
-                fprintf(stderr, "esp: PIO data read not implemented\n");
+                /* Data out.  */
+                ESP_ERROR("PIO data read not implemented\n");
                 s->rregs[ESP_FIFO] = 0;
             } else {
                 s->rregs[ESP_FIFO] = s->ti_buf[s->ti_rptr++];
             }
-            qemu_irq_raise(s->irq);
+            esp_raise_irq(s);
         }
         if (s->ti_size == 0) {
             s->ti_rptr = 0;
@@ -424,8 +447,8 @@ static uint32_t esp_mem_readb(void *opaq
         break;
     case ESP_RINTR:
         // Clear interrupt/error status bits
-        s->rregs[ESP_RSTAT] &= ~(STAT_IN | STAT_GE | STAT_PE);
-        qemu_irq_lower(s->irq);
+        s->rregs[ESP_RSTAT] &= ~(STAT_GE | STAT_PE);
+        esp_lower_irq(s);
         break;
     default:
         break;
@@ -438,7 +461,7 @@ static void esp_mem_writeb(void *opaque,
     ESPState *s = opaque;
     uint32_t saddr;
 
-    saddr = (addr & ESP_MASK) >> 2;
+    saddr = addr >> s->it_shift;
     DPRINTF("write reg[%d]: 0x%2.2x -> 0x%2.2x\n", saddr, s->wregs[saddr],
             val);
     switch (saddr) {
@@ -449,11 +472,8 @@ static void esp_mem_writeb(void *opaque,
     case ESP_FIFO:
         if (s->do_cmd) {
             s->cmdbuf[s->cmdlen++] = val & 0xff;
-        } else if ((s->rregs[ESP_RSTAT] & STAT_PIO_MASK) == 0) {
-            uint8_t buf;
-            buf = val & 0xff;
-            s->ti_size--;
-            fprintf(stderr, "esp: PIO data write not implemented\n");
+        } else if (s->ti_size == TI_BUFSZ - 1) {
+            ESP_ERROR("fifo overrun\n");
         } else {
             s->ti_size++;
             s->ti_buf[s->ti_wptr++] = val & 0xff;
@@ -478,6 +498,7 @@ static void esp_mem_writeb(void *opaque,
             //s->ti_size = 0;
             s->rregs[ESP_RINTR] = INTR_FC;
             s->rregs[ESP_RSEQ] = 0;
+            s->rregs[ESP_RFLAGS] = 0;
             break;
         case CMD_RESET:
             DPRINTF("Chip reset (%2.2x)\n", val);
@@ -487,7 +508,7 @@ static void esp_mem_writeb(void *opaque,
             DPRINTF("Bus reset (%2.2x)\n", val);
             s->rregs[ESP_RINTR] = INTR_RST;
             if (!(s->wregs[ESP_CFG1] & CFG1_RESREPT)) {
-                qemu_irq_raise(s->irq);
+                esp_raise_irq(s);
             }
             break;
         case CMD_TI:
@@ -496,6 +517,8 @@ static void esp_mem_writeb(void *opaque,
         case CMD_ICCS:
             DPRINTF("Initiator Command Complete Sequence (%2.2x)\n", val);
             write_response(s);
+            s->rregs[ESP_RINTR] = INTR_FC;
+            s->rregs[ESP_RSTAT] |= STAT_MI;
             break;
         case CMD_MSGACC:
             DPRINTF("Message Accepted (%2.2x)\n", val);
@@ -516,9 +539,10 @@ static void esp_mem_writeb(void *opaque,
             break;
         case CMD_ENSEL:
             DPRINTF("Enable selection (%2.2x)\n", val);
+            s->rregs[ESP_RINTR] = 0;
             break;
         default:
-            DPRINTF("Unhandled ESP command (%2.2x)\n", val);
+            ESP_ERROR("Unhandled ESP command (%2.2x)\n", val);
             break;
         }
         break;
@@ -529,14 +553,12 @@ static void esp_mem_writeb(void *opaque,
         break;
     case ESP_WCCF ... ESP_WTEST:
         break;
-    case ESP_CFG2:
-        s->rregs[saddr] = val & CFG2_MASK;
-        break;
-    case ESP_CFG3 ... ESP_RES4:
+    case ESP_CFG2 ... ESP_RES4:
         s->rregs[saddr] = val;
         break;
     default:
-        break;
+        ESP_ERROR("invalid write of 0x%02x at [0x%x]\n", val, saddr);
+        return;
     }
     s->wregs[saddr] = val;
 }
@@ -550,7 +572,7 @@ static CPUReadMemoryFunc *esp_mem_read[3
 static CPUWriteMemoryFunc *esp_mem_write[3] = {
     esp_mem_writeb,
     NULL,
-    NULL,
+    esp_mem_writeb,
 };
 
 static void esp_save(QEMUFile *f, void *opaque)
@@ -559,7 +581,7 @@ static void esp_save(QEMUFile *f, void *
 
     qemu_put_buffer(f, s->rregs, ESP_REGS);
     qemu_put_buffer(f, s->wregs, ESP_REGS);
-    qemu_put_be32s(f, &s->ti_size);
+    qemu_put_sbe32s(f, &s->ti_size);
     qemu_put_be32s(f, &s->ti_rptr);
     qemu_put_be32s(f, &s->ti_wptr);
     qemu_put_buffer(f, s->ti_buf, TI_BUFSZ);
@@ -581,7 +603,7 @@ static int esp_load(QEMUFile *f, void *o
 
     qemu_get_buffer(f, s->rregs, ESP_REGS);
     qemu_get_buffer(f, s->wregs, ESP_REGS);
-    qemu_get_be32s(f, &s->ti_size);
+    qemu_get_sbe32s(f, &s->ti_size);
     qemu_get_be32s(f, &s->ti_rptr);
     qemu_get_be32s(f, &s->ti_wptr);
     qemu_get_buffer(f, s->ti_buf, TI_BUFSZ);
@@ -601,6 +623,8 @@ void esp_scsi_attach(void *opaque, Block
 
     if (id < 0) {
         for (id = 0; id < ESP_MAX_DEVS; id++) {
+            if (id == (s->rregs[ESP_CFG1] & 0x7))
+                continue;
             if (s->scsi_dev[id] == NULL)
                 break;
         }
@@ -620,21 +644,24 @@ void esp_scsi_attach(void *opaque, Block
         s->scsi_dev[id] = scsi_disk_init(bd, 0, esp_command_complete, s);
 }
 
-void *esp_init(target_phys_addr_t espaddr,
+void *esp_init(target_phys_addr_t espaddr, int it_shift,
+               espdma_memory_read_write dma_memory_read,
+               espdma_memory_read_write dma_memory_write,
                void *dma_opaque, qemu_irq irq, qemu_irq *reset)
 {
     ESPState *s;
     int esp_io_memory;
 
     s = qemu_mallocz(sizeof(ESPState));
-    if (!s)
-        return NULL;
 
     s->irq = irq;
+    s->it_shift = it_shift;
+    s->dma_memory_read = dma_memory_read;
+    s->dma_memory_write = dma_memory_write;
     s->dma_opaque = dma_opaque;
 
     esp_io_memory = cpu_register_io_memory(0, esp_mem_read, esp_mem_write, s);
-    cpu_register_physical_memory(espaddr, ESP_SIZE, esp_io_memory);
+    cpu_register_physical_memory(espaddr, ESP_REGS << it_shift, esp_io_memory);
 
     esp_reset(s);