
	.incdir	"f:\git\goldmomo_endlos\assembler_files\ucore\hcore\"
	.include	"include/uctrl_port.i"
	.include	"include/preformance_counter.i"
	.incdir	"f:\git\goldmomo_endlos\assembler_files\ucore\"
	.include	"include/ucore_ctrl.i"

;*********************************************************************
;* start
;*********************************************************************	
	
	.def	spLast		$d4000000
	.def	stackSize	1048576	;4 MegaByte per core
	
	.def	screenWidth	360
	.def	screenHeight	225
	
	.org	hcoreStartPc
	
	nop
	nop
	nop
	nop
	nop
	nop
	
startHCore

	move	hold,stackSize
	moveh	hold,last,>>stackSize
	or	r0,last,0
	nop
	
	gssr	hold,0			;get core number
	fead	last,r0	
	nop
	mul	r0			;stackOffset = core number (0..n) * stackSize
	
	;set stack offset
	
	move	hold,spLast
	moveh	hold,last,>>spLast
	sub	r127,last,r0		;stackPointer (r127) = spLast - stackOffset
	
	;set other cores (not core 0) to sleep
	
	gssr	hold,0
	cmpeq	last,0
	br.tc	stop
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	
	;setup video
	
	gpc	r126,2
	br	setupVideo
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	
	;
	
	;enable measure 
	.execm	enablePerformanceCounter(unused)
	
loop	
	;wait vblank

	gpc	r126,2
	;br	waitBlank
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	
	;switch frames
	
	gpc	r126,2
	br	switchFrames
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot

	;soft trigger
	
	.execm	softTriggerPerformanceCounter(unused)	
	
	;draw bg
	
	gpc	r126,2
	br	drawBG
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
		
	
	;******** performance 1 *********

	;push data to stack
	
	sub	r127,r127,6
	nop
	gssr	hold,pfcInstCacheHitCnt		;cache hit cnt
	st	last,r127,0
	gssr	hold,pfcFetchReloadCycleCnt	;fetch reload cycle cnt
	st	last,r127,2
	gssr	hold,pfcScoreStallCnt		;score stall cnt
	st	last,r127,4
	
	;draw string (counter)

	move	hold,font1Bit		
        moveh	hold,last,>>font1Bit  
        add	r0,last,0		;r0 = src
        
	move	hold,drawFrame
	moveh	hold,last,>>drawFrame
	ld	r1,last,0		;r1 = dest
	
       
        move	hold,$ffff		
        moveh	hold,last,$ffff	
        or	r3,last,0		;r3 = color
	
	move	hold,drawChar1Bit		
        moveh	hold,last,>>drawChar1Bit  
        add	r6,last,0		;r6 = draw function
	
	move	hold,performance1String		
        moveh	hold,last,>>performance1String  
        add	r7,last,0		;r7 = text address
	
	gpc	r126,2
	br	drawString	
	move	r4,10			;r4 = x			delay slot
	move	r5,26			;r5 = y			delay slot
	move	r2,720			;r2 = dest width	delay slot
	nop				;			delay slot
	nop	;delay slot
	
	;restore stack
	
	add r127,r127,6
	nop
	nop
	
	;******** performance 2 *********

	;push data to stack
	
	sub	r127,r127,6
	nop
	gssr	hold,pfcExecuteStallCnt		;exec stall cnt
	st	last,r127,0
	gssr	hold,pfcDispatchMergeStallCnt	;dsipatch merge stall cnt
	st	last,r127,2
	gssr	hold,pfcDBufferHitCnt		;dbuffer hit cnt
	st	last,r127,4
	
	;draw string (counter)

	move	hold,font1Bit		
        moveh	hold,last,>>font1Bit  
        add	r0,last,0		;r0 = src
        
	move	hold,drawFrame
	moveh	hold,last,>>drawFrame
	ld	r1,last,0		;r1 = dest
	
       
        move	hold,$ffff		
        moveh	hold,last,$ffff	
        or	r3,last,0		;r3 = color
	
	move	hold,drawChar1Bit		
        moveh	hold,last,>>drawChar1Bit  
        add	r6,last,0		;r6 = draw function
	
	move	hold,performance2String		
        moveh	hold,last,>>performance2String  
        add	r7,last,0		;r7 = text address
	
	gpc	r126,2
	br	drawString	
	move	r4,10			;r4 = x			delay slot
	move	r5,34			;r5 = y			delay slot
	move	r2,720			;r2 = dest width	delay slot
	nop				;			delay slot
	nop	;delay slot
	
	;restore stack
	
	add r127,r127,6
	nop
	nop
	
	;******** performance 3 *********

	;push data to stack
	
	sub	r127,r127,4
	nop	
	gssr	hold,pfcDBufferMissCnt		;dbuffer hit cnt
	st	last,r127,0
	gssr	hold,pfcSoftTriggerCounter	;dbuffer hit cnt
	st	last,r127,2
	
	;draw string (counter)

	move	hold,font1Bit		
        moveh	hold,last,>>font1Bit  
        add	r0,last,0		;r0 = src
        
	move	hold,drawFrame
	moveh	hold,last,>>drawFrame
	ld	r1,last,0		;r1 = dest
	
       
        move	hold,$ffff		
        moveh	hold,last,$ffff	
        or	r3,last,0		;r3 = color
	
	move	hold,drawChar1Bit		
        moveh	hold,last,>>drawChar1Bit  
        add	r6,last,0		;r6 = draw function
	
	move	hold,performance3String		
        moveh	hold,last,>>performance3String  
        add	r7,last,0		;r7 = text address
	
	gpc	r126,2
	br	drawString	
	move	r4,10			;r4 = x			delay slot
	move	r5,42			;r5 = y			delay slot
	move	r2,720			;r2 = dest width	delay slot
	nop				;			delay slot
	nop	;delay slot
	
	;restore stack
	
	add r127,r127,4

	;
	
	br	loop
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	
stop	br	stop
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	
;******************************
;*
;* fill8 (8 longs)
;*	
;* r0 = value
;* r1 = address
;* r2 = count
;*
	
