lsftopc.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
          /*LINTLIBRARY*/
          /*PROTOLIB1*/

#include "main.h"
#include "lsftopc.h"
//#include <math.h>
#include <stdio.h>


/*************************************************************************
*
* NAME
*       LSFtoPC
*
* FUNCTION
*
*       convert line spectral frequencies to predictor coefficients
*
* SYNOPSIS
*
*       LSFtoPC(f, pc, frame_num)
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       f               fxpt_16 i       lsp frequencies
*       pc              fxpt_16 o       LPC predictor coefficients
*       frame_num       int     i       Frame number
*
*
***************************************************************************
*
* DESCRIPTION
*
*       LSFtoPC converts line spectral frequencies to predictor
*       coefficients.
*
*       The analysis filter may be reconstructed:
*
*               A(z) = 1/2 [ P(z) + Q(z) ]
*
*       CELP's LPC predictor coefficient convention is:
*              p+1         -(i-1)
*       A(z) = SUM   a   z          where a  = +1.0
*              i=1    i                    1
*
***************************************************************************/
#if 0
void LSFtoPC(
fxpt_16 lsf[ORDER],             /* 0.15 format */
fxpt_16 pc[ORDER+1],            /* 2.13 format */
int     frame_num)
{
  int   i, j;
  fxpt_32       a[ORDER/2 +1], a1[ORDER/2 +1], a2[ORDER/2 +1];  /* 15.16 */
  fxpt_32       b[ORDER/2 +1], b1[ORDER/2 +1], b2[ORDER/2 +1];  /* 15.16 */
  fxpt_16       p[ORDER/2], q[ORDER/2]; /* 2.13 format */

  /*  Check input for ill-conditioned cases */
  if ((lsf[0] <= 0) || (lsf[0] >= 16384)) {
    CELP_PRINTF("LSFtoPC: LSFs out of bounds; lsf[0] = %d at frame %d\n",
                lsf[0], frame_num);
  }

  for (i=1; i<ORDER; i++)       {
    if(lsf[i] <= lsf[i-1])
      CELP_PRINTF("LSFtoPC: nonmonotonic LSFs at frame %d\n", frame_num);
    if ((lsf[i] <= 0) || (lsf[i] >= 16384))
      CELP_PRINTF("LSFtoPC: LSFs out of bounds; lsf[%d] = %d at frame %d\n",
                  i, lsf[i], frame_num);
  }

  /*  Initialization */
  /*    pi = 4*atan(1.0);*/


  for (i = 0; i <= ORDER/2; i++) {
    a[i] = 0;
    a1[i] = 0;
    a2[i] = 0;
    b[i] = 0;
    b1[i] = 0;
    b2[i] = 0;
  }

  /*  LSF filter parameters */
  for (i = 0; i < ORDER/2; i++) {
    p[i] = fxpt_mult16_round(-16384,
                             fxpt_cos16(fxpt_shl16_fast(lsf[2*i], 1)), 15);

    q[i] = fxpt_mult16_round(-16384,
                             fxpt_cos16(fxpt_shl16_fast(lsf[2*i+1], 1)), 15);
  }

  /*  Impulse response of analysis filter */
  for (i = 0; i <= ORDER; i++) {

    switch (i) {
    case 0:
      a[0] = 65536;
      b[0] = 65536;
      break;
    case 1:
      a[0] = 65536;
      b[0] = -65536;
      break;
    default:
      a[0] = 0;
      b[0] = 0;
    }

    for(j = 0; j < ORDER/2; j++) {

      a[j+1] = fxpt_add32(a[j], a2[j]);
      a[j+1] = fxpt_add32(a[j+1], fxpt_mult64_fix(a1[j], p[j], 13));

      b[j+1] = fxpt_add32(b[j], b2[j]);
      b[j+1] = fxpt_add32(b[j+1], fxpt_mult64_fix(b1[j], q[j], 13));

      a2[j] = a1[j];
      a1[j] = a[j];
      b2[j] = b1[j];
      b1[j] = b[j];
    }

    if (i > 0)
      pc[i-1] =
        fxpt_saturate16(fxpt_negate32(fxpt_shr32_fast(fxpt_add32(a[ORDER/2],
                                                                 b[ORDER/2]),
                                                      4)));
  }

  /*  Convert to CELPs PC array configuration */
  for(i = 9; i >= 0; i--)
    pc[i+1] = fxpt_negate16(pc[i]);

  pc[0] = 8192;         /* 1.0 in 2.13 format */
}
#endif


#if 0

