.file		"interrupt.s"
	.globl		_ivor_setup
	.globl		T32_LoggerHookFlag

	.text
	.align		2

_ivor_setup:
	addis		r11,r0,IVOR0_handler@ha
	addi		r1,r11,IVOR0_handler@l
	mtivpr		r1
	mtivor0		r1
	addi		r1,r1,0x0100
	mtivor1		r1
	addi		r1,r1,0x0100
	mtivor2		r1
	addi		r1,r1,0x0100
	mtivor3		r1
	addi		r1,r1,0x0100
	mtivor4		r1
	addi		r1,r1,0x0100
	mtivor5		r1
	addi		r1,r1,0x0100
	mtivor6		r1
	addi		r1,r1,0x0100
	mtivor7		r1
	addi		r1,r1,0x0100
	mtivor8		r1
	addi		r1,r1,0x0100
	mtivor9		r1
	addi		r1,r1,0x0100
	mtivor10	r1
	addi		r1,r1,0x0100
	mtivor11	r1
	addi		r1,r1,0x0100
	mtivor12	r1
	addi		r1,r1,0x0100
	mtivor13	r1
	addi		r1,r1,0x0100
	mtivor14	r1
	addi		r1,r1,0x0100
	mtivor15	r1
	blr

	.org 0x0000000
IVOR0_handler:
	nop
	nop
	nop
	b IVOR0_handler
	
	.org 0x00000100
IVOR1_handler:
	nop
	nop
	nop
	b IVOR1_handler
	
	.org 0x00000200
IVOR2_handler:
	nop
	nop
	nop
	b IVOR2_handler
	
	.org 0x00000300
IVOR3_handler:
	nop
	nop
	nop
	b IVOR3_handler
	
	.org 0x00000400
IVOR4_handler:
	nop
	nop
	nop
	b IVOR4_handler
	
	.org 0x00000500
IVOR5_handler:
	nop
	nop
	nop
	b IVOR5_handler
	
	.org 0x00000600
IVOR6_handler:
	nop
	nop
	nop
	b IVOR6_handler
	
	.org 0x00000700
IVOR7_handler:
	nop
	nop
	nop
	b IVOR7_handler
	
	.org 0x00000800
IVOR8_handler:			# system call
	nop
	nop
	nop
	rfi
	
	.org 0x00000900
IVOR9_handler:
	nop
	nop
	nop
	b IVOR9_handler
	
	.org 0x00000A00
IVOR10_handler:
	nop
	nop
	nop
	b IVOR10_handler
	
	.org 0x00000B00
IVOR11_handler:
	nop
	nop
	nop
	b IVOR11_handler
	
	.org 0x00000C00
IVOR12_handler:
	nop
	nop
	nop
	b IVOR12_handler
	
	.org 0x00000D00
IVOR13_handler:
	nop
	nop
	nop
	b IVOR13_handler
	
	.org 0x00000E00
IVOR14_handler:
	nop
	nop
	nop
	b IVOR14_handler
	
	.org 0x00000F00
IVOR15_handler:					# debug interrupt
	mtsprg0 r3
	mtsprg1 r4
	mfcr	r3
	mtsprg2 r3
	mtsprg3 r5

	mfdbsr	r3
	rlwinm	r4,r3,1,4,5			# extract debug events and store in condition register
	rlwinm	r3,r3,10,6,6 
	or.	r3,r3,r4			# if !(DBSR[BRT] || DBSR[IRPT] ||DBSR[RET]) -> (**1)
	mtcrf	0x40,r3				# DBSR[BRT]->CR1[LT], DBSR[IRPT]->CR1[GT], DBSR[RET]->CR1[EQ]
	beq	logger_debugevent_other		# (**1) goto logger_debugevent_other

	lis	r4,(T32_LoggerStruct)@ha
	addi	r4,r4,(T32_LoggerStruct)@l	
	lwz	r5,4(r4)			# int nSize = T32_LoggerStruct.index;
	lwz	r3,8(r4)			# int nIndex = T32_LoggerStruct.index;
	cmpw	r3,r5				# if (nIndex >= nSize)
	bge	logger_bufferwrap		#   goto logger_bufferwrap

logger_continue_after_bufferwrap:
	addi	r3,r3,1				# nIndex++
	stw	r3,8(r4)			# T32_LoggerStruct.index = nIndex
	subi	r5,r3,1				# nIndex--
	slwi	r5,r5,4				# int nByteIndex = nIndex << 4;
	lwz	r4,0(r4)			
	add	r5,r4,r5			# char* pLoggerEntry = (char*)T32_LoggerStruct.ptr + nByteIndex

_logger_timebase_read:
	mtsprg4 r2

