	cpu Z8601
	page 0
	include stddefZ8.inc

;********************************************************************
;
;              Apple Widget Voicecoil Servo - Z8 firmware
;                         Version 341-0260-A
;            original code (c) by Apple Computer Inc., 1983
;
;
;        disassembled, commented and converted into as source 
;                     by Patrick Schaefer, 2012
;
;    use as build 2011-08-02 or later. Last changes: 2013-07-31 PS
;
;********************************************************************

ROMsize		SET 2048


; *******************************************************************
;  Hardware
; *******************************************************************
;
; *** Port 0
ParkHeads	EQU 001h	; P0.0 1 to disable power amp and park heads
Mux1D0		EQU 002h	; P0.1 HA-2405 (U3E) D0
Mux1D1		EQU 004h	; P0.2 HA-2405 (U3E) D1
Mux1Ena		EQU 008h	; P0.3 HA-2405 (U3E) Enable
OffsetDacStrb	EQU 010h	; P0.4 L291 strobe 0 = active, 1 = 0V
RecalMode	EQU 020h	; P0.5 1 = select recal mode (velocity control)
SettlingMode	EQU 040h	; P0.6 1 = select settling mode
AccessMode	EQU 080h	; P0.7 1 = select access mode
; *** Port 1
;  bit 0..4 offset magnitude (1Fh = 0; 1Eh = 1/16 Iref; ...; 00h = 31/16 Iref)
OffsetDacZero	EQU 01Fh
OffsetDacSign	EQU 020h	; bit 5 offset sign (0=minus, 1=plus)
Mux2D0		EQU 040h	; bit 6 HA-2405 (U5D) D0
Mux2D1		EQU 080h	; bit 7 HA-2405 (U5D) D1
; *** Port 2
U2Gfast		EQU 001h	; P2.0 1 = OpAmp U2G fast, 0 = slow
Port21		EQU 002h	; P2.1 in  On Track Window ??
PosErr_Cmp	EQU 004h	; P2.2 in  1 when Position Error > DAC value
Port23		EQU 008h	; P2.3 0 = zero integrator U1B, 
				;      1 integrator zeroed by Pulse A
Port24		EQU 010h	; P2.4 in  Final Track Window ??
U2Hfast		EQU 020h	; P2.5 1 = OpAmp U2H fast, 0 = slow
Mux3D0		EQU 040h	; P2.6 HA-2405 (U3C) D0
OddEven		EQU 080h	; P2.7 1 = Odd, 0 = Even
; *** Port 3
Servo_SI	EQU 001h	; P3.0 SIO RxD in
Port31		EQU 002h
Port32		EQU 004h
Port33		EQU 008h
; Mode	    | P3.1 			P3.2 			P3.3 
;	    | (T1 clock or INT2)	(INT0)			(INT1)
; ----------+--------------------------------------------------------------------+
; idle	    | ??			??			??
;	    |
; Recal	    | Pulse A NAND Pulse B	??			??
;  (P0.5=1) |
; Settling  | U1E output		??			Position Error Sample
;  (P0.6=1) |
; Access    | Pulse A XOR Pulse B	??			??
;  (P0.7=1) |
;
SioRdy		EQU 010h	; P3.4 SIO Ready
SrvoRdy		EQU 020h	; P3.5 Servo Ready
SrvoErr		EQU 040h	; P3.6 Servo Error
Servo_SO	EQU 080h	; P3.7 SIO TxD out


; *******************************************************************
;  internal RAM
; *******************************************************************
;
CmdByte0	EQU 04h		; bytes received from host
CmdByte1	EQU 05h
CmdByte2	EQU 06h
CmdByte3	EQU 07h
CmdByte4	EQU 08h
BytePtr		EQU 09h		; byte pointer for serial comms
; 		EQU 0Ah
; 		EQU 0Bh
;		EQU 0Ch
;		EQU 0Dh
;		EQU 0Eh
;		EQU 0Fh
;		EQU 10h
SM_State	EQU 11h		; state machine
T0_MSB		EQU 12h		; MSB for access timeout (T0) 
Temp1		EQU 13h	
Temp2		EQU 14h
Temp3		EQU 15h
P1_Mask		EQU 16h		; bit 6 (040h) set during init,
				; bit 7 (080h) set when 57k6
StatusByte0	EQU 17h		; bytes sent to host
StatusByte1	EQU 18h
StatusByte2	EQU 19h
StatusByte3	EQU 1Ah
SM_FaultState	EQU 1Bh		; state where failure occured
DelayL		EQU 1Ch		; delay time value
DelayH		EQU 1Dh
CmdByte0H	EQU 1Eh		; upper 4 bit of CmdByte0 (00..0Fh)
FaultCtr	EQU 1Fh		; fault counter
SioBaud		EQU 20h		; 3 for 19k2, 1 for 57k6
LastRCmd0	EQU 21h		; last received command
LastRCmd1	EQU 22h
LastRCmd2	EQU 23h
LastRCmd3	EQU 24h
LastPCmd0	EQU 25h		; last processed command
LastPCmd1	EQU 26h
LastPCmd2	EQU 27h
LastPCmd3	EQU 28h
LastPCmdAbort	EQU 29h		; 1 when last command has been aborted
OffsetFlag	EQU 2Ah		; bit 7 set when offset control on (DAC <> 0x1Fh)
SAR_ANDmask	EQU 2Bh		; for Auto Offset approximation
SAR_ORmask	EQU 2Ch
; 2D:2Fh unused
; 30:3Fh used during Access



