1
/* Copyright (C) 2003 Jean-Marc Valin */
4
@brief Fixed-point operations with debugging
7
Redistribution and use in source and binary forms, with or without
8
modification, are permitted provided that the following conditions
11
- Redistributions of source code must retain the above copyright
12
notice, this list of conditions and the following disclaimer.
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.
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.
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.
40
extern long long spx_mips;
41
#define MIPS_INC spx_mips++,
43
#define QCONST16(x,bits) ((spx_word16_t)(.5+(x)*(((spx_word32_t)1)<<(bits))))
44
#define QCONST32(x,bits) ((spx_word32_t)(.5+(x)*(((spx_word32_t)1)<<(bits))))
47
#define VERIFY_SHORT(x) ((x)<=32767&&(x)>=-32768)
48
#define VERIFY_INT(x) ((x)<=2147483647LL&&(x)>=-2147483648LL)
50
static inline short NEG16(int x)
55
fprintf (stderr, "NEG16: input is not short: %d\n", (int)x);
58
if (!VERIFY_SHORT(res))
59
fprintf (stderr, "NEG16: output is not short: %d\n", (int)res);
63
static inline int NEG32(long long x)
68
fprintf (stderr, "NEG16: input is not int: %d\n", (int)x);
72
fprintf (stderr, "NEG16: output is not int: %d\n", (int)res);
77
#define EXTRACT16(x) _EXTRACT16(x, __FILE__, __LINE__)
78
static inline short _EXTRACT16(int x, char *file, int line)
83
fprintf (stderr, "EXTRACT16: input is not short: %d in %s: line %d\n", x, file, line);
90
#define EXTEND32(x) _EXTEND32(x, __FILE__, __LINE__)
91
static inline int _EXTEND32(int x, char *file, int line)
96
fprintf (stderr, "EXTEND32: input is not short: %d in %s: line %d\n", x, file, line);
103
#define SHR16(a, shift) _SHR16(a, shift, __FILE__, __LINE__)
104
static inline short _SHR16(int a, int shift, char *file, int line)
107
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(shift))
109
fprintf (stderr, "SHR16: inputs are not short: %d >> %d in %s: line %d\n", a, shift, file, line);
112
if (!VERIFY_SHORT(res))
113
fprintf (stderr, "SHR16: output is not short: %d in %s: line %d\n", res, file, line);
117
#define SHL16(a, shift) _SHL16(a, shift, __FILE__, __LINE__)
118
static inline short _SHL16(int a, int shift, char *file, int line)
121
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(shift))
123
fprintf (stderr, "SHL16: inputs are not short: %d %d in %s: line %d\n", a, shift, file, line);
126
if (!VERIFY_SHORT(res))
127
fprintf (stderr, "SHL16: output is not short: %d in %s: line %d\n", res, file, line);
132
static inline int SHR32(long long a, int shift)
135
if (!VERIFY_INT(a) || !VERIFY_SHORT(shift))
137
fprintf (stderr, "SHR32: inputs are not int: %d %d\n", (int)a, shift);
140
if (!VERIFY_INT(res))
142
fprintf (stderr, "SHR32: output is not int: %d\n", (int)res);
147
static inline int SHL32(long long a, int shift)
150
if (!VERIFY_INT(a) || !VERIFY_SHORT(shift))
152
fprintf (stderr, "SHL32: inputs are not int: %d %d\n", (int)a, shift);
155
if (!VERIFY_INT(res))
157
fprintf (stderr, "SHL32: output is not int: %d\n", (int)res);
163
#define PSHR16(a,shift) (SHR16(ADD16((a),((1<<((shift))>>1))),shift))
164
#define PSHR32(a,shift) (SHR32(ADD32((a),((EXTEND32(1)<<((shift))>>1))),shift))
165
#define VSHR32(a, shift) (((shift)>0) ? SHR32(a, shift) : SHL32(a, -(shift)))
167
#define SATURATE16(x,a) (((x)>(a) ? (a) : (x)<-(a) ? -(a) : (x)))
168
#define SATURATE32(x,a) (((x)>(a) ? (a) : (x)<-(a) ? -(a) : (x)))
170
//#define SHR(a,shift) ((a) >> (shift))
171
//#define SHL(a,shift) ((a) << (shift))
173
#define ADD16(a, b) _ADD16(a, b, __FILE__, __LINE__)
174
static inline short _ADD16(int a, int b, char *file, int line)
177
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
179
fprintf (stderr, "ADD16: inputs are not short: %d %d in %s: line %d\n", a, b, file, line);
182
if (!VERIFY_SHORT(res))
184
fprintf (stderr, "ADD16: output is not short: %d+%d=%d in %s: line %d\n", a,b,res, file, line);
190
#define SUB16(a, b) _SUB16(a, b, __FILE__, __LINE__)
191
static inline short _SUB16(int a, int b, char *file, int line)
194
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
196
fprintf (stderr, "SUB16: inputs are not short: %d %d in %s: line %d\n", a, b, file, line);
199
if (!VERIFY_SHORT(res))
200
fprintf (stderr, "SUB16: output is not short: %d in %s: line %d\n", res, file, line);
205
#define ADD32(a, b) _ADD32(a, b, __FILE__, __LINE__)
206
static inline int _ADD32(long long a, long long b, char *file, int line)
209
if (!VERIFY_INT(a) || !VERIFY_INT(b))
211
fprintf (stderr, "ADD32: inputs are not int: %d %d in %s: line %d\n", (int)a, (int)b, file, line);
214
if (!VERIFY_INT(res))
216
fprintf (stderr, "ADD32: output is not int: %d in %s: line %d\n", (int)res, file, line);
222
static inline int SUB32(long long a, long long b)
225
if (!VERIFY_INT(a) || !VERIFY_INT(b))
227
fprintf (stderr, "SUB32: inputs are not int: %d %d\n", (int)a, (int)b);
230
if (!VERIFY_INT(res))
231
fprintf (stderr, "SUB32: output is not int: %d\n", (int)res);
236
#define ADD64(a,b) (MIPS_INC(a)+(b))
238
/* result fits in 16 bits */
239
static inline short MULT16_16_16(int a, int b)
242
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
244
fprintf (stderr, "MULT16_16_16: inputs are not short: %d %d\n", a, b);
247
if (!VERIFY_SHORT(res))
248
fprintf (stderr, "MULT16_16_16: output is not short: %d\n", res);
253
#define MULT16_16(a, b) _MULT16_16(a, b, __FILE__, __LINE__)
254
static inline int _MULT16_16(int a, int b, char *file, int line)
257
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
259
fprintf (stderr, "MULT16_16: inputs are not short: %d %d in %s: line %d\n", a, b, file, line);
261
res = ((long long)a)*b;
262
if (!VERIFY_INT(res))
263
fprintf (stderr, "MULT16_16: output is not int: %d in %s: line %d\n", (int)res, file, line);
268
#define MAC16_16(c,a,b) (spx_mips--,ADD32((c),MULT16_16((a),(b))))
269
#define MAC16_16_Q11(c,a,b) (EXTRACT16(ADD16((c),EXTRACT16(SHR32(MULT16_16((a),(b)),11)))))
270
#define MAC16_16_Q13(c,a,b) (EXTRACT16(ADD16((c),EXTRACT16(SHR32(MULT16_16((a),(b)),13)))))
271
#define MAC16_16_P13(c,a,b) (EXTRACT16(ADD32((c),SHR32(ADD32(4096,MULT16_16((a),(b))),13))))
274
#define MULT16_32_QX(a, b, Q) _MULT16_32_QX(a, b, Q, __FILE__, __LINE__)
275
static inline int _MULT16_32_QX(int a, long long b, int Q, char *file, int line)
278
if (!VERIFY_SHORT(a) || !VERIFY_INT(b))
280
fprintf (stderr, "MULT16_32_Q%d: inputs are not short+int: %d %d in %s: line %d\n", Q, (int)a, (int)b, file, line);
282
if (ABS32(b)>=(EXTEND32(1)<<(15+Q)))
283
fprintf (stderr, "MULT16_32_Q%d: second operand too large: %d %d in %s: line %d\n", Q, (int)a, (int)b, file, line);
284
res = (((long long)a)*(long long)b) >> Q;
285
if (!VERIFY_INT(res))
286
fprintf (stderr, "MULT16_32_Q%d: output is not int: %d*%d=%d in %s: line %d\n", Q, (int)a, (int)b,(int)res, file, line);
291
static inline int MULT16_32_PX(int a, long long b, int Q)
294
if (!VERIFY_SHORT(a) || !VERIFY_INT(b))
296
fprintf (stderr, "MULT16_32_P%d: inputs are not short+int: %d %d\n", Q, (int)a, (int)b);
298
if (ABS32(b)>=(EXTEND32(1)<<(15+Q)))
299
fprintf (stderr, "MULT16_32_Q%d: second operand too large: %d %d\n", Q, (int)a, (int)b);
300
res = ((((long long)a)*(long long)b) + ((EXTEND32(1)<<Q)>>1))>> Q;
301
if (!VERIFY_INT(res))
302
fprintf (stderr, "MULT16_32_P%d: output is not int: %d*%d=%d\n", Q, (int)a, (int)b,(int)res);
308
#define MULT16_32_Q11(a,b) MULT16_32_QX(a,b,11)
309
#define MAC16_32_Q11(c,a,b) ADD32((c),MULT16_32_Q11((a),(b)))
310
#define MULT16_32_Q12(a,b) MULT16_32_QX(a,b,12)
311
#define MULT16_32_Q13(a,b) MULT16_32_QX(a,b,13)
312
#define MULT16_32_Q14(a,b) MULT16_32_QX(a,b,14)
313
#define MULT16_32_Q15(a,b) MULT16_32_QX(a,b,15)
314
#define MULT16_32_P15(a,b) MULT16_32_PX(a,b,15)
315
#define MAC16_32_Q15(c,a,b) ADD32((c),MULT16_32_Q15((a),(b)))
317
static inline int SATURATE(int a, int b)
326
static inline int MULT16_16_Q11_32(int a, int b)
329
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
331
fprintf (stderr, "MULT16_16_Q11: inputs are not short: %d %d\n", a, b);
333
res = ((long long)a)*b;
335
if (!VERIFY_INT(res))
336
fprintf (stderr, "MULT16_16_Q11: output is not short: %d*%d=%d\n", (int)a, (int)b, (int)res);
340
static inline short MULT16_16_Q13(int a, int b)
343
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
345
fprintf (stderr, "MULT16_16_Q13: inputs are not short: %d %d\n", a, b);
347
res = ((long long)a)*b;
349
if (!VERIFY_SHORT(res))
350
fprintf (stderr, "MULT16_16_Q13: output is not short: %d*%d=%d\n", a, b, (int)res);
354
static inline short MULT16_16_Q14(int a, int b)
357
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
359
fprintf (stderr, "MULT16_16_Q14: inputs are not short: %d %d\n", a, b);
361
res = ((long long)a)*b;
363
if (!VERIFY_SHORT(res))
364
fprintf (stderr, "MULT16_16_Q14: output is not short: %d\n", (int)res);
368
static inline short MULT16_16_Q15(int a, int b)
371
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
373
fprintf (stderr, "MULT16_16_Q15: inputs are not short: %d %d\n", a, b);
375
res = ((long long)a)*b;
377
if (!VERIFY_SHORT(res))
379
fprintf (stderr, "MULT16_16_Q15: output is not short: %d\n", (int)res);
385
static inline short MULT16_16_P13(int a, int b)
388
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
390
fprintf (stderr, "MULT16_16_P13: inputs are not short: %d %d\n", a, b);
392
res = ((long long)a)*b;
394
if (!VERIFY_INT(res))
395
fprintf (stderr, "MULT16_16_P13: overflow: %d*%d=%d\n", a, b, (int)res);
397
if (!VERIFY_SHORT(res))
398
fprintf (stderr, "MULT16_16_P13: output is not short: %d*%d=%d\n", a, b, (int)res);
402
static inline short MULT16_16_P14(int a, int b)
405
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
407
fprintf (stderr, "MULT16_16_P14: inputs are not short: %d %d\n", a, b);
409
res = ((long long)a)*b;
411
if (!VERIFY_INT(res))
412
fprintf (stderr, "MULT16_16_P14: overflow: %d*%d=%d\n", a, b, (int)res);
414
if (!VERIFY_SHORT(res))
415
fprintf (stderr, "MULT16_16_P14: output is not short: %d*%d=%d\n", a, b, (int)res);
419
static inline short MULT16_16_P15(int a, int b)
422
if (!VERIFY_SHORT(a) || !VERIFY_SHORT(b))
424
fprintf (stderr, "MULT16_16_P15: inputs are not short: %d %d\n", a, b);
426
res = ((long long)a)*b;
428
if (!VERIFY_INT(res))
429
fprintf (stderr, "MULT16_16_P15: overflow: %d*%d=%d\n", a, b, (int)res);
431
if (!VERIFY_SHORT(res))
432
fprintf (stderr, "MULT16_16_P15: output is not short: %d*%d=%d\n", a, b, (int)res);
437
#define DIV32_16(a, b) _DIV32_16(a, b, __FILE__, __LINE__)
439
static inline int _DIV32_16(long long a, long long b, char *file, int line)
444
fprintf(stderr, "DIV32_16: divide by zero: %d/%d in %s: line %d\n", (int)a, (int)b, file, line);
447
if (!VERIFY_INT(a) || !VERIFY_SHORT(b))
449
fprintf (stderr, "DIV32_16: inputs are not int/short: %d %d in %s: line %d\n", (int)a, (int)b, file, line);
452
if (!VERIFY_SHORT(res))
454
fprintf (stderr, "DIV32_16: output is not short: %d / %d = %d in %s: line %d\n", (int)a,(int)b,(int)res, file, line);
464
#define DIV32(a, b) _DIV32(a, b, __FILE__, __LINE__)
465
static inline int _DIV32(long long a, long long b, char *file, int line)
470
fprintf(stderr, "DIV32: divide by zero: %d/%d in %s: line %d\n", (int)a, (int)b, file, line);
474
if (!VERIFY_INT(a) || !VERIFY_INT(b))
476
fprintf (stderr, "DIV32: inputs are not int/short: %d %d in %s: line %d\n", (int)a, (int)b, file, line);
479
if (!VERIFY_INT(res))
480
fprintf (stderr, "DIV32: output is not int: %d in %s: line %d\n", (int)res, file, line);
484
#define PDIV32(a,b) DIV32(ADD32((a),(b)>>1),b)
485
#define PDIV32_16(a,b) DIV32_16(ADD32((a),(b)>>1),b)