--- previous/src/esp.c	2018/04/24 19:27:36	1.1
+++ previous/src/esp.c	2018/04/24 19:31:22	1.1.1.3
@@ -11,40 +11,43 @@
 #include "dma.h"
 #include "scsi.h"
 
-#define LOG_SCSI_LEVEL LOG_DEBUG
+#define LOG_ESPDMA_LEVEL    LOG_DEBUG   /* Print debugging messages for ESP DMA registers */
+#define LOG_ESPCMD_LEVEL    LOG_DEBUG   /* Print debugging messages for ESP commands */
+#define LOG_ESPREG_LEVEL    LOG_DEBUG   /* Print debugging messages for ESP registers */
+#define LOG_ESPFIFO_LEVEL   LOG_DEBUG   /* Print debugging messages for ESP FIFO */
 
 
 #define IO_SEG_MASK	0x1FFFF
 
 typedef enum {
     DISCONNECTED,
-    RESELECTABLE,
-    SELECTING,
     INITIATOR,
-    DISABLINGSEL,
-    COMPLETING,
-    DMAING,
-    ACCEPTINGMSG,
-    SENDINGMSG,
-    PADDING,
-    GETTINGMSG,
-    GETTINGLUN,
-    IGNOREILLCMD
+    TARGET
 } SCSI_STATE;
 
-SCSI_STATE state;
+SCSI_STATE esp_state;
 
-/* SCSI DMA Command/Status Registers */
-Uint8 csr_value0;
-Uint8 csr_value1;
+
+/* ESP FIFO */
+#define ESP_FIFO_SIZE 16
+Uint8 esp_fifo_read(void);
+void esp_fifo_write(Uint8 val);
+void esp_fifo_clear(void);
+
+/* ESP Command Register */
+Uint8 esp_cmd_state;
+#define ESP_CMD_INPROGRESS  0x01
+#define ESP_CMD_WAITING     0x02
+void esp_start_command(Uint8 cmd);
+void esp_finish_command(void);
+void esp_command_clear(void);
+void esp_command_write(Uint8 cmd);
 
 /* ESP Registers */
-Uint8 readtranscountl;
-Uint8 readtranscounth;
 Uint8 writetranscountl;
 Uint8 writetranscounth;
-Uint8 fifo;
-Uint8 command;
+Uint8 fifo[ESP_FIFO_SIZE];
+Uint8 command[2];
 Uint8 status;
 Uint8 selectbusid;
 Uint8 intstatus;
@@ -57,285 +60,471 @@ Uint8 configuration;
 Uint8 clockconv;
 Uint8 esptest;
 
+Uint32 esp_counter;
+
+
+/* Command Register */
+#define CMD_DMA      0x80
+#define CMD_CMD      0x7f
+
+#define CMD_TYP_MASK 0x70
+#define CMD_TYP_MSC  0x00
+#define CMD_TYP_TGT  0x20
+#define CMD_TYP_INR  0x10
+#define CMD_TYP_DIS  0x40
+
+/* Miscellaneous Commands */
+#define CMD_NOP      0x00
+#define CMD_FLUSH    0x01
+#define CMD_RESET    0x02
+#define CMD_BUSRESET 0x03
+/* Initiator Commands */
+#define CMD_TI       0x10
+#define CMD_ICCS     0x11
+#define CMD_MSGACC   0x12
+#define CMD_PAD      0x18
+#define CMD_SATN     0x1a
+/* Disconnected Commands */
+#define CMD_RESEL    0x40
+#define CMD_SEL      0x41
+#define CMD_SELATN   0x42
+#define CMD_SELATNS  0x43
+#define CMD_ENSEL    0x44
+#define CMD_DISSEL   0x45
+/* Target Commands */
+#define CMD_SEMSG    0x20
+#define CMD_SESTAT   0x21
+#define CMD_SEDAT    0x22
+#define CMD_DISSEQ   0x23
+#define CMD_TERMSEQ  0x24
+#define CMD_TCCS     0x25
+#define CMD_DIS      0x27
+#define CMD_RMSGSEQ  0x28
+#define CMD_RCOMM    0x29
+#define CMD_RDATA    0x2A
+#define CMD_RCSEQ    0x2B
+
+/* Status Register */
+#define STAT_MASK    0xF8
+#define STAT_PHASE   0x07
+
+#define STAT_VGC     0x08
+#define STAT_TC      0x10
+#define STAT_PE      0x20
+#define STAT_GE      0x40
+#define STAT_INT     0x80
+
+/* Bus ID Register */
+#define BUSID_DID    0x07
+
+/* Interrupt Status Register */
+#define INTR_SEL     0x01
+#define INTR_SELATN  0x02
+#define INTR_RESEL   0x04
+#define INTR_FC      0x08
+#define INTR_BS      0x10
+#define INTR_DC      0x20
+#define INTR_ILL     0x40
+#define INTR_RST     0x80
+
+/* Sequence Step Register */
+#define SEQ_0        0x00
+#define SEQ_SELTIMEOUT 0x02
+#define SEQ_CD       0x04
+
+/* Configuration Register */
+#define CFG1_RESREPT 0x40
+
+
 /* ESP Status Variables */
-Uint8 irq_status;
 Uint8 mode_dma;
 
-/* ESP FIFO */
-#define ESP_FIFO_SIZE 16
-Uint8 esp_fifo[ESP_FIFO_SIZE];
-Uint8 fifo_read_ptr;
-Uint8 fifo_write_ptr;
-
-/* Command buffer */
-#define SCSI_CMD_BUF_SIZE 16
-Uint8 commandbuf[SCSI_CMD_BUF_SIZE];
-int command_len;
+/* Experimental */
+#define ESP_CLOCK_FREQ  20      /* ESP is clocked at 20 MHz */
+#define ESP_DELAY       100     /* Standard wait time for ESP interrupt (except bus reset and selection timeout) */
 
 
-/* Experimental */
+/* ESP DMA control and status registers */
 