AccForward	EQU 004h	; CmdByte0 bit 2: 1 = forward (to spindle), 0 = back
OfsForward	EQU 080h	; CmdByte2 bit 7: 1 = forward (to spindle), 0 = back
OfsSetAuto	EQU 040h	; CmdByte2 bit 6: 1 = auto offset enable
OfsReadDAC	EQU 020h	; CmdByte2 bit 5: 1 = take offset value from DAC
Sta57k6		EQU 080h	; CmdByte3 bit 7: 1 = set comm rate to 57k6


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


	org 0000h
Int_Vec0	DW HW_Interrupt		; P3.2
Int_Vec1	DW HW_Interrupt		; P3.3
Int_Vec2	DW HW_Interrupt		; P3.1
Int_Vec3	DW HW_Interrupt		; P3.0 and SIO in
Int_Vec4	DW T0_Interrupt		; T0 and SIO out
Int_Vec5	DW T1_Interrupt		; T1

; *** State 0 System Initialization
Start 		ld P3M,#001h		; totem-pole I/O, parity off
		clr P3
		ld P01M,#004h		; P0, P1 output, internal stack
		ld SPL,#080h
		di     
		clr IMR
		clr TMR
		srp #000h
	assume RP:000h
		clr R4			; clear internal RAM
		ld R5,#122
ZeroRAM_Lp	ld 05h(R5),R4
		djnz R5,ZeroRAM_Lp
		ld >P1,#OffsetDacZero	; set offset DAC to zero
		ld P2M,#016h		; set P2.4, P2.2, P2.1 as input
		clr P2
		ld P1_Mask,#040h
; *** State 1 SIO Communication Setup
		ld SM_State,#1
		ei     
		ld SioBaud,#3		; default baud rate = 19k2

; command C: Home (park heads)
;
Cmd_Home	ld >P0,#OffsetDacStrb+ParkHeads	; park and wait
		and >P3,#0FFh-SrvoRdy	; clear Servo Ready
		call RxCmdBytes		; get host command
		call Cmd_Decode		; get cmd vector into RR4
		cp CmdByte0H,#0		;  ReadStatus?
		jr Z, Cmd_Home1
		jp @RR4			; otherwise jump to vector
; send status command
Cmd_Home1	clr SPH
		call @RR4		; call Cmd_ReadStatus
		tm SPH,#Sta57k6		; TxStatusBytes stored CmdByte3 here
		jr Z, Cmd_Home2
		ld SioBaud,#1		; go to 57k6	
		or P1_Mask,#080h
Cmd_Home2	jr Cmd_Home

; dead code (never accessed)
		cp SM_State,#10		; error state?
		jp Z, SM_State_10
		clr TMR


; *******************************************************************
; command 4/7: Recal
;
Cmd_Recal	ld >P0,#OffsetDacStrb+ParkHeads	; park and wait
		and >P3,#0F0h-SrvoErr-SrvoRdy-SioRdy	; clear Err and Rdy flags
		ld Temp3,#6		; ??
		ld DelayL,#255
		ld DelayH,#255
		call Delay
		call Delay
		clr FaultCtr		; first attempt
; *** State 2 Recal State
SM_State_2	ld SM_State,#2
		and >P3,#0FFh-SioRdy-SrvoRdy	; clear SioRdy, SrvoRdy
		di     
		clr IRQ
		ld T0,#0FFh		; load access timer T0
		ld PRE0,#0FFh		;  Timer 0 int/63 continuous 
		ld T0_MSB,#10
		cp FaultCtr,#0		; is this a retry?
		jr NZ, SM_St2_1		;  yes --> (we already have T1 set up)
		tm >CmdByte0,#030h	; normal recal?
		jr NZ, SM_St2_2		;  yes -->
		ld T1,#72/4		; load pulse count for Format Recal
		ld PRE1,#004h		;  Timer 1 ext/1 single (count P3.1)
		jr SM_St2_1
SM_St2_2	ld T1,#32/4		; load pulse count for Data Recal
		ld PRE1,#004h		;  Timer 1 ext/1 single (count P3.1)
SM_St2_1	clr CmdByte0		; set ports for recal motion
		ld >P1,#Mux2D1+Mux2D0+01Ah	; DAC value 5 (01Ah)
		ld >P0,#RecalMode+OffsetDacStrb+Mux1Ena
		ld >0Eh,#0C9h		; 01101010b
; *** State 3 Start Recal Motion
;  Wait for the number of PulseA pulses given in T1. Each pulse equals to four 
;  tracks on disk.
		ld >SM_State,#3
		ld TMR,#00Fh		; load & enable T0 & T1
		ld IMR,#010h		; enable T0 interrupt (access timeout)
		ei     
		clr IRQ			;  and clear pending interrupts
SM_St3_Lp	cp SM_State,#10		; error state?
		jp Z, SM_State_10
		tcm IRQ,#004h		; pulse at P3.1?
		jr NZ, SM_St3_Lp	;  no --> wait
		and IRQ,#0FBh		; acknowledge pulse
		tcm IRQ,#020h		; T1 expired?
		jr NZ, SM_St3_Lp	;  no --> wait
		and IRQ,#0DAh		; acknowledge ints and set ports for
		ld >P0,#Mux1Ena+Mux1D1	;    final approach
		and >P2,#0FFh-OddEven-Mux3D0-U2Hfast-Port24-Port23
; *** State 4 Recal Final Approach
		ld >SM_State,#4
		ld >P1,#0FFh-OffsetDacSign
		di     
		ld IMR,#010h		; enable T0 interrupt (access timeout)
		ei     