fill8
	nop

fill8Loop
	nop
	sub	r2,r2,1
	st	r0,r1,0
	st	r0,r1,2
	st	r0,r1,4
	st	r0,r1,6
	cmpeq	r2,0
	br.tc	fill8Loop
	st	r0,r1,8		;delay slot
	st	r0,r1,10	;delay slot
	st	r0,r1,12	;delay slot
	st	r0,r1,14	;delay slot
	add	r1,r1,16	;delay slot

	jmp	r126,0
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot		
	
;******************************
;*
;* drawString
;*
;* r0 = source (752bit width)
;* r1 = dest
;* r2 = dest width
;* r3 = color
;* r4 = x start
;* r5 = y start
;* r6 = drawFunction
;* r7 = text address
;*	

drawString

	.def	drawStringStackUse	16

	;prolog

	sub	r127,r127,drawStringStackUse	;sp-
	nop
	nop
	st	r16,r127,0	
	st	r17,r127,2	
	st	r18,r127,4
	st	r19,r127,6
	st	r20,r127,8
	st	r21,r127,10
	st	r22,r127,12	
	st	r126,r127,14
	
	;
	
	or	r16,r0,r0			;store source
	or	r17,r1,r1			;store dest
	or	r18,r2,r2			;store width
	or	r19,r3,r3			;store color
	or	r20,r6,r6			;store draw function
	or	r21,r7,r7			;store text address
	
	add	r22,r127,drawStringStackUse	;stack last
	
	;x & y is pixelwise
	
	fead	r2,r5		;	y * width
	add	r4,r4,r4	;x*2
	mul	hold		;
	add	r17,last,r17	;dest+=
	add	r17,last,r4	;dest+=x
	
	;char loop
	
textLoop
	
	ld	r4,r21,0	;load word
	add	r21,r21,2	;next address
	
	cmpeq	r4,37		;%
	br.ts	textProcessCmd
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
	cmpeq	r4,0		;end
	br.ts	textLoopEnd
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
	;no cmd and not end so draw char
	
	gpc	r126,2
	br	textDrawChar
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
	br	textLoop
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
	
textLoopEnd	
	
	;epilog
	
	ld	r16,r127,0	;
	ld	r17,r127,2	;
	ld	r18,r127,4
	ld	r19,r127,6
	ld	r20,r127,8	
	ld	r21,r127,10	
	ld	r22,r127,12	
	ld	r126,r127,14
	
	nop	
	jmp	r126,0
	add	r127,r127,drawStringStackUse	;delay slot
	nop					;delay slot
	nop					;delay slot
	nop					;delay slot
	nop		;delay slot
	
