g729cp.c

语音编码G.729 语音编码G.729

/*-------------------------------------------------------------*
 * g729cp.c - init & line packing for G.729/D/E w/B codecs
 *-------------------------------------------------------------*/

#include <memory.h>
#include "g729cp.h"

#if defined(_MSC_VER)
#	define __inline__	__inline
#endif

/* line packing ops */
__inline__ static void  put32_BE(unsigned char *out, INT32 bits);
__inline__ static INT32 get32_BE(unsigned char *in);
__inline__ static void  put24_BE(unsigned char *out, INT32 bits);
__inline__ static INT32 get24_BE(unsigned char *in);
__inline__ static void  put16_BE(unsigned char *out, INT32 bits);
__inline__ static INT32 get16_BE(unsigned char *in);
__inline__ static void  float_to_sint16(INT16 *dst, FLOAT *src, int len);

void
g729_init_coder(struct cod_state *cod, int vad_enable)
{
	cod->frame = 0;
	cod->vad_enable = vad_enable;

	init_pre_process(&cod->preproc_s);
	init_coder_ld8c(&cod->cod_s, vad_enable);

	/* for G.729B */
	if (vad_enable)
		init_cod_cng(&cod->cod_s.cng_s);
}

int
g729_coder(struct cod_state * cod, INT16 *DataBuff, char *Vout, int* poutlen)
{
	int parm[2 + PRM_SIZE_E];        /* Analysis parameters + frame type */

	{
		int x;
		for (x = 0; x < L_FRAME; ++x)
			cod->cod_s.new_speech[x] = DataBuff[x];
	}

    if (cod->frame == 32767)
		cod->frame = 256;
    else
		cod->frame++;

    pre_process(&cod->preproc_s, cod->cod_s.new_speech, L_FRAME);
	/* FIXME: change rate, if necessary */
	coder_ld8c(&cod->cod_s, parm, cod->frame, cod->vad_enable, G729);

	g729_line_pack(parm, Vout, poutlen);

	return 0;
}

void
g729_init_decoder(struct dec_state * dec)
{
	init_decod_ld8c(&dec->dec_s);
	init_post_filter(&dec->postfilt_s);
	init_post_process(&dec->postproc_s);

	memset(dec->synth_buf, 0, sizeof(dec->synth_buf[0]) * L_ANA_BWD);
    dec->synth = dec->synth_buf + MEM_SYN_BWD;

    dec->voicing = 60;                        /* voicing from previous frame */
	dec->sf_voic = 0;                        /* voicing for subframe        */
    dec->ga1_post = GAMMA1_PST_E;
    dec->ga2_post = GAMMA2_PST_E;
    dec->ga_harm  = GAMMA_HARM_E;

	/* for G.729b */
	init_dec_cng(&dec->dec_s.cng_s);
}

