********************************************************************************
* A Hewlett-Packard Software Product
* Copyright Hewlett-Packard Co. 1993
*
* All Rights Reserved. Reproduction, adaptation, or translation without prior
* written  permission  is prohibited, except as allowed under copyright laws.
********************************************************************************
* Customizable Foreground Monitor for HP64747 Motorola 68030/EC030 Emulator
*
* The emulation monitor is the vehicle by which the following emulator
* functions are performed:
*
*      read/write emulation/target memory space
*      display/modify MC68030/EC030 registers
*      display/modify MC68881/68882 FPU registers
*      execute user program
*      break away from user program
*
* The monitor must be linked on a 4 Kbyte boundary and cannot exceed 4 Kbytes in
* size.  The starting address is specified in the "ORG" statement below (address
* must end in $000).  This address must match the address used for the monitor
* address configuration question within the emulation interface.  In this manner,
* communication between the monitor and the emulator operating software can be
* established.
*
* The monitor consists of $700 bytes of data space followed by $800 bytes of
* code space.  The emulator operating software communicates with the monitor
* through the data space and no changes should be made to this portion of the
* monitor.  See areas marked as "USER MODIFIABLE" for specific customizations
* typically required by target systems.
********************************************************************************

	CHIP 68030
	ORG $XXXXX000             must be on a 4K boundary (ending in $000)

********************************************************************************
*****************   M O N I T O R   D A T A   S E C T I O N   ******************
********************************************************************************

********************************************************************************
* Monitor Vector Table - 256 bytes 
********************************************************************************
MONDATA       	DC.L    MONSTACK           Reset Initial Interrupt Stack Pointer
		DC.L    RESET_ENTRY        Reset Initial Program Counter
                DC.L    ERROR_ENTRY        Bus Error
		DC.L    ERROR_ENTRY        Address Error
		DC.L	ERROR_ENTRY        Illegal Instruction Entry
		DC.L    ERROR_ENTRY        Zero Divide
		DC.L    ERROR_ENTRY        CHK,CHK2 Instructions
		DC.L    ERROR_ENTRY        cpTRAPcc,TRAPcc,TRAPV Instructions
		DC.L    ERROR_ENTRY        Privilege Violation
		DC.L	TRACE_ENTRY        Trace Entry
		DC.L    ERROR_ENTRY        Line 1010 Emulator
		DC.L    ERROR_ENTRY        Line 1111 Emulator
		DC.L    ERROR_ENTRY        Unassigned/Reserved
		DC.L    ERROR_ENTRY        Coprocessor Protocol Violation
		DC.L    ERROR_ENTRY        Format Error
		DC.L    ERROR_ENTRY        Uninitialized Interrupt
                DCB.L   4,BREAKPOINT_ENTRY Unassigned/Reserved
                DCB.L   4,INTERRUPT_ENTRY  Unassigned/Reserved (vector 20)
		DC.L    ERROR_ENTRY        Spurious Interrupt
		DCB.L   6,ERROR_ENTRY      Level 1-6 Auto Vector
		DC.L    ERROR_ENTRY        Level 7 Auto Vector
		DCB.L   16,ERROR_ENTRY     Trap #0-15 Instructions
		DCB.L   7,ERROR_ENTRY      FPCP Exceptions
		DC.L    ERROR_ENTRY        Unassigned/Reserved
		DC.L    ERROR_ENTRY        MMU configuration error
		DCB.L   2,ERROR_ENTRY      MC68851 MMU Exceptions
MON_ENTRY	DC.L	0                  Unassigned/Reserved (vector 59)
MON_EXIT	DC.L	0                  Unassigned/Reserved (vector 60)
MON_MOP		DC.L	0                  Unassigned/Reserved (vector 61)
MON_CMB		DC.L	0                  Unassigned/Reserved (vector 62)
MON_FGTRAN	DC.L	0                  Unassigned/Reserved (vector 63)

********************************************************************************
* Monitor Variables - 256 bytes
********************************************************************************
MON_RANGE       DC.L MONPROG,MONPROGEND    Monitor Code Address Range for CMP2

* Monitor Control Registers
CMD_HANDSHAKE   DC.B 0            nozero if command to be executed
CMD_CONTROL     DC.B 2            command to execute (initial 2 is revision!)
CMD_RESULT      DC.B 0            result of executed command
MON_INTRLEVEL   DC.B 0            minimum interrupt level
MON_CONTROL     DC.W 0            miscellaneous flags
MON_LASTENTRY   DC.W 0            last entry into the monitor
MEM_SRCADDR     DC.L 0            source address for read/write
MEM_DSTADDR     DC.L 0            destination address for read/write
MEM_FCS         DC.W 0            function codes for read/write
MEM_SIZE        DC.W 0            access size for read/write
MEM_BYTES       DC.W 0            number of bytes to transfer
MON_KAFCS       DC.W 0            reserved
MON_KAADDR      DC.L 0            reserved
REG_MASK        DC.W 0            alternate registers to read/write
MEM_MMUSR       DC.W 0            result of ptest on bus fault

TMP             DS.B 20           spares
TMP2            DS.B 20

* Table of Exception Stack Frame Sizes
FRAMESIZE       DC.W 8            format $0 - four word stack frame
                DC.W 8            format $1 - throwaway four word stack frame
                DC.W 12           format $2 - six word stack frame
                DC.W 0,0,0,0,0,0  format $3 thru $8 - invalid
                DC.W 20           format $9 - coprocessor mid stack frame
                DC.W 32           format $A - short bus cycle fault
                DC.W 92           format $B - long bus cycle fault
                DC.W 0,0,0,0      format $C thru $F - invalid

* User Registers
MPC  	        DC.L 0            program counter
                DC.W 0            padding (reserved)
