;******************************************************************************
;            TEXAS INSTRUMENTS INCORPORATED PROPRIETARY INFORMATION           
;                                                                             
;   Property of Texas Instruments -- For  Unrestricted  Internal  Use  Only 
;   Unauthorized reproduction and/or distribution is strictly prohibited.  This 
;   product  is  protected  under  copyright  law  and  trade  secret law as an 
;   unpublished work.  Created 2010, (C) Copyright 1997 Texas Instruments.  All 
;   rights reserved.                                                            
;                  
;                                                           
;   Filename       	: pri32.asm
;
;   Description    	: 32BIS compiled Coprocessor functions
;
;   Version number	: 1
;
;   Date 		: 28/6/2010 
;
;
;****************************************************************************
	.include pri.h

	.global	 	FS_ADD	 
	.global 	FS_SUB
	 .state32	
	.ref _pri_rts_redirection
m0	.set	r2
e0	.set	r3
m1	.set	r4
e1	.set	r5
shift	.set	r6
tmp	.set	lr

 
FS_ADD:  STMFD    SP!,{R5}  
	  LDR  R5, pri_rts_redirect
	  LDR  R5, [R5]
	  CMP  R5, #1
  	  LDMFD SP!,{R5}	  
          BNE  FS_ADD_rts
	  INT_LOCK
	  MOVE_OP_SP
	  CDP   p5,#F_ADD,ResultReg0,SP_OP2,SP_OP1  ; Perform the  Single Precision Addition
     	  MOVE_RESTOARM_SP
          INT_UNLOCK
	  BX lr


FS_SUB:  STMFD    SP!,{R5}   
	 LDR  R5, pri_rts_redirect
	 LDR  R5, [R5]
	 CMP  R5, #1
	 LDMFD SP!,{R5}
         BNE  FS_SUB_rts
 	 INT_LOCK
         MOVE_OP_SP
	 CDP   p5,#F_SUB,ResultReg0,SP_OP2,SP_OP1  ; Perform the Single Precision Substraction
         MOVE_RESTOARM_SP
         INT_UNLOCK
	 BX lr

pri_rts_redirect .word _pri_rts_redirection

FS_SUB_rts:	.asmfunc stack_usage(24)
	EOR	r1, r1, #0x80000000	; NEGATE INPUT #2

FS_ADD_rts:
	STMFD	sp!, {r2-r6, lr}

	MOV	m1, r1, LSL #8		; PUT INPUT #2 MANTISSA IN m1
	MOV	e1, r1, LSL #1		; PUT INPUT #2 EXPONENT IN e1
	MOVS	e1, e1, LSR #24		;
	BNE	$1			;

	CMP	m1, #0			; IF DENORMALIZED NUMBER (m0 != 0 AND
	MOVNE	r0, #0			; e1 == 0), THEN UNDERFLOW
	LDMFD	sp!, {r2-r6, pc}	; ELSE IT IS ZERO SO RETURN INPUT #1

$1:	ORR	m1, m1, #0x80000000	; SET IMPLIED ONE IN MANTISSA

	CMP	e1, #0xFF		; IF e1 == 0xFF, THEN OVERFLOW
	
	BEQ	ovfl1			;

	MOV	m1, m1, LSR #2		; ADJUST THE MANTISSA
	CMP	r1, #0			; IF INPUT #2 IS NEGATIVE,
	RSBMI	m1, m1, #0		;  THEN NEGATE THE MANTISSA
	
	MOV	m0, r0, LSL #8		; PUT INPUT #1 MANTISSA IN m0
	MOV	e0, r0, LSL #1		; PUT INPUT #1 EXPONENT IN e0
	MOVS	e0, e0, LSR #24		;
	BNE	$2			;

	CMP	m0, #0			; IF DENORMALIZED NUMBER (m0 != 0 AND
	MOVNE	r0, #0			; e0 == 0), THEN UNDERFLOW
	MOVEQ	r0, r1			; ELSE IT IS ZERO SO RETURN INPUT #2
	LDMFD	sp!, {r2-r6, pc}	;

$2:	ORR	m0, m0, #0x80000000	; SET IMPLIED ONE IN MANTISSA

	CMP	e0, #0xFF		; IF e0 == 0xFF, THEN OVERFLOW
	BEQ	ovfl0			;

	MOV	m0, m0, LSR #2		; ADJUST THE MANTISSA
	CMP	r0, #0			; IF INPUT #1 IS NEGATIVE,
	RSBMI	m0, m0, #0		;  THEN NEGATE THE MANTISSA
	
	SUBS	shift, e0, e1		; GET THE SHIFT AMOUNT
	MOVMI	tmp, m0			; IF THE SHIFT AMOUNT IS NEGATIVE, THEN
	MOVMI	m0, m1			;  SWAP THE TWO MANTISSA SO THAT m0
	MOVMI	m1, tmp			;  CONTAINS THE LARGER VALUE,
	RSBMI	shift, shift, #0	;  AND NEGATE THE SHIFT AMOUNT,
	MOVMI	e0, e1			;  AND ENSURE THE LARGER EXP. IS IN e0

	CMP	shift, #30		; IF THE 2nd MANTISSA IS SIGNIFICANT,
	ADDMI	m0, m0, m1, ASR shift	; ADD IT TO THE FIRST MANTISSA
	CMP	m0, #0x0		;
	MOVEQ	r0, #0			; IF THE RESULT IS ZERO, 
	LDMEQFD	sp!, {r2-r6, pc}	;  THEN UNDERFLOW
	RSBMI	m0, m0, #0x0		; IF THE RESULT IS NEGATIVE, THEN
	MOVMI	tmp, #0x1		;  NEGATE THE RESULT AND
	MOVPL	tmp, #0x0		; NOTE THE SIGN

loop:	MOVS	m0, m0, LSL #1		; NORMALIZE THE RESULTING MANTISSA
	SUB	e0, e0, #1		; ADJUSTING THE EXPONENT AS NECESSARY
	BPL	loop			;

	ADDS	m0, m0, #0x80		; ROUND THE MANTISSA TO THE NEAREST
	ADDCS	e0, e0, #1		; ADJUST EXPONENT IF AN OVERFLOW OCCURS
	MOVCC	m0, m0, LSL #1		; REMOVE THE IMPLIED ONE

	ADDS	e0, e0, #2		; NORMALIZE THE EXPONENT
	MOVLE	r0, #0			; CHECK FOR UNDERFLOW
	LDMLEFD	sp!, {r2-r6, pc}	;
	CMP	e0, #0xFF		; CHECK FOR OVERFLOW
	BCS	ovfl			;

	MOV	r0, m0, LSR #9		; REPACK THE MANTISSA INTO r0
	ORR	r0, r0, e0, LSL #23	; REPACK THE EXPONENT INTO r0
	ORR	r0, r0, tmp, LSL #31	; REPACK THE SIGN INTO r0

	LDMFD	sp!, {r2-r6, pc}	;
	
ovfl1:	MOV	r0, r1			; OVERFLOW
ovfl0:  MOV	tmp, r0, LSR #31	;
ovfl:	MOV	tmp, tmp, LSL #31	; ISOLATE SIGN BIT
	MOV	r0, #0xFF		; SET UP Emax EXPONENT
	MOV	r0, r0, LSL #23
	ORR	r0, r0, tmp		; COMBINE SIGN AND EXPONENT
	LDMFD	sp!, {r2-r6, pc}	;