SM_St4_Lp	cp SM_State,#10		; error state?
		jp Z, SM_State_10
		tcm IRQ,#001h		; pulse at P3.2 (on track)?
		jr NZ, SM_St4_Lp	;  no --> wait
		ld DelayL,#1
		ld DelayH,#1
		call Delay
		and IRQ,#0DEh		; acknowledge pulse
		or >P0,#OffsetDacStrb+Mux1D1+Mux1D0
		ld DelayL,#15
		ld DelayH,#1
		call Delay
; *** State 5 Settling Control
SM_State_5	ld SM_State,#5
		and >P2,#0FFh-Mux3D0	; set ports for position settling
		ld >P0,#SettlingMode+OffsetDacStrb+Mux1Ena+Mux1D0
		di     
		clr IMR			
		clr TMR
		and IRQ,#00Fh		; allow only P3.0 - P3.3 interrupts
		ld T0,#0FFh		; load arm setting timer
		ld PRE0,#01Ch		;  Timer 0 int/7 single
		ld T1,#003h		; load track crossing counter
		ld PRE1,#004h		;  Timer 1 ext/1 single
		ld TMR,#00Fh		; load & enable T0 & T1
		ei     
		and >CmdByte0,#03Fh
		ld SPH,#2		; retry counter
SM_St5_Lp	cp SM_State,#10		; error state?
		jp Z, SM_State_10
		tm IRQ,#010h		; T0 expired?
		jr Z, SM_St5_Lp		;  no, wait
		and IRQ,#0EFh		; acknowledge T0 interrupt
		and TMR,#00Ch		; and stop T0
		tm >P3,#Port31		; P3.1 zero (not on track)?
		jr Z, SM_State_6	;  no --> we have landed
;
		dec SPH			; otherwise try again
		jp Z, State5_Error	;  until counter expired
		or TMR,#003h		; load & start T0
		jr SM_St5_Lp

; *** State 6
SM_State_6	ld SM_State,#6
		ld DelayL,#255
		ld DelayH,#4
		call Delay
		or >P2,#Mux3D0	
		cp >FaultCtr,#0		; is this a retry?
		jr NZ, SM_St6_1		;  yes -->
		and >P3,#0FFh-SrvoErr	; clear SrvoErr
SM_St6_1	ld DelayL,#255
		ld DelayH,#10
		call Delay
		di     
		and IRQ,#0DAh		; acknowledge P3.1 & P3.2 int
		ld IMR,#020h		; enable T1 interrupt
		ld TMR,#00Ch		; load & start T1
		ei     
		cp OffsetFlag,#080h	; offset control active?
		jr NZ, SM_St6_2		;  no -->
		clr OffsetFlag		; otherwise turn it off
		pop R5
		pop R4
		pop CmdByte3
		pop CmdByte2
		pop CmdByte1
		pop CmdByte0
		jr SM_St6_3

SM_St6_2	tm >CmdByte0,#010h	; access with offset?
		jr Z, SM_State_6a	;  no --> 


; command 1: SetOffset
;
Cmd_SetOffset	call CallOffset
		jp SM_State_5


; *** State 6a
SM_State_6a	ld SM_State,#6
		or >P3,#SrvoRdy		; set ServoRdy
		call RxCmdBytes		;  and wait for host command
		cp SM_State,#10		; error state?
		jp Z, SM_State_10
; *** State 7 Start SIO Communication
		ld SM_State,#7
		call Cmd_Decode		; get Cmd vector into RR4
		cp CmdByte0H,#0		; ReadStatus command?
		jr Z, SM_St7_1		;  yes -->
		tm >CmdByte0,#080h	; Access command
		jr NZ, SM_St7_2		;  yes (bit 7 was 1) --> 
SM_St6_3	jp @RR4			; execute cmd and continue with State 2
; ReadStatus command
SM_St7_1	call @RR4		; execute ReadStatus and
		jr SM_State_6a		;  continue with State 6
; Access command
SM_St7_2	tcm >P1,#OffsetDacZero	; DAC at zero?
		jr Z, SM_St6_3		; yes, so we don't have an offset
		ld OffsetFlag,#080h	; 
		push CmdByte0
		push CmdByte1
		push CmdByte2
		push CmdByte3
		push R4
		push R5
		clr CmdByte2
		call CallOffset
		jp SM_State_5


; *******************************************************************
; command 8/9: Access / AccessOffset
;
; *** State 8 Access State (Seek)
Cmd_Access	ld SM_State,#8
		and >P3,#0FFh-SrvoRdy	; clear SrvoRdy
		di     
		and IRQ,#027h
		clr TMR
		tm >CmdByte0,#AccForward	; access direction forward?
		jr NZ, SM_St8_1			;  yes (bit 3 was 1) -->
		and >P1,#0FFh-OffsetDacSign	; negative
		clr P1_Mask
		jr SM_St8_12
SM_St8_1	or >P1,#OffsetDacSign		; positive
		ld P1_Mask,#OffsetDacSign
SM_St8_12	ld T1,>CmdByte1		; load difference counter T1
		or TMR,#004h		;   with magnitude LSB
		ld Temp1,>CmdByte0	; magnitude MSB
		and Temp1,#003h		; mask out command bits
		srp #030h
	assume RP:030h
		ld R3,CmdByte1		; set difference LSB
		ld R2,Temp1		; and MSB
		tm R2,#003h		; MSB zero?
		jr NZ, SM_St8_2		;  no -->
		cp R3,#0		; LSB zero?
		jp Z, SM_State_6	; yes --> go settling (State 6)
