xplarm.s

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

	ldr	r2, [r0, #20]
	ldr	r3, [r0, #24]
	ldr	r12, [r0, #28]

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

	movne r0, #0
	movne pc, lr

	ldr	r1, [r0, #32]
	ldr	r2, [r0, #36]
	ldr	r3, [r0, #40]
	ldr	r12, [r0, #44]
	
	cmp r1, #0
	cmpeq r2, #0
	cmpeq r3, #HXF_ONE_F
	cmpeq r12, #0

	movne r0, #0
	movne pc, lr

	ldr	r1, [r0, #48]
	ldr	r2, [r0, #52]
	ldr	r3, [r0, #56]
	ldr	r12, [r0, #60]	
	
	cmp r1, #0
	cmpeq r2, #0
	cmpeq r3, #0
	cmpeq r12, #HXF_ONE_F

	movne r0, #0
	movne pc, lr

	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HMat4F_SetIdentity
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		void HMat4F_SetIdentity(HMATRIX4F *psMatrix);
;** ************************************************************************ **
|Matrix4f_SetIdentity| PROC
	mov r1, #0x0
	mov r2, #HXF_ONE_F
	
	str	r2, [r0]
	str	r1, [r0, #4]
	str	r1, [r0, #8]
	str	r1, [r0, #12]

	str	r1, [r0, #16]
	str	r2, [r0, #20]
	str	r1, [r0, #24]
	str	r1, [r0, #28]

	str	r1, [r0, #32]
	str	r1, [r0, #36]
	str	r2, [r0, #40]
	str	r1, [r0, #44]
	
	str	r1, [r0, #48]
	str	r1, [r0, #52]
	str	r1, [r0, #56]
	str	r2, [r0, #60]
		
	mov		pc, lr
	ENDP

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

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

	movne r0, #0
	movne pc, lr

	ldr	r1, [r0, #16]
	ldr	r2, [r0, #20]
	ldr	r3, [r0, #24]
	ldr	r12, [r0, #28]

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

	movne r0, #0
	movne pc, lr

	ldr	r1, [r0, #32]
	ldr	r2, [r0, #36]
	ldr	r3, [r0, #40]
	ldr	r12, [r0, #44]
	
	cmp r1, #0
	cmpeq r2, #0
	cmpeq r3, #HFX_ONE
	cmpeq r12, #0

	movne r0, #0
	movne pc, lr

	ldr	r1, [r0, #48]
	ldr	r2, [r0, #52]
	ldr	r3, [r0, #56]
	ldr	r12, [r0, #60]	
	
	cmp r1, #0
	cmpeq r2, #0
	cmpeq r3, #0
	cmpeq r12, #HFX_ONE

	movne r0, #0
	movne pc, lr

	mov		pc, lr
	ENDP


;** ************************************************************************ **
; Name:					HMat4FX_SetIdentity
; Description:		
; Input Arguments: 		
; Output Argument:		
; Prototype in C:		void HMat4FX_SetIdentity(HMATRIX4FX *psMatrix);
;** ************************************************************************ **
|Matrix4x_SetIdentity| PROC
	mov r1, #0x0
	mov r2, #HFX_ONE
	
	str	r2, [r0]
	str	r1, [r0, #4]
	str	r1, [r0, #8]
	str	r1, [r0, #12]

	str	r1, [r0, #16]
	str	r2, [r0, #20]
	str	r1, [r0, #24]
	str	r1, [r0, #28]

	str	r1, [r0, #32]
	str	r1, [r0, #36]
	str	r2, [r0, #40]
	str	r1, [r0, #44]
	
	str	r1, [r0, #48]
	str	r1, [r0, #52]
	str	r1, [r0, #56]
	str	r2, [r0, #60]
		
	mov		pc, lr
	ENDP


;/***********************************************************************************
; Function Name      : HMat4FX_Multiply
; Description        : Multiplies two matrices together.
;************************************************************************************/
;void HMat4FX_Multiply(HMATRIX4F *in_pDst, const HMATRIX4F *in_pA, const HMATRIX4F *in_pB)
;{
;	HMATRIX4F tmp;
;
;	if(in_pA == in_pB && in_pA == in_pDst)
;	{
;		memcpy(&tmp, in_pA, sizeof(tmp));
;
;		in_pA = (const HMATRIX4F*)&tmp;
;		in_pB = (const HMATRIX4F*)&tmp;
;	}
;;	else if(in_pA == (const HMATRIX4F*)in_pDst)
;	{
;		memcpy(&tmp, in_pA, sizeof(tmp));
;		in_pA = (const HMATRIX4F*)&tmp;
;	}
;	else if(in_pB == (const HMATRIX4F*)in_pDst)
;	{
;		memcpy(&tmp, in_pB, sizeof(tmp));
;		in_pB = (const HMATRIX4F*)&tmp;
;	}
;
;	// row 0
;	in_pDst->_11 = (in_pA->_11 * in_pB->_11) +  (in_pA->_12 * in_pB->_21)
;				 + (in_pA->_13 * in_pB->_31) +  (in_pA->_14 * in_pB->_41);
;	in_pDst->_12 = (in_pA->_11 * in_pB->_12) +  (in_pA->_12 * in_pB->_22)
;				 + (in_pA->_13 * in_pB->_32) +  (in_pA->_14 * in_pB->_42);
;	in_pDst->_13 = (in_pA->_11 * in_pB->_13) +  (in_pA->_12 * in_pB->_23)
;				 + (in_pA->_13 * in_pB->_33) +  (in_pA->_14 * in_pB->_43);
;	in_pDst->_14 = (in_pA->_11 * in_pB->_14) +  (in_pA->_12 * in_pB->_24)
;				 + (in_pA->_13 * in_pB->_34) +  (in_pA->_14 * in_pB->_44);
;
;	// row 1
;	in_pDst->_21 = (in_pA->_21 * in_pB->_11) +  (in_pA->_22 * in_pB->_21)
;				 + (in_pA->_23 * in_pB->_31) +  (in_pA->_24 * in_pB->_41);
;	in_pDst->_22 = (in_pA->_21 * in_pB->_12) +  (in_pA->_22 * in_pB->_22)
;				 + (in_pA->_23 * in_pB->_32) +  (in_pA->_24 * in_pB->_42);
;	in_pDst->_23 = (in_pA->_21 * in_pB->_13) +  (in_pA->_22 * in_pB->_23)
;				 + (in_pA->_23 * in_pB->_33) +  (in_pA->_24 * in_pB->_43);
;	in_pDst->_24 = (in_pA->_21 * in_pB->_14) +  (in_pA->_22 * in_pB->_24)
;				 + (in_pA->_23 * in_pB->_34) +  (in_pA->_24 * in_pB->_44);
;
;	// row 2
;	in_pDst->_31 = (in_pA->_31 * in_pB->_11) +  (in_pA->_32 * in_pB->_21)
;				 + (in_pA->_33 * in_pB->_31) +  (in_pA->_34 * in_pB->_41);
;	in_pDst->_32 = (in_pA->_31 * in_pB->_12) +  (in_pA->_32 * in_pB->_22)
;				 + (in_pA->_33 * in_pB->_32) +  (in_pA->_34 * in_pB->_42);
;	in_pDst->_33 = (in_pA->_31 * in_pB->_13) +  (in_pA->_32 * in_pB->_23)
;				 + (in_pA->_33 * in_pB->_33) +  (in_pA->_34 * in_pB->_43);
;	in_pDst->_34 = (in_pA->_31 * in_pB->_14) +  (in_pA->_32 * in_pB->_24)
;				 + (in_pA->_33 * in_pB->_34) +  (in_pA->_34 * in_pB->_44);
;
;	// row 3
;	in_pDst->_41 = (in_pA->_41 * in_pB->_11) +  (in_pA->_42 * in_pB->_21)
;				 + (in_pA->_43 * in_pB->_31) +  (in_pA->_44 * in_pB->_41);
;	in_pDst->_42 = (in_pA->_41 * in_pB->_12) +  (in_pA->_42 * in_pB->_22)
;				 + (in_pA->_43 * in_pB->_32) +  (in_pA->_44 * in_pB->_42);
;	in_pDst->_43 = (in_pA->_41 * in_pB->_13) +  (in_pA->_42 * in_pB->_23)
;				 + (in_pA->_43 * in_pB->_33) +  (in_pA->_44 * in_pB->_43);
;	in_pDst->_44 = (in_pA->_41 * in_pB->_14) +  (in_pA->_42 * in_pB->_24)
;				 + (in_pA->_43 * in_pB->_34) +  (in_pA->_44 * in_pB->_44);
;}
;** ************************************************************************ **
|Matrix4x_Multiply| PROC
	stmfd	sp!, {r4-r5, r11, lr}  ; r0-12 are availiable for use

	; Register Map
	;	r0 R
	;	r1 A
	;	r2 B
	;	r3 A._xx
	;	r4 B._x2
	;	r5 B._x3
	;	r11 - tmp
	;	r12 - tmp
	;	r14 Loop Counter
	;
	; wr0 - R._x1 
	; wr1 - R._x2
	; wr2 - R._x3
	; wr3 - R._x4
	; 
	;	r14 - preserved on stack at sp-4
	;	r0(dest matrix) - preserved on stack at sp-8
	;	Loop counter - preserved at at sp-12
	
	; if A == R|| B == R) copy matrix to stack

	sub sp, sp, #64 ; add space for a copy of the matrix on the stack

	cmp r0, r2 ;  if in_pDst != in_pB
	bne HMAT4FX_MULTIPLY_NO_COPY
	
	mov r3, r2 
	mov r2, sp ; set up r1 with the pointer to the copy on the stack

	; Copy Row 1
	ldr r4,  [r3]		
	ldr r5,  [r3, #4]
	ldr r12, [r3, #8]
	ldr r11, [r3, #12]

	str r4,  [r2]		
	str r5,  [r2, #4]
	str r12, [r2, #8]
	str r11, [r2, #12]

	; Copy Row 2
	ldr r4,  [r3, #16]		
	ldr r5,  [r3, #20]
	ldr r12, [r3, #24]
	ldr r11, [r3, #28]

	str r4,  [r2, #16]		
	str r5,  [r2, #20]
	str r12, [r2, #24]
	str r11, [r2, #28]

	; Copy Row 3
	ldr r4,  [r3, #32]		
	ldr r5,  [r3, #36]
	ldr r12, [r3, #40]
	ldr r11, [r3, #44]

	str r4,  [r2, #32]		
	str r5,  [r2, #36]
	str r12, [r2, #40]
	str r11, [r2, #44]

	; Copy Row 4
	ldr r4,  [r3, #48]		
	ldr r5,  [r3, #52]
	ldr r12, [r3, #56]
	ldr r11, [r3, #60]

	str r4,  [r2, #48]		
	str r5,  [r2, #52]
	str r12, [r2, #56]
	str r11, [r2, #60]

HMAT4FX_MULTIPLY_NO_COPY
	; Set up the shift factor for wMMX Fixed 64->32 conversion
	mov r4, #16		;
	tmcr wcgr0, r4  ;

	mov r14, #0 ; initalize the loop counter
HMAT4FX_MULTIPLY_LOOP ; expects loop counter in r3
						; Comments on math are for first iteration
						; for subsequent iteration add 4 to A's elements.. ie a->e
						; and increment the column indices for R -- R12 -> R22

	; Clear Result Registers. 
	wzero wr0
	wzero wr1
	wzero wr2
	wzero wr3

	; Load inital data 
	ldr		r3, [r1]		; A._11
	ldr		r4, [r2]		; B._11 
	ldr		r5, [r2, #4]	; B._12

	tmia	wr0, r4, r3		; R._11 = B._11*A._11 
	tmia	wr1, r5, r3		; R._13 = B._13*A._11 
		 
	ldr		r4, [r2, #8]	; B._13 
	ldr		r5, [r2, #12]	; B._14 

	tmia	wr2, r4, r3		; R._12 = B._12*A._11 
	tmia	wr3, r5, r3		; R._14 = B._14*A._11 

	ldr		r3,  [r1, #4]	; A._12  
	ldr		r4, [r2, #16]	; B._21 
	ldr		r5,  [r2, #20]	; B._22 

	tmia	wr0, r4, r3		; R._11 += B._21*A._12 
	tmia	wr1, r5, r3		; R._13 += B._23*A._12	

	ldr		r4,  [r2, #24]	; B._23 
	ldr		r5, [r2, #28]	; B._24 

	tmia	wr2, r4, r3		; R._12 += B._22*A._12 
	tmia	wr3, r5, r3		; R._14 += B._24*A._12	
	
	ldr		r3, [r1, #8]	; A._13
	ldr		r4, [r2, #32]	; B._31 
	ldr		r5,  [r2, #36]	; B._32 

	tmia	wr0, r4, r3		; R._11 += B._31*A._13  
	tmia	wr1, r5, r3		; R._13 += B._33*A._13  

	ldr		r4,  [r2, #40]	; B._33 
	ldr		r5, [r2, #44]	; B._34 

	tmia	wr2, r4, r3		; R._12 += B._32*A._13  
	tmia	wr3, r5, r3		; R._14 += B._34*A._13  

	ldr		r3, [r1, #12]	; A._14
	ldr		r4, [r2, #48]	; B._41
	ldr		r5, [r2, #52]	; B._42
	 
	tmia	wr0, r4, r3		; R._11 += B._41*A._14	
	tmia	wr1, r5, r3		; R._13 += B._43*A._14	

	ldr		r4, [r2, #56]	; B._43 
	ldr		r5, [r2, #60]	; B._44 
	
	tmia	wr2, r4, r3		; R._12 += B._42*A._14	
	tmia	wr3, r5, r3		; R._14 += B._44*A._14	

	; Packup and store results. 
	wsrldg wr0, wr0, wcgr0  
	wsrldg wr1, wr1, wcgr0
	wsrldg wr2, wr2, wcgr0  
	wsrldg wr3, wr3, wcgr0
 	
	add	r1, r1, #16		; incr A pointer to next data
	add r11, r0, r14  ; calc result offset
	add r14, r14, #16 ; Increment counter for next iteration

	wstrw wr0, [r11] 		; store x from wmmx	
	wstrw wr1, [r11, #4] 	; store y from wmmx	
	wstrw wr2, [r11, #8] 	; store z from wmmx	
	wstrw wr3, [r11, #12] 	; store w from wmmx	

	cmp r14, #64
	bne HMAT4FX_MULTIPLY_LOOP

	add sp, sp, #64 ; remove the copy space from the stack
	ldmfd	sp!, {r4-r5, r11, pc} ; restore registers
	ENDP


;/* ************************************************************************* *\
;	FUNCTION: HVec3F_Transform3x3
;	DESCRIPTION:
;		[]
;
;\* ************************************************************************* */
;HVECTOR3F* HVec3F_Transform3x3(HVECTOR3F* in_pDst, const HVECTOR3F* in_pB,
;							const HMATRIX4F* in_pMat)
;{
;	HVECTOR3F tmp;
;	if(in_pDst == in_pB)
;	{
;		// FIXME verify this never happens and remove
;		memcpy(&tmp, in_pB, sizeof(tmp));
;		in_pB = &tmp;
;	}
;
;	in_pDst->x = (in_pMat->_11 * in_pB->x) + (in_pMat->_21 * in_pB->y)
;			   + (in_pMat->_31 * in_pB->z);
;	in_pDst->y = (in_pMat->_12 * in_pB->x) + (in_pMat->_22 * in_pB->y)
;			   + (in_pMat->_32 * in_pB->z);
;	in_pDst->z = (in_pMat->_13 * in_pB->x) + (in_pMat->_23 * in_pB->y)
;			   + (in_pMat->_33 * in_pB->z);
;
;	return in_pDst;
;}
|Vector3x_Transform3x3| PROC
	stmfd sp!, {r4-r5}

	; Set up a shifter amount in the wMMX control register.
	mov r3, #16
	tmcr wcgr0, r3         

	; Clear the wmmx destination registers
	wzero wr0
	wzero wr1
	wzero wr2
	
	ldr r12, [r1]			; Load v1.x 
	ldr r4, [r2]            ; Load M_11 
	ldr r5, [r2, #4]        ; Load M_12
	ldr r3, [r2, #8]        ; Load M_13 

	tmia wr0, r12, r4       ; r.x += v1.x * M_11
	tmia wr1, r12, r5		; r.y += v1.x * M_12
	tmia wr2, r12, r3		; r.z += v1.x * M_13

	ldr r12, [r1, #4]		; Load v1.y 
	ldr r4, [r2, #16]		; Load M_21
	ldr r5, [r2, #20]		; Load M_22
	ldr r3, [r2, #24]		; Load M_23 

	tmia wr0, r12, r4		; r.x += v1.y * M_21
	tmia wr1, r12, r5		; r.y += v1.y * M_22
	tmia wr2, r12, r3		; r.z += v1.y * M_23

	ldr r12, [r1, #8]		; Load v1.z
	ldr r4, [r2, #32]       ; Load M_31
	ldr r5, [r2, #36]       ; Load M_32
	ldr r3, [r2, #40]       ; Load M_33 

	tmia wr0, r12, r4		; r.x += v1.z * M_31
	tmia wr1, r12, r5		; r.y += v1.z * M_32
	tmia wr2, r12, r3		; r.z += v1.z * M_33

	wsrldg	wr0, wr0, wcgr0 	; r.x >> 16
	wsrldg	wr1, wr1, wcgr0 	; r.y >> 16
	wsrldg	wr2, wr2, wcgr0 	; r.z >> 16

	wstrw wr0, [r0]
	wstrw wr1, [r0, #4]
	wstrw wr2, [r0, #8]
	
	ldmfd sp!, {r4-r5}
	mov		pc, lr
	ENDP


;/* ************************************************************************* *\
;	FUNCTION: HVec4F_Transform
;	DESCRIPTION:
;		[]
;
;\* ************************************************************************* */
;HVECTOR3F* HVec3FX_Transform(HVECTOR3F* in_pDst, const HVECTOR3F* in_pB,
;							const HMATRIX4F* in_pMat)
;{
;	HVECTOR3F tmp;
;	if(in_pDst == in_pB)
;	{
;		memcpy(&tmp, in_pB, sizeof(tmp));
;		in_pB = &tmp;
;	}
;
;	in_pDst->x = (in_pMat->_11 * in_pB->x) + (in_pMat->_21 * in_pB->y)
;			   + (in_pMat->_31 * in_pB->z) + (in_pMat->_41);
;	in_pDst->y = (in_pMat->_12 * in_pB->x) + (in_pMat->_22 * in_pB->y)
;			   + (in_pMat->_32 * in_pB->z) + (in_pMat->_42);
;	in_pDst->z = (in_pMat->_13 * in_pB->x) + (in_pMat->_23 * in_pB->y)
;			   + (in_pMat->_33 * in_pB->z) + (in_pMat->_43);
;
;	return in_pDst;
;}
|Vector3x_Transform| PROC
	stmfd sp!, {r4-r5}

	; Set up a shifter amount in the wMMX control register.
	mov r3, #16
	tmcr wcgr0, r3         

	; Clear the wmmx destination registers
	wzero wr0
	wzero wr1