* This version saves the whole environment including CCR.
* This is the environment structure:-
* 0	LONG pc;							4
* 4	WORD CCR;							2
* 6	LONG d0,d1,d2,d3,d4,d5,d6,d7,a0,a1,a2,a3,a4,a5,a6,a7;		64
* 70	LONG fp0[3],fp1[3],fp2[3],fp3[3],fp4[3],fp5[3],fp6[3],fp7[3];	96
* 166	ULONG res_num
* 170

reg_pc	= 0
reg_CCR	= 4
reg_d0	= 6
reg_d1	= 10
reg_d2	= 14
reg_d3	= 18
reg_d4	= 22
reg_d5	= 26
reg_d6	= 30
reg_d7	= 34
reg_a0	= 38
reg_a1	= 42
reg_a2	= 46
reg_a3	= 50
reg_a4	= 54
reg_a5	= 58
reg_a6	= 62
reg_a7	= 66
reg_fp0	= 70
reg_fp1	= 82
reg_fp2	= 94
reg_fp3	= 106
reg_fp4	= 118
reg_fp5	= 130
reg_fp6	= 142
reg_fp7	= 154
res_num = 166
sizeof_env = 170



errnum		ds.l	1	;below code will put trap number here
ilnumber	ds.l	1	;for user use
numcalls	ds.l	1	;number of times called without clear
oldtrap		ds.l	1	;pointer to the old trap address
ilfunc		ds.l	1	;pointer to user inline function or NULL


* As this eventually calls our clean-up, we can use d0, d1, a0, a1.
* The Amiga OS pushes the trap number onto the stack as a long word.
* In this first part, avoid using any registers.
* The oldtrap might need them!

trapit		cmp.l	#64,(a7)	;64 to 255 are user interrrupts
		bcc.b	.NotTrapped	;(the computer designer!)

		cmp.l	#25,(a7)
		bcs.b	.Trapped

		cmp.l	#32,(a7)	;25 to 31 not trapped (auto-vectors)
		bcc.b	.Trapped	;these are used by exec interrupts

.NotTrapped	move.l	oldtrap(pc),-(a7)	;the original trap address
		rts				;rts to old trap

*NOTE:	The trap number has to be less than 64 so it is in the lower word.

.Trapped	move.l	(a7)+,d0	;remove trap number from stack
		lea.l	errnum(pc),a0	;get address of errnum
		move.l	d0,(a0)		;put trap number in errnum for C code

;NOTE: Top of stack is now the status word for everything except
;      68000 bus or 68000 address exception.

		movea.l	(4).w,a0	;ExecBase
		move.w	$128(a0),d1	;AttnFlags of ExecBase

*NOTE:	The lowest nibble of AttnFlags contains info about the CPU.
*	If the 4 bits are all clear, it is a 68000.
*	If bit 0 set it is a 68010.
*	If bit 0 and 1 are set it is a 68020.
*	If bit 0, 1 and 2 are set it is a 68030.
*	If all 4 bits are set it is a 68040.


		and.l	#$0f,d1		;Is it 68010 or greater? (upper cleared)
		bne.b	.NOT68000	;branch if not 68000

.MR68000	cmp.w	#4,d0		;is it 2(bus error) or 3(address error)
		bcc.b	.NOTaddress

		addq.w	#8,a7		;bus or address error, extra 8 bytes

.NOTaddress	moveq.l	#6,d1		;Only status and PC to come off now
		bra.b	.donetrap

.NOT68000	lea.l	Xtable(pc),a0	;format table for 680X0 cpu's
		move.w	$06(a7),d1	;get format number (bits 15-12)
		andi.w	#$f000,d1	;isolate the number
		rol.w	#4,d1		;place upper 4 bits in lower 4 bits
		add.w	d1,a0		;there is only a byte in d1...
		move.b	(a0),d1

.donetrap	move.w	(a7),d0		;get status
		add.w	d1,a7		;TOTALLY correct the stack
		andi.w	#$5fff,d0	;supervisor/trace off in status
		move.w	d0,SR


* NOW IN USER MODE with user stack pointer

		move.l	ilfunc(pc),d0
		beq.b	.initenv		;branch if not inline

		lea.l	numcalls(pc),a1
		addq.l	#1,(a1)			;increment number of calls
		bne.b	.calls_ok

		subq.l	#1,(a1)			;$ffffffff is maximum!

