              IF      IODRIVERINIT
FDRESTORE OKRTS
              FIN     IODRIVERINIT
              IF      IODRIVERRAM
*             S2000 Floppy Driver Working Storage

DISKINTFREE      FCB  1                "Floppy Disk is NOT busy flag"
DISKINTDCB       RMB  2                address of current DCB
DISKINTDRIVE     FCB  $FF              Last drive selected (initz'd to "NONE")
DISKINTBUFFER    RMB  2                Pointer to base of Sector buffer
DISKINTBYTECOUNT RMB  1                1 byte counter 0..256 (Yes, I know what I'm doing!)

DISKINTTIMEOUTBLOCK    SET     *
              FDB     NEXTTIMEOUT    timeout block for floppies
              FDB     0              fuse length
              FDB     DISKTIMEOUT
NTIMEOUTS     SET     NTIMEOUTS+1
NEXTTIMEOUT   SET     DISKINTTIMEOUTBLOCK
           PAGE
           INCLUDE S2000VFDDCBS.ASM
              FIN     IODRIVERRAM
              IF      IODRIVERBODY
 PAGE S2000 Floppy disk driver
*             PHYSICAL DISK DRIVERS STORAGE "DEFS"

::            SET     *
              ORG     DSKINFO:SIZE

*             TACKS ON TO END OF STANDARD OF DISK INFO TABLE

FDREADWRITE   RMB     1                 0 IS READ, <>0 IS WRITE
FDSEEKRETRY   RMB     1                 NUMBER OF RE-SEEKS
FDRETRY       RMB     1                 READ/WRITE RETRY COUNT
FDDRIVE       RMB     1                 DRIVE NUMBER TO USE
FDCYL         RMB     1                 what track we're on (-1 if lost)
FDTARGETCYL   RMB     1                 WHERE WE WANT TO BE
FDSECTOR      RMB     1                 GIMME THIS ONE
FDHEADCHAIN   RMB     2                 head of shared-head queue
FDNEXTCHAIN   RMB     2                 next on shared-head queue
fdprecomptrk  rmb     1                 first track where pre-comp req'd on writes
fdseekcmd     rmb     1                 1791 Seek command + step rate
fdrestorecmd  rmb     1                 1791 Restore command + step rate
fdnegnspt     rmb     1                 -nspt used in arcane arithmetic
FDSIZE        EQU     *
              ORG     ::
           PAGE
FDDRIVER   FDB     FDRESTORE
           FDB     FDREAD
           FDB     FDWRITE
           FDB     FDWAITDONE
           FDB     ILLDEVICEOP         FDSTATUS handled completely by SDOS1.1
           FDB     FDCONTROL

*          FDCONTROL -- CONTROL OPERATION ENTRY POINT FOR SECTOR I/O DRIVER

FDCONTROL  CMPA    #CC:DISMOUNTDISK      SINCE SDOS PASSES THIS THRU
           BEQ     FDDISMOUNT            B/ ITS A DISMOUNT!
           JMP     ILLDEVICEOP           NOT A LEGAL CONTROL CALL

*         FDREAD/WRITE -- START SINGLE SECTOR TRANSFER

FDWRITE   LDAA     #1
          BRA      FDREAD.1
FDREAD    CLRA
FDREAD.1  JSR      FDSETUPDRIVE       GO SET UP ALL THE PARAMETERS IN THE DCB
          LDX      #DISKINTFREE          see if controller is busy
          TST      0,X
          BNE      FDSTARTIO
          JSR      SDOS+SDOS:WAITEVENT
FDSTARTIO LDX      DCBPOINTER         NOW NOBODY'S USING DRIVE
          CLR      DCB:DONEFLAG,X     KICK INTERRUPT ROUTINE INTO MOTION
          STX      DISKINTDCB
          LDX      #DISKINTSTARTS20001791
          JSR      SDOS+SDOS:STARTIO
FDDISMOUNT,FDWAIT2   OKRTS

FDWAITDONE
          LDAA     DCB:DONEFLAG,X     IS IT DONE?
          BNE      FDWAIT1            B/ YES
          JSR      SDOS+SDOS:WAITEVENT
          LDX      DCBPOINTER
FDWAIT1   LDX      DCB:LASTERROR,X
          BEQ      FDWAIT2            B/ NO ERRORS
          JMP      ERRETX
          PAGE
*         FDSETUPDRIVE -- SETS UP FDDRIVE TABLE FOR INTERRUPT DRIVEN TRANSFER

FDSETUPDRIVE 
          LDX      DCBPOINTER
          STAA     FDREADWRITE,X      SAVE THE READ/WRITE FLAG
          LDAB     #32                --> 3 "RESTORE TO HOME" TRIES
          STAB     FDRETRY,X          SAVE THE READ/WRITE RETRY COUNT
          LDAA     #4                 SETUP SEEK RETRY COUNT
          STAA     FDSEEKRETRY,X
          CLR      DCB:LASTERROR,X    "NO ERRORS"
          CLR      DCB:LASTERROR+1,X
          PAGE
          LDX      DSKINFO:SECTORDB,X COMPUTE TARGET CYLINDER AND SECTOR
          LDAA     RDSI:LSN+1,X       GET LSN
          LDAB     RDSI:LSN+2,X
          LDX      DCBPOINTER         SO WE'RE TO POKE AT DCB AGAIN
          CLR      FDTARGETCYL,X      SHIFT 1ST QUOTIENT BIT INTO QUOTIENT(= 0)
          ASLB                        DOUBLE THE REMAINDER
          ROLA
          ASLB                        AND AGAIN
          ROLA
          ADDA     FDNEGNSPT,X        ADD -NSPT TO GENERATE QUOTIENT BIT
          BCS      *+4                B/ Q BIT IS ONE
          SUBA     FDNEGNSPT,X   RESTORE BY ADDING DIVISOR; LEAVE CARRY=0
          ROL      FDTARGETCYL,X      SHIFT QUOTIENT BIT INTO QUOTIENT
          ASLB                        DOUBLE THE REMAINDER
          ROLA
          ADDA     FDNEGNSPT,X        ADD -NSPT TO GENERATE QUOTIENT BIT
          BCS      *+4                B/ Q BIT IS ONE
          SUBA     FDNEGNSPT,X   RESTORE BY ADDING DIVISOR; LEAVE CARRY=0
          ROL      FDTARGETCYL,X      SHIFT QUOTIENT BIT INTO QUOTIENT
          ASLB                        DOUBLE THE REMAINDER
          ROLA
          ADDA     FDNEGNSPT,X        ADD -NSPT TO GENERATE QUOTIENT BIT
          BCS      *+4                B/ Q BIT IS ONE
          SUBA     FDNEGNSPT,X   RESTORE BY ADDING DIVISOR; LEAVE CARRY=0
          ROL      FDTARGETCYL,X      SHIFT QUOTIENT BIT INTO QUOTIENT
          ASLB                        DOUBLE THE REMAINDER
          ROLA
          ADDA     FDNEGNSPT,X        ADD -NSPT TO GENERATE QUOTIENT BIT
          BCS      *+4                B/ Q BIT IS ONE
          SUBA     FDNEGNSPT,X   RESTORE BY ADDING DIVISOR; LEAVE CARRY=0
          ROL      FDTARGETCYL,X      SHIFT QUOTIENT BIT INTO QUOTIENT
          ASLB                        DOUBLE THE REMAINDER
          ROLA
          ADDA     FDNEGNSPT,X        ADD -NSPT TO GENERATE QUOTIENT BIT
          BCS      *+4                B/ Q BIT IS ONE
          SUBA     FDNEGNSPT,X   RESTORE BY ADDING DIVISOR; LEAVE CARRY=0
          ROL      FDTARGETCYL,X      SHIFT QUOTIENT BIT INTO QUOTIENT
          ASLB                        DOUBLE THE REMAINDER
          ROLA
          ADDA     FDNEGNSPT,X        ADD -NSPT TO GENERATE QUOTIENT BIT
          BCS      *+4                B/ Q BIT IS ONE
          SUBA     FDNEGNSPT,X   RESTORE BY ADDING DIVISOR; LEAVE CARRY=0
          ROL      FDTARGETCYL,X      SHIFT QUOTIENT BIT INTO QUOTIENT
          ASLB                        DOUBLE THE REMAINDER
          ROLA
          ADDA     FDNEGNSPT,X        ADD -NSPT TO GENERATE QUOTIENT BIT
          BCS      *+4                B/ Q BIT IS ONE
          SUBA     FDNEGNSPT,X   RESTORE BY ADDING DIVISOR; LEAVE CARRY=0
          ROL      FDTARGETCYL,X      SHIFT QUOTIENT BIT INTO QUOTIENT
          STAA     FDSECTOR,X
          OKRTS
          PAGE       Series 2000 1791 Disk interrupt routines
*
*   Equates for Series 2000 1791 floppy disk controller
*
S2000WDSTATUS   EQU        $FC3F        WD 1791 Status Register
S2000WDCMD      EQU        $FC3F        WD 1791 Command Register
S2000WDTRACK    EQU        $FC3E        WD 1791 Track Editor
S2000WDSECTOR   EQU        $FC3D        WD 1791 Sector Register
S2000WDDATA     EQU        $FC3C        WD 1791 Data Register
S2000VIAORA     EQU        $FC40        VIA register controlling floppy

WDLATE          EQU        %00000100    1791 DATA LATE SENSE MASK
WDDRQ           EQU        %00000010    Data Request Bit sense mask
WDBUSY          EQU        %00000001    1791 Busy Flag Sense mask
        PAGE
DISKINTSTARTS20001791 ; Start I/O for Series 2000 1791 disk controller
; ASSERT: INTERRUPTS ARE DISABLED HERE!
; and in all routines labelled DISKINT...
        CLR    DISKINTFREE                mark controller busy
        LDX    DISKINTDCB
        LDAA   S2000VIAORA            Select desired unit
        ANDA   #%11110000             De-select old unit
        ORAA   FDDRIVE,X           Select new unit
        STAA   S2000VIAORA
        LDAA   #4000*2/(4+7+(2+5+28+5+4+4+5)+4+2+4+2+4) = delay longer than index hole time
*       Note: the summed numbers in the divisor are cycle counts...
*       of instructions executed in DISKINTWAITREADY
*       (the nested parentheses are cycle counts for DISKABORT)
*       The "4000" represents a 4 millisecond delay (nominal hole time)
*       and the 2 represents a 2Mhz computer
DISKINTWAITREADY ; Wait for Disk Ready (index hole not present)
        PSHA                       Save remaining index hole delay
        JSR    DISKABORT           get type I status
        PULA                       Restore remainding index hole delay
        BITB   #%00000010          Over index hole ?
        BEQ    SEEK                No, must be diskette in drive
        DECA                      Down count the delay
        BNE    DISKINTWAITREADY   B/ could still be index hole...
        LDAA   #ERR:DEVICENOTREADY/256
        LDAB   #ERR:DEVICENOTREADY&$FF
        JMP    DISKERROR1          go tell the world
*       Miscellaneous ideas: this routine could be invoked at seekdone...
*       to ensure that each attempt to read verifies presence of disk
*       Also, we could check for the presence of the "ready" bit...
*       to handle the case of new types of floppies being added to S2000
*       But, we'll save these features for a rainy day.
        PAGE
*       SEEK -- See if the head must be moved with a seek operation;
*       if it must, the seek is done without verification, as a seek
*       failure will be picked up by a subsequent read/write as
*       "record not found" status, for which the remedy will be
*       a restore operation.  The restore operation IS verified; if
*       it fails, a "seek error" is registered, and the restore is
*       retried, up to the seek-retry count.

SEEK ; See if Seek is needed
        LDAA   FDDRIVE,X               Get desired drive number
        LDAB   DISKINTDRIVE            Drive changed ?
        STAA   DISKINTDRIVE            (Save this drive as last drive selected)
        CBA                            ...?
        BNE    SEEKREQUIRED            B/ Yes, Seek is needed to verify on trak
        LDAA   FDCYL,X                 No, same cylinder?
        CMPA   FDTARGETCYL,X
        BEQ    SEEKDONE                B/ No seek needed
SEEKREQUIRED ; NO WAY TO AVOID SEEKING
        LDAA   FDCYL,X                 Where the heads currently are
        CMPA   #-1                     a seek is necessary: if FDCYL
        BEQ    SEEKHOMEJ1              contains a -1, then I am lost
        LDAB   FDTARGETCYL,X           Where the heads should be
        STAA   S2000WDTRACK            Tell 1791 where heads are
        STAB   S2000WDDATA             and where he should put them
        LDAA   FDSEEKCMD,X             Get seek command/step rate for drive
        JSR    DISKINTISSUECOMMAND     Tell 1791 what to do
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        LDAA   S2000WDSTATUS           Interrupt goes here, get disk status
        BITA   #WDBUSY                 Is 1791 done ?
        BNE    NOTDISKINTERRUPT        B/ No, must be some other device
        LDX    DISKINTDCB
        BITA   #%00011000              Seek or CRC error ?
        BNE    SEEKERR                 B/ Yes, go record it!
        LDAA   FDTARGETCYL,X           I assert that I am on the right track
        JSR    DISKSETCYLADD           (Well, I'll find out when I try to read)
SEEKDONE ; (X) contains DCB address
        JSR    DISKABORT               Kill disk, see if it is ready
        BPL    DISKREADWRITE           B/ Ready, go do read or write
        page
; Rumor has it that this code is unnecessary for Tandon drives,
; But Rumor did not say if it was needed for T & E drives!
DISKMAKEREADY ; Disk is not ready, make it ready
        LDAA   #%11010001              abort with interrupt on Ready transition
        JSR    DISKINTISSUECOMMAND
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        LDAA   S2000WDSTATUS           Interrupt occured, get disk status        BITA   #WDBUSY                 Is disk still busy?
        BITA   #WDBUSY                 Is disk still busy?
        BNE    NOTDISKINTERRUPT        B/ Yes, can't be disk!
        LDX    DISKINTDCB
DISKREADWRITE ; Now disk is Ready (Up to speed); Do the Read or Write
        LDX    DSKINFO:SECTORDB,X      put buffer page address in DISKINTBUFFER
        LDX    RDSI:SECTORBASE,X
        STX    DISKINTBUFFER
        LDX    DISKINTDCB 
        LDAA   FDSECTOR,X               Get the desired sector number
        STAA   S2000WDSECTOR            Tell 1791 what sector to search for
        LDAB   S2000VIAORA              Turn off Write Pre-comp, select side 0
        ANDB   #%10101111
        ORAB   #%10000000               Select Head Load Timeout
        CMPA   #17                      Now select side 1 if Sector # > 17
        BLS    DISKREADWRITE1           B/ Side zero
        ORAB   #%00010000               Side 1 desired
DISKREADWRITE1
        STAB   S2000VIAORA              Tell disk hardware; 100 Us. delay
        TST    FDREADWRITE,X            go off and do the read or write, as
        BNE    DISKWRITEJ               appropriate
        JMP    DISKREAD

NOTDISKINTERRUPT ; S2000 disk didn't interrupt us, check other devices
        JMP    INTERRUPTDEVICEPOLL      Go poll for normal device interrupt

SEEKHOMEJ1 JMP SEEKHOME
DISKWRITEJ JMP DISKWRITE
        PAGE
SEEKERR LDX    DISKINTDCB
        STAA   DSKINFO:SEEKERRSTS+1,X          count a seek error, and
        INC    DSKINFO:SEEKERRCNT+1,X             save the error status
        BNE    SEEK3ERR1
        INC    DSKINFO:SEEKERRCNT,X
SEEK3ERR1
        LDAA   #-1                     Say that I lost my place...
        BSR    DISKSETCYLADD
        DEC    FDSEEKRETRY,X           DOWN COUNT # TRIES LEFT
        BEQ    DISKSEEKERROR           B/ GAK! CROAK! DIE....
SEEKHOME
        BSR    DISKABORT               KILL WHATEVER DISK IS DOING
        LDAA   FDRESTORECMD,X          Grab restore command
        JSR    DISKINTISSUECOMMAND     Go do the restore
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        LDAA   S2000WDSTATUS           Interrupt occured, get disk status
        BITA   #WDBUSY                 Is disk still busy?
        BNE    NOTDISKINTERRUPT        B/ Yes, can't be disk!
        BITA   #%00000100              Check for arrival at cylinder zero
        BEQ    SEEKERR                 B/ DIDN'T GET TO CYL 0 FOR SOME REASON!?
        CLRA
        BSR    DISKSETCYLADD           SET "I'M AT CYLINDER 0"
        JMP    SEEK                    GO TRY SEEK TO PROPER TRACK AGAIN
        PAGE
DISKSEEKERROR
        LDAA   #ERR:DISKSEEK/256       GET APPROPRIATE ERROR CODE
        LDAB   #ERR:DISKSEEK&$FF
        BRA    DISKERROR1

DISKWPERR
        LDAA   #ERR:DSKWRTPROT/256
        LDAB   #ERR:DSKWRTPROT&$FF
        BRA    DISKERROR1

DISKERROR
        LDAA   #ERR:DISKREAD/256       ASSUME READ ERROR
        LDAB   #ERR:DISKREAD&$FF
        TST    FDREADWRITE,X           WAS IT A READ OR A WRITE?
        BEQ    DISKERROR1              B/ IT'S A READ
        LDAA   #ERR:DISKWRITE/256
        LDAB   #ERR:DISKWRITE&$FF
DISKERROR1
        LDX    DISKINTDCB              just to be safe...
        STAA   DCB:LASTERROR,X
        STAB   DCB:LASTERROR+1,X
DISKDONE
        LDX    DISKINTDCB              To handle entry when DiskRead/Write done
        INC    DCB:DONEFLAG,X          SIGNAL "DISK DONE"
        INC    DISKINTFREE                SO TASK KNOWS WE'RE FREE
DISKDONE1
*       LDAA   #%10001111              Deselect all drives by selecting #15
*       STAA   S2000VIAORA
*       Note: Rev C of S2000 hardware does this automatically on timeout
        LDX    #SDOS+SDOS:IOINT        MAKE INTERRUPT VECTOR GO TO POLLED INT
        STX    SYSPGIRQVECTOR+1
        JMP    SDOS+SDOS:RTI
        PAGE
DISKABORT
        LDAA   #%11010000              abort with no interrupts
        STAA   S2000WDCMD
WAIT28US ; WAIT 28 Us FOR STATUS TO BE VALID; THEN READ IT AND ACK INTERRUPT
        LDAA   #(28*2)//(2+4)          = # times around loop to make 28 Us.
        DECA                           2 cycles
        BNE    *-1                     4 cycles
        LDX    DISKINTDCB              In case (x) doesn't point to DCB
        LDAA   S2000WDDATA             To acknowledge any surprise interrupt
*                                      (i.e., Data Late just before Abort cmd)
        LDAB   S2000WDSTATUS           return with status in B
        RTS

DISKSETCYLADD ; Set Cylinder address of current DCB to (A)
        LDX    DISKINTDCB              Which DCB to update
        LDX    FDHEADCHAIN,X
DISKSETCYLADD.1
        STAA   FDCYL,X                 all DCB's sharing the same head
        LDX    FDNEXTCHAIN,X             mechanism are chained together
        BNE    DISKSETCYLADD.1             so that all FDCYL values will
        LDX    DISKINTDCB                    be equally correct
        RTS
        PAGE
DISKWRITE ; Sector number register is set, write a sector
        LDAA   S2000WDTRACK            check if write pre-compensation needed
        CMPA   FDPRECOMPTRK,X
        BLT    DISKWRITE.1
        ORAB   #$40                    Set Write Pre-Compensation
        STAB   S2000VIAORA             Tell the hardware
DISKWRITE.1
        LDAA   #256-2                  = # bytes transferred by loop below
        STAA   DISKINTBYTECOUNT
        JSR    DISKABORT               Get disk status
        BITB   #%01000000              IS DISK WRITE PROTECTED ?
        BNE    DISKWPERR               B/ Congrat, User is Prize Dummy!
        LDAA   #%10100000              "write sector" command
        JSR    DISKINTISSUECOMMAND     go tell the disk what to do
        LDAA   S2000WDSTATUS           Interrupt occured, get disk status
        CMPA   #WDDRQ+WDBUSY           Is it the data request we expect ?
        BNE    DISKWRITE.3             B/ Not yet, go check for something else
        LDX    DISKINTBUFFER           Yes, get the first data byte fast!
        LDAB   0,X
        STAB   S2000WDDATA             Give it to 1791 chip
        LDX    #DISKWRITE.2            Set up for DRQ int for 2nd sector byte
        STX    SYSPGIRQVECTOR+1        since 900 Us will pass first
        RTI                            This interrupt has been handled!
DISKWRITE.2 ; Take interrupt for 2nd byte of sector
        LDAA   S2000WDSTATUS           This is interrupt for sector byte # 2
        CMPA   #WDDRQ+WDBUSY           Is it the data request that we expect?
        BNE    DISKWRITE.3             B/ No, its something else!
        LDX    DISKINTBUFFER           Yes, get pointer to sector buffer
        LDAB   1,X                     Get 2nd byte of buffer
        STAB   S2000WDDATA             And give it to 1791
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    DISKWRITEBYTEWAITDRQ    B/ DON'T HAVE TO SWITCH STACKS
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        LDX    DISKINTBUFFER           SWITCHED STACKS, RELOAD (X)
        PAGE
DISKWRITEBYTEWAITDRQ ; Wait for next data request to occur (within 32 Us.)
        LDAB   #(32*2)//(4+4+2+4)      ; Assert: (A) is WDDRQ+WDBUSY
DISKWRITEBYTEWAITLOOP ; Wait for next data request
        CMPA   S2000WDSTATUS           Has data request occurred?
        BEQ    DISKWRITEBYTE           B/ 1791 Wants another byte
        DECB                           Check for timeout
        BNE    DISKWRITEBYTEWAITLOOP   B/ Not yet.
        LDAA   S2000WDSTATUS           Take snapshot of status register NOW
        CPX    DISKINTBUFFER           Fail on 3rd byte?
        BNE    DISKWRITEFAILED         B/ No.
        CMPA   #WDLATE+WDDRQ+WDBUSY    Data late on 3rd byte?
        BEQ    DISKDATALATEJ           B/ Yes, don't record as error status
        BRA    DISKWRITEFAILED         ??? No data request within window!

DISKWRITEBYTE ; 1791 has issued another Data Request
        LDAB   2,X                     Fetch next byte to give 1791
        STAB   S2000WDDATA             and ram it down its throat!
        INX                            Advance the buffer pointer
        DEC    DISKINTBYTECOUNT        decrement number of bytes left to give
        BNE    DISKWRITEBYTEWAITDRQ    go wait for next request
*    Last DRQ occurs 32 Us. before last byte must be written.
*    WD1791 must write current byte, last byte, 2 CRC bytes,
*    and last, but not least, a byte of 1 bits before done
*    and then 28 Us. must elapse before status is valid.
*    So total delay after last DRQ is 5*32Us.+28Us.
        LDAB   #((5*32+28)*2)//6       Wait for 1791 status of write to settle
        DECB
        BNE    *-1                     Whee!!! This is fun (I think)
        LDAA   S2000WDSTATUS           Get copy of status register
        BEQ    DISKVERIFY              B/ Normal write status
DISKWRITEFAILED ; 1791 failed to respond in expected way on disk write
        PSHA                           Save error status
        JSR    DISKABORT               Kill off whatever the disk is doing
        PULA
DISKWRITE4J
        JMP    DISKWRITE4              And go save snapshot of disk status
        PAGE
DISKWRITE.3 ; Interrupt occurred, and it isn't just DRQ from 1791
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        BITA   #WDLATE                 Data late error?
        BNE    DISKDATALATEJ           B/ Yes, don't record as error status
        BITA   #WDBUSY                 Is 1791 still busy ?
        BNE    NOTDISKINTERRUPTJ2      B/ Yes, must be some other device
        TSTA                           Is disk ready ?
        BPL    DISKWRITE4J             B/ Yes, Status indicates what is wrong
        JMP    SEEKDONE                No, go make disk ready (again?)

NOTDISKINTERRUPTJ2 JMP INTERRUPTDEVICEPOLL

DISKDATALATEJ JMP DISKDATALATE
        PAGE
DISKVERIFY ; 1791 Sector register already set up, do sector verify
        JSR    DISKRVCOMMON    Do stuff common to Read/verify
        JSR    DISKINTISSUECOMMAND     Go make the disk do it
        LDAA   S2000WDSTATUS           Interrupts come here, is it the 1791?
        CMPA   #WDDRQ+WDBUSY           (Data request and still busy ?)
        BNE    DISKVERIFY.3            B/ No, check for other conditions
        LDAB   S2000WDDATA             Get 1st data byte
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        LDX    DISKINTBUFFER           Compare with 1st byte of sector buffer
        CMPB   0,X
        BNE    DISKVERIFYMATCHFAIL     B/ First byte doesn't match!
DISKVERIFYBYTEWAITDRQ ; Wait for next Data request to occur (within 32 Us.)
; Assert: (A) = WDDRQ+WDBUSY
        LDAB   #(32*2)//(4+4+2+4)      # times around wait loop before timeout
DISKVERIFYBYTEWAITLOOP ; Wait as hard as we can!
        CMPA   S2000WDSTATUS           Another data byte arrive ?
        BEQ    DISKVERIFYBYTE          B/ Yes
        DECB                           Timed out?
        BNE    DISKVERIFYBYTEWAITLOOP  B/ Not yet
        LDAA   S2000WDSTATUS           1791 Timed out, get status
        CPX    DISKINTBUFFER           Fail on 2nd byte?
        BNE    DISKWRITEFAILED         B/ No, record failure status
        CMPA   #WDLATE+WDDRQ+WDBUSY    Late on 2nd byte?
        BNE    DISKWRITEFAILED         B/ No, This attempt to write failed
DISKVERIFYDATALATE ; Data is late on 1st or 2nd byte of Verify read
        JSR    DISKABORT               Stop whatever the 1791 is doing
        DEC    FDRETRY,X               Remember we shot another try
        BEQ    DISKERRORJ              B/ End of the line, time to give up.
        BRA    DISKVERIFY              No, try to verify without data late

DISKVERIFYMATCHFAIL ; Bytes written to disk don't match memory!!
        LDAA   #%00101001              Set Verify Status = "CRC err"+"Busy"
        BRA    DISKWRITEFAILED         and go record it

******* NOTE: ARTIFICIAL STATUS OF $29 IS A PROPERTY OF THIS DRIVER,
******* NOT A PROPERTY OF THE WD1791!

*       A few notes here...
*       Apparantly, the 1791 is prone to forgetting a Data Late if the CPU
*       hands it a byte very, very shortly after the data would be too late.
*       In this circumstance, the 1791 substitutes a zero byte for the 1st
*       byte of the data sector on a write, but no error is detectable by
*       the CPU.  This verify routine then reads back the data, and discovers
*       a mismatch on the first byte, and signals $29 error status.
*       MAKING THIS $29 ERROR STATUS GO AWAY WOULD PROBABLY MAKE THE BAD WRITE
*       UNDETECTABLE AND THEREFORE DEADLY, so we just live with it.
*       These timing splinters are due to the latency of I/O interrupts
*       being used to drive the programmed I/O disk transfers of the S2000.
        PAGE
DISKVERIFYBYTE ; Another Data request has occurred
        LDAB   S2000WDDATA             Grab the data byte
        CMPB   1,X                     No, check to make sure byte matches
        BNE    DISKVERIFYMATCHFAIL     B/ Bytes don't Match
        INX
        DEC    DISKINTBYTECOUNT        256th byte compared?
        BNE    DISKVERIFYBYTEWAITDRQ   B/ No, wait for next byte
*   After last DRQ, WD1791 must read 2 more CRC bytes
*   Then we must wait 28 Us. more before WD1791 Status is valid
*   So total wait must be 2*32 Us. + 28 Us.
        LDAB   #((2*32+28)*2)//6       Wait for Status to settle
        DECB
        BNE    *-1
        LDAA   S2000WDSTATUS           Take snapshot of status register
        BNE    DISKWRITEFAILEDJ        B/ A read error occurred
        JMP    DISKDONE                Successful Write

DISKWRITEFAILEDJ JMP DISKWRITEFAILED

DISKVERIFY.3 ; Not interrupt due to data request
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        BITA   #WDLATE                 A data late error ?
        BNE    DISKVERIFYDATALATE      B/ Yes, don't count this as an error!
        BITA   #WDBUSY                 Is 1791 still busy ?
        BNE    NOTDISKINTERRUPTJ4      B/ yes, must be some other device
DISKWRITE4
        LDX    DISKINTDCB              In case of entry from Read/Write failed
        STAA   DSKINFO:WRITEERRSTS+1,X SAVE WRITE ERROR STATUS
        INC    DSKINFO:WRITEERRCNT+1,X COUNT # WRITE ERRORS
        BNE    DISKWRITE5
        INC    DSKINFO:WRITEERRCNT,X
DISKWRITE5
        LDX    DSKINFO:SECTORDB,X      SAVE COPY OF ERRORING LSN
        LDAA   RDSI:LSN,X              FETCH ERRORING LSN...
        PSHA
        LDAA   RDSI:LSN+1,X
        LDAB   RDSI:LSN+2,X
        LDX    DISKINTDCB              AND SAVE FOR ERROR STATUS
        STAA   DSKINFO:ERRLSN+1,X
        STAB   DSKINFO:ERRLSN+2,X
        PULA
        STAA   DSKINFO:ERRLSN,X
        DEC    FDRETRY,X
        BEQ    DISKERRORJ              B/ NO MORE TRIES LEFT
        LDAB   FDRETRY,X               MULTIPLE OF 8 TRIES LEFT?
        BITB   #%111                   ....?
        BNE    SEEKDONEJ               B/ No, just try it again.
        JMP    SEEKHOME                GO SEE IF RE-SEEK HELPS

SEEKDONEJ  JMP   SEEKDONE

DISKERRORJ
        JMP    DISKERROR

NOTDISKINTERRUPTJ4 JMP INTERRUPTDEVICEPOLL
        PAGE
DISKRVCOMMON
        LDAA   #256-1                  Set up counter to xfer bytes in loop
        STAA   DISKINTBYTECOUNT        and so error detected if >256 moved
        LDAA   #%10000000              "read sector" command
        RTS

DISKDATALATE ; Disk driver did not get/give data from/to WD1791 fast enough
*   because the CPU was not ready to take an interrupt when DRQ was raised
*   Since the driver hangs in a tight loop to transfer all but the first bytes,
*   this can only occur for the first few bytes of a sector, and is normal
*   Therefore, this class of error is not recorded in the Error Statistics;
*   But to prevent the driver from "going away" forever, this DOES
*   cause the Retry counter to be decremented.
*   **** THIS CAN CAUSE DISK WRITE ERROR WITH NO ERROR STATUS ***
        JSR    DISKABORT               We detect this error while 1791 is busy
        BRA    DISKWRITE5              Go decrement retry counter.
        PAGE
DISKREAD ; 1791 Sector register already set up, do sector read
        BSR    DISKRVCOMMON    Go do stuff common to disk Read/Verify
        JSR    DISKINTISSUECOMMAND     Go make the disk do it
        LDAA   S2000WDSTATUS           Interrupts come here, is it the 1791?
        CMPA   #WDDRQ+WDBUSY           (Data request and still busy ?)
        BNE    DISKREAD.3              B/ No, check for other conditions
        LDAB   S2000WDDATA             Get 1st data byte
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        LDX    DISKINTBUFFER           Store it into sector buffer
        STAB   0,X                     save the data byte in sector buffer
DISKREADBYTEWAITDRQ ; Wait for next Data request to occur (within 32 Us.)
; Assert: (A) = WDDRQ+WDBUSY
        LDAB   #(32*2)//(4+4+2+4)      # times around wait loop before timeout
DISKREADBYTEWAITLOOP ; Wait as hard as we can!
        CMPA   S2000WDSTATUS           Another data byte arrive ?
        BEQ    DISKREADBYTE            B/ Yes
        DECB                           Timed out?
        BNE    DISKREADBYTEWAITLOOP    B/ Not yet
        LDAA   S2000WDSTATUS           1791 Timed out, get status
        CPX    DISKINTBUFFER           Fail on 2nd byte?
        BNE    DISKREADFAILED          B/ No, record failure status
        CMPA   #WDLATE+WDDRQ+WDBUSY    Late on 2nd byte?
        BEQ    DISKDATALATE            B/ Yes, don't record in error status
DISKREADFAILED ; The read attempt has failed disasterously, kill the read
        PSHA                           Save funny status
        JSR    DISKABORT               Kill whatever the disk is doing
        PULA                           Get crazy status back.
DISKREAD4
        LDX    DISKINTDCB              In case of entry from Read failed
        STAA   DSKINFO:READERRSTS+1,X  SAVE READ ERROR STATUS
        INC    DSKINFO:READERRCNT+1,X  COUNT # READ ERRORS
        BNE    DISKWRITE5
        INC    DSKINFO:READERRCNT,X
DISKWRITE5J
        BRA    DISKWRITE5              GO CHECK RETRY COUNT

DISKREADBYTE ; Another Data request has occurred
        LDAB   S2000WDDATA             Grab the data byte
        STAB   1,X                     save the data byte in sector buffer
        INX
        DEC    DISKINTBYTECOUNT        Last byte just read?
        BNE    DISKREADBYTEWAITDRQ     B/ No, go wait for another
*   After last DRQ, WD1791 must read 2 more CRC bytes
*   Then we must wait 28 Us. more before WD1791 Status is valid
*   So total wait must be 2*32 Us. + 28 Us.
        LDAB   #((2*32+28)*2)//6       Wait for Status to settle
        DECB
        BNE    *-1
        LDAA   S2000WDSTATUS           Take snapshot of status register
        BNE    DISKREADFAILED          B/ A read error occurred
        JMP    DISKDONE                Successful Read

DISKREAD.3 ; Not interrupt due to data request
        INC    SDOS+SDOS:STACKSWITCHED Switch to interrupt stack
        BNE    *+10                    Ooooh... I hope I counted right
        LDX    SDOS+SDOS:CURRENTASK
        STS    TCB:STACK,X
        LDS    #INTERRUPTSTACKEND-1
        BITA   #WDLATE                 Is data simply late?
        BNE    DISKDATALATE            B/ Yes, don't record as error status
        BITA   #WDBUSY                 Is 1791 still busy ?
        BEQ    DISKREAD4               There is something wrong with the read
        JMP    INTERRUPTDEVICEPOLL
        PAGE
DISKINTISSUECOMMAND ; (A) contains 1791 command
; Return address points to new place that interrupt should be aimed
        STAA   S2000WDCMD              Tell the 1791 to stick this up his...
        LDX    #6*TICKSPERSECOND+NTIMEOUTBLOCKS Set up time-out on operation
        STX    DISKINTTIMEOUTBLOCK+TIMEOUT:FUSE
COUNTCOMMAND ; COMMAND HAS BEEN ISSUED, COUNT IT IN DCB
        LDX    DISKINTDCB              Because DCB ptr sometimes not in (X)
        INC    DSKINFO:OPSCOUNT+2,X    COUNT # OPERATIONS ISSUED TO FLOPPY
        BNE    COUNTCOMMAND1
        INC    DSKINFO:OPSCOUNT+1,X
        BNE    COUNTCOMMAND1
        INC    DSKINFO:OPSCOUNT,X
COUNTCOMMAND1
        PULA                            Re-aim interrupt vector to return addr
        PULB
        STAA   SYSPGIRQVECTOR+1
        STAB   SYSPGIRQVECTOR+2
*
*       Note: no interrupt poll chain code is necessary...
*       due to dynamic linking of floppy disk to very front of poll chain
*
DISKTIMEOUT2
        JMP    SDOS+SDOS:RTI

DISKTIMEOUT
        LDX    DISKINTDCB              point at DCB, again
        LDAA   DCB:DONEFLAG,X
        BNE    DISKTIMEOUT2            B/ DISK IS DONE, GO AWAY
        JSR    DISKABORT               Kill any disk activity
        LDAA   #-1                     Timed out --> Head location is unknown!
        JSR    DISKSETCYLADD
        LDAA   #ERR:DEVICETIMEDOUT/256
        LDAB   #ERR:DEVICETIMEDOUT&$FF
        STAB   DISKINTDRIVE            FORCE SEEK W/VERIFY ON NEXT READ/WRITE
        JMP    DISKERROR1
        FIN    IODRIVERBODY
