1 /*
2  *  Copyright (c) 2012 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 #include "modules/audio_coding/codecs/isac/fix/source/pitch_estimator.h"
12 #include "common_audio/signal_processing/include/signal_processing_library.h"
13 #include "modules/audio_coding/codecs/isac/fix/source/settings.h"
14 #include "modules/audio_coding/codecs/isac/fix/source/structs.h"
15 #include "rtc_base/compile_assert_c.h"
16 
17 // Number of segments in a pitch subframe.
18 static const int kSegments = 5;
19 
20 // A division factor of 1/5 in Q15.
21 static const int16_t kDivFactor = 6553;
22 
23 // Interpolation coefficients; generated by design_pitch_filter.m.
24 // Coefficients are stored in Q14.
25 static const int16_t kIntrpCoef[PITCH_FRACS][PITCH_FRACORDER] = {
26   {-367, 1090, -2706,  9945, 10596, -3318,  1626, -781,  287},
27   {-325,  953, -2292,  7301, 12963, -3320,  1570, -743,  271},
28   {-240,  693, -1622,  4634, 14809, -2782,  1262, -587,  212},
29   {-125,  358,  -817,  2144, 15982, -1668,   721, -329,  118},
30   {   0,    0,    -1,     1, 16380,     1,    -1,    0,    0},
31   { 118, -329,   721, -1668, 15982,  2144,  -817,  358, -125},
32   { 212, -587,  1262, -2782, 14809,  4634, -1622,  693, -240},
33   { 271, -743,  1570, -3320, 12963,  7301, -2292,  953, -325}
34 };
35 
CalcLrIntQ(int16_t fixVal,int16_t qDomain)36 static __inline size_t CalcLrIntQ(int16_t fixVal,
37                                   int16_t qDomain) {
38   int32_t roundVal = 1 << (qDomain - 1);
39 
40   return (fixVal + roundVal) >> qDomain;
41 }
42 
WebRtcIsacfix_PitchFilter(int16_t * indatQQ,int16_t * outdatQQ,PitchFiltstr * pfp,int16_t * lagsQ7,int16_t * gainsQ12,int16_t type)43 void WebRtcIsacfix_PitchFilter(int16_t* indatQQ, // Q10 if type is 1 or 4,
44                                                        // Q0 if type is 2.
45                                int16_t* outdatQQ,
46                                PitchFiltstr* pfp,
47                                int16_t* lagsQ7,
48                                int16_t* gainsQ12,
49                                int16_t type) {
50   int    k, ind, cnt;
51   int16_t sign = 1;
52   int16_t inystateQQ[PITCH_DAMPORDER];
53   int16_t ubufQQ[PITCH_INTBUFFSIZE + QLOOKAHEAD];
54   const int16_t Gain = 21299;     // 1.3 in Q14
55   int16_t oldLagQ7;
56   int16_t oldGainQ12, lagdeltaQ7, curLagQ7, gaindeltaQ12, curGainQ12;
57   size_t frcQQ = 0;
58   int32_t indW32 = 0;
59   const int16_t* fracoeffQQ = NULL;
60 
61   // Assumptions in ARM assembly for WebRtcIsacfix_PitchFilterCoreARM().
62   RTC_COMPILE_ASSERT(PITCH_FRACORDER == 9);
63   RTC_COMPILE_ASSERT(PITCH_DAMPORDER == 5);
64 
65   // Set up buffer and states.
66   memcpy(ubufQQ, pfp->ubufQQ, sizeof(pfp->ubufQQ));
67   memcpy(inystateQQ, pfp->ystateQQ, sizeof(inystateQQ));
68 
69   // Get old lag and gain value from memory.
70   oldLagQ7 = pfp->oldlagQ7;
71   oldGainQ12 = pfp->oldgainQ12;
72 
73   if (type == 4) {
74     sign = -1;
75 
76     // Make output more periodic.
77     for (k = 0; k < PITCH_SUBFRAMES; k++) {
78       gainsQ12[k] = (int16_t)(gainsQ12[k] * Gain >> 14);
79     }
80   }
81 
82   // No interpolation if pitch lag step is big.
83   if (((lagsQ7[0] * 3 >> 1) < oldLagQ7) || (lagsQ7[0] > (oldLagQ7 * 3 >> 1))) {
84     oldLagQ7 = lagsQ7[0];
85     oldGainQ12 = gainsQ12[0];
86   }
87 
88   ind = 0;
89 
90   for (k = 0; k < PITCH_SUBFRAMES; k++) {
91     // Calculate interpolation steps.
92     lagdeltaQ7 = lagsQ7[k] - oldLagQ7;
93     lagdeltaQ7 = (int16_t)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(
94                   lagdeltaQ7, kDivFactor, 15);
95     curLagQ7 = oldLagQ7;
96     gaindeltaQ12 = gainsQ12[k] - oldGainQ12;
97     gaindeltaQ12 = (int16_t)(gaindeltaQ12 * kDivFactor >> 15);
98 
99     curGainQ12 = oldGainQ12;
100     oldLagQ7 = lagsQ7[k];
101     oldGainQ12 = gainsQ12[k];
102 
103     // Each frame has 4 60-sample pitch subframes, and each subframe has 5
104     // 12-sample segments. Each segment need to be processed with
105     // newly-updated parameters, so we break the pitch filtering into
106     // two for-loops (5 x 12) below. It's also why kDivFactor = 0.2 (in Q15).
107     for (cnt = 0; cnt < kSegments; cnt++) {
108       // Update parameters for each segment.
109       curGainQ12 += gaindeltaQ12;
110       curLagQ7 += lagdeltaQ7;
111       indW32 = CalcLrIntQ(curLagQ7, 7);
112       if (indW32 < PITCH_FRACORDER - 2) {
113         // WebRtcIsacfix_PitchFilterCore requires indW32 >= PITCH_FRACORDER -
114         // 2; otherwise, it will read from entries of ubufQQ that haven't been
115         // written yet. (This problem has only been seen in fuzzer tests, not
116         // in real life.) See Chromium bug 581901.
117         indW32 = PITCH_FRACORDER - 2;
118       }
119       frcQQ = ((indW32 << 7) + 64 - curLagQ7) >> 4;
120 
121       if (frcQQ >= PITCH_FRACS) {
122         frcQQ = 0;
123       }
124       fracoeffQQ = kIntrpCoef[frcQQ];
125 
126       // Pitch filtering.
127       WebRtcIsacfix_PitchFilterCore(PITCH_SUBFRAME_LEN / kSegments, curGainQ12,
128         indW32, sign, inystateQQ, ubufQQ, fracoeffQQ, indatQQ, outdatQQ, &ind);
129     }
130   }
131 
132   // Export buffer and states.
133   memcpy(pfp->ubufQQ, ubufQQ + PITCH_FRAME_LEN, sizeof(pfp->ubufQQ));
134   memcpy(pfp->ystateQQ, inystateQQ, sizeof(pfp->ystateQQ));
135 
136   pfp->oldlagQ7 = oldLagQ7;
137   pfp->oldgainQ12 = oldGainQ12;
138 
139   if (type == 2) {
140     // Filter look-ahead segment.
141     WebRtcIsacfix_PitchFilterCore(QLOOKAHEAD, curGainQ12, indW32, 1, inystateQQ,
142                 ubufQQ, fracoeffQQ, indatQQ, outdatQQ, &ind);
143   }
144 }
145 
146 
WebRtcIsacfix_PitchFilterGains(const int16_t * indatQ0,PitchFiltstr * pfp,int16_t * lagsQ7,int16_t * gainsQ12)147 void WebRtcIsacfix_PitchFilterGains(const int16_t* indatQ0,
148                                     PitchFiltstr* pfp,
149                                     int16_t* lagsQ7,
150                                     int16_t* gainsQ12) {
151   int  k, n, m;
152   size_t ind, pos, pos3QQ;
153 
154   int16_t ubufQQ[PITCH_INTBUFFSIZE];
155   int16_t oldLagQ7, lagdeltaQ7, curLagQ7;
156   const int16_t* fracoeffQQ = NULL;
157   int16_t scale;
158   int16_t cnt = 0, tmpW16;
159   size_t frcQQ, indW16 = 0;
160   int32_t tmpW32, tmp2W32, csum1QQ, esumxQQ;
161 
162   // Set up buffer and states.
163   memcpy(ubufQQ, pfp->ubufQQ, sizeof(pfp->ubufQQ));
164   oldLagQ7 = pfp->oldlagQ7;
165 
166   // No interpolation if pitch lag step is big.
167   if (((lagsQ7[0] * 3 >> 1) < oldLagQ7) || (lagsQ7[0] > (oldLagQ7 * 3 >> 1))) {
168     oldLagQ7 = lagsQ7[0];
169   }
170 
171   ind = 0;
172   pos = ind + PITCH_BUFFSIZE;
173   scale = 0;
174   for (k = 0; k < PITCH_SUBFRAMES; k++) {
175 
176     // Calculate interpolation steps.
177     lagdeltaQ7 = lagsQ7[k] - oldLagQ7;
178     lagdeltaQ7 = (int16_t)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(
179                    lagdeltaQ7, kDivFactor, 15);
180     curLagQ7 = oldLagQ7;
181     oldLagQ7 = lagsQ7[k];
182 
183     csum1QQ = 1;
184     esumxQQ = 1;
185 
186     // Same as function WebRtcIsacfix_PitchFilter(), we break the pitch
187     // filtering into two for-loops (5 x 12) below.
188     for (cnt = 0; cnt < kSegments; cnt++) {
189       // Update parameters for each segment.
190       curLagQ7 += lagdeltaQ7;
191       indW16 = CalcLrIntQ(curLagQ7, 7);
192       frcQQ = ((indW16 << 7) + 64 - curLagQ7) >> 4;
193 
194       if (frcQQ >= PITCH_FRACS) {
195         frcQQ = 0;
196       }
197       fracoeffQQ = kIntrpCoef[frcQQ];
198 
199       pos3QQ = pos - (indW16 + 4);
200 
201       for (n = 0; n < PITCH_SUBFRAME_LEN / kSegments; n++) {
202         // Filter to get fractional pitch.
203 
204         tmpW32 = 0;
205         for (m = 0; m < PITCH_FRACORDER; m++) {
206           tmpW32 += ubufQQ[pos3QQ + m] * fracoeffQQ[m];
207         }
208 
209         // Subtract from input and update buffer.
210         ubufQQ[pos] = indatQ0[ind];
211 
212         tmp2W32 = WEBRTC_SPL_MUL_16_32_RSFT14(indatQ0[ind], tmpW32);
213         tmpW32 += 8192;
214         tmpW16 = tmpW32 >> 14;
215         tmpW32 = tmpW16 * tmpW16;
216 
217         if ((tmp2W32 > 1073700000) || (csum1QQ > 1073700000) ||
218             (tmpW32 > 1073700000) || (esumxQQ > 1073700000)) {  // 2^30
219           scale++;
220           csum1QQ >>= 1;
221           esumxQQ >>= 1;
222         }
223         csum1QQ += tmp2W32 >> scale;
224         esumxQQ += tmpW32 >> scale;
225 
226         ind++;
227         pos++;
228         pos3QQ++;
229       }
230     }
231 
232     if (csum1QQ < esumxQQ) {
233       tmp2W32 = WebRtcSpl_DivResultInQ31(csum1QQ, esumxQQ);
234 
235       // Gain should be half the correlation.
236       tmpW32 = tmp2W32 >> 20;
237     } else {
238       tmpW32 = 4096;
239     }
240     gainsQ12[k] = (int16_t)WEBRTC_SPL_SAT(PITCH_MAX_GAIN_Q12, tmpW32, 0);
241   }
242 
243   // Export buffer and states.
244   memcpy(pfp->ubufQQ, ubufQQ + PITCH_FRAME_LEN, sizeof(pfp->ubufQQ));
245   pfp->oldlagQ7 = lagsQ7[PITCH_SUBFRAMES - 1];
246   pfp->oldgainQ12 = gainsQ12[PITCH_SUBFRAMES - 1];
247 
248 }
249