.file		"logger.s"
	.text
	.align		2


_rombase:
	.globl		_start
	.globl		main
	.globl		debuginterrupt
	.globl		T32_LoggerStruct
	.globl		T32_LoggerHook
	.globl		debugrfi

	.org		_rombase+0x100

_start:
	addi		r0,r0,0

# init stack pointer
	addis		r11,r0,__SP_INIT@ha
	addi		r1,r11,__SP_INIT@l
	addis		r13,r0,_SDA_BASE_@ha
	addi		r13,r13,_SDA_BASE_@l
	addis		r2,r0,_SDA2_BASE_@ha
	addi		r2,r2,_SDA2_BASE_@l
	addi		r0,r0,0
	stwu		r0,-64(r1)

	bl		main
	


#**************************************************************************
#
#	High Performance Logger Trace Exception for PPC4XX processors
#
#	without timestamp
#
#**************************************************************************

	.org		_rombase+0x2000
	
debuginterrupt:
	mtspr	sprg0,r3
	mtspr	sprg1,r4
	mfcr	r3
	mtspr	sprg2,r3
	mtspr	sprg3,r5

	mfspr	r3,dbsr
	rlwinm.	r3,r3,0,1,2
	beq	otherdebuginterrupt		# Branch or Exception ?

	rlwinm.	r3,r3,0,1,1			# set ZERO flag to mark cause of interrupt

	lis	r3,0xffff
	mtspr	dbsr,r3				# clear DBSR

	lis	r4,(T32_LoggerStruct+8)@ha
	lwz	r5,(T32_LoggerStruct+4)@l(r4)	# size of buffer
	lwz	r3,(T32_LoggerStruct+8)@l(r4)	# current index
	cmpw	cr6,r3,r5
	bge	cr6,debuginterrupt1		# end of buffer reached ?
debuginterrupt3:
	addi	r3,r3,1
	stw	r3,(T32_LoggerStruct+8)@l(r4)	# store new index
	subi	r5,r3,1
	slwi	r5,r5,4
	lwz	r4,(T32_LoggerStruct+0)@l(r4)
	add	r5,r4,r5			# get pointer to trace record to R5

	beq	exceptiondebuginterrupt		# Branch or Exception interrupt ?

	mfsrr2	r3
	stw	r3,8(r5)			# execution address

	lis	r4,0xf000
	stw	r4,0(r5)			# trace cycle type

	lwz	r3,0(r3)			# read opcode

	srwi	r4,r3,26
	cmpwi	r4,0x10
	beq	isb
	cmpwi	r4,0x12
	beq	isfb
	cmpwi	r4,0x13
	beq	isop2
	b	otherdebuginterrupt

debuginterrupt1:
	lwz	r5,(T32_LoggerStruct+16)@l(r4)
	rlwinm	r5,r5,0,23,23
	cmpwi	cr7,r5,0
	beq	cr7,debuginterrupt2		# stack mode ?

	mfspr	r3,dbcr
	lis	r5,0x0600
	andc	r3,r3,r5			# turn off trace exceptions
	mtspr	dbcr,r3
		
	b	debuginterruptdone

debuginterrupt2:
	lwz	r5,(T32_LoggerStruct+20)@l(r4)
	ori	r5,r5,1
	stw	r5,(T32_LoggerStruct+20)@l(r4)
	li	r3,0
	b	debuginterrupt3

isop2:
	srwi	r4,r3,1
	clrlwi	r4,r4,22
	cmpwi	r4,0x10
	beq	isblr
	cmpwi	r4,0x210
	beq	isbctr
	b	otherdebuginterrupt

isb:
	rlwinm.	r4,r3,0,8,8
	bne	isb4

	bdnz	isb4				# decrement counter

isb4:
	rlwinm.	r4,r3,0,31,31
	beq	isb1

	mfsrr2	r4
	addi	r4,r4,4
	mtlr	r4				# update LR
isb1:
	rlwinm.	r4,r3,0,30,30
	beq	isb2

	extsh	r4,r3				# ba target
	clrrwi	r4,r4,2
	b	isb3