int
g729_decoder(struct dec_state *dec, INT16 *DataBuff, char *Vinp, int inplen)
{
	FLOAT  pst_out[L_FRAME];            /* Postfilter output           */
	int    parm[3 + PRM_SIZE_E];        /* Synthesis parameters + BFI */
	FLOAT  Az_dec[M_BWDP1*2];           /* Decoded Az for post-filter */
	FLOAT  *ptr_Az;
	int    Ftyp;
	int    Vad;
	int    T0_first;                       /* Pitch lag in 1st subframe   */
	int    m_pst;
	int    bwd_dominant;
	int    long_h_st;
	int    i;

	if (inplen == G729_VOICE_FRAME_SIZE)
		Ftyp = G729_TYPE_VOICE;
	else if (inplen == G729_SID_FRAME_SIZE)
		Ftyp = G729_TYPE_SID;
	else if (inplen == G729_VOICE_D_FRAME_SIZE)
		Ftyp = G729_TYPE_VOICE_D;
	else if (inplen == G729_VOICE_E_FRAME_SIZE)
		Ftyp = G729_TYPE_VOICE_E;
	else if (inplen == 0)
		Ftyp = G729_TYPE_DONTSEND;
	else
		return -1;
 
	g729_line_unpack(parm, Vinp, Ftyp);

	decod_ld8c(&dec->dec_s, parm, dec->voicing, dec->synth_buf, Az_dec, &T0_first, &bwd_dominant, &m_pst, &Vad);

	/* Postfilter */
	ptr_Az = Az_dec;

	/* Adaptive parameters for postfiltering */
	/* ------------------------------------- */
	if (Ftyp != G729_TYPE_VOICE_E)
	{
		long_h_st = LONG_H_ST;
		dec->ga1_post = GAMMA1_PST;
		dec->ga2_post = GAMMA2_PST;
		dec->ga_harm = GAMMA_HARM;
	}
	else
	{	/* G729_TYPE_VOICE_E */
		long_h_st = LONG_H_ST_E;
		/* If backward mode is dominant => progressively reduce postfiltering */
		if (parm[2] == 1 && bwd_dominant == 1)
		{
			dec->ga_harm -= (F)0.0125;
			if (dec->ga_harm < 0) dec->ga_harm = 0;
			dec->ga1_post -= (F)0.035;
			if (dec->ga1_post < 0) dec->ga1_post = 0;
			dec->ga2_post -= (F)0.0325;
			if (dec->ga2_post < 0) dec->ga2_post = 0;
		}
		else
		{
			dec->ga_harm += (F)0.0125;
			if (dec->ga_harm > GAMMA_HARM_E) dec->ga_harm = GAMMA_HARM_E;
			dec->ga1_post += (F)0.035;
			if (dec->ga1_post > GAMMA1_PST_E) dec->ga1_post = GAMMA1_PST_E;
			dec->ga2_post += (F)0.0325;
			if (dec->ga2_post > GAMMA2_PST_E) dec->ga2_post = GAMMA2_PST_E;
		}
	}

	/* memcpy(pst_out, dec->synth, sizeof(*dec->synth) * L_FRAME); */

    dec->voicing = 0;
    for (i = 0; i < L_FRAME; i += L_SUBFR)
	{
        poste(&dec->postfilt_s, T0_first, dec->synth + i, ptr_Az, pst_out + i, &dec->sf_voic,
            dec->ga1_post, dec->ga2_post, dec->ga_harm, long_h_st, m_pst, Vad);
        if (dec->sf_voic != 0)
			dec->voicing = dec->sf_voic;
        ptr_Az += m_pst + 1;
    }

    post_process(&dec->postproc_s, pst_out, L_FRAME);

	float_to_sint16(DataBuff, pst_out, L_FRAME);

	return 0;
}