; 
; r4 = char
;

textDrawChar

	sub	r127,r127,2	;sp--
	nop
	nop
	st	r126,r127,0	;store lr

	;draw
	
	sub	hold,r4,32	;char offset
	and	r4,last,$ff	;secure and
	gpc	r126,2
	jmp	r20,0		;draw function
	or	r0,r16,r16	;source       delay slot
	or	r1,r17,r17	;dest         delay slot
	or	r2,r18,r18	;width        delay slot
	or	r3,r19,r19	;color        delay slot
	nop		;delay slot
	
	;
	
	ld	r126,r127,0	;get lr
	add	r127,r127,2	;sp++
	
	jmp	r126,0
	add	r17,r17,16	;destination + 8 pixel
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop		;delay slot
	
; r22 = stack	
	
textProcessCmd

	ld	r4,r21,0	;load cmd
	add	r21,r21,2
	
	cmpeq	r0,120	;x -> hex		
	br	textProcessCmdHex
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
	br	textLoop	;unknown
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
;	
	
textProcessCmdHex

	ld	r9,r22,0	;value from stack
	add	r22,r22,2	;goto next (up)
	
	move	r10,7
	
textProcessCmdHexLoop

	lsr	r4,r9,28	;use upper 4 bit	;delay slot
	gpc	r126,2	
	br	textDrawChar
	cmple	r4,9                                    ;delay slot
	add.ts	r4,r4,48	;16+32  0..9            ;delay slot
	add.tc	r4,r4,55	;23+32  A..F            ;delay slot
	nop						;delay slot
	nop	;delay slot	

	cmpeq	r10,0
	br.tc	textProcessCmdHexLoop
	sub	r10,r10,1	;delay slot
	lsl	r9,r9,4		;delay slot
	nop			;delay slot
	nop			;delay slot
	nop	;delay slot	
	
	br	textLoop	;go back
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
	
;******************************
;*
;* drawChar1Bit
;*
;* r0 = source (752bit width)
;* r1 = dest
;* r2 = dest width
;* r3 = color
;* r4 = char offset
;*

drawChar1Bit

	move	hold,$8000
	or	r8,last,0	;shift mask
	
	and	hold,r4,1	;even/odd
	cmpeq	last,0
	lsr.tc	r8,r8,8		;shift mask >> 8 if odd
	
	lsr	hold,r4,1	;offset / 2
	add	r0,last,r0	;to source address
	nop	
	move	r4,3
	ldw	r5,r0,0		;load 16 bit
	
lineLoop

	or	r7,r8,r8	;reset msk
	ldw	r6,r0,47	;next line load 16 bit
	nop
	
	lsr	hold,r7,0
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,0		;color to dest if bit set
	
	lsr	hold,r7,1
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,2		;color to dest if bit set
	
	lsr	hold,r7,2
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,4		;color to dest if bit set
	
	lsr	hold,r7,3
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,6		;color to dest if bit set
	
	lsr	hold,r7,4
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,8		;color to dest if bit set
	
	lsr	hold,r7,5
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,10	;color to dest if bit set
	
	lsr	hold,r7,6
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,12	;color to dest if bit set
	
	lsr	hold,r7,7
	and	hold,last,r5
	cmpeq	last,0
	st.tc	r3,r1,14	;color to dest if bit set
	
	;next line
	
	or	r7,r8,r8	;reset msk
	add	r1,r1,r2     	;delay slot
	ldw	r5,r0,94	;preload next line load 16 bit (would be thrown away, at last access)
	
	lsr	hold,r7,0
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,0		;color to dest if bit set
	
	lsr	hold,r7,1
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,2		;color to dest if bit set
	
	lsr	hold,r7,2
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,4		;color to dest if bit set
	
	lsr	hold,r7,3
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,6		;color to dest if bit set
	
	lsr	hold,r7,4
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,8		;color to dest if bit set
	
	lsr	hold,r7,5
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,10	;color to dest if bit set
	
	lsr	hold,r7,6
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,12	;color to dest if bit set
	
	lsr	hold,r7,7
	and	hold,last,r6
	cmpeq	last,0
	st.tc	r3,r1,14	;color to dest if bit set
	
	cmpeq	r4,0
	br.tc	lineLoop
	add	r0,r0,94	;delay slot
	sub	r4,r4,1		;delay slot
	add	r1,r1,r2     	;delay slot
	nop			;delay slot
	nop	;delay slot	
	 
	;epilog
		
	jmp	r126,0
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
;******************************
;*
;* setupVideo
;*

