
; Derived directly from:

;    ylib.s : routines for math functions

;    Copyright (C) 1993  Claus Vohwinkel

;    This program is free software; you can redistribute it and/or modify
;    it under the terms of the GNU General Public License as published by
;    the Free Software Foundation; either version 2 of the License , or
;    (at your option) any later version.

;    This program is distributed in the hope that it will be useful,
;    but WITHOUT ANY WARRANTY; without even the implied warranty of
;    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;    GNU General Public License for more details.
;
;    You should have received a copy of the GNU General Public License
;    along with this program; see the file COPYING.  If not, write to
;    the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.

;    You may contact the author by:
;       e-mail:  vohwinkel@vxdesy.desy.de
;      us-mail:  Claus Vohwinkel
;                SCRI/FSU
;                400 Science Library
;                Tallahassee, FL 32306



;  *** NB: Portions of ylib have been removed/modified/replaced by myself, <mech@toth.org.uk>


R0 RN 0
R1 RN 1
R2 RN 2
R3 RN 3
IP RN 12
SP RN 13
R14 RN 14
PC RN 15
F0 FN 0
F1 FN 1
F2 FN 2
F3 FN 3

        AREA |C$$code|, CODE, READONLY


 EXPORT ypow
 EXPORT yexp
 EXPORT ylog10
 EXPORT ylog

 EXPORT ymodf
 EXPORT yfmod
 EXPORT yceil
 EXPORT yfloor



yceil
 STMFD SP!,{R0,R1}
 LDFD F0,[SP],#8
 rndep f0, f0
 mov pc, r14

yfloor
 STMFD SP!,{R0,R1}
 LDFD F0,[SP],#8
 rndem f0, f0
 mov pc, r14


