spandsp
0.0.6
|
00001 /* 00002 * SpanDSP - a series of DSP components for telephony 00003 * 00004 * saturated.h - General saturated arithmetic routines. 00005 * 00006 * Written by Steve Underwood <steveu@coppice.org> 00007 * 00008 * Copyright (C) 2001, 2008 Steve Underwood 00009 * 00010 * All rights reserved. 00011 * 00012 * This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU Lesser General Public License version 2.1, 00014 * as published by the Free Software Foundation. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Lesser General Public License for more details. 00020 * 00021 * You should have received a copy of the GNU Lesser General Public 00022 * License along with this program; if not, write to the Free Software 00023 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00024 */ 00025 00026 /*! \file */ 00027 00028 #if !defined(_SPANDSP_SATURATED_H_) 00029 #define _SPANDSP_SATURATED_H_ 00030 00031 /*! \page saturated_page Saturated arithmetic 00032 00033 \section saturated_page_sec_1 What does it do? 00034 00035 00036 \section saturated_page_sec_2 How does it work? 00037 00038 */ 00039 00040 #if defined(__cplusplus) 00041 extern "C" 00042 { 00043 #endif 00044 00045 /* This is the same as saturate16(), but is here for historic reasons */ 00046 static __inline__ int16_t saturate(int32_t amp) 00047 { 00048 int16_t amp16; 00049 00050 /* Hopefully this is optimised for the common case - not clipping */ 00051 amp16 = (int16_t) amp; 00052 if (amp == amp16) 00053 return amp16; 00054 if (amp > INT16_MAX) 00055 return INT16_MAX; 00056 return INT16_MIN; 00057 } 00058 /*- End of function --------------------------------------------------------*/ 00059 00060 static __inline__ int16_t saturate16(int32_t amp) 00061 { 00062 int16_t amp16; 00063 00064 /* Hopefully this is optimised for the common case - not clipping */ 00065 amp16 = (int16_t) amp; 00066 if (amp == amp16) 00067 return amp16; 00068 if (amp > INT16_MAX) 00069 return INT16_MAX; 00070 return INT16_MIN; 00071 } 00072 /*- End of function --------------------------------------------------------*/ 00073 00074 /*! Saturate to 15 bits, rather than the usual 16 bits. This is often a useful function. */ 00075 static __inline__ int16_t saturate15(int32_t amp) 00076 { 00077 if (amp > 16383) 00078 return 16383; 00079 if (amp < -16384) 00080 return -16384; 00081 return (int16_t) amp; 00082 } 00083 /*- End of function --------------------------------------------------------*/ 00084 00085 static __inline__ uint16_t saturateu16(int32_t amp) 00086 { 00087 uint16_t amp16; 00088 00089 /* Hopefully this is optimised for the common case - not clipping */ 00090 amp16 = (uint16_t) amp; 00091 if (amp == amp16) 00092 return amp16; 00093 if (amp > UINT16_MAX) 00094 return UINT16_MAX; 00095 return 0; 00096 } 00097 /*- End of function --------------------------------------------------------*/ 00098 00099 static __inline__ uint8_t saturateu8(int32_t amp) 00100 { 00101 uint8_t amp8; 00102 00103 /* Hopefully this is optimised for the common case - not clipping */ 00104 amp8 = (uint8_t) amp; 00105 if (amp == amp8) 00106 return amp8; 00107 if (amp > UINT8_MAX) 00108 return UINT8_MAX; 00109 return 0; 00110 } 00111 /*- End of function --------------------------------------------------------*/ 00112 00113 static __inline__ int16_t fsaturatef(float famp) 00114 { 00115 if (famp > (float) INT16_MAX) 00116 return INT16_MAX; 00117 if (famp < (float) INT16_MIN) 00118 return INT16_MIN; 00119 return (int16_t) lrintf(famp); 00120 } 00121 /*- End of function --------------------------------------------------------*/ 00122 00123 static __inline__ int16_t fsaturate(double damp) 00124 { 00125 if (damp > (double) INT16_MAX) 00126 return INT16_MAX; 00127 if (damp < (double) INT16_MIN) 00128 return INT16_MIN; 00129 return (int16_t) lrint(damp); 00130 } 00131 /*- End of function --------------------------------------------------------*/ 00132 00133 /* Saturate to a 16 bit integer, using the fastest float to int conversion */ 00134 static __inline__ int16_t ffastsaturatef(float famp) 00135 { 00136 if (famp > (float) INT16_MAX) 00137 return INT16_MAX; 00138 if (famp < (float) INT16_MIN) 00139 return INT16_MIN; 00140 return (int16_t) lfastrintf(famp); 00141 } 00142 /*- End of function --------------------------------------------------------*/ 00143 00144 /* Saturate to a 16 bit integer, using the fastest double to int conversion */ 00145 static __inline__ int16_t ffastsaturate(double damp) 00146 { 00147 if (damp > (double) INT16_MAX) 00148 return INT16_MAX; 00149 if (damp < (double) INT16_MIN) 00150 return INT16_MIN; 00151 return (int16_t) lfastrint(damp); 00152 } 00153 /*- End of function --------------------------------------------------------*/ 00154 00155 /* Saturate to a 16 bit integer, using the closest float to int conversion */ 00156 static __inline__ float ffsaturatef(float famp) 00157 { 00158 if (famp > (float) INT16_MAX) 00159 return (float) INT16_MAX; 00160 if (famp < (float) INT16_MIN) 00161 return (float) INT16_MIN; 00162 return famp; 00163 } 00164 /*- End of function --------------------------------------------------------*/ 00165 00166 /* Saturate to a 16 bit integer, using the closest double to int conversion */ 00167 static __inline__ double ffsaturate(double famp) 00168 { 00169 if (famp > (double) INT16_MAX) 00170 return (double) INT16_MAX; 00171 if (famp < (double) INT16_MIN) 00172 return (double) INT16_MIN; 00173 return famp; 00174 } 00175 /*- End of function --------------------------------------------------------*/ 00176 00177 static __inline__ int16_t saturated_add16(int16_t a, int16_t b) 00178 { 00179 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__)) 00180 __asm__ __volatile__( 00181 " addw %2,%0;\n" 00182 " jno 0f;\n" 00183 " movw $0x7fff,%0;\n" 00184 " adcw $0,%0;\n" 00185 "0:" 00186 : "=r" (a) 00187 : "0" (a), "ir" (b) 00188 : "cc" 00189 ); 00190 return a; 00191 #elif defined(__GNUC__) && defined(__arm5__) 00192 int16_t result; 00193 00194 __asm__ __volatile__( 00195 " sadd16 %0,%1,%2;\n" 00196 : "=r" (result) 00197 : "0" (a), "ir" (b) 00198 ); 00199 return result; 00200 #else 00201 return saturate((int32_t) a + (int32_t) b); 00202 #endif 00203 } 00204 /*- End of function --------------------------------------------------------*/ 00205 00206 static __inline__ int32_t saturated_add32(int32_t a, int32_t b) 00207 { 00208 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__)) 00209 __asm__ __volatile__( 00210 " addl %2,%0;\n" 00211 " jno 0f;\n" 00212 " movl $0x7fffffff,%0;\n" 00213 " adcl $0,%0;\n" 00214 "0:" 00215 : "=r" (a) 00216 : "0" (a), "ir" (b) 00217 : "cc" 00218 ); 00219 return a; 00220 #elif defined(__GNUC__) && defined(__arm5__) 00221 int32_t result; 00222 00223 __asm__ __volatile__( 00224 " qadd %0,%1,%2;\n" 00225 : "=r" (result) 00226 : "0" (a), "ir" (b) 00227 ); 00228 return result; 00229 #else 00230 int32_t sum; 00231 00232 sum = a + b; 00233 if ((a ^ b) >= 0) 00234 { 00235 if ((sum ^ a) < 0) 00236 sum = (a < 0) ? INT32_MIN : INT32_MAX; 00237 } 00238 return sum; 00239 #endif 00240 } 00241 /*- End of function --------------------------------------------------------*/ 00242 00243 static __inline__ int16_t saturated_sub16(int16_t a, int16_t b) 00244 { 00245 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__)) 00246 __asm__ __volatile__( 00247 " subw %2,%0;\n" 00248 " jno 0f;\n" 00249 " movw $0x8000,%0;\n" 00250 " sbbw $0,%0;\n" 00251 "0:" 00252 : "=r" (a) 00253 : "0" (a), "ir" (b) 00254 : "cc" 00255 ); 00256 return a; 00257 #elif defined(__GNUC__) && defined(__arm5__) 00258 int16_t result; 00259 00260 __asm__ __volatile__( 00261 " ssub16 %0,%1,%2;\n" 00262 : "=r" (result) 00263 : "0" (a), "ir" (b) 00264 ); 00265 return result; 00266 #else 00267 return saturate((int32_t) a - (int32_t) b); 00268 #endif 00269 } 00270 /*- End of function --------------------------------------------------------*/ 00271 00272 static __inline__ int32_t saturated_sub32(int32_t a, int32_t b) 00273 { 00274 #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__)) 00275 __asm__ __volatile__( 00276 " subl %2,%0;\n" 00277 " jno 0f;\n" 00278 " movl $0x80000000,%0;\n" 00279 " sbbl $0,%0;\n" 00280 "0:" 00281 : "=r" (a) 00282 : "0" (a), "ir" (b) 00283 : "cc" 00284 ); 00285 return a; 00286 #elif defined(__GNUC__) && defined(__arm5__) 00287 int32_t result; 00288 00289 __asm__ __volatile__( 00290 " qsub %0,%1,%2;\n" 00291 : "=r" (result) 00292 : "0" (a), "ir" (b) 00293 ); 00294 return result; 00295 #else 00296 int32_t diff; 00297 00298 diff = a - b; 00299 if ((a ^ b) < 0) 00300 { 00301 if ((diff ^ a) & INT32_MIN) 00302 diff = (a < 0L) ? INT32_MIN : INT32_MAX; 00303 } 00304 return diff; 00305 #endif 00306 } 00307 /*- End of function --------------------------------------------------------*/ 00308 00309 static __inline__ int16_t saturated_mul16(int16_t a, int16_t b) 00310 { 00311 if (a == INT16_MIN && b == INT16_MIN) 00312 return INT16_MAX; 00313 /*endif*/ 00314 return (int16_t) (((int32_t) a*(int32_t) b) >> 15); 00315 } 00316 /*- End of function --------------------------------------------------------*/ 00317 00318 static __inline__ int32_t saturated_mul16_32(int16_t a, int16_t b) 00319 { 00320 return ((int32_t) a*(int32_t) b) << 1; 00321 } 00322 /*- End of function --------------------------------------------------------*/ 00323 00324 static __inline__ int16_t saturated_abs16(int16_t a) 00325 { 00326 return (a == INT16_MIN) ? INT16_MAX : (int16_t) abs(a); 00327 } 00328 /*- End of function --------------------------------------------------------*/ 00329 00330 #if defined(__cplusplus) 00331 } 00332 #endif 00333 00334 #endif 00335 /*- End of file ------------------------------------------------------------*/