void
g729_line_pack(int* prm, unsigned char *Vout, int* poutlen)
{
	switch (prm[0])
	{
	case G729_TYPE_DONTSEND:
	{
		*poutlen = 0;
		break;
	}
	case G729_TYPE_SID:
	{
		INT32 bits;

		*poutlen = G729_SID_FRAME_SIZE;

		bits =	((prm[1] << 15) & 0x01) | ((prm[2] << 10) & 0x1f) |
				((prm[3] <<  6) & 0x0f) | ((prm[4] <<  1) & 0x1f);
		put16_BE(Vout, bits);

		break;
	}
	case G729_TYPE_VOICE_D:
	{
		INT32 bits;

		*poutlen = G729_VOICE_D_FRAME_SIZE;

		bits =	((prm[1] & 0x0ff) << 24) | ((prm[2] & 0x3ff) << 14) |
				((prm[3] & 0x0ff) <<  6) | ((prm[4] >> 3) & 0x3f);
		put32_BE(Vout, bits);
		Vout += 4;
		bits =	((prm[4] & 0x007) << 29) | ((prm[5] & 0x003) << 27) |
				((prm[6] & 0x03f) << 21) | ((prm[7] & 0x00f) << 17) |
				((prm[8] & 0x1ff) <<  8) | ((prm[9] & 0x003) <<  6) |
				(prm[10] & 0x03f);
		put32_BE(Vout, bits);

		break;
	}
	case G729_TYPE_VOICE:
	{
		INT32 bits;

		*poutlen = G729_VOICE_FRAME_SIZE;

		bits =	((prm[1] & 0xff) << 24) | ((prm[2] & 0x3ff) << 14) |
				((prm[3] & 0xff) <<  6) | ((prm[4] & 0x001) <<  5) |
				((prm[5] >> 8) & 0x1f);
		put32_BE(Vout, bits);
		Vout += 4;
		bits =	((prm[5] & 0xff) << 24) | ((prm[6] & 0x0f) << 20) |
				((prm[7] & 0x7f) << 13) | ((prm[8] & 0x1f) <<  8) |
				((prm[9] >> 5) & 0xff);
		put32_BE(Vout, bits);
		Vout += 4;
		bits =	((prm[9] & 0x1f) << 11) | ((prm[10] & 0x0f) << 7) |
				(prm[11] & 0x7f);
		put16_BE(Vout, bits);

		break;
	}
	case G729_TYPE_VOICE_E:
	{
		INT32 bits;

		*poutlen = G729_VOICE_E_FRAME_SIZE;

		prm++;   /* alignment shift */
		if (!prm[0])
		{	/* forward mode */
			/* first 2 bits are 00 */
			bits =	((prm[ 1] & 0x0ff) << 22) | ((prm[ 2] & 0x3ff) << 12) |
					((prm[ 3] & 0x0ff) <<  4) | ((prm[ 4] & 0x001) <<  3) |
					((prm[ 5] >> 4) & 0x07);
			put32_BE(Vout, bits);
			Vout += 4;
			bits =	((prm[ 5] & 0x00f) << 28) | ((prm[ 6] & 0x07f) << 21) |
					((prm[ 7] & 0x07f) << 14) | ((prm[ 8] & 0x07f) <<  7) |
					((prm[ 9] & 0x07f));
			put32_BE(Vout, bits);
			Vout += 4;
			bits =	((prm[10] & 0x07f) << 25) | ((prm[11] & 0x01f) << 20) |
					((prm[12] & 0x07f) << 13) | ((prm[13] & 0x07f) <<  6) |
					((prm[14] >> 1) & 0x3f);
			put32_BE(Vout, bits);
			Vout += 4;
			bits =	((prm[14] & 0x001) << 23) | ((prm[15] & 0x07f) << 16) |
					((prm[16] & 0x07f) <<  9) | ((prm[17] & 0x07f) <<  2) |
					((0x01)/*DC*/);
			put24_BE(Vout, bits);
		}
		else
		{	/* backward mode */
			/* first 2 bits are 11 */
			bits =	((0x03          ) << 30) |
					((prm[ 1] & 0x00ff) << 22) | ((prm[ 2] & 0x0001) << 21) |
					((prm[ 3] & 0x1fff) <<  8) | ((prm[ 4] >> 2) & 0xff);
			put32_BE(Vout, bits);
			Vout += 4;
			bits =	((prm[ 4] & 0x0003) << 30) | ((prm[ 5] & 0x007f) << 23) |
					((prm[ 6] & 0x007f) << 16) | ((prm[ 7] & 0x007f) <<  9) |
					((prm[ 8] & 0x007f) <<  2) | ((prm[ 9] >> 3) & 0x03);
			put32_BE(Vout, bits);
			Vout += 4;
			bits =	((prm[ 9] & 0x0007) << 29) | ((prm[10] & 0x1fff) << 16) |
					((prm[11] & 0x03ff) <<  6) | ((prm[12] >> 1) & 0x3f);
			put32_BE(Vout, bits);
			Vout += 4;
			bits =	((prm[12] & 0x0001) << 23) | ((prm[13] & 0x007f) << 16) |
					((prm[14] & 0x007f) <<  9) | ((prm[15] & 0x007f) <<  2) |
					((0x01)/*DC*/);
			put24_BE(Vout, bits);
		}
		break;
	}
	default:
	{
		*poutlen = 0;
	}}
}

