
#******************************************************************************
#
#    EMULATION FOREGROUND MONITOR FOR 64758 EMULATOR
#
# MKT:@(#) 64758-18003 A.00.02 70632 EMUL FOREGROUND MONITOR            26Sep90                       
#
#******************************************************************************
#
#    The emulation foreground monitor is the vechile by which the following
#    emulator functions are effected if the 64758 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 64758 foreground monitor must start on a 4k byte boundary.
#    The desired 4k byte boundary should be specified in
#
#        .text  "FG_MON"               >0x0XXXXXXXX
#
#    statement at the start of the monitor.  The address must be specified
#    in the emulator configuration below.
#
#        cf mon=fg..xxxxxxxx,yyyyyyyy
#                                      xxxxxxxx: real location address
#                                      yyyyyyyy: virtual location address
#
#    In this manner, communication between the foreground monitor and the
#    emulator operating software can be established.
#
#
#    The few sections of the foreground monitor cannot be modified and their
#    location with respect to the start of the foreground monitor cannot be
#    altered. These include the following:
#
#        MONITOR RESET BREAK ROUTINE
#        MONITOR VARIABLES
#        MONITOR ENTRY FOREGROUND
#        MONITOR ENTRY BACKGROUND
#
#    The "MONITOR RESET BREAK ROUTINE" is used exclusively by the emulator
#    to transition 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 VARIABLES" section contains three parts.  The first section
#    is a group of variables that act as the communications path between the
#    foreground monitor and the emulation controller.   The second section is
#    the "DATA_BUF" which is used to transfer a block of data between the
#    emulation controller and the target system memory.  The third section is
#    the "FGMON_ADDRESS" which is used to calculate monitor address .  The
#    last section is the "JAM_STACK" which is a stack used by the monitor when 
#    it is in a background state for a few bus cycles upon transition to the
#    foreground monitor.
#
#    The "MONITOR ENTRY FOREGROUND" routine is executed after transition from
#    the background monitor to the foreground monitor.
#
#    The "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.
#
#
#    To perform single stepping , the "Single Step Trap" vector in the target
#    system must point to "STEP_ENTRY" in the foreground monitor.
#
#**********************************************************************#
#	.title "oddyssey n70632 foregound monitor"
#
#
#**********************************************************************#
# MONITOR OFFSET VALUES 
# The controller will access certain values within the monitor.  This
# is accomplished by reading and writing monitor memory at predetermined
# monitor offsets.  The following monitor offsets are defined.
#
#   Location          Monitor Offset
#   --------          --------------
#   REGISTERS             0xc60
#   CMD_NUM               0xd80
#   COUNT                 0xd84
#   START_ADDRESS         0xd88
#   ACCESSMODE            0xd8c
#   RESET_FLAG            0xdac
#   SEMAPHORE             0xdb0
#   ARE_YOU_THERE         0xdb1
#   RUNNING               0xdb2
#   HALT_SUPPORT          0xdb3
#   MON_ENTRY             0xdb4
#   DATA_BUF              0xdc0
#   REAL_ADDRESS          0xf00
#   VIR_ADDRESS           0xf04
#   FG_ADDROFF            0xf08
#   JAM_STACK             0xfa0
#   BREAKENTRY            0xfe0
#   RESETENTRY            0xff0
#**********************************************************************#

	.file "fm64758.s"


#**********************************************************************#
# MONITER CONTERNTS


# Monitoer Command Value - Controller will request one of these commands
	.set	CMDEXIT,	1
	.set	CMDRDMEM,	2
	.set	CMDWRMEM,	3

	.set	CMDWAITRST,	6

	.set	CMDRDIO,	8
	.set	CMDWRIO,	9


# Semaphore values - For Monitor / Controller Synhronization
	.set	NO_CMD,		0
	.set	NEW_CMD,	1
	.set	CMD_STARTED,	2
	.set	CMD_FINISHED,	3
	
# Monitor status - Are_you_there values
	.set	RESPOND,	0
	.set	MON_LOOP,	0x12
	.set	WAIT_CMB,	0x34
	.set	WAIT_RST,	0x56
	.set	EXIT_MON,	0x78

# Access modes - For Target memory
	.set	BYTEACCESS,	1
	.set	HALFWORDACCESS,	2
	.set	WORDACCESS,	4

# IOB signals bit positions
	.set	OKTORUN,	4
	.set	ANYBRK,		0
# Other contents
	.set	FALSE,		0
	.set	TRUE,		1

# Regster Mask data for FG-monitor
	.set	PSW_MASK,	0x0c4041f0f
	.set	PSW_VAL,	0x010000000
	.set	TKCW_MASK,	0x00000ff01
	.set	TKCW_VAL,	0x00000e000
	.set	SYCW_MASK,	0x0000003f7
	.set	SYCW_VAL,	0x000000070