MSR             DC.W $2700        status register
MREGS
MD0             DC.L 0            data registers D0-D7
MD1             DC.L 0
MD2             DC.L 0
MD3             DC.L 0
MD4             DC.L 0
MD5             DC.L 0
MD6             DC.L 0
MD7             DC.L 0
MA0             DC.L 0            address registers A0-A7
MA1             DC.L 0
MA2             DC.L 0
MA3             DC.L 0
MA4             DC.L 0
MA5             DC.L 0
MA6             DC.L 0
MA7             DC.L 0
MUSP            DC.L 0            user stack pointer
MMSP            DC.L 0            master stack pointer
MISP            DC.L 0            interrupt stack pointer
MVBR            DC.L 0            vector base register
MCACR           DC.L 0            cache control register
MCAAR           DC.L 0            cache address register
MSFC            DC.L 0            source function code register
MDFC            DC.L 0            destination function code register
                DC.W 0            padding (reserved)
MVO             DC.W 0            stack frame format word

* Temporary data buffers for monitor
FMT9            DS.B 12           partial copy of user's fmt $9 stack frame
TDLIST          DCB.L 6,0         used for mmu write-protect recovery

********************************************************************************
* Monitor Transfer Buffer - 1024 bytes
********************************************************************************
MONTRANS        DS.B $400

********************************************************************************
* Monitor Interrupt Stack - 256 bytes
********************************************************************************
		DS.B $100
MONSTACK

********************************************************************************
* Monitor Equates
********************************************************************************
* Equates for CMD_CONTROL
CC_SWINMON      EQU     1     ;are you there?
CC_EXITMON      EQU     2     ;exit monitor
CC_MEMREAD      EQU     3     ;read memory
CC_MEMWRITE     EQU     4     ;write memory
CC_FPUREAD      EQU     5     ;read fpu registers 
CC_FPUWRITE     EQU     6     ;write fpu registers 
CC_MMUREAD      EQU     7     ;read mmu registers 
CC_MMUWRITE     EQU     8     ;write mmu registers 

* Equates for CMD_RESULT vector numbers (set by monitor/read by driver)
CR_SUCCESS      EQU     0     ;success
CR_RESET        EQU     1     ;failed due to reset
CR_BUSERROR     EQU     2     ;failed due to bus error
CR_ACCERROR     EQU    12     ;failed due to access violation
CR_FMTERROR     EQU    14     ;failed due to FPU format error exception
CR_MMUERROR     EQU    56     ;failed due to MMU configuration exception

* Equates for MON_CONTROL
MC_TRACEMODE    EQU     0     ;single step being performed
MC_KEEPALIVE    EQU     1     ;enable keep alive access (background only)
MC_MMUENABLED   EQU     2     ;mmu enabled (foreground only)
MC_IDLE         EQU     3     ;monitor in idle wait
MC_CMBWAIT      EQU    12     ;monitor in CMB wait loop

* Equates for MON_LASTENTRY (vector offsets)
ML_UNDEFINED    EQU     0    
ML_RESET        EQU     1     ;emulation/target reset
ML_BREAKPOINT   EQU  $010     ;breakpoint exception
ML_TRACE        EQU  $024     ;trace exception
ML_INTERRUPT    EQU  $07C     ;interrupt exception
                     
* Equates for MEM_FCS
FCODE_UD        EQU     1     ;user data space
FCODE_UP        EQU     2     ;user program space
FCODE_SD        EQU     5     ;supervisor data space
FCODE_SP        EQU     6     ;supervisor program space
FCODE_CPU       EQU     7     ;cpu space

* Equates for MEM_SIZE
MS_BYTES        EQU     0
MS_WORDS        EQU     1
MS_LONGS        EQU     2

* Equates for REG_MASK (bit positions)
RM_FP0          EQU     0     ;fpu registers
RM_FP1          EQU     1
RM_FP2          EQU     2
RM_FP3          EQU     3
RM_FP4          EQU     4
RM_FP5          EQU     5
RM_FP6          EQU     6
RM_FP7          EQU     7
RM_FPIAR        EQU    10
RM_FPSR         EQU    11
RM_FPCR         EQU    12

RM_TT0          EQU     0     ;mmu registers
RM_TT1          EQU     1
RM_MMUSR        EQU     2
RM_TC           EQU     3
RM_SRP          EQU     4
RM_CRP          EQU     5

********************************************************************************
***************  M O N I T O R   E N T R Y   R O U T I N E S  ******************
********************************************************************************
MONPROG
********************************************************************************
* Monitor entry point for a trace exception.  Since there is no unique hardware
* event for the emulator to detect the start of trace exception processing, the
* emulator requires that the user's trace vector point to this address.
********************************************************************************
TRACE_ENTRY
	MOVE.W  #ML_TRACE,TMP           save entry point

	CMPI.W  #$2024,6(A7)            if stack frame format does not indicate
	BEQ     CHECK_MONSTATE          a trace exception, set lsb to prevent
	ORI.W   #1,TMP                  user's stack frame from being removed
	BRA     CHECK_MONSTATE          (ie: traced aline/fline instructions)

********************************************************************************
* Monitor entry point for reset into monitor.  When the processor initiates
* reset exception processing, the emulator transitions the monitor state
* machine into the foreground control state and "jams" this address onto the
* data bus during the reset vector fetch.   If a target reset occurs during
* execution of the monitor, any command in process is aborted.
********************************************************************************
RESET_ENTRY
	MOVE.W  #ML_RESET,MON_LASTENTRY save entry point
	MOVE.B  #CR_RESET,CMD_RESULT    abort any command
	CLR.W   MVO                     clear for exit
	BRA     CHECK_MONSTATE

********************************************************************************
* Monitor entry point for software breakpoints.  When the emulator detects the
* appropriate breakpoint acknowledge cycle, it terminates the bus cycle with a
* bus error and "jams" this address on the data bus when the processor fetches
* the illegal instruction vector.
********************************************************************************
BREAKPOINT_ENTRY
	MOVE.W  #ML_BREAKPOINT,TMP      save entry point
	BRA     CHECK_MONSTATE

