                .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 }
;>                    )
;>      FUNCTION PosHeads( Wait : BOOLEAN { !r8/bit 6 }
;>                         Parent : BYTE { !rB }
;>                         Cylinder : WORD { !!rC }
;>                         Head : BYTE { !rE }
;>                         Sector : BYTE { !rF }
;>                       ) : BOOLEAN
;>      FUNCTION CalcMagDir( Cylinder : WORD { !!rC } ) :
;>                         BOOLEAN
;>                         Magnitude : WORD { !r5 }
;>                         Direction : BYTE { !r9 }
;>      FUNCTION ServoOk : BOOLEAN
;>                         RecalMagDir : BYTE { !r0 }
;>      PROCEDURE SelectHead( Head : BYTE { !rE }
;>      PROCEDURE Auto_Offset
;>      FUNCTION Get_Zone( Cylinder : WORD { !!rC }
;>
;>>>>>>>>>>>>>>>>>>>>>>>>
                .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
;>        DiskStat.SeekComplete := True
;>        DiskStat.OnTrack := True
;>        GlobalCylinder := Cylinder
;>        SelectHead( Head )
;>        GlobalSector   := Sector
;>        SectorCount := SectorCount + 1
;>        Chk_Offset
;>      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
                
Seek_Lp:         Ld     !r8,#Wait
                Call    PosHeads
                Jr      Nz,Seek_End
                
                And     DiskStat,#$FF-Offset_On ;remove any offsets
                Ld      SeekType,#Access_Offset ;all reseek are with offset
                
                Tm      Excpt_Stat,#Recovery
                Jr      Z,Seek_Abt
                
                Push    !r1 ;save counter
                 Ld     !r2,#.HIBYTE. 20 ;wait 200 ms before retrying
                 Ld     !r3,#.LOWBYTE. 20
                Call    MsWait
                Pop     !r1 ;get counter back
                
                Djnz    !r1,Seek_Lp
                
Seek_Abt:        Ld     !r0,#0 ;byte 0
                 Ld     !r1,#Stat_Srvo
                Call    SetStatus
                Or      DiskStat,#SeekComplete
                Call    Abort
                
Seek_Wait:      Call    Wait_For_Rdy
                Jr      Z,Seek_Lp

Seek_End:       Or      DiskStatus,#On_Track+Seek_Complete
                Ld      Cylinder,!rC
                Ld      Cylinder+1,!rD
                Call    SelectHead
                Ld      Sector,!rF
                Incw    SeekCount
                
                
                Call    Chk_Offset
                Jr      Z,Seek_Lp
                
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_End

WtRdy_Tmr:      Tm      Irq,#Timer1 ;check for timer interrupt
                Jr      Z,Seek_Wt_Lp
                Dec     !r1
                Jr      Nz,Seek_Wt
                
WtRdy_End:      Pop     !r1
                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( 1 )
;>       IF ServoOk
;>        THEN
;>         IF ( CalcMagnitudeDirection( Cylinder, Magnitude, Direction ) <> 0 )
;>          THEN
;>           DiskStat.SeekComplete := False
;>           DiskStat.On_Track := False
;>           SetDeadManTimer
;>           ServorStore( IF NOT( Recovery )
;>                         THEN Access+Direction+
;>                              Magnitude[ 1 ], Magnitude[ 2 ],
;>                              0, 0 )
;>                         ELSE
;>                          IF ( SeekType = Access_Offset )
;>                           THEN Access_Offset+Direction+
;>                                Magnitude[ 1 ], Magnitude[ 2 ],
;>                                AutoOffset, 0 )
;>                           ELSE
;>                            IF NOT( Get_Zone )
;>                             THEN Access+Direction+
;>                                  Magnitude[ 1 ], Magnitude[ 2 ],
;>                                  0, 0 )
;>                             ELSE Access_Offset+Direction+
;>                                  Magnitude[ 1 ], Magnitude[ 2 ],
;>                                  Manual_Offset, 0 )
;>           ClearDeadManTimer
;>          IF ( SeekType = Access_Offset ) THEN DiskStat.Offset_On := True
;>         Cur_Cyl := LocalCylinder
;>       PositionHeads := NOT( LoadStatus.ServoErr )
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

