                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Module: Srvo1.b1
;>
;>      This module contains all the specifications and source code for 
;>      controlling the Widget Servo Board ( i.e., communication
;>      protocol, seek and head positioning, spare table lookup, etc. )
;>
;>      PROCEDURE Seek( Cylinder : WORD { !!rC }
;>                      Head : BYTE { !rE }
;>                      Sector : BYTE { !rF }
;>                    )
;>      PROCEDURE Wait_xMs( x : BYTE { !r1 }
;>      FUNCTION PosHeads( Wait : BOOLEAN { !r8/bit 6 }
;>                         Parent : BYTE { !rB }
;>                         Cylinder : WORD { !!rC }
;>                         Head : BYTE { !rE }
;>                         Sector : BYTE { !rF }
;>                       ) : BOOLEAN
;>      PROCEDURE SelectHead( Head : BYTE { !rE } )
;>      FUNCTION CalcMagDir( Cylinder : WORD { !!rC } ) :
;>                         BOOLEAN
;>                         Magnitude : WORD { !r5 }
;>                         Direction : BYTE { !r9 }
;>      FUNCTION FindDistance( Cylinder : WORD { !!rC } ) : BOOLEAN
;>      FUNCTION ServoOk : BOOLEAN
;>                         RecalMagDir : BYTE { !r0 }
;>      PROCEDURE SelectHead( Head : BYTE { !rE }
;>      PROCEDURE Auto_Offset
;>      PROCEDURE Chk_Offset
;>
;>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Procedure: Seek
;>
;>      This procedure accepts cylinder, head, and sector values
;>      and then calls position heads.
;>
;>      Inputs:
;>              Cylinder: WORD { !!rC }
;>              Head    : BYTE { !rE }
;>              Sector  : BYTE { !rF }
;>
;>      Outputs: { none }
;>
;>      Global Varibles Changed:
;>              Cylinder, Head, Sector
;>
;>      Algorithm:
;>
;>      BEGIN
;>       LastSeekAddress := CurrentSeekAddress
;>       DiskStat.Parked := False
;>       IF DiskStat.SeekComplete
;>        THEN
;>             i := 4
;>             WHILE ( i >       0 ) AND NOT( PositionHeads( Wait,
;>                                      Dmt_Seek, Cylinder, Head, Sector ) DO
;>
;>              IF NOT( Recovery ) THEN Abort
;>              i := i -1
;>      
;>             IF ( i = 0 )
;>              THEN Abort
;>         ELSE Wait up to 1 sec for ServoReady, if timeout Abort
;>        LastCylinder := GlobalCylinder
;>        LastHead     := GlobalHead
;>        LastSector   := GlobalSector
;>        GlobalCylinder := Cylinder
;>        GlobalHead     := Head
;>        GlobalSector   := Sector
;>        SectorCount := SectorCount + 1
;>        DiskStat.SeekComplete := True
;>        DiskStat.OnTrack := True
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

Seek:
                Call    Ext_Push
                
                Ld      Lst_HiCyl,Cylinder ;save last seek address
                Ld      Lst_LoCyl,Cylinder+1
                Ld      Lst_Head,Head
                Ld      Lst_Sector,Sector
                And     DiskStat,#$FF-Parked
                
                Tm      DiskStat,#SeekComplete ;check for head/arm motion
                Jr      Z,Seek_Wait
                
Seek_ReTry:     Ld      !r1,#4
                And     DiskStat,#$FF-Long_Seek
                
Seek_Lp:         Ld     !r8,#Wait
                Call    PosHeads
                Jr      Nz,Seek_End
                
                Tm      Excpt_Stat,#Recovery
                Jr      Z,Seek_Abt
                
                Push    !r1 ;save counter
                 Ld     !r2,#.HIBYTE. 50 ;wait 200 ms before retrying
                 Ld     !r3,#.LOWBYTE. 50
                Call    MsWait
                Pop     !r1 ;get counter back
                
                Djnz    !r1,Seek_Lp
                
Seek_Abt:        Ld     !r0,#0 ;byte 0
                 Ld     !r1,#Stat_Srvo
                Call    SetStatus
                Call    Abort
                
Seek_Wait:      Call    Wait_For_Rdy
                Jr      Z,Seek_Lp
                
Seek_End:       Cp      Seek_Type,#Access_Offset ;check for offset needed
                Jr      Nz,Sk_End_1
                
                Or      DiskStat,#Offset_On

Sk_End_1:       Or      DiskStat,#On_Track+Seek_Complete
                Ld      Cylinder,!rC
                Ld      Cylinder+1,!rD
                Ld      Head,!rE
                Ld      Sector,!rF
                Incw    SeekCount
                
                Call    SelectHead
                Call    Chk_Offset
                
Seek_Ret:       Call    Ext_Pop
                
                Jp      Bank_Ret

;*******************************

Wait_For_Rdy:   Push    !r1
                Ld      !r1,#100 ;wait for up to a second
Seek_Wt:        And     Irq,#$FF-Timer1 ;clear old timer interrupts

Seek_Wt_Lp:     Tm      Port2,#SioRdy ;wait for the servo to come back
                Jr      Z,WtRdy_Tmr
                
                Call    Load_Status ;check for ServoReady
                Tm      !r0,#ServoRdy
                Jr      Nz,WtRdy_Ret

WtRdy_Tmr:      Tm      Irq,#Timer1 ;check for timer interrupt
                Jr      Z,Seek_Wt_Lp
                Dec     !r1
                Jr      Nz,Seek_Wt
                
WtRdy_Ret:      Pop     !r1
                Ret

                .LSTOFF
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Procedure:      Wait_xMs
;>
;>      Wait for x milliseconds, where x is the number {up to 256}
;>      of ms to wait.
;>
;>      Input:
;>              x : BYTE { !r1 }
;>
;>      Output: none
;>
;>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

Wait_xMs:       Ld      !r0,#152        ;loop for 1 ms
Wait_1Lp:       Nop
                Nop
                Djnz    !r0,Wait_1Lp    ;inner loop
                
                Djnz    !r1,Wait_xMs    ;outer loop
                Ret
                
                .LSTOFF
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Function: PosHeads  { Position Heads }
;>
;>      This function is responsible for positioning the heads of
;>      the drive; by supplying the cylinder and head value of
;>      the desired track. PosHeads can be used in one of two modes:
;>      1) as an overlapped function where the heads can be in motion
;>      while the functions returns back to the user or 2) the function
;>      will wait for the heads to settle before returning to the caller.
;>
;>      Inputs:
;>              Wait        : BOOLEAN { !r8/bit 6 }
;>              Cylinder    : WORD { !!rC }
;>              Head        : BYTE { !rE }
;>
;>      Outputs:
;>              PositionHeads : BOOLEAN { zero flag is set if servo err }
;>
;>      Global Variables Changed: Head, Cur_Cyl
;>
;>      Algorithm:
;>
;>      BEGIN
;>       DiskStat.Offset_On := False
;>       SelectHead( Head )
;>       ServoOk
;>       CalcMagnitudeDirection( Cylinder, Magnitude, Direction )
;>       DiskStat.SeekComplete := False
;>       DiskStat.On_Track := False
;>       SetDeadManTimer
;>       ServorStore( SeekType+Direction+
;>                    Magnitude[ 1 ], Magnitude[ 2 ],
;>                    AutoOffset, 0 )
;>       ClearDeadManTimer
;>       Cur_Cyl := LocalCylinder
;>       IF ( SeekType = Access_Offset ) THEN DiskStat.Offset_On := True
;>       PositionHeads := NOT( LoadStatus.ServoErr )
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

PosHeads:
                Call    Ext_Push ;save callers variables
                
                And     DiskStat,#$FF-Offset_On
                
                 Ld     !rE,#1 ;always select Head 1 before seeking
                Call    SelectHead
                
PosHds_Rpt:     Call    ServoOk ;test if Servo is in a reasonable state
                Jr      Z,PosHds_6
                
                Call    CalcMagDir
                
PosHds_While:   And    DiskStatus,#$FF-SeekComplete-On_Track
                Call    Set_Dmt
                 Srp    #Wrk_Scr
                 Ld     !r0,Seek_Type
                 Or     !r0,Wrk_Sys+$09 ; Plus Direction
                 Or     !r0,Wrk_Sys+$05 ; Plus Ms Magnitude
                 Ld     !r1,Wrk_Sys+$06 ; Ls Magnitude
                 Ld     !r2,#Off_Auto
                 Ld     !r3,#S_Rate_57_6
                 Srp    #Wrk_Sys
                Call    ServoCmnd
                Di                      ;Clear Dead_Man_Timer
                
PosHds_4:       Ld      !rE,#.HIBYTE. 25000 ;intermidiate timer of 1sec
                Ld      !rF,#.LOWBYTE. 25000
PosHds_42:      Call    LoadStatus      ;sample ServoError
                Tm      !r0,#ServoErr
                Jr      Nz,PosHds_5      ;if servo error then exit
                
PosHds_3:       Tm      !r8,#Wait       ;IF Wait AND NOT( ServoRdy )
                Jr      Z,PosHds_5
                
                Decw    !!rE
                Jr      Z,PosHds_5
                
                Tm      !r0,#ServoRdy   ; THEN loop until timeout OR ServoRdy
                Jr      Z,PosHds_42

                Or      DiskStat,#SeekComplete

PosHds_5:       Ld      Cur_Cyl,!rC
                Ld      Cur_Cyl+1,!rD

PosHds_6:       Call    Ext_Pop ;get caller's variables back
                Call    LoadStatus
                Tcm     !r0,#ServoErr   ;return Z = true if servo error
                Jp      Bank_Ret
                
                .LSTOFF
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Procedure: SelectHead
;>
;>      This procedure is responsible for selecting the correct head
;>      on the disk.
;>
;>      Inputs:
;>              Head : BYTE { !rE }
;>
;>      Outputs: { none }
;>
;>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

SelectHead:     Or      !rE,!rE ;test for Head0 or Head1
                Jr      Z,Sel_Head0
                Or      Port3,#Hs0       ;select head 1
                Jr      SelHd_End
        
Sel_Head0:      And     Port3,#$FF-Hs0   ;select head 0
SelHd_End:      Ld      Head,!rE

SelHd_Ret:      Ret
                
                .LSTOFF
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Function: CalcMagDir  { Calculate Magnitude and Direction }
;>
;>      This function takes the current cylinder number ( from the
;>      conversion of the logical block ) and generates a magnitude
;>      and direction from the the position that the heads are 
;>      currently at ( the servo needs to know a RELATIVE distance,
;>      not an absolute distance ).
;>
;>      Inputs:
;>              LocalCylinder : WORD { !rC, !rD }
;>
;>      Outputs:
;>              CalcMagDir : BOOLEAN { !r4 }
;>              Magnitude : WORD { !r5, !r6 }
;>              Direction : BYTE { !r9 }
;>
;>      Local Variables :
;>              Temp : BOOLEAN { }
;>              GlobalPTR : PTR { !r2, !r3 }
;>
;>      Algorithm:
;>
;>      BEGIN
;>       IF LocalCylinder <> GlobalCylinder
;>        THEN
;>              IF LocalCylinder > GlobalCylinder
;>               THEN
;>                    Direction := Positive
;>                    Magnitude := LocalCylinder - GlobalCylinder
;>               ELSE
;>                    Direction := Negative
;>                    Magnitude := GlobalCylinder - LocalCylinder
;>              Temp := True
;>        ELSE
;>              Temp := False
;>       CalcMagDir := Temp
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

CalcMagDir:
                Ld      !r1,Cur_Cyl+1 ;get distance to seek
                Ld      !r0,Cur_Cyl
                Sub     !r1,!rD
                Sbc     !r0,!rC
                Clr     !r4     ;assume no seek needed
                
                Jr      Lt,C_MagDir_Else
                
                Clr     !r5     ;assume 0 track seek
                Clr     !r6
                Ld      !r9,#0 ;set direction negative
                
                Clr     !r3     ;check for no seek condition
                Or      !r3,!r1
                Or      !r3,!r0
                Jr      Z,C_MagDir_Ret
                Jr      S_Glbl_Cyl
                
C_MagDir_Else:  Com     !r1     ;2's complement sutraction result
                Com     !r0
                Add     !r1,#1
                Adc     !r0,#0
                
                Ld      !r9,#Hd_Dir_Frwd     ;set direction positive
                
S_Glbl_Cyl:     Ld      !r5,!r0 ;load magnitude
                Ld      !r6,!r1
                
                Ld      !r4,#01 ;set Seek
C_MagDir_Ret:   Or      !r4,!r4 ;set SeekNeeded flag
                Jp      Bank_Ret

                .LSTOFF
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Function: ServoOk
;>
;>      This function is responsible for determining if the
;>      servo is in a reasonable state to perform commands. In 
;>      other words if ServoReady and NOT( ServoError ) then
;>      the servo is healthy, wealthy and wise. If, on the other
;>      hand, ServoError is active then the state of ServoReady
;>      determines the type of action to perform to try to get
;>      the servo back to a nice state:
;>
;>      ServoError and ServoReady: Read Status
;>      ServorError and Not( ServoReady ): Data Recal
;>
;>      Inputs: { none }
;>
;>      Outputs:
;>              ServOk: BOOLEAN { Zero Flag, True if NOT( ServoOk ) }
;>
;>      Algorithm:
;>
;>      BEGIN
;>       CASE ServoStatus OF
;>
;>        ServoError:
;>              IF Write_Operation THEN WrBuf_To_BUf2
;>              Restore( DataRecal )
;>              Buf2_to_WrBuf
;>
;>        NOT( ServoRdy ) AND NOT( ServoError ):
;>              IF Write_Operation THEN WrBuf_To_BUf2
;>              ResetServo
;>              Buf2_to_WrBuf
;>
;>       ServoOk := NOT( ServoError )
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

ServoOk:
                Tm      Excpt_Status,#Recovery         ;IF Recovery
                Jr      Z,S_Ok_End
                
                Call    LoadStatus
                
                Tm      !r0,#ServoErr ;IF ServoError
                Jr      Z,SOk_ChkRdy

SOk_Err:        Call    Chk_MvWrData    ;save write data if Write_Op
                 Ld     !r0,#DataRecal ;IF ServoErr AND NOT( ServoRdy )
                Call    Restore        ; THEN Restore
                Call    Buf2_To_WrBuf   ;assume write_op
                Jr      S_Ok_End

SOk_ChkRdy:     Tm      !r0,#ServoRdy ;IF NOT( ServoErr )
                Jr      Nz,S_Ok_End   ; AND ServoRdy
                
SOk_Park:       Call    Chk_MvWrData    ;save write data if Write_Op
                Call    S_Reset       ;OtherWise unpark the servo
                Call    Buf2_To_WrBuf   ;assume write_op
                Jr      S_Ok_End

S_Ok_End:       Call    LoadStatus
                Tcm     !r0,#ServoErr   ;return z = true if servo error
                Jp      Bank_Ret
                
;********************

Chk_MvWrData:   Tm      DiskStat,#Wr_Op ;check for write_op
                Jr      Z,MvWr_End
                
                Call    WrBuf_To_Buf2
MvWr_End:       Ret
                
                .LSTOFF
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Procedure: Auto_Offset
;>
;>      This procedure is used to set auto_offset to the servo processor.
;>
;>      Inputs: { none }
;>
;>      Outputs: { none }
;>
;>      Algorithm:
;>
;>      BEGIN
;>       ServoCmnd( Offset, 0, Off_Auto, S_Rate_57_6 )
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

Auto_Offset:
                Push    Head   ;save current Head value
                 Ld     !rE,#1 ;select Head 1 before offsetting
                Call    SelectHead
                
                 Ld     !r1,#1  ;add 1 ms of head settling
                Call    Wait_xMs
                
                Call    Set_Dmt ;set deadman timer
                
                 Srp    #Wrk_Scr
                 Ld     !r0,#Access_Offset
                 Ld     !r1,#0
                 Ld     !r2,#Off_Auto
                 Ld     !r3,#S_Rate_57_6
                 Srp    #Wrk_Sys
                Call    ServoCmnd
                
                Call    Clr_Dmt
                Call    Wait_For_Rdy
                Jr      Z,Auto_Off_Ret
               
                Or      Disk_Stat,#Offset_On
Auto_Off_Ret:   Pop     !rE             ;reselect old Head value
                Call    SelectHead
                Jp      Bank_Ret


;******************

Chk_Offset:     Cp      Seek_Type,#Access_Offset ;check if auto offset needed
                Jr      Nz,Chk_Off_Ret
                
ChkOff_NoOff:   Tm      DiskStat,#Offset_On ;THEN check for offset already on
                Jr      Nz,Chk_Off_Ret

                Call    Auto_Offset ;otherwise Auto_Offset
                
Chk_Off_Ret:    Ret

                .LSTOFF
                