********************************************************************************
* Monitor entry point for software breakpoints.  When the emulator detects the
* interrupt acknowledge cycle after generating a level 7 NMI, it terminates the
* bus cycle with autovector and "jams" this address on the data bus when the
* processor fetches the level 7 autovector vector.
********************************************************************************
INTERRUPT_ENTRY
	MOVE.W  #ML_INTERRUPT,TMP       save entry point
	BRA     CHECK_MONSTATE

********************************************************************************
* Exception handler for exceptions generated during monitor commands.  During
* monitor command execution, all target interrupts are blocked and VBR points
* to the monitor's vector table.  This entry point can also be used to trap
* exceptions generated during target execution if VBR points to the base
* address of the monitor or individual vectors point to this address.  In this
* case, the user's stack frame is not removed by setting the lsb in TMP.
********************************************************************************
ERROR_ENTRY
	MOVE.W  6(A7),TMP               save vector offset as entry point with
	ORI.W   #1,TMP                  lsb set to prevent stack frame removal

	* fall through to CHECK_MONSTATE *

********************************************************************************
* Check state of emulation ready (ERY) to determine if hardware state machine
* correctly transitioned into the "in monitor" state.  If not, the monitor will
* force this state change by accessing the MON_ENTRY variable.
********************************************************************************
CHECK_MONSTATE
	BTST    #2,MON_CMB
	BEQ.B   CHECK_REENTRY
	TST.B	MON_ENTRY

	* fall through to CHECK_REENTRY *

********************************************************************************
* Check for monitor reentry.  Compare the PC and SR in the exception stack
* frame with the address range of the monitor to determine if this is a 
* transition from user program or exception during monitor command execution.  
********************************************************************************
CHECK_REENTRY
	CMPI.B  #CR_RESET,CMD_RESULT
	BEQ     MON_MAIN

	ORI.W	#$0700,SR               block interrupts during startup
	MOVE.L  D0,TMP2                 flush data cache because monitor
	MOVEC   CACR,D0                 entry causes stale data
	BSET    #11,D0
	MOVEC   D0,CACR
	MOVE.L  TMP2,D0
	MOVE.L  A0,TMP2
	MOVEA.L 2(A7),A0
	CMP2.L  MON_RANGE,A0            compare stacked PC
	MOVEA.L TMP2,A0
	BCS     USER_ENTRY
	BTST    #5,(A7)                 compare stacked SR supervisor bit
	BEQ     USER_ENTRY

	LEA     TRACE_ENTRY,A0          check for access violation interrupt
	CMP.L   2(A7),A0                generated after a successful single
	BNE.B   MON_EXCEPTION           step;  if found, return to TRACE_ENTRY
	RTE

********************************************************************************
***********************  M O N I T O R   E X C E P T I O N  ********************
********************************************************************************

********************************************************************************
* Exception handler for exceptions generated during monitor command execution.
* The monitor attempts to recover from certain bus faults during memory read
* and write commands.  All other exceptions abort the command in process.
********************************************************************************
MON_EXCEPTION
	LINK.L  A6,#$FFFFFFE0           save working registers
	PMOVE   TC,(A7)
	PMOVE   MMUSR,4(A7)
	MOVEM.L D0-D2/A0-A2,6(A7)

	MOVE.W  $A(A6),D0               check stack frame format
	ANDI.W  #$EFFC,D0
	CMPI.W  #$A008,D0
	BNE.B   MON_ABORTCMD            not bus error

	CLR.W   MEM_MMUSR
	BTST    #MC_MMUENABLED,MON_CONTROL+1
	BEQ     MON_ABORTCMD            mmu not enabled in configuration
	BTST.B  #7,(A7)
	BEQ.B   MON_ABORTCMD            mmu not enabled in TC register

	MOVE.W  $0E(A6),D2              special status word
	MOVE.L  $14(A6),A2              fault address
	PTESTR  D2,(A2),#7              execute ptest and return status
	PMOVE   MMUSR,MEM_MMUSR

	MOVE.W  MEM_MMUSR,D0            check for non-resident page
	ANDI.W  #$C400,D0
	CMPI.W  #$0400,D0
	BEQ     NONRESIDENT

	MOVE.W  MEM_MMUSR,D0            check for write-protect violation
	ANDI.W  #$2C00,D0
	CMPI.W  #$0800,D0
	BEQ     WPROTECTED

********************************************************************************
* Monitor command must be aborted due to exception.  Save vector number as the
* error code and copy the exception stack frame to the monitor transfer buffer
* for subsequent processing by the drivers.  If aborting a read/write command,
* return to epilog function to undo any temporary changes related to the MMU.
********************************************************************************
MON_ABORTCMD
	PMOVE   (A7),TC            restore user's mmu registers in case they
	PMOVE   4(A7),MMUSR        were modified
	UNLK    A6                 discard saved registers

	BFEXTU	6(A7){6:8},D0      use vector number as error code
	MOVE.B  D0,CMD_RESULT
	
	LEA     MONTRANS,A0        copy exception stack frame
	BFEXTU  6(A7){0:4},D0
	MOVE.W  (FRAMESIZE,PC,D0*2),D0
	LSR.W   #2,D0
	SUBQ    #1,D0
MA_LOOP	MOVE.L  (A7)+,(A0)+
	DBF     D0,MA_LOOP

	CMPI.B  #CC_MEMREAD,CMD_CONTROL   return to read/write epilog in case
	BEQ     RDWR_EPILOG               the MMU was disabled to perform a
	CMPI.B  #CC_MEMWRITE,CMD_CONTROL  physical access
	BEQ     RDWR_EPILOG
	BRA     MON_MAIN