void
g729_line_unpack(int* prm, unsigned char *Vinp, int Ftyp)
{
	prm++;        /* skip bfi and align prm with packing routine */

	prm[-1] = 0;  /* no frame erasures */
	prm[ 0] = Ftyp;
	
	switch (Ftyp)
	{
	case G729_TYPE_DONTSEND:
	{
		break;
	}
	case G729_TYPE_SID:
	{
		INT32 bits;

		bits = get16_BE(Vinp);

		prm[1] = (bits >> 15) & 0x01;
		prm[2] = (bits >> 10) & 0x1f;
		prm[3] = (bits >>  6) & 0x0f;
		prm[4] = (bits >>  1) & 0x1f;

		break;
	}
	case G729_TYPE_VOICE_D:
	{
		INT32 bits[2];

		bits[0] = get32_BE(Vinp);
		Vinp += 4;
		bits[1] = get32_BE(Vinp);

		prm[ 1] = (bits[0] >> 24) & 0x0ff;
		prm[ 2] = (bits[0] >> 14) & 0x3ff;
		prm[ 3] = (bits[0] >>  6) & 0x0ff;
		prm[ 4] = ((bits[0] & 0x3f) << 3) | ((bits[1] >> 29) & 0x07);
		prm[ 5] = (bits[1] >> 27) & 0x003;
		prm[ 6] = (bits[1] >> 21) & 0x03f;
		prm[ 7] = (bits[1] >> 17) & 0x00f;
		prm[ 8] = (bits[1] >>  8) & 0x1ff;
		prm[ 9] = (bits[1] >>  6) & 0x003;
		prm[10] = (bits[1]        & 0x03f);
		
		break;
	}
	case G729_TYPE_VOICE:
	{
		INT32 bits[3];

		bits[0] = get32_BE(Vinp);
		Vinp += 4;
		bits[1] = get32_BE(Vinp);
		Vinp += 4;
		bits[2] = get16_BE(Vinp);

		prm[ 1] = (bits[0] >> 24) & 0x0ff;
		prm[ 2] = (bits[0] >> 14) & 0x3ff;
		prm[ 3] = (bits[0] >>  6) & 0x0ff;
		prm[ 4] = (bits[0] >>  5) & 0x001;
		prm[ 5] = ((bits[0] & 0x1f) << 8) | ((bits[1] >> 24) & 0xff);
		prm[ 6] = (bits[1] >> 20) & 0x00f;
		prm[ 7] = (bits[1] >> 13) & 0x07f;
		prm[ 8] = (bits[1] >>  8) & 0x01f;
		prm[ 9] = ((bits[1] & 0xff) << 5) | ((bits[2] >> 11) & 0x1f);
		prm[10] = (bits[2] >>  7) & 0x00f;
		prm[11] = (bits[2]        & 0x07f);

		/* check parity and put 1 in parm[5] if parity error */
		prm[4] = check_parity_pitch(prm[3], prm[4]);
		
		break;
	}
	case G729_TYPE_VOICE_E:
	{
		INT32 bits[4];
		int mode;

		bits[0] = get32_BE(Vinp);
		Vinp += 4;
		bits[1] = get32_BE(Vinp);
		Vinp += 4;
		bits[2] = get32_BE(Vinp);
		Vinp += 4;
		bits[3] = get24_BE(Vinp);

		mode = (bits[0] >> 30) & 0x03;
		prm[1] = mode & 1;

		if (mode == 0x00)
		{	/* forward mode */
			prm++;   /* alignment shift */

			prm[ 1] = (bits[0] >> 22) & 0x00ff;
			prm[ 2] = (bits[0] >> 12) & 0x03ff;
			prm[ 3] = (bits[0] >>  4) & 0x00ff;
			prm[ 4] = (bits[0] >>  3) & 0x0001;
			prm[ 5] = ((bits[0] & 0x07) << 4) | ((bits[1] >> 28) & 0x0f);
			prm[ 6] = (bits[1] >> 21) & 0x007f;
			prm[ 7] = (bits[1] >> 14) & 0x007f;
			prm[ 8] = (bits[1] >>  7) & 0x007f;
			prm[ 9] = (bits[1]      ) & 0x007f;
			prm[10] = (bits[2] >> 25) & 0x007f;
			prm[11] = (bits[2] >> 20) & 0x001f;
			prm[12] = (bits[2] >> 13) & 0x007f;
			prm[13] = (bits[2] >>  6) & 0x007f;
			prm[14] = ((bits[2] & 0x3f) << 1) | ((bits[3] >> 23) & 0x01);
			prm[15] = (bits[3] >> 16) & 0x007f;
			prm[16] = (bits[3] >>  9) & 0x007f;
			prm[17] = (bits[3] >>  2) & 0x007f;

			/* check parity and put 1 in prm[4] if parity error */
			prm[4] += (prm[3] >> 1) & 1;
			prm[4] = check_parity_pitch(prm[3], prm[4]);
		}
		else if (mode == 0x03)
		{	/* backward mode */
			prm++;   /* alignment shift */

			prm[ 1] = (bits[0] >> 22) & 0x00ff;
			prm[ 2] = (bits[0] >> 21) & 0x0001;
			prm[ 3] = (bits[0] >>  8) & 0x1fff;
			prm[ 4] = ((bits[0] & 0xff) << 2) | ((bits[1] >> 30) & 0x03);
			prm[ 5] = (bits[1] >> 23) & 0x007f;
			prm[ 6] = (bits[1] >> 16) & 0x007f;
			prm[ 7] = (bits[1] >>  9) & 0x007f;
			prm[ 8] = (bits[1] >>  2) & 0x007f;
			prm[ 9] = ((bits[1] & 0x03) << 3) | ((bits[2] >> 29) & 0x07);
			prm[10] = (bits[2] >> 16) & 0x1fff;
			prm[11] = (bits[2] >>  6) & 0x03ff;
			prm[12] = ((bits[2] & 0x3f) << 1) | ((bits[3] >> 23) & 0x01);
			prm[13] = (bits[3] >> 16) & 0x007f;
			prm[14] = (bits[3] >>  9) & 0x007f;
			prm[15] = (bits[3] >>  2) & 0x007f;

			/* check parity and put 1 in prm[4] if parity error */
			prm[2] += (prm[1] >> 1) & 1;
			prm[2] = check_parity_pitch(prm[1], prm[2]);
		}
		else
		{	/* frame erased */
			prm[-1] = 1;	/* frame erased */
			prm[ 0] = G729_TYPE_DONTSEND;
		}
		break;
	}
	default:
	{
		prm[-1] = 1;	/* frame erased */
		prm[ 0] = G729_TYPE_DONTSEND;
	}}
}

