********************************************************************************
* 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 HP64748 Motorola 68020/EC020 Emulator
*
* The emulation monitor is the vehicle by which the following emulator
* functions are performed:
*
*      read/write emulation/target memory space
*      display/modify 68020 registers
*      display/modify 68881/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 $800 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 68020
	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    BREAKPOINT_ENTRY  Illegal Instruction
		DC.L    ERROR_ENTRY       Zero Divide
		DC.L    ERROR_ENTRY       CHK,CHK2 Instruction
		DC.L    ERROR_ENTRY       cpTRAPcc,TRAPcc,TRAPV Instructions
		DC.L    ERROR_ENTRY       Privilege violation
		DC.L    TRACE_ENTRY       Trace Entry
		DC.L    ERROR_ENTRY       Unimplemented A-Line Instruction
		DC.L    ERROR_ENTRY       Unimplemented F-Line Instruction
		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   8,ERROR_ENTRY     Unassigned, Reserved
		DC.L    ERROR_ENTRY       Spurious Interrupt
		DCB.L   6,ERROR_ENTRY     Level 1-6 Interrupt Auto Vector
		DC.L    INTERRUPT_ENTRY   Level 7 Interrupt Auto Vector
		DCB.L   32,ERROR_ENTRY    remaining vector locations

********************************************************************************
* Monitor Variables - 256 bytes
********************************************************************************
MON_RANGE	DC.L MONPROG,MONPROGEND   code range for CMP2

* Monitor Control Registers 
CMD_HANDSHAKE   DC.B 0            nonzero if command being 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 (forgound only)
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            destionation address for read/write
MEM_FCS         DC.W 0            function codes to use for read/write
MEM_SIZE        DC.W 0            access size to use 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 register mask
		DS.B 42           spares

* 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 - not used
		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 - not used

* 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

FMT9            DS.W 10           copy of entry/exit format $9 stack frame
TMP             DS.L 2            spares
TMP2            DS.L 2

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

********************************************************************************
* Monitor Interrupt Stack - 512 bytes
********************************************************************************
		DS.B $200
MONSTACK

********************************************************************************
* Miscellaneous 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