* The user function must not expect anything in d0,d1,a0,a1!
* It receives no parameters/args and returns nothing
* This function should not be stack intensive!

.calls_ok	movea.l	d0,a0			;inline function
		jsr	(a0)			;execute the user function
	
		lea.l	inlineenv(pc),a0
		bra.b	restore_a0

* We have an initial trap; the program is going to quit.
* The trap should be freed by the cleanup code.

.initenv	lea.l	initenv(pc),a0		;it must be initial environment
		bra.b	restore_a0

*---------------------------------------------------------------------------*

* Expects address of environment on stack so it can be called from outside

restore		movea.l	4(a7),a0

* Expects address of environment in a0

restore_a0	movea.l	(4).w,a1
		btst.b	#4,$129(a1)			;Is there an FPCP?
		beq.b	.skip

		fmovem.x	reg_fp0(a0),fp0-fp7	;recover FP registers

.skip		move.l	reg_a7(a0),a1		;get the SP
		move.l	(a0),(a1)		;insert pc return address
		addq.l	#1,res_num(a0)
		bne.b	.res_num_ok

		subq.l	#1,res_num(a0)		;max is $ffffffff

.res_num_ok	move.w	reg_CCR(a0),CCR		;recover CCR
		movem.l	reg_d0(a0),d0-d7/a0-a7	;recover regs,CCR unchanged
		rts				;All Done, same environment



*NOTE: We can't do much if we're left in trace mode!
*      An almost immediate interrupt occurs, very erratic operation....
*
* Table of the TOTAL number of bytes to add to the stack for 68010 or greater.
* This includes the status word and the pc long word.
* This is governed by the format nibble in the format word
*       <<-- I'm not sure of all these values... -->>
* One would expect a minimum of 6 (status and pc), but some are zero!
* Maybe they don't exist...
* An RTE doesn't work for address exception (Why?) so adding to the stack
* is the only way to correct it.

Xtable		dc.b	8,8		;68010, 68020, 68030, 68040
		dc.b	12,0
		dc.b	0,0
		dc.b	0,0
		dc.b	58,20
		dc.b	32,92
		dc.b	0,0
		dc.b	0,0

*---------------------------------------------------------------------------*

* The address of the environment to be filled is 2nd on the stack
* The return address is 1st on the stack (called from jsr)
* Use movem and movea to avoid changing the CCR

save_env	movem.l	a0,-(a7)		;a0 at base of stack

* env at 8  -> a0, existing a0 to be put on the stack for save_env_a0
* rts at 4

		movem.l	4+4(a7),a0		;get environment to a0
		bsr.b	save_env_a0

;returns with env in a0 (as it was)

		move.l	4(a7),reg_pc(a0)	;return address
		move.w	reg_CCR(a0),CCR		;recover CCR
		movea.l	(a7)+,a0		;as it was...	
		rts

*---------------------------------------------------------------------------*

* This expects a0 to be saved on the stack on the stack
* The current a0 holds the address of the environment struct to be filled
* This is dodgy code...

save_env_a0	movem.l	d0-d1/a1/a6,-(a7)	;save dodgy regs

		movem.l	a0,-(a7)		;save env
		movea.l	(4).w,a6		;SysBase
		jsr	$fffffdf0(a6)		;GetCC
		movea.l	(a7)+,a0		;recover env

		move.w	d0,reg_CCR(a0)		;save CCR in env

		movem.l	(a7)+,d0-d1/a1/a6	;recover dodgy regs

		move.l	(a7),(a0)		;save the return at reg_pc
		movem.l	d0-d7,reg_d0(a0)	;save registers d0-d7
		move.l	4(a7),reg_a0(a0)	;save a0 (off the stack)
		movem.l	a1-a7,reg_a1(a0)	;save registers a1-a7

		addq.l	#8,reg_a7(a0)		;a0 and return on the stack

		movea.l	(4).w,a1
		btst.b	#4,$129(a1)			;Is there a FPCP?
		beq.b	.skip

		fmovem.x	fp0-fp7,reg_fp0(a0)	;save FP registers

.skip		move.w	reg_CCR(a0),CCR		;recover CCR
		movea.l	reg_a1(a0),a1		;recover a1
		rts				;All Done, same environment

*---------------------------------------------------------------------------*

