quantize.c

音频编码

/*
 * MP3 quantization
 *
 *	Copyright (c) 1999-2000 Mark Taylor
 *	Copyright (c) 1999-2003 Takehiro Tominaga
 *	Copyright (c) 2000-2005 Robert Hegemann
 *	Copyright (c) 2001-2005 Gabriel Bouvigne
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.     See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

/* $Id: quantize.c,v 1.170.2.2 2005/11/20 14:08:25 bouvigne Exp $ */

#ifdef HAVE_CONFIG_H
# include <config.h>
#endif

#include <math.h>
#include <assert.h>
#include <stdlib.h>
#include "bitstream.h"
#include "l3side.h"
#include "quantize.h"
#include "reservoir.h"
#include "quantize_pvt.h"
#include "lame-analysis.h"
#include "vbrquantize.h"
#include "machine.h"
#include "util.h"

#ifdef WITH_DMALLOC
#include <dmalloc.h>
#endif

/* Robert Hegemann - 2002-10-24
 * sparsing of mid side channels
 * 2DO: replace mld by something with finer resolution
 */
static void
ms_sparsing(lame_internal_flags* gfc, int gr)
{
    int sfb, m, k, i, j = 0;
    int width;
    FLOAT treshold = 0;
    
    if ( gfc->sparsing == 0 ) return;
    m = gfc->l3_side.tt[gr][0].sfb_lmax;
    for ( sfb = 0; sfb < m; ++sfb ) {
        width = gfc->scalefac_band.l[sfb+1] - gfc->scalefac_band.l[sfb];
        treshold = pow( 10, -(gfc->sparseA-gfc->mld_l[sfb]*gfc->sparseB)/10.);
        for ( i = 0; i < width; i += 2, j += 2 ) {
            FLOAT* m0 = gfc->l3_side.tt[gr][0].xr+j;
            FLOAT* m1 = gfc->l3_side.tt[gr][0].xr+j+1;
            FLOAT* s0 = gfc->l3_side.tt[gr][1].xr+j;
            FLOAT* s1 = gfc->l3_side.tt[gr][1].xr+j+1;
            FLOAT m02 = *m0 * *m0;
            FLOAT m12 = *m1 * *m1;
            FLOAT s02 = *s0 * *s0;
            FLOAT s12 = *s1 * *s1;
            FLOAT u0 = m02*treshold;
            FLOAT u1 = m12*treshold;
            FLOAT v0 = s02*treshold;
            FLOAT v1 = s12*treshold;
            if ( s02 < u0 && s12 < u1 ) { 
                *m0 += *s0; *s0 = 0; 
                *m1 += *s1; *s1 = 0; 
            }
            if ( m02 < v0 && m12 < v1 ) { 
                *s0 += *m0; *m0 = 0; 
                *s1 += *m1; *m1 = 0; 
            }
        }
    }    
    for ( sfb = gfc->l3_side.tt[gr][0].sfb_smin; sfb < SBPSY_s; ++sfb ) {
        width = gfc->scalefac_band.s[sfb+1] - gfc->scalefac_band.s[sfb];
        treshold = pow( 10, -(gfc->sparseA-gfc->mld_s[sfb]*gfc->sparseB)/10.);
        for ( i = 0; i < width; i += 2, j += 6 ) 
        for ( k = 0; k < 3; ++k ) {
            FLOAT* m0 = gfc->l3_side.tt[gr][0].xr+j+k;
            FLOAT* m1 = gfc->l3_side.tt[gr][0].xr+j+k+3;
            FLOAT* s0 = gfc->l3_side.tt[gr][1].xr+j+k;
            FLOAT* s1 = gfc->l3_side.tt[gr][1].xr+j+k+3;
            FLOAT m02 = *m0 * *m0;
            FLOAT m12 = *m1 * *m1;
            FLOAT s02 = *s0 * *s0;
            FLOAT s12 = *s1 * *s1;
            FLOAT u0 = m02*treshold;
            FLOAT u1 = m12*treshold;
            FLOAT v0 = s02*treshold;
            FLOAT v1 = s12*treshold;
            if ( s02 < u0 && s12 < u1 ) { 
                *m0 += *s0; *s0 = 0; 
                *m1 += *s1; *s1 = 0; 
            }
            if ( m02 < v0 && m12 < v1 ) { 
                *s0 += *m0; *m0 = 0; 
                *s1 += *m1; *m1 = 0; 
            }
        }
    }
    m = ( sfb == SBPSY_s ) ? 3 : 1;
    if ( gfc->sparsing == 2 ) {
        for ( ; j < 575; j += 2*m ) 
        for ( i = 0; i < m; ++i ) {
            FLOAT* m0 = gfc->l3_side.tt[gr][0].xr+j+i;
            FLOAT* m1 = gfc->l3_side.tt[gr][0].xr+j+i+m;
            FLOAT* s0 = gfc->l3_side.tt[gr][1].xr+j+i;
            FLOAT* s1 = gfc->l3_side.tt[gr][1].xr+j+i+m;
            *m0 += *s0; *s0 = 0; 
            *m1 += *s1; *s1 = 0; 
        }
    }
    else {
    for ( ; j < 575; j += 2*m ) 
    for ( i = 0; i < m; ++i ) {
        FLOAT* m0 = gfc->l3_side.tt[gr][0].xr+j+i;
        FLOAT* m1 = gfc->l3_side.tt[gr][0].xr+j+i+m;
        FLOAT* s0 = gfc->l3_side.tt[gr][1].xr+j+i;
        FLOAT* s1 = gfc->l3_side.tt[gr][1].xr+j+i+m;
        FLOAT m02 = *m0 * *m0;
        FLOAT m12 = *m1 * *m1;
        FLOAT s02 = *s0 * *s0;
        FLOAT s12 = *s1 * *s1;
        FLOAT u0 = m02*treshold;
        FLOAT u1 = m12*treshold;
        FLOAT v0 = s02*treshold;
        FLOAT v1 = s12*treshold;
        if ( s02 < u0 && s12 < u1 ) { 
            *m0 += *s0; *s0 = 0; 
            *m1 += *s1; *s1 = 0; 
        }
        if ( m02 < v0 && m12 < v1 ) { 
            *s0 += *m0; *m0 = 0; 
            *s1 += *m1; *m1 = 0; 
        }
    }
    }
}