setupVideo

	move	r3,0
	move	hold,video320x225x32
	moveh	hold,last,>>video320x225x32	
	or	r0,last,0			;source data
	move	hold,UcVideoRHCountWVideoOn	;dest start
	or	r1,last,0
	
	
setupVideoLoop
	
	ldw	r2,r0,r3			;get data[r3]
	.execm	ucpSetParam(0,r1)		;param 0 = address ($8010 ++)
	.execm	ucpSetParam(1,r2)		;param 1 = value
	.execm	ucpCall(upfCallSubUCtrlWrite)	;set uctrl register
	cmpeq	r3,26
	br.tc	setupVideoLoop
	add	r3,r3,1
	add	r1,r1,1
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
	jmp	r126,0
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot
	nop	;delay slot	
	
video320x225x32	
		
	word	$00010097
	word	$017f017d
	word	$071f076f
	word	$0002001e
	word	$03a203a3
	word	$00000040
	word	$00000000
	word	$00000000
	word	$00080170
	word	$001e03a2
	word	$00040000
	word	$d0000000
	word	$00000000
	word	$00000000
	
;******************************
;*
;* waitBlank
;*	

waitBlank

	;emulator test

	.execm	ucpSetParam(0,UuLedR)	;param 0 = UuLedr / isEmu
	.execm	ucpCallReturn(t0,upfCallSubUCtrlRead)	;get uctrl register
	nop

	and	hold,t0,1
	cmpeq	last,0			
	br.tc	waitBlankEnd		;t0 <> 0 (emulator) -> waitBlankEnd 
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop	;delay slot	
	
	;sw test

	.execm	ucpSetParam(0,UcRSwichtWLedG)	;param 0 = UcRSwichtWLedG 
	.execm	ucpCallReturn(t0,upfCallSubUCtrlRead)	;get uctrl register
	nop

	and	hold,t0,1
	cmpeq	last,1			
	br.ts	waitBlankEnd		;t0(0) == 1 (SW0 is set) -> waitBlankEnd 
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop	;delay slot	
		
	move	hold,UcVideoHITVCOUNT
	or	t0,last,0		;hitv count
	move	t1,930			;line
	
	.execm	ucpSetParam(0,t0)		;param 0 = address (hitv count)
	.execm	ucpSetParam(1,t1)		;param 1 = 930
	.execm	ucpCall(upfCallSubUCtrlWrite)	;set uctrl register
	
	move	hold,UcVideoRPVcountHit
	or	t0,last,0		;vcount hit adr
	
	.execm	ucpSetParam(0,t0)		;param 0 = address (vcount hit)
	
waitBlankLoop

	.execm	ucpCallReturn(t0,upfCallSubUCtrlRead)	;set uctrl register
	nop

	and	hold,t0,1
	cmpeq	last,1		;vcount hit?
	br.tc	waitBlankLoop	;no do again
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop	;delay slot	
	