; set velocity curve value
SM_St8_2	tm R2,#003h		; MSB zero?
		jr NZ, SM_St8_3		;  no -->
		tm R3,#0E0h		; LSB < 63 (1Fh)?
		jr NZ, SM_St8_3		;  no -->
		call GetCurveValue	; get DAC value
		ld P1,P1_Mask		;  and update P1
		jr SM_St8_4
SM_St8_3	and P1,#0FFh-OffsetDacZero	; set DAC at maximum
SM_St8_4	ld PRE1,#004h		;  Timer 1 ext/1 single
		ld T0,#0FFh
		ld PRE0,#0FFh		;  Timer 0 int/63 continuous 
		ld T0_MSB,#10
		ld P0,#098h
		clr R13			; use curve table
		call L684
		dec T1
		decw RR2		; difference -1
		or P2,#U2Gfast		; set P2.0 (U2G fast)
		and IRQ,#0FDh		; acknowledge P3.1 interrupt
		ld IMR,#012h		; enable T0 and P3.3 int
		or TMR,#007h		; load T1, load & start T0
		tm R2,#003h		; MSB zero?
		jr NZ, SM_St8_5		;  no -->
		tm R3,#0E0h		; LSB < 63 (1Fh)?
		jr NZ, SM_St8_5		;  no -->
		tm R3,#0FFh		; LSB zero
		jr Z, SM_St8_6		;  yes -->
		or P2,#Port23		; set P2.3 (auto integrator control)
		call GetCurveValue
		ld P1,P1_Mask
		jr SM_St8_7
;
SM_St8_6	or IRQ,#020h		; force T1 interrupt
		or P2,#Port23		; set P2.3 (auto integrator control)
		ld DelayL,#175
		ld DelayH,#1
		call Delay
		ld R13,#1		; use fixed DAC value
		jr SM_St8_8
;
SM_St8_7	call GetCurveValue
		ld P1,P1_Mask
SM_St8_5	decw RR2
		tm R2,#003h		; MSB zero?
		jr NZ, SM_St8_9		;  no -->
		tm R3,#0E0h		; LSB < 63 (1Fh)?
		jr NZ, SM_St8_9		;  no -->
		call GetCurveValue
SM_St8_9	ld DelayL,#80
		ld DelayH,#3
		call Delay
SM_St8_8	or TMR,#00Ch		; load & start T1
		and IRQ,#0FBh		; clear pending P3.1 interrupt
		ei     
SM_St8_Lp2	tcm IRQ,#020h		; T1 int occurred?
		jr Z, SM_St8_10		;  yes -->
SM_St8_Lp1	cp SM_State,#10		; error state?
		jp Z, SM_State_10
		tm IRQ,#004h		
		jr Z, SM_St8_Lp1	; wait for P3.1 interrupt
		decw RR2
		and IRQ,#0FBh		; clear P3.1 interrupt
		tm IRQ,#020h		; T1 int occurred?
		jr NZ, SM_St8_10	;  yes -->
		tm R2,#003h		; MSB zero?
		jr NZ, SM_St8_11	;  no -->
		cp R2,#0FFh		; LSB = 255?
		jr Z, SM_State_9	;  yes -->
		cp T1,#01Fh
		jr ugt, SM_St8_11
		ld P1,P1_Mask
		or P2,#Port23		; set P2.3 (auto integrator control)
		call GetCurveValue
		srp #030h
	assume RP:030h
		jr SM_St8_Lp2
;
SM_St8_10	cp R2,#1
		jr ugt, SM_State_9
		cp R2,#2
		jr ugt, SM_State_9
		cp R13,#1		; fixed DAC value?
		jr Z, SM_State_9	;  yes -->
		and IRQ,#0DFh
		ld T1,#0
		or TMR,#00Ch		; load & start T1
SM_St8_11	and P1,#0FFh-OffsetDacZero	; DAC at maximum
		srp #030h
	assume RP:030h
		jr SM_St8_Lp2

; *** State 9 Seek Final Approach (one track to go)
SM_State_9	ld SM_State,#9
		and P2,#0FFh		; ??
		or P1,#OffsetDacZero
		and P1,#0FFh-4		; set offset DAC to 4
		cp R13,#1		; fixed DAC value?
		jr NZ, SM_St9_1		;  no -->
		and P1,#0FFh-6		; set offset DAC to 6
SM_St9_1	and P1,#0FFh-Mux2D0-Mux2D1	; select Mux2 channel 0
		or P1,R15
		ld P0,#OffsetDacStrb+Mux1Ena	; select Mux1 channel 0
		ld DelayL,#1
		ld DelayH,#1
		call Delay
		and TMR,#003h		; load and start T0
		jr Z, SM_St9_2
SM_St9_2	cp SM_State,#10		; error state?
		jr Z, SM_State_10
		tm P2,#Port24		; Final Track Window??
		jr Z, SM_St9_2
		and P2,#0FFh-Port23	; clear P2.3 (zero integrator)
		or P1,#OffsetDacZero	; set offset DAC to zero
		and IRQ,#0FEh		; acknowledge P3.2 interrupt
		ld P0,#Mux1Ena		; select Mux1 channel 0	
		di     
		ld IMR,#010h		; enable T0 interrupt
		ei     
SM_St9_Lp	cp SM_State,#10		; error state?
		jr Z, SM_State_10
		tcm IRQ,#001h		; P3.2 pulse occurred?
		jr NZ, SM_St9_Lp	;  no --> wait
		ld DelayL,#255
		ld DelayH,#1
		cp R13,#1		; fixed DAC value?
		jr NZ, SM_St9_3		;  no -->
		ld DelayH,#2