-//Uint32 dma;
-Uint32 do_cmdvar;
-Uint32 dma_counter;
-Uint32 data_len;
-Uint32 dma_left;
-Uint32 cmdlen;
-Uint32 async_len;
-Uint8 *async_buf;
-int dma_enabled = 1;
-void (*dma_cb);
-
-static bool no_target=false;
-static int bus_id_command=0;
-
-
-void SCSI_CSR0_Read(void) {
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK] = csr_value0;
- 	Log_Printf(LOG_SCSI_LEVEL,"SCSI DMA control read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
-
-void SCSI_CSR0_Write(void) {
-    Log_Printf(LOG_SCSI_LEVEL,"SCSI DMA control write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-    csr_value0 = IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+void ESP_DMA_CTRL_Read(void) {
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK] = esp_dma.control;
+ 	Log_Printf(LOG_ESPDMA_LEVEL,"ESP DMA control read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_DMA_CTRL_Write(void) {
+    Log_Printf(LOG_ESPDMA_LEVEL,"ESP DMA control write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+    esp_dma.control = IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
         
-    if ((csr_value0 & 0x04) == 0x04) {
-        Log_Printf(LOG_SCSI_LEVEL, "flush FIFO\n");
+    if (esp_dma.control&ESPCTRL_FLUSH) {
+        Log_Printf(LOG_ESPDMA_LEVEL, "flush DMA buffer\n");
+		if (ConfigureParams.System.bTurbo) {
+			tdma_flush_buffer(0);
+		} else {
+			dma_esp_flush_buffer();
+		}
     }
-    if ((csr_value0 & 0x01) == 0x01) {
-        Log_Printf(LOG_SCSI_LEVEL, "scsi chip is WD33C92\n");
+    if (esp_dma.control&ESPCTRL_CHIP_TYPE) {
+        Log_Printf(LOG_ESPDMA_LEVEL, "SCSI controller is WD33C92\n");
     } else {
-        Log_Printf(LOG_SCSI_LEVEL, "scsi chip is NCR53C90\n");
+        Log_Printf(LOG_ESPDMA_LEVEL, "SCSI controller is NCR53C90\n");
     }
-    if ((csr_value0 & 0x02) == 0x02) {
-        Log_Printf(LOG_SCSI_LEVEL, "reset scsi chip\n");
+    if (esp_dma.control&ESPCTRL_RESET) {
+        Log_Printf(LOG_ESPDMA_LEVEL, "reset SCSI controller\n");
         esp_reset_hard();
     }
-    if ((csr_value0 & 0x08) == 0x08) {
-        Log_Printf(LOG_SCSI_LEVEL, "dma from scsi to mem\n");
+    if (esp_dma.control&ESPCTRL_DMA_READ) {
+        Log_Printf(LOG_ESPDMA_LEVEL, "DMA from SCSI to mem\n");
+    } else {
+        Log_Printf(LOG_ESPDMA_LEVEL, "DMA from mem to SCSI\n");
     }
-    if ((csr_value0 & 0x10) == 0x10) {
-        // set_interrupt(INT_SCSI_DMA, SET_INT);
-        // esp_raise_irq(); 
-        Log_Printf(LOG_SCSI_LEVEL, "mode DMA\n");
-    }else{
-        // set_interrupt(INT_SCSI_DMA, RELEASE_INT);
-        //esp_lower_irq();
-        Log_Printf(LOG_SCSI_LEVEL, "mode PIO\n");
-    }
-    if ((csr_value0 & 0x20) == 0x20) {
-        Log_Printf(LOG_SCSI_LEVEL, "interrupt enable");
-    }
-    switch (csr_value0 & 0xC0) {
-        case 0x00:
-            Log_Printf(LOG_SCSI_LEVEL, "10 MHz clock\n");
+    if (esp_dma.control&ESPCTRL_MODE_DMA) {
+        Log_Printf(LOG_ESPDMA_LEVEL, "mode DMA\n");
+    } else {
+        Log_Printf(LOG_ESPDMA_LEVEL, "mode PIO\n");
+    }
+    if (esp_dma.control&ESPCTRL_ENABLE_INT) {
+        Log_Printf(LOG_ESPDMA_LEVEL, "Enable ESP interrupt");
+        if (status&STAT_INT) {
+            set_interrupt(INT_SCSI, SET_INT);
+        }
+    } else {
+        Log_Printf(LOG_ESPDMA_LEVEL, "Block ESP interrupt");
+        set_interrupt(INT_SCSI, RELEASE_INT);
+    }
+    switch (esp_dma.control&ESPCTRL_CLKMASK) {
+        case ESPCTRL_CLK10MHz:
+            Log_Printf(LOG_ESPDMA_LEVEL, "10 MHz clock\n");
             break;
-        case 0x40:
-            Log_Printf(LOG_SCSI_LEVEL, "12.5 MHz clock\n");
+        case ESPCTRL_CLK12MHz:
+            Log_Printf(LOG_ESPDMA_LEVEL, "12.5 MHz clock\n");
             break;
-        case 0xC0:
-            Log_Printf(LOG_SCSI_LEVEL, "16.6 MHz clock\n");
+        case ESPCTRL_CLK16MHz:
+            Log_Printf(LOG_ESPDMA_LEVEL, "16.6 MHz clock\n");
             break;
-        case 0x80:
-            Log_Printf(LOG_SCSI_LEVEL, "20 MHz clock\n");
+        case ESPCTRL_CLK20MHz:
+            Log_Printf(LOG_ESPDMA_LEVEL, "20 MHz clock\n");
             break;
         default:
             break;
     }
 }
 
-void SCSI_CSR1_Read(void) {
-    
-    /*
-     * dma fifo status register
-     */
-#define	S5RDMAS_STATE		0xc0	/* DMA/SCSI bank state */
-#define	S5RDMAS_D0S0		0x00	/* DMA rdy for bank 0, SCSI in bank 0 */
-#define	S5RDMAS_D0S1		0x40	/* DMA req for bank 0, SCSI in bank 1 */
-#define	S5RDMAS_D1S1		0x80	/* DMA rdy for bank 1, SCSI in bank 1 */
-#define	S5RDMAS_D1S0		0xc0	/* DMA req for bank 1, SCSI in bank 0 */
-#define	S5RDMAS_OUTFIFOMASK	0x38	/* output fifo byte (INVERTED) */
-#define	S5RDMAS_INFIFOMASK	0x07	/* input fifo byte (INVERTED) */
-    
-#define	S5RDMA_FIFOALIGNMENT	4	/* mass storage chip buffer size */
-
-    
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK] = 0x80;
- 	Log_Printf(LOG_SCSI_LEVEL,"SCSI DMA FIFO status read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+void ESP_DMA_FIFO_STAT_Read(void) {
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK] = esp_dma.status;
+ 	Log_Printf(LOG_ESPDMA_LEVEL,"ESP DMA FIFO status read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
 }
 
-void SCSI_CSR1_Write(void) {
- 	Log_Printf(LOG_SCSI_LEVEL,"SCSI DMA FIFO status write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-    csr_value1 = IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+void ESP_DMA_FIFO_STAT_Write(void) {
+ 	Log_Printf(LOG_ESPDMA_LEVEL,"ESP DMA FIFO status write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+    esp_dma.status = IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
 }
 
+void ESP_DMA_set_status(void) { /* this is just a guess */
+    if ((esp_dma.status&ESPSTAT_STATE_MASK) == ESPSTAT_STATE_D0S1) {
+        //Log_Printf(LOG_WARN,"DMA in buffer 0, SCSI in buffer 1\n");
+        esp_dma.status = (esp_dma.status&~ESPSTAT_STATE_MASK)|ESPSTAT_STATE_D1S0;
+    } else {
+        //Log_Printf(LOG_WARN,"DMA in buffer 1, SCSI in buffer 0\n");
+        esp_dma.status = (esp_dma.status&~ESPSTAT_STATE_MASK)|ESPSTAT_STATE_D0S1;
+    }
+}
 
 /* ESP Registers */
 
-void SCSI_TransCountL_Read(void) { // 0x02014000
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=readtranscountl;
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP TransCountL read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+void ESP_TransCountL_Read(void) { // 0x02014000
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=esp_counter&0xFF;
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP TransCountL read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
 }
 
-void SCSI_TransCountL_Write(void) {
+void ESP_TransCountL_Write(void) {
     writetranscountl=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP TransCountL write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP TransCountL write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
 }
 
-void SCSI_TransCountH_Read(void) { // 0x02014001
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=readtranscounth;
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP TransCoundH read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+void ESP_TransCountH_Read(void) { // 0x02014001
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=(esp_counter>>8)&0xFF;
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP TransCoundH read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
 }
 
-void SCSI_TransCountH_Write(void) {
+void ESP_TransCountH_Write(void) {
     writetranscounth=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP TransCountH write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP TransCountH write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
 }
 
-void SCSI_FIFO_Read(void) { // 0x02014002
-    if ((fifo_write_ptr - fifo_read_ptr) > 0) {
-        IoMem[IoAccessCurrentAddress & IO_SEG_MASK] = esp_fifo[fifo_read_ptr];
-        Log_Printf(LOG_SCSI_LEVEL,"ESP FIFO read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-        Log_Printf(LOG_WARN,"ESP FIFO Read, size = %i, val=%02x", fifo_write_ptr - fifo_read_ptr, IoMem[IoAccessCurrentAddress & IO_SEG_MASK]);
-        fifo_read_ptr++;
-        fifoflags = fifoflags - 1;
-//        esp_raise_irq();
+void ESP_FIFO_Read(void) { // 0x02014002
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK] = esp_fifo_read();
+    Log_Printf(LOG_ESPREG_LEVEL,"ESP FIFO read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_FIFO_Write(void) {
+    esp_fifo_write(IoMem[IoAccessCurrentAddress & IO_SEG_MASK]);
+    Log_Printf(LOG_ESPREG_LEVEL,"ESP FIFO write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_Command_Read(void) { // 0x02014003
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=command[0];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP Command read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_Command_Write(void) {
+    esp_command_write(IoMem[IoAccessCurrentAddress & IO_SEG_MASK]);
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP Command write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_Status_Read(void) { // 0x02014004
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=(status&STAT_MASK)|(SCSIbus.phase&STAT_PHASE);
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP Status read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_SelectBusID_Write(void) {
+    selectbusid=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP SelectBusID write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_IntStatus_Read(void) { // 0x02014005
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=intstatus;
+    Log_Printf(LOG_ESPREG_LEVEL,"ESP IntStatus read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+    
+    if (status&STAT_INT) {
+            intstatus = 0x00;
+            status &= ~(STAT_VGC | STAT_PE | STAT_GE);
+            //seqstep = 0x00; /* FIXME: Is the data sheet really wrong with this? */
+            esp_lower_irq();
+    }
+}
+
+void ESP_SelectTimeout_Write(void) {
+    selecttimeout=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP SelectTimeout write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_SeqStep_Read(void) { // 0x02014006
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=seqstep;
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP SeqStep read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_SyncPeriod_Write(void) {
+    syncperiod=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP SyncPeriod write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_FIFOflags_Read(void) { // 0x02014007
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=fifoflags;
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP FIFOflags read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_SyncOffset_Write(void) {
+    syncoffset=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP SyncOffset write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_Configuration_Read(void) { // 0x02014008
+    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=configuration;
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP Configuration read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_Configuration_Write(void) {
+    configuration=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP Configuration write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_ClockConv_Write(void) { // 0x02014009
+    clockconv=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP ClockConv write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+void ESP_Test_Write(void) { // 0x0201400a
+    esptest=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
+ 	Log_Printf(LOG_ESPREG_LEVEL,"ESP Test write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+/* System reads this register to check if we use old or new SCSI controller.
+ * Return 0 to report old chip. */
+void ESP_Conf2_Read(void) { // 0x0201400b
+    if (ConfigureParams.System.nSCSI == NCR53C90)
+        IoMem[IoAccessCurrentAddress&IO_SEG_MASK] = 0x00;
+    Log_Printf(LOG_ESPREG_LEVEL,"ESP Configuration2 read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+}
+
+
+/* Helper functions */
+
+/* Functions for reading and writing ESP FIFO */
+Uint8 esp_fifo_read(void) {
+    int i;
+    Uint8 val;
+    
+    if (fifoflags > 0) {
+        val = fifo[0];
+        for (i=0; i<(ESP_FIFO_SIZE-1); i++)
+            fifo[i]=fifo[i+1];
+        fifo[ESP_FIFO_SIZE-1] = 0x00;
+        fifoflags--;
+        Log_Printf(LOG_ESPFIFO_LEVEL,"ESP FIFO: Reading byte, val=%02x, size = %i", val, fifoflags);
+    } else {
+        val = 0x00;
+        Log_Printf(LOG_WARN, "ESP FIFO read: FIFO is empty!\n");
+    }
+    return val;
+}
+
+void esp_fifo_write(Uint8 val) {
+    if (fifoflags==ESP_FIFO_SIZE) {
+        Log_Printf(LOG_WARN, "ESP FIFO write: FIFO overflow! Top of FIFO overwritten\n");
+        fifo[fifoflags-1] = val;
+        status |= STAT_GE;
     } else {
-        IoMem[IoAccessCurrentAddress & IO_SEG_MASK] = 0x00;
-        Log_Printf(LOG_WARN, "ESP FIFO is empty!\n");
-    } 
+        fifoflags++;
+        fifo[fifoflags-1] = val;
+        Log_Printf(LOG_ESPFIFO_LEVEL,"ESP FIFO: Writing byte %i, val=%02x", fifoflags-1, fifo[fifoflags-1]);
+    }
+}
+
+void esp_fifo_clear(void) {
+    int i;
+    for (i=0; i<ESP_FIFO_SIZE; i++) {
+        fifo[i] = 0;
+    }
+    fifoflags &= 0xE0;
 }
 
-void SCSI_FIFO_Write(void) {
-    esp_fifo[fifo_write_ptr] = IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
-    fifo_write_ptr++;
-    fifoflags = fifoflags + 1;
-    Log_Printf(LOG_SCSI_LEVEL,"ESP FIFO write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+/* Functions for handling dual ranked command register */
+void esp_command_write(Uint8 cmd) {
+    if ((command[1]&CMD_CMD)==CMD_RESET && (cmd&CMD_CMD)!=CMD_NOP) {
+        Log_Printf(LOG_WARN, "ESP command write: Chip reset in command register, not executing command.\n");
+    } else {
+        command[1] = cmd;
+        
+        if (esp_cmd_state&ESP_CMD_WAITING) {
+            Log_Printf(LOG_WARN, "ESP command write: Error! Top of command register overwritten.\n");
+            status |= STAT_GE;
+        }
+    }
     
-    if (fifo_write_ptr > (ESP_FIFO_SIZE - 1)) {
-        Log_Printf(LOG_SCSI_LEVEL, "ESP FIFO overflow! Resetting FIFO!\n");
-        esp_flush_fifo();
+    if ((command[1]&CMD_CMD)==CMD_RESET || (command[1]&CMD_CMD)==CMD_BUSRESET) {
+        esp_start_command(command[1]);
+        return;
+    }
+    
+    if (esp_cmd_state&ESP_CMD_INPROGRESS) {
+        esp_cmd_state |= ESP_CMD_WAITING;
+    } else {
+        command[0] = command[1];
+        command[1] = 0x00;
+        esp_start_command(command[0]);
     }
-    Log_Printf(LOG_SCSI_LEVEL,"ESP FIFO size = %i", fifo_write_ptr - fifo_read_ptr);
 }
 
-void SCSI_Command_Read(void) { // 0x02014003
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=command;
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP Command read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+void esp_command_clear(void) {
+    command[0] = 0x00;
+    if ((command[1]&CMD_CMD)!=CMD_RESET) {
+        command[1] = 0x00;
+        esp_cmd_state &= ~ESP_CMD_WAITING;
+    }
 }
 
-void SCSI_Command_Write(void) {
-    command=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP Command write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+void esp_finish_command(void) {
+    esp_cmd_state &= ~ESP_CMD_INPROGRESS;
+    if (esp_cmd_state&ESP_CMD_WAITING) {
+        command[0] = command[1];
+        command[1] = 0x00;
+        esp_cmd_state &= ~ESP_CMD_WAITING;
+        esp_start_command(command[0]);
+    }
+}
+
+void esp_start_command(Uint8 cmd) {
+    esp_cmd_state |= ESP_CMD_INPROGRESS;
     
-    if (command & CMD_DMA) {
-        readtranscountl = writetranscountl;
-        readtranscounth = writetranscounth;
+    /* Check if command is valid for actual state */
+    if ((cmd&CMD_TYP_MASK)!=CMD_TYP_MSC) {
+        if ((esp_state==TARGET && !(cmd&CMD_TYP_TGT)) ||
+            (esp_state==INITIATOR && !(cmd&CMD_TYP_INR)) ||
+            (esp_state==DISCONNECTED && !(cmd&CMD_TYP_DIS))) {
+            Log_Printf(LOG_WARN, "ESP Command: Illegal command for actual ESP state ($%02X)!\n",cmd);
+            esp_command_clear();
+            intstatus |= INTR_ILL;
+            CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
+            return;
+        }
+    }
+    
+    /* Check if the command is a DMA command */
+    if (cmd & CMD_DMA) {
+        /* Load the internal counter on every DMA command, do not decrement actual registers! */
+        esp_counter = writetranscountl | (writetranscounth << 8);
+        if (esp_counter == 0) { /* 0 means maximum value */
+            esp_counter = 0x10000;
+        }
         status &= ~STAT_TC;
         mode_dma = 1;
     } else {
         mode_dma = 0;
     }
     
-    if ((command & CMD_CMD) != CMD_NOP)
-    	status = (status&STAT_MASK)|STAT_CD;
-    
-    switch (command & CMD_CMD) {
+    switch (cmd & CMD_CMD) {
             /* Miscellaneous */
         case CMD_NOP:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: NOP\n");
-    	    // status = (status&STAT_MASK)|STAT_ST;
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: NOP\n");
+            esp_finish_command();
             break;
         case CMD_FLUSH:
-            Log_Printf(LOG_SCSI_LEVEL,"ESP Command: flush FIFO\n");
+            Log_Printf(LOG_ESPCMD_LEVEL,"ESP Command: flush FIFO\n");
             esp_flush_fifo();
-    	    status = (status&STAT_MASK)|STAT_ST;
             break;
         case CMD_RESET:
-            Log_Printf(LOG_SCSI_LEVEL,"ESP Command: reset chip\n");
+            Log_Printf(LOG_ESPCMD_LEVEL,"ESP Command: reset chip\n");
             esp_reset_hard();
             break;
         case CMD_BUSRESET:
-            //Reset all Devices on SCSI bus
-            Log_Printf(LOG_WARN, "ESP Command: reset SCSI bus\n");
-            esp_reset_soft();
-            if (!(configuration & CFG1_RESREPT)) {
-                intstatus = INTR_RST;
-                status = (status&STAT_MASK)|STAT_MI;
-            	seqstep = 0;
-                Log_Printf(LOG_SCSI_LEVEL,"Bus Reset raising IRQ configuration=%x\n",configuration);
-                //esp_raise_irq(); // temporary disabled for experiment! moved to read config reg.
-            } else 
-                Log_Printf(LOG_SCSI_LEVEL,"Bus Reset not interrupting configuration=%x\n",configuration);
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: reset SCSI bus\n");
+            esp_bus_reset();
             break;
             /* Disconnected */
+        case CMD_RESEL:
+            Log_Printf(LOG_WARN, "ESP Command: reselect sequence\n");
+            abort();
+            break;
         case CMD_SEL:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: select without ATN sequence\n");
-	    //abort();
-            handle_satn(); // enabled for experiment!
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: select without ATN sequence\n");
+            esp_select(false);
             break;
         case CMD_SELATN:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: select with ATN sequence\n");
-            handle_satn();
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: select with ATN sequence\n");
+            esp_select(true);
             break;
         case CMD_SELATNS:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: select with ATN and stop sequence\n");
-	    abort();
+            Log_Printf(LOG_WARN, "ESP Command: select with ATN and stop sequence\n");
+            abort();
             break;
         case CMD_ENSEL:
-            Log_Printf(LOG_WARN, "ESP Command: enable selection/reselection no_target=%x intstatus=%x status=%x seqstep=%x",
-			no_target,intstatus,status,seqstep);
-            if (no_target) {
-                Log_Printf(LOG_SCSI_LEVEL, "ESP retry timeout");
-                intstatus = INTR_RESEL;
-            	seqstep = SEQ_SELTIMEOUT;
-            } else {
-                Log_Printf(LOG_WARN, "Reselect: target %i, lun %i\n", SCSIcommand.target, SCSIcommand.lun);
-                status = (status&STAT_MASK)|STAT_MI;
-                intstatus = INTR_RESEL|INTR_FC;
-                esp_fifo[0] = (1<<SCSIcommand.target); // target bit
-                esp_fifo[1] = 0x80|(SCSIcommand.lun&0x07); // identify message (identifymask|(lun&lun_mask))
-                fifoflags = 2;
-                fifo_write_ptr = 2;
-                fifo_read_ptr = 0;
-//                esp_raise_irq();
-            }
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: enable selection/reselection\n");
+            esp_finish_command(); /* Our disk doesn't do reselections */
             break;
         case CMD_DISSEL:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: disable selection/reselection\n");
-            intstatus = INTR_FC;
-            esp_raise_irq();
+            Log_Printf(LOG_WARN, "ESP Command: disable selection/reselection\n");
+            abort();
             break;
             /* Initiator */
         case CMD_TI:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: transfer information\n");
-            handle_ti();
-    	    esp_raise_irq();
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: transfer information\n");
+            esp_transfer_info();
             break;
         case CMD_ICCS:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: initiator command complete sequence\n");
-            write_response();
-            intstatus = INTR_FC;
-            status = (status&STAT_MASK)|STAT_MI;
-            esp_raise_irq();
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: initiator command complete sequence\n");
+            esp_initiator_command_complete();
             break;
         case CMD_MSGACC:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: message accepted\n");
-            status = (status&STAT_MASK)|STAT_ST;	   
-            intstatus = INTR_BS;
-            seqstep = SEQ_0;
-            fifoflags = 0x00;
-            esp_raise_irq();
-            break;            
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: message accepted\n");
+            esp_message_accepted();
+            break;
         case CMD_PAD:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: transfer pad\n");
-            status = STAT_TC | STAT_DI;
-            intstatus = INTR_FC;
-            seqstep = SEQ_0;
-            esp_raise_irq();
+            Log_Printf(LOG_ESPCMD_LEVEL, "ESP Command: transfer pad\n");
+            esp_transfer_pad();
             break;
         case CMD_SATN:
-            Log_Printf(LOG_SCSI_LEVEL, "ESP Command: set ATN\n");
+            Log_Printf(LOG_WARN, "ESP Command: set ATN\n");
+            abort();
             break;
             /* Target */
         case CMD_SEMSG:
@@ -348,460 +537,383 @@ void SCSI_Command_Write(void) {
         case CMD_RCOMM:
         case CMD_RDATA:
         case CMD_RCSEQ:
-
-            status = (status&STAT_MASK)|STAT_ST;	   
-            intstatus = INTR_ILL;
-            seqstep = SEQ_0;
-            fifoflags = 0x00;
-            esp_raise_irq();
-            Log_Printf(LOG_WARN, "ESP Command: Target commands not supported!\n");
+            Log_Printf(LOG_WARN, "ESP Command: Target commands not emulated!\n");
+            abort();
             break;
         case CMD_DIS:
-
-
-            Log_Printf(LOG_WARN, "ESP Command: DISCONNECT !\n");
-            status = (status&STAT_MASK)|STAT_ST;	   
-            intstatus = INTR_DC; 
+            Log_Printf(LOG_WARN, "ESP Command: DISCONNECT not emulated!\n");
+            abort();
+            SCSIbus.phase = PHASE_ST;
+            intstatus = INTR_DC;
             seqstep = SEQ_0;
             break;
-
             
         default:
             Log_Printf(LOG_WARN, "ESP Command: Illegal command!\n");
+            esp_command_clear();
             intstatus |= INTR_ILL;
+            CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
             break;
     }
 }
 
-void SCSI_Status_Read(void) { // 0x02014004
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=status;
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP Status read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
-
-void SCSI_SelectBusID_Write(void) {
-    selectbusid=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP SelectBusID write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-	no_target=false;
-}
-
-void SCSI_IntStatus_Read(void) { // 0x02014005
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=intstatus;
-    Log_Printf(LOG_SCSI_LEVEL,"ESP IntStatus read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-    
-    if (irq_status == 1) {
-            intstatus = 0x00;
-            status &= ~(STAT_VGC | STAT_PE | STAT_GE );
-            seqstep = SEQ_0;
-            esp_lower_irq();
-    }
-}
-
-void SCSI_SelectTimeout_Write(void) {
-    selecttimeout=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP SelectTimeout write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
-
-void SCSI_SeqStep_Read(void) { // 0x02014006
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=seqstep;
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP SeqStep read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
-
-void SCSI_SyncPeriod_Write(void) {
-    syncperiod=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP SyncPeriod write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
-
-void SCSI_FIFOflags_Read(void) { // 0x02014007
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=fifoflags;
- 	Log_Printf(LOG_WARN,"ESP FIFOflags read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
-
-void SCSI_SyncOffset_Write(void) {
-    syncoffset=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP SyncOffset write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
-
-void SCSI_Configuration_Read(void) { // 0x02014008
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=configuration;
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP Configuration read at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-    esp_raise_irq(); // experimental!
-}
 
-void SCSI_Configuration_Write(void) {
-    configuration=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP Configuration write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+/* This is the handler function for ESP delayed interrupts */
+void ESP_InterruptHandler(void) {
+	CycInt_AcknowledgeInterrupt();
+    esp_raise_irq();
 }
 
-void SCSI_ClockConv_Write(void) { // 0x02014009
-    IoMem[IoAccessCurrentAddress & IO_SEG_MASK]=clockconv;
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP ClockConv write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
-}
 
-void SCSI_Test_Write(void) { // 0x0201400a
-    esptest=IoMem[IoAccessCurrentAddress & IO_SEG_MASK];
- 	Log_Printf(LOG_SCSI_LEVEL,"ESP Test write at $%08x val=$%02x PC=$%08x\n", IoAccessCurrentAddress, IoMem[IoAccessCurrentAddress & IO_SEG_MASK], m68k_getpc());
+void esp_raise_irq(void) {
+    if(!(status & STAT_INT)) {
+        status |= STAT_INT;
+        
+        if (esp_dma.control&ESPCTRL_ENABLE_INT) {
+            set_interrupt(INT_SCSI, SET_INT);
+        }
+        
+        if (LOG_ESPCMD_LEVEL == LOG_WARN) {
+            printf("[ESP] Raise IRQ: state=");
+            switch (esp_state) {
+                case DISCONNECTED: printf("disconnected"); break;
+                case INITIATOR: printf("initiator"); break;
+                case TARGET: printf("target"); break;
+                default: printf("unknown"); break;
+            }
+            printf(", phase=");
+            switch (SCSIbus.phase&STAT_PHASE) {
+                case PHASE_DO: printf("data out"); break;
+                case PHASE_DI: printf("data in"); break;
+                case PHASE_CD: printf("command"); break;
+                case PHASE_ST: printf("status"); break;
+                case PHASE_MI: printf("msg in"); break;
+                case PHASE_MO: printf("msg out"); break;
+                default: printf("unknown"); break;
+            }
+            if (status&STAT_TC) {
+                printf(", transfer complete");
+            } else {
+                printf(", transfer not complete");
+            }
+            printf(", sequence step=%i", seqstep);
+            printf(", interrupt status:\n");
+            if (intstatus&INTR_RST) printf("bus reset\n");
+            if (intstatus&INTR_BS) printf("bus service\n");
+            if (intstatus&INTR_DC) printf("disconnected\n");
+            if (intstatus&INTR_FC) printf("function complete\n");
+            if (intstatus&INTR_ILL) printf("illegal command\n");
+            if (intstatus&INTR_RESEL) printf("reselected\n");
+            if (intstatus&INTR_SEL) printf("selected\n");
+            if (intstatus&INTR_SELATN) printf("selected with ATN\n");
+        }
+    }
 }
 
-/* System reads this register to check if we use old or new SCSI controller.
- * Return 0 to report old chip. */
-void SCSI_Conf2_Read(void) { // 0x0201400b
-    if (ConfigureParams.System.nSCSI == NCR53C90)
-        IoMem[IoAccessCurrentAddress&IO_SEG_MASK] = 0x00;
+void esp_lower_irq(void) {
+    if (status & STAT_INT) {
+        status &= ~STAT_INT;
+        
+        set_interrupt(INT_SCSI, RELEASE_INT);
+        
+        Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Lower IRQ\n");
+        
+        esp_finish_command();
+    }
 }
 
-
 /* Functions */
 
+/* Reset chip */
 void esp_reset_hard(void) {
-    Log_Printf(LOG_WARN, "ESP hard reset\n");
-    /* hard reset */
+    Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Hard reset\n");
+
     clockconv = 0x02;
     configuration &= ~0xF8; // clear chip test mode, parity enable, parity test, scsi request/int disable, slow cable mode
+    esp_fifo_clear();
     syncperiod = 0x05;
     syncoffset = 0x00;
+    status &= ~STAT_INT; // release interrupt
+    set_interrupt(INT_SCSI, RELEASE_INT);
     intstatus = 0x00;
-    status &= ~(STAT_VGC | STAT_PE | STAT_GE); // need valid group code bit? clear transfer complete aka valid group code, parity error, gross error
-    esp_flush_fifo();
-    esp_lower_irq();
+    status &= ~(STAT_VGC | STAT_PE | STAT_GE); // clear transfer complete aka valid group code, parity error, gross error
     esp_reset_soft();
+    esp_finish_command();
 }
 
+
+
 void esp_reset_soft(void) {
-    status &= ~STAT_TC; // clear transfer count zero
-    
-    /* incomplete, need to work on everything below this point */
+    status &= ~STAT_TC; /* clear transfer count zero */
     
-    dma_cb = NULL; // clear DMA interface - how to do this?
+    /* check, if this is complete */
     mode_dma = 0;
-    readtranscountl = 0x00;
-    readtranscounth = 0x00;
-    writetranscountl = 0x00;
-    writetranscounth = 0x00;
-
+    esp_counter = 0; /* reset counter, but not actual registers! */
+    
     seqstep = 0x00;
+    
+    /* writetranscountl, writetranscounth, selectbusid, selecttimeout are not initialized by reset */
 
-    command = 0x00;
-    selectbusid = 0x00;
-    selecttimeout= 0x00;
-    fifoflags = 0x00;
-    esptest = 0x00;
+    /* This part is "disconnect reset" */
+    esp_command_clear();
+    esp_state = DISCONNECTED;
 }
 
-void esp_raise_irq(void) {
-    if(!(status & STAT_INT)) {
-        status |= STAT_INT;
-        irq_status = 1;
-        
-        set_interrupt(INT_SCSI, SET_INT);
-        
-        //Log_Printf(LOG_SCSI_LEVEL, "Raise IRQ\n");
-        char s[16];
-        switch (state) {
-            case DISCONNECTED: sprintf(s, "disconntected"); break;
-            case RESELECTABLE: sprintf(s, "reselectable"); break;
-            case SELECTING: sprintf(s, "selecting"); break;
-            case INITIATOR: sprintf(s, "initiator"); break;
-            case DISABLINGSEL: sprintf(s, "diablingsel"); break;
-            case COMPLETING: sprintf(s, "completing"); break;
-            case DMAING: sprintf(s, "dmaing"); break;
-            case ACCEPTINGMSG: sprintf(s, "acceptingmsg"); break;
-            case SENDINGMSG: sprintf(s, "sendingmsg"); break;
-            case PADDING: sprintf(s, "padding"); break;
-            case GETTINGMSG: sprintf(s, "gettingmsg"); break;
-            case GETTINGLUN: sprintf(s, "gettinglun"); break;
-            case IGNOREILLCMD: sprintf(s, "ignoreillcmd"); break;
-            default: sprintf(s, "unknown"); break;
-        }
-//        Log_Printf(LOG_WARN, "Raise IRQ: state: %s\n", s);
-        Log_Printf(LOG_WARN, "Raise IRQ:");
-        switch (status&STAT_MI) {
-            case STAT_DO: sprintf(s, "data out"); break;
-            case STAT_DI: sprintf(s, "data in"); break;
-            case STAT_CD: sprintf(s, "command"); break;
-            case STAT_ST: sprintf(s, "status"); break;
-            case STAT_MI: sprintf(s, "msg in"); break;
-            case STAT_MO: sprintf(s, "msg out"); break;
-            default: sprintf(s, "unknown"); break;
-        }
-        Log_Printf(LOG_WARN, "Status: %02x (phase: %s, %s%s)\n", status, s,
-                   status&STAT_TC?"transfer complete":"",
-                   status&STAT_INT?", interrupt":"");
-        Log_Printf(LOG_WARN, "Sequence Step: %02x\n", seqstep);
-        Log_Printf(LOG_WARN, "Interrupt Status: %02x (%s%s%s%s%s%s%s%s)\n", intstatus,
-                   intstatus&INTR_RST?"bus reset, ":"",
-                   intstatus&INTR_BS?"bus service, ":"",
-                   intstatus&INTR_DC?"disconnected, ":"",
-                   intstatus&INTR_FC?"function compl, ":"",
-                   intstatus&INTR_ILL?"illegal comm, ":"",
-                   intstatus&INTR_RESEL?"reselected, ":"",
-                   intstatus&INTR_SEL?"selected, ":"",
-                   intstatus&INTR_SELATN?"selected w ATN":"");
-    }
-}
 
-void esp_lower_irq(void) {
-    if (status & STAT_INT) {
-        status &= ~STAT_INT;
-        irq_status = 0;
-        
-        set_interrupt(INT_SCSI, RELEASE_INT);
-        
-        Log_Printf(LOG_SCSI_LEVEL, "Lower IRQ\n");
+/* Reset SCSI bus */
+void esp_bus_reset(void) {
+    
+    esp_reset_soft();
+    if (!(configuration & CFG1_RESREPT)) {
+        intstatus = INTR_RST;
+        SCSIbus.phase = PHASE_MI; /* CHECK: why message in phase? */
+        Log_Printf(LOG_ESPCMD_LEVEL,"[ESP] SCSI bus reset raising IRQ (configuration=$%02X)\n",configuration);
+        CycInt_AddRelativeInterruptUs(500, 0, INTERRUPT_ESP); /* CHECK: how is this delay defined? */
+    } else {
+        Log_Printf(LOG_ESPCMD_LEVEL,"[ESP] SCSI bus reset not interrupting (configuration=$%02X)\n",configuration);
+        esp_finish_command();
     }
 }
 
+
+/* Flush FIFO */
 void esp_flush_fifo(void) {
-    fifo_read_ptr = 0;
-    fifo_write_ptr = 0;
-    esp_fifo[0] = 0;
-    fifoflags &= 0xE0;
+    esp_fifo_clear();
+    esp_finish_command();
 }
 
-Uint32 get_cmd (void) {
+
+/* Select with or without ATN */
+void esp_select(bool atn) {
+    int cmd_size;
+    Uint8 identify_msg = 0;
+    Uint8 commandbuf[SCSI_CDB_MAX_SIZE];
+
+    seqstep = 0;
+    
+    /* First select our target */
+    Uint8 target = selectbusid & BUSID_DID; /* Get bus ID from register */
+    bool timeout = SCSIdisk_Select(target);
+    if (timeout) {
+        /* If a timeout occurs, generate disconnect interrupt */
+        intstatus = INTR_DC;
+        esp_command_clear();
+        esp_state = DISCONNECTED;
+        int seltout = (selecttimeout * 8192 * clockconv) / ESP_CLOCK_FREQ; /* timeout in microseconds */
+        Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Select: Target %i, timeout after %i microseconds",target,seltout);
+        CycInt_AddRelativeInterruptUs(seltout, 0, INTERRUPT_ESP);
+        return;
+    }
     
+    /* Next get our command */
     if(mode_dma == 1) {
-        command_len = readtranscountl | (readtranscounth << 8);
-        //dma_memory_read(command_len);
-        memcpy(commandbuf, dma_read_buffer, command_len);
+        cmd_size = esp_counter;
+        Log_Printf(LOG_WARN, "[ESP] Select: Reading command using DMA, size %i byte (not implemented!)",cmd_size);
+        abort();
     } else {
-        command_len = fifo_write_ptr - fifo_read_ptr;
-        memcpy(commandbuf, esp_fifo, command_len);
+        if (atn) { /* Read identify message from FIFO */
+            SCSIbus.phase = PHASE_MO;
+            seqstep = 1;
+            identify_msg = esp_fifo_read();
+            Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Select: Reading message from FIFO");
+            Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Select: Identify Message: $%02X",identify_msg);
+        }
         
-        esp_flush_fifo();
+        /* Read command from FIFO */
+        SCSIbus.phase = PHASE_CD;
+        seqstep = 3;
+        for (cmd_size = 0; cmd_size < SCSI_CDB_MAX_SIZE && fifoflags > 0; cmd_size++) {
+            commandbuf[cmd_size] = esp_fifo_read();
+        }
+
+        Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Select: Reading command from FIFO, size: %i byte",cmd_size);
     }
-    Log_Printf(LOG_SCSI_LEVEL, "get_cmd: len %i", command_len);
     
-    return command_len;
-}
+    Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Select: Target: %i",target);
 
-void do_cmd(void) {
-    Uint8 busid = selectbusid;
-    do_busid_cmd(busid);
-}
+    SCSIdisk_Receive_Command(commandbuf, identify_msg);
+    seqstep = 4;
+    esp_command_clear();
 
-void handle_satn(void) {
-    Uint8 scsi_command_group;
+    intstatus = INTR_BS | INTR_FC;
     
-    get_cmd(); // make get_cmd void function?
-    // clear valid command status
-    status &= ~STAT_VGC;
-
-    /* Decode command to determine the command group and thus the
-     * length of the incoming command. Set "valid group code" bit
-     * in status register if the group is 0, 1, 5, 6, or 7 (group
-     * 2 is also valid on NCR53C90A).
-     */
-    scsi_command_group = (commandbuf[1] & 0xE0) >> 5;
-    if(scsi_command_group < 3 || scsi_command_group > 4) {
-        if(ConfigureParams.System.nSCSI == NCR53C90 && scsi_command_group == 2) {
-            Log_Printf(LOG_WARN, "Invalid command group %i on NCR53C90\n", scsi_command_group);
-        } else {
-            status |= STAT_VGC;
-        }
-    } else {
-        Log_Printf(LOG_WARN, "Invalid command group %i on NCR53C90A\n", scsi_command_group);
-    }
-
-    if (command_len != 0)
-        do_cmd();
+    esp_state = INITIATOR;
+    CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
 }
 
-void do_busid_cmd(Uint8 busid) {
-    int lun,lun2;
-    int target = busid & BUSID_DID;
-    Log_Printf(LOG_SCSI_LEVEL, "do_busid_cmd: busid $%02x",busid);
 
-    lun = (commandbuf[0]&0x07);
-    
-    scsi_command_analyzer(commandbuf, command_len, target,lun);
-    data_len = SCSIcommand.transfer_data_len;
+/* DMA done: this is called as part of transfer info or transfer pad
+ * after DMA transfer has completed. */
 
-    if ((target >= ESP_MAX_DEVS) || (SCSIcommand.timeout==true)) { // experimental
-    Log_Printf(LOG_SCSI_LEVEL, "No target found !! Target %d Lun %d raise irq %s at %d",target,lun,__FILE__,__LINE__);
-        intstatus = INTR_DC;
-        seqstep = 0x00;//SEQ_SELTIMEOUT;
-        status = (status&STAT_MASK)|STAT_MI; // seems weird... http://permalink.gmane.org/gmane.os.netbsd.ports.next68k/305
-        no_target=true;
-        esp_raise_irq();
-	return;
-    }
+enum {
+    ESP_IO_STATE_TRANSFERING,
+    ESP_IO_STATE_FLUSHING,
+    ESP_IO_STATE_DONE
+} esp_io_state;
 
-    if (SCSIcommand.nodevice==true) { // experimental
-    Log_Printf(LOG_SCSI_LEVEL, "No device found !! Target %d Lun %d raise irq %s at %d",target,lun,__FILE__,__LINE__);
-	
-        status = (status&STAT_MASK)|STAT_ST;
-        intstatus = INTR_BS|INTR_FC;//INTR_DC;
-        seqstep = SEQ_CD;//SEQ_SELTIMEOUT;
-        esp_raise_irq();
-        return;
-    }
+bool esp_transfer_done(bool write) {
+    Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Transfer done: ESP counter = %i, SCSI residual bytes: %i",
+               esp_counter,scsi_buffer.size);
     
-    status = (status&STAT_MASK)| STAT_ST;
+    if (esp_counter == 0) { /* Transfer done */
+        intstatus = INTR_FC;
+        status |= STAT_TC;
+        CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
+        return true;
+    } else if ((write && SCSIbus.phase!=PHASE_DI) || (!write && SCSIbus.phase!=PHASE_DO)) { /* Phase change detected */
+        esp_command_clear();
+        intstatus = INTR_BS;
+        CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
+        return true;
+    } /* else continue transfering data, no interrupt */
+    return false;
+}
 
-    if (data_len != 0) {
-        Log_Printf(LOG_SCSI_LEVEL, "executing command\n");
-        status = STAT_TC;
-        
-        dma_left = 0;
-        dma_counter = 0;
-        
-        if(SCSIcommand.transferdirection_todevice == 0) {
-            Log_Printf(LOG_SCSI_LEVEL, "DATA IN\n");
-            status = (status&STAT_MASK)| STAT_DI;
-        } else {
-            Log_Printf(LOG_SCSI_LEVEL, "DATA OUT\n");
-            status = (status&STAT_MASK)| STAT_DO;
+
+/* Transfer information */
+void esp_transfer_info(void) {
+    if(mode_dma) {
+        esp_io_state=ESP_IO_STATE_TRANSFERING;
+        CycInt_AddRelativeInterruptUs(SCSI_Seek_Time() + SCSI_Sector_Time(), 100, INTERRUPT_ESP_IO);
+    } else {
+        Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] start PIO transfer");
+        switch (SCSIbus.phase) {
+            case PHASE_DI:
+                esp_fifo_write(SCSIdisk_Send_Data());
+                CycInt_AddRelativeInterruptCycles(20, INTERRUPT_ESP);
+                break;
+            case PHASE_MI:
+                CycInt_AddRelativeInterruptCycles(20, INTERRUPT_ESP);
+                break;
+            case PHASE_ST:
+                /* FIXME: What should happen here? */
+                Log_Printf(LOG_WARN, "[ESP] Error! Transfer info status phase");
+                CycInt_AddRelativeInterruptCycles(20, INTERRUPT_ESP);
+                break;
+            default:
+                Log_Printf(LOG_WARN, "[ESP] PIO transfer (unimplemented)");
+                abort();
+                break;
         }
-    esp_transfer_data();
     }
+}
+void ESP_IO_Handler(void) {
+    CycInt_AcknowledgeInterrupt();
     
-    intstatus = INTR_BS | INTR_FC;
-    seqstep = SEQ_CD;
+    switch (esp_io_state) {
+        case ESP_IO_STATE_TRANSFERING:
+            switch (SCSIbus.phase) {
+                case PHASE_DI:
+                    dma_esp_write_memory();
+                    if (esp_transfer_done(true)) {
+                        esp_io_state=ESP_IO_STATE_FLUSHING;
+                    }
+                    break;
+                case PHASE_DO:
+                    dma_esp_read_memory();
+                    if (esp_transfer_done(false)) {
+                        return;
+                    }
+                    break;
+                    
+                default:
+                    break;
+            }
+            break;
+        case ESP_IO_STATE_FLUSHING:
+            Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Transfer done: Flushing DMA buffer.");
+            dma_esp_write_memory();
+            return;
+            
+        default:
+            Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Transfer: Unkown state (%i).",esp_io_state);
+            return;
+    }
     
-    esp_raise_irq();   
+    CycInt_AddRelativeInterruptUs(100, 0, INTERRUPT_ESP_IO);
 }
 
 
-void esp_transfer_data(void) {
-    Log_Printf(LOG_SCSI_LEVEL, "transfer %d/%d\n", dma_left, data_len);
-    
-//    async_len = len;
-//    async_buf = scsi_req_get_buf(req);
-    if(dma_left) {
-        esp_do_dma();
-    } else if(dma_counter != 0 && data_len <= 0) {
-        esp_dma_done();
+/* Transfer padding */
+void esp_transfer_pad(void) {
+    Log_Printf(LOG_ESPCMD_LEVEL, "[ESP] Transfer padding, ESP counter: %i bytes, SCSI resid: %i bytes\n",
+               esp_counter, scsi_buffer.size);
+
+    switch (SCSIbus.phase) {
+        case PHASE_DI:
+            while (SCSIbus.phase==PHASE_DI && esp_counter>0) {
+                SCSIdisk_Send_Data();
+                esp_counter--;
+            }
+            esp_transfer_done(true);
+            break;
+        case PHASE_DO:
+            while (SCSIbus.phase==PHASE_DO && esp_counter>0) {
+                SCSIdisk_Receive_Data(0);
+                esp_counter--;
+            }
+            esp_transfer_done(false);
+            break;
+            
+        default:
+            abort();
+            break;
     }
 }
 
 
-void esp_do_dma(void) {
+/* Initiator command complete */
+void esp_initiator_command_complete(void) {
     
-    Log_Printf(LOG_SCSI_LEVEL, "call esp_do_dma\n");
-
-    Uint32 len;
-    int to_device;
-    
-    Uint32 dma_translen; // experimental
-    dma_translen = readtranscountl | (readtranscounth << 8); // experimental
-    Log_Printf(LOG_SCSI_LEVEL, "call dma_write\n"); // experimental
-    dma_memory_write(dma_write_buffer, dma_translen, CHANNEL_SCSI);//experimental !!
-
-    
-    to_device = SCSIcommand.transferdirection_todevice;
-    
-    len = dma_left;
-    
-    if(do_cmdvar){
+    if(mode_dma == 1) {
+        Log_Printf(LOG_WARN, "ESP initiator command complete via DMA not implemented!");
         abort();
-    }
-    if(async_len == 0) {
+    } else {
+        /* Receive status byte */
+        esp_fifo_write(SCSIdisk_Send_Status()); /* Disk sets phase to msg in after status send */
 
-    }
-    if(len > async_len) {
-        len = async_len;
-    }
-    if(to_device) {
-//        dma_memory_read(async_buf, len);
-    } else { 
-//        dma_memory_write(async_buf, len);
-    }
-    
-    dma_left -= len;
-    async_buf += len;
-    async_len -= len;
-    
-    if(to_device)
-        data_len += len;
-    else
-        data_len -= len;
-    
-    if(async_len == 0) {
-//        scsi_req_continue(current_req);
-        
-        if (to_device || dma_left != 0 || (fifo_write_ptr - fifo_read_ptr) == 0) {
-//            return;
+        if (SCSIbus.phase!=PHASE_MI) { /* Stop sequence if no phase change to msg in occured */
+            esp_command_clear();
+            intstatus = INTR_BS;
+            CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
+            return;
         }
+        
+        /* Receive message byte */
+        esp_fifo_write(SCSIdisk_Send_Message()); /* 0x00 = command complete */
     }
-    esp_dma_done();
+
+    intstatus = INTR_FC;
+    CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
 }
 
-void esp_dma_done(void) {
-    Log_Printf(LOG_SCSI_LEVEL, "call esp_dma_done\n");
-    status = (status&STAT_MASK)|STAT_ST;
-    status |= STAT_TC;
+
+/* Message accepted */
+void esp_message_accepted(void) {
+    SCSIbus.phase = PHASE_ST; /* set at the end of iccs? */
     intstatus = INTR_BS;
-    seqstep = 0;
-    fifoflags = 0;
-    readtranscountl = 0;
-    readtranscounth = 0;
+    esp_state = DISCONNECTED; /* CHECK: only disconnected if message was cmd complete? */
+    CycInt_AddRelativeInterruptUs(ESP_DELAY, 20, INTERRUPT_ESP);
 }
 
 
-// transfer information
-void handle_ti(void){
-    Uint32 dma_len, min_len;
-    
-    dma_len = readtranscountl | (readtranscounth << 8);
-    
-    if(dma_len == 0) {
-        dma_len = 0x10000;
-    }
-    
-    dma_counter = dma_len;
-    
-    if(do_cmdvar)
-        min_len = (dma_len < 32) ? dma_len : 32;
-    else
-        min_len = (dma_len > data_len) ? dma_len : data_len;
-    Log_Printf(LOG_SCSI_LEVEL, "Transfer Information len %d\n", min_len);
-    
-    if(mode_dma) {
-        dma_left = min_len;
-        status &= ~STAT_TC;
-        esp_do_dma();
-    }else if(do_cmdvar){
-        Log_Printf(LOG_SCSI_LEVEL, "Command len: %i\n",cmdlen);
-        data_len = 0;
-        cmdlen = 0;
-        do_cmdvar = 0;
-        do_cmd();
-        return;
-    }
-}
 
-void esp_command_complete (void) {
-    Log_Printf(LOG_SCSI_LEVEL, "SCSI Command complete\n");
-    if (data_len != 0)
-        Log_Printf(LOG_SCSI_LEVEL, "SCSI Command completed unexpectedly\n");
-    data_len = 0;
-    dma_left = 0;
-    async_len = 0;
-    
-//    if(status) {
-//        Log_Printf(LOG_SCSI_LEVEL, "Command failed\n");
-//    }
-//  status = status; return status, not status register!!
-    status = STAT_ST;
-    seqstep = SEQ_0;
-    if(mode_dma)    
-	esp_dma_done();
-}
-
-void write_response(void) {
-    Log_Printf(LOG_SCSI_LEVEL, "Transfer status: $%02x\n", SCSIcommand.returnCode);
-    esp_fifo[0] = SCSIcommand.returnCode; // status
-    esp_fifo[1] = 0x00; // message
-    
-    if(mode_dma == 1) {
-        dma_memory_write(esp_fifo, 2, CHANNEL_SCSI);
-        status = (status & STAT_MASK) | STAT_TC | STAT_ST;
-        intstatus = INTR_BS | INTR_FC;
-        seqstep = SEQ_CD;
-    } else {
-        status = (status & STAT_MASK) | STAT_ST;
-        fifo_read_ptr = 0;
-        fifo_write_ptr = 2;
-        fifoflags = (fifoflags & 0xE0) | 2;
+#if 0 /* this is for target commands! */
+/* Decode command to determine the command group and thus the
+ * length of the incoming command. Set "valid group code" bit
+ * in status register if the group is 0, 1, 5, 6, or 7 (group
+ * 2 is also valid on NCR53C90A).
+ */
+Uint8 scsi_command_group = (commandbuf[0] & 0xE0) >> 5;
+if(scsi_command_group < 3 || scsi_command_group > 4) {
+    if(ConfigureParams.System.nSCSI == NCR53C90 && scsi_command_group == 2) {
+        Log_Printf(LOG_WARN, "[ESP] Select: Invalid command group %i on NCR53C90\n", scsi_command_group);
+        status &= ~STAT_VGC;
+    } else {
+        status |= STAT_VGC;
     }
+} else {
+    Log_Printf(LOG_WARN, "[ESP] Select: Invalid command group %i on NCR53C90A\n", scsi_command_group);
+    status &= ~STAT_VGC;
 }
+#endif