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),((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
)>=(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
)>=(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
) + ((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)