_logger_timebase_overflow:
	mftbu	r2
	mftb	r4
	mftbu	r3
	cmp	r3,r2
	bne	_logger_timebase_overflow	# repeat if TBU changed in the mean time
	
	clrlwi	r3,r3,16			# clear most significant halfword for cycle type
	oris	r3,r3,0xf000
	stw	r3,0(r5)			# T32_LoggerStruct.buffer[nIndex].tshigh = (T32_EXECUTE << 16) | (TBU & 0x0000FFFF)
	stw	r4,4(r5)			# T32_LoggerStruct.buffer[nIndex].tslow = TBL

	mfsprg4 r2
	
logger_debugevent_dispatch:
	bgt	cr1,logger_debugevent_irpt	# if (DBSR[IRPT]) goto logger_debugevent_irpt
	beq	cr1,logger_debugevent_ret	# if (DBSR[RET]) goto logger_debugevent_ret

logger_debugevent_brt:
	lis	r3,0x0400
	mtdbsr	r3				# clear DBSR[BRT] (W1C)

	mfcsrr0	r3
	stw	r3,8(r5)			# T32_LoggerStruct.buffer[nIndex].address = <instruction pointer>

	lwz	r3,0(r3)			# int nOpcode = *<instruction pointer>
	srwi	r4,r3,26
	cmpwi	r4,0x10				# if ((nOpcode >> 26) == 16)
	beq	logger_execute_bcx		#   goto logger_execute_bcx
	cmpwi	r4,0x12				# if ((nOpcode >> 26) == 18)
	beq	logger_execute_bx		#   goto logger_execute_bx
	cmpwi	r4,0x13				# if ((nOpcode >> 26) == 19)
	beq	logger_execute_branch19		#   goto logger_execute_branch19

logger_error1:	
	b	logger_error1			# should never occur: unhandled opcodes

logger_bufferwrap:
	lwz	r5,16(r4)
	rlwinm	r5,r5,0,23,23
	cmpwi	cr7,r5,0			# if !(T32_LoggerStruct.iflags & T32LOGGERDATA_IFLAG_STACK)
	beq	cr7,logger_bufferwrap_overflow	#   goto logger_bufferwrap_overflow

	mfdbcr0	r3				# STACK mode: buffer is full, disable debug interrupts
	lis	r5,0x0600
	andc	r3,r3,r5			# DBSR[BRT] = DBSR[IRPT] = 0
	mtdbcr0	r3
	b	logger_debuginterruptdone	# (final) return to application

logger_bufferwrap_overflow:
	lwz	r5,20(r4)
	ori	r5,r5,1
	stw	r5,20(r4)			# T32_LoggerStruct.oflags |= T32LOGGERDATA_OFLAG_OVERFLOW
	li	r3,0
	b	logger_continue_after_bufferwrap

logger_execute_branch19:
	srwi	r4,r3,1
	clrlwi	r4,r4,22
	cmpwi	r4,0x10				# if ((nOpcode >> 1) & 0x3ff == 16)
	beq	logger_execute_bclrx		#   goto logger_execute_bclrx
	cmpwi	r4,0x210			# if ((nOpcode >> 1) & 0x3ff == 528)
	beq	logger_execute_bcctrx		#   goto logger_execute_bcctrx
logger_error2:	
	b	logger_error2			# should never occur: unhandled opcodes

logger_execute_bcx:
	rlwinm.	r4,r3,0,8,8			# if !DECREMENTING_CTR(nOpcode)
	bne	logger_execute_bcx1		#   goto logger_execute_bcx1
	bdnz	logger_execute_bcx1		#   else { CTR--; goto logger_execute_bcx1 }
logger_execute_bcx1:
	rlwinm.	r4,r3,0,31,31			# if !BRANCH_LK(nOpcode)
	beq	logger_execute_bcx2		#   goto logger_execute_bcx2
	mfcsrr0	r4				# else
	addi	r4,r4,4
	mtlr	r4				# LR = <last_prog_address> + 4
logger_execute_bcx2:
	rlwinm.	r4,r3,0,30,30			# if !BRANCH_AA(nOpcode)
	beq	logger_execute_bcx3		#   goto logger_execute_bcx3
	extsh	r4,r3				# -> opcode BcA
	clrrwi	r4,r4,2				# <next_prog_address> = (sign extend)(nOpcode & 0x0000FFFC)
	b	logger_execute_done
logger_execute_bcx3:
	extsh	r4,r3				# -> opcode Bc
	clrrwi	r4,r4,2
	mfcsrr0	r3
	add	r4,r4,r3			# <next_prog_address> = <last_prog_address> +(sign extend)(nOpcode & 0x0000FFFC)
	b	logger_execute_done

logger_execute_bx:
	rlwinm.	r4,r3,0,31,31			# if !BRANCH_LK(nOpcode)
	beq	logger_execute_bx1		#   goto logger_execute_bx1
	mfcsrr0	r4
	addi	r4,r4,4
	mtlr	r4				# LR = <last_prog_address> + 4
logger_execute_bx1:	
	rlwinm.	r4,r3,0,30,30			# if !BRANCH_AA(nOpcode)
	beq	logger_execute_bx2		#   goto logger_execute_bx2
	extlwi	r4,r3,24,6			# -> opcode BA
	srawi	r4,r4,6				# <next_prog_address> = (sign extend)(nOpcode & 0x03FFFFFC)
	b	logger_execute_done
