spandsp  0.0.6
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 /* This is the same as saturate16(), but is here for historic reasons */
46 static __inline__ int16_t saturate(int32_t amp)
47 {
48  int16_t amp16;
49 
50  /* Hopefully this is optimised for the common case - not clipping */
51  amp16 = (int16_t) amp;
52  if (amp == amp16)
53  return amp16;
54  if (amp > INT16_MAX)
55  return INT16_MAX;
56  return INT16_MIN;
57 }
58 /*- End of function --------------------------------------------------------*/
59 
60 static __inline__ int16_t saturate16(int32_t amp)
61 {
62 #if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
63  int16_t z;
64 
65  __asm__ __volatile__(
66  " ssat %[z],#16,%[amp];\n"
67  : [z] "=r" (z)
68  : [amp] "r" (amp)
69  );
70  return z;
71 #else
72  int16_t z;
73 
74  /* Hopefully this is optimised for the common case - not clipping */
75  z = (int16_t) amp;
76  if (amp == z)
77  return z;
78  if (amp > INT16_MAX)
79  return INT16_MAX;
80  return INT16_MIN;
81 #endif
82 }
83 /*- End of function --------------------------------------------------------*/
84 
85 /*! Saturate to 15 bits, rather than the usual 16 bits. This is often a useful function. */
86 static __inline__ int16_t saturate15(int32_t amp)
87 {
88 #if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
89  int16_t z;
90 
91  __asm__ __volatile__(
92  " ssat %[z],#15,%[amp];\n"
93  : [z] "=r" (z)
94  : [amp] "r" (amp)
95  );
96  return z;
97 #else
98  if (amp > 16383)
99  return 16383;
100  if (amp < -16384)
101  return -16384;
102  return (int16_t) amp;
103 #endif
104 }
105 /*- End of function --------------------------------------------------------*/
106 
107 static __inline__ uint16_t saturateu16(int32_t amp)
108 {
109 #if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
110  uint16_t z;
111 
112  __asm__ __volatile__(
113  " usat %[z],#16,%[amp];\n"
114  : [z] "=r" (z)
115  : [amp] "r" (amp)
116  );
117  return z;
118 #else
119  uint16_t z;
120 
121  /* Hopefully this is optimised for the common case - not clipping */
122  z = (uint16_t) amp;
123  if (amp == z)
124  return z;
125  if (amp > UINT16_MAX)
126  return UINT16_MAX;
127  return 0;
128 #endif
129 }
130 /*- End of function --------------------------------------------------------*/
131 
132 static __inline__ uint8_t saturateu8(int32_t amp)
133 {
134 #if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
135  uint8_t z;
136 
137  __asm__ __volatile__(
138  " usat %[z],#8,%[amp];\n"
139  : [z] "=r" (z)
140  : [amp] "r" (amp)
141  );
142  return z;
143 #else
144  uint8_t z;
145 
146  /* Hopefully this is optimised for the common case - not clipping */
147  z = (uint8_t) amp;
148  if (amp == z)
149  return z;
150  if (amp > UINT8_MAX)
151  return UINT8_MAX;
152  return 0;
153 #endif
154 }
155 /*- End of function --------------------------------------------------------*/
156 
157 static __inline__ int16_t fsaturatef(float famp)
158 {
159  if (famp > (float) INT16_MAX)
160  return INT16_MAX;
161  if (famp < (float) INT16_MIN)
162  return INT16_MIN;
163  return (int16_t) lrintf(famp);
164 }
165 /*- End of function --------------------------------------------------------*/
166 
167 static __inline__ int16_t fsaturate(double damp)
168 {
169  if (damp > (double) INT16_MAX)
170  return INT16_MAX;
171  if (damp < (double) INT16_MIN)
172  return INT16_MIN;
173  return (int16_t) lrint(damp);
174 }
175 /*- End of function --------------------------------------------------------*/
176 
177 /* Saturate to a 16 bit integer, using the fastest float to int conversion */
178 static __inline__ int16_t ffastsaturatef(float famp)
179 {
180  if (famp > (float) INT16_MAX)
181  return INT16_MAX;
182  if (famp < (float) INT16_MIN)
183  return INT16_MIN;
184  return (int16_t) lfastrintf(famp);
185 }
186 /*- End of function --------------------------------------------------------*/
187 
188 /* Saturate to a 16 bit integer, using the fastest double to int conversion */
189 static __inline__ int16_t ffastsaturate(double damp)
190 {
191  if (damp > (double) INT16_MAX)
192  return INT16_MAX;
193  if (damp < (double) INT16_MIN)
194  return INT16_MIN;
195  return (int16_t) lfastrint(damp);
196 }
197 /*- End of function --------------------------------------------------------*/
198 
199 /* Saturate to a 16 bit integer, using the closest float to int conversion */
200 static __inline__ float ffsaturatef(float famp)
201 {
202  if (famp > (float) INT16_MAX)
203  return (float) INT16_MAX;
204  if (famp < (float) INT16_MIN)
205  return (float) INT16_MIN;
206  return famp;
207 }
208 /*- End of function --------------------------------------------------------*/
209 
210 /* Saturate to a 16 bit integer, using the closest double to int conversion */
211 static __inline__ double ffsaturate(double famp)
212 {
213  if (famp > (double) INT16_MAX)
214  return (double) INT16_MAX;
215  if (famp < (double) INT16_MIN)
216  return (double) INT16_MIN;
217  return famp;
218 }
219 /*- End of function --------------------------------------------------------*/
220 
221 static __inline__ int16_t saturated_add16(int16_t x, int16_t y)
222 {
223 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
224  __asm__ __volatile__(" addw %[y],%[x];\n"
225  " jno 0f;\n"
226  " movw $0x7FFF,%[x];\n"
227  " adcw $0,%[x];\n"
228  "0:"
229  : [x] "+r" (x)
230  : [y] "ir" (y)
231  : "cc");
232  return x;
233 #elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
234  int16_t z;
235 
236  __asm__ __volatile__(
237  " qadd16 %[z],%[x],%[y];\n"
238  : [z] "=r" (z)
239  : [x] "r" (x), [y] "r" (y)
240  );
241  return z;
242 //#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
243 #else
244  return saturate16((int32_t) x + y);
245 #endif
246 }
247 /*- End of function --------------------------------------------------------*/
248 
249 static __inline__ int32_t saturated_add32(int32_t x, int32_t y)
250 {
251 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
252  __asm__ __volatile__(" addl %[y],%[x];\n"
253  " jno 0f;\n"
254  " movl $0x7FFFFFFF,%[x];\n"
255  " adcl $0,%[x];\n"
256  "0:"
257  : [x] "+r" (x)
258  : [y] "ir" (y)
259  : "cc");
260  return x;
261 #elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
262  int32_t z;
263 
264  __asm__ __volatile__(" qadd %[z],%[x],%[y];\n"
265  : [z] "=r" (z)
266  : [x] "r" (x), [y] "r" (y));
267  return z;
268 //#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
269 #else
270  int32_t z;
271 
272  z = x + y;
273  if ((x ^ y) >= 0)
274  {
275  if ((z ^ x) < 0)
276  z = (x < 0) ? INT32_MIN : INT32_MAX;
277  }
278  return z;
279 #endif
280 }
281 /*- End of function --------------------------------------------------------*/
282 
283 static __inline__ int16_t saturated_sub16(int16_t x, int16_t y)
284 {
285 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
286  __asm__ __volatile__(" subw %[y],%[x];\n"
287  " jno 0f;\n"
288  " movw $0x8000,%[x];\n"
289  " sbbw $0,%[x];\n"
290  "0:"
291  : [x] "+r" (x)
292  : [y] "ir" (y)
293  : "cc");
294  return x;
295 #elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
296  int16_t z;
297 
298  __asm__ __volatile__(" qsub16 %[z],%[x],%[y];\n"
299  : [z] "=r" (z)
300  : [x] "r" (x), [y] "r" (y));
301  return z;
302 //#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
303 #else
304  return saturate16((int32_t) x - y);
305 #endif
306 }
307 /*- End of function --------------------------------------------------------*/
308 
309 static __inline__ int32_t saturated_sub32(int32_t x, int32_t y)
310 {
311 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
312  __asm__ __volatile__(" subl %[y],%[x];\n"
313  " jno 0f;\n"
314  " movl $0x80000000,%[x];\n"
315  " sbbl $0,%[x];\n"
316  "0:"
317  : [x] "+r" (x)
318  : [y] "ir" (y)
319  : "cc");
320  return x;
321 #elif defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
322  int32_t z;
323 
324  __asm__ __volatile__(" qsub %[z],%[x],%[y];\n"
325  : [z] "=r" (z)
326  : [x] "r" (x), [y] "r" (y));
327  return z;
328 //#elif defined(__GNUC__) && defined(__ARM_ARCH_5T__)
329 #else
330  int32_t z;
331 
332  z = x - y;
333  if ((x ^ y) < 0)
334  {
335  if ((z ^ x) < 0)
336  z = (x < 0L) ? INT32_MIN : INT32_MAX;
337  }
338  return z;
339 #endif
340 }
341 /*- End of function --------------------------------------------------------*/
342 
343 static __inline__ int16_t saturated_mul16(int16_t x, int16_t y)
344 {
345  int32_t z;
346 
347 #if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
348  __asm__ __volatile__(" smulbb %[z],%[x],%[y];\n"
349  " qadd %[z],%[z],%[z];\n"
350  : [z] "=r" (z)
351  : [x] "r" (x), [y] "r" (y));
352  /* The qadd added one to the shift of 15 */
353  return (int16_t) (z >> 16);
354 #else
355  z = (int32_t) x*y;
356  if (z == 0x40000000)
357  return INT16_MAX;
358  /*endif*/
359  return (int16_t) (z >> 15);
360 #endif
361 }
362 /*- End of function --------------------------------------------------------*/
363 
364 static __inline__ int32_t saturated_mul16_32(int16_t x, int16_t y)
365 {
366  int32_t z;
367 
368 #if defined(__GNUC__) && (defined(__ARM_ARCH_6__) || defined(__ARM_ARCH_7A__))
369  __asm__ __volatile__(" smulbb %[z],%[x],%[y];\n"
370  " qadd %[z],%[z],%[z];\n"
371  : [z] "=r" (z)
372  : [x] "r" (x), [y] "r" (y));
373  return z;
374 #else
375  z = (int32_t) x*y;
376  if (z == 0x40000000)
377  return INT32_MAX;
378  return z << 1;
379 #endif
380 }
381 /*- End of function --------------------------------------------------------*/
382 
383 static __inline__ int16_t saturated_abs16(int16_t x)
384 {
385  if (x == INT16_MIN)
386  return INT16_MAX;
387  return (int16_t) abs(x);
388 }
389 /*- End of function --------------------------------------------------------*/
390 
391 static __inline__ int32_t sat_abs32(int32_t x)
392 {
393  if (x == INT32_MIN)
394  return INT32_MAX;
395  return abs(x);
396 }
397 /*- End of function --------------------------------------------------------*/
398 
399 #if defined(__cplusplus)
400 }
401 #endif
402 
403 #endif
404 /*- End of file ------------------------------------------------------------*/