* Equates for CMD_RESULT (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 memory problem
CR_FMTERROR     EQU    14     ;failed due to format error

* Equates for MON_CONTROL
MC_TRACEMODE    EQU     0     ;single step being performed
MC_IDLE         EQU     3     ;reserved

* Equates for MON_LASTENTRY (vector offsets)
ML_UNDEFINED    EQU     0    
ML_RESET        EQU     1     ;emulation/target reset
ML_BREAKPOINT   EQU  $010     ;software breakpoint
ML_INTERRUPT    EQU  $07C     ;interrupt
ML_TRACE        EQU  $024     ;single step

* 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
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

********************************************************************************
****************  M O N I T O R   P R O G   S E C T I O N  *********************
********************************************************************************
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_REENTRY           a trace exception, set lsb to prevent
	ORI.W   #1,TMP                  user's stack frame from being removed
	BRA     CHECK_REENTRY           (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     MON_MAIN

********************************************************************************
* Monitor entry point for software breakpoints.  When the emulator detects a
* breakpoint acknowledge cycle for the configured breakpoint, it "jams" this
* address onto the data bus when the processor fetches the illegal instruction
* vector. 
********************************************************************************
BREAKPOINT_ENTRY
	MOVE.W  #ML_BREAKPOINT,TMP      save entry point
	BRA     CHECK_REENTRY

********************************************************************************
* Monitor entry point for level 7 interrupt.  During the interrupt acknowledge
* bus cycle, the emulation hardware "jams" this address on the data bus during
* the vector fetch.  A format $0 stack frame is written to memory.  A format $1
* throwaway stack is also written to memory if the interrupt stack was not
* active at the time of the interrupt.
********************************************************************************
INTERRUPT_ENTRY
	MOVE.W  #ML_INTERRUPT,TMP       save entry point
	BRA     CHECK_REENTRY

********************************************************************************
* 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.
********************************************************************************
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_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
	ORI.W   #$0700,SR               block interrupts
	MOVE.L  A0,TMP2
	MOVE.L  2(A7),A0
	CMP2.L  MON_RANGE,A0            compare stacked PC
	MOVE.L  TMP2,A0
	BCS.B   USER_ENTRY
	BTST.B  #5,(A7)                 compare stacked SR
	BEQ.B   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 copies the exception stack frame into the transfer buffer then
* terminates the command with an error.
********************************************************************************
MON_EXCEPTION
	BFEXTU	6(A7){6:8},D0           use vector number as error code
	MOVE.B  D0,CMD_RESULT
	
	LEA     MON_TRANS,A0            copy exception stack frame to the
	BFEXTU  6(A7){0:4},D0           start of the monitor transfer buffer
	MOVE.W  (FRAMESIZE,PC,D0*2),D0  for processing by the drivers
	LSR.W   #2,D0
	SUBQ    #1,D0
ME_LOOP	MOVE.L  (A7)+,(A0)+
	DBF     D0,ME_LOOP
	BRA     MON_MAIN

********************************************************************************
**************  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
	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                  save vector offset as entry point
	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             stack frame is a format $1 throwaway
	CMPI.B  #1,D0                   frame, pop and discard the stack frame
	BNE.B   NOT_THROWAWAY           and switch 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.
*
* Note:  the first TST.B in the monitor loop is required to give the drivers
* a chance to access CMD_CONTROL when the instruction cache is enabled.
********************************************************************************
MON_MAIN
	CLR.B   CMD_HANDSHAKE          flag command completion

	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            select user's interrupt stack and set 
	CMP.B   MON_INTRLEVEL,D0       priority mask to the configured value
	BGE.B   USE_PRIOR              or the value in effect on monitor 
	MOVE.B  MON_INTRLEVEL,D0       entry, whichever is lower
USE_PRIOR
	MOVE.W  SR,D1
	BFINS   D0,D1{19:5}
	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   MCACR                  don't remove!
	TST.B   CMD_HANDSHAKE
	BEQ.B   MON_LOOP               wait for handshake bit

	ORI.W   #$0700,SR              block interrupts, select monitor's
	LEA     MONSTACK,A7            vector table and interrupt stack
	LEA     MONDATA,A0
	MOVEC   A0,VBR

	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
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
WF_LOOP	MOVE.W  (A1)+,(8,A0,D3*2)
	DBF     D3,WF_LOOP

EXIT_CONTINUE
	BSR     WRITE_STACKFRAME        push fmt $0 or remainder of fmt $9

        MOVEA.L MVBR,A0                 restore registers
        MOVEC   A0,VBR
        MOVE.L  MCACR,D0
        ORI.L   #$8,D0                  clear instruction cache
        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
	CLR.B	CMD_HANDSHAKE           flag command completion
	RTE

********************************************************************************
* 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)
	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

********************************************************************************
************************   R E A D   M E M O R Y   *****************************
********************************************************************************
READ_MEMORY
	MOVEA.L	MEM_SRCADDR,A0          A0=source address
	MOVEA.L MEM_DSTADDR,A1          A1=destination address
	LEA     RD_JUMPTABLE,A2         A2=address of jump table
	BRA   	RDWR_MEMORY

RD_JUMPTABLE
	DC.W    RD_BYTES-RD_JUMPTABLE
	DC.W    RD_WORDS-RD_JUMPTABLE
	DC.W    RD_LONGS-RD_JUMPTABLE

RD_BYTES
	MOVES.B (A0)+,D0
	MOVE.B  D0,(A1)+
	DBF     D1,RD_BYTES
	RTS
RD_WORDS
	MOVES.W (A0)+,D0
	MOVE.W  D0,(A1)+
	DBF     D1,RD_WORDS
	RTS
RD_LONGS
	MOVES.L (A0)+,D0
	MOVE.L  D0,(A1)+
	DBF     D1,RD_LONGS
	RTS

********************************************************************************
***********************   W R I T E   M E M O R Y   ****************************
********************************************************************************
WRITE_MEMORY
	MOVEA.L MEM_DSTADDR,A0          A0=destination address
	MOVEA.L MEM_SRCADDR,A1          A1=source address
	LEA     WR_JUMPTABLE,A2         A2=address of jump table
	BRA     RDWR_MEMORY

WR_JUMPTABLE
	DC.W    WR_BYTES-WR_JUMPTABLE
	DC.W    WR_WORDS-WR_JUMPTABLE
	DC.W    WR_LONGS-WR_JUMPTABLE

WR_BYTES
	MOVE.B  (A1)+,D0
	MOVES.B D0,(A0)+
	DBF     D1,WR_BYTES
	RTS
WR_WORDS
	MOVE.W  (A1)+,D0
	MOVES.W D0,(A0)+
	DBF     D1,WR_WORDS
	RTS
WR_LONGS
	MOVE.L  (A1)+,D0
	MOVES.L D0,(A0)+
	DBF     D1,WR_LONGS
	RTS

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

REALIGN
	MOVE.W  D3,D4                   start with original access size
CHECKALIGN
	MOVE.L  A0,D5                   check address alignment
	LSR.L   D4,D5
	LSL.L   D4,D5
	CMP.L   A0,D5
	BEQ.B   ALIGNED
MISALIGNED
	SUBQ    #1,D4                   use next smaller access size
	BRA.B   CHECKALIGN
ALIGNED
	MOVE.W  D2,D1                   check for sufficient byte count
	LSR.W   D4,D1
	BEQ.B   MISALIGNED
	CMP.W   D3,D4
	BEQ.B   RDWR_ACCESS
	MOVEQ   #1,D1                   aligning - only do single access
RDWR_ACCESS
	MOVE    D1,D5                   save loop count
	SUBQ    #1,D1                   adjust loop count for DBF
	MOVE.W  (0,A2,D4.W*2),D6
	JSR     (0,A2,D6.W)             do requested access(es)
	LSL.W   D4,D5                   decrement byte count
	SUB.W   D5,D2           
	BNE.B   REALIGN                 access remaining bytes
	BRA     MON_MAIN

********************************************************************************
******************   R E A D   F P U   R E G I S T E R S   *********************
********************************************************************************

********************************************************************************
* Read selected MC68881/882 register values into the monitor transfer buffer.
* Register always stored in the following order:  FPCR,FPSR,FPIAR,FP0-FP7.
********************************************************************************
READ_FPUREGS
	LEA      MON_TRANS+$400,A0  save coprocessor context in unused
	FSAVE    -(A0)              memory at end of transfer buffer
	MOVE.W   REG_MASK,D0
	LEA      MON_TRANS,A1
	ADDA.W   MEM_BYTES,A1
RD_FPDATA
	BFTST    D0{24:8}
	BEQ.B    RD_FPIAR
	FMOVEM.X D0,-(A1)
RD_FPIAR
	BTST.L   #RM_FPIAR,D0
	BEQ.B    RD_FPSR
	FMOVE.L  FPIAR,-(A1)
RD_FPSR
	BTST.L   #RM_FPSR,D0
	BEQ.B    RD_FPCR
	FMOVE.L  FPSR,-(A1)
RD_FPCR
	BTST.L   #RM_FPCR,D0
	BEQ.B    RD_RESTORE
	FMOVE.L  FPCR,-(A1)
RD_RESTORE
	TST.B    (A0)               restore coprocessor context;  NULL state
	BEQ      MON_MAIN           frames are not restored since restoring
	FRESTORE (A0)+              one causes a FPCP hardware reset undoing
	BRA      MON_MAIN           any register changes

********************************************************************************
*****************   W R I T E   F P U   R E G I S T E R S   ********************
********************************************************************************

********************************************************************************
* Write selected MC68881/882 register values from the monitor transfer buffer.
* Register always stored in the following order:  FPCR,FPSR,FPIAR,FP0-FP7.
********************************************************************************
WRITE_FPUREGS
	LEA      MON_TRANS+$400,A0  save coprocessor context in unused
	FSAVE    -(A0)              memory at end of transfer buffer
	MOVE.W   REG_MASK,D0
	LEA      MON_TRANS,A1
WR_FPCR
	BTST.L   #RM_FPCR,D0
	BEQ.B    WR_FPSR
	FMOVE.L  (A1)+,FPCR
WR_FPSR
	BTST.L   #RM_FPSR,D0
	BEQ.B    WR_FPIAR
	FMOVE.L  (A1)+,FPSR
WR_FPIAR
	BTST.L   #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)               restore coprocessor context;  NULL state
	BEQ      MON_MAIN           frames are not restored since restoring
	FRESTORE (A0)+              one causes a FPCP hardware reset undoing
	BRA      MON_MAIN           any register changes

MONPROGEND

;****************************************************************************
; Identification strings to indicate revision level and origin of this file
;
; MKT:@(#) 64748A      A.02.01 68020 EMULATION FIRMWARE       64748     11Jan94                      
; CPR:@(#)                                                                      
; @(SUBID) MP:                                  
; @(SUBID) MAIN: /lsd/zodiac/inst/m68020/drivers 0.09  10Jan94 18:11:40         
;****************************************************************************

