/*********************************************************************
**
** Name: deqnt_msvq()
**
** Description:
**
** Dequantization using codebook indices with multi-stages
**
** Arguments:
**
** int16_t qout[] ---- (output) quantized data (Q15/Q17)
** int16_t codebook[] ---- codebooks, cb[0..numStages-1] (Q15/Q17)
** int16_t tos ---- the number of stages
** short *index ---- codebook index
**
** Return value: None
**
***********************************************************************/
void deqnt_msvq(int16_t qout[], const int16_t codebook[], int16_t tos,
const int16_t cb_size[], int16_t index[], int16_t dim)
{
register int16_t i;
const int16_t *cdbk_ptr;
int16_t temp;
int32_t L_temp;
/* ====== Clear output ====== */
v_zap(qout, dim);
/* ====== Add each stage ====== */
cdbk_ptr = codebook;
for (i = 0; i < tos; i++) {
/* v_add(qout, cdbk_ptr + index[i]*dim, dim); */
L_temp = melpe_L_mult(index[i], dim);
L_temp = melpe_L_shr(L_temp, 1);
temp = melpe_extract_l(L_temp);
v_add(qout, cdbk_ptr + temp, dim);
/* cdbk_ptr += cb_size[i] * dim; */
L_temp = melpe_L_mult(cb_size[i], dim);
L_temp = melpe_L_shr(L_temp, 1);
temp = melpe_extract_l(L_temp);
cdbk_ptr += temp;
}
}
/********************************************************************
**
** Function: lspVQ ()
**
** Description:
** Vector quantizes a set of int term filter coefficients
** using a multi-stage M-L tree search algorithm.
**
** Arguments:
**
** int16_t target[] : the target coefficients to be quantized (Q15/Q17)
** int16_t weight[] : weights for mse calculation (Q11)
** int16_t qout[] : the output array (Q15/Q17)
** int16_t codebook[] : codebooks, cb[0..numStages-1] (Q15/Q17)
** int16_t tos : the number of stages
** int16_t cb_size[] : codebook size (multistages)
** int16_t cb_index[] : codebook indeces; cb_index[0..numStages-1]
** (output)
** int16_t dim
** BOOLEAN flag
**
** Return value: None
**
***********************************************************************/
static void lspVQ(int16_t target[], int16_t weight[], int16_t qout[],
const int16_t codebook[], int16_t tos,
const int16_t cb_size[], int16_t cb_index[],
int16_t dim, BOOLEAN flag)
{
register int16_t i, entry;
register int16_t c1, s1;
const int16_t *cdbk_ptr, *cdbk_ptr2, *ptr1;
int16_t index[LSP_VQ_CAND][LSP_VQ_STAGES];
int16_t nextIndex[LSP_VQ_CAND][LSP_VQ_STAGES];
int16_t ncPrev;
int16_t cand[LSP_VQ_CAND][2 * LPC_ORD];
int16_t max_dMin, dMin[LSP_VQ_CAND], distortion;
int16_t *cand_target;
int32_t L_temp;
int16_t ptr_offset = 0;
int16_t temp1, temp2;
/*==================================================================*
* Initialize the data before starting the tree search. *
* - the number of candidates from the "previous" stage is set *
* to 1 since there is no previous stage! *
* - the candidate vector from the previous stage is set to zero *
* - the list of indeces for each candidate is set to 1 *
*==================================================================*/
for (i = 0; i < LSP_VQ_CAND; i++) {
v_zap(cand[i], dim);
v_zap(index[i], LSP_VQ_STAGES);
v_zap(nextIndex[i], LSP_VQ_STAGES);
}
cand_target = v_get(dim);
ncPrev = 1;
/*==================================================================*
* Now we start the search: *
* For each stage *
* For each candidate from the previous stage *
* For each entry in the current stage codebook *
* * add codebook vector to current candidate *
* * compute the distortion with the target *
* * retain candidate if one of the best so far *
*==================================================================*/
cdbk_ptr = codebook;
/* An observation for lspVQ() shows that if "flag" is FALSE, then we only */
/* need to keep track of the best one (instead of the best LSP_VQ_CAND, */
/* 8) cand[][] and index[][]. This has significant influence on */
/* execution speed. */
for (s1 = 0; s1 < tos; s1++) {
/* set the distortions to huge values */
fill(dMin, SW_MAX, LSP_VQ_CAND);
max_dMin = SW_MAX;
/* Loop for each previous candidate selected, and try each entry */
for (c1 = 0; c1 < ncPrev; c1++) {
ptr_offset = 0;
/* cand_target[] is the target vector with cand[c1] removed. */
/* This moves some operations from the for-entry loop here. */
/* save_saturation(); */
v_equ(cand_target, target, dim);
v_sub(cand_target, cand[c1], dim);
/* restore_saturation(); */
for (entry = 0; entry < cb_size[s1]; entry++) {
ptr1 = cdbk_ptr + ptr_offset; /* Pointer arithmetics. */
/* compute the distortion */
distortion =
WeightedMSE(dim, weight, ptr1, cand_target,
max_dMin);
/*======================================================*
* If the error for this entry is less than the worst *
* retained candidate so far, keep it. Note that the *
* error list is maintained in order of best to worst. *
*=======================================================*/
if (distortion < max_dMin) {
max_dMin =
InsertCand(c1, s1, dMin, distortion,
entry, nextIndex[0],
index[0]);
}
ptr_offset = melpe_add(ptr_offset, dim);
}
}
/* At this point ptr_offset is (cb_size[s1]*dim). */
/*==================================================================*
* Compute the number of candidate vectors which we kept for the *
* next stage. Note that if the size of the stages is less than *
* the number of candidates, we build them up using all entries *
* until we have kept numCand candidates. On the other hand, if *
* flag is FALSE and (s1 == tos - 1), then we only need to use *
* ncPrev = 1 because we only copy the best candidate before *
* exiting lspVQ(). *
*==================================================================*/
if (!flag && s1 == tos - 1)
ncPrev = 1;
else {
/* ncPrev = Min(ncPrev*cb_size[s1], LSP_VQ_CAND) for regular */
/* loops, and ncPrev = Min(ncPrev*cb_size[s1], LSP_INP_CAND) for */
/* the last lap. Explanations are available near the end of this *//* function. */
L_temp = melpe_L_mult(ncPrev, cb_size[s1]);
L_temp = melpe_L_shr(L_temp, 1);
temp1 = melpe_extract_l(L_temp); /* temp1 = ncPrev * cb_size[s1] */
if (s1 == tos - 1)
temp2 = LSP_INP_CAND;
else
temp2 = LSP_VQ_CAND;
if (temp1 < temp2)
ncPrev = temp1;
else
ncPrev = temp2;
}
/*==================================================================*
* We now have the best indices for the stage just completed, so *
* compute the new candidate vectors for the next stage... *
*==================================================================*/
for (c1 = 0; c1 < ncPrev; c1++) {
v_zap(cand[c1], dim);
cdbk_ptr2 = codebook;
temp1 = melpe_add(s1, 1);
v_equ(index[c1], nextIndex[c1], temp1);
for (i = 0; i < temp1; i++) {
/* v_add(cand[c1], cdbk_ptr2 + index[c1][i]*dim, dim); */
L_temp = melpe_L_mult(index[c1][i], dim);
L_temp = melpe_L_shr(L_temp, 1);
temp2 = melpe_extract_l(L_temp);
ptr1 = cdbk_ptr2 + temp2;
v_add(cand[c1], ptr1, dim);
/* cdbk_ptr2 += cb_size[i]*dim; */
L_temp = melpe_L_mult(cb_size[i], dim);
L_temp = melpe_L_shr(L_temp, 1);
temp2 = melpe_extract_l(L_temp);
cdbk_ptr2 += temp2;
}
}
/* cdbk_ptr += cb_size[s1] * dim; */
cdbk_ptr += ptr_offset;
}
/* Copy best candidate and indices into output. Here we use temp1 and */
/* temp2 to compute (c1*tos) and (c1*dim). */
/* Originally this function copies LSP_VQ_CAND (== 8) vectors before */
/* exiting if flag is TRUE. However, in the calling environment of */
/* lspVQ() when flag is passed in as TRUE, we only used LSP_INP_CAND */
/* (== 5). */
temp1 = 0;
temp2 = 0;
for (i = 0; i < ncPrev; i++) {
v_equ(&(cb_index[temp1]), index[i], tos);
v_equ(&qout[temp2], cand[i], dim);
temp1 = melpe_add(temp1, tos);
temp2 = melpe_add(temp2, dim);
}
v_free(cand_target);
}
void find_harm(int16_t input[], int16_t fsmag[], int16_t pitch,
int16_t num_harm, int16_t length)
{
register int16_t i, j, k;
int16_t find_hbuf[2 * FFTLENGTH];
int16_t iwidth, i2;
int16_t fwidth, mult_fwidth, shift, max;
int32_t *L_fsmag;
int32_t L_temp, L_max;
int64_t avg;
int16_t temp1, temp2;
L_fsmag = L_v_get(num_harm);
/* Find normalization factor of frame and scale input to maximum */
/* precision. */
max = 0;
for (i = 0; i < length; i++) {
temp1 = melpe_abs_s(input[i]);
if (temp1 > max)
max = temp1;
}
shift = melpe_norm_s(max);
/* initialize fsmag */
fill(fsmag, ONE_Q13, num_harm);
/* Perform peak-picking on FFT of input signal */
/* Calculate FFT of complex signal in scratch buffer */
v_zap(find_hbuf, 2 * FFTLENGTH);
for (i = 0; i < length; i++) {
find_hbuf[i] = melpe_shl(input[i], shift);
}
rfft(find_hbuf, FFTLENGTH);
/* Implement pitch dependent staircase function */
/* Harmonic bin width fwidth = Q6 */
fwidth = melpe_shr(melpe_divide_s(FFTLENGTH, pitch), 2);
/* iwidth = (int) fwidth */
iwidth = melpe_shr(fwidth, 6);
/* Originally here we have a check for setting iwidth to be at least 2. */
/* This is not necessary because FFTLENGTH (== 512) divided by PITCHMAX */
/* (== 160) will be at least 3. */
i2 = melpe_shr(iwidth, 1);
/* if (num_harm > 0.25*pitch) num_harm = 0.25*pitch */
/* temp1 = 0.25*pitch in Q0 */
temp1 = melpe_shr(pitch, 9);
if (num_harm > temp1)
num_harm = temp1;
/* initialize avg to make sure that it is non-zero */
mult_fwidth = fwidth; /* (k + 1)*fwidth, Q6 */
for (k = 0; k < num_harm; k++) {
/* i = ((k + 1) * fwidth) - i2 + 0.5 *//* Start at peak-i2 */
i = melpe_sub(melpe_shr(melpe_add(mult_fwidth, X05_Q6), 6), i2);
/* Calculate magnitude squared of coefficients */
L_max = 0;
for (j = 0; j < iwidth; j++) {
/* temp1 = find_hbuf[2*(j + i)]; */
/* temp2 = find_hbuf[2*(j + i) + 1]; */
temp2 = melpe_add(i, j);
temp1 = find_hbuf[2 * temp2];
temp2 = find_hbuf[2 * temp2 + 1];
L_temp =
melpe_L_add(melpe_L_mult(temp1, temp1), melpe_L_mult(temp2, temp2));
L_max = Max(L_max, L_temp);
}
L_fsmag[k] = L_max;
mult_fwidth = melpe_add(mult_fwidth, fwidth);
}
/* Normalize Fourier series values to average magnitude */
avg = 1;
for (k = 0; k < num_harm; k++)
avg = melpe_L40_add(avg, L_fsmag[k]);
temp1 = melpe_norm32(avg);
L_temp = (int32_t) melpe_L40_shl(avg, temp1); /* man. of avg */
temp1 = melpe_sub(31, temp1); /* exp. of avg */
/* now compute num_harm/avg. avg = L_temp(Q31) x 2^temp1 */
temp2 = melpe_shl(num_harm, 10);
/* num_harm = temp2(Q15) x 2^5 */
temp2 = melpe_divide_s(temp2, melpe_extract_h(L_temp));
/* now think fs as Q15 x 2^31. The constant below should be 31 */
/* but consider Q5 for num_harm and fsmag before sqrt Q11, and */
/* add two guard bits 30 = 31 + 5 - 4 - 2 */
shift = melpe_sub(30, temp1);
/* the sentence above is just for clarity. temp1 = sub(31, temp1) */
/* and shift = sub(30, temp1) can be shift = sub(temp1, 1) */
for (i = 0; i < num_harm; i++) {
/* temp1 = num_harm/(avg + 0.0001) */
/* fsmag[i] = sqrt(temp1*fsmag[i]) */
temp1 = melpe_extract_h(melpe_L_shl(L_fsmag[i], shift));
temp1 = melpe_extract_h(melpe_L_shl(melpe_L_mult(temp1, temp2), 2)); /* Q11 */
fsmag[i] = sqrt_Q15(temp1);
}
v_free(L_fsmag);
}
void main(int argc, char **argv)
{
void parse(int argc, char **argv);
int length, frame, eof_reached;
int num_frames = 0;
float speech_in[FRAME];
float speech_out[FRAME];
static struct melp_param melp_par; /* melp parameters */
unsigned int chbuf[CHSIZE];
FILE *fp_in, *fp_out;
/* Print user message */
printf("\n2.4 kb/s Proposed Federal Standard MELP speech coder\n");
printf(" C simulation, version 1.2\n\n");
/* Get input parameters from command line */
parse(argc, argv);
/* Open input, output, and parameter files */
if (( fp_in = fopen(in_name,"rb")) == NULL ) {
printf(" ERROR: cannot read file %s.\n",in_name);
exit(1);
}
if (( fp_out = fopen(out_name,"wb")) == NULL ) {
printf(" ERROR: cannot write file %s.\n",out_name);
exit(1);
}
/* Check length of channel input if needed */
if (melpmode == SYNTHESIS) {
fseek(fp_in,0L,2);
length = ftell(fp_in);
rewind(fp_in);
num_frames = 0.5 + length * (8.0 / NUM_CH_BITS) * (6.0/32);
}
/* Initialize MELP analysis and synthesis */
if (melpmode != SYNTHESIS)
melp_ana_init();
if (melpmode != ANALYSIS)
melp_syn_init();
/* Run MELP coder on input signal */
frame = 0;
melp_par.chptr = chbuf;
melp_par.chbit = 0;
eof_reached = 0;
while (eof_reached == 0) {
/* Perform MELP analysis */
if (melpmode != SYNTHESIS) {
/* read input speech */
length = readbl(speech_in,fp_in,FRAME);
if (length < FRAME) {
v_zap(&speech_in[length],FRAME-length);
eof_reached = 1;
}
/* Run MELP analyzer */
if (melpmode == ANA_SYN) {
/* reset pointers to short channel buffer */
melp_par.chptr = chbuf;
melp_par.chbit = 0;
}
melp_ana(speech_in,&melp_par);
/* Write channel output if needed */
if (melpmode == ANALYSIS && melp_par.chbit == 0) {
fwrite((void *) chbuf,sizeof(int),melp_par.chptr-chbuf,fp_out);
/* reset pointer to short channel buffer */
melp_par.chptr = chbuf;
}
if (melp_par.chptr >= &chbuf[CHSIZE] && melp_par.chbit > 0) {
printf("\nERROR: Ran out of channel buffer memory.\n");
exit(1);
}
}
/* Perform MELP synthesis (skip first frame) */
if (melpmode != ANALYSIS) {
if (melpmode == ANA_SYN) {
/* reset pointers to short channel buffer */
melp_par.chptr = chbuf;
melp_par.chbit = 0;
}
/* Read channel input if needed */
if (melpmode == SYNTHESIS && melp_par.chbit == 0) {
fread((void *) chbuf,sizeof(int),CHSIZE,fp_in);
/* reset pointer to short channel buffer */
melp_par.chptr = chbuf;
}
melp_syn(&melp_par,speech_out);
if (frame > 0)
writebl(speech_out,fp_out,FRAME);
}
frame++;
if (melpmode == SYNTHESIS) {
if (frame >= num_frames)
eof_reached = 1;
}
}
/* Write channel output if needed */
if (melpmode == ANALYSIS) {
if (melp_par.chbit > 0)
fwrite((void *) chbuf,sizeof(int),melp_par.chptr-chbuf+1,fp_out);
else
fwrite((void *) chbuf,sizeof(int),melp_par.chptr-chbuf,fp_out);
}
fclose(fp_in);
fclose(fp_out);
}
void melp_ana_init()
{
int j;
bpvc_ana_init(FRAME,PITCHMIN,PITCHMAX,NUM_BANDS,2,MINLENGTH);
pitch_ana_init(PITCHMIN,PITCHMAX,FRAME,LPF_ORD,MINLENGTH);
p_avg_init(PDECAY,DEFAULT_PITCH,3);
v_zap(speech,IN_BEG+FRAME);
pitch_avg=DEFAULT_PITCH;
fill(fpitch,DEFAULT_PITCH,2);
v_zap(lpfsp_del,LPF_ORD);
/* Initialize multi-stage vector quantization (read codebook) */
vq_par.num_best = MSVQ_M;
vq_par.num_stages = 4;
vq_par.dimension = 10;
/*
* Allocate memory for number of levels per stage and indices
* and for number of bits per stage
*/
MEM_ALLOC(MALLOC,vq_par.num_levels,vq_par.num_stages,int);
MEM_ALLOC(MALLOC,vq_par.indices,vq_par.num_stages,int);
MEM_ALLOC(MALLOC,vq_par.num_bits,vq_par.num_stages,int);
vq_par.num_levels[0] = 128;
vq_par.num_levels[1] = 64;
vq_par.num_levels[2] = 64;
vq_par.num_levels[3] = 64;
vq_par.num_bits[0] = 7;
vq_par.num_bits[1] = 6;
vq_par.num_bits[2] = 6;
vq_par.num_bits[3] = 6;
vq_par.cb = msvq_cb;
/* Scale codebook to 0 to 1 */
v_scale(vq_par.cb,(2.0/FSAMP),3200);
/* Initialize Fourier magnitude vector quantization (read codebook) */
fs_vq_par.num_best = 1;
fs_vq_par.num_stages = 1;
fs_vq_par.dimension = NUM_HARM;
/*
* Allocate memory for number of levels per stage and indices
* and for number of bits per stage
*/
MEM_ALLOC(MALLOC,fs_vq_par.num_levels,fs_vq_par.num_stages,int);
MEM_ALLOC(MALLOC,fs_vq_par.indices,fs_vq_par.num_stages,int);
MEM_ALLOC(MALLOC,fs_vq_par.num_bits,fs_vq_par.num_stages,int);
fs_vq_par.num_levels[0] = FS_LEVELS;
fs_vq_par.num_bits[0] = FS_BITS;
fs_vq_par.cb = fsvq_cb;
/* Initialize fixed MSE weighting and inverse of weighting */
vq_fsw(w_fs, NUM_HARM, 60.0);
/* Pre-weight codebook (assume single stage only) */
if (fsvq_weighted == 0)
{
fsvq_weighted = 1;
for (j = 0; j < fs_vq_par.num_levels[0]; j++)
window(&fs_vq_par.cb[j*NUM_HARM],w_fs,&fs_vq_par.cb[j*NUM_HARM],
NUM_HARM);
}
}