########################################################
##### uPD70632 FOREGROUND MONITOR ######################
########################################################
	.text	"FG_MON">0x000000000

MONITOR_START:
	.word	0
	.word	0
	.word	NMI_ROUTINE
	.word	SERIOUS_ENTRY
	.word	SYSTEM_ENTRY
	.word	0
	.word	0
	.word	L0STAK_ENTRY
	.word	AREA_ENTRY
	.word	PAGE_ENTRY
	.word	MEMORY_ENTRY
	.word	ADDR_TRN_ENTRY
	.word	STEP_ENTRY
	.word	BRK_ENTRY
	.word	ADDR_ENTRY
	.word	0
	.word	RSV_INST_ENTRY
	.word	PRIVILEG_ENTRY
	.word	RSV_ADDR_ENTRY
	.word	ILL_ADDR_ENTRY
	.word	ILL_DATA_ENTRY
	.word	INTGER_ENTRY
	.word	FLOAT_ENTRY
	.word	DECIMAL_ENTRY
	.word	EXE0_ENTRY
	.word	EXE1_ENTRY
	.word	EXE2_ENTRY
	.word	EXE3_ENTRY
	.word	AST_ENTRY
	.word	ATT_ENTRY

	.word	0
	.word	0
	.word	0
	.word	0
	.word	0
	.word	0
	.word	0
	.word	0
MON_STACK:

########################################################
##### NMI routine for background #######################
########################################################
	.align	4
NMI_ROUTINE:
	add.w	#0x8,r31
	jr	BGBG

########################################################
##### Monitor Target Memory Read #######################
########################################################
READ_MEMORY:
	mov.b	#CMD_STARTED,SEMAPHORE
	mov.w	START_ADDRESS,r0
	stpr	#7,r1
	and.w	#0x1,r1
	cmp.w	#0,r1			-- Check virtual mode
	jz	SET_READ_VALUE

SET_READ_VADDR:				-- This is virtual mode
	mov.w	r0,r1
	and.w	#0x0fffff000,r1
	or.w	#0x0e05,r1		-- Generate PTE data
	and.w	#0x000000fff,r0
	or.w	VIR_ADDR,r0
	add.w	#0x1000,r0		-- Generate PTE address
	mov.w	#0x0ffffffff,r28
	updpte	r0,r1			-- Set PTE date

SET_READ_VALUE:
	movea.w	DATA_BUF,r1
	mov.w	ACCESSMODE,r2
	mov.w	COUNT,r3

	cmp.w	r2,#BYTEACCESS
	je	RD_MEM_B

	div.w	#2,r3
	cmp.w	r2,#HALFWORDACCESS
	je	RD_MEM_H

	div.w	#2,r3
	jr	RD_MEM_W


RD_MEM_B:
	mov.b	[r0+],[r1+]
	dbr	r3,RD_MEM_B
	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

RD_MEM_H:
	mov.h	[r0+],[r1+]
	dbr	r3,RD_MEM_H
	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

RD_MEM_W:
	mov.w	[r0+],[r1+]
	dbr	r3,RD_MEM_W
	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

########################################################
##### Monitor Target Memory Write ######################
########################################################
WRITE_MEMORY:
	mov.b	#CMD_STARTED,SEMAPHORE
	mov.w	START_ADDRESS,r0
	stpr	#7,r1
	and.w	#0x1,r1
	cmp.w	#0,r1			-- Check virtual mode
	jz	SET_WRITE_VALUE

SET_WRITE_VADDR:			-- This is virtual mode
	mov.w	r0,r1
	and.w	#0x0fffff000,r1
	or.w	#0x0e05,r1		-- Generate PTE data
	and.w	#0x000000fff,r0
	or.w	VIR_ADDR,r0
	add.w	#0x1000,r0		-- Generate PTE address
	mov.w	#0x0ffffffff,r28
	updpte	r0,r1			-- Set PTE data

SET_WRITE_VALUE:
	movea.w	DATA_BUF,r5
	mov.w	ACCESSMODE,r2
	mov.w	COUNT,r3

	cmp.w	r2,#BYTEACCESS
	je	WR_MEM_B

	div.w	#2,r3
	cmp.w	r2,#HALFWORDACCESS
	je	WR_MEM_H

	div.w	#2,r3
	jr	WR_MEM_W


WR_MEM_B:
	mov.b	[r5+],[r0+]
	dbr	r3,WR_MEM_B

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP


WR_MEM_H:
	mov.h	[r5+],[r0+]
	dbr	r3,WR_MEM_H

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP


WR_MEM_W:
	mov.w	[r5+],[r0+]
	dbr	r3,WR_MEM_W

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

