.include "nios_macros.s"
	.text
	.global nr_installuserisr
	.global nr_installuserisr2

;-------------------------------
; void nr_installuserisr(int trapNumber, nios_isrhandlerproc *trapProc, int context);
; void nr_installuserisr2(int trapNumber, nios_isrhandlerproc2 *trapProc, int context);
;
; Definition: "System ISR" -- the routine pointed to in
;             the vector table. It cannot easily call other routines,
;             and traps are disabled.
;
;             "User ISR" -- A normal routine with normal
;             facilities; the only difference from a normal (noninterrupt)
;             routine is that it can only be itself interrupted by
;             a higher priority (lower numbered) interrupt.
;
; This installs a trap handler for a particular trap number, and
; ensures that the trap handler (trapProc) is called with
; interrupts reenabled (for interrupts of higher priority only),
; and passed its necessary "context" value, which can be anything.
;
; The two names are synonyms for the same routine -- but one
; is typed to accept a nios_isrhandlerproc2, which has additional
; arguments. The arguments are always passed, but would be ignored
; by original nios_isrhandlerproc-prototype routines.
;
;----------------------------------------
;         Name: nr_installuserisr
;  Description: Install a trap routine
;        Input: %o0 = trap number
;               %o1 = trap handler routine
;               %o2 = context to be passed to handler
;       Output: none
; Side Effects: %g0 & %g1 altered
;    CWP Depth: 0
;

.ifdef __nios32__
	.equ nmul,2
.else
	.equ nmul,1
.endif

.equ _ISRManagerDebugging_,0

nr_installuserisr:
nr_installuserisr2:
	MOV	%g0,%o0			; %g0 = Trap Number
	LSLI	%g0,nmul		; (2 on n32, 1 on n16) %g0 = offset into vector table

	MOVIA	%g1,_ns_userisrfunnel@h	; %g1 = funnel for the user trap handlers
	MOVIA	%g2,nasys_vector_table	; %g2 -> vector table
	ADD	%g2,%g0			; %g2 -> entry for this trap in vector table
	LSLI	%g0,1			; our info table is 2 words per trap...
	MOVIA	%g3,_nr_userisrtable
	ADD	%g3,%g0			; %g3 -> entry in our User ISR Table

	ST	[%g3],%o1		; Store the handler routine...
	PFX	1
	ST	[%g3],%o2		; Store the handler routine's context.

	ST	[%g2],%g1		; install the funnel proc last, of course, safety.

	JMP	%o7
	NOP

	.comm	_nr_userisrtable,64*2*2*nmul,4	; two words per 64 ISR's: proc & context

.equ __nios_flat__, 1	; comment this line out to use the normal register windowed isr manager

.if __nios_flat__

;  
; ISR funnel handler for non-flat systems running flat and/or non-flat code.  
;
; THIS IS NOT FULLY TESTED
;
.equ FLAT_STACK_SPACE, 	((24+19)*2*nmul)

_ns_userisrfunnel:

	SUBIP	%fp,FLAT_STACK_SPACE	; space to save the %g registers, ISTATUS, %o0, & return
	MOV	%sp,%fp			; let %sp=%fp, till we RESTORE.

	;
	; Save the globals: ISR may use them.
	;
	STS	[%sp,24+0],%g0
	STS	[%sp,24+1],%g1
	STS	[%sp,24+2],%g2
	STS	[%sp,24+3],%g3
	STS	[%sp,24+4],%g4
	STS	[%sp,24+5],%g5
	STS	[%sp,24+6],%g6
	STS	[%sp,24+7],%g7

	;
	; Save the return address to the interruptee, across the RESTORE
	;
	MOV	%g4,%o7

	RESTORE
	;
	; Save trap return address: calling
	; the user ISR will alter it.
	;
	STS	[%sp,24+8],%g4			; save the trap return address

	;
	; Save CTL1, ISTATUS, so we can later TRET
	;
	PFX	1				; CTL1 = ISTATUS
	RDCTL	%g0
	STS	[%sp,24+9],%g0			; save ISTATUS
	
        ; Save %o's, except %sp (%o6)
        STS    	[%sp,24+10],%o0
        STS    	[%sp,24+11],%o1
        STS    	[%sp,24+12],%o2
        STS    	[%sp,24+13],%o3
        STS    	[%sp,24+14],%o4
        STS    	[%sp,24+15],%o5
        STS    	[%sp,24+16],%o7
        
        ; Save %i7 and %fp
	STS	[%sp,24+17],%i7
        STS    	[%sp,24+18],%fp
        
	;
	; Find the user ISR in our table, and its context
	;

	RDCTL	%g5
	LSRI	%g5,9
	ANDIP	%g5,0x003f			; %g5 = irq number now

	MOV	%g0,%g5
