             AREA |C$$code|,CODE,READONLY

             GET "Hdr.Common"

; This macro forms a mask ready to extract and zero the required bits

             MACRO
             mask $mr,$sh

             mvn $mr,#0
             mov $mr,$mr,lsr $sh
             mov $mr,$mr,lsl $sh
             MEND

; Given a mask in r3, this macro extracts and then sets to zero
; bits from the low order word of the double precision variable

             MACRO
             zerolw

             bic r4,r0,r3
             and r0,r0,r3
             MEND

; As above, except for the high order word

             MACRO
             zerohw

             bic r2,r1,r3
             orr r4,r4,r2
             and r1,r1,r3
             MEND

; With the aid of the macros above this function returns floor(x)
; the largest integer less than or equal to x

             EXPORT vfp_floor
vfp_floor    stmfd r13!,{r4}
             p1arg
             vmov.F64 r0,r1,d0
             mov r3,#&FF
             orr r3,r3,#&700
             and r2,r1,r3,lsl #20
             mov r3,r3,lsr #1
             rsbs r2,r3,r2,lsr #20
             blt %FT20               ; abs(x) < 1.0
             mov r4,#0
             rsbs r2,r2,#52
             ble %FT10               ; shift <= 0 so x is already an integer
             mask r3,r2
             zerolw
             subs r2,r2,#32
             ble %FT10               ; no more shifting to do
             mask r3,r2
             zerohw

; Either the number was already an integer or is has now been rounded
; so return if x > 0 or subtract 1 if x < 0 and it wasnt an integer

10           vmov.F64 d0,r0,r1
             tst r1,#&80000000
             teqne r4,#0
             vmovne.F64 d1,#1.0
             vsubne.F64 d0,d0,d1
             pres
             ldmia r13!,{r4}
             mov pc,r14

; If abs(x) < 1.0 then we simply return 0 or -1

20           tst r1,#&80000000
             vmov.F64 d0,#-1.0
             vsubeq.F64 d0,d0,d0
             pres
             ldmia r13!,{r4}
             mov pc,r14

; Having implemented the floor function, ceil is relatively simple

             EXPORT vfp_ceil
vfp_ceil     mov ip,sp
             stmdb sp!,{fp,ip,lr,pc}
             sub fp,ip,#4 
             p1arg
             vmov.F64 d1,d0
             bl vfp_floor             ; calculate floor(x)
             vcmp.F64 d0,d1
             vmrs apsr_nzcv,fpscr
             vmovne.F64 d1,#1.0       ; if x != floor(x)
             vaddne.F64 d0,d0,d1      ; ceil(x)=floor(x)+1
             pres
             ldmdb fp,{fp,sp,pc}

; This macro tests the two words of a floating point variable
; to check if it is zero. This is used by the frexp function below

             MACRO
             ifzero

             IF FPE_POINTERS
             bics r3,r0,#&80000000
             cmpeq r1,#0
             ELSE
             bics r3,r1,#&80000000
             cmpeq r0,#0
             ENDIF
             MEND

; Function frexp to split a number into the fraction and exponent
; first save the integer pointer in r4 for later.

             EXPORT vfp_frexp
vfp_frexp    stmfd r13!,{r4}
             IF FPE_POINTERS
             mov r4,r2
             ELIF {TARGET_FPU_SOFTVFP_VFP}
             mov r4,r2
             ELIF {TARGET_FPU_VFP}
             mov r4,r0
             vmov.F64 r0,r1,d0
             ENDIF

; r1 holds the high order word,
; and r4 the pointer to return the exponent
; test if the input is zero and return a zero exponent if so

             ifzero
             moveq r2,#0
             beq %FT20

; otherwise extract the exponent into r2 and modify the floating
; point representation to have exponent -1 representing the
; fractional part in the range 0.5 to 1

             getfexp r2,d0

; save the exponent at the integer pointer and return the fraction
; as a floating point result

20           str r2,[r4]
             pres
             ldmia r13!,{r4}
             mov pc,r14

; Function ldexp does the opposite of the above, takes the fraction
; and exponent and returns x=f*2^p

             EXPORT vfp_ldexp
vfp_ldexp    stmfd r13!,{r4}
             IF FPE_POINTERS
             mov r4,r2
             ELIF {TARGET_FPU_SOFTVFP_VFP}
             mov r4,r2
             ELIF {TARGET_FPU_VFP}
             mov r4,r0
             vmov.F64 r0,r1,d0
             ENDIF

; First check if the fraction is zero if so do nothing and return

             ifzero
             moveq pc,r14

; otherwise the exponent is in r4, so convert this to biased form
; and insert into the exponent field in r1

             sub r4,r4,#1
             mov r2,#&FF
             orr r2,r2,#&300
             add r4,r4,r2
             orr r2,r2,#&400
             IF FPE_POINTERS
             bic r0,r0,r2,lsl #20
             orr r0,r0,r4,lsl #20
             vmov.F64 d0,r1,r0
             ELSE
             bic r1,r1,r2,lsl #20
             orr r1,r1,r4,lsl #20
             vmov.F64 d0,r0,r1
             ENDIF

; the mantissa remains unchanged, so now return the result

             pres
             ldmia r13!,{r4}
             mov pc,r14

             END