yfmod
 STMFD SP!,{R0-R3}
 LDFD F1,[SP,#8]
 TEQ R2,#0
 MNFMIE F1,F1
 LDFD F0,[SP],#16
 DVFEZ F2,F0,F1
 rndez f2, f2
 MUFE F1,F1,F2
 SUFE F0,F0,F1
 mov pc, r14

ymodf STMFD SP!,{R0,R1}
 LDFD F0,[SP],#8
 rndez f1, f0
 SUFE F0,F0,F1
 STFD F1,[R2]
 mov pc, r14


;      ----------

ypow TEQ R0,#0
 BMI POWNEG
 TEQEQ R1,#0
 BEQ POWZER
 STMFD SP!,{R2,R3,R14}
 LDFD F0,[SP]
 rnde F1,F0
 SUFE F1,F0,F1
 CMF F1,#0
 BNE POWUSU
 ABSE F1,F0
 LDFS F2,POTRESH
 CMF F1,F2
 BGT POWUSU

 FIX R2,F0
 STMIA SP,{R0,R1}
 LDFD F1,[SP],#12   ; WE KILL R14 ON STACK AS WELL
 TEQ R2,#0
 BEQ PPZ
 mvfe F0,#1
 TEQ R2,#0
 MOVEQ PC,R14
 RDFMIE F1,F1,#1
 RSBMI R2,R2,#0

POWLOOP
 MOVS R2,R2,LSR #1
 MUFCSE F0,F0,F1
 MOVEQ PC,R14
 MUFE F1,F1,F1
 B POWLOOP

PPZ CMF F1,#0
 ADFVSE F0,F1,#0  ; RETURN IF nan
 MOVVS PC,R14
 SUB IP,R0,#&0FF00000
 TEQ IP,#&70000000
 BEQ PAWX           ; +- INF ** 0
 mvfe F0,#1
 mov pc, r14


POWNEG
 STMFD SP!,{R0,R1}
 LDFD F2,[SP],#8
 CMF F2,#0
 BVS PNAN
 MOVS IP,R2
 RDFMIE F0,F2,#1
 MVFPLE F0,F2
 BICMIS IP,IP,#&80000000
 TEQEQ R3,#0
 BEQ PMZ
 MVFe F1,F0
 MOV R0,IP,LSL #12
 MOV IP,IP,LSR #20
 SUB IP,IP,#&FF
 TEQ IP,#&700
 BEQ PF1            ;  INF OR NAN
 SUBS IP,IP,#&300
 BMI PF1            ; .. fraction
 ORR R0,R0,R3,LSR #20
 MOV R1,R3,LSL #12
 BEQ PN2
PN1 MUFE F0,F0,F0
 TEQ R0,#0
 MUFMIE F0,F0,F1
 MOVS R1,R1,LSL #1
 ADCS R0,R0,R0
 SUBS IP,IP,#1
 BNE PN1
PN2 TEQ R0,#0
 TEQEQ R1,#0
 MOVEQ PC,R14

PF1 MVFez F0,F2      ; original first arg
 STMFD SP!,{R2,R3}
 LDFD F1,[SP],#8 ; original scnd arg
 POWD F0,F0,F1
 mov pc, r14

PMZ ;  handle (-x)^0
 BICS IP,R0,#&80000000
 TEQEQ R1,#0
 BEQ PAWX      ; (-0)^0
 MOVS IP,IP,LSR #20
 SUBS IP,IP,#&FF
 TEQ IP,#&700
 BEQ PAWX
 mvfe F0,#1
 mov pc, r14

POWZER ;  handle 0 ^ X
 STMFD SP!,{R2,R3}
 LDFD F0,[SP],#8
 CMF F0,#0
 ADFVSE F0,F0,#1
 MOVVS PC,R14
 BEQ PAWX
 mvfe F0,#0
 RDFMIE F0,F0,#1
 mov pc, r14

PAWX ; supply an invalid operation
 MNFe F0,#1
 LOGD F0,F0
 mov pc, r14

PNAN STMFD SP!,{R2,R3}
 LDFD F1,[SP],#8
 ADFE F0,F1,F2
 mov pc, r14


POTRESH DCFS 65000.0


POWUSU BL ylog
 LDFD F1,[SP],#8
 MUFE F0,F0,F1
 LDMFD SP!,{R14}
 b yexpf

yexp
 STMFD SP!,{R0,R1}
 LDFD F0,[SP],#8
yexpf LDFS F2,OVT
 ABSE F3,F0
 ADR R3,LOG2E
 lfm F1,1,[R3],#12
 CMF F3,F2
 MUFE F0,F0,F1
 BPL OVER_UNDER
YEXPZ
 RnDE F1,F0
; NRME F1,F1    

 FIX R2,F1

 SUFE F0,F0,F1
; -1/2 <= F0 <= 1/2
 MUFE F1,F0,F0
; CALCULATE P
 lfm F2,2,[R3],#24   ; P2,P1
 MUFE F2,F2,F1
 ADFE F2,F2,F3
 MUFE F2,F2,F1
 lfm F3,1,[R3],#12   ; P0
 ADFE F2,F2,F3
 MUFE F0,F0,F2
 lfm F2,2,[R3],#24    ;Q1 Q0
 ADFE F2,F1,F2
 MUFE F2,F2,F1
 ADFE F2,F2,F3
 SUFE F1,F2,F0
 ADFE F0,F0,F2
 DVFE F0,F0,F1
 TEQ R2,#0
 MOVEQ PC,R14
 CMP R2,#&4000
 BGE OVERFL
 ADD R0,R2,#&FF
 ADDS R0,R0,#&3F00
 MOV R1,#&80000000
 MOV R2,#0
 BLE DENORM_OR_ZER
EX2 STMFD SP!,{R0,R1,R2}
 lfm F3,1,[SP],#12
 MUFd F0,F0,F3
 mov pc, r14
 
TOVT DCFS 16448.0
OVT DCFS 11400.0
LOG2E & &00003FFF
      & &B8AA3B29
      & &5C17F0BC
P2    & &00003FF9
      & &BD2E42AB
      & &70BDAA7B
P1    & &00004003
      & &A19DD498
      & &9F60DB06
P0    & &00004009
      & &BD3D047F
      & &734DBD67
Q1    & &00004006
      & &E92F287A
      & &E89542C1
Q0    & &0000400B
      & &8881B17C
      & &3A652AD2


DENORM_OR_ZER
 RSB IP,R0,#1
 MOV R3,#&80000000
 MOV R1,R3,LSR IP
 SUBS IP,IP,#32
 MOVPL R2,R3,LSR IP
 B EX2

OVER_UNDER
 BVS ENAN
 CMF F0,#0
 MVFLTE F0,#0
 MOVLT PC,R14
 ; we would need 2**f0 at this point but we have an overflow so we dont care
 EXPD F0,F0
 mov pc, r14

OVERFL
 LDFS F0,TOVT
 EXPD F0,F0
 mov pc, r14

ENAN ADFE F0,F0,#0
 mov pc, r14

; -----------------------------------------------------------


ylog10 STMFD SP!,{R14}
       BL ylog
       LDFE F1,L10E
       MUFE F0,F0,F1
       LDMFD SP!,{PC}

L10E & &00003FFD
     & &DE5BD8A9
     & &37287195

ylog
 LDFD F1,SQX2
 MOVS R2,R0,ASR #20
 BMI LSUP
 MOV R3,#&FF
 ORR R3,R3,#&300
 BEQ LGZER_OR_DEN
 BIC R0,R0,R2,LSL #20
 ORR R0,R0,R3,LSL #20
 SUB R2,R2,R3           
 TEQ R2,#&400          ; INFINITY OR NAN AS INPUT
 BEQ LSUP1
LN1 STMFD SP!,{R0,R1}
 LDFD F0,[SP],#8

; F0 IS NOW BETWEEN 1 AND 2
; F1 CONTAINS SQRT2
; R2 IS ORIGINAL EXPONENT
 CMF F0,F1
 MUFGTE F0,F0,#0.5
 ADDGT R2,R2,#1
; WE NEED NOW F0 = (F0-1)/(1+F0)
 SUFE F1,F0,#1
 ADFE F2,F0,#1
 ADR R1,LGCO7
 DVFE F0,F1,F2
 lfm F2,2,[R1],#24
 MUFE F1,F0,F0  
 MOV R0,#5
 MUFE F2,F2,F1
 ADFE F2,F2,F3
LG2
 MUFE F2,F2,F1
 lfm F3,1,[R1],#12
 SUBS R0,R0,#1
 ADFE F2,F2,F3
 BPL LG2
 TEQ R2,#0
 MUFE F0,F0,F2
 MOVEQ PC,R14
 FLTNEE F1,R2
 LFM F3,1,[R1],#12
 MUFE F1,F1,F3
 ADFd F0,F0,F1
 mov pc, r14

LGZER_OR_DEN ;
 orrs r2, r0, r1
 MVFEQsz F0, #0
 BEQ LSP1      ; input was zero
 RSB R2,R3,#1
LGNORM MOVS R1,R1,LSL #1
 ADC R0,R0,R0
 SUB R2,R2,#1
 TST R0,#&100000
 BEQ LGNORM
LGNORM1
 ORR R0,R0,R3,LSL #20
 B LN1

LSUP1 ADD R0,R0,#&40000000
LSUP STMFD SP!,{R0,R1}
     LDFD F0,[SP],#8
LSP1 LGNd F0,F0
     mov pc, r14

SQX2 DCFD 1.41421356237309504880
LGCO7    & &00003FFC
       & &98AA3FC1
       & &56F198AE
ZCO6    & &00003FFC
       & &9CB03E62
       & &9FC48DB6
       & &00003FFC
       & &BA34901A
       & &A65F3672
       & &00003FFC
       & &E38E20BB
       & &67572CF5
       & &00003FFD
       & &924924AD
       & &BBFE1303
       & &00003FFD
       & &CCCCCCCC
       & &ABC1FB26
       & &00003FFE
       & &AAAAAAAA
       & &AAB39730
       & &00003FFF
       & &FFFFFFFF
       & &FFFFFF4C
LOG2   & &00003FFE
       & &B17217F7
       & &D1CF79AC


|tab.3|
 KEEP |tab.3|
 DCD 5376
 DCD 12024
 DCD 19815
 DCD 27394
 DCD 34750
 DCD 41877
 DCD 48762
 DCD 55398
 DCD 61770
 DCD 67867
 DCD 73677
 DCD 79186
 DCD 84377
 DCD 89240
 DCD 93748
 DCD 97887
 DCD 101635
 DCD 104961
 DCD 107845
 DCD 110218
 DCD 112068
 DCD 113409
 DCD 114654
 DCD 115341
 DCD 115422
 DCD 114907
 DCD 113267
 DCD 110876
 DCD 107701
 DCD 103741
 DCD 98918
 DCD 93152
 DCD 92222
 DCD 96728
 DCD 100941
 DCD 104850
 DCD 108425
 DCD 111685
 DCD 114529
 DCD 117129
 DCD 119149
 DCD 120699
 DCD 122335
 DCD 123565
 DCD 124351
 DCD 124648
 DCD 124500
 DCD 123853
 DCD 122666
 DCD 120373
 DCD 117526
 DCD 114220
 DCD 110158
 DCD 105438
 DCD 99982
 DCD 93742
 DCD 86655
 DCD 78666
 DCD 69713
 DCD 59729
 DCD 48641
 DCD 36365
 DCD 22820
 DCD 7901



 EXPORT |ysqrt|
|ysqrt|                      ; By Mech.  Uses Newton-Raphson approximation to get 1/sqrt(x)
                             ; and then multiplies by x to get sqrt(x)
                             ; probably not 100% percent accurate, but more than good enough for LAME
 orrs r12, r1, r0, lsl #1
 moveq r15, r14
 bcs invalsqrt
 stmfd r13!, {r0, r1}
  ldfd f3, [r13], #8
 mov r2, r0, lsr #1
 rsb r3, r2, #0x5f000000
 add r3, r3, #0xe80000
 mov r0, r3, lsr #14
 ldr r12, |L..4|
 and r0, r0, #63
 ldr r12, [r12, r0, lsl #2]
 mov r1, #0
 sub r0, r3, r12
 stmfd r13!, {r0, r1}
  ldfd f0, [r13], #8
  mufd f2, f3, f0
  mufd f2, f2, f0
  rsfd f2, f2, #3
  ldfd f1, |L..4|+4
  mufd f0, f0, f2
  mufd f2, f3, f0
  mufd f2, f2, f0
  sufd f1, f1, f2
  ldfd f2, |L..4|+12
  mufd f0, f0, f1
  mufd f0, f0, f2
  mufd f2, f3, f0
  mufd f1, f2, f0
  rsfd f1, f1, #3
  mufd f1, f1, #0.5
  mufd f0, f1, f2
 mov r15, r14

invalsqrt
 stmfd r13!, {r0, r1}
  ldfd f0, [r13], #8
  sqtd f0, f0
 mov r15, r14

|L..4|
 DCD |tab.3|
 DCD &40280000, &0 ; double 12.0
 DCD &3fb00000, &0 ; double 0.625


 END