PosHeads:
                Call    Ext_Push ;save callers variables
                
                 Ld     !rE,#1 ;do all servoing off of head 1
                Call    SelectHead
                
PosHds_Rpt:     Call    ServoOk ;test if Servo is in a reasonable state
                Jr      Z,PosHds_6 ;leave if servo can't be made useable
                
                Call    CalcMagDir
                Jr      Z,PosHds_5
                
PosHds_While:   And    DiskStatus,#$FF-SeekComplete-On_Track-Parked-Offset_On

                Cp      Seek_Type,#Access_Offset
                Jr      Z,GtSk_LdAO
                
                Tm      Excpt_Stat,#Recovery
                Ld      ScrReg2,#0              ;default is no offsets
                Jr      Z,GtSk_LdNo
                
                Call    Get_Zone
                Ld      ScrReg2,#0              ;default is no offsets
                Jr      Lt,GtSk_LdNo
                
                Ld      ScrReg2,!r0     ;set amount of offset
                Jr      GS_LdAO1
                
GtSk_LdAO:      Ld      ScrReg2,#Off_Auto
GS_LdAO1:       Ld      ScrReg0,#Access_Offset
                Jr      GS_LdNo1

GtSk_LdNo:      Ld      ScrReg0,#Access
GS_LdNo1:       Srp     #Wrk_Scr
                Or      !r0,Wrk_Sys+$09 ; Plus Direction
                Or      !r0,Wrk_Sys+$05 ; Plus Ms Magnitude
                Ld      !r1,Wrk_Sys+$06 ; Ls Magnitude
                Ld      !r3,#S_Rate_57_6
                Srp     #Wrk_Sys
                Call    Set_Dmt
                Call    ServoCmnd
                Di                      ;Clear Dead_Man_Timer
                
                Cp      SeekType,#Access_Offset ;check for auto_offset
                Jr      Nz,PosHds_4
                
                Or      DiskStat,#Offset_On
                
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    Chk_SStat
                Jp      Bank_Ret
                
;*********************

Chk_SStat:      Call    LoadStatus ;get current servo status
                Ld      !r1,!r0    ;get copies of both ServoErr and ServoRdy
                Com     !r0        ;ServoErr := NOT( ServoErr )
                Rl      !r0        ;get ServoErr in same position as ServoRdy
                And     !r0,!r1    ;return AND( ServoRdy, NOT( ServoErr ) )
                And     !r0,#ServoRdy ;set zero flag
                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:      Jp      Bank_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
                Ld      !r2,Cur_Cyl+1
                Sub     !r2,!rD ;get CurrentCylinder - Cylinder
                Sbc     !r1,!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,!r2
                Or      !r3,!r1
                Jr      Z,C_MagDir_Ret
                Jr      S_Glbl_Cyl
                
C_MagDir_Else:  Com     !r2     ;2's complement sutraction result
                Com     !r1
                Add     !r2,#1
                Adc     !r1,#0
                
                Ld      !r9,#Hd_Dir_Frwd     ;set direction positive
                
S_Glbl_Cyl:     Ld      !r5,!r1 ;load magnitude
                Ld      !r6,!r2
                
                Or      !r4,#01 ;set Seek
C_MagDir_Ret:   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 ) }
;>              RecalcMagDir: BYTE { !r0 }
;>
;>      Local Variables:
;>              Retry   : BYTE { !rF }
;>              SioRetry: BYTE { !r8 }
;>              Done    : BOOLEAN { }
;>              ControllerStatusPTR : PTR { !!rC }
;>
;>      Algorithm:
;>
;>      BEGIN
;>       IF Recovery 
;>        THEN
;>         IF ServoError
;>          THEN
;>           IF Write_Operation THEN WrBuf_To_Buf2
;>           Restore( DataRecal )
;>           Buf2_To_WrBuf
;>          ELSE
;>           IF NOT( ServoReady )
;>            THEN
;>             IF Write_Operation THEN WrBuf_To_Buf2
;>             S_Reset
;>             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    ResetServo      ;otherwise un-park servo
                 Ld     !r0,#DataRecal
                Call    Restore
                Call    Buf2_To_WrBuf   ;assume write_op
                Jr      S_Ok_End

