xplarm.s

Lido PXA270平台开发板的最新BSP,包括源代码

;/* ************************************************************************ *\
;**    INTEL Corporation Proprietary Information
;**
;**    This listing is supplied under the terms of a license
;**    agreement with INTEL Corporation and may not be copied
;**    nor disclosed except in accordance with the terms of
;**    that agreement.
;**
;**    Copyright (c) 2003 Intel Corporation.
;**    All Rights Reserved.
;**
;** ************************************************************************ **
;**	FILE: XPLARM.s nee HXFMathARM.s
;**	DESCRIPTION: Optimized fixed point / fat float routines. 
;**	
;**	CREATED: November 30, 2004
;**
;\* ************************************************************************ */
	INCLUDE XPL.inc ; Definitions of the HXFState Structure

;** ************************************************************************ **
;**	CONSTANTS
;** ************************************************************************ **

;** ************************************************************************ **
;**	EXPORTS
;** ************************************************************************ **
	EXPORT	|Matrix4f_IsIdentity|
	EXPORT	|Matrix4f_SetIdentity|

	EXPORT	|Matrix4x_IsIdentity|
	EXPORT	|Matrix4x_SetIdentity|
	EXPORT  |Vector4x_Transform| 
	EXPORT  |Vector3x_Transform|
	EXPORT  |Vector3x_Transform3x3|
	EXPORT	|Vector3x_Normalize|

	EXPORT  |Fixed_Abs|
	EXPORT	|Fixed_Cos|
	EXPORT	|Fixed_Div|
	EXPORT	|Fixed_InvSqrt|
	EXPORT	|Fixed_Mul|
	EXPORT	|Fixed_Sin|

	EXPORT	|Normal_Pack3x|

	EXPORT	|Color4s_IsZero|
	EXPORT	|Color4s_Multiply|
	EXPORT	|Color4s_Pack4x|
	EXPORT	|ARGB_Color4x|
	EXPORT	|ABGR_Color4s|
	
	EXPORT	|XtoF|
	EXPORT	|ArrayXtoF|

	EXPORT	|FtoX|
	EXPORT	|ArrayFtoX|

	EXPORT	|HXFAbsF|
	EXPORT	|HXFClz|
	EXPORT	|HXFSignedClz|
	EXPORT	|HXFCtz|


;** ************************************************************************ **
;**	VARIABLES
;** ************************************************************************ **
	IMPORT |g_HXFSinTable|

;** ************************************************************************ **
;**	MACROS
;** ************************************************************************ **


;** ************************************************************************ **
;**	FUNCTIONS
;** ************************************************************************ **
	AREA .text, CODE, READONLY

;** ************************************************************************ **
; Name:					HAbsFX
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HFIXED HAbsFX(HFIXED);
;** ************************************************************************ **
|Fixed_Abs| PROC

	cmp r0, #0
	rsb r0, r0, #0

	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HMulFX
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HFIXED HMulFX(HFIXED, HFIXED);
;** ************************************************************************ **
|Fixed_Mul| PROC
	smull	r3, r1, r0, r1					;[R1 | R3] = [HI | LO]
	mov		r1, r1, lsl #16
	orr		r0, r1, r3, lsr #16				;Concatenating Lo and Hi

	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HXFDivFX
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HFIXED HDivFX(HFIXED, HFIXED);
;** ************************************************************************ **
|Fixed_Div| PROC

	; Compute result sign and 
	cmp r0, #0
	moveq pc, lr
	rsblt r0, r0, #0
	movlt r12, #1
	movge r12, #0
	
	cmp r1, #0
	moveq r0, #0
	moveq pc, lr	
	rsblt r1, r1, #0
	eorlt r12, r12, #1

	; Normalize number to lead bit in 31 slot	
	clz r2, r0
	mov r0, r0, lsl r2
	
	clz r3, r1
	mov r1, r1, lsl r3
	
	; compute result exponent
	sub r3, r2, r3
	
	cmp r3, #16
	moveq r0, #0
	moveq pc, lr	
	
    ; Clear out the result registers so we can mult & acc 32-bit values into them.
	mov r2, #0          ; Clear the result register
	
	; Do the division computation
	HXF_ONEBITDIVIDE 31, r0, r1, r2	; 1
	HXF_ONEBITDIVIDE 30, r0, r1, r2	; 2
    HXF_ONEBITDIVIDE 29, r0, r1, r2 ; 3
	HXF_ONEBITDIVIDE 28, r0, r1, r2 ; 4

	cmp r3, #12
	bge HDIVFX_DONE

	HXF_ONEBITDIVIDE 27, r0, r1, r2	; 5 
	HXF_ONEBITDIVIDE 26, r0, r1, r2	; 6  	
	HXF_ONEBITDIVIDE 25, r0, r1, r2	; 7  
	HXF_ONEBITDIVIDE 24, r0, r1, r2	; 8  
	
	cmp r3, #8
	bge HDIVFX_DONE

	HXF_ONEBITDIVIDE 23, r0, r1, r2	; 9  
	HXF_ONEBITDIVIDE 22, r0, r1, r2	; 10  
	HXF_ONEBITDIVIDE 21, r0, r1, r2	; 11  
	HXF_ONEBITDIVIDE 20, r0, r1, r2	; 12  
	
	cmp r3, #4
	bge HDIVFX_DONE

	HXF_ONEBITDIVIDE 19, r0, r1, r2	; 13  
	HXF_ONEBITDIVIDE 18, r0, r1, r2	; 14  	
	HXF_ONEBITDIVIDE 17, r0, r1, r2	; 15  
	HXF_ONEBITDIVIDE 16, r0, r1, r2	; 16  
	
	cmp r3, #0
	bge HDIVFX_DONE

	HXF_ONEBITDIVIDE 15, r0, r1, r2	; 17
	HXF_ONEBITDIVIDE 14, r0, r1, r2	; 18
    HXF_ONEBITDIVIDE 13, r0, r1, r2 ; 19
	HXF_ONEBITDIVIDE 12, r0, r1, r2 ; 20
	
	cmp r3, #-4
	bge HDIVFX_DONE

	HXF_ONEBITDIVIDE 11, r0, r1, r2	; 21
	HXF_ONEBITDIVIDE 10, r0, r1, r2	; 22 	
	HXF_ONEBITDIVIDE  9, r0, r1, r2	; 23 
	HXF_ONEBITDIVIDE  8, r0, r1, r2	; 24 
	
	cmp r3, #-8
	bge HDIVFX_DONE

	HXF_ONEBITDIVIDE  7, r0, r1, r2	; 25 
	HXF_ONEBITDIVIDE  6, r0, r1, r2	; 26  
	HXF_ONEBITDIVIDE  5, r0, r1, r2	; 27  
	HXF_ONEBITDIVIDE  4, r0, r1, r2	; 28  
	
	cmp r3, #-12
	bge HDIVFX_DONE
	
	HXF_ONEBITDIVIDE  3, r0, r1, r2	; 29  
	HXF_ONEBITDIVIDE  2, r0, r1, r2	; 30  	
	HXF_ONEBITDIVIDE  1, r0, r1, r2	; 31  

