.extern _tpl_run_elected
.extern _tpl_reentrancy_counter
.extern _tpl_eeic_handler
.extern _tpl_kernel_stack_bottom


; could be generated by goil? =>
TPL_KERN_OFFSET_S_RUNNING .equ 0
TPL_KERN_OFFSET_S_ELECTED .equ 4
TPL_KERN_OFFSET_RUNNING .equ 8
TPL_KERN_OFFSET_ELECTED .equ 12
TPL_KERN_OFFSET_RUNNING_ID .equ 16
TPL_KERN_OFFSET_ELECTED_ID .equ 20
TPL_KERN_OFFSET_NEED_SWITCH .equ 24
TPL_KERN_OFFSET_NEED_SCHEDULE .equ 25

NO_NEED_SWITCH_NOR_SCHEDULE	.equ 0
NEED_SWITCH	                .equ 1
; <=

.public	__irq_handler
__irq_handler:
	; Save volatile registers on the calling task stack
	pushsp r2, r2
	pushsp r4, r20
	stsr 1, r20 ; retrieve eipsw: calling status
	pushsp r20, r20
	stsr 0, r20 ; retrieve eipc: calling pc
	pushsp r20, r20
	
	; Switch to kernel stack
	mov r3, r4	; save current sp
	movhi HIGHW1(#_tpl_kernel_stack_bottom), r0, r11 ; load kernel sp from structure
	movea LOWW(#_tpl_kernel_stack_bottom), r11, r3 ; update sp

	; We have now kernel stack
	pushsp r4, r4 ; push running stack pointer on kernel stack
	pushsp r31, r31 ; push link register on kernel stack

	; Initialize NEED_SWITCH and SCHEDULE in tpl_kern structure
	mov NO_NEED_SWITCH_NOR_SCHEDULE, r10
	mov #_tpl_kern, r12
	st.w r10, TPL_KERN_OFFSET_NEED_SWITCH[r12]

	; Call interrupt handler
	jarl _tpl_eeic_handler, r31

	popsp r31, r31; retrieve link register from kernel stack
	; Switch back to running stack
	popsp r5, r5 ; retrieve running stack pointer from kernel stack
	mov r3, r4 ; save Kernel stack pointer
	mov r5, r3

	; We have now running stack

	; Check the tpl_need_switch variable
	; to see if a switch should occurs
	mov #_tpl_kern, r12
	ld.w TPL_KERN_OFFSET_NEED_SWITCH[r12], r10
	andi NEED_SWITCH, r10, r11
	bz irq_no_context_switch

	; Save the rest of the context
	pushsp r21, r31

	; Save the current context.
	mov TPL_KERN_OFFSET_S_RUNNING, r10
	movhi HIGHW1(#_tpl_kern), r10, r11 ; get pointer to the descriptor of the running task
	ld.w LOWW(#_tpl_kern)[r11], r10

	st.w r3, [r10] ; save running task sp

	; Switch back to kernel stack
	mov r4, r3

	; Call tpl_run_elected() with arg=1 (save)
	mov 1, r6 ; call with the SAVE value.
	jarl _tpl_run_elected, r31

	; Update the current context according to SP given by the OS
	mov TPL_KERN_OFFSET_S_RUNNING, r10
	movhi HIGHW1(#_tpl_kern), r10, r11 ; Get pointer to the descriptor of the new running task
	ld.w LOWW(#_tpl_kern)[r11], r10

	ld.w [r10], r11 ; Get SP of elected task
	ld.w [r11], r3

	; epilogue
	popsp r21, r31

irq_no_context_switch:
	popsp r20, r20
	ldsr r20, 0 ; restore eipc: calling pc
	popsp r20, r20
	ldsr r20, 1 ; restore eipsw: calling status
	popsp r4, r20
	popsp r2, r2

	eiret