00001
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #include "dsp.h"
00047
00048 #if defined(FORCE_ALL_GENERICS) || \
00049 defined(FORCE_GENERIC_WIN16_KAISER) || \
00050 !defined(TARGET_SPECIFIC_WIN16_KAISER)
00051
00052
00053 static dsp16_t dsp16_op_kaiser_i0(S32 ax, int *power)
00054 {
00055 S32 ans, y, k, num_temp;
00056
00057 const S32 A0 = (S32) (1.0 * (1 << 12));
00058 const S32 A1 = (S32) ((3.5156229 * (1 << 12)) / (3.75*3.75));
00059 const S32 A2 = (S32) ((3.0899424 * (1 << 12)) / (3.75*3.75*3.75*3.75));
00060 const S32 A3 = (S32) ((1.2067492 * (1 << 12)) / (3.75*3.75*3.75*3.75*3.75*3.75));
00061
00062 const S32 B0 = DSP16_Q(0.39894228);
00063 const S32 B1 = DSP16_Q(0.01328592);
00064 const S32 B2 = DSP16_Q(0.00225319);
00065
00066 const S32 cst_inv_ln_2 = ((S32) DSP_Q(32-DSP16_QB, DSP16_QB, 1./0.69314718055994530941723212145818)) >> (DSP16_QB >> 1);
00067 const S32 cst_ln_2 = DSP16_Q(0.69314718055994530941723212145818);
00068
00069
00070 if (ax < (S32) DSP_Q(32-DSP16_QB, DSP16_QB, 3.75))
00071 {
00072
00073 *power = 3;
00074
00075
00076
00077 #if (DSP16_QB > 12)
00078 y = ax >> (DSP16_QB - 12);
00079 #else
00080 y = ax << (12 - DSP16_QB);
00081 #endif
00082 y = (y*y) >> 12;
00083
00084 ans = A3;
00085 ans = (((ans*y) >> 12) + A2);
00086 ans = (((ans*y) >> 12) + A1);
00087 ans = (((ans*y) >> 12) + A0);
00088
00089 #if (DSP16_QB > 15)
00090 ans = ans << (DSP16_QB - 15);
00091 #else
00092 ans = ans >> (15 - DSP16_QB);
00093 #endif
00094 }
00095 else
00096 {
00097
00098 ans = ((S32) DSP_Q(32-DSP16_QB, DSP16_QB, 3.75)) / ax;
00099
00100 y = B2;
00101 y = (((y*ans) >> DSP16_QB) + B1);
00102 y = (((y*ans) >> DSP16_QB) + B0);
00103
00104
00105 #if (DSP16_QB & 1)
00106 ans = (ax >> ((DSP16_QB >> 1) + 1));
00107 #else
00108 ans = (ax >> (DSP16_QB >> 1));
00109 #endif
00110
00111 k = ((cst_inv_ln_2*ans) >> DSP16_QB) + 1;
00112 num_temp = ax - k*cst_ln_2;
00113 if (num_temp > 0)
00114 num_temp = ax - (++k)*cst_ln_2;
00115
00116 ans = dsp16_op_exp(num_temp);
00117
00118 *power = k;
00119
00120
00121
00122
00123 k = 0;
00124 num_temp = ax >> DSP16_QB;
00125 while(num_temp)
00126 {
00127 num_temp >>= 2;
00128 k++;
00129 }
00130
00131 *power -= k;
00132
00133
00134 num_temp = ax >> (k << 1);
00135
00136
00137 num_temp = dsp16_op_sqrt(num_temp);
00138
00139
00140 ans = (y*ans) / num_temp;
00141 }
00142
00143 return ans;
00144 }
00145
00146
00147
00148
00149
00150
00151
00152 void dsp16_win_kaiser(dsp16_t *vect1, int size, int alpha)
00153 {
00154 int n, power_num, power_den;
00155 S32 pi_alpha, pi_alpha_div;
00156 S32 temp32;
00157 dsp16_t s, t, temp16, num, den;
00158
00159
00160 s = (DSP16_Q(1.) / (size - 1)) << 1;
00161
00162 pi_alpha = ((S32) DSP_Q(32-DSP16_QB, DSP16_QB, CST_PI))*alpha;
00163
00164 pi_alpha_div = pi_alpha >> (DSP16_QB >> 1);
00165
00166 den = dsp16_op_kaiser_i0(pi_alpha, &power_den);
00167
00168 t = 0;
00169
00170 for(n=0; n<(size >> 1); n++)
00171 {
00172
00173 temp16 = t - DSP16_Q(1);
00174
00175 temp16 = (((S32) temp16) * ((S32) temp16)) >> DSP16_QB;
00176
00177
00178 temp16 = dsp32_op_sqrt((DSP16_Q(1.) - temp16) << (DSP32_QB - DSP16_QB)) >> (DSP32_QB - DSP16_QB);
00179
00180
00181 #if (DSP16_QB & 1)
00182 temp32 = (pi_alpha_div * ((S32) temp16)) >> ((DSP16_QB >> 1) + 1);
00183 #else
00184 temp32 = (pi_alpha_div * ((S32) temp16)) >> (DSP16_QB >> 1);
00185 #endif
00186
00187 num = dsp16_op_kaiser_i0(temp32, &power_num);
00188
00189
00190 power_num = power_den - power_num;
00191
00192 temp32 = ((S32) num) << (DSP16_QB);
00193 temp32 = temp32 / (S32) den;
00194 temp32 >>= power_num;
00195
00196 vect1[n] = temp32;
00197 vect1[size-n-1] = temp32;
00198
00199
00200 t += s;
00201 }
00202
00203
00204 if (size & 1)
00205 vect1[size >> 1] = DSP16_Q(1.);
00206 }
00207
00208 #endif