HDIVFX_DONE
	; convert back to Fixed point	-- lz in r14	
	;	ResultExp = 0 - 15 - lz(w)
	;   Result Shift to FX = 15 - ResultExp
	;   Result Shift to FX = 15 - 0 - 15 - lz(w)
	;   Result Shift to FX = 30 - lz(w)
	add r3, r3, #15
	mov r0, r2, lsr r3

	; restore sign
	cmp r12, #1
	rsbeq r0, r0, #0
	
	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HSinFX
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HFIXED HSinFX( HFIXED );
;** ************************************************************************ **
|Fixed_Sin| PROC
	
	; Convert to positive and store original value. 
	movs r12, r0
	rsblt r0, r0, #0 
	
	; r0 * Scale Factor(0x000000B6) 0.0027777778 = 1.0f/360.0f
	mov r1, #0x000000B6 ; = 1/360
	smull r3, r4, r0, r1
	
	;  
	ldr r1, =g_HXFSinTable
	mov r4, r3, lsr #23	; get major index 
	mov r3, r3, lsr #16 ; get fractional bits for linear interp
	and r3, r3, #0x7F   ; mask of weight bits
	
	add r1, r1, r4, lsl #1   ; Compute Base Addr
	ldrsh r0, [r1]   ; Load Base value 
	ldrsh r4, [r1, #2]   ; Load Second Value

	; compute weighted value
	mul r1, r4, r3 
	rsb r3, r3, #0x80
	mul r4, r0, r3
	
	add r0, r4, r1
	mov r0, r0, asr #6
	
	; Account for input of sign
	cmp r12, #0
	rsblt r0, r0, #0
	
	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HCosFX
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HFIXED HCosFX( HFIXED );
;** ************************************************************************ **
|Fixed_Cos| PROC
	
    add r0, r0, #0x005A0000

	; Convert to positive and store original value. 
	movs r12, r0
	rsblt r0, r0, #0 
	
	; r0 * Scale Factor(0x000000B6) 0.0027777778 = 1.0f/360.0f
	mov r1, #0x000000B6 ; = 1/360
	smull r3, r4, r0, r1
	
	;  
	ldr r1, =g_HXFSinTable
	mov r4, r3, lsr #23	; get major index 
	mov r3, r3, lsr #16 ; get fractional bits for linear interp
	and r3, r3, #0x7F   ; mask of weight bits
	
	add r1, r1, r4, lsl #1   ; Compute Base Addr
	ldrsh r0, [r1]   ; Load Base value 
	ldrsh r4, [r1, #2]   ; Load Second Value

	; compute weighted value
	mul r1, r4, r3 
	rsb r3, r3, #0x80
	mul r4, r0, r3
	
	add r0, r4, r1
	mov r0, r0, asr #6

	; Account for input of sign
	cmp r12, #0
	rsblt r0, r0, #0
	
	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HInvSqrtFX
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HFIXED HInvSqrtFX(HFIXED);
;** ************************************************************************ **
|Fixed_InvSqrt| PROC
	
	; Check for Negative Number. 
	cmp r0, #0
	movlt r0, #0
	movlt pc, lr
	
	HXF_INVSQRTFX r0, r1, r2, r3, r12

	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HXFColor4S_IsZero
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HBOOL HXFColor4S_IsZero(const HXFCOLOR4S*);
;** ************************************************************************ **
|Color4s_IsZero| PROC
	wldrd 	wr0, [r0]

	wzero	wr14   ; Set up compare value
	mov 	r0, #0 ; initialize result to 0
	
	wshufh 	wr0, wr0, #0x24 ; replicate argb to brgb so we ignore alpha in this test. 	
	tandch  r15
		
	mvneq 	r0, r0	; if zero invert result to 0xFFFFFFFF
	
	mov		pc, lr
	ENDP
	

;** ************************************************************************ **
; Name:					HXFColor4S_Multiply
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HBOOL HXFColor4S_Multiply(HXFCOLOR4S*, const HXFCOLOR4S*, const HXFCOLOR4S*);
;** ************************************************************************ **
|Color4s_Multiply| PROC
	wldrd wr0, [r1]
	wldrd wr1, [r2]
		
	wmulum 	wr0, wr0, wr1
	wstrd 	wr0, [r0]
	
	mov		pc, lr
	ENDP
	

;** ************************************************************************ **
; Name:					HXFColor4SToABGR
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HUINT32 HXFColor4SToABGR(const HXFCOLOR4S*);
;** ************************************************************************ **
|ABGR_Color4s| PROC
	wldrd wr0, [r0]

	mov 	r1, #8		; shift value for 8.8 to 0.16 conversion
	tmcr 	wcgr1, r1
	wzero	wr14

	wshufh		wr0, wr0, #0xC6 ; ARGB(11 10 01 00)  to  ABGR(11 00 01 10)
	wsrlhg		wr0, wr0, wcgr1 ; Convert 0.16 result to 8.8
	wpackhus	wr0, wr0, wr14	; convert to 8 bit and saturate > 0

	textrmsw  r0, wr0, #0
		
	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HXFPackColorFX
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HXFCOLOR4S* HXFPackColorFX(HXFCOLOR4S* in_pDst, const HFIXED* pClr)
;** ************************************************************************ **
|Color4s_Pack4x| PROC
	
	; Red
	ldr r2, [r1] 
	cmp r2, #HFX_ONE
	movge r2, #HFX_ONE
	subge r2, r2, #1
	cmp r2, #0
	movlt r2, #0
	strh r2, [r0, #4]

	; Green
	ldr r2, [r1, #4] 
	cmp r2, #HFX_ONE
	movge r2, #HFX_ONE
	subge r2, r2, #1
	cmp r2, #0
	movlt r2, #0
	strh r2, [r0, #2]

	; Blue
	ldr r2, [r1, #8] 
	cmp r2, #HFX_ONE
	movge r2, #HFX_ONE
	subge r2, r2, #1
	cmp r2, #0
	movlt r2, #0
	strh r2, [r0]

	; Alpha
	ldr r2, [r1, #12] 
	cmp r2, #HFX_ONE
	movge r2, #HFX_ONE
	subge r2, r2, #1
	cmp r2, #0
	movlt r2, #0	
	strh r2, [r0, #6]
	
	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HXFColor4FXToARGB
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		HUINT32 HXFColor4FXToARGB(const HFIXED r, const HFIXED g, const HFIXED b, const HFIXED a)
;** ************************************************************************ **
|ARGB_Color4x| PROC
	
	mov r12, #HFX_ONE
	
	; Red
	cmp r0, #HFX_ONE
	movge r0, #0x0000FF00
	
	cmp   r0, #0
	movlt r0, #0x00000000
	
	and r0, r0, #0x0000FF00
	mov r0, r0, lsl #8   ; 0x00FF0000
	
	; Green
	cmp r1, #HFX_ONE
	movge r1, #0x0000FF00
	
	cmp   r1, #0
	movlt r1, #0x00000000
	
	and r1, r1, #0x0000FF00
	orr r0, r0, r1	; 0x0000FF00
	
	; Blue
	cmp r2, #HFX_ONE
	movge r2, #0x0000FF00

	cmp   r2, #0
	movlt r2, #0x00000000

	and r2, r2, #0x0000FF00
	orr r0, r0, r2, lsr #8	; 0x000000FF
		
	; Alpha
	cmp r3, #HFX_ONE
	movge r3, #0x0000FF00

	cmp   r3, #0
	movlt r3, #0x00000000

	and r3, r3, #0x0000FF00
	orr r0, r0, r3, lsl #16 ; 0xFF000000

	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HMat4F_IsIdentity
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		BOOL HMat4F_IsIdentity(HMATRIX4F *psMatrix);
;** ************************************************************************ **
|Matrix4f_IsIdentity| PROC
	
	ldr	r1, [r0]
	ldr	r2, [r0, #4]
	ldr	r3, [r0, #8]
	ldr	r12, [r0, #12]

	cmp r1, #HXF_ONE_F
	cmpeq r2, #0
	cmpeq r3, #0
	cmpeq r12, #0

	movne r0, #0
	movne pc, lr

	ldr	r1, [r0, #16]