********************************************************************************
* Bus fault occurred due to an access to non-resident page.  Call the target
* system's bus fault handler to swap-in non-resident pages.  The monitor
* copies all its registers followed by the bus fault stack frame to the user's
* interrupt stack, restores the user's registers and jumps directly to the
* user's bus fault handler.  The user's bus fault handler must return via RTE.
* Upon return, the monitor will restore all its registers and retries the
* original access iff the non-resident page was swapped in.
********************************************************************************
NONRESIDENT
********************************************************************************
* USER MODIFIABLE CODE - Uncomment the following code segment if you want the
* monitor to call your bus fault handler to swap in non-resident pages.
********************************************************************************
*	MOVE.L  A7,TMP                 save all monitor registers (except SSP)
*	MOVE.L  MISP,A7                on user's interrupt stack
*	MOVEM.L D0-D7/A0-A6,-(A7)
*	
*	BFEXTU  $A(A6){0:4},D0         copy stack frame to user's stack
*	MOVE.W  (FRAMESIZE,PC,D0*2),D0
*	LSR.W   #1,D0
*	SUBQ.W  #1,D0
*SFCOPY	MOVE.W  (4,A6,D0.W*4),-(A7)
*	DBF     D0,SFCOPY
*	LEA     USER_RETURN,A0         change PC in user's stack frame in
*	MOVE.L  A0,2(A7)               order to return here on user's RTE
*	
*	MOVE.L  MVBR,D0                restore most registers in case they are
*	MOVEC   D0,VBR                 needed by user's access fault handler
*	MOVE.L  MCACR,D0
*	MOVEC   D0,CACR
*	MOVEM.L MREGS,D0-D7/A0-A6
*	
*	MOVE.L  ([MVBR,PC],8),A0       call user's access fault handler
*	JMP     (A0)
*USER_RETURN
*	NOP                            return to a few NOPs just in case fault
*	NOP                            handler decides not to retry the access
*	NOP                            and increments PC past the instruction
*	PFLUSHA
*	MOVEM.L (A7)+,D0-D7/A0-A6      restore monitor's registers
*	MOVE.L  TMP,A7
*	MOVEC   A5,VBR                
*	BRA     MON_RETRY
*	
*	PTESTR  D2,(A2),#7
*	PMOVE   MMUSR,MEM_MMUSR
*	BTST.B  #2,MEM_MMUSR
*	BEQ     MON_RETRY
********************************************************************************
	BRA     MON_ABORTCMD

********************************************************************************
* Attempt to recover from write protect violation during memory write command.
* The monitor attempts to clear the WP bit in one or more table descriptors
* in memory and retries the access.  Modified table descriptors are restored
* upon completion of the command.
********************************************************************************
WPROTECTED
	CMPI.B  #CC_MEMWRITE,CMD_CONTROL
	BNE     MON_ABORTCMD       not memory write command
	
	BTST    #6,D2
	BNE     MON_ABORTCMD       not write access in ssw
	
	BCLR.B  #7,(A7)            temporarily disable mmu
	PMOVE   (A7),TC
	BSET.B  #7,(A7)
	
	BSR     RESTORE_WP_BITS    clear list and restore previous changes
	
	MOVE.W  MEM_MMUSR,D0       get number of levels
	ANDI.W  #7,D0
	MOVEQ   #0,D1
	
PTEST_LOOP
	SUBQ    #1,D0
	BLT.B   PTEST_DONE
	JMP     (PTEST_JMPTABLE,PC,D1.W*8)
	
PTEST_JMPTABLE
	PTESTR  D2,(A2),#1,A1
	BRA.W   PTEST_EVAL
	PTESTR  D2,(A2),#2,A1
	BRA.W   PTEST_EVAL
	PTESTR  D2,(A2),#3,A1
	BRA.W   PTEST_EVAL
	PTESTR  D2,(A2),#4,A1
	BRA.W   PTEST_EVAL
	PTESTR  D2,(A2),#5,A1
	BRA.W   PTEST_EVAL
	PTESTR  D2,(A2),#6,A1
	
PTEST_EVAL
	ADDQ.W  #1,D1              check MMUSR for write protect violation
	PMOVE   MMUSR,(A0)         (note: A2 points to area used as scratch)
	ANDI.B  #$FC,(A0)
	CMPI.B  #$08,(A0)
	BNE.B   PTEST_LOOP
	
	ADD.L   #3,A1              attempt to clear WP bit in last descriptor
	BCLR.B  #2,(A1)            accessed and save the address of the byte
	BTST.B  #2,(A1)
	BNE.B   PTEST_DONE         clear WP failed - probably in rom
	MOVE.L  A1,(A0)+           clear WP succeeded - save address in WPLIST
	BRA.B   PTEST_LOOP
	
PTEST_DONE
	MOVE.L  #0,(A0)            terminate list of modified descriptors
	TST.B   D0
	BLT     MON_RETRY
	BRA     MON_ABORTCMD       clear WP bit failed - abort recovery

********************************************************************************
* Restore WP bits previously cleared in user's translation table.  TDLIST
* contains the zero terminated list of addresses which were modified during
* exception processing.  Upon return, A0 will point to the initialized list.
********************************************************************************
RESTORE_WP_BITS
	LEA     TDLIST,A0          initialize A0 for caller
	TST.L   (A0)
	BEQ.B   RESTORE_DONE       empty list
	
	SUBQ    #4,A7              disable mmu if enabled
	PMOVE   TC,(A7)
	BCLR.B  #7,(A7)
	BEQ.B   RESTORE_LOOP       mmu was not enabled
	PMOVE   (A7),TC
	BSET.B  #7,(A7)
RESTORE_LOOP
	BSET.B  #2,([A0])          set WP bit in table descriptor
	CLR.L   (A0)+
	TST.L   (A0)
	BNE.B   RESTORE_LOOP       not end-of-list
	PMOVE   (A7),TC            restore original value of TC
	ADDQ    #4,A7
	LEA     TDLIST,A0          initialize A0 for caller
RESTORE_DONE
	RTS

********************************************************************************
* Recover from bus fault exception.  Retest the fault address 
********************************************************************************
MON_RETRY
	PMOVE   (A7),TC            restore saved registers and attempt to 
	PMOVE   4(A7),MMUSR        were modified
	MOVEM.L 6(A7),D0-D2/A0-A2  and complete bus cycle with RTE
	UNLK    A6
	RTE

********************************************************************************
**************  E N T R Y   F R O M   U  S E R   P R O G R A M   ***************
********************************************************************************

