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 * $Id: saturated.h,v 1.4 2009/02/05 12:21:36 steveu Exp $ 00026 */ 00027 00028 /*! \file */ 00029 00030 #if !defined(_SPANDSP_SATURATED_H_) 00031 #define _SPANDSP_SATURATED_H_ 00032 00033 /*! \page saturated_page Saturated arithmetic 00034 00035 \section saturated_page_sec_1 What does it do? 00036 00037 00038 \section saturated_page_sec_2 How does it work? 00039 00040 */ 00041 00042 #if defined(__cplusplus) 00043 extern "C" 00044 { 00045 #endif 00046 00047 static __inline__ int16_t saturate(int32_t amp) 00048 { 00049 int16_t amp16; 00050 00051 /* Hopefully this is optimised for the common case - not clipping */ 00052 amp16 = (int16_t) amp; 00053 if (amp == amp16) 00054 return amp16; 00055 if (amp > INT16_MAX) 00056 return INT16_MAX; 00057 return INT16_MIN; 00058 } 00059 /*- End of function --------------------------------------------------------*/ 00060 00061 /*! Saturate to 15 bits, rather than the usual 16 bits. This is often a useful function. */ 00062 static __inline__ int16_t saturate15(int32_t amp) 00063 { 00064 if (amp > 16383) 00065 return 16383; 00066 if (amp < -16384) 00067 return -16384; 00068 return (int16_t) amp; 00069 } 00070 /*- End of function --------------------------------------------------------*/ 00071 00072 static __inline__ int16_t fsaturatef(float famp) 00073 { 00074 if (famp > (float) INT16_MAX) 00075 return INT16_MAX; 00076 if (famp < (float) INT16_MIN) 00077 return INT16_MIN; 00078 return (int16_t) lrintf(famp); 00079 } 00080 /*- End of function --------------------------------------------------------*/ 00081 00082 static __inline__ int16_t fsaturate(double damp) 00083 { 00084 if (damp > (double) INT16_MAX) 00085 return INT16_MAX; 00086 if (damp < (double) INT16_MIN) 00087 return INT16_MIN; 00088 return (int16_t) lrint(damp); 00089 } 00090 /*- End of function --------------------------------------------------------*/ 00091 00092 /* Saturate to a 16 bit integer, using the fastest float to int conversion */ 00093 static __inline__ int16_t ffastsaturatef(float famp) 00094 { 00095 if (famp > (float) INT16_MAX) 00096 return INT16_MAX; 00097 if (famp < (float) INT16_MIN) 00098 return INT16_MIN; 00099 return (int16_t) lfastrintf(famp); 00100 } 00101 /*- End of function --------------------------------------------------------*/ 00102 00103 /* Saturate to a 16 bit integer, using the fastest double to int conversion */ 00104 static __inline__ int16_t ffastsaturate(double damp) 00105 { 00106 if (damp > (double) INT16_MAX) 00107 return INT16_MAX; 00108 if (damp < (double) INT16_MIN) 00109 return INT16_MIN; 00110 return (int16_t) lfastrint(damp); 00111 } 00112 /*- End of function --------------------------------------------------------*/ 00113 00114 /* Saturate to a 16 bit integer, using the closest float to int conversion */ 00115 static __inline__ float ffsaturatef(float famp) 00116 { 00117 if (famp > (float) INT16_MAX) 00118 return (float) INT16_MAX; 00119 if (famp < (float) INT16_MIN) 00120 return (float) INT16_MIN; 00121 return famp; 00122 } 00123 /*- End of function --------------------------------------------------------*/ 00124 00125 /* Saturate to a 16 bit integer, using the closest double to int conversion */ 00126 static __inline__ double ffsaturate(double famp) 00127 { 00128 if (famp > (double) INT16_MAX) 00129 return (double) INT16_MAX; 00130 if (famp < (double) INT16_MIN) 00131 return (double) INT16_MIN; 00132 return famp; 00133 } 00134 /*- End of function --------------------------------------------------------*/ 00135 00136 static __inline__ int16_t saturated_add16(int16_t a, int16_t b) 00137 { 00138 #if defined(__GNUC__) && defined(__i386__) 00139 __asm__ __volatile__( 00140 " addw %2,%0;\n" 00141 " jno 0f;\n" 00142 " movw $0x7fff,%0;\n" 00143 " adcw $0,%0;\n" 00144 "0:" 00145 : "=r" (a) 00146 : "0" (a), "ir" (b) 00147 : "cc" 00148 ); 00149 return a; 00150 #else 00151 return saturate((int32_t) a + (int32_t) b); 00152 #endif 00153 } 00154 /*- End of function --------------------------------------------------------*/ 00155 00156 static __inline__ int32_t saturated_add32(int32_t a, int32_t b) 00157 { 00158 #if defined(__GNUC__) && defined(__i386__) 00159 __asm__ __volatile__( 00160 " addl %2,%0;\n" 00161 " jno 0f;\n" 00162 " movl $0x7fffffff,%0;\n" 00163 " adcl $0,%0;\n" 00164 "0:" 00165 : "=r" (a) 00166 : "0" (a), "ir" (b) 00167 : "cc" 00168 ); 00169 return a; 00170 #else 00171 uint32_t A; 00172 00173 if (a < 0) 00174 { 00175 if (b >= 0) 00176 return a + b; 00177 /*endif*/ 00178 A = (uint32_t) -(a + 1) + (uint32_t) -(b + 1); 00179 return (A >= INT32_MAX) ? INT32_MIN : -(int32_t) A - 2; 00180 } 00181 /*endif*/ 00182 if (b <= 0) 00183 return a + b; 00184 /*endif*/ 00185 A = (uint32_t) a + (uint32_t) b; 00186 return (A > INT32_MAX) ? INT32_MAX : A; 00187 #endif 00188 } 00189 /*- End of function --------------------------------------------------------*/ 00190 00191 static __inline__ int16_t saturated_sub16(int16_t a, int16_t b) 00192 { 00193 return saturate((int32_t) a - (int32_t) b); 00194 } 00195 /*- End of function --------------------------------------------------------*/ 00196 00197 static __inline__ int16_t saturated_mul16(int16_t a, int16_t b) 00198 { 00199 if (a == INT16_MIN && b == INT16_MIN) 00200 return INT16_MAX; 00201 /*endif*/ 00202 return (int16_t) (((int32_t) a*(int32_t) b) >> 15); 00203 } 00204 /*- End of function --------------------------------------------------------*/ 00205 00206 static __inline__ int32_t saturated_mul_16_32(int16_t a, int16_t b) 00207 { 00208 return ((int32_t) a*(int32_t) b) << 1; 00209 } 00210 /*- End of function --------------------------------------------------------*/ 00211 00212 static __inline__ int16_t saturated_abs16(int16_t a) 00213 { 00214 return (a == INT16_MIN) ? INT16_MAX : (int16_t) abs(a); 00215 } 00216 /*- End of function --------------------------------------------------------*/ 00217 00218 #if defined(__cplusplus) 00219 } 00220 #endif 00221 00222 #endif 00223 /*- End of file ------------------------------------------------------------*/