########################################################
##### Monitor Target IO Read ###########################
########################################################
READ_IO:
	mov.b	#CMD_STARTED,SEMAPHORE
	mov.w	START_ADDRESS,r0
	movea.w	DATA_BUF,r5
	mov.w	ACCESSMODE,r2
	mov.w	COUNT,r3

	cmp.w	r2,#BYTEACCESS
	je	RD_IO_B

	div.w	#2,r3
	cmp.w	r2,#HALFWORDACCESS
	je	RD_IO_H

	div.w	#2,r3
	jr	RD_IO_W


RD_IO_B:
	in.b	[r0+],[r5+]
	dbr	r3,RD_IO_B

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

RD_IO_H:
	in.h	[r0+],[r5+]
	dbr	r3,RD_IO_H

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

RD_IO_W:
	in.w	[r0+],[r5+]
	dbr	r3,RD_IO_W

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

########################################################
##### Monitor Target Memory Write ######################
########################################################
WRITE_IO:
	mov.b	#CMD_STARTED,SEMAPHORE
	mov.w	START_ADDRESS,r0
	movea.w	DATA_BUF,r5
	mov.w	ACCESSMODE,r2
	mov.w	COUNT,r3

	cmp.w	r2,#BYTEACCESS
	je	WR_IO_B

	div.w	#2,r3
	cmp.w	r2,#HALFWORDACCESS
	je	WR_IO_H

	div.w	#2,r3
	jr	WR_IO_W


WR_IO_B:
	out.b	[r5+],[r0+]
	dbr	r3,WR_IO_B

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

WR_IO_H:
	out.h	[r5+],[r0+]
	dbr	r3,WR_IO_H

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

WR_IO_W:
	out.w	[r5+],[r0+]
	dbr	r3,WR_IO_W

	mov.b	#CMD_FINISHED,SEMAPHORE
	jr	MONITOR_LOOP

########################################################
##### RUN FROM RESET ###################################
########################################################
RUN_RESET:
	mov.b	#WAIT_RST,ARE_YOU_THERE
	jr	RUN_RESET

							
########################################################
##### Interrrupt and Exception Entry Program ###########
########################################################
# 
	.align 4

################################
# Non Maskable Interrupt entry #
################################
					-- NMI user stack info

					--      +---------------------+
					-- + 4	|         PSW         |
					--      +---------------------+
					--   0	|      PC( Next )     |
					--      +---------------------+

NMI_ENTRY:
	mov.w	#0x01000000,MON_ENTRY	-- Set NMI_ENTRY_CODE
	mov.w	[r31+],TPC		-- Read PC,PSW from user stack
	mov.w	[r31+],TPSW
	jr	I_E_ENTRY

	.align 4

###################
# Interrupt entry #
###################
					-- INT user stack info

					--      +---------------------+
					-- + 4	|         PSW         |
					--      +---------------------+
					--   0	|      PC( Next )     |
					--      +---------------------+

INT_ENTRY:
	mov.w	#0x02000000,MON_ENTRY	-- Set NMI_ENTRY_CODE
	mov.w	[r31+],TPC		-- Read PC,PSW from user stack
	mov.w	[r31+],TPSW
	jr	I_E_ENTRY

	.align 4

#########################
# Exception type1 entry #
#########################
					-- Exception type1 user stack info

					--      +---------------------+
					-- + 8	| EXCP_CODE|     4    |
					--      +---------------------+
					-- + 4	|         PSW         |
					--      +---------------------+
					--   0	| PC(Current or Next) |
					--      +---------------------+
EXC1_ENTRY:

SYSTEM_ENTRY:
BRK_ENTRY:
STEP_ENTRY:
RSV_INST_ENTRY:
PRIVILEG_ENTRY:
RSV_ADDR_ENTRY:
ILL_ADDR_ENTRY:
ILL_DATA_ENTRY:
ATT_ENTRY:
AST_ENTRY:
SOFT_ENTTY:
	mov.w	[r31+],TPC		-- Read PC,PSW,EXCEPTION_CODE from user stack
	mov.w	[r31+],TPSW
	mov.w	[r31+],MON_ENTRY
	and.w	#0xff000000,MON_ENTRY
	jr	I_E_ENTRY

	.align 4
#########################
# Exception type2 entry #
#########################
					-- Exception type2 user stack info

					--      +---------------------+
					-- +12	|    PC ( Current )   |
					--      +---------------------+
					-- + 8	| EXCP_CODE|     4    |
					--      +---------------------+
					-- + 4	|         PSW         |
					--      +---------------------+
					--   0	|      PC( Next )     |
					--      +---------------------+

EXC2_ENTRY:

ADDR_ENTRY:
SERIOUS_ENTRY:
L0STAK_ENTRY:
PAGE_ENTRY:
AREA_ENTRY:
MEMORY_ENTRY:
ADDR_TRN_ENTRY:
INTGER_ENTRY:
FLOAT_ENTRY:
DECIMAL_ENTRY:
EXE0_ENTRY:
EXE1_ENTRY:
EXE2_ENTRY:
EXE3_ENTRY:
EMUL_ENTRY:
FPP_ENTRY:
NON_FPP_ENTRY:

	mov.w	[r31+],TPC		-- Read PC,PSW,EXCEPTION_CODE from user stack
	mov.w	[r31+],TPSW
	mov.w	[r31+],MON_ENTRY
	add.w	#0x4,r31		-- Adjust stack pointer
	and.w	#0xff000000,MON_ENTRY
	jr	I_E_ENTRY

I_E_ENTRY:
	mov.w	r1,TR1
	stpr	#7,r1
	and.w	#0x01,r1
	jz	FSET_REAL_ADDR		-- Check virtual mode

FSET_VIR_ADDR:
	mov.w	VIR_ADDR,FG_ADDROFF
	jr	FREENTRY_CHECK

FSET_REAL_ADDR:
	mov.w	REAL_ADDR,FG_ADDROFF

FREENTRY_CHECK:
	movea.w	MONITOR_START,r1
	and.w	#0x0fff,r1
	or.w	FG_ADDROFF,r1		-- Generate mon_start_address; V or R
	cmp.w	r1,TPC			-- Compare PC and mon_start_address
	jn	NORMAL_EXC
	movea.w	MONITOR_END,r1
	and.w	#0x0fff,r1
	or.w	FG_ADDROFF,r1		-- Generate mon_end_address; V or R
	cmp.w	r1,TPC			-- Compare PC and mon_end_address
	jp	NORMAL_EXC
	cmp.w	TPC,#INT_ENTRY
	je	NORMAL_EXC
	cmp.w	TPC,#NMI_ENTRY
	je	NORMAL_EXC
	cmp.w	TPC,#EXC1_ENTRY
	je	NORMAL_EXC
	cmp.w	TPC,#EXC2_ENTRY
	je	NORMAL_EXC
	jr	MONITOR_LOOP		-- This is reentry

NORMAL_EXC:
	mov.w	TPSW,PPSW
	getpsw	r1
	and.w	#0x10000000,r1
	cmp.w	#0x10000000,r1
	je	ISP_USE_EXC
	ldpr	r31,#1
	jr	STORE_EXC

ISP_USE_EXC:
	ldpr	r31,#0

STORE_EXC:
	mov.w	TPC,PPC
	mov.w	TR1,r1
	mov.w	PPC,PCPC
	stpr	#7,PSYCW
	stpr	#8,PTKCW
	stpr	#0,PISP
	jr	FG_ENTRY

########################################################
##### Monitor Loop #####################################
########################################################
	.align 4
	.org	MONITOR_START + 0x0580

MONITOR_LOOP:
	cmp.b	#NEW_CMD,SEMAPHORE	-- Waiting for monitor command
	jz	h`CMD_REQUEST

ARE_YOU_THERE_R:
	mov.b	#MON_LOOP,ARE_YOU_THERE
	jr	MONITOR_LOOP

CMD_REQUEST:
	mov.w	CMD_NUM,r0
	cmp.w	#CMDEXIT,r0
	jz	MONITOR_EXIT
	cmp.w	#CMDRDMEM,r0
	jz	READ_MEMORY
	cmp.w	#CMDWRMEM,r0
	jz	h`WRITE_MEMORY
	cmp.w	#CMDRDIO,r0
	jz	READ_IO
	cmp.w	#CMDWRIO,r0
	jz	WRITE_IO
	cmp.w	#CMDWAITRST,r0
	jz	RUN_RESET
	jr	ARE_YOU_THERE_R

########################################################
##### Monitor Exit Program #############################
########################################################

MONITOR_EXIT:
	mov.b	#TRUE,HALT_SUPPORT
	mov.b	#NO_CMD,SEMAPHORE

	cmp.b	#0,COUNT
	jz	CHECK_SP		-- Run from current_PC

	mov.w	START_ADDRESS,PPC	-- Run from run_address

CHECK_SP:
	getpsw	r0
	and.w	#0x10000000,r0
	cmp.w	#0x10000000,r0
	jz	ISP_USE			-- Check user stack: l0sp or isp
	mov.w	PL0SP,r31
	sub.w	#0x20,r31
	ldpr	r31,#1
	ldpr	PISP,#0
	jr	SET_EXIT_VALUE

ISP_USE:
	mov.w	PISP,r31
	sub.w	#0x20,r31
	ldpr	r31,#0
	ldpr	PL0SP,#1