********************************************************************************
* Save all user registers and process and remove the exception stack frame.
********************************************************************************
USER_ENTRY
       	MOVEM.L D0-D7/A0-A6,MREGS       save all registers
	MOVE.L  USP,A0
	MOVE.L  A0,MUSP
	MOVEC   VBR,A0
	MOVE.L  A0,MVBR
	MOVEC   CACR,A0
	MOVE.L  A0,MCACR
	MOVEC   CAAR,A0
	MOVE.L  A0,MCAAR
	MOVEC   SFC,A0
	MOVE.L  A0,MSFC
	MOVEC   DFC,A0 
	MOVE.L  A0,MDFC

	MOVE.W  TMP,D0                  set entry point to vector offset
	ANDI.W  #$0FFC,D0
	MOVE.W  D0,MON_LASTENTRY

	MOVE.W  6(A7),MVO               if stack frame format word indicates
	BFEXTU  MVO{0:4},D0             a format $1 throwaway frame, pop and
	CMPI.B  #1,D0                   discard the stack frame and switch
	BNE.B   NOT_THROWAWAY           over to user's master stack
	ADDA.W  (FRAMESIZE,PC,D0*2),A7  
	ORI.W   #$1000,SR
	MOVE.W  6(A7),MVO
	BFEXTU  MVO{0:4},D0
NOT_THROWAWAY
	MOVE.W 	(A7),MSR                save PC and SR from stack frame
	MOVE.L  2(A7),MPC

	ANDI.W  #1,TMP                  pop stack frame unless needed for
	BNE.B   SSP_ADJUSTED            user's program (aline/fline/error)
	ADDA.W  (FRAMESIZE,PC,D0*2),A7
SSP_ADJUSTED
	MOVEC   MSP,A0                  save master/interrupt stack pointers
	MOVE.L  A0,MMSP
	MOVEC   ISP,A0
	MOVE.L  A0,MISP

********************************************************************************
* If monitor entry interrupted a coprocessor instruction, save last 6 words of
* format $9 coprocessor-mid stack frame so that it can be restored on exit.
********************************************************************************
COPROCESSOR_MID
	CMPI.B  #9,D0
	BNE.B   FIND_TBITS
	MOVE.L  A7,A0
	LEA     FMT9,A1
	MOVE.W  #5,D0
CM_LOOP	MOVE.W  -(A0),(A1)+
	DBF     D0,CM_LOOP

	* fall through to FIND_TBITS *

********************************************************************************
* When tracing, if an instruction forced an exception as part of its normal
* execution (ie: TRAP or CHK), the forced exception processing occurs before
* the trace exception is processed;  in this case, the T0/T1 bits in the status
* register are saved in the forced exception stack frame instead of our trace
* exception stack frame
********************************************************************************
FIND_TBITS
	BTST    #MC_TRACEMODE,MON_CONTROL+1
	BEQ.B   MON_MAIN                not tracing

	TST.W   TMP
	BNE.B   CLEAR_TBITS             stack frame was not removed
	BFTST   MSR{0:2}         
	BEQ.B   CLEAR_TBITS             removed stack frame did not have T0/T1
	BRA.B   MON_MAIN
CLEAR_TBITS
	CMPA.L  MA7,A7                  double check that a new stack frame
	BGT     MON_MAIN                exists on the stack since last exit
	BFCLR   (A7){0:2}               then unconditionally clear T0/T1

	* fall through to MON_MAIN *

********************************************************************************
********************   M O N I T O R   I D L E   L O O P   *********************
********************************************************************************

********************************************************************************
* During the monitor idle loop, the monitor can be interrupted by the target
* system.  The interrupt priority level (IPL) is set to the value defined in
* configuration or the user's IPL in effect upon monitor entry, whichever is
* greater.  Most user registers are restored while running in the idle loop.
* When a command is requested, the monitor blocks all interrupts and uses its
* own vector table and stack to trap any exceptions generated during command
* execution.
********************************************************************************
MON_MAIN
	CLR.W   CMD_HANDSHAKE          flag command completion (clears command)
	BSET    #MC_IDLE,MON_CONTROL+1 flag idle loop entry

	MOVE.L  MVBR,D0                restore all user registers during idle
	MOVEC   D0,VBR                 loop in case they are needed by target
	MOVE.L  MCACR,D0               interrupt exception handler
	MOVEC   D0,CACR
	MOVE.L  MCAAR,D0                            
	MOVEC   D0,CAAR
	MOVE.L  MSFC,D0                            
	MOVEC   D0,SFC
	MOVE.L  MDFC,D0
	MOVEC   D0,DFC
  	MOVE.L  MUSP,D0
	MOVEC   D0,USP
	MOVE.L  MMSP,D0
	MOVEC   D0,MSP
	MOVE.L  MISP,D0
	MOVEC   D0,ISP
	MOVEM.L MREGS,D0-D7/A0-A6  

	BFEXTU  MSR{5:3},D0            set interrupt priority and clear M bit
	CMP.B   MON_INTRLEVEL,D0       to select user's interrupt stack
	BGE.B   USE_PRIOR
	MOVE.B  MON_INTRLEVEL,D0
USE_PRIOR
	MOVE.W  SR,D1
	BFINS   D0,D1{19:5}            sets ipl and clears M bit
	MOVE.W  D1,SR

MON_LOOP
************************************************************************
* USER MODIFIABLE CODE - 
*   If the target system requires that a watchdog timer be reset on a
*   regular basis, code can be added here to access a particular address
*   or to call a target system subroutine.  Since the foreground monitor
*   is interruptable, target systems which reset watchdog timers from
*   an interrupt handler may require no changes.  However, the monitor
*   interrupt priority configuration question and the interrupt priority
*   mask in the SR must be set appropriately on monitor entry.
************************************************************************
	TST.B   CMD_HANDSHAKE
	BEQ.B   MON_LOOP               wait for nonzero handshake

	MOVE    #$2700,SR              block interrupts, select monitor's
	LEA     MONSTACK,A7            vector table and interrupt stack
	LEA     MONDATA,A5             and flag idle loop exit
	MOVEC   A5,VBR
	BCLR    #MC_IDLE,MON_CONTROL+1

	MOVEQ   #0,D0                  execute command
	MOVE.B  CMD_CONTROL,D0
	CMPI.W  #(CMD_TABLE_END-CMD_TABLE)/2,D0
	BPL     MON_MAIN
	MOVE.B  #CR_SUCCESS,CMD_RESULT
	MOVE.W  (CMD_TABLE,PC,D0.W*2),D1
	JMP     (CMD_TABLE,PC,D1.W)

