00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #if !defined(_SPANDSP_SATURATED_H_)
00029 #define _SPANDSP_SATURATED_H_
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #if defined(__cplusplus)
00041 extern "C"
00042 {
00043 #endif
00044
00045
00046 static __inline__ int16_t saturate(int32_t amp)
00047 {
00048 int16_t amp16;
00049
00050
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
00059
00060 static __inline__ int16_t saturate16(int32_t amp)
00061 {
00062 int16_t amp16;
00063
00064
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
00073
00074
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
00084
00085 static __inline__ uint16_t saturateu16(int32_t amp)
00086 {
00087 uint16_t amp16;
00088
00089
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
00098
00099 static __inline__ uint8_t saturateu8(int32_t amp)
00100 {
00101 uint8_t amp8;
00102
00103
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
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
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
00132
00133
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
00143
00144
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
00154
00155
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
00165
00166
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
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
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
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
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
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
00314 return (int16_t) (((int32_t) a*(int32_t) b) >> 15);
00315 }
00316
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
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
00329
00330 #if defined(__cplusplus)
00331 }
00332 #endif
00333
00334 #endif
00335