#include "compiler.h"
#include "sections.h"
#include "picorv32.h"
#include "ioregs.h"


	// The IRQ dispatch code is written in assembly to make
	// better use of the register bank switching: can simply
	// use the saved registers here, no saving needed.
	// registers need to be
	.pushsection ".init.irq","ax"
	.balign 4
	.globl _irq
	.option push
	.option norvc	// Alignment matters more here
_irq:
	addqxi sp,sp,0
	// s10 contains the IRQ return address, s11 the mask of
	// IRQs to be handled.

	// Send EOI for all interrupts (previously done in hardware)
	sw s11,SYS_EOI(zero)

	// Fast dispatch for the ABC-bus interrupt handler
	andi s0,s11,1 << ABC_IRQ
	beqz s0,1f
	jal irqhandler_abc_0
	sub s11,s11,s0
	beqz s11,.L_done
1:

	li s1, 0
.Lirq_loop:
	// ctz would make this more efficient...
#if IRQ_VECTORS > 16
	slli t0,s11,16
	bnez t0,1f
	srli s11,s11,16
	addi s1,s1,16*4
1:
#endif
#if IRQ_VECTORS > 8
	zext.b t0,s11
	bnez t0,2f
	srli s11,s11,8
	addi s1,s1,8*4
2:
#endif
#if IRQ_VECTORS > 4
	andi t0,s11,15
	bnez t0,3f
	srli s11,s11,4
	addi s1,s1,4*4
3:
#endif
#if IRQ_VECTORS > 2
	andi t0,s11,3
	bnez t0,4f
	srli s11,s11,2
	addi s1,s1,2*4
4:
#endif
#if IRQ_VECTORS > 1
	andi t0,s11,1
	bnez t0,5f
	srli s11,s11,1
	addi s1,s1,1*4
5:
#endif
	srli a0,s1,2	// Vector number
	mv   a1,s10	// PC (including the rvc flag)
	.option norelax	// ld will mess up trying to relax this instruction
	jalr s1,%lo(__irq_handler_table) // Must be in zero page
	.option relax
	srli s11,s11,1
	addi s1,s1,4*1
	bnez s11,.Lirq_loop

.L_done:
	mret
	.type _irq, @function
	.size _irq, . - _irq
	.option pop

	// Debug functions to read and write x-registers from interrupt
	// mode by register number
	.pushsection ".text.hot.rdxreg","ax"
	.globl	rdxreg
	.balign 4
rdxreg:
	la a3,_xreg_smc
	lw a2,8(a3)		// addqxi a0,zero,0
	andi a0,a0,31
	slli a0,a0,15		// rs1
	or a2,a2,a0
	sw a2,(a3)
	jr a3
	.type rdxreg, @function
	.size rdxreg, . - rdxreg
	.popsection

	.pushsection ".text.hot.wrxreg","ax"
	.globl	wrxreg
	.balign 4
wrxreg:
	la a3,_xreg_smc
	lw a2,12(a3)		// addxqi zero,a1,0
	andi a0,a0,31
	slli a0,a0,7		// rd
	or a2,a2,a0
	sw a2,(a3)
	jr a3
	.type wrxreg, @function
	.size wrxreg, . - wrxreg
	.popsection

	__rwtext
	.balign 4
	.option norvc
_xreg_smc:
	nop
	ret
	addqxi a0,zero,0
	addxqi zero,a1,0
	.type _xreg_smc, @function
	.size _xreg_smc, . - _xreg_smc