waitBlankEnd

	jmp	r126,0
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	
;******************************
;*
;* drawBG
;*	
	.macro	calculate2D(_t0,_t1,_t2,_t3,_u,_v,_su,_sv,_du,_dv,_angle,_zoom,_centerX,_centerY,_offsetX,_offsetY)
	
	move	hold,$1ff
	and	_angle,last,_angle	;angle & $1dd
	move	hold,sincosTable
	moveh	hold,last,>>sincosTable
	add	hold,last,_angle	;+ angle
	ldws	_t1,last,128		;tcos
	ldws	_t0,last,0		;tsin
		
	fead	_t1,_zoom	;	
	fead	_t0,_zoom	;	
	mul	_t1		; 		tcos * hzoom
	mul	_t2		; 		tsin * hzoom
	nop
	
	asr	_t1,_t1,8 	;_t1 = scos = tcos * hzoom >> 8
	asr	_t2,_t2,8	;_t2 = ssin = tsin * hzoom >> 8
	nop
	
	;calculate su/sv and du/dv
	
	or	_su,_t1,_t1	;_su = su = scos
	move	hold,0
	sub	_sv,last,_t2	;_sv = sv = 0 - ssin
	
	or	_du,_t2,_t2	;_du = du = ssin
	or	_dv,_t1,_t1	;_dv = dv = scos
	
	;calculate center
	
	; u = (((cosy + sinx)) + (offsetTextX << 16));
        ; v = (((cosx - siny)) + (offsetTextY << 16));
	
	fead	_t1,_centerY	;		scos * centerY
	fead	_t1,_centerX	;		scos * centerX
	mul_fead	_t0,_t2,_centerY		;_t0 = cosy = scos * centerY	and 	ssin * centerY	
	mul_fead	_t1,_t2,_centerX		;_r1 = cosx = scos * centerX	and	ssin * centerX		
	mul	_t2		;_t2 = siny = ssin * centerY		
	mul	_t3		;_t3 = sinx = ssin * centerX
	
	lsl	hold,_offsetY,16	; 	(offsetTextY << 16)
	add	hold,last,_t1		;				+ cosx
	sub	_v,last,_t2		;v = 					- siny
	
	lsl	hold,_offsetX,16		; 			  (offsetTextX << 16)
	add	hold,last,_t3		;  		sinx + 
	add	_u,last,_t0		;u =	cosy +
	
	.endmacro
	
drawBG	

	;prolog
	
	sub	r127,r127,2	;sp-
	nop
	nop
	st	r126,r127,0	
	
	;
	
	move	hold,texture512x512x8888
	moveh	hold,last,>>texture512x512x8888
	or	r31,last,0		;r31 = source
	
	move	hold,drawFrame
	moveh	hold,last,>>drawFrame
	ld	r32,last,0		;r32 = dest
	
	move	r33,512			;r33 = source width (in pixel)
	move	r34,360			;r34 = dest width (in pixel)
	move	r35,0		;y_start
	;move	r36,0		;u = 0
	;move	r37,0		;v = 0
	
	move	r14,0	
	move	r15,$100
	
	;modify angle and zoom
	
	move	hold,angle
	moveh	hold,last,>>angle
	or	r0,last,0
	nop
	nop
	
	ld	r14,r0,0		;r14 = angle
	ld	r15,r0,2		;r15 = zoom
	ld	r16,r0,4		;r16 = zoomadd
	
	move	r17,0
	add	r14,r14,1	;angle + 1
	

	
	cmplos	r15,$400	;!zoom < $600
	sub.tc	r16,r17,r16	;zoomadd = 0 - zoomadd
	nop
	
	cmplos	r15,$10		;zoom < $0
	sub.ts	r16,r17,r16	;zoomadd = 0 - zoomadd
	
	nop
	nop
	add	r15,r15,r16	;zoom = zoom + zoomadd
	
	
	nop
	st	r14,r0,0
	st	r15,r0,2
	st	r16,r0,4
		
	lsr	r15,r15,1	;slow zoom	
		
	;
	
	move	r16,180		;centerx
	move	r17,112		;centery
	move	r18,$100	;offsetx
	move	r19,$100	;offsety
	
	rsub	r16,r16,0	;-centerx
	rsub	r17,r17,0	;-centery
	
	
	;calculate
	
	.execm	calculate2D(r0,r1,r2,r3,r36,r37,r38,r39,r40,r41,r14,r15,r16,r17,r18,r19)
		
drawLoop	
	
	or	r16,r32,r32	;dest adr
	or	r17,r34,r34	;dest width
	or	r18,r31,r31	;source adr
	or	r19,r33,r33	;source width
	move	r20,0		;x = 0
	move	r21,360	;length
	
	
	;
	
	gpc	r126,2
	br	drawScanline
	or	r22,r35,r35	;y	delay slot
	or	r23,r36,r36	;u	delay slot
	or	r24,r37,r37	;v	delay slot
	or	r25,r40,r40	;du	delay slot
	or	r26,r41,r41	;dv	delay slot
	
	;

	cmpeq	r35,224
	br.tc	drawLoop
	add	r35,r35,1	;delay slot
	add	r36,r36,r38	;u=u+su			;delay slot
	add	r37,r37,r39	;v=v+sv			;delay slot		
	nop			;delay slot
	nop			;delay slot
	
	;epilog

	ld	r126,r127,0	;get lr
	add	r127,r127,2	;sp++
	
	jmp	r126,0
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop			;delay slot
	

angle	word	$0000
zoom	word	$10	
zoomadd	word	1
	
