audiostuff: spandsp-0.0.3/spandsp-0.0.3/src/gsm0610

comparison spandsp-0.0.3/spandsp-0.0.3/src/gsm0610_rpe.c @ 5:f762bf195c4b

import spandsp-0.0.3

author	Peter Meerwald <pmeerw@cosy.sbg.ac.at>
date	Fri, 25 Jun 2010 16:00:21 +0200
parents
children

comparison

equal deleted inserted replaced

-:26cd8f1ef0b1
+:f762bf195c4b
+/*
+* SpanDSP - a series of DSP components for telephony
+*
+* gsm0610_rpe.c - GSM 06.10 full rate speech codec.
+*
+* Written by Steve Underwood <steveu@coppice.org>
+*
+* Copyright (C) 2006 Steve Underwood
+*
+* All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2, as
+* published by the Free Software Foundation.
+*
+* This program 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 General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program; if not, write to the Free Software
+* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*
+* This code is based on the widely used GSM 06.10 code available from
+* http://kbs.cs.tu-berlin.de/~jutta/toast.html
+*
+* $Id: gsm0610_rpe.c,v 1.13 2006/11/19 14:07:24 steveu Exp $
+*/
+/*! \file */
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+#include <assert.h>
+#include <inttypes.h>
+#if defined(HAVE_TGMATH_H)
+#include <tgmath.h>
+#endif
+#if defined(HAVE_MATH_H)
+#include <math.h>
+#endif
+#include <stdlib.h>
+#include "spandsp/telephony.h"
+#include "spandsp/dc_restore.h"
+#include "spandsp/gsm0610.h"
+#include "gsm0610_local.h"
+/* 4.2.13 .. 4.2.17  RPE ENCODING SECTION */
+/* 4.2.13 */
+static void weighting_filter(const int16_t *e,     // signal [-5..0.39.44] IN
+int16_t x[40])
+{
+#if defined(__GNUC__)  &&  defined(__i386__)
+/* Table 4.4   Coefficients of the weighting filter */
+/* This must be padded to a multiple of 4 for MMX to work */
+static const int16_t gsm_H[12] =
+{
+-134, -374, 0, 2054, 5741, 8192, 5741, 2054, 0, -374, -134, 0
+};
+__asm__ __volatile__(
+" emms;\n"
+" addl $-10,%%ecx;\n"
+" movl $0x1000,%%eax;\n"
+" movd %%eax,%%mm5;\n"              /* for rounding */
+" movq %[gsm_H],%%mm1;\n"
+" movq %[gsm_H8],%%mm2;\n"
+" movq %[gsm_H16],%%mm3;\n"
+" xorl %%esi,%%esi;\n"
+" .p2align 2;\n"
+"1:;\n"
+" movq (%%ecx,%%esi,2),%%mm0;\n"
+" pmaddwd %%mm1,%%mm0;\n"
+" movq 8(%%ecx,%%esi,2),%%mm4;\n"
+" pmaddwd %%mm2,%%mm4;\n"
+" paddd %%mm4,%%mm0;\n"
+" movq 16(%%ecx,%%esi,2),%%mm4;\n"
+" pmaddwd %%mm3,%%mm4;\n"
+" paddd %%mm4,%%mm0;\n"
+" movq %%mm0,%%mm4;\n"
+" punpckhdq %%mm0,%%mm4;\n"         /* mm4 has high int32 of mm0 dup'd */
+" paddd %%mm4,%%mm0;\n"
+" paddd %%mm5,%%mm0;\n"             /* Add for roundoff */
+" psrad $13,%%mm0;\n"
+" packssdw %%mm0,%%mm0;\n"
+" movd %%mm0,%%eax;\n"              /* eax has result */
+" movw %%ax,(%%edi,%%esi,2);\n"
+" incl %%esi;\n"
+" cmpl $39,%%esi;\n"
+" jle 1b;\n"
+" emms;\n"
+:
+: "c" (e), "D" (x), [gsm_H] "X" (*((int64_t *) gsm_H)), [gsm_H8] "X" (*((int64_t *) (gsm_H + 4))), [gsm_H16] "X" (*((int64_t *) (gsm_H + 8)))
+: "eax", "edx", "esi"
+);
+#else
+int32_t L_result;
+int k;
+/* The coefficients of the weighting filter are stored in a table
+(see table 4.4).  The following scaling is used:
+H[0..10] = integer(real_H[0..10] * 8192);
+*/
+/* Initialization of a temporary working array wt[0...49] */
+/* for (k =  0;  k <=  4;  k++) wt[k] = 0;
+* for (k =  5;  k <= 44;  k++) wt[k] = *e++;
+* for (k = 45;  k <= 49;  k++) wt[k] = 0;
+*
+*  (e[-5..-1] and e[40..44] are allocated by the caller,
+*  are initially zero and are not written anywhere.)
+*/
+e -= 5;
+/* Compute the signal x[0..39] */
+for (k = 0;  k < 40;  k++)
+{
+L_result = 8192 >> 1;
+/* for (i = 0; i <= 10; i++)
+* {
+*      L_temp   = gsm_l_mult(wt[k + i], gsm_H[i]);
+*      L_result = gsm_l_add(L_result, L_temp);
+* }
+*/
+#undef STEP
+#define STEP(i,H) (e[k + i] * (int32_t) H)
+/* Every one of these multiplications is done twice,
+but I don't see an elegant way to optimize this.
+Do you?
+*/
+L_result += STEP( 0,  -134);
+L_result += STEP( 1,  -374);
+// += STEP( 2,  0   );
+L_result += STEP( 3,  2054);
+L_result += STEP( 4,  5741);
+L_result += STEP( 5,  8192);
+L_result += STEP( 6,  5741);
+L_result += STEP( 7,  2054);
+// += STEP( 8,  0   );
+L_result += STEP( 9,  -374);
+L_result += STEP(10,  -134);
+/* 2 adds vs. >> 16 => 14, minus one shift to compensate for
+those we lost when replacing L_MULT by '*'. */
+L_result >>= 13;
+x[k] = saturate(L_result);
+}
+/*endfor*/
+#endif
+}
+/*- End of function --------------------------------------------------------*/
+/* 4.2.14 */
+static void rpe_grid_selection(int16_t x[40], int16_t xM[13], int16_t *Mc_out)
+{
+int i;
+int32_t L_result;
+int32_t L_temp;
+int32_t EM;     /* xxx should be L_EM? */
+int16_t Mc;
+int32_t L_common_0_3;
+/* The signal x[0..39] is used to select the RPE grid which is
+represented by Mc. */
+EM = 0;
+Mc = 0;
+#undef STEP
+#define STEP(m,i)                           \
+L_temp = x[m + 3*i] >> 2;               \
+L_result += L_temp*L_temp;
+/* Common part of 0 and 3 */
+L_result = 0;
+STEP(0, 1);
+STEP(0, 2);
+STEP(0, 3);
+STEP(0, 4);
+STEP(0, 5);
+STEP(0, 6);
+STEP(0, 7);
+STEP(0, 8);
+STEP(0, 9);
+STEP(0, 10);
+STEP(0, 11);
+STEP(0, 12);
+L_common_0_3 = L_result;
+/* i = 0 */
+STEP(0, 0);
+L_result <<= 1; /* implicit in L_MULT */
+EM = L_result;
+/* i = 1 */
+L_result = 0;
+STEP(1, 0);
+STEP(1, 1);
+STEP(1, 2);
+STEP(1, 3);
+STEP(1, 4);
+STEP(1, 5);
+STEP(1, 6);
+STEP(1, 7);
+STEP(1, 8);
+STEP(1, 9);
+STEP(1, 10);
+STEP(1, 11);
+STEP(1, 12);
+L_result <<= 1;
+if (L_result > EM)
+{
+Mc = 1;
+EM = L_result;
+}
+/*endif*/
+/* i = 2 */
+L_result = 0;
+STEP(2, 0);
+STEP(2, 1);
+STEP(2, 2);
+STEP(2, 3);
+STEP(2, 4);
+STEP(2, 5);
+STEP(2, 6);
+STEP(2, 7);
+STEP(2, 8);
+STEP(2, 9);
+STEP(2, 10);
+STEP(2, 11);
+STEP(2, 12);
+L_result <<= 1;
+if (L_result > EM)
+{
+Mc = 2;
+EM = L_result;
+}
+/*endif*/
+/* i = 3 */
+L_result = L_common_0_3;
+STEP(3, 12);
+L_result <<= 1;
+if (L_result > EM)
+{
+Mc = 3;
+EM = L_result;
+}
+/*endif*/
+/* Down-sampling by a factor 3 to get the selected xM[0..12]
+RPE sequence. */
+for (i = 0;  i < 13;  i++)
+xM[i] = x[Mc + 3*i];
+/*endfor*/
+*Mc_out = Mc;
+}
+/*- End of function --------------------------------------------------------*/
+/* 4.12.15 */
+static void apcm_quantization_xmaxc_to_exp_mant(int16_t xmaxc,
+int16_t *exp_out,
+int16_t *mant_out)
+{
+int16_t exp;
+int16_t mant;
+/* Compute exponent and mantissa of the decoded version of xmaxc */
+exp = 0;
+if (xmaxc > 15)
+exp = (int16_t) ((xmaxc >> 3) - 1);
+/*endif*/
+mant = xmaxc - (exp << 3);
+if (mant == 0)
+{
+exp = -4;
+mant = 7;
+}
+else
+{
+while (mant <= 7)
+{
+mant = (int16_t) (mant << 1 | 1);
+exp--;
+}
+/*endwhile*/
+mant -= 8;
+}
+/*endif*/
+assert(exp >= -4  &&  exp <= 6);
+assert(mant >= 0  &&  mant <= 7);
+*exp_out  = exp;
+*mant_out = mant;
+}
+/*- End of function --------------------------------------------------------*/
+static void apcm_quantization(int16_t xM[13],
+int16_t xMc[13],
+int16_t *mant_out,
+int16_t *exp_out,
+int16_t *xmaxc_out)
+{
+/* Table 4.5   Normalized inverse mantissa used to compute xM/xmax */
+static const int16_t gsm_NRFAC[8] =
+{
+29128, 26215, 23832, 21846, 20165, 18725, 17476, 16384
+};
+int i;
+int itest;
+int16_t xmax;
+int16_t xmaxc;
+int16_t temp;
+int16_t temp1;
+int16_t temp2;
+int16_t exp;
+int16_t mant;
+/* Find the maximum absolute value xmax of xM[0..12]. */
+xmax = 0;
+for (i = 0;  i < 13;  i++)
+{
+temp = xM[i];
+temp = gsm_abs(temp);
+if (temp > xmax)
+xmax = temp;
+/*endif*/
+}
+/*endfor*/
+/* Quantizing and coding of xmax to get xmaxc. */
+exp = 0;
+temp = xmax >> 9;
+itest = 0;
+for (i = 0;  i <= 5;  i++)
+{
+itest |= (temp <= 0);
+temp >>= 1;
+assert(exp <= 5);
+if (itest == 0)
+exp++;
+/*endif*/
+}
+/*endfor*/
+assert(exp <= 6  &&  exp >= 0);
+temp = (int16_t) (exp + 5);
+assert(temp <= 11  &&  temp >= 0);
+xmaxc = gsm_add((xmax >> temp), exp << 3);
+/* Quantizing and coding of the xM[0..12] RPE sequence
+to get the xMc[0..12] */
+apcm_quantization_xmaxc_to_exp_mant(xmaxc, &exp, &mant);
+/* This computation uses the fact that the decoded version of xmaxc
+can be calculated by using the exponent and the mantissa part of
+xmaxc (logarithmic table).
+So, this method avoids any division and uses only a scaling
+of the RPE samples by a function of the exponent.  A direct
+multiplication by the inverse of the mantissa (NRFAC[0..7]
+found in table 4.5) gives the 3 bit coded version xMc[0..12]
+of the RPE samples.
+*/
+/* Direct computation of xMc[0..12] using table 4.5 */
+assert(exp <= 4096  &&  exp >= -4096);
+assert(mant >= 0  &&  mant <= 7);
+temp1 = (int16_t) (6 - exp);    /* Normalization by the exponent */
+temp2 = gsm_NRFAC[mant];        /* Inverse mantissa */
+for (i = 0;  i < 13;  i++)
+{
+assert(temp1 >= 0  &&  temp1 < 16);
+temp = xM[i] << temp1;
+temp = gsm_mult(temp, temp2);
+temp >>= 12;
+xMc[i] = (int16_t) (temp + 4);      /* See note below */
+}
+/*endfor*/
+/* NOTE: This equation is used to make all the xMc[i] positive. */
+*mant_out  = mant;
+*exp_out   = exp;
+*xmaxc_out = xmaxc;
+}
+/*- End of function --------------------------------------------------------*/
+/* 4.2.16 */
+static void apcm_inverse_quantization(int16_t xMc[13],
+int16_t mant,
+int16_t exp,
+int16_t xMp[13])
+{
+/* Table 4.6   Normalized direct mantissa used to compute xM/xmax */
+static const int16_t gsm_FAC[8] =
+{
+18431, 20479, 22527, 24575, 26623, 28671, 30719, 32767
+};
+int i;
+int16_t temp;
+int16_t temp1;
+int16_t temp2;
+int16_t temp3;
+/* This part is for decoding the RPE sequence of coded xMc[0..12]
+samples to obtain the xMp[0..12] array.  Table 4.6 is used to get
+the mantissa of xmaxc (FAC[0..7]).
+*/
+assert(mant >= 0  &&  mant <= 7);
+temp1 = gsm_FAC[mant];          /* See 4.2-15 for mant */
+temp2 = gsm_sub(6, exp);        /* See 4.2-15 for exp */
+temp3 = gsm_asl(1, gsm_sub (temp2, 1));
+for (i = 0;  i < 13;  i++)
+{
+assert(xMc[i] >= 0  &&  xMc[i] <= 7);   /* 3 bit unsigned */
+temp = (int16_t) ((xMc[i] << 1) - 7);   /* Restore sign */
+assert(temp <= 7  &&  temp >= -7);      /* 4 bit signed */
+temp <<= 12;                            /* 16 bit signed */
+temp = gsm_mult_r(temp1, temp);
+temp = gsm_add(temp, temp3);
+xMp[i] = gsm_asr(temp, temp2);
+}
+/*endfor*/
+}
+/*- End of function --------------------------------------------------------*/
+/* 4.2.17 */
+static void rpe_grid_positioning(int16_t Mc,
+int16_t xMp[13],
+int16_t ep[40])
+{
+int i = 13;
+/* This procedure computes the reconstructed long term residual signal
+ep[0..39] for the LTP analysis filter.  The inputs are the Mc
+which is the grid position selection and the xMp[0..12] decoded
+RPE samples which are upsampled by a factor of 3 by inserting zero
+values.
+*/
+assert(0 <= Mc  &&  Mc <= 3);
+switch (Mc)
+{
+case 3:
+*ep++ = 0;
+case 2:
+do
+{
+*ep++ = 0;
+case 1:
+*ep++ = 0;
+case 0:
+*ep++ = *xMp++;
+}
+while (--i);
+}
+/*endswitch*/
+while (++Mc < 4)
+*ep++ = 0;
+/*endwhile*/
+}
+/*- End of function --------------------------------------------------------*/
+void gsm0610_rpe_encoding(gsm0610_state_t *s,
+int16_t *e,          // [-5..-1][0..39][40..44]
+int16_t *xmaxc,
+int16_t *Mc,
+int16_t xMc[13])
+{
+int16_t x[40];
+int16_t xM[13];
+int16_t xMp[13];
+int16_t mant;
+int16_t exp;
+weighting_filter(e, x);
+rpe_grid_selection(x, xM, Mc);
+apcm_quantization(xM, xMc, &mant, &exp, xmaxc);
+apcm_inverse_quantization(xMc, mant, exp, xMp);
+rpe_grid_positioning(*Mc, xMp, e);
+}
+/*- End of function --------------------------------------------------------*/
+void gsm0610_rpe_decoding(gsm0610_state_t *s,
+int16_t xmaxc,
+int16_t Mcr,
+int16_t xMcr[13],
+int16_t erp[40])
+{
+int16_t exp;
+int16_t mant;
+int16_t xMp[13];
+apcm_quantization_xmaxc_to_exp_mant(xmaxc, &exp, &mant);
+apcm_inverse_quantization(xMcr, mant, exp, xMp);
+rpe_grid_positioning(Mcr, xMp, erp);
+}
+/*- End of function --------------------------------------------------------*/
+/*- End of file ------------------------------------------------------------*/

Mercurial > hg > audiostuff

comparison spandsp-0.0.3/spandsp-0.0.3/src/gsm0610_rpe.c @ 5:f762bf195c4b