CMD_TABLE
	DC.W    YO_MONITOR-CMD_TABLE        ;command 0 (dummy)
	DC.W    YO_MONITOR-CMD_TABLE        ;command 1 - are you there?
	DC.W    EXIT_MONITOR-CMD_TABLE      ;command 2 - exit monitor
	DC.W    READ_MEMORY-CMD_TABLE       ;command 3 - read memory
	DC.W    WRITE_MEMORY-CMD_TABLE      ;command 4 - write memory
	DC.W    READ_FPUREGS-CMD_TABLE      ;command 5 - read fpu registers
	DC.W    WRITE_FPUREGS-CMD_TABLE     ;command 6 - write fpu registers
	DC.W    READ_MMUREGS-CMD_TABLE      ;command 7 - read mmu registers
	DC.W    WRITE_MMUREGS-CMD_TABLE     ;command 8 - write mmu registers
CMD_TABLE_END

********************************************************************************
*****************   S O F T W A R E   H A N D S H A K E   **********************
********************************************************************************
YO_MONITOR
	BRA   	MON_MAIN

********************************************************************************
***********************   E X I T   M O N I T O R   ****************************
********************************************************************************

********************************************************************************
* The monitor exits via RTE from the user's interrupt stack.  If the SR M bit is
* clear, a format $0 (4-word) or format $9 (10-word) coprocessor-mid stack frame
* is placed on the interrupt stack.  If a format $9 stack frame was generated on
* monitor entry, the same format $9 stack frame is restored on exit allowing 
* interrupted coprocessor instruction to be continued.  If the M bit is set, a
* format $1 (4-word) throwaway stack frame is placed on the interrupt stack and
* the format $0 or format $9 stack frame is placed on the master stack.
********************************************************************************
EXIT_MONITOR
	LEA     ([MISP,PC],-8),A0       prepare to push format $0, $1, or $9
	MOVE.W  MSR,D0                  stack frame on user's interrupt stack
	MOVE.L  MPC,D1
	BTST    #12,D0                  test M bit (master mode)
	BEQ.B   WRITE_FMT0_FMT9

WRITE_FMT1
	BSET    #13,D0                  set S bit (supervisor mode) and push
	MOVE.W  #$107C,D2               format $1 throwaway stack frame on
	BSR     WRITE_STACKFRAME        interrupt stack then switch to master
	LEA     ([MMSP,PC],-8),A0       stack and restore value of S bit
	MOVE.W  MSR,D0

WRITE_FMT0_FMT9
	BFEXTU  MVO{0:4},D2             check MVO to determine if a format $9
	CMPI.B  #9,D2                   coprocessor-mid stack frame needs to
	BEQ.B   WRITE_FMT9              be restored on exit;  if not, set the
	ANDI.W  #$0FFF,D2               stack frame format to $0
	BRA.B   EXIT_CONTINUE

WRITE_FMT9
	MOVE.W  MVO,D2                  restore format $9 coproc-mid stack
	SUB.L   #12,A0                  frame saved on monitor entry, the
	LEA   	FMT9,A1                 last 4 words common to $0 and $9
	MOVEQ   #5,D3                   stack frames are pushed below
LOOP3	MOVE.W  (A1)+,(8,A0,D3*2)
	DBF     D3,LOOP3

EXIT_CONTINUE
	BSR     WRITE_STACKFRAME        push fmt $0 or remainder of fmt $9
	CLR.W	CMD_HANDSHAKE           flag command completion (clears command)

        MOVEA.L MVBR,A0                 restore registers
        MOVEC   A0,VBR
        MOVE.L  MCACR,D0
        ORI.L   #$808,D0                clear data/instruction caches
        MOVEC   D0,CACR
        MOVEA.L MCAAR,A0
        MOVEC   A0,CAAR
        MOVEA.L MSFC,A0                            
        MOVEC   A0,SFC
        MOVEA.L MDFC,A0
        MOVEC   A0,DFC
  	MOVEA.L MUSP,A0
        MOVE.L  A0,USP

	MOVE.L  MMSP,A0                 initialize MSP/ISP stack pointers based
	MOVE.L  MA7,A7                  on what was stacked
	BTST	#4,MSR
	BEQ.B  	XNOT_MASTER
	MOVE.L  A7,A0
	LEA     ([MISP,PC],-8),A7
XNOT_MASTER
	MOVEC   A0,MSP
        MOVEM.L MREGS,D0-D7/A0-A6

	* flush ATC cache for deMMUer if MMU configured
	BTST    #MC_MMUENABLED,MON_CONTROL+1
	BEQ.B   WAIT_FOR_CMB      
	PFLUSHA

********************************************************************************
* Wait for coordinated measurement start (if active).  The CMB_READY flag true
* indicates that it is okay to continue exiting the monitor.  If the CMB_BREAK
* flag true occurs before CMB_READY, the exit is aborted and the monitor returns
* to the main idle loop.
********************************************************************************
WAIT_FOR_CMB
	TST.B   MON_FGTRAN
CMB_WAIT_LOOP
	BTST    #1,MON_CMB
	BEQ.B   BG_EXIT_RTE                 cmb ready
	ORI.W   #1<<MC_CMBWAIT,MON_CONTROL  set cmb wait flag
	BTST    #0,MON_CMB
	BNE.B   CMB_WAIT_LOOP
	TST.B   MON_FGTRAN                  cmb break
	BRA	MON_MAIN

	ALIGN 4
BG_EXIT_RTE
	CMP.B	MON_EXIT,D6	        fix on even word boundary
	NOP
        RTE				fix on odd word boundary
	NOP

