spandsp 3.0.0
saturated.h
Go to the documentation of this file.
1/*
2 * SpanDSP - a series of DSP components for telephony
3 *
4 * saturated.h - General saturated arithmetic routines.
5 *
6 * Written by Steve Underwood <steveu@coppice.org>
7 *
8 * Copyright (C) 2001, 2008 Steve Underwood
9 *
10 * All rights reserved.
11 *
12 * This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU Lesser General Public License version 2.1,
14 * as published by the Free Software Foundation.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public
22 * License along with this program; if not, write to the Free Software
23 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 */
25
26/*! \file */
27
28#if !defined(_SPANDSP_SATURATED_H_)
29#define _SPANDSP_SATURATED_H_
30
31/*! \page saturated_page Saturated arithmetic
32
33\section saturated_page_sec_1 What does it do?
34
35
36\section saturated_page_sec_2 How does it work?
37
38*/
39
40#if defined(__cplusplus)
41extern "C"
42{
43#endif
44
45static __inline__ int16_t saturate16(int32_t amp)
46{
47#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
48 int16_t z;
49
50 __asm__ __volatile__(
51 " ssat %[z],#16,%[amp];\n"
52 : [z] "=r" (z)
53 : [amp] "r" (amp)
54 );
55 return z;
56#else
57 int16_t z;
58
59 /* Hopefully this is optimised for the common case - not clipping */
60 z = (int16_t) amp;
61 if (amp == z)
62 return z;
63 if (amp > INT16_MAX)
64 return INT16_MAX;
65 return INT16_MIN;
66#endif
67}
68/*- End of function --------------------------------------------------------*/
69
70/*! Saturate to 15 bits, rather than the usual 16 bits. This is often a useful function. */
71static __inline__ int16_t saturate15(int32_t amp)
72{
73#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
74 int16_t z;
75
76 __asm__ __volatile__(
77 " ssat %[z],#15,%[amp];\n"
78 : [z] "=r" (z)
79 : [amp] "r" (amp)
80 );
81 return z;
82#else
83 if (amp > 16383)
84 return 16383;
85 if (amp < -16384)
86 return -16384;
87 return (int16_t) amp;
88#endif
89}
90/*- End of function --------------------------------------------------------*/
91
92static __inline__ uint16_t saturateu16(int32_t amp)
93{
94#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
95 uint16_t z;
96
97 __asm__ __volatile__(
98 " usat %[z],#16,%[amp];\n"
99 : [z] "=r" (z)
100 : [amp] "r" (amp)
101 );
102 return z;
103#else
104 uint16_t z;
105
106 /* Hopefully this is optimised for the common case - not clipping */
107 z = (uint16_t) amp;
108 if (amp == z)
109 return z;
110 if (amp > UINT16_MAX)
111 return UINT16_MAX;
112 return 0;
113#endif
114}
115/*- End of function --------------------------------------------------------*/
116
117static __inline__ uint8_t saturateu8(int32_t amp)
118{
119#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
120 uint8_t z;
121
122 __asm__ __volatile__(
123 " usat %[z],#8,%[amp];\n"
124 : [z] "=r" (z)
125 : [amp] "r" (amp)
126 );
127 return z;
128#else
129 uint8_t z;
130
131 /* Hopefully this is optimised for the common case - not clipping */
132 z = (uint8_t) amp;
133 if (amp == z)
134 return z;
135 if (amp > UINT8_MAX)
136 return UINT8_MAX;
137 return 0;
138#endif
139}
140/*- End of function --------------------------------------------------------*/
141
142static __inline__ int16_t fsaturatef(float famp)
143{
144 if (famp > (float) INT16_MAX)
145 return INT16_MAX;
146 if (famp < (float) INT16_MIN)
147 return INT16_MIN;
148 return (int16_t) lrintf(famp);
149}
150/*- End of function --------------------------------------------------------*/
151
152static __inline__ int16_t fsaturate(double damp)
153{
154 if (damp > (double) INT16_MAX)
155 return INT16_MAX;
156 if (damp < (double) INT16_MIN)
157 return INT16_MIN;
158 return (int16_t) lrint(damp);
159}
160/*- End of function --------------------------------------------------------*/
161
162/* Saturate to a 16 bit integer, using the fastest float to int conversion */
163static __inline__ int16_t ffastsaturatef(float famp)
164{
165 if (famp > (float) INT16_MAX)
166 return INT16_MAX;
167 if (famp < (float) INT16_MIN)
168 return INT16_MIN;
169 return (int16_t) lfastrintf(famp);
170}
171/*- End of function --------------------------------------------------------*/
172
173/* Saturate to a 16 bit integer, using the fastest double to int conversion */
174static __inline__ int16_t ffastsaturate(double damp)
175{
176 if (damp > (double) INT16_MAX)
177 return INT16_MAX;
178 if (damp < (double) INT16_MIN)
179 return INT16_MIN;
180 return (int16_t) lfastrint(damp);
181}
182/*- End of function --------------------------------------------------------*/
183
184/* Saturate to a 16 bit integer, using the closest float to int conversion */
185static __inline__ float ffsaturatef(float famp)
186{
187 if (famp > (float) INT16_MAX)
188 return (float) INT16_MAX;
189 if (famp < (float) INT16_MIN)
190 return (float) INT16_MIN;
191 return famp;
192}
193/*- End of function --------------------------------------------------------*/
194
195/* Saturate to a 16 bit integer, using the closest double to int conversion */
196static __inline__ double ffsaturate(double famp)
197{
198 if (famp > (double) INT16_MAX)
199 return (double) INT16_MAX;
200 if (famp < (double) INT16_MIN)
201 return (double) INT16_MIN;
202 return famp;
203}
204/*- End of function --------------------------------------------------------*/
205
206static __inline__ int16_t sat_add16(int16_t x, int16_t y)
207{
208#if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
209 __asm__ __volatile__(" addw %[y],%[x];\n"
210 " jno 0f;\n"
211 " movw $0x7FFF,%[x];\n"
212 " adcw $0,%[x];\n"
213 "0:"
214 : [x] "+r" (x)
215 : [y] "ir" (y)
216 : "cc");
217 return x;
218#elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
219 int16_t z;
220
221 __asm__ __volatile__(
222 " qadd16 %[z],%[x],%[y];\n"
223 : [z] "=r" (z)
224 : [x] "r" (x), [y] "r" (y)
225 );
226 return z;
227//#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
228#else
229 return saturate16((int32_t) x + y);
230#endif
231}
232/*- End of function --------------------------------------------------------*/
233
234static __inline__ int32_t sat_add32(int32_t x, int32_t y)
235{
236#if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
237 __asm__ __volatile__(" addl %[y],%[x];\n"
238 " jno 0f;\n"
239 " movl $0x7FFFFFFF,%[x];\n"
240 " adcl $0,%[x];\n"
241 "0:"
242 : [x] "+r" (x)
243 : [y] "ir" (y)
244 : "cc");
245 return x;
246#elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
247 int32_t z;
248
249 __asm__ __volatile__(" qadd %[z],%[x],%[y];\n"
250 : [z] "=r" (z)
251 : [x] "r" (x), [y] "r" (y));
252 return z;
253//#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
254#else
255 int32_t z;
256
257 z = x + y;
258 if ((x ^ y) >= 0)
259 {
260 if ((z ^ x) < 0)
261 z = (x < 0) ? INT32_MIN : INT32_MAX;
262 }
263 return z;
264#endif
265}
266/*- End of function --------------------------------------------------------*/
267
268static __inline__ int16_t sat_sub16(int16_t x, int16_t y)
269{
270#if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
271 __asm__ __volatile__(" subw %[y],%[x];\n"
272 " jno 0f;\n"
273 " movw $0x8000,%[x];\n"
274 " sbbw $0,%[x];\n"
275 "0:"
276 : [x] "+r" (x)
277 : [y] "ir" (y)
278 : "cc");
279 return x;
280#elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
281 int16_t z;
282
283 __asm__ __volatile__(" qsub16 %[z],%[x],%[y];\n"
284 : [z] "=r" (z)
285 : [x] "r" (x), [y] "r" (y));
286 return z;
287//#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
288#else
289 return saturate16((int32_t) x - y);
290#endif
291}
292/*- End of function --------------------------------------------------------*/
293
294static __inline__ int32_t sat_sub32(int32_t x, int32_t y)
295{
296#if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
297 __asm__ __volatile__(" subl %[y],%[x];\n"
298 " jno 0f;\n"
299 " movl $0x80000000,%[x];\n"
300 " sbbl $0,%[x];\n"
301 "0:"
302 : [x] "+r" (x)
303 : [y] "ir" (y)
304 : "cc");
305 return x;
306#elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
307 int32_t z;
308
309 __asm__ __volatile__(" qsub %[z],%[x],%[y];\n"
310 : [z] "=r" (z)
311 : [x] "r" (x), [y] "r" (y));
312 return z;
313//#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
314#else
315 int32_t z;
316
317 z = x - y;
318 if ((x ^ y) < 0)
319 {
320 if ((z ^ x) < 0)
321 z = (x < 0L) ? INT32_MIN : INT32_MAX;
322 }
323 return z;
324#endif
325}
326/*- End of function --------------------------------------------------------*/
327
328static __inline__ int16_t sat_mul16(int16_t x, int16_t y)
329{
330 int32_t z;
331
332#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
333 __asm__ __volatile__(" smulbb %[z],%[x],%[y];\n"
334 " qadd %[z],%[z],%[z];\n"
335 : [z] "=r" (z)
336 : [x] "r" (x), [y] "r" (y));
337 /* The qadd added one to the shift of 15 */
338 return (int16_t) (z >> 16);
339#else
340 z = (int32_t) x*y;
341 if (z == 0x40000000)
342 return INT16_MAX;
343 /*endif*/
344 return (int16_t) (z >> 15);
345#endif
346}
347/*- End of function --------------------------------------------------------*/
348
349static __inline__ int32_t sat_mul32_16(int16_t x, int16_t y)
350{
351 int32_t z;
352
353#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
354 __asm__ __volatile__(" smulbb %[z],%[x],%[y];\n"
355 " qadd %[z],%[z],%[z];\n"
356 : [z] "=r" (z)
357 : [x] "r" (x), [y] "r" (y));
358 return z;
359#else
360 z = (int32_t) x*y;
361 if (z == 0x40000000)
362 return INT32_MAX;
363 return z << 1;
364#endif
365}
366/*- End of function --------------------------------------------------------*/
367
368static __inline__ int32_t sat_mac32_16(int32_t z, int16_t x, int16_t y)
369{
370#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
371 int32_t product;
372
373 __asm__ __volatile__(" smulbb %[p],%[x],%[y];\n"
374 " qdadd %[z],%[z],%[p];\n"
375 : [z] "+r" (z)
376 : [x] "r" (x), [y] "r" (y), [p] "r" (product));
377 return z;
378#else
379 return sat_add32(z, sat_mul32_16(x, y));
380#endif
381}
382/*- End of function --------------------------------------------------------*/
383
384static __inline__ int32_t sat_msu32_16(int32_t z, int16_t x, int16_t y)
385{
386#if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
387 int32_t product;
388
389 __asm__ __volatile__(" smulbb %[p],%[x],%[y];\n"
390 " qdsub %[z],%[z],%[p];\n"
391 : [z] "+r" (z)
392 : [x] "r" (x), [y] "r" (y), [p] "r" (product));
393 return z;
394#else
395 return sat_sub32(z, sat_mul32_16(x, y));
396#endif
397}
398/*- End of function --------------------------------------------------------*/
399
400static __inline__ int16_t sat_abs16(int16_t x)
401{
402 if (x == INT16_MIN)
403 return INT16_MAX;
404 return (int16_t) abs(x);
405}
406/*- End of function --------------------------------------------------------*/
407
408static __inline__ int32_t sat_abs32(int32_t x)
409{
410 if (x == INT32_MIN)
411 return INT32_MAX;
412 return abs(x);
413}
414/*- End of function --------------------------------------------------------*/
415
416#if defined(__cplusplus)
417}
418#endif
419
420#endif
421/*- End of file ------------------------------------------------------------*/