;NOTE movem and lea both leave CCR unchanged
* (This doesn't keep status unchanged)


set_init_trap	movem.l	a0,-(a7)
		lea.l	initenv(pc),a0
		bsr.b	save_env_a0		;returns with a0 unchanged
* NOTE:
* The grand cleanup function MUST correct the stack.
* The C "exit()" function would do nicely.
* This will end up with the same stack pointer as when initialized.
* The chances are that it's not OK to simply RTS...
* That is why the stack has to be corrected.

		move.l	4+4(a7),(a0)		;Our grand cleanup function
						;placed at reg_pc

		bsr.b	put_trap		;Insert new tc_TrapCode

		movea.l	(a7)+,a0
		rts

*---------------------------------------------------------------------------*

;NOTE movem and lea both leave CCR unchanged
* This saves the positional information and initializes the user function
* (This doesn't keep status unchanged)

set_iltrap	movem.l	a0,-(a7)
		lea.l	inlineenv(pc),a0
		bsr.b	save_env_a0		;returns with a0 unchanged
		move.l	4(a7),(a0)		;the return address...
		lea.l	ilfunc(pc),a0		;the inline user function
		move.l	4+4(a7),(a0)		;insert it's address here
		lea.l	numcalls(pc),a0		;Clear numcalls
		clr.l	(a0)
		bsr.b	put_trap		;Insert new tc_TrapCode
		movea.l	(a7)+,a0
		rts

*---------------------------------------------------------------------------*
* Insert new tc_TrapCode, assumes calling routine has saved a0

put_trap	move.l	a1,-(a7)
		lea.l	oldtrap(pc),a1
		tst.l	(a1)
		bne.b	.trapped

		movea.l	(4).w,a0		;ExecBase
		movea.l	$114(a0),a0		;ThisTask

		lea.l	$32(a0),a0		;get tc_TrapCode address
		move.l	(a0),(a1)		;save old trap
		lea.l	trapit(pc),a1
		move.l	a1,(a0)			;insert our new trap

.trapped	movea.l	(a7)+,a1
		rts

*---------------------------------------------------------------------------*

* Insert a user-defined number for later check
* The number should be on the stack
* (This doesn't keep status unchanged)

set_ilnumber	move.l	a0,-(a7)
		lea.l	ilnumber(pc),a0
		move.l	4+4(a7),(a0)
		movea.l	(a7)+,a0
		rts

*---------------------------------------------------------------------------*

* if there is no inline user function an inline trap hasn't been set
* (This doesn't keep status unchanged)

clear_iltrap	move.l	a0,-(a7)
		lea.l	initenv(pc),a0
		tst.l	(a0)			;if nothing, trap must be freed
		bne.b	.il_only

		bsr.w	free_trap
		bra.b	.done

.il_only	lea.l	ilfunc(pc),a0
		clr.l	(a0)

.done		movea.l	(a7)+,a0
		rts


*---------------------------------------------------------------------------*

* Free the initial trap
* (This doesn't keep status unchanged)

clear_init_trap	move.l	a0,-(a7)

		lea.l	ilfunc(pc),a0
		tst.l	(a0)			;if nothing, trap must be freed
		bne.b	.init_only

		bsr.w	free_trap
		bra.b	.done

.init_only	lea.l	initenv(pc),a0		;this marks it as unuseable
		clr.l	(a0)

.done		movea.l	(a7)+,a0
		rts


*---------------------------------------------------------------------------*


* Restore the original tc_TrapCode
* (This doesn't keep status unchanged)

free_trap	movem.l	a0-a1,-(a7)

		lea.l	ilfunc(pc),a0
		clr.l	(a0)			;ensure it can't be re-used

		lea.l	initenv(pc),a0		;ensure it can't be re-used
		clr.l	(a0)

		movea.l	(4).w,a0		;ExecBase
		movea.l	$114(a0),a0		;ThisTask

		lea.l	50(a0),a0		;address of existing tc_TrapCode
		lea.l	oldtrap(pc),a1		;address of saved trap

		tst.l	(a1)			;ensure we have something
		beq.b	.quit			;might be called twice!

		move.l	(a1),(a0)		;restore old trap
		clr.l	(a1)			;ensure it can't be re-used

.quit		movem.l	(a7)+,a0-a1
		rts



* 1st long word is the address
initenv		ds.b	sizeof_env	;environment for grand cleanup

* 1st long word is the address
inlineenv	ds.b	sizeof_env	;environment for inline






* << END OF FILE >> *