/* convert from L/R <-> Mid/Side */
static void
ms_convert(III_side_info_t *l3_side, int gr)
{
    int i;
    for (i = 0; i < 576; ++i) {
	FLOAT l, r;
        l = l3_side->tt[gr][0].xr[i];
        r = l3_side->tt[gr][1].xr[i];
        l3_side->tt[gr][0].xr[i] = (l+r) * (FLOAT)(SQRT2*0.5);
        l3_side->tt[gr][1].xr[i] = (l-r) * (FLOAT)(SQRT2*0.5);
    }
}

/************************************************************************
 *
 *      init_outer_loop()
 *  mt 6/99                                    
 *
 *  initializes cod_info, scalefac and xrpow
 *
 *  returns 0 if all energies in xr are zero, else 1                    
 *
 ************************************************************************/


void init_xrpow_core_c(gr_info *const cod_info, 
                        FLOAT xrpow[576],
                        int upper,
                        FLOAT* sum)
{
    int i;
    FLOAT tmp;
    *sum = 0;
    for (i = 0; i <= upper; ++i) {
        tmp = fabs (cod_info->xr[i]);
        *sum += tmp;
        xrpow[i] = sqrt (tmp * sqrt(tmp));

        if (xrpow[i] > cod_info->xrpow_max)
            cod_info->xrpow_max = xrpow[i];
    }
}


#ifdef HAVE_XMMINTRIN_H

#include <xmmintrin.h>

typedef union {
    __m128   _m128;
    float     _float[4];
} vecfloat_union;