__inline__ static void
put32_BE(unsigned char *out, INT32 bits)
{
	out[0] = bits >> 24;
	out[1] = bits >> 16;
	out[2] = bits >> 8;
	out[3] = bits;
}

__inline__ static INT32
get32_BE(unsigned char *in)
{
	return (in[0] << 24) | (in[1] << 16) | (in[2] << 8) | in[3];
}

__inline__ static void
put24_BE(unsigned char *out, INT32 bits)
{
	out[0] = bits >> 16;
	out[1] = bits >> 8;
	out[2] = bits;
}

__inline__ static INT32
get24_BE(unsigned char *in)
{
	return (in[0] << 16) | (in[1] << 8) | in[2];
}

__inline__ static void
put16_BE(unsigned char *out, INT32 bits)
{
	out[0] = bits >> 8;
	out[1] = bits;
}

__inline__ static INT32
get16_BE(unsigned char *in)
{
	return (in[0] << 8) | in[1];
}

__inline__ static void
float_to_sint16(INT16 *dst, FLOAT *src, int len)
{
	for ( ; len; --len, ++dst, ++src)
	{	/* round and convert to int  */
		FLOAT t = *src;
		if (t >= (F)0.0)
			t += (F)0.5;
		else
			t -= (F)0.5;

		if (t > (F)32767.0)
			*dst = 32767;
		else if (t < (F)-32768.0)
			*dst = -32768;
		else
			*dst = (INT16) t;
	}
}