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 "arch.h"
48#include "os_support.h"
49#include "math_approx.h"
50#include <math.h>
51
52#ifdef FIXED_POINT
53
54typedef struct {
55   spx_int16_t m;
56   spx_int16_t e;
57} spx_float_t;
58
59static const spx_float_t FLOAT_ZERO = {0,0};
60static const spx_float_t FLOAT_ONE = {16384,-14};
61static const spx_float_t FLOAT_HALF = {16384,-15};
62
63#define MIN(a,b) ((a)<(b)?(a):(b))
64static 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
97static 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
132static 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
167static 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
180static inline int FLOAT_GT(spx_float_t a, spx_float_t b)
181{
182   return FLOAT_LT(b,a);
183}
184
185static 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
208static 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
217static 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
225static 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
233static 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
241static 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
246static 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 */
264static 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 */
286static 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 */
314static 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
337static 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
380