SM_St9_3	call Delay
		and IRQ,#0DEh		; acknowledge P3.2 pulse
		or P0,#OffsetDacStrb+Mux1D1+Mux1D0		
		ld DelayL,#1
		ld DelayH,#1
		call Delay
		clr FaultCtr
		jp SM_State_5		; begin access settling


; *******************************************************************
;  Error Handling
; *******************************************************************
;
; come here if State 5 failed twice
State5_Error	ld StatusByte0,P0
		ld StatusByte1,P1
		ld StatusByte2,P2
		ld StatusByte3,IRQ
		or StatusByte3,#090h
		ld SM_State,#10		; set error state
;
; *** State 10 Error Entry
;  State 10 is entered after T0_MSB access timeouts (T0 ints) occurred
;  or counter T1 expired. Status0:3 holds P0, P1, P2, IRQ; SM_FaultState
;  holds the last SM_State.
SM_State_10	di     
		cp SM_FaultState,#6	; fault in state 6?
		jr NZ, SM_St10_1	;  no -->
		and IRQ,#0DFh		; acknowledge T1 interrupt
		or TMR,#00Ch		; load & start T1
		clr IMR			; disable all interrupts
		ld DelayL,#255
		ld DelayH,#5
		call Delay		; wait 5ms
		ld FaultCtr,#1		; first retry
		ei     
		tm IRQ,#020h		; on track
		jp Z, SM_State_6	; retry
SM_St10_1	and P3,#0FFh-SioRdy-SrvoRdy
		ld T1,#004h		
		ld PRE1,#004h		;  Timer 1 ext/1 single
		or P3,#SrvoErr		; set ServoErr
		inc FaultCtr		; next retry
		cp FaultCtr,#3		; 3rd attempt?
		jp C, SM_State_2	;  no, do error recal
		ld P0,#OffsetDacStrb+ParkHeads
		ld P1,#Mux2D1+Mux2D0+OffsetDacZero
		ld SM_State,#12
		clr IMR
		clr IRQ
		ei     
		jp Cmd_Home		; after 3 attempts go parking

; illegal command
;
Cmd_Abort	ld SM_FaultState,SM_State	; remember faulty state
		ld SM_State,#11
		or P3,#SrvoErr
		ld LastPCmdAbort,#1	; mark last command as aborted
		ld DelayL,#205
		ld DelayH,#1
		call Delay
		tm P3,#SrvoRdy		; ServoReady set?
		jp NZ, SM_State_6a	;  yes --> go settling
		jp Cmd_Home		; otherwise go parking

	
; *******************************************************************
;  Command processing 
; *******************************************************************
;
; decode command given in CmdByte0 (04h), return vector in RR4
; In State 1 (after power-up) only ReadStatus and Recal are allowed
;
Cmd_Decode	ld CmdByte0H,CmdByte0	; get Servo Command Byte
		swap CmdByte0H
		and CmdByte0H,#00Fh	; isolate command nibble
		srp #80			; !!! invalid - a 2k device has
	assume RP:080h			;     only 124 registers (04:7Fh) !!!
		cp CmdByte0H,#0		; Read Status (0)?
		jr Z, LoadCmdAdrs	;  yes -->
		tm CmdByte0H,#4		; Recal or Home (4, 7)?
		jr NZ, LoadCmdAdrs	;  yes -->
		tm P3,#SrvoErr		; Servo Error?
		jr NZ, CmdReject	;  yes --> abort
		cp SM_State,#7		; State 7 (Start SIO Communication)?
		jr NZ, CmdReject	;  no --> abort
LoadCmdAdrs	ld R2,#CmdVectors /256
		ld R3,#CmdVectors #256
		rl CmdByte0H
		add R3,CmdByte0H	; calculate table index
CmdReject1	ldc R4,@RR2		; and get vector adress
		inc R3
		ldc R5,@RR2		; return command vector in RR4
		ret    
; reject command
CmdReject	ld R2,#AbortVector /256	
		ld R3,#AbortVector #256
		jr CmdReject1

CmdVectors	DW Cmd_ReadStatus	; 0 ReadStatus
		DW Cmd_SetOffset	; 1 SetOffset
		DW Cmd_Abort		; 2 (Diagnostic)
		DW Cmd_Abort		; 3
		DW Cmd_Recal		; 4 DataRecal
		DW Cmd_Abort		; 5
		DW Cmd_Abort		; 6
		DW Cmd_Recal		; 7 FormatRecal
		DW Cmd_Access		; 8 Access 
		DW Cmd_Access		; 9 AccessOffset
		DW Cmd_Abort		; A
		DW Cmd_Abort		; B
		DW Cmd_Home		; C Home
		DW Cmd_Abort		; D
		DW Cmd_Abort		; E
AbortVector	DW Cmd_Abort		; F


; command 0: ReadStatus
;
Cmd_ReadStatus	ld DelayL,#205
		ld DelayH,#2
		call Delay
		ld Temp3,CmdByte3
		and Temp3,#00Fh		; isolate status register
		ld R8,#StatusVectors /256
		ld R9,#StatusVectors #256
		rl Temp3			; generate index
		add R9,Temp3
		ldc R10,@RR8
		inc R9
		ldc R11,@RR8		; get address
		jp @RR10		;  call status routine
SendStatus	call TxStatusBytes	;  and send data to host
		ret    