SET_EXIT_VALUE:
	mov.w	r31,r0

	movea.w	BGBG,r1
	bsr	PUT_STACK_VALUE		-- Set background_exit address

	add.w	#0x4,r0
	getpsw	r1
	and.w	#0xff000000,r1
	bsr	PUT_STACK_VALUE		-- Set background_PSW

	add.w	#0x4,r0
	stpr	#7,r1
	and.w	#SYCW_MASK,r1
	or.w	#SYCW_VAL,r1
	bsr	PUT_STACK_VALUE		-- Set background_SYCW

	add.w	#0x4,r0
	mov.w	#TKCW_VAL,r1
	bsr	PUT_STACK_VALUE		-- Set background_TKCW

	add.w	#0x4,r0
	mov.w	PPC,r1
	bsr	PUT_STACK_VALUE		-- Set next_PC

	add.w	#0x4,r0
	mov.w	PPSW,r1
	bsr	PUT_STACK_VALUE		-- Set user_PSW

	add.w	#0x4,r0
	mov.w	PSYCW,r1
	bsr	PUT_STACK_VALUE		-- Set user_SYCW

	add.w	#0x4,r0
	mov.w	PTKCW,r1
	bsr	PUT_STACK_VALUE		-- Set user_TKCW

	jr	EXIT

PUT_STACK_VALUE:			-- Set value to JAM_AREA
	and.w	#0x0000001f,r0
	or.w	#0x00000fa0,r0
	cmp.h	#0x00fbc,r0
	jgt	PUT_STACK_LOOP

	or.w	FG_ADDROFF,r0

	mov.w	r1,[r0]
	rsr

PUT_STACK_LOOP:
	mov.w	r0,r2
	and.w	#0x0000001f,r2
	or.w	#0x00000fa0,r2
	or.w	FG_ADDROFF,r2
	mov.b	r1,[r2]

	rot.w	#0xf8,r1
	inc.b	r2
	and.w	#0x0001f,r2
	or.w	#0x00fa0,r2
	add.w	FG_ADDROFF,r2
	mov.b	r1,[r2]

	rot.w	#0xf8,r1
	inc.b	r2
	and.w	#0x0001f,r2
	or.w	#0x00fa0,r2
	add.w	FG_ADDROFF,r2
	mov.b	r1,[r2]

	rot.w	#0xf8,r1
	inc.b	r2
	and.w	#0x0001f,r2
	or.w	#0x00fa0,r2
	add.w	FG_ADDROFF,r2
	mov.b	r1,[r2]	
		rsr

EXIT:
	movea.w	PR3,r0			-- Load user register value
	mov.w	[r0+],r3
	mov.w	[r0+],r4
	mov.w	[r0+],r5
	mov.w	[r0+],r6
	mov.w	[r0+],r7
	mov.w	[r0+],r8
	mov.w	[r0+],r9
	mov.w	[r0+],r10
	mov.w	[r0+],r11
	mov.w	[r0+],r12
	mov.w	[r0+],r13
	mov.w	[r0+],r14
	mov.w	[r0+],r15
	mov.w	[r0+],r16
	mov.w	[r0+],r17
	mov.w	[r0+],r18
	mov.w	[r0+],r19
	mov.w	[r0+],r20
	mov.w	[r0+],r21
	mov.w	[r0+],r22
	mov.w	[r0+],r23
	mov.w	[r0+],r24
	mov.w	[r0+],r25
	mov.w	[r0+],r26
	mov.w	[r0+],r27
	mov.w	[r0+],r28
	mov.w	[r0+],r29
	mov.w	[r0+],r30

	movea.w	PL1SP,r0
	ldpr	[r0+],#2		-- Load L1SP
	ldpr	[r0+],#3		-- Load L2SP
	ldpr	[r0+],#4		-- Load L3SP
	ldpr	[r0+],#5		-- Load SBR
	
	movea.w	PPSW2,r0

	ldpr	[r0+],#15		-- Load PSW2
	ldpr	[r0+],#16		-- Load ATBR0
	ldpr	[r0+],#17		-- Load ATLR0
	ldpr	[r0+],#18		-- Load ATBR1
	ldpr	[r0+],#19		-- Load ATLR1
	ldpr	[r0+],#20		-- Load ATBR2
	ldpr	[r0+],#21		-- Load ATLR2
	ldpr	[r0+],#22		-- Load ATBR3
	ldpr	[r0+],#23		-- Load ATLR3
	ldpr	[r0+],#24		-- Load TRMOD	
	ldpr	[r0+],#25		-- Load ADTR0
	ldpr	[r0+],#26		-- Load ADTR1
	ldpr	[r0+],#27		-- Load ADTMR0
	ldpr	[r0+],#28		-- Load ADTMR1

	mov.w	PR0,r0
	mov.w	PR1,r1
	mov.w	PR2,r2
	mov.b	#TRUE,RUNNING
	mov.b	#TRUE,HALT_SUPPORT
	mov.b	#EXIT_MON,ARE_YOU_THERE

BGBG:
	brkret				-- Go to user program


##*****************************************************************************
## MONITOR ENTRY FOREGROUND !!!
##*****************************************************************************

