$ PROCESSOR (214)
$ TITLE      ('uPD72K2 FOREGROUND MONITOR')

;******************************************************************
;
;    EMULATION FOREGROUND MONITOR FOR 64759 EMULATOR
;
; MKT:@(#) 64759-18019 A.00.03 78K2 EMULATION FOREGROUND MONITOR        18Jul94                       
;
;******************************************************************
;
;    The emulation foreground monitor is the vechile by which the following
;    emulator functions are effected if the 64759 emulator is configured to
;    operate with a foreground monitor.
;
;        read/write target system memory
;        display/modify registers
;        execute user program
;        break away from user program
;
;    The 64759 foreground monitor must start on a 4k byte boundary other
;    than 0.  The desired 4k byte boundary should be specified in
;
;        LOCATE_ADR     EQU    xxxxH
;
;    statement at the start of the monitor.  The address must be specified
;    in the emulator configuration below.
;
;        cf mon=fg..xxxxx
;
;    In this manner, communication between the foreground monitor and the
;    emulator operating software can be established.
;
;        MONITOR VECTOR TABLE
;        MONITOR VARIABLES
;        KEY MONITOR ENTRY BACKGROUND
;        KEY MONITOR ENTRY FOREGROUND
;
;    The "MONITOR VECTOR TABLE" is used exclusively by the emulator to tran-
;    sition into the foreground monitor from reset, from software breakpoints
;    or from emulation generated breaks like the break command or a write to
;    rom.  The monitor vector is not a replacement for the target system's
;    exception vector table.
;
;    The "MONITOR VARIABLES" section contains group of variables that act as
;    communications path between the foreground monitor and the emulation
;    controller.
;
;    The "KEY MONITOR ENTRY BACKGROUND" routines involve a brief stop in the
;    background monitor and therefore cannot be modified or moved with respect
;    to the start of the monitor.
;
;    The "KEY MONITOR ENTRY FOREGROUND" routine is "FG_ENTRY".  This routine 
;    is executed after transition from the background monitor to the foreground
;    monitor.
;
;    Note that single stepping is not supported with the foreground monitor.
;
;******************************************************************

LOCATE_ADR	EQU	4000H

;------------------------------------------------------------------

FALSE		EQU	0
TRUE		EQU	1

;------------------------------------------------------------------
	ORG	080H+LOCATE_ADR
CMD_BLK:
CMD_NUM:	DS	2
PARAM1:		DS	2
COUNT:		DS	2
START:		DS	4
SEMA4:		DS	2
MONCHK:		DB	1
CPU_TYPE:	DS	1
INIT_FLG:	DS	1

	ORG     08FH+LOCATE_ADR
ARE_YOU_THERE:	DS	1
RUNNING:	DB	FALSE

	ORG	091H+LOCATE_ADR
IROM_FLG:	DS	1
IROM_SIZE:	DS	1
REGISTERS:
PCreg:		DS	2
PSWreg:		DS	1
SPreg:		DS	2
_BRK_REG:	DS	8	;BRK_REG
_BNK0:		DS	8	;BNK0
_BNK1:		DS	8	;BNK1
_BNK2:		DS	8	;BNK2
_BNK3:		DS	8	;BNK3
MMreg:		DS	1
P6reg:		DS	1
PM6reg:		DS	1
SAVE_PSW:	DB	0
NO_READ:	DW	0FFFFH
RUN_PC:		DW	0

	ORG	0C8H+LOCATE_ADR
ENABLE_EXTEND:	DB	01H

;------------------------------------------------------------------
;	CPU INITIALIZE DATA TABLE

	ORG	102H+LOCATE_ADR
IMC_TBL:
		DB	000H,001H,000H		; 0 "uPD78210"
		DB	002H,013H,000H		; 1 "uPD78212"
		DB	000H,013H,000H		; 2 "uPD78213"
		DB	008H,013H,000H		; 3 "uPD78214"
		DB	000H,013H,000H		; 5 "uPD78217"
		DB	080H,013H,000H		; 6 "uPD78218"
		DB	000H,013H,001H		; 7 "uPD78220"
		DB	008H,013H,001H		; 8 "uPD78224"
		DB	000H,013H,000H		; A "uPD78233"
		DB	008H,013H,000H		; B "uPD78234"
		DB	000H,013H,000H		; D "uPD78237"
		DB	080H,013H,000H		; E "uPD78238"

;	Interupt vecter initialize
INT_TBL40:		;vector code register
		DB	03H,04H,05H,06H,0AH,0BH,0CH,0DH,0EH,07H,08H,09H
		DB	10H,11H,12H,13H,14H,15H,00H,00H,00H,00H,00H,00H

INT_TBL80:		;macro service mode register
		DB	0C8H,0C6H,0C4H,0C2H,0D0H,0CEH,0D8H,0D6H,0CAH,0D4H
		DB	0D2H,0CCH,0C0H,0DEH,0DCH,0DAH,0BEH,0C0H,00H,00H
		DB	00H,00H, 00H, 00H

INT_TBLC0:		;macro service mode register
		DB	1CH,1AH,54H,18H,10H,12H,14H,1CH,16H,17H,6AH,15H
		DB	8AH,8CH,8EH,86H,78H,78H,00H,00H,00H,00H,00H,00H

IMC0		EQU	0FFC8H
IMC1		EQU	0FFC9H
EMMOD0		EQU	0FFCCH
SFRADR		EQU	0FF79H
MODE_REG	EQU	0FFFAH
REG_PM6		EQU	0FF26H

;------------------------------------------------------------------
; Monitor Commands
;
CMDEXIT 	EQU	01H
CMDRDMEM	EQU	02H
CMDWRMEM	EQU	03H
CMDRDREG	EQU	04H
CMDWRREG	EQU	05H
CMDRDIO 	EQU	06H
CMDWRIO 	EQU	07H
CMDWAITRST 	EQU	08H
CMDRDCMB	EQU	09H
;
NO_CMD		EQU	0
NEW_CMD 	EQU	1
CMD_STARTED	EQU	2
CMD_DONE	EQU	3

;------------------------------------------------------------------
; Monitor Status
;
MON_LOOP	EQU	11H
WAIT_RST	EQU	33H
;
SVI_PCL		EQU	0005H
SVI_X		EQU	0004H
SVI_A		EQU	0003H
SVI_PCH		EQU	0002H
SVI_PSW		EQU	0001H
_X		EQU	0
_A		EQU	1
_C		EQU	2
_B		EQU	3
_E		EQU	4
_D		EQU	5
_L		EQU	6
_H		EQU	7
BNK0_AREA	EQU	0FEF8H
BNK1_AREA	EQU	0FEF0H
BNK2_AREA	EQU	0FEE8H
BNK3_AREA	EQU	0FEE0H
;
MON_STACK	EQU	0200H+LOCATE_ADR
DATA_BUF	EQU	0200H+LOCATE_ADR
CMB_BUF		EQU	00FFH+LOCATE_ADR

;******************************************************************
_BLOCK	MACRO	DADR,SADR,CTR
	LOCAL	BL_LOOP
	MOVW    HL,DADR
	MOVW    DE,SADR
	MOV     B,CTR
BL_LOOP:
	MOV	A,[DE+]
	MOV	[HL+],A
	DBNZ	B,$BL_LOOP
	ENDM

_FILLB	MACRO	ADR,DAT,CTR
	LOCAL	FI_LOOP
	MOVW	HL,ADR
	MOV	A,DAT
	MOV	B,CTR
FI_LOOP:
	MOV	[HL+],A
	DBNZ	B,$FI_LOOP
	ENDM

;******************************************************************
;JUMP TABLE
	ORG	100H+LOCATE_ADR
	DW	300H+LOCATE_ADR

;******************************************************************
;* KEY MONITOR ENTRY BACKGROUND
;******************************************************************
	ORG	300H+LOCATE_ADR

BKG_ENTRY:
	MOV	A,!INIT_FLG	; entry from host reset state?
	CMP	A,#0
	BZ	$SAVE_STACK
;
; initialization at reset
;
	MOV	MM,#20H 	; MM register value
	MOV	A,!CPU_TYPE
	AND	A,#0FH		; check BNK0,1
	MOV	B,#3		;
	MULU	B		; A = [CPUSEL_REG] * 3
	ADDW	AX,#IMC_TBL	;
	MOVW	HL,AX		; HL = CPU_TABLE+[CPUSEL_REG]*4
	MOV	A,[HL+]		; IMC0 initialization (ROM size)
	MOV	!IMC0,A
	MOV	A,[HL+]		; IMC1 initialization (RAM size)
	MOV	!IMC1,A
	MOV	A,[HL]
	MOV	!SFRADR,A
	MOV	A,#00H
	MOV	!EMMOD0,A
	MOV	A,!IROM_FLG
	CMP	A,#0
	BZ	$IROM_NOCHANGE
;
	MOV	A,!IROM_SIZE
	MOV	!IMC0,A			; set IROM size

IROM_NOCHANGE:
	MOVW	SP,#MON_STACK
	CALL	!INT_INIT
	BR	$TO_FG
;
SAVE_STACK:
	MOV	A,!EMMOD0
	AND	A,#0FEH
	MOV	!EMMOD0,A
	MOV	A,!SVI_PSW
	MOV	!PSWreg,A
	MOV	A,!SVI_PCH
	MOV	!PCreg+1,A
	MOV	A,!SVI_A
	MOV	!_BRK_REG+1,A
	MOV	A,!SVI_X
	MOV	!_BRK_REG,A
	MOV	A,!SVI_PCL
	MOV	!PCreg,A
	MOVW	AX,SP
	MOV	!SPreg+1,A
	MOV	A,X
	MOV	!SPreg,A
	MOVW	SP,#MON_STACK
;
TO_FG:
	MOV	A,PM6
	MOV	!PM6reg,A
	AND	PM6,#0F0H
	MOVW	AX,#FG_ENTRY
	MOV	!SVI_PCH,A
	MOV	A,X
	MOV	!SVI_PCL,A
	MOV     A,#0FFH		
	MOV	!CMB_BUF,A
	NOP
	NOP
	NOP
	DB	04H

;******************************************************************
;* END OF KEY MONITOR ENTRY BACKGROUND
;******************************************************************

;******************************************************************
;* KEY MONITOR ENTRY FOREGROUND
;******************************************************************
FG_ENTRY:
	MOV	A,!INIT_FLG
	CMP	A,#00H
	BZ	$FG_BREAK
;
	MOV	A,#00H
	MOV	!INIT_FLG,A
	_FILLB	#REGISTERS,#0,#30H
	MOV	A,#02H
	MOV	!PSWreg,A	; initialize PSW
	BR	!MONITOR_LOOP

FG_BREAK:
	MOV	A,!PSWreg
	MOV	!SAVE_PSW,A	;save L
	MOVW	AX,HL
	MOVW	HL,#_BRK_REG	;save current bank of registers
	MOV	[HL+_H],A	;save H
	MOV	A,X
	MOV	[HL+_L],A	;save L
	MOV	A,B
	MOV	[HL+_B],A	;save B
	MOV	A,C
	MOV	[HL+_C],A	;save C
	MOV	A,D
	MOV	[HL+_D],A	;save D
	MOV	A,E
	MOV	[HL+_E],A	;save E
	MOV	A,!SAVE_PSW
	AND	A,#28H		;check BNK0,1
	CMP	A,#00H
	BZ	$SAVE_BNK0	;current bank=0
	CMP	A,#08H
	BZ	$SAVE_BNK1	;	     =1
	CMP	A,#20H
	BZ	$SAVE_BNK2	;	     =2
	CMP	A,#28H
	BR	!SAVE_BNK3	;	     =3
;	
SAVE_BNK0:
;Save BNK0 of registers
	_BLOCK	#_BNK0,#_BRK_REG,#8	; current
	_BLOCK	#_BNK1,#BNK1_AREA,#8	; BNK1
	_BLOCK	#_BNK2,#BNK2_AREA,#8	; BNK2
	_BLOCK	#_BNK3,#BNK3_AREA,#8	; BNK3
	BR	!SEND

SAVE_BNK1:
;save BNK1 of registers
	_BLOCK	#_BNK1,#_BRK_REG,#8	; current
	_BLOCK	#_BNK0,#BNK0_AREA,#8	; BNK0
	_BLOCK	#_BNK2,#BNK2_AREA,#8	; BNK2
	_BLOCK	#_BNK3,#BNK3_AREA,#8	; BNK3
	BR	$SEND

SAVE_BNK2:
;save BNK2 of registers
	_BLOCK	#_BNK2,#_BRK_REG,#8	; current
	_BLOCK	#_BNK0,#BNK0_AREA,#8	; BNK0
	_BLOCK	#_BNK1,#BNK1_AREA,#8	; BNK1
	_BLOCK	#_BNK3,#BNK3_AREA,#8	; BNK3
	BR	$SEND

SAVE_BNK3:
;save BNK3 of registers
	_BLOCK	#_BNK3,#_BRK_REG,#8	; current
	_BLOCK	#_BNK0,#BNK0_AREA,#8	; BNK0
	_BLOCK	#_BNK1,#BNK1_AREA,#8	; BNK1
	_BLOCK	#_BNK2,#BNK2_AREA,#8	; BNK2
SEND:
;******************************************************************
;* END OF KEY MONITOR ENTRY FOREGROUND
;******************************************************************

MONITOR_LOOP:
	MOV	A,!SEMA4	;read semaphore status
	CMP	A,#NEW_CMD
	BZ	$NEW_COMMAND
;
	MOV	A,#MON_LOOP
	MOV	!ARE_YOU_THERE,A
	BR	$MONITOR_LOOP

;******************************************************************
NEW_COMMAND:
	MOV	A,#CMD_STARTED
	MOV	!SEMA4,A

	MOV	A,!CMD_NUM
	CMP	A,#CMDEXIT
	BZ	$D_CMDEXIT
	CMP	A,#CMDRDMEM
	BZ	$D_CMDRDMEM
	CMP	A,#CMDWRMEM
	BZ	$D_CMDWRMEM
	CMP	A,#CMDRDREG
	BZ	$D_CMDRDREG
	CMP	A,#CMDWRREG
	BZ	$D_CMDWRREG
	CMP	A,#CMDWAITRST
	BZ	$D_CMDWAITRST

D_CMDEXIT:	BR	!DO_EXIT
D_CMDRDMEM:	BR	!DO_RDMEM
D_CMDWRMEM:	BR	!DO_WRMEM
D_CMDRDREG:	BR	!DO_RDREG
D_CMDWRREG:	BR	!DO_WRREG
D_CMDWAITRST:	BR	!DO_WAITRST

CMD_FINISHED:
	MOV	A,#CMD_DONE
	MOV	!SEMA4,A

TO_LOOP:
	BR	!MONITOR_LOOP

;********************************************************************
DO_EXIT:
	MOV	A,!START+2	;get pc passed to monitor
	MOV	X,A
	MOV	A,!START+3	;get pc passed to monitor
	MOVW	DE,AX		;get pc use flag
	MOV	A,!COUNT	;use passed pc if flag = 0
	CMP	A,#0
	BZ	$SETUP_PC

RESUME:
	MOV	A,!PCreg	;get current PC form register area
	MOV	X,A
	MOV	A,!PCreg+1
	MOVW	DE,AX

SETUP_PC:
	MOVW	AX,DE
	MOV	!RUN_PC+1,A
	MOV	A,X
	MOV	!RUN_PC,A
	MOV	A,#TRUE
	MOV	!RUNNING,A
	MOV	A,!SAVE_PSW
	AND	A,#28H		;check BNK0,1
	CMP	A,#00H
	BZ	$LOAD_BNK0	;current bank=0
	CMP	A,#08H
	BZ	$LOAD_BNK1	;current bank=1
	CMP	A,#20H
	BZ	$LOAD_BNK2	;current bank=2;
	BR	!LOAD_BNK3	;current bank=3;

LOAD_BNK0:
;load  registers(BNK1/2/3)
	_BLOCK	#BNK1_AREA,#_BNK1,#8
	_BLOCK	#BNK2_AREA,#_BNK2,#8
	_BLOCK	#BNK3_AREA,#_BNK3,#8
	BR	!EXIT

LOAD_BNK1:
;load  registers(BNK0/2/3)
	_BLOCK	#BNK0_AREA,#_BNK0,#8
	_BLOCK	#BNK2_AREA,#_BNK2,#8
	_BLOCK	#BNK3_AREA,#_BNK3,#8
	BR	!EXIT

LOAD_BNK2:
;load  registers(BNK0/1/3)
	_BLOCK	#BNK0_AREA,#_BNK0,#8
	_BLOCK	#BNK1_AREA,#_BNK1,#8
	_BLOCK	#BNK3_AREA,#_BNK3,#8
	BR	!EXIT

LOAD_BNK3:
;load  registers(BNK0/1/2)
	_BLOCK	#BNK0_AREA,#_BNK0,#8
	_BLOCK	#BNK1_AREA,#_BNK1,#8
	_BLOCK	#BNK2_AREA,#_BNK2,#8

EXIT:
;load the stack of user
	MOV	A,!SPreg
	MOV	X,A
	MOV	A,!SPreg+1
	MOVW	DE,AX		;set RETB stack

	DECW	DE
	MOV	A,!PSWreg
	MOV	[DE-],A
	MOV	A,!RUN_PC+1
	MOV	[DE-],A
	MOV	A,!RUN_PC
	MOV	[DE-],A
	MOV	A,!PM6reg		;stack P6 reg
	MOV	[DE],A
	MOVW	AX,DE
	MOVW	SP,AX
	
	MOVW	HL,#_BRK_REG
	MOV	A,[HL+_B]		;load BC
	MOV	B,A
	MOV	A,[HL+_C]
	MOV	C,A

	MOV	A,[HL+_D]		;load DE
	MOV	D,A
	MOV	A,[HL+_E]
	MOV	E,A

	MOV	A,[HL+_L]		;load HL
	MOV	X,A
	MOV	A,[HL+_H]
	MOVW	HL,AX

	MOV	A,!_BRK_REG+_X
	MOV	X,A
	MOV	A,!_BRK_REG+_A
	POP	PM6
	RETB

;********************************************************************
DO_RDMEM:
	MOV	A,!START+0	; get target bank number
	AND	A,#0FH
	MOV	C,A
	MOV	A,!START+2	; get target address
	MOV	X,A
	MOV	A,!START+3
	MOVW	HL,AX
	MOV	A,!COUNT	; get number of bytes to read
	MOV	B,A
	MOVW	AX,#DATA_BUF	; get xfer buffer address
	MOVW	DE,AX

RDMEM_LOOP:
	MOV	A,!ENABLE_EXTEND
	CMP	A,#00H
	BZ	$RDMEM_1
;
	MOV	A,MM
	MOV	!MMreg,A
	MOV	A,P6
	MOV	!P6reg,A
	OR	MM,#47H
	MOV	A,C
	MOV	P6,A

RDMEM_1:
	MOV	A,C
	CMP	A,#00H
	BNZ	$RDMEM_2
;
	MOVW	AX,HL
	CMPW	AX,#REG_PM6
	BNZ	$RDMEM_3
;
	MOV	A,!PM6reg
	MOV	[DE+],A
	INCW	HL
	BR	$RDMEM_4

RDMEM_2:
	MOV	A,&[HL+]
	MOV	[DE+],A
	BR	$RDMEM_4

RDMEM_3:
	MOV	A,[HL+]
	MOV	[DE+],A

RDMEM_4:
	MOV	A,!ENABLE_EXTEND
	CMP	A,#00H
	BZ	$RDMEM_NEXT
;
	MOV	A,!MMreg
	MOV	MM,A
	MOV	A,!P6reg
	MOV	P6,A

RDMEM_NEXT:
	DEC	B
	BZ	$RDMEM_END
;
	MOVW	AX,HL
	CMPW	AX,#0000H
	BNZ	$RDMEM_LOOP
;
	MOV	A,C
	INC	A
	AND	A,#0FH
	MOV	C,A
	BR	$RDMEM_LOOP

RDMEM_END:
	BR	!CMD_FINISHED

;********************************************************************
DO_WRMEM:
	MOV	A,!START+0	; get target bank number
	AND	A,#0FH
	MOV	C,A
	MOV	A,!START+2	; get target address
	MOV	X,A
	MOV	A,!START+3
	MOVW	HL,AX
	MOV	A,!COUNT	; get number of bytes to read
	MOV	B,A
	MOVW	AX,#DATA_BUF	; get xfer buffer address
	MOVW	DE,AX

WRMEM_LOOP:
	MOV	A,!ENABLE_EXTEND
	CMP	A,#00H
	BZ	$WRMEM_1
;
	MOV	A,MM
	MOV	!MMreg,A
	MOV	A,P6
	MOV	!P6reg,A
	OR	MM,#47H
	MOV	A,C
	MOV	P6,A

WRMEM_1:
	MOV	A,C
	CMP	A,#00H
	BNZ	$WRMEM_2
;
	MOVW	AX,HL
	CMPW	AX,#REG_PM6
	BNZ	$WRMEM_3
;
	MOV	A,[DE+]
	MOV	!PM6reg,A
	AND	A,#0F0H
	MOV	PM6,A
	INCW	HL
	BR	$WRMEM_4

WRMEM_2:
	MOV	A,[DE+]
	MOV	&[HL+],A
	BR	$WRMEM_4

WRMEM_3:
	MOV	A,[DE+]
	MOV	[HL+],A

WRMEM_4:
	MOV	A,!ENABLE_EXTEND
	CMP	A,#00H
	BZ	$WRMEM_NEXT
;
	MOV	A,!MMreg
	MOV	MM,A
	MOV	A,!P6reg
	MOV	P6,A

WRMEM_NEXT:
	DEC	B
	BZ	$WRMEM_END
;
	MOVW	AX,HL
	CMPW	AX,#0000H
	BNZ	$WRMEM_LOOP
	MOV	A,C
	INC	A
	AND	A,#0FH
	MOV	C,A
	BR	$WRMEM_LOOP

WRMEM_END:
	BR	!CMD_FINISHED

;******************************************************************
DO_RDREG:
	MOVW	HL,#DATA_BUF
	MOV	A,!COUNT	;B <- counter 
	MOV	B,A

RDREG0:
	MOVW	AX,[HL]		;AX <- register address 
	CMPW	AX,#0F00H
	BC	$RDREG1
	ADDW	AX,#0F000H
	CMPW	AX,#REG_PM6
	BNZ	$RDREG2
	MOVW	AX,#PM6reg
	BR	$RDREG2

RDREG1:
	ADDW	AX,#REGISTERS

RDREG2:
	MOVW	[HL],AX
	INCW	HL
	INCW	HL
	DBNZ	B,$RDREG0

	MOVW	HL,#DATA_BUF
	MOVW	DE,HL
	MOV	A,!COUNT
	MOV	B,A

RDREG3:
	MOVW	AX,[HL]
	PUSH	HL
	MOVW	HL,AX
	MOV	A,[HL]
	MOV	[DE+],A
	POP	HL
	INCW	HL
	INCW	HL
	DBNZ	B,$RDREG3
	BR	!CMD_FINISHED

;******************************************************************
DO_WRREG:
	MOVW	DE,#00H
	MOV	C,#00H
WRLOOP:
	MOVW	HL,#PARAM1
	MOVW	AX,[HL]		;set register address offset

	CMPW	AX,#0F00H
	BC	$WRREG0
	MOV	C,#01H		;not current reg
	ADDW	AX,#0F000H	;add sfr register address segment
	CMPW	AX,#REG_PM6
	BNZ	$WRREG1
	MOVW	AX,#PM6reg
	BR	$WRREG1
WRREG0:
	ADDW	AX,#REGISTERS
	ADDW	AX,DE
	
WRREG1:
	MOVW	HL,AX
	MOVW	DE,#DATA_BUF
	MOV	A,!COUNT	;2byte reg or 1byte reg?
	MOV	B,A

	MOVW	AX,HL
	CMPW	AX,#0FF11H	;if CR00 reg then word write	
	BZ	$WRREG3
	CMPW	AX,#0FF13H	;if CR00 reg then word write	
	BZ	$WRREG3
	CMPW	AX,#0FF71H	;if CR00 reg then word write	
	BZ	$WRREG3
	CMPW	AX,#0FF75H	;if CR00 reg then word write	
	BZ	$WRREG3

WRREG2:
	MOV	A,[DE+] 	;byte write registers
	MOV	[HL-],A
	DBNZ	B,$WRREG2
	BR	$WRREG4

WRREG3:
	MOVW	AX,[DE]		;word write registers
	DECW	HL
	XCH	A,X
	MOVW	[HL],AX

WRREG4:
	MOV	A,#00H
	CMP	C,A		;check C(current reg chk flag)
	BNZ	$WRSTEP

	MOVW	HL,#PARAM1
	MOV	A,[HL]		;A <- register No 
	CMP	A,#05		;
	BC	$WRSTEP
	CMP	A,#13		;
	BC	$BRKREG
	CMP	A,#21
	BC	$RBNK0
	CMP	A,#29
	BC	$RBNK1
	CMP	A,#37
	BC	$RBNK2
	CMP	A,#45
	BC	$WRSTEP2	;BC RBNK3
	BR	$WRSTEP

BRKREG:
	MOV	A,!SAVE_PSW	;get bank number
	AND	A,#28H

	CMP	A,#00H
	BZ	$BRKREG0	;jump for BNK0
	CMP	A,#08H
	BZ	$BRKREG1	;jump for BNK1
	CMP	A,#20H
	BZ	$BRKREG2	;jump for BNK2
	MOVW	DE,#32
BRKREG5:
	MOV	C,#1
	BR	!WRLOOP
BRKREG0:
	MOVW	DE,#8
	BR	$BRKREG5
BRKREG1:
	MOVW	DE,#16
	BR	$BRKREG5
BRKREG2:
	MOVW	DE,#24
	BR	$BRKREG5
	
WRSTEP:
	BR	!WREXIT
WRSTEP1:
	BR	!WRREG1
WRSTEP2:
	BR	!RBNK3
RBNK0:

	SUB	A,#13		;
	MOV	E,A
	MOV	A,!SAVE_PSW	
	AND	A,#28H
	CMP	A,#00H
	BNZ	$WREXIT
RBNK00:
	MOV	D,#00H
	MOVW	AX,#_BRK_REG
	ADDW	AX,DE
	MOV	C,#1
	BR	$WRSTEP1
RBNK1:
	SUB	A,#21		;
	MOV	E,A
	MOV	A,!SAVE_PSW	
	AND	A,#28H
	CMP	A,#08H
	BNZ	$WREXIT
	BR	$RBNK00
RBNK2:
	SUB	A,#29
	MOV	E,A
	MOV	A,!SAVE_PSW	
	AND	A,#28H
	CMP	A,#20H
	BNZ	$WREXIT
	BR	$RBNK00
RBNK3:
	SUB	A,#37
	MOV	E,A
	MOV	A,!SAVE_PSW	
	AND	A,#28H
	CMP	A,#28H
	BNZ	$WREXIT
	BR	$RBNK00
WREXIT:
	BR	!CMD_FINISHED

;******************************************************************
DO_WAITRST:
	MOV	A,#CMD_DONE
	MOV	!SEMA4,A

RST_LOOP:
	MOV	A,#WAIT_RST
	MOV	!ARE_YOU_THERE,A
	BR	RST_LOOP

INT_INIT:
;interrpt vector initialize
	MOV	A,#40H
	MOV	!MODE_REG,A
	_BLOCK	#0FFA0H,#INT_TBL40,#24	;VC initialize

	MOV	A,#80H
	MOV	!MODE_REG,A
	_BLOCK	#0FFA0H,#INT_TBL80,#24	;MSM initialize

	MOV	A,#0C0H
	MOV	!MODE_REG,A
	_BLOCK	#0FFA0H,#INT_TBLC0,#24	;MSA initialize

	MOV	A,#00H
	MOV	!MODE_REG,A		;initialize end

	_FILLB	#SVI_PCH,#00,#04	;svi area clear
	_FILLB	#REGISTERS,#00,#32H	;register clear

	MOV	A,#00H
	MOV	!IROM_FLG,A		;clear irom flag
	RET

	END
