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