.ifdef __nios32__
	LSLI	%g0,3
.else
	LSLI	%g0,2				; %g0 = offset into user isr table
.endif

	MOVIA	%g1,_nr_userisrtable
	ADD	%g0,%g1				; %g1 -> (proc,context) for this trap...
	LD	%g2,[%g0]			; %g2 = proc to call
	PFX	1
	LD	%o0,[%g0]			; %o0 = context for that proc

	;
	; Enable Interrupts
	;

	PFX 9
	WRCTL	%g0				; (value doesn't matter)

	;
	; And call the user ISR
	; user_isr_proc(context, irq_number, interruptee_pc)
	;

	MOV	%o1,%g5
	CALL	%g2
	MOV	%o2,%g4

	;
	; Disable interrupts.
	;

	PFX	8
	WRCTL	%g0			   

	; Assume %sp is not clobbered.  Flat code restores it, non-flat
        ; code uses it as %fp.

        ; Save %o's, except %sp (%o6)
        LDS    	%o0,[%sp,24+10]
        LDS    	%o1,[%sp,24+11]
        LDS    	%o2,[%sp,24+12]
        LDS    	%o3,[%sp,24+13]
        LDS    	%o4,[%sp,24+14]
        LDS    	%o5,[%sp,24+15]
        LDS    	%o7,[%sp,24+16]
        
        ; Save %i7 and %fp
	LDS	%i7,[%sp,24+17]
        LDS    	%fp,[%sp,24+18]

	;
	; To return from the interrupt, we must
	; restore all registers (%g's, ISTATUS, %i0)
	;
	; And, we have to bump the CWP down, for
	; only one reason: we need a register to load
	; with the address to return to from the
	; interrupt via TRET.
	;

	RDCTL	%g0
	SUBIP	%g0,0x0010		; CWP = what it was when we arrived at system ISR here.
	WRCTL	%g0			; (virtual SAVE: CWP -= 1)
	NOP				; curious WRCTL delay slot

	;
	; Restore some registers
	;

	MOV	%sp,%fp
	ADDIP	%fp,FLAT_STACK_SPACE		; Set interruptee's stack pointer to what it was.

	LDS	%o7,[%sp,24+8]			; restore the trap return address

	LDS	%g0,[%sp,24+9]			; get old ISTATUS
	PFX	1				; CTL1 = ISTATUS
	WRCTL	%g0				; and put it in.

	LDS	%g0,[%sp,24+0]			; restore all them %g registers
	LDS	%g1,[%sp,24+1]
	LDS	%g2,[%sp,24+2]
	LDS	%g3,[%sp,24+3]
	LDS	%g4,[%sp,24+4]
	LDS	%g5,[%sp,24+5]
	LDS	%g6,[%sp,24+6]
	LDS	%g7,[%sp,24+7]

	TRET	%o7

	NOP

.else

; NON-FLAT CPU
;
; This "user ISR funnel" code is a sort
; of tricky. A trap or interrupt bumps
; the CWP down one, and stores the old
; PC value in (new CWP's) %o7, the old
; processor STATUS into ISTATUS, and
; transfers control here.
;
; What we'll do is:
;   Preserve the %g registers on the stack,
;   Preserve ISTATUS on the stack,
;   Preserve the return address on the stack
;
;   Bump CWP up one
;   Reenable interrupts (for anything higher
;   priority than we're handling)
;
;   Execute a normal CALL to the user ISR
;
;   (Do the opposite of the above)
;   TRET %o7.
;
; Stack frame looks like:
;
;      sp[24+13] = %i7		(we alter %i7 when we CALL -- it's the ISR's return addr!)
;      sp[24+12] = %i0		(we alter %i0 to pass context to ISR.)
;      sp[24+11] = %i0		(we alter %i0 to pass context to ISR.)
;      sp[24+10] = %i0		(we alter %i0 to pass context to ISR.)
;      sp[24+9] = ISTATUS
;      sp[24+8] = RETURN ADDRESS
;      sp[24+7] = %g7
;      sp[24+...] = ...
;      sp[24+0] = %g0
;      sp[0..15] = space for CWP Underflow, as usual.
;                  there's already space for it, of course, but second-guessing where
;                  the C-compiler put stuff and offsetting more is too tedious, so we
;                  just bump further down.
;      ----sp----

_ns_userisrfunnel:

	SUBIP	%fp,(24+14)*2*nmul	; space to save the %g registers, ISTATUS, %o0, & return
	MOV	%sp,%fp			; let %sp=%fp, till we RESTORE.

	;
	; Save the globals: ISR may use them.
	;
	STS	[%sp,24+0],%g0
	STS	[%sp,24+1],%g1
	STS	[%sp,24+2],%g2
	STS	[%sp,24+3],%g3
	STS	[%sp,24+4],%g4
	STS	[%sp,24+5],%g5
	STS	[%sp,24+6],%g6
	STS	[%sp,24+7],%g7

	;
	; Save the return address to the interruptee, across the RESTORE
	;
	MOV	%g4,%o7

	RESTORE
	;
	; CWP is now in the interruptee's level.
	; Tread lightly.
	;

	;
	; Save trap return address: calling
	; the user ISR will alter it.
	;
	STS	[%sp,24+8],%g4			; save the trap return address

	;
	; Save CTL1, ISTATUS, so we can later TRET
	;
	PFX	1				; CTL1 = ISTATUS
	RDCTL	%g0
	STS	[%sp,24+9],%g0			; save ISTATUS

	;
	; Save %o0, since we pass an argument there.
	;
	STS	[%sp,24+10],%o0			; save %o0, which we use to pass context.
	STS	[%sp,24+11],%o1			; save %o1, which we use to pass context.
	STS	[%sp,24+12],%o2			; save %o2, which we use to pass context.

	;
	; Save %o7, since it's not ours, yet we're
	; doing a CALL, which puts return address
	; into %o7.
	;
	STS	[%sp,24+13],%o7

	;
	; Reenable interrupts: this is the earliest opportunity.
	;

	;PFX	9
	;WRCTL	%g0				; (value doesn't matter)

	;
	; Find the user ISR in our table, and its context
	;

	RDCTL	%g5

	LSRI	%g5,9
	ANDIP	%g5,0x003f			; %g5 = irq number now

	MOV	%g0,%g5
.ifdef __nios32__
	LSLI	%g0,3
.else
	LSLI	%g0,2				; %g0 = offset into user isr table
.endif

	MOVIA	%g1,_nr_userisrtable
	ADD	%g0,%g1				; %g1 -> (proc,context) for this trap...
	LD	%g2,[%g0]			; %g2 = proc to call
	PFX	1
	LD	%o0,[%g0]			; %o0 = context for that proc

	;
	; Enable Interrupts
	;

	PFX 9
	WRCTL	%g0				; (value doesn't matter)

	;
	; And call the user ISR
	; user_isr_proc(context, irq_number, interruptee_pc)
	;

	MOV	%o1,%g5
	CALL	%g2
	MOV	%o2,%g4

	;
	; Disable interrupts.
	;

	PFX	8
	WRCTL	%g0				; (value doesn't matter)

	;
	; To return from the interrupt, we must
	; restore all registers (%g's, ISTATUS, %i0)
	;
	; And, we have to bump the CWP down, for
	; only one reason: we need a register to load
	; with the address to return to from the
	; interrupt via TRET.
	;

	RDCTL	%g0
	SUBIP	%g0,0x0010		; CWP = what it was when we arrived at system ISR here.
	WRCTL	%g0			; (virtual SAVE: CWP -= 1)
	NOP				; curious WRCTL delay slot

	;
	; Restore some registers
	;

	MOV	%sp,%fp
	ADDIP	%fp,(24+14)*2*nmul		; Set interruptee's stack pointer to what it was.

	LDS	%o7,[%sp,24+8]			; restore the trap return address

	LDS	%g0,[%sp,24+9]			; get old ISTATUS
	PFX	1				; CTL1 = ISTATUS
	WRCTL	%g0				; and put it in.

	LDS	%i0,[%sp,24+10]			; restore %i0, which we corrupted passing context.
	LDS	%i1,[%sp,24+11]			; restore %i1, which we corrupted passing context.
	LDS	%i2,[%sp,24+12]			; restore %i2, which we corrupted passing context.
	LDS	%i7,[%sp,24+13]			; restore %i7, which we corrupted passing context.

	LDS	%g0,[%sp,24+0]			; restore all them %g registers
	LDS	%g1,[%sp,24+1]
	LDS	%g2,[%sp,24+2]
	LDS	%g3,[%sp,24+3]
	LDS	%g4,[%sp,24+4]
	LDS	%g5,[%sp,24+5]
	LDS	%g6,[%sp,24+6]
	LDS	%g7,[%sp,24+7]

	TRET	%o7

	NOP
        
.endif ; __nios_flat__

; end of file