void LSFtoPC(
fxpt_16 lsf[ORDER],             /* 0.15 format */
fxpt_16 pc[ORDER+1]             /* 2.13 format */
)
{
  int   i, j;

  /* a_next, b_next, a1, a2, b1, b2 are:  15.16 */
  fxpt_32 a_next, b_next;
  fxpt_32 *A1, *A2, *B1, *B2, *AB_temp;
  fxpt_32 a1[ORDER/2 +1] = {65536, 65536, 65536, 65536, 65536,     0};
  fxpt_32 b1[ORDER/2 +1] = {65536, 65536, 65536, 65536, 65536,     0};
  fxpt_32 a2[ORDER/2 +1] = { 0 };
  fxpt_32 b2[ORDER/2 +1] = { 0 };

  fxpt_16       p[ORDER/2], q[ORDER/2]; /* 2.13 format */


#ifdef PRINT_DEBUG_MESSAGES
  /*  Check input for ill-conditioned cases */
  if ((lsf[0] <= 0) || (lsf[0] >= 16384)) {
    CELP_PRINTF("LSFtoPC: LSFs out of bounds; lsf[0] = %d in frame\n",
                lsf[0]);
  }

  for (i=1; i<ORDER; i++)       {
    if(lsf[i] <= lsf[i-1])
      CELP_PRINTF("LSFtoPC: nonmonotonic LSFs in frame\n");
    if ((lsf[i] <= 0) || (lsf[i] >= 16384))
      CELP_PRINTF("LSFtoPC: LSFs out of bounds; lsf[%d] = %d in frame\n",
                  i, lsf[i]);
  }
#endif


  /*  LSF filter parameters */
  for (i = 0; i < ORDER/2; i++) {
    p[i] = fxpt_mult16_round(-16384,
                             fxpt_cos16(fxpt_shl16_fast(lsf[2*i], 1)), 15);

    q[i] = fxpt_mult16_round(-16384,
                             fxpt_cos16(fxpt_shl16_fast(lsf[2*i+1], 1)), 15);
  }

  /*  Impulse response of analysis filter */
  a_next = 65536;
  b_next = -65536;

  pc[0] = 8192;         /* 1.0 in 2.13 format */


  A1 = a1;
  B1 = b1;
  A2 = a2;
  B2 = b2;

  for (i = 1; i <= ORDER; i++) {

    if (i != 1) {
      a_next = 0;
      b_next = 0;
    }

    for(j = 0; j < ORDER/2; j++) {

      fxpt_32 a_val = a_next;
      fxpt_32 b_val = b_next;

      a_next = a_val + A2[j] + fxpt_mult64_fix(A1[j], p[j], 13);
      b_next = b_val + B2[j] + fxpt_mult64_fix(B1[j], q[j], 13);

      A2[j] = a_val;
      B2[j] = b_val;
    }

    AB_temp = A2;
    A2 = A1;
    A1 = AB_temp;

    AB_temp = B2;
    B2 = B1;
    B1 = AB_temp;


    pc[i] = fxpt_saturate16(fxpt_shr32_fast(a_next + b_next, 4));
  }

}

#endif

void LSFtoPC(
fxpt_16 lsf[ORDER],             /* 0.15 format */
fxpt_16 pc[ORDER+1]             /* 2.13 format */
)
{
  int   i, j;

  /* a_next, b_next, a1, a2, b1, b2 are:  15.16 */
  fxpt_32 a_next, b_next;
  fxpt_32 *A1, *A2, *B1, *B2, *AB_temp;
  fxpt_32 a1[ORDER/2 +1] = {65536, 65536, 65536, 65536, 65536,     0};
  fxpt_32 b1[ORDER/2 +1] = {65536, 65536, 65536, 65536, 65536,     0};
  fxpt_32 a2[ORDER/2 +1] = { 0 };
  fxpt_32 b2[ORDER/2 +1] = { 0 };

//  fxpt_16       p[ORDER/2], q[ORDER/2]; /* 2.13 format */
  fxpt_32       p[ORDER/2], q[ORDER/2]; /* 2.29 format */


#ifdef PRINT_DEBUG_MESSAGES
  /*  Check input for ill-conditioned cases */
  if ((lsf[0] <= 0) || (lsf[0] >= 16384)) {
    CELP_PRINTF("LSFtoPC: LSFs out of bounds; lsf[0] = %d in frame\n",
                lsf[0]);
  }

  for (i=1; i<ORDER; i++)       {
    if(lsf[i] <= lsf[i-1])
      CELP_PRINTF("LSFtoPC: nonmonotonic LSFs in frame\n");
    if ((lsf[i] <= 0) || (lsf[i] >= 16384))
      CELP_PRINTF("LSFtoPC: LSFs out of bounds; lsf[%d] = %d in frame\n",
                  i, lsf[i]);
  }
#endif


  /*  LSF filter parameters */
  for (i = 0; i < ORDER/2; i++) {
    p[i] = -(fxpt_shr32_fast(fxpt_cos32(fxpt_shl32_fast(lsf[2*i], 17)),1));
    q[i] = -(fxpt_shr32_fast(fxpt_cos32(fxpt_shl32_fast(lsf[2*i+1], 17)),1));
  }

  /*  Impulse response of analysis filter */
  a_next = 65536;
  b_next = -65536;

  pc[0] = 8192;         /* 1.0 in 2.13 format */


  A1 = a1;
  B1 = b1;
  A2 = a2;
  B2 = b2;

  for (i = 1; i <= ORDER; i++) {

    if (i != 1) {
      a_next = 0;
      b_next = 0;
    }

    for(j = 0; j < ORDER/2; j++) {

      fxpt_32 a_val = a_next;
      fxpt_32 b_val = b_next;

      a_next = a_val + A2[j] + fxpt_mult64_fix(A1[j], p[j], 29);
      b_next = b_val + B2[j] + fxpt_mult64_fix(B1[j], q[j], 29);

      A2[j] = a_val;
      B2[j] = b_val;
    }

    AB_temp = A2;
    A2 = A1;
    A1 = AB_temp;

    AB_temp = B2;
    B2 = B1;
    B1 = AB_temp;


    pc[i] = fxpt_saturate16_round(a_next + b_next, 4);
  }

}