; Status 1 is the standard status which holds the current offset in Byte 1 bit 5:0
StatusVectors	DW Status_0		; 0   (resend last or get fault conditions)
		DW Status_1		; 1 P0,  P1,  P2,  IRQ
		DW Status_2		; 2 SIO, P3,  TMR, FLAGS
		DW Status_3		; 3 T0,  T1,  IMR, RP
		DW Status_4		; 4 SPH, SPL, CmdByte0H, T0_MSB
		DW Status_5		; 5 0Eh, 0Ch, SM_FaultState, P1_Mask
		DW Status_6		; 6 last received command (21:24h)
		DW Status_7		; 7 IRQ, P0,  P3,  P1_Mask
		DW Status_8		; 8 last processed cmd (25:27), LastPCmdAbort
		DW Status_8		; 9
		DW Status_8		; A
		DW Status_8		; B
		DW Status_8		; C
		DW Status_8		; D
		DW Status_8		; E
		DW Status_8		; F

; Status 0
Status_0	jr SendStatus
; Status 1
Status_1	ld StatusByte0,P0
		ld StatusByte1,P1
		ld StatusByte2,P2
		ld StatusByte3,IRQ
		jr SendStatus
; Status 2
Status_2	ld StatusByte0,SIO
		ld StatusByte1,P3
		ld StatusByte2,TMR
		ld StatusByte3,FLAGS
		jr SendStatus
; Status 3
Status_3	ld StatusByte0,T0
		ld StatusByte1,T1
		ld StatusByte2,IMR
		ld StatusByte3,RP
		jr SendStatus
; Status 4
Status_4	ld StatusByte0,SPH
		ld StatusByte1,SPL
		ld StatusByte2,CmdByte0H
		ld StatusByte3,T0_MSB
		jr SendStatus
; Status 5
Status_5	ld StatusByte0,0Eh
		ld StatusByte1,0Ch
		ld StatusByte2,SM_FaultState
		ld StatusByte3,P1_Mask
		jr SendStatus
; Status 6
Status_6	ld StatusByte0,LastRCmd0
		ld StatusByte1,LastRCmd1
		ld StatusByte2,LastRCmd2
		ld StatusByte3,LastRCmd3
		jr SendStatus
; Status 7
Status_7	ld StatusByte0,IRQ
		ld StatusByte1,P0
		ld StatusByte2,P3
		ld StatusByte3,P1_Mask
		jp SendStatus
; Status 8-F
Status_8	ld StatusByte0,LastPCmd0
		ld StatusByte1,LastPCmd1
		ld StatusByte2,LastPCmd2
		ld StatusByte3,LastPCmdAbort
		jp SendStatus


; *******************************************************************
;  Offset DAC adjustment routines
; *******************************************************************
;
; set track offset
CallOffset	ld P3M,#001h
		di     
		clr TMR
		tm CmdByte2,#OfsSetAuto		; use auto offset?
		jr NZ, CallOffset1		;  yes -->
		tm CmdByte2,#01Fh		; offset value given = 0?
		jr Z, CallOffset2		;  yes -->
		ld DelayL,CmdByte2
		com DelayL
		and DelayL,#01Fh		; convert into DAC format
		ld DelayH,P1			; get current P1 state
		and DelayH,#0FFh-OffsetDacSign-OffsetDacZero	;  zero DAC
		tm CmdByte2,#OfsForward		; dir backwards (away from spindle)?
		jr NZ, CallOffset3		;  no (bit 7 was 1) -->
		or DelayH,#OffsetDacSign	; set sign bit
CallOffset3	or DelayH,DelayL		;  merge in magnitude
		ld P1,DelayH			; write back P1
		jr CallOffset4

; offset value = 0;
CallOffset2	and P2,#0FFh-U2Hfast	; clear P2.5 (U2H slow)
		or P1,#OffsetDacZero	; set offset DAC to zero
		jr CallOffset4

; set auto offset
; wait for Position Error Sample pulse, then look at Position Error signal from
; R/W board and adjust DAC in a successive aproximation fashion for minimum error
CallOffset1	and P2,#0FFh-U2Hfast	; clear P2.5 (U2H slow)
		and IRQ,#0FDh		; clear pending P3.1 interrupt	
CallOffset1_Lp	cp SM_State,#10		; error state?
		jr Z, CallOffset_Err
		tm IRQ,#002h		; Pos Err Sample (P3.3) pulse occurred?
		jr Z, CallOffset1_Lp	;  no --> wait
		ld SAR_ANDmask,#0FFh-10h	; AND mask for bit 4
		ld SAR_ORmask,#010h		; OR mask for bit 4
		and P1,#0FFh-OffsetDacSign-OffsetDacZero	;  zero DAC
		or P1,#OffsetDacZero	;  and set offset DAC to zero
		ld DelayL,#1
		ld DelayH,#1
		call Delay
		tm P2,#PosErr_Cmp	; too low (P2.2 = 0)?
		jr Z, CO_Auto_up	;  yes -->
; 
; DAC value is too high, decrease
		and P1,#0FFh-OffsetDacSign	; minus
CO_Auto_down	and P1,SAR_ANDmask	; clear bit
		rr SAR_ANDmask		; and shift down AND mask
		call Delay
		tm P2,#PosErr_Cmp	; too high (P2.2 = 1)?
		jr NZ, CO_Auto_down1	;  no -->
		or P1,SAR_ORmask	; oherwise re-set bit
		rr SAR_ORmask		;  and shift down OR mask
CO_Auto_down2	cp SAR_ORmask,#080h	; done?
		jr Z, CallOffset4	;  yes, write down result
		jr CO_Auto_down
CO_Auto_down1	rr SAR_ORmask		; shift down OR mask
		jr CO_Auto_down2
