Mercurial > hg > audiostuff
diff spandsp-0.0.6pre17/src/gsm0610_lpc.c @ 4:26cd8f1ef0b1
import spandsp-0.0.6pre17
author | Peter Meerwald <pmeerw@cosy.sbg.ac.at> |
---|---|
date | Fri, 25 Jun 2010 15:50:58 +0200 |
parents | |
children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/spandsp-0.0.6pre17/src/gsm0610_lpc.c Fri Jun 25 15:50:58 2010 +0200 @@ -0,0 +1,600 @@ +/* + * SpanDSP - a series of DSP components for telephony + * + * gsm0610_lpc.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 Lesser General Public License version 2.1, + * 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 Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser 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_lpc.c,v 1.29 2009/02/05 15:57:27 steveu Exp $ + */ + +/*! \file */ + +#if defined(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 "floating_fudge.h" +#include <stdlib.h> +#include <memory.h> + +#include "spandsp/telephony.h" +#include "spandsp/fast_convert.h" +#include "spandsp/bitstream.h" +#include "spandsp/bit_operations.h" +#include "spandsp/saturated.h" +#include "spandsp/vector_int.h" +#include "spandsp/gsm0610.h" + +#include "gsm0610_local.h" + +/* 4.2.4 .. 4.2.7 LPC ANALYSIS SECTION */ + +/* The number of left shifts needed to normalize the 32 bit + variable x for positive values on the interval + with minimum of + minimum of 1073741824 (01000000000000000000000000000000) and + maximum of 2147483647 (01111111111111111111111111111111) + and for negative values on the interval with + minimum of -2147483648 (-10000000000000000000000000000000) and + maximum of -1073741824 ( -1000000000000000000000000000000). + + In order to normalize the result, the following + operation must be done: norm_var1 = x << gsm0610_norm(x); + + (That's 'ffs', only from the left, not the right..) +*/ + +int16_t gsm0610_norm(int32_t x) +{ + assert(x != 0); + + if (x < 0) + { + if (x <= -1073741824) + return 0; + /*endif*/ + x = ~x; + } + /*endif*/ + return (int16_t) (30 - top_bit(x)); +} +/*- End of function --------------------------------------------------------*/ + +/* + (From p. 46, end of section 4.2.5) + + NOTE: The following lines gives [sic] one correct implementation + of the div(num, denum) arithmetic operation. Compute div + which is the integer division of num by denom: with + denom >= num > 0 +*/ +static int16_t gsm_div(int16_t num, int16_t denom) +{ + int32_t num32; + int32_t denom32; + int16_t div; + int k; + + /* The parameter num sometimes becomes zero. + Although this is explicitly guarded against in 4.2.5, + we assume that the result should then be zero as well. */ + + assert(num >= 0 && denom >= num); + if (num == 0) + return 0; + /*endif*/ + num32 = num; + denom32 = denom; + div = 0; + k = 15; + while (k--) + { + div <<= 1; + num32 <<= 1; + + if (num32 >= denom32) + { + num32 -= denom32; + div++; + } + /*endif*/ + } + /*endwhile*/ + + return div; +} +/*- End of function --------------------------------------------------------*/ + +#if defined(__GNUC__) && defined(SPANDSP_USE_MMX) +static void gsm0610_vec_vsraw(const int16_t *p, int n, int bits) +{ + static const int64_t ones = 0x0001000100010001LL; + + if (n == 0) + return; + /*endif*/ +#if defined(__x86_64__) + __asm__ __volatile__( + " leaq -16(%%rsi,%%rax,2),%%rdx;\n" /* edx = top - 16 */ + " emms;\n" + " movd %%ecx,%%mm3;\n" + " movq %[ones],%%mm2;\n" + " psllw %%mm3,%%mm2;\n" + " psrlw $1,%%mm2;\n" + " cmpq %%rdx,%%rsi;" + " ja 4f;\n" + + " .p2align 2;\n" + /* 8 words per iteration */ + "6:\n" + " movq (%%rsi),%%mm0;\n" + " movq 8(%%rsi),%%mm1;\n" + " paddsw %%mm2,%%mm0;\n" + " psraw %%mm3,%%mm0;\n" + " paddsw %%mm2,%%mm1;\n" + " psraw %%mm3,%%mm1;\n" + " movq %%mm0,(%%rsi);\n" + " movq %%mm1,8(%%rsi);\n" + " addq $16,%%rsi;\n" + " cmpq %%rdx,%%rsi;\n" + " jbe 6b;\n" + + " .p2align 2;\n" + "4:\n" + " addq $12,%%rdx;\n" /* now edx = top-4 */ + " cmpq %%rdx,%%rsi;\n" + " ja 3f;\n" + + " .p2align 2;\n" + /* do up to 6 words, two per iteration */ + "5:\n" + " movd (%%rsi),%%mm0;\n" + " paddsw %%mm2,%%mm0;\n" + " psraw %%mm3,%%mm0;\n" + " movd %%mm0,(%%rsi);\n" + " addq $4,%%rsi;\n" + " cmpq %%rdx,%%rsi;\n" + " jbe 5b;\n" + + " .p2align 2;\n" + "3:\n" + " addq $2,%%rdx;\n" /* now edx = top-2 */ + " cmpq %%rdx,%%rsi;\n" + " ja 2f;\n" + + " movzwl (%%rsi),%%eax;\n" + " movd %%eax,%%mm0;\n" + " paddsw %%mm2,%%mm0;\n" + " psraw %%mm3,%%mm0;\n" + " movd %%mm0,%%eax;\n" + " movw %%ax,(%%rsi);\n" + + " .p2align 2;\n" + "2:\n" + " emms;\n" + : + : "S" (p), "a" (n), "c" (bits), [ones] "m" (ones) + : "edx" + ); +#else + __asm__ __volatile__( + " leal -16(%%esi,%%eax,2),%%edx;\n" /* edx = top - 16 */ + " emms;\n" + " movd %%ecx,%%mm3;\n" + " movq %[ones],%%mm2;\n" + " psllw %%mm3,%%mm2;\n" + " psrlw $1,%%mm2;\n" + " cmpl %%edx,%%esi;" + " ja 4f;\n" + + " .p2align 2;\n" + /* 8 words per iteration */ + "6:\n" + " movq (%%esi),%%mm0;\n" + " movq 8(%%esi),%%mm1;\n" + " paddsw %%mm2,%%mm0;\n" + " psraw %%mm3,%%mm0;\n" + " paddsw %%mm2,%%mm1;\n" + " psraw %%mm3,%%mm1;\n" + " movq %%mm0,(%%esi);\n" + " movq %%mm1,8(%%esi);\n" + " addl $16,%%esi;\n" + " cmpl %%edx,%%esi;\n" + " jbe 6b;\n" + + " .p2align 2;\n" + "4:\n" + " addl $12,%%edx;\n" /* now edx = top-4 */ + " cmpl %%edx,%%esi;\n" + " ja 3f;\n" + + " .p2align 2;\n" + /* do up to 6 words, two per iteration */ + "5:\n" + " movd (%%esi),%%mm0;\n" + " paddsw %%mm2,%%mm0;\n" + " psraw %%mm3,%%mm0;\n" + " movd %%mm0,(%%esi);\n" + " addl $4,%%esi;\n" + " cmpl %%edx,%%esi;\n" + " jbe 5b;\n" + + " .p2align 2;\n" + "3:\n" + " addl $2,%%edx;\n" /* now edx = top-2 */ + " cmpl %%edx,%%esi;\n" + " ja 2f;\n" + + " movzwl (%%esi),%%eax;\n" + " movd %%eax,%%mm0;\n" + " paddsw %%mm2,%%mm0;\n" + " psraw %%mm3,%%mm0;\n" + " movd %%mm0,%%eax;\n" + " movw %%ax,(%%esi);\n" + + " .p2align 2;\n" + "2:\n" + " emms;\n" + : + : "S" (p), "a" (n), "c" (bits), [ones] "m" (ones) + : "edx" + ); +#endif +} +/*- End of function --------------------------------------------------------*/ +#endif + +/* 4.2.4 */ +static void autocorrelation(int16_t amp[GSM0610_FRAME_LEN], int32_t L_ACF[9]) +{ + int k; + int16_t smax; + int16_t scalauto; +#if !(defined(__GNUC__) && defined(SPANDSP_USE_MMX)) + int i; + int temp; + int16_t *sp; + int16_t sl; +#endif + + /* The goal is to compute the array L_ACF[k]. The signal s[i] must + be scaled in order to avoid an overflow situation. */ + + /* Dynamic scaling of the array s[0..159] */ + /* Search for the maximum. */ +#if defined(__GNUC__) && defined(SPANDSP_USE_MMX) + smax = saturate(vec_min_maxi16(amp, GSM0610_FRAME_LEN, NULL)); +#else + for (smax = 0, k = 0; k < GSM0610_FRAME_LEN; k++) + { + temp = saturated_abs16(amp[k]); + if (temp > smax) + smax = (int16_t) temp; + /*endif*/ + } + /*endfor*/ +#endif + + /* Computation of the scaling factor. */ + if (smax == 0) + { + scalauto = 0; + } + else + { + assert(smax > 0); + scalauto = (int16_t) (4 - gsm0610_norm((int32_t) smax << 16)); + } + /*endif*/ + + /* Scaling of the array s[0...159] */ +#if defined(__GNUC__) && defined(SPANDSP_USE_MMX) + if (scalauto > 0) + gsm0610_vec_vsraw(amp, GSM0610_FRAME_LEN, scalauto); + /*endif*/ +#else + if (scalauto > 0) + { + for (k = 0; k < GSM0610_FRAME_LEN; k++) + amp[k] = gsm_mult_r(amp[k], 16384 >> (scalauto - 1)); + /*endfor*/ + } + /*endif*/ +#endif + + /* Compute the L_ACF[..]. */ +#if defined(__GNUC__) && defined(SPANDSP_USE_MMX) + for (k = 0; k < 9; k++) + L_ACF[k] = vec_dot_prodi16(amp, amp + k, GSM0610_FRAME_LEN - k) << 1; + /*endfor*/ +#else + sp = amp; + sl = *sp; + L_ACF[0] = ((int32_t) sl*(int32_t) sp[0]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] = ((int32_t) sl*(int32_t) sp[-1]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] = ((int32_t) sl*(int32_t) sp[-2]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]); + L_ACF[3] = ((int32_t) sl*(int32_t) sp[-3]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]); + L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]); + L_ACF[4] = ((int32_t) sl*(int32_t) sp[-4]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]); + L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]); + L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]); + L_ACF[5] = ((int32_t) sl*(int32_t) sp[-5]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]); + L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]); + L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]); + L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]); + L_ACF[6] = ((int32_t) sl*(int32_t) sp[-6]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]); + L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]); + L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]); + L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]); + L_ACF[6] += ((int32_t) sl*(int32_t) sp[-6]); + L_ACF[7] = ((int32_t) sl*(int32_t) sp[-7]); + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]); + L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]); + L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]); + L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]); + L_ACF[6] += ((int32_t) sl*(int32_t) sp[-6]); + L_ACF[7] += ((int32_t) sl*(int32_t) sp[-7]); + L_ACF[8] = ((int32_t) sl*(int32_t) sp[-8]); + for (i = 9; i < GSM0610_FRAME_LEN; i++) + { + sl = *++sp; + L_ACF[0] += ((int32_t) sl*(int32_t) sp[0]); + L_ACF[1] += ((int32_t) sl*(int32_t) sp[-1]); + L_ACF[2] += ((int32_t) sl*(int32_t) sp[-2]); + L_ACF[3] += ((int32_t) sl*(int32_t) sp[-3]); + L_ACF[4] += ((int32_t) sl*(int32_t) sp[-4]); + L_ACF[5] += ((int32_t) sl*(int32_t) sp[-5]); + L_ACF[6] += ((int32_t) sl*(int32_t) sp[-6]); + L_ACF[7] += ((int32_t) sl*(int32_t) sp[-7]); + L_ACF[8] += ((int32_t) sl*(int32_t) sp[-8]); + } + /*endfor*/ + for (k = 0; k < 9; k++) + L_ACF[k] <<= 1; + /*endfor*/ +#endif + /* Rescaling of the array s[0..159] */ + if (scalauto > 0) + { + assert(scalauto <= 4); + for (k = 0; k < GSM0610_FRAME_LEN; k++) + amp[k] <<= scalauto; + /*endfor*/ + } + /*endif*/ +} +/*- End of function --------------------------------------------------------*/ + +/* 4.2.5 */ +static void reflection_coefficients(int32_t L_ACF[9], int16_t r[8]) +{ + int i; + int m; + int n; + int16_t temp; + int16_t ACF[9]; + int16_t P[9]; + int16_t K[9]; + + /* Schur recursion with 16 bits arithmetic. */ + if (L_ACF[0] == 0) + { + for (i = 8; i--; *r++ = 0) + ; + /*endfor*/ + return; + } + /*endif*/ + + assert(L_ACF[0] != 0); + temp = gsm0610_norm(L_ACF[0]); + + assert(temp >= 0 && temp < 32); + + /* ? overflow ? */ + for (i = 0; i <= 8; i++) + ACF[i] = (int16_t) ((L_ACF[i] << temp) >> 16); + /*endfor*/ + + /* Initialize array P[..] and K[..] for the recursion. */ + for (i = 1; i <= 7; i++) + K[i] = ACF[i]; + /*endfor*/ + for (i = 0; i <= 8; i++) + P[i] = ACF[i]; + /*endfor*/ + /* Compute reflection coefficients */ + for (n = 1; n <= 8; n++, r++) + { + temp = P[1]; + temp = saturated_abs16(temp); + if (P[0] < temp) + { + for (i = n; i <= 8; i++) + *r++ = 0; + /*endfor*/ + return; + } + /*endif*/ + + *r = gsm_div(temp, P[0]); + + assert(*r >= 0); + if (P[1] > 0) + *r = -*r; /* r[n] = sub(0, r[n]) */ + /*endif*/ + assert(*r != INT16_MIN); + if (n == 8) + return; + /*endif*/ + + /* Schur recursion */ + temp = gsm_mult_r(P[1], *r); + P[0] = saturated_add16(P[0], temp); + + for (m = 1; m <= 8 - n; m++) + { + temp = gsm_mult_r(K[m], *r); + P[m] = saturated_add16(P[m + 1], temp); + + temp = gsm_mult_r(P[m + 1], *r); + K[m] = saturated_add16(K[m], temp); + } + /*endfor*/ + } + /*endfor*/ +} +/*- End of function --------------------------------------------------------*/ + +/* 4.2.6 */ +static void transform_to_log_area_ratios(int16_t r[8]) +{ + int16_t temp; + int i; + + /* The following scaling for r[..] and LAR[..] has been used: + + r[..] = integer (real_r[..]*32768.); -1 <= real_r < 1. + LAR[..] = integer (real_LAR[..] * 16384); + with -1.625 <= real_LAR <= 1.625 + */ + + /* Computation of the LAR[0..7] from the r[0..7] */ + for (i = 1; i <= 8; i++, r++) + { + temp = saturated_abs16(*r); + assert(temp >= 0); + + if (temp < 22118) + { + temp >>= 1; + } + else if (temp < 31130) + { + assert(temp >= 11059); + temp -= 11059; + } + else + { + assert(temp >= 26112); + temp -= 26112; + temp <<= 2; + } + /*endif*/ + + *r = (*r < 0) ? -temp : temp; + assert(*r != INT16_MIN); + } + /*endfor*/ +} +/*- End of function --------------------------------------------------------*/ + +/* 4.2.7 */ +static void quantization_and_coding(int16_t LAR[8]) +{ + int16_t temp; + + /* This procedure needs four tables; the following equations + give the optimum scaling for the constants: + + A[0..7] = integer(real_A[0..7] * 1024) + B[0..7] = integer(real_B[0..7] * 512) + MAC[0..7] = maximum of the LARc[0..7] + MIC[0..7] = minimum of the LARc[0..7] */ + +#undef STEP +#define STEP(A,B,MAC,MIC) \ + temp = saturated_mul16(A, *LAR); \ + temp = saturated_add16(temp, B); \ + temp = saturated_add16(temp, 256); \ + temp >>= 9; \ + *LAR = (int16_t) ((temp > MAC) \ + ? \ + MAC - MIC \ + : \ + ((temp < MIC) ? 0 : temp - MIC)); \ + LAR++; + + STEP(20480, 0, 31, -32); + STEP(20480, 0, 31, -32); + STEP(20480, 2048, 15, -16); + STEP(20480, -2560, 15, -16); + + STEP(13964, 94, 7, -8); + STEP(15360, -1792, 7, -8); + STEP( 8534, -341, 3, -4); + STEP( 9036, -1144, 3, -4); +#undef STEP +} +/*- End of function --------------------------------------------------------*/ + +void gsm0610_lpc_analysis(gsm0610_state_t *s, + int16_t amp[GSM0610_FRAME_LEN], + int16_t LARc[8]) +{ + int32_t L_ACF[9]; + + autocorrelation(amp, L_ACF); + reflection_coefficients(L_ACF, LARc); + transform_to_log_area_ratios(LARc); + quantization_and_coding(LARc); +} +/*- End of function --------------------------------------------------------*/ +/*- End of file ------------------------------------------------------------*/