andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved. |
| 3 | * |
| 4 | * Use of this source code is governed by a BSD-style license |
| 5 | * that can be found in the LICENSE file in the root of the source |
| 6 | * tree. An additional intellectual property rights grant can be found |
| 7 | * in the file PATENTS. All contributing project authors may |
| 8 | * be found in the AUTHORS file in the root of the source tree. |
| 9 | */ |
| 10 | |
| 11 | |
| 12 | /* |
| 13 | * This file contains the function WebRtcSpl_LevinsonDurbin(). |
| 14 | * The description header can be found in signal_processing_library.h |
| 15 | * |
| 16 | */ |
| 17 | |
pbos@webrtc.org | f24ac59 | 2013-05-27 09:49:58 +0000 | [diff] [blame] | 18 | #include "webrtc/common_audio/signal_processing/include/signal_processing_library.h" |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 19 | |
| 20 | #define SPL_LEVINSON_MAXORDER 20 |
| 21 | |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 22 | int16_t WebRtcSpl_LevinsonDurbin(int32_t *R, int16_t *A, int16_t *K, |
| 23 | int16_t order) |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 24 | { |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 25 | int16_t i, j; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 26 | // Auto-correlation coefficients in high precision |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 27 | int16_t R_hi[SPL_LEVINSON_MAXORDER + 1], R_low[SPL_LEVINSON_MAXORDER + 1]; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 28 | // LPC coefficients in high precision |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 29 | int16_t A_hi[SPL_LEVINSON_MAXORDER + 1], A_low[SPL_LEVINSON_MAXORDER + 1]; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 30 | // LPC coefficients for next iteration |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 31 | int16_t A_upd_hi[SPL_LEVINSON_MAXORDER + 1], A_upd_low[SPL_LEVINSON_MAXORDER + 1]; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 32 | // Reflection coefficient in high precision |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 33 | int16_t K_hi, K_low; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 34 | // Prediction gain Alpha in high precision and with scale factor |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 35 | int16_t Alpha_hi, Alpha_low, Alpha_exp; |
| 36 | int16_t tmp_hi, tmp_low; |
| 37 | int32_t temp1W32, temp2W32, temp3W32; |
| 38 | int16_t norm; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 39 | |
| 40 | // Normalize the autocorrelation R[0]...R[order+1] |
| 41 | |
| 42 | norm = WebRtcSpl_NormW32(R[0]); |
| 43 | |
| 44 | for (i = order; i >= 0; i--) |
| 45 | { |
| 46 | temp1W32 = WEBRTC_SPL_LSHIFT_W32(R[i], norm); |
| 47 | // Put R in hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 48 | R_hi[i] = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 49 | R_low[i] = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 50 | - WEBRTC_SPL_LSHIFT_W32((int32_t)R_hi[i], 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 51 | } |
| 52 | |
| 53 | // K = A[1] = -R[1] / R[0] |
| 54 | |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 55 | temp2W32 = WEBRTC_SPL_LSHIFT_W32((int32_t)R_hi[1],16) |
| 56 | + WEBRTC_SPL_LSHIFT_W32((int32_t)R_low[1],1); // R[1] in Q31 |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 57 | temp3W32 = WEBRTC_SPL_ABS_W32(temp2W32); // abs R[1] |
| 58 | temp1W32 = WebRtcSpl_DivW32HiLow(temp3W32, R_hi[0], R_low[0]); // abs(R[1])/R[0] in Q31 |
| 59 | // Put back the sign on R[1] |
| 60 | if (temp2W32 > 0) |
| 61 | { |
| 62 | temp1W32 = -temp1W32; |
| 63 | } |
| 64 | |
| 65 | // Put K in hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 66 | K_hi = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 67 | K_low = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 68 | - WEBRTC_SPL_LSHIFT_W32((int32_t)K_hi, 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 69 | |
| 70 | // Store first reflection coefficient |
| 71 | K[0] = K_hi; |
| 72 | |
| 73 | temp1W32 = WEBRTC_SPL_RSHIFT_W32(temp1W32, 4); // A[1] in Q27 |
| 74 | |
| 75 | // Put A[1] in hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 76 | A_hi[1] = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 77 | A_low[1] = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 78 | - WEBRTC_SPL_LSHIFT_W32((int32_t)A_hi[1], 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 79 | |
| 80 | // Alpha = R[0] * (1-K^2) |
| 81 | |
| 82 | temp1W32 = (((WEBRTC_SPL_MUL_16_16(K_hi, K_low) >> 14) + WEBRTC_SPL_MUL_16_16(K_hi, K_hi)) |
| 83 | << 1); // temp1W32 = k^2 in Q31 |
| 84 | |
| 85 | temp1W32 = WEBRTC_SPL_ABS_W32(temp1W32); // Guard against <0 |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 86 | temp1W32 = (int32_t)0x7fffffffL - temp1W32; // temp1W32 = (1 - K[0]*K[0]) in Q31 |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 87 | |
| 88 | // Store temp1W32 = 1 - K[0]*K[0] on hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 89 | tmp_hi = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 90 | tmp_low = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 91 | - WEBRTC_SPL_LSHIFT_W32((int32_t)tmp_hi, 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 92 | |
| 93 | // Calculate Alpha in Q31 |
| 94 | temp1W32 = ((WEBRTC_SPL_MUL_16_16(R_hi[0], tmp_hi) |
| 95 | + (WEBRTC_SPL_MUL_16_16(R_hi[0], tmp_low) >> 15) |
| 96 | + (WEBRTC_SPL_MUL_16_16(R_low[0], tmp_hi) >> 15)) << 1); |
| 97 | |
| 98 | // Normalize Alpha and put it in hi and low format |
| 99 | |
| 100 | Alpha_exp = WebRtcSpl_NormW32(temp1W32); |
| 101 | temp1W32 = WEBRTC_SPL_LSHIFT_W32(temp1W32, Alpha_exp); |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 102 | Alpha_hi = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 103 | Alpha_low = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 104 | - WEBRTC_SPL_LSHIFT_W32((int32_t)Alpha_hi, 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 105 | |
| 106 | // Perform the iterative calculations in the Levinson-Durbin algorithm |
| 107 | |
| 108 | for (i = 2; i <= order; i++) |
| 109 | { |
| 110 | /* ---- |
| 111 | temp1W32 = R[i] + > R[j]*A[i-j] |
| 112 | / |
| 113 | ---- |
| 114 | j=1..i-1 |
| 115 | */ |
| 116 | |
| 117 | temp1W32 = 0; |
| 118 | |
| 119 | for (j = 1; j < i; j++) |
| 120 | { |
| 121 | // temp1W32 is in Q31 |
| 122 | temp1W32 += ((WEBRTC_SPL_MUL_16_16(R_hi[j], A_hi[i-j]) << 1) |
| 123 | + (((WEBRTC_SPL_MUL_16_16(R_hi[j], A_low[i-j]) >> 15) |
| 124 | + (WEBRTC_SPL_MUL_16_16(R_low[j], A_hi[i-j]) >> 15)) << 1)); |
| 125 | } |
| 126 | |
| 127 | temp1W32 = WEBRTC_SPL_LSHIFT_W32(temp1W32, 4); |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 128 | temp1W32 += (WEBRTC_SPL_LSHIFT_W32((int32_t)R_hi[i], 16) |
| 129 | + WEBRTC_SPL_LSHIFT_W32((int32_t)R_low[i], 1)); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 130 | |
| 131 | // K = -temp1W32 / Alpha |
| 132 | temp2W32 = WEBRTC_SPL_ABS_W32(temp1W32); // abs(temp1W32) |
| 133 | temp3W32 = WebRtcSpl_DivW32HiLow(temp2W32, Alpha_hi, Alpha_low); // abs(temp1W32)/Alpha |
| 134 | |
| 135 | // Put the sign of temp1W32 back again |
| 136 | if (temp1W32 > 0) |
| 137 | { |
| 138 | temp3W32 = -temp3W32; |
| 139 | } |
| 140 | |
| 141 | // Use the Alpha shifts from earlier to de-normalize |
| 142 | norm = WebRtcSpl_NormW32(temp3W32); |
| 143 | if ((Alpha_exp <= norm) || (temp3W32 == 0)) |
| 144 | { |
| 145 | temp3W32 = WEBRTC_SPL_LSHIFT_W32(temp3W32, Alpha_exp); |
| 146 | } else |
| 147 | { |
| 148 | if (temp3W32 > 0) |
| 149 | { |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 150 | temp3W32 = (int32_t)0x7fffffffL; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 151 | } else |
| 152 | { |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 153 | temp3W32 = (int32_t)0x80000000L; |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 154 | } |
| 155 | } |
| 156 | |
| 157 | // Put K on hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 158 | K_hi = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp3W32, 16); |
| 159 | K_low = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp3W32 |
| 160 | - WEBRTC_SPL_LSHIFT_W32((int32_t)K_hi, 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 161 | |
| 162 | // Store Reflection coefficient in Q15 |
| 163 | K[i - 1] = K_hi; |
| 164 | |
| 165 | // Test for unstable filter. |
| 166 | // If unstable return 0 and let the user decide what to do in that case |
| 167 | |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 168 | if ((int32_t)WEBRTC_SPL_ABS_W16(K_hi) > (int32_t)32750) |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 169 | { |
| 170 | return 0; // Unstable filter |
| 171 | } |
| 172 | |
| 173 | /* |
| 174 | Compute updated LPC coefficient: Anew[i] |
| 175 | Anew[j]= A[j] + K*A[i-j] for j=1..i-1 |
| 176 | Anew[i]= K |
| 177 | */ |
| 178 | |
| 179 | for (j = 1; j < i; j++) |
| 180 | { |
| 181 | // temp1W32 = A[j] in Q27 |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 182 | temp1W32 = WEBRTC_SPL_LSHIFT_W32((int32_t)A_hi[j],16) |
| 183 | + WEBRTC_SPL_LSHIFT_W32((int32_t)A_low[j],1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 184 | |
| 185 | // temp1W32 += K*A[i-j] in Q27 |
| 186 | temp1W32 += ((WEBRTC_SPL_MUL_16_16(K_hi, A_hi[i-j]) |
| 187 | + (WEBRTC_SPL_MUL_16_16(K_hi, A_low[i-j]) >> 15) |
| 188 | + (WEBRTC_SPL_MUL_16_16(K_low, A_hi[i-j]) >> 15)) << 1); |
| 189 | |
| 190 | // Put Anew in hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 191 | A_upd_hi[j] = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 192 | A_upd_low[j] = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 193 | - WEBRTC_SPL_LSHIFT_W32((int32_t)A_upd_hi[j], 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 194 | } |
| 195 | |
| 196 | // temp3W32 = K in Q27 (Convert from Q31 to Q27) |
| 197 | temp3W32 = WEBRTC_SPL_RSHIFT_W32(temp3W32, 4); |
| 198 | |
| 199 | // Store Anew in hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 200 | A_upd_hi[i] = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp3W32, 16); |
| 201 | A_upd_low[i] = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp3W32 |
| 202 | - WEBRTC_SPL_LSHIFT_W32((int32_t)A_upd_hi[i], 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 203 | |
| 204 | // Alpha = Alpha * (1-K^2) |
| 205 | |
| 206 | temp1W32 = (((WEBRTC_SPL_MUL_16_16(K_hi, K_low) >> 14) |
| 207 | + WEBRTC_SPL_MUL_16_16(K_hi, K_hi)) << 1); // K*K in Q31 |
| 208 | |
| 209 | temp1W32 = WEBRTC_SPL_ABS_W32(temp1W32); // Guard against <0 |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 210 | temp1W32 = (int32_t)0x7fffffffL - temp1W32; // 1 - K*K in Q31 |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 211 | |
| 212 | // Convert 1- K^2 in hi and low format |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 213 | tmp_hi = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 214 | tmp_low = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 215 | - WEBRTC_SPL_LSHIFT_W32((int32_t)tmp_hi, 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 216 | |
| 217 | // Calculate Alpha = Alpha * (1-K^2) in Q31 |
| 218 | temp1W32 = ((WEBRTC_SPL_MUL_16_16(Alpha_hi, tmp_hi) |
| 219 | + (WEBRTC_SPL_MUL_16_16(Alpha_hi, tmp_low) >> 15) |
| 220 | + (WEBRTC_SPL_MUL_16_16(Alpha_low, tmp_hi) >> 15)) << 1); |
| 221 | |
| 222 | // Normalize Alpha and store it on hi and low format |
| 223 | |
| 224 | norm = WebRtcSpl_NormW32(temp1W32); |
| 225 | temp1W32 = WEBRTC_SPL_LSHIFT_W32(temp1W32, norm); |
| 226 | |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 227 | Alpha_hi = (int16_t)WEBRTC_SPL_RSHIFT_W32(temp1W32, 16); |
| 228 | Alpha_low = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32 |
| 229 | - WEBRTC_SPL_LSHIFT_W32((int32_t)Alpha_hi, 16)), 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 230 | |
| 231 | // Update the total normalization of Alpha |
| 232 | Alpha_exp = Alpha_exp + norm; |
| 233 | |
| 234 | // Update A[] |
| 235 | |
| 236 | for (j = 1; j <= i; j++) |
| 237 | { |
| 238 | A_hi[j] = A_upd_hi[j]; |
| 239 | A_low[j] = A_upd_low[j]; |
| 240 | } |
| 241 | } |
| 242 | |
| 243 | /* |
| 244 | Set A[0] to 1.0 and store the A[i] i=1...order in Q12 |
| 245 | (Convert from Q27 and use rounding) |
| 246 | */ |
| 247 | |
| 248 | A[0] = 4096; |
| 249 | |
| 250 | for (i = 1; i <= order; i++) |
| 251 | { |
| 252 | // temp1W32 in Q27 |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 253 | temp1W32 = WEBRTC_SPL_LSHIFT_W32((int32_t)A_hi[i], 16) |
| 254 | + WEBRTC_SPL_LSHIFT_W32((int32_t)A_low[i], 1); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 255 | // Round and store upper word |
pbos@webrtc.org | c49ec13 | 2013-04-09 16:40:28 +0000 | [diff] [blame] | 256 | A[i] = (int16_t)WEBRTC_SPL_RSHIFT_W32((temp1W32<<1)+(int32_t)32768, 16); |
andrew@webrtc.org | b015cbe | 2012-10-22 18:19:23 +0000 | [diff] [blame] | 257 | } |
| 258 | return 1; // Stable filters |
| 259 | } |