********************************************************************************
* Create a 4-word stack frame on the user's stack.  A0 must point to the user's
* stack (predecremented) and D0/D1/D2 must contain SR/PC/VO values respectively.
* This routine verifies all write accesses and aborts if it detects unresponsive
* memory or other access violation (grd/wrrom/hung bus cycle).
********************************************************************************
WRITE_STACKFRAME
	MOVE.L  A0,MA7                 save stack pointer for errors/reentry

	MOVE.W  D0,(A0)                create stack frame on user's stack
	MOVE.L  D1,2(A0)
	MOVE.W  D2,6(A0)
	BTST  	#3,MON_CMB
	BNE.B   STACK_PROBLEMS         access violation (grd/wrrom/hung)
	CMP.W   (A0),D0
	BNE.B   STACK_PROBLEMS         read != write
	CMP.L   2(A0),D1
	BNE.B   STACK_PROBLEMS         read != write
	CMP.W   6(A0),D2
	BNE.B   STACK_PROBLEMS         read != write
	RTS

STACK_PROBLEMS
	MOVE.B  #CR_ACCERROR,CMD_RESULT
	BRA     MON_MAIN

********************************************************************************
* READ_MEMORY Subroutine
********************************************************************************
READ_MEMORY
	MOVEA.L	MEM_SRCADDR,A0          ;A0=source address
	MOVEA.L MEM_DSTADDR,A1          ;A1=destination address
	LEA     (RD_JUMPTABLE,PC),A2    ;A2=address of jump table
	BRA   	RDWR_MEMORY

RD_JUMPTABLE
	DC.W RD_BYTE-RD_JUMPTABLE
	DC.W RD_WORD-RD_JUMPTABLE
	DC.W RD_LONG-RD_JUMPTABLE

RD_BYTE
	MOVES.B (A0)+,D0
	MOVE.B  D0,(A1)+
	BRA     RDWR_CHECK
RD_WORD
	MOVES.W (A0)+,D0
	MOVE.W  D0,(A1)+
	BRA     RDWR_CHECK
RD_LONG
	MOVES.L (A0)+,D0
	MOVE.L  D0,(A1)+
	BRA     RDWR_CHECK

********************************************************************************
* WRITE_MEMORY Subroutine
********************************************************************************
WRITE_MEMORY
	MOVEA.L MEM_DSTADDR,A0          ;A0=destination address
	MOVEA.L MEM_SRCADDR,A1          ;A1=source address
	LEA     (WR_JUMPTABLE,PC),A2    ;A2=address of jump table
	BRA     RDWR_MEMORY

WR_JUMPTABLE
	DC.W    WR_BYTE-WR_JUMPTABLE
	DC.W    WR_WORD-WR_JUMPTABLE
	DC.W    WR_LONG-WR_JUMPTABLE

WR_BYTE
	MOVE.B  (A1)+,D0
	MOVES.B D0,(A0)+
	BRA     RDWR_CHECK
WR_WORD
	MOVE.W  (A1)+,D0
	MOVES.W D0,(A0)+
	BRA     RDWR_CHECK
WR_LONG
	MOVE.L  (A1)+,D0
	MOVES.L D0,(A0)+
	BRA     RDWR_CHECK

********************************************************************************
RDWR_MEMORY
	MOVE.W  MEM_BYTES,D2       number of bytes
	MOVE.W  MEM_SIZE,D3        access size
	MOVE.W  MEM_FCS,D4         function code
	MOVEC   D4,SFC
	MOVEC   D4,DFC

	CLR.L   -(A7)              temporarily disable MMU if physical access
	BTST    #3,D4              is requested (original value of TC register
	BEQ.B   REALIGN            is saved on the stack)
	PMOVE   TC,(A7)
	BCLR    #7,(A7)
	BEQ.B   REALIGN
	PMOVE   (A7),TC
	BSET    #7,(A7)

REALIGN
	MOVE.W  D3,D4              start with requested access size
CHECKALIGN
	MOVE.L  A0,D5              make sure address is aligned on access
	LSR.L   D4,D5              size boundary - if not, select next
	LSL.L   D4,D5              smaller access size
	CMP.L   A0,D5
	BEQ.B   ALIGNED
MISALIGNED
	SUBQ    #1,D4              use next smaller access size
	BRA.B   CHECKALIGN
ALIGNED
	MOVE.W  D2,D1              make sure there are sufficient bytes
	LSR.W   D4,D1              to perform atleast one access - if not,
	BEQ.B   MISALIGNED         select next smaller access size

	CMP.W   D3,D4              limit loop count to 1 if smaller access
	BEQ.B   RDWR_ACCESS        size was selected
	MOVEQ   #1,D1
RDWR_ACCESS
	MOVE    D1,D5              save loop count
	SUBQ    #1,D1              adjust loop count for DBF
	MOVE.W  (0,A2,D4.W*2),D6
	LEA     (0,A2,D6.W),A3

RDWR_LOOP
	JMP     (A3)               do requested access
RDWR_CHECK
	BTST    #3,MON_CMB         check for access violation
	DBNE    D1,RDWR_LOOP
	BNE.B   RDWR_ERROR

	LSL.W   D4,D5              reduce byte count and exit if zero
	SUB.W   D5,D2
	BNE.B   REALIGN
	BRA.B   RDWR_EPILOG

RDWR_ERROR
	MOVE.B  #CR_ACCERROR,CMD_RESULT

RDWR_EPILOG
	BSR     RESTORE_WP_BITS    undo any changes to translation tables

	BTST    #7,(A7)            enable MMU if previously disabled for
	BEQ     MON_MAIN           physical access
	PMOVE   (A7),TC
	BRA     MON_MAIN
	
********************************************************************************
* READ_FPUREGS Subroutine
********************************************************************************
READ_FPUREGS
        * transfer selected MC68881/882 register values from the FPCP to the
        * monitor transfer buffer;  register values in memory are always stored
        * in the same order from low to high memory:  FPCR,FPSR,FPIAR,FP0-FP7
	LEA       MONTRANS+$400,A0
	FSAVE     -(A0)
	MOVE.W    REG_MASK,D0
	LEA       MONTRANS,A1           ;A1=destination address
	ADDA.W    MEM_BYTES,A1          ;adjusted for predecrement