void init_xrpow_core_sse(gr_info *const cod_info, 
                        FLOAT xrpow[576],
                        int upper,
                        FLOAT* sum)
{
    int i;
    FLOAT tmp;
    int upper4 = (upper/4)*4;


    const int fabs_mask[4] = {0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
    const __m128 vec_fabs_mask = _mm_loadu_ps((float *) fabs_mask);
    vecfloat_union vec_xrpow_max;
    vecfloat_union vec_sum;

    _mm_prefetch((char*)cod_info->xr, _MM_HINT_T0);
    _mm_prefetch((char*)xrpow, _MM_HINT_T0);

    vec_xrpow_max._m128 = _mm_set_ps1(0);
    vec_sum._m128 = _mm_set_ps1(0);

    for (i = 0; i < upper4; i+=4) {
        __m128 vec_tmp;

        vec_tmp = _mm_loadu_ps(&(cod_info->xr[i])); //load
        vec_tmp = _mm_and_ps(vec_tmp, vec_fabs_mask); //fabs

        vec_sum._m128 = _mm_add_ps(vec_sum._m128, vec_tmp);

        vec_tmp = _mm_sqrt_ps(_mm_mul_ps(vec_tmp, _mm_sqrt_ps(vec_tmp)));

        _mm_storeu_ps(&(xrpow[i]), vec_tmp); //store into xrpow[]

        vec_xrpow_max._m128 = _mm_max_ps(vec_xrpow_max._m128, vec_tmp); //retrieve max
    }

    *sum = vec_sum._float[0] + vec_sum._float[1] + vec_sum._float[2] + vec_sum._float[3];
    cod_info->xrpow_max = Max(vec_xrpow_max._float[0],
                                Max(vec_xrpow_max._float[1],
                                Max(vec_xrpow_max._float[2], vec_xrpow_max._float[3])));



    for (i = upper4; i <= upper; ++i) {
        tmp = fabs (cod_info->xr[i]);
        *sum += tmp;
        xrpow[i] = sqrt (tmp * sqrt(tmp));

        if (xrpow[i] > cod_info->xrpow_max)
            cod_info->xrpow_max = xrpow[i];
    }
}


#endif





void init_xrpow_core_init(lame_internal_flags * const gfc)
{
    gfc->init_xrpow_core = init_xrpow_core_c;
    
#ifdef HAVE_XMMINTRIN_H
    if (gfc->CPU_features.SSE)
        gfc->init_xrpow_core = init_xrpow_core_sse;
#endif
}



static int 
init_xrpow(
    lame_internal_flags *gfc,
    gr_info *const cod_info, 
    FLOAT xrpow[576] )
{
    FLOAT sum = 0;
    int i;
    int upper = cod_info->max_nonzero_coeff;

    assert( xrpow != NULL );
    cod_info->xrpow_max = 0;
    
    /*  check if there is some energy we have to quantize
     *  and calculate xrpow matching our fresh scalefactors
     */
    memset(&(xrpow[upper]), 0, (575-upper)*sizeof(xrpow[upper]));


    gfc->init_xrpow_core(cod_info, xrpow, upper, &sum);

    /*  return 1 if we have something to quantize, else 0
     */
    if (sum > (FLOAT)1E-20) {
        int j = 0;
        if (gfc->substep_shaping & 2)
            j = 1;

        for (i = 0; i < cod_info->psymax; i++)
            gfc->pseudohalf[i] = j;

        return 1;
    }

    memset(&cod_info->l3_enc, 0, sizeof(int)*576);
    return 0;
}





extern FLOAT athAdjust( FLOAT a, FLOAT x, FLOAT athFloor );



/*
Gabriel Bouvigne feb/apr 2003
Analog silence detection in partitionned sfb21
or sfb12 for short blocks

From top to bottom of sfb, changes to 0
coeffs which are below ath. It stops on the first
coeff higher than ath.
*/
void psfb21_analogsilence(
        lame_global_flags *gfp,
        lame_internal_flags *gfc,
	    gr_info *const cod_info
    )
{
    ATH_t * ATH = gfc->ATH;
    FLOAT *xr = cod_info->xr;

    if (cod_info->block_type == NORM_TYPE) {
        int gsfb;
        int stop=0;
        for (gsfb = PSFB21-1; gsfb>=0 && !stop; gsfb--) {
            int start = gfc->scalefac_band.psfb21[ gsfb ];
            int end = gfc->scalefac_band.psfb21[ gsfb+1 ];
            int j;
            FLOAT ath21;
            ath21 = athAdjust(ATH->adjust, ATH->psfb21[gsfb], ATH->floor);

            if (gfc->nsPsy.longfact[21] != 0)
                ath21 *= gfc->nsPsy.longfact[21];

            for (j = end-1; j>=start; j--) {
                if ( fabs(xr[j]) < ath21)
                    xr[j] = 0;
                else {
                    stop = 1;
                    break;
                }
            }
        }
    } else if (cod_info->block_type == SHORT_TYPE) {
        /*note: short blocks coeffs are reordered*/
        int block;
        for (block = 0; block<3; block++) {

            int gsfb;
            int stop=0;
            for (gsfb = PSFB12-1; gsfb>=0 && !stop; gsfb--) {
                int start = gfc->scalefac_band.s[12] * 3 +
                            (gfc->scalefac_band.s[13] - gfc->scalefac_band.s[12]) * block +
                            (gfc->scalefac_band.psfb12[gsfb] - gfc->scalefac_band.psfb12[0]);
                int end = start + (gfc->scalefac_band.psfb12[gsfb+1] - gfc->scalefac_band.psfb12[gsfb]);
                int j;
                FLOAT ath12;
                ath12 = athAdjust(ATH->adjust, ATH->psfb12[gsfb], ATH->floor);

                if (gfc->nsPsy.shortfact[12] != 0)
                    ath12 *= gfc->nsPsy.shortfact[12];

                for (j = end-1; j>=start; j--) {
                    if ( fabs(xr[j]) < ath12)
                        xr[j] = 0;
                    else {
                        stop = 1;
                        break;
                    }
                }
            }
        }
    }

}
                         




static void
init_outer_loop(
    lame_global_flags *gfp,
    lame_internal_flags *gfc,
    gr_info *const cod_info)
{
    int sfb, j;
    /*  initialize fresh cod_info
     */
    cod_info->part2_3_length      = 0;
    cod_info->big_values          = 0;
    cod_info->count1              = 0;
    cod_info->global_gain         = 210;
    cod_info->scalefac_compress   = 0;
    /* mixed_block_flag, block_type was set in psymodel.c */
    cod_info->table_select [0]    = 0;
    cod_info->table_select [1]    = 0;
    cod_info->table_select [2]    = 0;
    cod_info->subblock_gain[0]    = 0;
    cod_info->subblock_gain[1]    = 0;
    cod_info->subblock_gain[2]    = 0;
    cod_info->subblock_gain[3]    = 0;    /* this one is always 0 */
    cod_info->region0_count       = 0;
    cod_info->region1_count       = 0;
    cod_info->preflag             = 0;
    cod_info->scalefac_scale      = 0;
    cod_info->count1table_select  = 0;