isb2:
	extsh	r4,r3				# b target
	clrrwi	r4,r4,2
	mfsrr2	r3
	add	r4,r4,r3

isb3:
	b	instdone


isfb:
	rlwinm.	r4,r3,0,31,31
	beq	isfb1

	mfsrr2	r4
	addi	r4,r4,4
	mtlr	r4				# update LR
isfb1:	
	rlwinm.	r4,r3,0,30,30
	beq	isfb2

	extlwi	r4,r3,24,6			# ba target
	srawi	r4,r4,6
	b	isfb3

isfb2:
	extlwi	r4,r3,24,6			# b target
	srawi	r4,r4,6
	mfsrr2	r3
	add	r4,r4,r3

isfb3:
	b	instdone


isblr:
	rlwinm.	r4,r3,0,31,31
	beq	isblr1

	mflr	r4
	mfsrr2	r3
	addi	r3,r3,4
	mtlr	r3				# update LR
	b	instdone

isblr1:
	mflr	r4
	b	instdone

isbctr:
	rlwinm.	r4,r3,0,31,31
	beq	isbctr1

	mfctr	r4
	mfsrr2	r3
	addi	r3,r3,4
	mtlr	r3				# update LR
	b	instdone

isbctr1:
	mfctr	r4
	b	instdone

exceptiondebuginterrupt:
	mfsrr0	r3
	stw	r3,8(r5)			# address before interrupt
	mfsrr2	r3
	stw	r3,12(r5)			# address of interrupt
	lis	r3,0xf002
	stw	r3,0(r5)			# trace cycle type
	b	debuginterruptdone

instdone:
	mtsrr2	r4				# branch target
	stw	r4,12(r5)

debuginterruptdone:
	mfspr	r5,sprg3
	mfspr	r3,sprg2
	mtcrf	0xff,r3
	mfspr	r3,sprg0
	mfspr	r4,sprg1
	rfci


otherdebuginterrupt:
	mfspr	r5,sprg3
	mfspr	r3,sprg2
	mtcrf	0xff,r3
	mfspr	r3,sprg0
	mfspr	r4,sprg1

	mtspr	dac1,r3				# store R3 in DAC1
	lis	r3,0x8104
	mtspr	dbcr,r3				# enable External debug, switch to JTAG mode
T32_LoggerHook:
loop1:
	b	loop1
	nop


debugrfi:
	mtspr	sprg0,r3
	mtspr	sprg1,r4
	mfcr	r3
	mtspr	sprg2,r3
	mtspr	sprg3,r5

	lis	r4,(T32_LoggerStruct+8)@ha
	lwz	r5,(T32_LoggerStruct+4)@l(r4)
	lwz	r3,(T32_LoggerStruct+8)@l(r4)
	cmpw	r3,r5
	bge	debugrfi1
debugrfi3:
	addi	r3,r3,1
	stw	r3,(T32_LoggerStruct+8)@l(r4)
	subi	r5,r3,1
	slwi	r5,r5,4
	lwz	r4,(T32_LoggerStruct+0)@l(r4)
	add	r5,r4,r5

	mfsrr2	r3
	stw	r3,8(r5)
	mfsrr0	r3
	stw	r3,12(r5)
	lis	r3,0xf003
	stw	r3,0(r5)

debugrfidone:
	mfspr	r5,sprg3
	mfspr	r3,sprg2
	mtcrf	0xff,r3
	mfspr	r3,sprg0
	mfspr	r4,sprg1
	rfi

debugrfi1:
	lwz	r5,(T32_LoggerStruct+16)@l(r4)
	rlwinm	r5,r5,0,23,23
	cmpwi	cr7,r5,0
	beq	cr7,debugrfi2			# stack mode ?

	mfspr	r3,dbcr
	lis	r5,0x0600
	andc	r3,r3,r5			# turn off trace exceptions
	mtspr	dbcr,r3
		
	b	debugrfidone

debugrfi2:
	lwz	r5,(T32_LoggerStruct+20)@l(r4)
	ori	r5,r5,1
	stw	r5,(T32_LoggerStruct+20)@l(r4)
	li	r3,0
	b	debugrfi3