;
; DAC value is too low, increase
CO_Auto_up	or P1,#OffsetDacSign	; plus
CO_Auto_up3	and P1,SAR_ANDmask	; clear bit
		rr SAR_ANDmask		; and shift down AND mask
		call Delay
		tm P2,#PosErr_Cmp	; too low (P2.2 = 0)?
		jr Z, CO_Auto_up1	;  yes -->
		or P1,SAR_ORmask	; oherwise re-set bit
		rr SAR_ORmask		;  and shift down OR mask
CO_Auto_up2	cp SAR_ORmask,#080h	; done?
		jr Z, CallOffset4	;  yes, write down result
		jr CO_Auto_up3
CO_Auto_up1	rr SAR_ORmask		; shift down OR mask
		jr CO_Auto_up2

CallOffset4	and P3,#0FFh-SrvoRdy	; clear ServoRdy
		and P2,#0FFh-Mux3D0	
		or P2,#U2Hfast		; set P2.5 (U2H fast)
CallOffset_Err	ld DelayL,#15
		ld DelayH,#1
		call Delay
		and CmdByte0,#0EFh	; clear offset request from command
		clr CmdByte2
		ei     
		ret    


; *******************************************************************
;  Communications
; *******************************************************************
;
; calculate command checksum, result in Temp1
;
CalcChkSum	ld Temp1,CmdByte0
		add Temp1,CmdByte1
		add Temp1,CmdByte2
		add Temp1,CmdByte3
		com Temp1
		ret 
   

; receive host command and return the result in 21:24h
; last command is moved to 25:28h, current to 21:24h.
;
RxCmdBytes	ld P3M,#041h		; parity off, SIO active, all pins I/O
		ld PRE0,#005h		; CPUclk/1, continuous count
		ld T0,SioBaud		; 3 for 19k2 or 1 for 57k6
SM_ComState	di     
		and IRQ,#027h		; clear serial interrupts
		ld BytePtr,#CmdByte0	; first command byte in 04h
		or TMR,#00Bh		; enable T1, T0 and load T0
		ei     
		or P3,#SioRdy
		ld LastPCmd0,LastRCmd0	; move last received into last processed
		ld LastPCmd1,LastRCmd1
		ld LastPCmd2,LastRCmd2
		ld LastPCmd3,LastRCmd3
WaitSioIrq	cp SM_State,#10		; skip when error state
		jr Z, ComptSio
SaveSioData	tm IRQ,#008h		; wait for serial in interrupt
		jr Z, WaitSioIrq
		and P3,#0FFh-SrvoErr	; clear ServoErr
		ld @BytePtr,SIO		; get cmd from SIO
		inc BytePtr		; increment memory location
		cp BytePtr,#CmdByte4+1
		jr Z, TestChkSum	; leave when we got all five
		and IRQ,#0F7h		; acknowledge interrupt
		jr SaveSioData		; and get next one
; all data collected
TestChkSum	and IRQ,#0F7h		; acknowledge last interrupt	
		call CalcChkSum
		cp Temp1,CmdByte4	; checksum ok?
		jr Z, ComptSio		;  yes, valid command received
		or P3,#SrvoErr		;  otherwise set ServoErr
		jr SM_ComState		;  and retry
; we got a valid command
ComptSio	and P3,#0FFh-SioRdy	; clear SioRdy
		ld LastRCmd0,CmdByte0	; update last received
		ld LastRCmd1,CmdByte1
		ld LastRCmd2,CmdByte2
		ld LastRCmd3,CmdByte3
		ld P3M,#001h		; SIO inactive
		ret    


; send status back to host
;
TxStatusBytes	ld P3M,#41h		; parity off, SIO active, all pins I/O
		di     
		and IRQ,#027h		; clear serial interrupts
		ld BytePtr,#CmdByte0	; first byte to transmit
		or TMR,#003h		; load and enable T0
		ei     
		and P3,#0FFh-SrvoErr	; clear SrvoErr
		or P3,#SioRdy		; set SioRdy
		ld DelayL,#019h
		ld DelayH,#001h
		call Delay
		ld SPH,CmdByte3		; remember 57k6 flag
		ld CmdByte0,StatusByte0
		ld CmdByte1,StatusByte1
		ld CmdByte2,StatusByte2
		ld CmdByte3,StatusByte3
		call CalcChkSum		; calculate checksum
		ld CmdByte4,Temp1	;  and store in CmdByte4
		ld DelayL,#001h
		ld DelayH,#001h
TxStB_Lp2	ld SIO,@BytePtr
TxStB_Lp1	tcm IRQ,#010h		; wait for serial out interrupt
		jr NZ, TxStB_Lp1
		and IRQ,#0EFh		; acknowledge interrupt
		call Delay
		inc BytePtr
		cp BytePtr,#CmdByte4+1	; last one sent?
		jr NZ, TxStB_Lp2	;  no, do next byte
;
		and P3,#0FFh-SioRdy	; clear SioRdy
		and IRQ,#0EFh		; acknowledge last interrupt
		clr CmdByte3
		clr CmdByte2
		clr CmdByte0
		clr CmdByte1
		clr LastPCmdAbort
		ld P3M,#001h		; SIO inactive
		ret    


; wait, time given in DelayL:H 
;
Delay		ld Temp1,DelayH
DelayLoop2	ld Temp2,DelayL
DelayLoop1	dec Temp2
		jr NZ, DelayLoop1
		dec Temp1
		jr NZ, DelayLoop2
		ret    


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


; called from State 8 (Access)
L684		ld 0Ch,CmdByte0		; get magnitude MSB
		and 0Ch,#003h		;  mask out siginficant bits
		ld 0Dh,CmdByte1		; get magnitude LSB
		tcm 0Ch,#003h		; MSB zero?
		jr Z, L69c		;  no, go on
		tm 0Dh,#0FCh		; LSB < 3?
		jr NZ, L69c		;  no, go on
		ld 0Ch,0Dh
		jr L69f