;******************************
;*
;* drawScanline
;*
;* r16 = dest adr	-
;* r17 = dest width 	-
;* r18 = source adr	-
;* r19 = source width	-
;* r20 = x
;* r21 = length
;* r22 = y		-
;* r23 = u 16:16	-
;* r24 = v 16:16	-
;* r25 = du 16:16	-
;* r26 = dv 16:16	-
;*	    
	.macro	drawScanLineGetSourcePixel(_result,_u,_v,_du,_dv,_source_adr,_source_width_x2,_x,_yu1m2,_tr1)
	
	and	hold,_x,1	
	add	hold,last,_yu1m2	;    	     x&1 + (y&1 *2)
	add	_result,last,1		;a =     1 + x&1 + (y&1 *2)
	rsub	_tr1,last,5		;b = 5 - 1 + x&1 + (y&1 *2) 
	add	_x,_x,1
	lsl	hold,_result,14		;xa = (a * 16384) & 0xffff;
	moveh	_result,last,0
	lsl	hold,_tr1,14		;ya = (b * 16384) & 0xffff;
	moveh	_tr1,last,0
	
	add	hold,_result,_u
	lsl	hold,last,7			;      u << 7   
	lsr	hold,last,23			;rx = (u << 7)>>7 >> 16
	lsl	_result,last,1			;rx *= 2 (addr to long)	
	add	hold,_tr1,_v
	lsl	hold,last,7			;      v << 7
	lsr	hold,last,23			;ry = (v << 7)>>7 >> 16
	fead	last,_source_width_x2		;ry * source Width (x2)
	add	_u,_u,_du			;u = u + du
	mul	hold				;  		  [	ry * source Width (x2)			]
	add	hold,last,_result		;  		  [			  + rx (x2)		]
	ldc	_result,last,_source_adr	;_result	ld[ 				+sourceAdr 	]
	add	_v,_v,_dv			;v = v + dv
	
	.endmacro

drawScanline

	or	r48,r20,r20	;x
	or	hold,r22,r22	;
	and	hold,last,1
	lsl	r49,last,1	;r49 = y & 1 * 2

	; dest += x_start + y * visibleWidth;   // calc dest start position
	  
	add	r17,r17,r17	;r17 = dest width * 2
	add	r19,r19,r19	;r19 = source width * 2
	add	r20,r20,r20	;r20 = x * 2
	
	fead	r22,r17		;			y * visible width 
	nop
	mul	hold
	add	hold,last,r16	;	    dest_adr + 
	add	r16,last,r20	;dest_adr =                                 + x 
	
	
	;r15  = count - 1
	
	lsr	hold,r21,4	;	count / 16
	sub	hold,last,1	; -1
	cmples	last,0		; if < 0 draw Reset
	br.ts	skipDrawScanlineX16
	or	r20,last,0	;counterX16 = count / 16 - 1
	nop			;delay slot
	nop			;delay slot
	nop			;delay slot
	nop			;delay slot
		
