Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 1 | /* |
| 2 | ** Copyright 2003-2010, VisualOn, Inc. |
| 3 | ** |
| 4 | ** Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | ** you may not use this file except in compliance with the License. |
| 6 | ** You may obtain a copy of the License at |
| 7 | ** |
| 8 | ** http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | ** |
| 10 | ** Unless required by applicable law or agreed to in writing, software |
| 11 | ** distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | ** See the License for the specific language governing permissions and |
| 14 | ** limitations under the License. |
| 15 | */ |
| 16 | |
| 17 | /*********************************************************************** |
| 18 | * File: pitch_f4.c * |
| 19 | * * |
| 20 | * Description: Find the closed loop pitch period with * |
| 21 | * 1/4 subsample resolution. * |
| 22 | * * |
| 23 | ************************************************************************/ |
| 24 | |
| 25 | #include "typedef.h" |
| 26 | #include "basic_op.h" |
| 27 | #include "math_op.h" |
| 28 | #include "acelp.h" |
| 29 | #include "cnst.h" |
| 30 | |
| 31 | #define UP_SAMP 4 |
| 32 | #define L_INTERPOL1 4 |
| 33 | |
| 34 | /* Local functions */ |
| 35 | |
| 36 | #ifdef ASM_OPT |
| 37 | void Norm_corr_asm( |
| 38 | Word16 exc[], /* (i) : excitation buffer */ |
| 39 | Word16 xn[], /* (i) : target vector */ |
| 40 | Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ |
| 41 | Word16 L_subfr, |
| 42 | Word16 t_min, /* (i) : minimum value of pitch lag. */ |
| 43 | Word16 t_max, /* (i) : maximum value of pitch lag. */ |
| 44 | Word16 corr_norm[] /* (o) Q15 : normalized correlation */ |
| 45 | ); |
| 46 | #else |
| 47 | static void Norm_Corr( |
| 48 | Word16 exc[], /* (i) : excitation buffer */ |
| 49 | Word16 xn[], /* (i) : target vector */ |
| 50 | Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ |
| 51 | Word16 L_subfr, |
| 52 | Word16 t_min, /* (i) : minimum value of pitch lag. */ |
| 53 | Word16 t_max, /* (i) : maximum value of pitch lag. */ |
| 54 | Word16 corr_norm[] /* (o) Q15 : normalized correlation */ |
| 55 | ); |
| 56 | #endif |
| 57 | |
| 58 | static Word16 Interpol_4( /* (o) : interpolated value */ |
| 59 | Word16 * x, /* (i) : input vector */ |
| 60 | Word32 frac /* (i) : fraction (-4..+3) */ |
| 61 | ); |
| 62 | |
| 63 | |
| 64 | Word16 Pitch_fr4( /* (o) : pitch period. */ |
| 65 | Word16 exc[], /* (i) : excitation buffer */ |
| 66 | Word16 xn[], /* (i) : target vector */ |
| 67 | Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ |
| 68 | Word16 t0_min, /* (i) : minimum value in the searched range. */ |
| 69 | Word16 t0_max, /* (i) : maximum value in the searched range. */ |
| 70 | Word16 * pit_frac, /* (o) : chosen fraction (0, 1, 2 or 3). */ |
| 71 | Word16 i_subfr, /* (i) : indicator for first subframe. */ |
| 72 | Word16 t0_fr2, /* (i) : minimum value for resolution 1/2 */ |
| 73 | Word16 t0_fr1, /* (i) : minimum value for resolution 1 */ |
| 74 | Word16 L_subfr /* (i) : Length of subframe */ |
| 75 | ) |
| 76 | { |
| 77 | Word32 fraction, i; |
| 78 | Word16 t_min, t_max; |
| 79 | Word16 max, t0, step, temp; |
| 80 | Word16 *corr; |
| 81 | Word16 corr_v[40]; /* Total length = t0_max-t0_min+1+2*L_inter */ |
| 82 | |
| 83 | /* Find interval to compute normalized correlation */ |
| 84 | |
| 85 | t_min = t0_min - L_INTERPOL1; |
| 86 | t_max = t0_max + L_INTERPOL1; |
| 87 | corr = &corr_v[-t_min]; |
| 88 | /* Compute normalized correlation between target and filtered excitation */ |
| 89 | #ifdef ASM_OPT /* asm optimization branch */ |
| 90 | Norm_corr_asm(exc, xn, h, L_subfr, t_min, t_max, corr); |
| 91 | #else |
| 92 | Norm_Corr(exc, xn, h, L_subfr, t_min, t_max, corr); |
Mans Rullgard | 62a62ef | 2011-04-05 17:40:06 +0300 | [diff] [blame] | 93 | #endif |
Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 94 | |
| 95 | /* Find integer pitch */ |
| 96 | |
| 97 | max = corr[t0_min]; |
| 98 | t0 = t0_min; |
| 99 | for (i = t0_min + 1; i <= t0_max; i++) |
| 100 | { |
| 101 | if (corr[i] >= max) |
| 102 | { |
Mans Rullgard | 62a62ef | 2011-04-05 17:40:06 +0300 | [diff] [blame] | 103 | max = corr[i]; |
| 104 | t0 = i; |
Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 105 | } |
| 106 | } |
| 107 | /* If first subframe and t0 >= t0_fr1, do not search fractionnal pitch */ |
| 108 | if ((i_subfr == 0) && (t0 >= t0_fr1)) |
| 109 | { |
| 110 | *pit_frac = 0; |
| 111 | return (t0); |
| 112 | } |
| 113 | /*------------------------------------------------------------------* |
| 114 | * Search fractionnal pitch with 1/4 subsample resolution. * |
| 115 | * Test the fractions around t0 and choose the one which maximizes * |
| 116 | * the interpolated normalized correlation. * |
| 117 | *------------------------------------------------------------------*/ |
| 118 | |
| 119 | step = 1; /* 1/4 subsample resolution */ |
| 120 | fraction = -3; |
| 121 | if ((t0_fr2 == PIT_MIN)||((i_subfr == 0) && (t0 >= t0_fr2))) |
| 122 | { |
| 123 | step = 2; /* 1/2 subsample resolution */ |
| 124 | fraction = -2; |
| 125 | } |
| 126 | if(t0 == t0_min) |
| 127 | { |
| 128 | fraction = 0; |
| 129 | } |
| 130 | max = Interpol_4(&corr[t0], fraction); |
| 131 | |
| 132 | for (i = fraction + step; i <= 3; i += step) |
| 133 | { |
| 134 | temp = Interpol_4(&corr[t0], i); |
| 135 | if(temp > max) |
| 136 | { |
| 137 | max = temp; |
| 138 | fraction = i; |
| 139 | } |
| 140 | } |
| 141 | /* limit the fraction value in the interval [0,1,2,3] */ |
| 142 | if (fraction < 0) |
| 143 | { |
| 144 | fraction += UP_SAMP; |
| 145 | t0 -= 1; |
| 146 | } |
| 147 | *pit_frac = fraction; |
| 148 | return (t0); |
| 149 | } |
| 150 | |
| 151 | |
| 152 | /*********************************************************************************** |
| 153 | * Function: Norm_Corr() * |
| 154 | * * |
| 155 | * Description: Find the normalized correlation between the target vector and the * |
| 156 | * filtered past excitation. * |
| 157 | * (correlation between target and filtered excitation divided by the * |
| 158 | * square root of energy of target and filtered excitation). * |
| 159 | ************************************************************************************/ |
| 160 | #ifndef ASM_OPT |
| 161 | static void Norm_Corr( |
| 162 | Word16 exc[], /* (i) : excitation buffer */ |
| 163 | Word16 xn[], /* (i) : target vector */ |
| 164 | Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ |
| 165 | Word16 L_subfr, |
| 166 | Word16 t_min, /* (i) : minimum value of pitch lag. */ |
| 167 | Word16 t_max, /* (i) : maximum value of pitch lag. */ |
| 168 | Word16 corr_norm[]) /* (o) Q15 : normalized correlation */ |
| 169 | { |
| 170 | Word32 i, k, t; |
| 171 | Word32 corr, exp_corr, norm, exp, scale; |
| 172 | Word16 exp_norm, excf[L_SUBFR], tmp; |
| 173 | Word32 L_tmp, L_tmp1, L_tmp2; |
| 174 | |
| 175 | /* compute the filtered excitation for the first delay t_min */ |
| 176 | k = -t_min; |
| 177 | |
| 178 | #ifdef ASM_OPT /* asm optimization branch */ |
| 179 | Convolve_asm(&exc[k], h, excf, 64); |
| 180 | #else |
| 181 | Convolve(&exc[k], h, excf, 64); |
| 182 | #endif |
| 183 | |
| 184 | /* Compute rounded down 1/sqrt(energy of xn[]) */ |
Mans Rullgard | 62a62ef | 2011-04-05 17:40:06 +0300 | [diff] [blame] | 185 | L_tmp = 0; |
Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 186 | for (i = 0; i < 64; i+=4) |
| 187 | { |
| 188 | L_tmp += (xn[i] * xn[i]); |
| 189 | L_tmp += (xn[i+1] * xn[i+1]); |
| 190 | L_tmp += (xn[i+2] * xn[i+2]); |
| 191 | L_tmp += (xn[i+3] * xn[i+3]); |
| 192 | } |
| 193 | |
| 194 | L_tmp = (L_tmp << 1) + 1; |
| 195 | exp = norm_l(L_tmp); |
| 196 | exp = (32 - exp); |
| 197 | //exp = exp + 2; /* energy of xn[] x 2 + rounded up */ |
| 198 | scale = -(exp >> 1); /* (1<<scale) < 1/sqrt(energy rounded) */ |
| 199 | |
| 200 | /* loop for every possible period */ |
| 201 | |
| 202 | for (t = t_min; t <= t_max; t++) |
| 203 | { |
| 204 | /* Compute correlation between xn[] and excf[] */ |
Mans Rullgard | 62a62ef | 2011-04-05 17:40:06 +0300 | [diff] [blame] | 205 | L_tmp = 0; |
Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 206 | L_tmp1 = 0; |
| 207 | for (i = 0; i < 64; i+=4) |
| 208 | { |
| 209 | L_tmp += (xn[i] * excf[i]); |
| 210 | L_tmp1 += (excf[i] * excf[i]); |
| 211 | L_tmp += (xn[i+1] * excf[i+1]); |
| 212 | L_tmp1 += (excf[i+1] * excf[i+1]); |
| 213 | L_tmp += (xn[i+2] * excf[i+2]); |
| 214 | L_tmp1 += (excf[i+2] * excf[i+2]); |
| 215 | L_tmp += (xn[i+3] * excf[i+3]); |
| 216 | L_tmp1 += (excf[i+3] * excf[i+3]); |
| 217 | } |
| 218 | |
| 219 | L_tmp = (L_tmp << 1) + 1; |
| 220 | L_tmp1 = (L_tmp1 << 1) + 1; |
| 221 | |
| 222 | exp = norm_l(L_tmp); |
| 223 | L_tmp = (L_tmp << exp); |
| 224 | exp_corr = (30 - exp); |
| 225 | corr = extract_h(L_tmp); |
| 226 | |
| 227 | exp = norm_l(L_tmp1); |
| 228 | L_tmp = (L_tmp1 << exp); |
| 229 | exp_norm = (30 - exp); |
| 230 | |
| 231 | Isqrt_n(&L_tmp, &exp_norm); |
| 232 | norm = extract_h(L_tmp); |
| 233 | |
| 234 | /* Normalize correlation = correlation * (1/sqrt(energy)) */ |
| 235 | |
| 236 | L_tmp = vo_L_mult(corr, norm); |
| 237 | |
| 238 | L_tmp2 = exp_corr + exp_norm + scale; |
| 239 | if(L_tmp2 < 0) |
| 240 | { |
| 241 | L_tmp2 = -L_tmp2; |
| 242 | L_tmp = L_tmp >> L_tmp2; |
| 243 | } |
| 244 | else |
| 245 | { |
| 246 | L_tmp = L_tmp << L_tmp2; |
| 247 | } |
| 248 | |
Mans Rullgard | 62a62ef | 2011-04-05 17:40:06 +0300 | [diff] [blame] | 249 | corr_norm[t] = vo_round(L_tmp); |
Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 250 | /* modify the filtered excitation excf[] for the next iteration */ |
| 251 | |
| 252 | if(t != t_max) |
| 253 | { |
| 254 | k = -(t + 1); |
| 255 | tmp = exc[k]; |
| 256 | for (i = 63; i > 0; i--) |
| 257 | { |
| 258 | excf[i] = add1(vo_mult(tmp, h[i]), excf[i - 1]); |
| 259 | } |
| 260 | excf[0] = vo_mult(tmp, h[0]); |
| 261 | } |
| 262 | } |
| 263 | return; |
| 264 | } |
| 265 | |
| 266 | #endif |
| 267 | /************************************************************************************ |
| 268 | * Function: Interpol_4() * |
| 269 | * * |
| 270 | * Description: For interpolating the normalized correlation with 1/4 resolution. * |
| 271 | **************************************************************************************/ |
| 272 | |
| 273 | /* 1/4 resolution interpolation filter (-3 dB at 0.791*fs/2) in Q14 */ |
| 274 | static Word16 inter4_1[4][8] = |
| 275 | { |
| 276 | {-12, 420, -1732, 5429, 13418, -1242, 73, 32}, |
| 277 | {-26, 455, -2142, 9910, 9910, -2142, 455, -26}, |
| 278 | {32, 73, -1242, 13418, 5429, -1732, 420, -12}, |
| 279 | {206, -766, 1376, 14746, 1376, -766, 206, 0} |
| 280 | }; |
| 281 | |
| 282 | /*** Coefficients in floating point |
| 283 | static float inter4_1[UP_SAMP*L_INTERPOL1+1] = { |
| 284 | 0.900000, |
| 285 | 0.818959, 0.604850, 0.331379, 0.083958, |
| 286 | -0.075795, -0.130717, -0.105685, -0.046774, |
| 287 | 0.004467, 0.027789, 0.025642, 0.012571, |
| 288 | 0.001927, -0.001571, -0.000753, 0.000000}; |
| 289 | ***/ |
| 290 | |
| 291 | static Word16 Interpol_4( /* (o) : interpolated value */ |
| 292 | Word16 * x, /* (i) : input vector */ |
| 293 | Word32 frac /* (i) : fraction (-4..+3) */ |
| 294 | ) |
| 295 | { |
| 296 | Word16 sum; |
| 297 | Word32 k, L_sum; |
| 298 | Word16 *ptr; |
| 299 | |
| 300 | if (frac < 0) |
| 301 | { |
| 302 | frac += UP_SAMP; |
| 303 | x--; |
| 304 | } |
| 305 | x = x - L_INTERPOL1 + 1; |
| 306 | k = UP_SAMP - 1 - frac; |
| 307 | ptr = &(inter4_1[k][0]); |
| 308 | |
| 309 | L_sum = vo_mult32(x[0], (*ptr++)); |
| 310 | L_sum += vo_mult32(x[1], (*ptr++)); |
| 311 | L_sum += vo_mult32(x[2], (*ptr++)); |
| 312 | L_sum += vo_mult32(x[3], (*ptr++)); |
Mans Rullgard | 62a62ef | 2011-04-05 17:40:06 +0300 | [diff] [blame] | 313 | L_sum += vo_mult32(x[4], (*ptr++)); |
Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 314 | L_sum += vo_mult32(x[5], (*ptr++)); |
| 315 | L_sum += vo_mult32(x[6], (*ptr++)); |
Mans Rullgard | 62a62ef | 2011-04-05 17:40:06 +0300 | [diff] [blame] | 316 | L_sum += vo_mult32(x[7], (*ptr++)); |
Mans Rullgard | 219e262 | 2011-04-22 09:22:33 +0300 | [diff] [blame] | 317 | |
| 318 | sum = extract_h(L_add(L_shl2(L_sum, 2), 0x8000)); |
| 319 | return (sum); |
| 320 | } |
| 321 | |
| 322 | |
| 323 | |
| 324 | |