;
L69c		call L6e6
L69f		ld 0Fh,0Ch
		tm 0Ch,#003h		; MSB zero?
		jr Z, L6be		;  yes -->
		tm CmdByte0,#004h	; access direction reverse?
		jr Z, L6b6		;  yes -->
L6ac		rl 0Eh			; preset to 0C9h before recal
		rl 0Eh
		dec 0Fh
		jr Z, L6be
		jr L6ac
;
L6b6		rr 0Eh
		rr 0Eh
		dec 0Fh
		jr NZ, L6b6
L6be		ld R15,0Eh
		and R15,#0C0h		; mask out bit 6+7
		ld 10h,R15
		cp 10h,#0C0h		
		jr Z, Clr_OddEven
		cp 10h,#080h
		jr Z, Clr_OddEven
		cp 10h,#0
		jr Z, Set_OddEven
		cp 10h,#040h
		jr Z, Set_OddEven
		ld SM_State,#10		; set error state (should never happen)
		ret    
;
Clr_OddEven	and P2,#0FFh-OddEven
		jr Clr_OE1
Set_OddEven	or P2,#OddEven
Clr_OE1		ret
    
L6e6		ld 0Ah,#008h
		ld 0Bh,#004h
		cp 0Bh,0Ch
		jr ugt,L6f3
		scf    
		ret   
 
L6f3		rlc 0Dh
		rlc 0Ch
		jr C,L6fe
		cp 0Bh,0Ch
		jr ugt,L702
L6fe		sub 0Ch,0Bh
		scf    
L702		dec 0Ah
		jr NZ, L6f3
		rlc 0Dh
		ret    


; called from State 8 (Access)
;  get DAC value from table indexed by R3 (seek magnitude)
;  use fixed value 8 (017h) when R13 = 1
;  return P1 contents merged with new DAC value in P1_Mask
;
CurveTable  	DB 0E0h, 0E0h, 0E0h, 0E0h
		DB 0E0h, 0E1h, 0E1h, 0E2h
		DB 0E2h, 0E3h, 0E3h, 0E4h
		DB 0E5h, 0E5h, 0E6h, 0E7h
		DB 0E7h, 0E8h, 0E9h, 0EAh
		DB 0Ebh, 0ECh, 0EDh, 0EEh
		DB 0EFh, 0F0h, 0F1h, 0F3h
		DB 0F4h, 0F6h, 0F8h, 0FFh

GetCurveValue	srp #030h
	assume RP:030h
		cp R13,#1			; R13 = 1?
		jr Z, GetCV_1			;  --> load DAC with 0F7h
;
		ld Temp1,R3
		ld R0,#CurveTable /256
		ld R1,#CurveTable #256
		ld SPH,#01Fh
		sub SPH,Temp1
		add R1,SPH			; calculate table index
		jr NC,GetCV_2
		inc R0
GetCV_2		ldc R5,@RR0			; get DAC value from table
		dec R0				; ??
		ld Temp2,R5
GetCV_4		ld Temp1,P1			; get P1 value
		and Temp1,#0FFh-OffsetDacZero	; remove DAC bits
		and Temp2,#OffsetDacZero
		or Temp2,Temp1			;  and merge in Temp2
		ld P1_Mask,Temp2		; store DAC value in P1_Mask
		jr GetCV_3			;  and leave
GetCV_1		ld Temp2,#0FFh-8		; set DAC to 8
		jr GetCV_4
GetCV_3		ret    


; *******************************************************************
;  interrupt 0-3
; *******************************************************************
;
HW_Interrupt	di     
		ld IMR,#010h
		and P2,#0FEh		; clear P2.0
		and IRQ,#0FDh
		ei     
		iret 
  

; *******************************************************************
;  interrupt 4: access timeout
; *******************************************************************
;
T0_Interrupt	tcm SM_State,#3		; Start Recal Motion
		jr Z, T0_Int_3489
		tcm SM_State,#4		; Recal Final Approach
		jr Z, T0_Int_3489
		tcm SM_State,#8		; Access State
		jr Z, T0_Int_3489
		cp SM_State,#9		; Seek Final Approach
		jr Z, T0_Int_3489
		call SaveErrState
		iret   
; State 3, 4, 8, 9: decrement MSB and return
T0_Int_3489	dec T0_MSB
		jr Z, T0_Int1
		and IRQ,#0EFh		; acknowledge T0 int
		iret   
T0_Int1		call SaveErrState
		iret   


; *******************************************************************
;  interrupt 5: track pulse counter
; *******************************************************************
;
T1_Interrupt	cp SM_State,#6
		jr Z, T1_Int_6
		call SaveErrState
		iret   
; State 6
T1_Int_6	; (code deleted)
		call SaveErrState
		iret  
 
; save error status
SaveErrState	and TMR,#003h		; load & enable T0
		ld StatusByte0,P0
		ld StatusByte1,P1
		ld StatusByte2,P2
		ld StatusByte3,IRQ
		ld SM_FaultState,SM_State	; remember faulty state
		ld SM_State,#10		; set Error State
		and P3,#0FFh-SrvoRdy	; clear ServoRdy
		or P3,#SrvoErr		;  and set ServoErr
		and IRQ,#0CFh		; acknowledge T0 and T1 int
		ret    



	if $>ROMsize
		error "\aROM size exceeded !!!"
	elseif
		DB ROMsize-$ dup(0FFh)	; pad with $FF's
	endif
	end