drawScanlineX16

	.execm	drawScanLineGetSourcePixel( r0,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r1,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r2,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r3,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r4,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r5,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r6,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r7,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r8,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel( r9,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel(r10,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel(r11,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel(r12,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel(r13,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel(r14,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	.execm	drawScanLineGetSourcePixel(r15,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	
	st	r0,r16,0
	st	r1,r16,2
	st	r2,r16,4
	st	r3,r16,6
	st	r4,r16,8
	st	r5,r16,10
	st	r6,r16,12	;delay slot
	st	r7,r16,14	;delay slot
	st	r8,r16,16
	st	r9,r16,18
	st	r10,r16,20
	st	r11,r16,22
	st	r12,r16,24
	
	cmpeq	r20,0
	br.tc	drawScanlineX16
	st	r13,r16,26	;delay slot
	st	r14,r16,28	;delay slot
	st	r15,r16,30	;delay slot
	add	r16,r16,32	;delay slot
	sub	r20,r20,1	;delay slot
	
skipDrawScanlineX16	
	
	;draw reset (mod 16) (counter is r21)	
	
	and	hold,r21,$f	;mod 16
	sub	hold,last,1	; -1
	cmples	last,0		; if < 0 no draw Reset
	br.ts	skipDrawScanlineX16Rest
	or	r21,last,0	;r21 = count & $f - 1
	nop			;delay slot
	nop			;delay slot
	nop			;delay slot
	nop			;delay slot
	
drawScanlineX16Rest	
	
	.execm	drawScanLineGetSourcePixel(r0,r23,r24,r25,r26,r18,r19,r48,r49,r50)
	
	cmpeq	r21,0
	br.tc	drawScanlineX16Rest
	st	r0,r16,0	;delay slot
	add	r16,r16,2	;delay slot
	sub	r21,r21,1	;delay slot
	nop			;delay slot
	nop			;delay slot
	
skipDrawScanlineX16Rest	
	
	;prolog
	
	jmp	r126,0
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	nop			;delay slot

;******************************
;*
;* switchFrames
;*	

switchFrames

	move	hold,displayFrame
	moveh	hold,last,>>displayFrame
	or	t0,last,0
	
	move	hold,drawFrame
	moveh	hold,last,>>drawFrame
	or	t1,last,0
	nop
	
	ld	t2,t0,0
	ld	t3,t1,0
	
	;t2 = display
	;t3 = draw
	
	move	hold,UcVideoMEMStartAdrLow
	or	t4,last,0
	move	hold,UcVideoMEMStartAdrHigh
	or	t5,last,0
	
	or	t6,t3,t3	;low
		
	.execm	ucpSetParam(0,t4)		;param 0 = address (start address low)
	.execm	ucpSetParam(1,t6)		;param 1 = value
	.execm	ucpCall(upfCallSubUCtrlWrite)	;set uctrl register
	
	lsr	t6,t6,16	;high
	move	hold,UcVideoMEMLatch
	or	t4,last,0
	
	.execm	ucpSetParam(0,t5)		;param 0 = address (start address high)
	.execm	ucpSetParam(1,t6)		;param 1 = value
	.execm	ucpCall(upfCallSubUCtrlWrite)	;set uctrl register
	
	.execm	ucpSetParam(0,t4)		;param 0 = address (latch out)
	.execm	ucpCallReturn(t4,upfCallSubUCtrlWrite)	;set uctrl register
	
	nop
	fead	t4,t4	;sync
	
	jmp	r126,0
	st	t3,t0,0	;delay slot
	st	t2,t1,0	;delay slot
	nop		;delay slot
	nop		;delay slot
	nop		;delay slot
	
displayFrame	word	frame0
drawFrame	word	frame1	
	
performance1String
	.stringz	"IHIT %x IRLD %x SCRS %x"
	
performance2String
	.stringz	"EXES %x DPMS %x DBHT %x"	

performance3String
	.stringz	"DBMS %x SFCN %x"
	
sincosTable
   
	.data   $00000192,$032404B6,$064707D9,$096A0AFB,$0C8B0E1B,$0FAB1139,$12C71455,$15E1176D,$18F81A82,$1C0B1D93,$1F19209F,$222323A6,$252726A7,$282629A3,$2B1F2C98,$2E102F87,$30FB326E,$33DE354D,$36B93824,$398C3AF2,$3C563DB7,$3F174073,$41CD4325,$447A45CC,$471C4869,$49B34AFB,$4C3F4D81,$4EBF4FFB,$51335268,$539A54C9,$55F5571D,$58425964,$5A825B9C,$5CB35DC7,$5ED75FE3,$60EB61F0,$62F163EE,$64E865DD,$66CF67BC,$68A6698B,$6A6D6B4A,$6C236CF8,$6DC96E96,$6F5E7022,$70E2719D,$72547307,$73B5745F,$750475A5,$764176D8,$776B77FA,$78847909,$79897A05,$7A7C7AEE,$7B5C7BC5,$7C297C88,$7CE37D39,$7D897DD5,$7E1D7E5F,$7E9C7ED5,$7F097F37,$7F617F86,$7FA67FC1,$7FD87FE9,$7FF57FFD,$7FFF7FFD,$7FF57FE9,$7FD87FC1,$7FA67F86,$7F617F37,$7F097ED5,$7E9C7E5F,$7E1D7DD5,$7D897D39,$7CE37C88,$7C297BC5,$7B5C7AEE,$7A7C7A05,$79897909,$788477FA,$776B76D8,$764175A5,$7504745F,$73B57307,$7254719D,$70E27022,$6F5E6E96,$6DC96CF8,$6C236B4A,$6A6D698B,$68A667BC,$66CF65DD,$64E863EE,$62F161F0,$60EB5FE3,$5ED75DC7,$5CB35B9C,$5A825964,$5842571D,$55F554C9,$539A5268,$51334FFB,$4EBF4D81,$4C3F4AFB,$49B34869,$471C45CC,$447A4325,$41CD4073,$3F173DB7,$3C563AF2,$398C3824,$36B9354D,$33DE326E,$30FB2F87,$2E102C98,$2B1F29A3,$282626A7,$252723A6,$2223209F,$1F191D93,$1C0B1A82,$18F8176D,$15E11455,$12C71139,$0FAB0E1B,$0C8B0AFB,$096A07D9,$064704B6,$03240192,$0000FE6E,$FCDCFB4A,$F9B9F827,$F696F505,$F375F1E5,$F055EEC7,$ED39EBAB,$EA1FE893,$E708E57E,$E3F5E26D,$E0E7DF61,$DDDDDC5A,$DAD9D959,$D7DAD65D,$D4E1D368,$D1F0D079,$CF05CD92,$CC22CAB3,$C947C7DC,$C674C50E,$C3AAC249,$C0E9BF8D,$BE33BCDB,$BB86BA34,$B8E4B797,$B64DB505,$B3C1B27F,$B141B005,$AECDAD98,$AC66AB37,$AA0BA8E3,$A7BEA69C,$A57EA464,$A34DA239,$A129A01D,$9F159E10,$9D0F9C12,$9B189A23,$99319844,$975A9675,$959394B6,$93DD9308,$9237916A,$90A28FDE,$8F1E8E63,$8DAC8CF9,$8C4B8BA1,$8AFC8A5B,$89BF8928,$88958806,$877C86F7,$867785FB,$85848512,$84A4843B,$83D78378,$831D82C7,$8277822B,$81E381A1,$8164812B,$80F780C9,$809F807A,$805A803F,$80288017,$800B8003,$80018003,$800B8017,$8028803F,$805A807A,$809F80C9,$80F7812B,$816481A1,$81E3822B,$827782C7,$831D8378,$83D7843B,$84A48512,$858485FB,$867786F7,$877C8806,$88958928,$89BF8A5B,$8AFC8BA1,$8C4B8CF9,$8DAC8E63,$8F1E8FDE,$90A2916A,$92379308,$93DD94B6,$95939675,$975A9844,$99319A23,$9B189C12,$9D0F9E10,$9F15A01D,$A129A239,$A34DA464,$A57EA69C,$A7BEA8E3,$AA0BAB37,$AC66AD98,$AECDB005,$B141B27F,$B3C1B505,$B64DB797,$B8E4BA34,$BB86BCDB,$BE33BF8D,$C0E9C249,$C3AAC50E,$C674C7DC,$C947CAB3,$CC22CD92,$CF05D079,$D1F0D368,$D4E1D65D,$D7DAD959,$DAD9DC5A,$DDDDDF61,$E0E7E26D,$E3F5E57E,$E708E893,$EA1FEBAB,$ED39EEC7,$F055F1E5,$F375F505,$F696F827,$F9B9FB4A,$FCDCFE6E,$00000192,$032404B6,$064707D9,$096A0AFB,$0C8B0E1B,$0FAB1139,$12C71455,$15E1176D,$18F81A82,$1C0B1D93,$1F19209F,$222323A6,$252726A7,$282629A3,$2B1F2C98,$2E102F87,$30FB326E,$33DE354D,$36B93824,$398C3AF2,$3C563DB7,$3F174073,$41CD4325,$447A45CC,$471C4869,$49B34AFB,$4C3F4D81,$4EBF4FFB,$51335268,$539A54C9,$55F5571D,$58425964,$5A825B9C,$5CB35DC7,$5ED75FE3,$60EB61F0,$62F163EE,$64E865DD,$66CF67BC,$68A6698B,$6A6D6B4A,$6C236CF8,$6DC96E96,$6F5E7022,$70E2719D,$72547307,$73B5745F,$750475A5,$764176D8,$776B77FA,$78847909,$79897A05,$7A7C7AEE,$7B5C7BC5,$7C297C88,$7CE37D39,$7D897DD5,$7E1D7E5F,$7E9C7ED5,$7F097F37,$7F617F86,$7FA67FC1,$7FD87FE9,$7FF57FFD