jitter buffer: histogram shifting function (not used yet)
[speexdsp.git] / libspeex / pseudofloat.h
1 /* Copyright (C) 2005 Jean-Marc Valin */
2 /**
3    @file pseudofloat.h
4    @brief Pseudo-floating point
5  * This header file provides a lightweight floating point type for
6  * use on fixed-point platforms when a large dynamic range is 
7  * required. The new type is not compatible with the 32-bit IEEE format,
8  * it is not even remotely as accurate as 32-bit floats, and is not
9  * even guaranteed to produce even remotely correct results for code
10  * other than Speex. It makes all kinds of shortcuts that are acceptable
11  * for Speex, but may not be acceptable for your application. You're
12  * quite welcome to reuse this code and improve it, but don't assume
13  * it works out of the box. Most likely, it doesn't.
14  */
15 /*
16    Redistribution and use in source and binary forms, with or without
17    modification, are permitted provided that the following conditions
18    are met:
19    
20    - Redistributions of source code must retain the above copyright
21    notice, this list of conditions and the following disclaimer.
22    
23    - Redistributions in binary form must reproduce the above copyright
24    notice, this list of conditions and the following disclaimer in the
25    documentation and/or other materials provided with the distribution.
26    
27    - Neither the name of the Xiph.org Foundation nor the names of its
28    contributors may be used to endorse or promote products derived from
29    this software without specific prior written permission.
30    
31    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
32    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
33    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
34    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
35    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
36    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
37    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
38    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
39    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
40    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
41    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
42 */
43
44 #ifndef PSEUDOFLOAT_H
45 #define PSEUDOFLOAT_H
46
47 #include "misc.h"
48 #include "os_support.h"
49 #include "math_approx.h"
50 #include <math.h>
51
52 #ifdef FIXED_POINT
53
54 typedef struct {
55    spx_int16_t m;
56    spx_int16_t e;
57 } spx_float_t;
58
59 static const spx_float_t FLOAT_ZERO = {0,0};
60 static const spx_float_t FLOAT_ONE = {16384,-14};
61 static const spx_float_t FLOAT_HALF = {16384,-15};
62
63 #define MIN(a,b) ((a)<(b)?(a):(b))
64 static inline spx_float_t PSEUDOFLOAT(spx_int32_t x)
65 {
66    int e=0;
67    int sign=0;
68    if (x<0)
69    {
70       sign = 1;
71       x = -x;
72    }
73    if (x==0)
74    {
75       spx_float_t r = {0,0};
76       return r;
77    }
78    e = spx_ilog2(ABS32(x))-14;
79    x = VSHR32(x, e);
80    if (sign)
81    {
82       spx_float_t r;
83       r.m = -x;
84       r.e = e;
85       return r;
86    }
87    else      
88    {
89       spx_float_t r;
90       r.m = x;
91       r.e = e;
92       return r;
93    }
94 }
95
96
97 static inline spx_float_t FLOAT_ADD(spx_float_t a, spx_float_t b)
98 {
99    spx_float_t r;
100    if (a.m==0)
101       return b;
102    else if (b.m==0)
103       return a;
104    if ((a).e > (b).e) 
105    {
106       r.m = ((a).m>>1) + ((b).m>>MIN(15,(a).e-(b).e+1));
107       r.e = (a).e+1;
108    }
109    else 
110    {
111       r.m = ((b).m>>1) + ((a).m>>MIN(15,(b).e-(a).e+1));
112       r.e = (b).e+1;
113    }
114    if (r.m>0)
115    {
116       if (r.m<16384)
117       {
118          r.m<<=1;
119          r.e-=1;
120       }
121    } else {
122       if (r.m>-16384)
123       {
124          r.m<<=1;
125          r.e-=1;
126       }
127    }
128    /*printf ("%f + %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/
129    return r;
130 }
131
132 static inline spx_float_t FLOAT_SUB(spx_float_t a, spx_float_t b)
133 {
134    spx_float_t r;
135    if (a.m==0)
136       return b;
137    else if (b.m==0)
138       return a;
139    if ((a).e > (b).e)
140    {
141       r.m = ((a).m>>1) - ((b).m>>MIN(15,(a).e-(b).e+1));
142       r.e = (a).e+1;
143    }
144    else 
145    {
146       r.m = ((a).m>>MIN(15,(b).e-(a).e+1)) - ((b).m>>1);
147       r.e = (b).e+1;
148    }
149    if (r.m>0)
150    {
151       if (r.m<16384)
152       {
153          r.m<<=1;
154          r.e-=1;
155       }
156    } else {
157       if (r.m>-16384)
158       {
159          r.m<<=1;
160          r.e-=1;
161       }
162    }
163    /*printf ("%f + %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/
164    return r;
165 }
166
167 static inline int FLOAT_LT(spx_float_t a, spx_float_t b)
168 {
169    if (a.m==0)
170       return b.m>0;
171    else if (b.m==0)
172       return a.m<0;   
173    if ((a).e > (b).e)
174       return ((a).m>>1) < ((b).m>>MIN(15,(a).e-(b).e+1));
175    else 
176       return ((b).m>>1) > ((a).m>>MIN(15,(b).e-(a).e+1));
177
178 }
179
180 static inline int FLOAT_GT(spx_float_t a, spx_float_t b)
181 {
182    return FLOAT_LT(b,a);
183 }
184
185 static inline spx_float_t FLOAT_MULT(spx_float_t a, spx_float_t b)
186 {
187    spx_float_t r;
188    r.m = (spx_int16_t)((spx_int32_t)(a).m*(b).m>>15);
189    r.e = (a).e+(b).e+15;
190    if (r.m>0)
191    {
192       if (r.m<16384)
193       {
194          r.m<<=1;
195          r.e-=1;
196       }
197    } else {
198       if (r.m>-16384)
199       {
200          r.m<<=1;
201          r.e-=1;
202       }
203    }
204    /*printf ("%f * %f = %f\n", REALFLOAT(a), REALFLOAT(b), REALFLOAT(r));*/
205    return r;   
206 }
207
208 static inline spx_float_t FLOAT_AMULT(spx_float_t a, spx_float_t b)
209 {
210    spx_float_t r;
211    r.m = (spx_int16_t)((spx_int32_t)(a).m*(b).m>>15);
212    r.e = (a).e+(b).e+15;
213    return r;   
214 }
215
216
217 static inline spx_float_t FLOAT_SHL(spx_float_t a, int b)
218 {
219    spx_float_t r;
220    r.m = a.m;
221    r.e = a.e+b;
222    return r;
223 }
224
225 static inline spx_int16_t FLOAT_EXTRACT16(spx_float_t a)
226 {
227    if (a.e<0)
228       return EXTRACT16((EXTEND32(a.m)+(EXTEND32(1)<<(-a.e-1)))>>-a.e);
229    else
230       return a.m<<a.e;
231 }
232
233 static inline spx_int32_t FLOAT_EXTRACT32(spx_float_t a)
234 {
235    if (a.e<0)
236       return (EXTEND32(a.m)+(EXTEND32(1)<<(-a.e-1)))>>-a.e;
237    else
238       return EXTEND32(a.m)<<a.e;
239 }
240
241 static inline spx_int32_t FLOAT_MUL32(spx_float_t a, spx_word32_t b)
242 {
243    return VSHR32(MULT16_32_Q15(a.m, b),-a.e-15);
244 }
245
246 static inline spx_float_t FLOAT_MUL32U(spx_word32_t a, spx_word32_t b)
247 {
248    int e1, e2;
249    spx_float_t r;
250    if (a==0 || b==0)
251    {
252       return FLOAT_ZERO;
253    }
254    e1 = spx_ilog2(ABS32(a));
255    a = VSHR32(a, e1-14);
256    e2 = spx_ilog2(ABS32(b));
257    b = VSHR32(b, e2-14);
258    r.m = MULT16_16_Q15(a,b);
259    r.e = e1+e2-13;
260    return r;
261 }
262
263 /* Do NOT attempt to divide by a negative number */
264 static inline spx_float_t FLOAT_DIV32_FLOAT(spx_word32_t a, spx_float_t b)
265 {
266    int e=0;
267    spx_float_t r;
268    if (a==0)
269    {
270       return FLOAT_ZERO;
271    }
272    e = spx_ilog2(ABS32(a))-spx_ilog2(b.m-1)-15;
273    a = VSHR32(a, e);
274    if (ABS32(a)>=SHL32(EXTEND32(b.m-1),15))
275    {
276       a >>= 1;
277       e++;
278    }
279    r.m = DIV32_16(a,b.m);
280    r.e = e-b.e;
281    return r;
282 }
283
284
285 /* Do NOT attempt to divide by a negative number */
286 static inline spx_float_t FLOAT_DIV32(spx_word32_t a, spx_word32_t b)
287 {
288    int e0=0,e=0;
289    spx_float_t r;
290    if (a==0)
291    {
292       return FLOAT_ZERO;
293    }
294    if (b>32767)
295    {
296       e0 = spx_ilog2(b)-14;
297       b = VSHR32(b, e0);
298       e0 = -e0;
299    }
300    e = spx_ilog2(ABS32(a))-spx_ilog2(b-1)-15;
301    a = VSHR32(a, e);
302    if (ABS32(a)>=SHL32(EXTEND32(b-1),15))
303    {
304       a >>= 1;
305       e++;
306    }
307    e += e0;
308    r.m = DIV32_16(a,b);
309    r.e = e;
310    return r;
311 }
312
313 /* Do NOT attempt to divide by a negative number */
314 static inline spx_float_t FLOAT_DIVU(spx_float_t a, spx_float_t b)
315 {
316    int e=0;
317    spx_int32_t num;
318    spx_float_t r;
319    if (b.m<=0)
320    {
321       speex_warning_int("Attempted to divide by", b.m);
322       return FLOAT_ONE;
323    }
324    num = a.m;
325    a.m = ABS16(a.m);
326    while (a.m >= b.m)
327    {
328       e++;
329       a.m >>= 1;
330    }
331    num = num << (15-e);
332    r.m = DIV32_16(num,b.m);
333    r.e = a.e-b.e-15+e;
334    return r;
335 }
336
337 static inline spx_float_t FLOAT_SQRT(spx_float_t a)
338 {
339    spx_float_t r;
340    spx_int32_t m;
341    m = SHL32(EXTEND32(a.m), 14);
342    r.e = a.e - 14;
343    if (r.e & 1)
344    {
345       r.e -= 1;
346       m <<= 1;
347    }
348    r.e >>= 1;
349    r.m = spx_sqrt(m);
350    return r;
351 }
352
353 #else
354
355 #define spx_float_t float
356 #define FLOAT_ZERO 0.f
357 #define FLOAT_ONE 1.f
358 #define FLOAT_HALF 0.5f
359 #define PSEUDOFLOAT(x) (x)
360 #define FLOAT_MULT(a,b) ((a)*(b))
361 #define FLOAT_AMULT(a,b) ((a)*(b))
362 #define FLOAT_MUL32(a,b) ((a)*(b))
363 #define FLOAT_DIV32(a,b) ((a)/(b))
364 #define FLOAT_EXTRACT16(a) (a)
365 #define FLOAT_EXTRACT32(a) (a)
366 #define FLOAT_ADD(a,b) ((a)+(b))
367 #define FLOAT_SUB(a,b) ((a)-(b))
368 #define REALFLOAT(x) (x)
369 #define FLOAT_DIV32_FLOAT(a,b) ((a)/(b))
370 #define FLOAT_MUL32U(a,b) ((a)*(b))
371 #define FLOAT_SHL(a,b) (a)
372 #define FLOAT_LT(a,b) ((a)<(b))
373 #define FLOAT_GT(a,b) ((a)>(b))
374 #define FLOAT_DIVU(a,b) ((a)/(b))
375 #define FLOAT_SQRT(a) (spx_sqrt(a))
376
377 #endif
378
379 #endif