spandsp  1.99.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)
41 extern "C"
42 {
43 #endif
44 
45 static __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. */
71 static __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 
92 static __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 
117 static __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 
142 static __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 
152 static __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 */
163 static __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 */
174 static __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 */
185 static __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 */
196 static __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 
206 static __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 
234 static __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 
268 static __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 
294 static __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 
328 static __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 
349 static __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 
368 static __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 
384 static __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 
400 static __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 
408 static __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 ------------------------------------------------------------*/