logger_execute_bx2:
	extlwi	r4,r3,24,6			# -> opcode B
	srawi	r4,r4,6
	mfcsrr0 r3
	add	r4,r4,r3			# <next_prog_address> = <last_prog_address> +(sign extend)(nOpcode & 0x03FFFFFC)
	b	logger_execute_done

logger_execute_bclrx:
	rlwinm.	r4,r3,0,31,31			# if !BRANCH_LK(nOpcode)
	beq	logger_execute_bclrx1		#   goto logger_execute_bclrx1
	mflr	r4				# <next_prog_address> = LR
	mfcsrr0 r3
	addi	r3,r3,4
	mtlr	r3				# LR = <last_prog_address> + 4
logger_execute_bclrx1:
	mflr	r4				# <next_prog_address> = LR
	b	logger_execute_done

logger_execute_bcctrx:
	rlwinm.	r4,r3,0,31,31			# if !BRANCH_LK(nOpcode)
	beq	logger_execute_bcctrx1		#   goto logger_execute_bcctrx1
	mfctr	r4				# <next_prog_address> = CTR
	mfcsrr0	r3
	addi	r3,r3,4
	mtlr	r3				# LR = <last_prog_address> + 4
	b	logger_execute_done
logger_execute_bcctrx1:
	mfctr	r4				# <next_prog_address> = CTR

logger_execute_done:
	mtcsrr0	r4				# branch target
	stw	r4,12(r5)
	b	logger_debuginterruptdone

logger_debugevent_irpt:
	lis	r3,0x0200
	mtdbsr	r3				# clear DBSR[TRPT] (W1C)
	mfsrr0	r3
	stw	r3,8(r5)			# T32_LoggerStruct.buffer[nIndex].address = <interrupted instruction address>
	mfcsrr0	r3
	stw	r3,12(r5)			# T32_LoggerStruct.buffer[nIndex].data = <interrupt entry address>
	li	r3,0xf002
	sth	r3,0(r5)			# (high_halfword)(T32_LoggerStruct.buffer[nIndex].tshigh) = T32_INTERRUPT
logger_debuginterruptdone:
	mfsprg3	r5				# restore R5
	mfsprg2	r3				# restore CR
	mtcrf	0xff,r3
	mfsprg0	r3				# restore R3
	mfsprg1	r4				# restore R4
	rfci					# return (from debug interrupt handler)

logger_debugevent_ret:
	li	r3,0				# clear DBSR[RET] (W1C)
	ori	r3,r3,0x8000
	mtdbsr	r3

	mfcsrr0	r3
	stw	r3,8(r5)			# T32_LoggerStruct.buffer[nIndex].data = <address of RFI>
	mfsrr0	r3
	mtcsrr0 r3				# CSRR0 = SRR0
	stw	r3,12(r5)			# T32_LoggerStruct.buffer[nIndex].address = <RFI return address>
	li	r3,0xf003
	sth	r3,0(r5)			# (high_halfword)(T32_LoggerStruct.buffer[nIndex].tshigh) = T32_RFI
	mfsrr1	r3
	mtcsrr1	r3				# CSRR1 = SRR1
	
logger_debugevent_ret_done:
	mfsprg3	r5				# restore R5
	mfsprg2	r3				# restore CR
	mtcrf	0xff,r3
	mfsprg0	r3				# restore R3
	mfsprg1	r4				# restore R4
	rfci					# return (from debug interrupt handler)

logger_debugevent_other:
	mfsprg3	r5				# restore R5
	mfsprg2	r3				# restore CR
	mtcrf	0xff,r3
	
	lis	r3,(T32_LoggerStruct+20)@ha	# signal "core hit breakpoint" via oflags
	lwz	r4,(T32_LoggerStruct+20)@l(r3)
	oris	r4,r4,1
	stw	r4,(T32_LoggerStruct+20)@l(r3)	# T32_LoggerStruct.oflags |= T32LOGGERDATA_OFLAG_BP
	
	mfsprg0	r3				# restore R3
	mfsprg1	r4				# restore R4

	# TRACE32 behavior of LOGGER used with SYStem.Option.HOOK for e500 cores:
	# If PC is at address of T32_LoggerHook when halted,
	# R3  is restored from DAC1
	# PC  is restored from CSRR0
	# MSR is restored from CSRR1

logger_hook_prepare:
	mtdac1	r3

logger_signal_hook_reached:
	lis	r3,(T32_LoggerHookFlag)@ha	# write a value (must be !=0) to flag variable
	addi	r3,r3,(T32_LoggerHookFlag)@l	# to signal "breakpoint hit" condition to TRACE32
	stw	r3, 0(r3)			# TRACE32 has to clear this variable before next Go

T32_LoggerHook:
	b	T32_LoggerHook