########################################################
#### From Background Entry #############################
########################################################
FROM_BG:
	mov.w	r31,PISP

########################################################
##### Foreground Entry Program #########################
########################################################
FG_ENTRY:
	mov.w	r0,PR0			-- Store user register value
	movea.w	PR1,r0
	mov.w	r1,[r0+]
	mov.w	r2,[r0+]
	mov.w	r3,[r0+]
	mov.w	r4,[r0+]
	mov.w	r5,[r0+]
	mov.w	r6,[r0+]
	mov.w	r7,[r0+]
	mov.w	r8,[r0+]
	mov.w	r9,[r0+]
	mov.w	r10,[r0+]
	mov.w	r11,[r0+]
	mov.w	r12,[r0+]
	mov.w	r13,[r0+]
	mov.w	r14,[r0+]
	mov.w	r15,[r0+]
	mov.w	r16,[r0+]
	mov.w	r17,[r0+]
	mov.w	r18,[r0+]
	mov.w	r19,[r0+]
	mov.w	r20,[r0+]
	mov.w	r21,[r0+]
	mov.w	r22,[r0+]
	mov.w	r23,[r0+]
	mov.w	r24,[r0+]
	mov.w	r25,[r0+]
	mov.w	r26,[r0+]
	mov.w	r27,[r0+]
	mov.w	r28,[r0+]
	mov.w	r29,[r0+]
	mov.w	r30,[r0+]

	movea.w	PL0SP,r0
	stpr	#1,[r0+]		-- Store L0SP
	stpr	#2,[r0+]		-- Store L1SP
	stpr	#3,[r0+]		-- Store L2SP
	stpr	#4,[r0+]		-- Store L3SP
	stpr	#5,[r0+]		-- Store SBR
	stpr	#6,[r0+]		-- Store TR

	movea.w	PPIR,r0
	stpr	#9,[r0+]		-- Store PIR
	stpr	#15,[r0+]		-- Store PSW2
	stpr	#16,[r0+]		-- Store ATBR0
	stpr	#17,[r0+]		-- Store ATLR0
	stpr	#18,[r0+]		-- Store ATBR1
	stpr	#19,[r0+]		-- Store ATLR1
	stpr	#20,[r0+]		-- Store ATBR2
	stpr	#21,[r0+]		-- Store ATLR2
	stpr	#22,[r0+]		-- Store ATBR3
	stpr	#23,[r0+]		-- Store ATLR3
	stpr	#24,[r0+]		-- Store TRMOD
	stpr	#25,[r0+]		-- Store ADTR0
	stpr	#26,[r0+]		-- Store ADTR1
	stpr	#27,[r0+]		-- Store ADTMR0
	stpr	#28,[r0+]		-- Store ADTMR1

	mov.w	PPSW,r0			-- Check user stack
	and.w	#0x10000000,r0
	cmp.w	#0x10000000,r0
	jne	USE_LSP
	mov.w	PISP,PR31		-- Use ISP
	jr	MONITOR_LOOP

USE_LSP:
	mov.w	PPSW,r0
	and.w	#0x03000000,r0
	cmp.w	#0x00000000,r0
	jne	N_L0
	mov.w	PL0SP,PR31		-- Use L0SP
	jr	MONITOR_LOOP

N_L0:
	cmp.w	#0x01000000,r0
	jne	N_L1
	mov.w	PL1SP,PR31		-- Use L1SP
	jr	MONITOR_LOOP

N_L1:
	cmp.w	#0x02000000,r0
	jne	N_L2
	mov.w	PL2SP,PR31		-- Use L2SP
	jr	MONITOR_LOOP

N_L2:
	mov.w	PL3SP,PR31		-- Use L3SP
	jr	MONITOR_LOOP

	nop				-- Protect unused_prefetch for Break entry
	nop
	nop
	nop

	nop
	nop
	nop
	nop

	nop
	nop
	nop
	nop

	nop
	nop
	nop
	nop

##*****************************************************************************
## END OF MONITOR ENTRY FOREGROUND !!!
##*****************************************************************************


##*****************************************************************************
## MONITOR ENTRY BACKGROUND !!!
##*****************************************************************************

########################################################
##### Break Entry Program ##############################
########################################################
# Break entry 
	.org	MONITOR_START + 0x09f0
	.align	4

BREAK_BIT:
BREAK_ENTRY:
	mov.w	r0,TR0
	mov.w	r1,TR1
	mov.w	r2,TR2
	add.w	#0x14,r31
	mov.w	r31,TISP
	mov.w	#MON_STACK,r31		-- Set background stack pointer

#########################
### Check Reset entry ###
#########################
	cmp.b	#TRUE,RESET_FLAG	-- Check reset_falg
	jne	NOT_INIT
	mov.b	#0,RESET_FLAG		-- This is reset entry
	and.w	#0x0fff,r31
	or.w	FG_ADDROFF,r31
	mov.w	r31,TISP
	mov.w	r31,PR31
	mov.w	r31,PISP		-- Set ISP data
	stpr	#9,PPIR

	jr	REENTRY_BREAK

