Adds back gain sum checkgain_sum

9 files changed, 72 insertions, 8 deletions

diff --git a/silk/VQ_WMat_EC.c b/silk/VQ_WMat_EC.c
index 1434971a..45375baf 100644
--- a/silk/VQ_WMat_EC.c
+++ b/silk/VQ_WMat_EC.c
@@ -36,18 +36,21 @@ void silk_VQ_WMat_EC_c(
     opus_int8                   *ind,                           /* O    index of best codebook vector               */
     opus_int32                  *res_nrg_Q15,                   /* O    best residual energy                        */
     opus_int32                  *rate_dist_Q8,                  /* O    best total bitrate                          */
+    opus_int                    *gain_Q7,                       /* O    sum of absolute LTP coefficients            */
     const opus_int32            *XX_Q17,                        /* I    correlation matrix                          */
     const opus_int32            *xX_Q17,                        /* I    correlation vector                          */
     const opus_int8             *cb_Q7,                         /* I    codebook                                    */
+    const opus_uint8            *cb_gain_Q7,                    /* I    codebook effective gain                     */
     const opus_uint8            *cl_Q5,                         /* I    code length for each codebook vector        */
     const opus_int              subfr_len,                      /* I    number of samples per subframe              */
+    const opus_int32            max_gain_Q7,                    /* I    maximum sum of absolute LTP coefficients    */
     const opus_int              L                               /* I    number of vectors in codebook               */
 )
 {
-    opus_int   k;
+    opus_int   k, gain_tmp_Q7;
     const opus_int8 *cb_row_Q7;
     opus_int32 neg_xX_Q24[ 5 ];
-    opus_int32 sum1_Q15, sum2_Q24;
+    opus_int32 sum1_Q15, sum2_Q24, penalty;
     opus_int32 bits_res_Q8, bits_tot_Q8;
 
     /* Negate and convert to new Q domain */
@@ -62,9 +65,13 @@ void silk_VQ_WMat_EC_c(
     *res_nrg_Q15 = silk_int32_MAX;
     cb_row_Q7 = cb_Q7;
     for( k = 0; k < L; k++ ) {
+        gain_tmp_Q7 = cb_gain_Q7[k];
+
         /* Weighted rate */
         /* Quantization error: 1 - 2 * xX * cb + cb' * XX * cb */
         sum1_Q15 = SILK_FIX_CONST( 1.001, 15 );
+        /* Penalty for too large gain */
+        penalty = silk_ADD_LSHIFT32( 0, silk_max( silk_SUB32( gain_tmp_Q7, max_gain_Q7 ), 0 ), 11 );
 
         /* first row of XX_Q17 */
         sum2_Q24 = silk_MLA( neg_xX_Q24[ 0 ], XX_Q17[  1 ], cb_row_Q7[ 1 ] );
@@ -104,13 +111,14 @@ void silk_VQ_WMat_EC_c(
         /* find best */
         if( sum1_Q15 >= 0 ) {
             /* Translate residual energy to bits using high-rate assumption (6 dB ==> 1 bit/sample) */
-            bits_res_Q8 = silk_SMULBB( subfr_len, silk_lin2log( sum1_Q15 ) - (15 << 7) );
+            bits_res_Q8 = silk_SMULBB( subfr_len, silk_lin2log( sum1_Q15 + penalty ) - (15 << 7) );
             /* In the following line we reduce the codelength component by half ("-1"); seems to slghtly improve quality */
             bits_tot_Q8 = silk_ADD_LSHIFT32( bits_res_Q8, cl_Q5[ k ], 3-1 );
             if( bits_tot_Q8 <= *rate_dist_Q8 ) {
                 *rate_dist_Q8 = bits_tot_Q8;
-                *res_nrg_Q15 = sum1_Q15;
+                *res_nrg_Q15 = sum1_Q15 + penalty;
                 *ind = (opus_int8)k;
+                *gain_Q7 = gain_tmp_Q7;
             }
         }
 
diff --git a/silk/float/find_pred_coefs_FLP.c b/silk/float/find_pred_coefs_FLP.c
index 86799742..2f3c2451 100644
--- a/silk/float/find_pred_coefs_FLP.c
+++ b/silk/float/find_pred_coefs_FLP.c
@@ -66,7 +66,7 @@ void silk_find_pred_coefs_FLP(
 
         /* Quantize LTP gain parameters */
         silk_quant_LTP_gains_FLP( psEncCtrl->LTPCoef, psEnc->sCmn.indices.LTPIndex, &psEnc->sCmn.indices.PERIndex,
-            &psEncCtrl->LTPredCodGain, XXLTP, xXLTP, psEnc->sCmn.subfr_length, psEnc->sCmn.nb_subfr, psEnc->sCmn.arch );
+              &psEnc->sCmn.sum_log_gain_Q7, &psEncCtrl->LTPredCodGain, XXLTP, xXLTP, psEnc->sCmn.subfr_length, psEnc->sCmn.nb_subfr, psEnc->sCmn.arch );
 
         /* Control LTP scaling */
         silk_LTP_scale_ctrl_FLP( psEnc, psEncCtrl, condCoding );
@@ -89,6 +89,7 @@ void silk_find_pred_coefs_FLP(
         }
         silk_memset( psEncCtrl->LTPCoef, 0, psEnc->sCmn.nb_subfr * LTP_ORDER * sizeof( silk_float ) );
         psEncCtrl->LTPredCodGain = 0.0f;
+        psEnc->sCmn.sum_log_gain_Q7 = 0;
     }
 
     /* Limit on total predictive coding gain */
diff --git a/silk/float/main_FLP.h b/silk/float/main_FLP.h
index 41369cc3..5ea14451 100644
--- a/silk/float/main_FLP.h
+++ b/silk/float/main_FLP.h
@@ -188,6 +188,7 @@ void silk_quant_LTP_gains_FLP(
     silk_float                      B[ MAX_NB_SUBFR * LTP_ORDER ],      /* O    Quantized LTP gains                         */
     opus_int8                       cbk_index[ MAX_NB_SUBFR ],          /* O    Codebook index                              */
     opus_int8                       *periodicity_index,                 /* O    Periodicity index                           */
+    opus_int32                      *sum_log_gain_Q7,                   /* I/O  Cumulative max prediction gain  */
     silk_float                      *pred_gain_dB,                      /* O    LTP prediction gain                         */
     const silk_float                XX[ MAX_NB_SUBFR * LTP_ORDER * LTP_ORDER ], /* I    Correlation matrix                  */
     const silk_float                xX[ MAX_NB_SUBFR * LTP_ORDER ],     /* I    Correlation vector                          */
diff --git a/silk/float/wrappers_FLP.c b/silk/float/wrappers_FLP.c
index 81e0b66e..e8f43831 100644
--- a/silk/float/wrappers_FLP.c
+++ b/silk/float/wrappers_FLP.c
@@ -175,6 +175,7 @@ void silk_quant_LTP_gains_FLP(
     silk_float                      B[ MAX_NB_SUBFR * LTP_ORDER ],      /* O    Quantized LTP gains                            */
     opus_int8                       cbk_index[ MAX_NB_SUBFR ],          /* O    Codebook index                              */
     opus_int8                       *periodicity_index,                 /* O    Periodicity index                           */
+    opus_int32                      *sum_log_gain_Q7,                   /* I/O  Cumulative max prediction gain  */
     silk_float                      *pred_gain_dB,                        /* O    LTP prediction gain                            */
     const silk_float                XX[ MAX_NB_SUBFR * LTP_ORDER * LTP_ORDER ], /* I    Correlation matrix                    */
     const silk_float                xX[ MAX_NB_SUBFR * LTP_ORDER ],        /* I    Correlation vector                            */
@@ -195,7 +196,7 @@ void silk_quant_LTP_gains_FLP(
         xX_Q17[ i ] = (opus_int32)silk_float2int( xX[ i ] * 131072.0f );
     }
 
-    silk_quant_LTP_gains( B_Q14, cbk_index, periodicity_index, &pred_gain_dB_Q7, XX_Q17, xX_Q17, subfr_len, nb_subfr, arch );
+    silk_quant_LTP_gains( B_Q14, cbk_index, periodicity_index, sum_log_gain_Q7, &pred_gain_dB_Q7, XX_Q17, xX_Q17, subfr_len, nb_subfr, arch );
 
     for( i = 0; i < nb_subfr * LTP_ORDER; i++ ) {
         B[ i ] = (silk_float)B_Q14[ i ] * ( 1.0f / 16384.0f );
diff --git a/silk/main.h b/silk/main.h
index 94315b47..270bfd0c 100644
--- a/silk/main.h
+++ b/silk/main.h
@@ -208,6 +208,7 @@ void silk_quant_LTP_gains(
     opus_int16                  B_Q14[ MAX_NB_SUBFR * LTP_ORDER ],          /* O    Quantized LTP gains             */
     opus_int8                   cbk_index[ MAX_NB_SUBFR ],                  /* O    Codebook Index                  */
     opus_int8                   *periodicity_index,                         /* O    Periodicity Index               */
+    opus_int32                  *sum_log_gain_Q7,                           /* I/O  Cumulative max prediction gain  */
     opus_int                    *pred_gain_dB_Q7,                           /* O    LTP prediction gain             */
     const opus_int32            XX_Q17[ MAX_NB_SUBFR*LTP_ORDER*LTP_ORDER ], /* I    Correlation matrix in Q18       */
     const opus_int32            xX_Q17[ MAX_NB_SUBFR*LTP_ORDER ],           /* I    Correlation vector in Q18       */
@@ -221,17 +222,20 @@ void silk_VQ_WMat_EC_c(
     opus_int8                   *ind,                           /* O    index of best codebook vector               */
     opus_int32                  *res_nrg_Q15,                   /* O    best residual energy                        */
     opus_int32                  *rate_dist_Q8,                  /* O    best total bitrate                          */
+    opus_int                    *gain_Q7,                       /* O    sum of absolute LTP coefficients            */
     const opus_int32            *XX_Q17,                        /* I    correlation matrix                          */
     const opus_int32            *xX_Q17,                        /* I    correlation vector                          */
     const opus_int8             *cb_Q7,                         /* I    codebook                                    */
+    const opus_uint8            *cb_gain_Q7,                    /* I    codebook effective gain                     */
     const opus_uint8            *cl_Q5,                         /* I    code length for each codebook vector        */
     const opus_int              subfr_len,                      /* I    number of samples per subframe              */
+    const opus_int32            max_gain_Q7,                    /* I    maximum sum of absolute LTP coefficients    */
     const opus_int              L                               /* I    number of vectors in codebook               */
 );
 
 #if !defined(OVERRIDE_silk_VQ_WMat_EC)
-#define silk_VQ_WMat_EC(ind, rate_dist_Q15, gain_Q8, XX_Q17, xX_Q17, cb_Q7, cl_Q5, subfr_len, L, arch) \
-    ((void)(arch),silk_VQ_WMat_EC_c(ind, rate_dist_Q15, gain_Q8, XX_Q17, xX_Q17, cb_Q7, cl_Q5, subfr_len, L))
+#define silk_VQ_WMat_EC(ind, rate_dist_Q15, gain_Q7, gain_Q8, XX_Q17, xX_Q17, cb_Q7, cb_gain_Q7, cl_Q5, subfr_len, max_gain_Q7, L, arch) \
+    ((void)(arch),silk_VQ_WMat_EC_c(ind, rate_dist_Q15, gain_Q7, gain_Q8, XX_Q17, xX_Q17, cb_Q7, cb_gain_Q7, cl_Q5, subfr_len, max_gain_Q7, L))
 #endif
 
 /************************************/
diff --git a/silk/quant_LTP_gains.c b/silk/quant_LTP_gains.c
index 6fd28d20..979c6fa1 100644
--- a/silk/quant_LTP_gains.c
+++ b/silk/quant_LTP_gains.c
@@ -30,11 +30,13 @@ POSSIBILITY OF SUCH DAMAGE.
 #endif
 
 #include "main.h"
+#include "tuning_parameters.h"
 
 void silk_quant_LTP_gains(
     opus_int16                  B_Q14[ MAX_NB_SUBFR * LTP_ORDER ],          /* O    Quantized LTP gains             */
     opus_int8                   cbk_index[ MAX_NB_SUBFR ],                  /* O    Codebook Index                  */
     opus_int8                   *periodicity_index,                         /* O    Periodicity Index               */
+    opus_int32                  *sum_log_gain_Q7,                           /* I/O  Cumulative max prediction gain  */
     opus_int                    *pred_gain_dB_Q7,                           /* O    LTP prediction gain             */
     const opus_int32            XX_Q17[ MAX_NB_SUBFR*LTP_ORDER*LTP_ORDER ], /* I    Correlation matrix in Q18       */
     const opus_int32            xX_Q17[ MAX_NB_SUBFR*LTP_ORDER ],           /* I    Correlation vector in Q18       */
@@ -47,17 +49,25 @@ void silk_quant_LTP_gains(
     opus_int8            temp_idx[ MAX_NB_SUBFR ];
     const opus_uint8     *cl_ptr_Q5;
     const opus_int8      *cbk_ptr_Q7;
+    const opus_uint8     *cbk_gain_ptr_Q7;
     const opus_int32     *XX_Q17_ptr, *xX_Q17_ptr;
     opus_int32           res_nrg_Q15_subfr, res_nrg_Q15, rate_dist_Q7_subfr, rate_dist_Q7, min_rate_dist_Q7;
+    opus_int32           sum_log_gain_tmp_Q7, best_sum_log_gain_tmp_Q7, max_gain_Q7, gain_Q7;
 
     /***************************************************/
     /* iterate over different codebooks with different */
     /* rates/distortions, and choose best */
     /***************************************************/
     min_rate_dist_Q7 = silk_int32_MAX;
+    best_sum_log_gain_tmp_Q7 = 0;
     for( k = 0; k < 3; k++ ) {
+        /* Safety margin for pitch gain control, to take into account factors
+           such as state rescaling/rewhitening. */
+        opus_int32 gain_safety = SILK_FIX_CONST( 0.4, 7 );
+
         cl_ptr_Q5  = silk_LTP_gain_BITS_Q5_ptrs[ k ];
         cbk_ptr_Q7 = silk_LTP_vq_ptrs_Q7[        k ];
+        cbk_gain_ptr_Q7 = silk_LTP_vq_gain_ptrs_Q7[ k ];
         cbk_size   = silk_LTP_vq_sizes[          k ];
 
         /* Set up pointers to first subframe */
@@ -66,20 +76,29 @@ void silk_quant_LTP_gains(
 
         res_nrg_Q15 = 0;
         rate_dist_Q7 = 0;
+        sum_log_gain_tmp_Q7 = *sum_log_gain_Q7;
         for( j = 0; j < nb_subfr; j++ ) {
+           max_gain_Q7 = silk_log2lin( ( SILK_FIX_CONST( MAX_SUM_LOG_GAIN_DB / 6.0, 7 ) - sum_log_gain_tmp_Q7 )
+           + SILK_FIX_CONST( 7, 7 ) ) - gain_safety;
+
             silk_VQ_WMat_EC(
                 &temp_idx[ j ],         /* O    index of best codebook vector                           */
                 &res_nrg_Q15_subfr,     /* O    residual energy                                         */
                 &rate_dist_Q7_subfr,    /* O    best weighted quantization error + mu * rate            */
+                &gain_Q7,               /* O    sum of absolute LTP coefficients                        */
                 XX_Q17_ptr,             /* I    correlation matrix                                      */
                 xX_Q17_ptr,             /* I    correlation vector                                      */
                 cbk_ptr_Q7,             /* I    codebook                                                */
+                cbk_gain_ptr_Q7,        /* I    codebook effective gains                                */
                 cl_ptr_Q5,              /* I    code length for each codebook vector                    */
                 subfr_len,              /* I    number of samples per subframe                          */
+                max_gain_Q7,            /* I    maximum sum of absolute LTP coefficients                */
                 cbk_size,               /* I    number of vectors in codebook                           */
                 arch                    /* I    Run-time architecture                                   */
             );
 
+            sum_log_gain_tmp_Q7 = silk_max(0, sum_log_gain_tmp_Q7
+                                + silk_lin2log( gain_safety + gain_Q7 ) - SILK_FIX_CONST( 7, 7 ));
             res_nrg_Q15  = silk_ADD_POS_SAT32( res_nrg_Q15, res_nrg_Q15_subfr );
             rate_dist_Q7 = silk_ADD_POS_SAT32( rate_dist_Q7, rate_dist_Q7_subfr );
 
@@ -91,6 +110,7 @@ void silk_quant_LTP_gains(
             min_rate_dist_Q7 = rate_dist_Q7;
             *periodicity_index = (opus_int8)k;
             silk_memcpy( cbk_index, temp_idx, nb_subfr * sizeof( opus_int8 ) );
+            best_sum_log_gain_tmp_Q7 = sum_log_gain_tmp_Q7;
         }
     }
 
@@ -108,4 +128,5 @@ void silk_quant_LTP_gains(
     }
 
     *pred_gain_dB_Q7 = (opus_int)silk_SMULBB( -3, silk_lin2log( res_nrg_Q15 ) - ( 15 << 7 ) );
+    *sum_log_gain_Q7 = best_sum_log_gain_tmp_Q7;
 }
diff --git a/silk/tables.h b/silk/tables.h
index 7b84c8d2..8b0380ee 100644
--- a/silk/tables.h
+++ b/silk/tables.h
@@ -77,6 +77,7 @@ extern const opus_uint8  silk_LTP_per_index_iCDF[ 3 ];
 extern const opus_uint8  * const silk_LTP_gain_iCDF_ptrs[ NB_LTP_CBKS ];                            /*   3 */
 extern const opus_uint8  * const silk_LTP_gain_BITS_Q5_ptrs[ NB_LTP_CBKS ];                         /*   3 */
 extern const opus_int8   * const silk_LTP_vq_ptrs_Q7[ NB_LTP_CBKS ];                                /* 168 */
+extern const opus_uint8  * const silk_LTP_vq_gain_ptrs_Q7[NB_LTP_CBKS];
 extern const opus_int8   silk_LTP_vq_sizes[ NB_LTP_CBKS ];                                          /*   3 */
 
 extern const opus_uint8  silk_LTPscale_iCDF[ 3 ];                                                   /*   4 */
diff --git a/silk/tables_LTP.c b/silk/tables_LTP.c
index 30d2a1ea..5e12c864 100644
--- a/silk/tables_LTP.c
+++ b/silk/tables_LTP.c
@@ -265,6 +265,30 @@ const opus_int8 * const silk_LTP_vq_ptrs_Q7[NB_LTP_CBKS] = {
     (opus_int8 *)&silk_LTP_gain_vq_2[0][0]
 };
 
+/* Maximum frequency-dependent response of the pitch taps above,
+   computed as max(abs(freqz(taps))) */
+static const opus_uint8 silk_LTP_gain_vq_0_gain[8] = {
+      46,      2,     90,     87,     93,     91,     82,     98
+};
+
+static const opus_uint8 silk_LTP_gain_vq_1_gain[16] = {
+     109,    120,    118,     12,    113,    115,    117,    119,
+      99,     59,     87,    111,     63,    111,    112,     80
+};
+
+static const opus_uint8 silk_LTP_gain_vq_2_gain[32] = {
+     126,    124,    125,    124,    129,    121,    126,     23,
+     132,    127,    127,    127,    126,    127,    122,    133,
+     130,    134,    101,    118,    119,    145,    126,     86,
+     124,    120,    123,    119,    170,    173,    107,    109
+};
+
+const opus_uint8 * const silk_LTP_vq_gain_ptrs_Q7[NB_LTP_CBKS] = {
+    &silk_LTP_gain_vq_0_gain[0],
+    &silk_LTP_gain_vq_1_gain[0],
+    &silk_LTP_gain_vq_2_gain[0]
+};
+
 const opus_int8 silk_LTP_vq_sizes[NB_LTP_CBKS] = {
     8, 16, 32
 };
diff --git a/silk/tuning_parameters.h b/silk/tuning_parameters.h
index cbf68bac..8093d0b4 100644
--- a/silk/tuning_parameters.h
+++ b/silk/tuning_parameters.h
@@ -53,6 +53,9 @@ extern "C"
 /* LPC analysis defines: regularization and bandwidth expansion */
 #define FIND_LPC_COND_FAC                               1e-5f
 
+/* Max cumulative LTP gain */
+#define MAX_SUM_LOG_GAIN_DB                             250.0f
+
 /***********************/
 /* High pass filtering */
 /***********************/