RD_FPDATA
	BFTST     D0{24:8}
	BEQ.B     RD_FPIAR
	FMOVEM.X  D0,-(A1)
RD_FPIAR
	BTST      #RM_FPIAR,D0
	BEQ.B     RD_FPSR
	FMOVE.L   FPIAR,-(A1)
RD_FPSR
	BTST      #RM_FPSR,D0
	BEQ.B     RD_FPCR
	FMOVE.L   FPSR,-(A1)
RD_FPCR
	BTST      #RM_FPCR,D0
	BEQ.B     RD_RESTORE
	FMOVE.L   FPCR,-(A1)
RD_RESTORE
	TST.B     (A0)                 ;do not restore NULL state frame since
	BEQ.B     RD_FPDONE            ;it effectively resets the FPCP
	FRESTORE  (A0)+
RD_FPDONE
	BRA       MON_MAIN

********************************************************************************
* WRITE_FPUREGS Subroutine
********************************************************************************
WRITE_FPUREGS
	* write selected MC68881/882 register values from the monitor transfer
	* buffer;  register values in memory are always stored in the same
	* order from lower to higher memory:  FPCR,FPSR,FPIAR,FP0-FP7
	LEA       MONTRANS+$400,A0
	FSAVE     -(A0)
	MOVE.W    REG_MASK,D0          ;D0=registers to write
	LEA       MONTRANS,A1          ;A1=source address
WR_FPCR
	BTST      #RM_FPCR,D0
	BEQ.B     WR_FPSR
	FMOVE.L   (A1)+,FPCR
WR_FPSR
	BTST      #RM_FPSR,D0
	BEQ.B     WR_FPIAR
	FMOVE.L   (A1)+,FPSR
WR_FPIAR
	BTST      #RM_FPIAR,D0
	BEQ.B     WR_FPDATA
	FMOVE.L   (A1)+,FPIAR
WR_FPDATA
	BFTST     D0{24:8}
	BEQ.B     WR_RESTORE
	FMOVEM.X  (A1)+,D0
WR_RESTORE
	TST.B     (A0)                 ;do not restore NULL state frame since
	BEQ.B     WR_FPDONE            ;it effectively resets the FPCP undoing
	FRESTORE  (A0)+                ;any register modifications
WR_FPDONE
	BRA       MON_MAIN

********************************************************************************
* READ_MMUREGS Subroutine
********************************************************************************
READ_MMUREGS
	* read selected MMU register values into the monitor transfer buffer;
	* register values in memory are always stored in the same order from
	* lower to higher memory:  TT0/TT1/MMUSR/TC/SRP/CRP
	MOVE.W    REG_MASK,D0          ;D0=registers to read
	LEA       MONTRANS,A1          ;A1=destination address
RD_TT0
	BTST      #RM_TT0,D0
	BEQ.B     RD_TT1
	PMOVE     TT0,(A1)
	ADDA.L    #4,A1
RD_TT1
	BTST      #RM_TT1,D0
	BEQ.B     RD_MMUSR
	PMOVE     TT1,(A1)
	ADDA.L    #4,A1
RD_MMUSR
	BTST      #RM_MMUSR,D0
	BEQ.B     RD_TC
	PMOVE     MMUSR,(A1)
	ADDA.L    #2,A1
RD_TC
	BTST      #RM_TC,D0
	BEQ.B     RD_SRP
	PMOVE     TC,(A1)
	ADDA.L    #4,A1
RD_SRP
	BTST      #RM_SRP,D0
	BEQ.B     RD_CRP
	PMOVE     SRP,(A1)
	ADDA.L    #8,A1
RD_CRP
	BTST      #RM_CRP,D0
	BEQ.B     RD_MMUDONE
	PMOVE     CRP,(A1)
RD_MMUDONE
	BRA       MON_MAIN

********************************************************************************
* WRITE_MMUREGS Subroutine
********************************************************************************
WRITE_MMUREGS
	* write selected MMU register values from the monitor transfer buffer;
	* register values in memory are always stored in the same order from
	* lower to higher memory:  TT0/TT1/MMUSR/TC/SRP/CRP
	MOVE.W    REG_MASK,D0          ;D0=registers to read
	LEA       MONTRANS,A0          ;A0=source address
WR_TT0
	BTST      #RM_TT0,D0
	BEQ.B     WR_TT1
	PMOVE     (A0),TT0
	ADDA.L    #4,A0
WR_TT1
	BTST      #RM_TT1,D0
	BEQ.B     WR_MMUSR
	PMOVE     (A0),TT1
	ADDA.L    #4,A0
WR_MMUSR
	BTST      #RM_MMUSR,D0
	BEQ.B     WR_TC
	PMOVE     (A0),MMUSR
	ADDA.L    #2,A0
WR_TC
	BTST      #RM_TC,D0
	BEQ.B     WR_SRP
	PMOVE     (A0),TC
	ADDA.L    #4,A0
WR_SRP
	BTST      #RM_SRP,D0
	BEQ.B     WR_CRP
	PMOVE     (A0),SRP
	ADDA.L    #8,A0
WR_CRP
	BTST      #RM_CRP,D0
	BEQ.B     WR_MMUDONE
	PMOVE     (A0),CRP
WR_MMUDONE
	PFLUSHA 
	BRA       MON_MAIN

MONPROGEND

;****************************************************************************
; Identification strings to indicate revision level and origin of this file
;
; MKT:@(#) 64747A      A.02.02 M68030/EC030 EMUL FIRMWARE     64747     07Jun94                      
; CPR:@(#)                                                                      
; @(SUBID) MP:                                  
; @(SUBID) MAIN: /lsd/zodiac/inst/m68EC030/drivers 0.11  07Jun94 08:43:14       
;****************************************************************************