NOT_INIT:

###################################
### Check From Mon or User-prog ###
###################################
CHECK_REENTRY:

	mov.w	#JAM_AREA,r0		-- Check break from monitor
	mov.w	#JAM_AREA_TEMP,r1
	mov.d	[r0+],[r1+]
	mov.d	[r0+],[r1+]
	mov.d	[r0+],[r1+]
	mov.d	[r0+],[r1+]

	mov.w	TISP,r0
	sub.w	#0x14,r0
	and.w	#0x0000001f,r0
	or.w	#0x00000fa0,r0
	mov.w	[r0],TCPC		-- Read user current_PC
	add.w	#0x0000000c,r0
	mov.w	[r0],r1
	mov.w	r1,TSYCW		-- Read user SYCW
	and.w	#0x1,r1			-- Check virtual or real
	jz	BSET_REAL_ADDR

BSET_VIR_ADDR:
	mov.w	VIR_ADDR,FG_ADDROFF
	jr	CMP_ADDRESS

BSET_REAL_ADDR:
	mov.w	REAL_ADDR,FG_ADDROFF

CMP_ADDRESS:
	movea.w	MONITOR_START,r1
	and.w	#0x0fff,r1
	or.w	FG_ADDROFF,r1		-- Generate mon_start_address; V or R
	cmp.w	r1,TCPC			-- Compare PC and mon_start_address
	jn	NORMAL_BREAK
	movea.w	MONITOR_END,r1
	and.w	#0x0fff,r1
	or.w	FG_ADDROFF,r1		-- Generate mon_end_address; V or R
	cmp.w	r1,TCPC			-- Compare PC and mon_end_address
	jp     	NORMAL_BREAK

######################
# From Monitor entry #
######################
REENTRY_BREAK:
					--BG to FG Stack info (REENTRY)
					--
					--0x10   nPC		MONITOR_LOOP address
					--0x14   PSW		MASK 0c4001f0f
					--			OR   010040000
					--0x18   SYCW		MASK 00000ff01
					--			OR   00000e000
					--0x1c   TKCW		MASK 0000003f7
					--			OR   000000070
					--0x20           Initial stack pointer

	mov.w	PISP,r0
	sub.w	#0x10,r0
	movea.w	MONITOR_LOOP,r1
	and.w	#0x0fff,r1
	or.w	FG_ADDROFF,r1
	bsr	PUT_STACK_VALUE		--Set MONITOR_LOOP address

	jr	BG_TO_FG

###########################
# From User-Promram entry #
###########################
NORMAL_BREAK:	
	mov.w	TISP,r0
	mov.w   r0,PISP

	mov.w	PISP,r0
	sub.w	#0x14,r0
	and.w	#0x0000001f,r0
	or.w	#0x00000fa0,r0

	mov.w	[r0+],PCPC		-- Store user Current PC
	mov.w	[r0+],PPC		-- Store user Next PC
	mov.w	[r0+],PPSW		-- Store user PSW
	mov.w	[r0+],PSYCW		-- Store user SYCW
	mov.w	[r0+],PTKCW		-- Store user TKCW

	mov.w	PISP,r0
	sub.w	#0x10,r0
	movea.w	FROM_BG,r1
	and.w	#0x000000fff,r1

	or.w	FG_ADDROFF,r1
	bsr	PUT_STACK_VALUE		-- Set foreground PC
	
BG_TO_FG:

	mov.w	PISP,r0
	sub.w	#0x0c,r0
	mov.w	PPSW,r1
	and.w	#PSW_MASK,r1
	or.w	#PSW_VAL,r1
	bsr	PUT_STACK_VALUE		--Set foreground PSW

	add.w	#0x4,r0
	mov.w	PSYCW,r1
	and.w	#SYCW_MASK,r1
	or.w	#SYCW_VAL,r1
	bsr	PUT_STACK_VALUE		--Set foreground SYCW

	add.w	#0x4,r0
	mov.w	PTKCW,r1
	and.w	#TKCW_MASK,r1
	or.w	#TKCW_VAL,r1
	bsr	PUT_STACK_VALUE		--Set foreground TKCW

	mov.w	PISP,r31		--Set foreground stack pointer
	sub.w	#0x10,r31
	mov.w	TR0,r0
	mov.w	TR1,r1
	mov.w	TR2,r2

	nop
BG_TO_FG_BIT:
	brkret				-- Go to foreground

##*****************************************************************************
## END OF MONITOR ENTRY BACKGROUND !!
##*****************************************************************************


##*****************************************************************************
## MONITOR VARIABLES !!!
##*****************************************************************************

