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  int16_t amp16;
63 
64  /* Hopefully this is optimised for the common case - not clipping */
65  amp16 = (int16_t) amp;
66  if (amp == amp16)
67  return amp16;
68  if (amp > INT16_MAX)
69  return INT16_MAX;
70  return INT16_MIN;
71 }
72 /*- End of function --------------------------------------------------------*/
73 
74 /*! Saturate to 15 bits, rather than the usual 16 bits. This is often a useful function. */
75 static __inline__ int16_t saturate15(int32_t amp)
76 {
77  if (amp > 16383)
78  return 16383;
79  if (amp < -16384)
80  return -16384;
81  return (int16_t) amp;
82 }
83 /*- End of function --------------------------------------------------------*/
84 
85 static __inline__ uint16_t saturateu16(int32_t amp)
86 {
87  uint16_t amp16;
88 
89  /* Hopefully this is optimised for the common case - not clipping */
90  amp16 = (uint16_t) amp;
91  if (amp == amp16)
92  return amp16;
93  if (amp > UINT16_MAX)
94  return UINT16_MAX;
95  return 0;
96 }
97 /*- End of function --------------------------------------------------------*/
98 
99 static __inline__ uint8_t saturateu8(int32_t amp)
100 {
101  uint8_t amp8;
102 
103  /* Hopefully this is optimised for the common case - not clipping */
104  amp8 = (uint8_t) amp;
105  if (amp == amp8)
106  return amp8;
107  if (amp > UINT8_MAX)
108  return UINT8_MAX;
109  return 0;
110 }
111 /*- End of function --------------------------------------------------------*/
112 
113 static __inline__ int16_t fsaturatef(float famp)
114 {
115  if (famp > (float) INT16_MAX)
116  return INT16_MAX;
117  if (famp < (float) INT16_MIN)
118  return INT16_MIN;
119  return (int16_t) lrintf(famp);
120 }
121 /*- End of function --------------------------------------------------------*/
122 
123 static __inline__ int16_t fsaturate(double damp)
124 {
125  if (damp > (double) INT16_MAX)
126  return INT16_MAX;
127  if (damp < (double) INT16_MIN)
128  return INT16_MIN;
129  return (int16_t) lrint(damp);
130 }
131 /*- End of function --------------------------------------------------------*/
132 
133 /* Saturate to a 16 bit integer, using the fastest float to int conversion */
134 static __inline__ int16_t ffastsaturatef(float famp)
135 {
136  if (famp > (float) INT16_MAX)
137  return INT16_MAX;
138  if (famp < (float) INT16_MIN)
139  return INT16_MIN;
140  return (int16_t) lfastrintf(famp);
141 }
142 /*- End of function --------------------------------------------------------*/
143 
144 /* Saturate to a 16 bit integer, using the fastest double to int conversion */
145 static __inline__ int16_t ffastsaturate(double damp)
146 {
147  if (damp > (double) INT16_MAX)
148  return INT16_MAX;
149  if (damp < (double) INT16_MIN)
150  return INT16_MIN;
151  return (int16_t) lfastrint(damp);
152 }
153 /*- End of function --------------------------------------------------------*/
154 
155 /* Saturate to a 16 bit integer, using the closest float to int conversion */
156 static __inline__ float ffsaturatef(float famp)
157 {
158  if (famp > (float) INT16_MAX)
159  return (float) INT16_MAX;
160  if (famp < (float) INT16_MIN)
161  return (float) INT16_MIN;
162  return famp;
163 }
164 /*- End of function --------------------------------------------------------*/
165 
166 /* Saturate to a 16 bit integer, using the closest double to int conversion */
167 static __inline__ double ffsaturate(double famp)
168 {
169  if (famp > (double) INT16_MAX)
170  return (double) INT16_MAX;
171  if (famp < (double) INT16_MIN)
172  return (double) INT16_MIN;
173  return famp;
174 }
175 /*- End of function --------------------------------------------------------*/
176 
177 static __inline__ int16_t saturated_add16(int16_t a, int16_t b)
178 {
179 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
180  __asm__ __volatile__(
181  " addw %2,%0;\n"
182  " jno 0f;\n"
183  " movw $0x7fff,%0;\n"
184  " adcw $0,%0;\n"
185  "0:"
186  : "=r" (a)
187  : "0" (a), "ir" (b)
188  : "cc"
189  );
190  return a;
191 #elif defined(__GNUC__) && defined(__arm5__)
192  int16_t result;
193 
194  __asm__ __volatile__(
195  " sadd16 %0,%1,%2;\n"
196  : "=r" (result)
197  : "0" (a), "ir" (b)
198  );
199  return result;
200 #else
201  return saturate((int32_t) a + (int32_t) b);
202 #endif
203 }
204 /*- End of function --------------------------------------------------------*/
205 
206 static __inline__ int32_t saturated_add32(int32_t a, int32_t b)
207 {
208 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
209  __asm__ __volatile__(
210  " addl %2,%0;\n"
211  " jno 0f;\n"
212  " movl $0x7fffffff,%0;\n"
213  " adcl $0,%0;\n"
214  "0:"
215  : "=r" (a)
216  : "0" (a), "ir" (b)
217  : "cc"
218  );
219  return a;
220 #elif defined(__GNUC__) && defined(__arm5__)
221  int32_t result;
222 
223  __asm__ __volatile__(
224  " qadd %0,%1,%2;\n"
225  : "=r" (result)
226  : "0" (a), "ir" (b)
227  );
228  return result;
229 #else
230  int32_t sum;
231 
232  sum = a + b;
233  if ((a ^ b) >= 0)
234  {
235  if ((sum ^ a) < 0)
236  sum = (a < 0) ? INT32_MIN : INT32_MAX;
237  }
238  return sum;
239 #endif
240 }
241 /*- End of function --------------------------------------------------------*/
242 
243 static __inline__ int16_t saturated_sub16(int16_t a, int16_t b)
244 {
245 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
246  __asm__ __volatile__(
247  " subw %2,%0;\n"
248  " jno 0f;\n"
249  " movw $0x8000,%0;\n"
250  " sbbw $0,%0;\n"
251  "0:"
252  : "=r" (a)
253  : "0" (a), "ir" (b)
254  : "cc"
255  );
256  return a;
257 #elif defined(__GNUC__) && defined(__arm5__)
258  int16_t result;
259 
260  __asm__ __volatile__(
261  " ssub16 %0,%1,%2;\n"
262  : "=r" (result)
263  : "0" (a), "ir" (b)
264  );
265  return result;
266 #else
267  return saturate((int32_t) a - (int32_t) b);
268 #endif
269 }
270 /*- End of function --------------------------------------------------------*/
271 
272 static __inline__ int32_t saturated_sub32(int32_t a, int32_t b)
273 {
274 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))
275  __asm__ __volatile__(
276  " subl %2,%0;\n"
277  " jno 0f;\n"
278  " movl $0x80000000,%0;\n"
279  " sbbl $0,%0;\n"
280  "0:"
281  : "=r" (a)
282  : "0" (a), "ir" (b)
283  : "cc"
284  );
285  return a;
286 #elif defined(__GNUC__) && defined(__arm5__)
287  int32_t result;
288 
289  __asm__ __volatile__(
290  " qsub %0,%1,%2;\n"
291  : "=r" (result)
292  : "0" (a), "ir" (b)
293  );
294  return result;
295 #else
296  int32_t diff;
297 
298  diff = a - b;
299  if ((a ^ b) < 0)
300  {
301  if ((diff ^ a) & INT32_MIN)
302  diff = (a < 0L) ? INT32_MIN : INT32_MAX;
303  }
304  return diff;
305 #endif
306 }
307 /*- End of function --------------------------------------------------------*/
308 
309 static __inline__ int16_t saturated_mul16(int16_t a, int16_t b)
310 {
311  if (a == INT16_MIN && b == INT16_MIN)
312  return INT16_MAX;
313  /*endif*/
314  return (int16_t) (((int32_t) a*(int32_t) b) >> 15);
315 }
316 /*- End of function --------------------------------------------------------*/
317 
318 static __inline__ int32_t saturated_mul16_32(int16_t a, int16_t b)
319 {
320  return ((int32_t) a*(int32_t) b) << 1;
321 }
322 /*- End of function --------------------------------------------------------*/
323 
324 static __inline__ int16_t saturated_abs16(int16_t a)
325 {
326  return (a == INT16_MIN) ? INT16_MAX : (int16_t) abs(a);
327 }
328 /*- End of function --------------------------------------------------------*/
329 
330 #if defined(__cplusplus)
331 }
332 #endif
333 
334 #endif
335 /*- End of file ------------------------------------------------------------*/