S_Ok_End:       Call    Chk_SStat
                Jp      Bank_Ret
                
;********************

Chk_MvWrData:    Ld     !r0,#0 ;byte0
                 Ld     !r1,#Stat_Srvo
                Call SetStatus
                
                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
;>       Temp := Head
;>       SelectHead( 1 )
;>       ServoCmnd( Offset, 0, Off_Auto, S_Rate_57_6 )
;>       @Get_Zone := ServoStatus( 1 ).Offset_Value
;>       DiskStat.Offset_On := True
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

Auto_Offset:
                 Call   Ext_Push
                 Push   Head    ;save the current head value
                 
                  Ld    !rE,#1  ;select head 1 for offsetting
                 Call   SelectHead
                 
                 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    Wait_For_Rdy
                
                 Srp    #Wrk_Scr
                 Ld     !r0,#ReadStatus
                 Ld     !r1,#0
                 Ld     !r2,#0
                 Ld     !r3,#1          ;read servo stat 1 to find offset value
                 Srp    #Wrk_Sys
                Call    ServoStatus
                Call    Clr_Dmt
                
                Ld      !r2,#.HIBYTE. (SStatus0+1) ;get offset amount
                Ld      !r3,#.LOWBYTE. (SStatus0+1)
                Lde     !r0,@!!r2
                Xor     !r0,#$1F             ;get correct polarity
                And     !r0,#$3F        ;mask off unwanted stuff
                
                 Push   !r0
                 Ld     !rC,Cylinder
                 Ld     !rD,Cylinder+1
                Call    Get_Zone
                 Pop    !r0
                Lde     @!!r2,!r0       ;store new value
                
Auto_Off_Ret:   Pop     !rE     ;re-select original head
                Call    SelectHead
                Call    Ext_Pop
                
                Or      Disk_Stat,#Offset_On
                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:    Jp      Bank_Ret
                
                .LSTOFF
                .DO     External
                .LSTON
                .Page
;>>>>>>>>>>>>>>>>>>>>>>>>
;>
;>      Function: Get_Zone
;>
;>      This function is responsible for identifying the zone
;>      that the heads are currently positioned in; a zone being
;>      a section of the drive that is assumed to have a homogeneous
;>      servo offset characteristic.
;>
;>      Inputs:
;>              Cylinder    : WORD { !!rC }
;>
;>      Outputs:
;>              Get_Zone : BYTE { !r0 },
;>              Get_Zone1 : BOOLEAN { zero flag is set if servo err }
;>
;>      Algorithm:
;>
;>      BEGIN
;>       Get_Zone := Zone_Table[ Cylinder/NbrZones ]
;>       IF ( Get_Zone <= MinOffset )
;>        THEN Get_Zone1 := False
;>        ELSE GEt_Zone1 := True
;>      END
;>
;>>>>>>>>>>>>>>>>>>>>>>>>
                .LSTOFF
                .FIN
                .DO     Internal
                .LSTON
                .Page
                .FIN

Get_Zone:       Ld      !r0,!rC         ;use seek address for zone calc
                Ld      !r1,!rD
                Ld      !r2,#ZoneShift
                Rcf
GtZn_Lp:        Rrc     !r0 ;rotate down the cylinder value
                Rrc     !r1
                Djnz    !r2,GtZn_Lp
                
                Ld      !r2,#.HIBYTE. Zone_Table
                Ld      !r3,#.LOWBYTE. Zone_Table
                Add     !r3,!r1         ;add offset to table
                Adc     !r2,#0
                Lde     !r0,@!!r2       ;get offset value
                
                Tm      !r0,#Dir_Frwd   ;mask all but sign bit
                Ld      !r1,#Off_Dir_Frwd ;assume forward
                Jr      Nz,GtZn_Push
                
                Ld      !r1,#0  ;otherwise reverse
GtZn_Push:      And     !r0,#$1F
                Cp      !r0,#MinOffset  ;check for lower bound on offset
                Push    Flags
                Or      !r0,!r1 ;merge sign and magnitude
                Pop     Flags
                Jp      Bank_Ret
                
                .LSTOFF