########################################################
##### Register Buffer ##################################
########################################################
# Register Stack Area
	.org	MONITOR_START + 0x0c60
REGISTERS:
PR0:		.word		0
PR1:		.word		0
PR2:		.word		0
PR3:		.word		0
PR4:		.word		0
PR5:		.word		0
PR6:		.word		0
PR7:		.word		0
PR8:		.word		0
PR9:		.word		0
PR10:		.word		0
PR11:		.word		0
PR12:		.word		0
PR13:		.word		0
PR14:		.word		0
PR15:		.word		0
PR16:		.word		0
PR17:		.word		0
PR18:		.word		0
PR19:		.word		0
PR20:		.word		0
PR21:		.word		0
PR22:		.word		0
PR23:		.word		0
PR24:		.word		0
PR25:		.word		0
PR26:		.word		0
PR27:		.word		0
PR28:		.word		0
PR29:		.word		0
PR30:		.word		0
PR31:		.word		0x000001000
PPC:		.word		0x0fffffff0
PPSW:		.word		0x010000000
PSYCW:		.word		0x000000070
PISP:		.word		0x000000020
PL0SP:		.word		0
PL1SP:		.word		0
PL2SP:		.word		0
PL3SP:		.word		0
PSBR:		.word		0x000000000
PTR:		.word		0
PTKCW:		.word		0x00000e000
PPIR:		.word		0
PPSW2:		.word		0x00000f002
PATBR0:		.word		0
PATLR0:		.word		0
PATBR1:		.word		0
PATLR1:		.word		0
PATBR2:		.word		0
PATLR2:		.word		0
PATBR3:		.word		0
PATLR3:		.word		0
PTRMOD:		.word		0x000000000
PADTR0:		.word		0
PADTR1:		.word		0
PADTMR0:	.word		0
PADTMR1:	.word		0
PCPC:		.word		0
TR0:		.word		0
TR1:		.word		0
TR2:		.word		0
TISP:		.word		0
TCPC:		.word		0
TPC:		.word		0
TPSW:		.word		0
TSYCW:		.word		0
TTKCW:		.word		0      

########################################################
##### Firmware Communication data area No 1 ############
########################################################
# address = 0xd80
	.org	MONITOR_START + 0x0d80

CMD_NUM:	.word		NO_CMD
COUNT:		.word		0
START_ADDRESS:	.word		0
ACCESSMODE:	.word	        BYTEACCESS
		.word		0
		.word		0
		.word		0
		.word		0
		.word		0
		.word		0
		.word		0
RESET_FLAG:	.byte		TRUE

########################################################
##### Firmware Communication data area No 2 ############
########################################################
# address = 0xdb0
	.org	MONITOR_START + 0x0db0

SEMAPHORE:	.byte		NO_CMD
ARE_YOU_THERE:	.byte		RESPOND
RUNNING:	.byte		FALSE
HALT_SUPPORT:	.byte		FALSE
MON_ENTRY:	.word		0x80000000
MONITOR_ADDRESS:.word		0x0
FG_ADDRESS:	.word		MONITOR_START

########################################################
##### Data Buffer ######################################
########################################################
# Modify and Display memory data area
	.org	MONITOR_START + 0x0dc0

DATA_BUF:
	.space	270		

########################################################
##### Firmware Communication data area No 3 ############
########################################################
# Real and virtual address value
	.org	MONITOR_START + 0x0f00

REAL_ADDR:
	.word	0

VIR_ADDR:
	.word   0

FG_ADDROFF:
	.word	0

########################################################
##### Jam Area #########################################
########################################################
# Jam stack area
	.org	MONITOR_START + 0x0fa0

JAM_AREA:

	.org	MONITOR_START + 0x0fc0

JAM_AREA_TEMP:

##*****************************************************************************
## END OF MONITOR VARIABLES !!!
##*****************************************************************************


##*****************************************************************************
## MONITOR RESET BREAK ROUTINE !!!
##*****************************************************************************

########################################################
##### Debug Interrupt Address ##########################
########################################################
# Break entry    0xffffffe0 is debug interrupt entry address
	.org	MONITOR_START	+ 0x0fe0

BREAD_ADDRESS:
	jr	BREAK_ENTRY

########################################################
##### Foreground Monitor ID ############################
########################################################
# Check for loaded fgmon
	.org	MONITOR_START	+ 0x0fe8

	.strz	"64758FG"

########################################################
##### Reset Start Address ##############################
########################################################
# Reset entry    0xfffffff0 is reset address
	.org	MONITOR_START	+ 0x0ff0

RESET_ADDRESS:
	mov.w	#0x20,r31
	ldpr	r31,#0
	jr	MONITOR_LOOP

##*****************************************************************************
## END OF MONITOR RESET BREAK ROUTINE !!!
##*****************************************************************************

MONITOR_END:
	

