--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/spandsp-0.0.6pre17/src/gsm0610_encode.c	Fri Jun 25 15:50:58 2010 +0200
@@ -0,0 +1,342 @@
+/*
+ * SpanDSP - a series of DSP components for telephony
+ *
+ * gsm0610_encode.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_encode.c,v 1.30 2009/02/10 13:06:46 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/saturated.h"
+#include "spandsp/gsm0610.h"
+
+#include "gsm0610_local.h"
+
+/* 4.2 FIXED POINT IMPLEMENTATION OF THE RPE-LTP CODER */
+
+/* The RPE-LTD coder works on a frame by frame basis.  The length of
+   the frame is equal to 160 samples.  Some computations are done
+   once per frame to produce at the output of the coder the
+   LARc[1..8] parameters which are the coded LAR coefficients and 
+   also to realize the inverse filtering operation for the entire
+   frame (160 samples of signal d[0..159]).  These parts produce at
+   the output of the coder:
+
+   Procedure 4.2.11 to 4.2.18 are to be executed four times per
+   frame.  That means once for each sub-segment RPE-LTP analysis of
+   40 samples.  These parts produce at the output of the coder.
+*/
+static void encode_a_frame(gsm0610_state_t *s, gsm0610_frame_t *f, const int16_t amp[])
+{
+    int k;
+    int16_t *dp;
+    int16_t *dpp;
+    int16_t so[GSM0610_FRAME_LEN];
+    int i;
+
+    dp = s->dp0 + 120;
+    dpp = dp;
+    gsm0610_preprocess(s, amp, so);
+    gsm0610_lpc_analysis(s, so, f->LARc);
+    gsm0610_short_term_analysis_filter(s, f->LARc, so);
+
+    for (k = 0;  k < 4;  k++)
+    {
+        gsm0610_long_term_predictor(s,
+                                    so + k*40,
+                                    dp,
+                                    s->e + 5,
+                                    dpp,
+                                    &f->Nc[k],
+                                    &f->bc[k]);
+        gsm0610_rpe_encoding(s, s->e + 5, &f->xmaxc[k], &f->Mc[k], f->xMc[k]);
+
+        for (i = 0;  i < 40;  i++)
+            dp[i] = saturated_add16(s->e[5 + i], dpp[i]);
+        /*endfor*/
+        dp += 40;
+        dpp += 40;
+    }
+    /*endfor*/
+    memcpy((char *) s->dp0,
+           (char *) (s->dp0 + GSM0610_FRAME_LEN),
+           120*sizeof(*s->dp0));
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(int) gsm0610_set_packing(gsm0610_state_t *s, int packing)
+{
+    s->packing = packing;
+    return 0;    
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(gsm0610_state_t *) gsm0610_init(gsm0610_state_t *s, int packing)
+{
+    if (s == NULL)
+    {
+        if ((s = (gsm0610_state_t *) malloc(sizeof (*s))) == NULL)
+            return  NULL;
+        /*endif*/
+    }
+    /*endif*/
+    memset((char *) s, '\0', sizeof (gsm0610_state_t));
+    s->nrp = 40;
+    s->packing = packing;
+    return s;
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(int) gsm0610_release(gsm0610_state_t *s)
+{
+    return 0;
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(int) gsm0610_free(gsm0610_state_t *s)
+{
+    if (s)
+        free(s);
+    /*endif*/
+    return 0;
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(int) gsm0610_pack_none(uint8_t c[], const gsm0610_frame_t *s)
+{
+    int i;
+    int j;
+    int k;
+    
+    i = 0;
+    for (j = 0;  j < 8;  j++)
+        c[i++] = (uint8_t) s->LARc[j];
+    for (j = 0;  j < 4;  j++)
+    {
+        c[i++] = (uint8_t) s->Nc[j];
+        c[i++] = (uint8_t) s->bc[j];
+        c[i++] = (uint8_t) s->Mc[j];
+        c[i++] = (uint8_t) s->xmaxc[j];
+        for (k = 0;  k < 13;  k++)
+            c[i++] = (uint8_t) s->xMc[j][k];
+    }
+    /*endfor*/
+    return 76;
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(int) gsm0610_pack_wav49(uint8_t c[], const gsm0610_frame_t *s)
+{
+    uint16_t sr;
+    int i;
+ 
+	sr = 0;
+	sr = (sr >> 6) | (s->LARc[0] << 10);
+	sr = (sr >> 6) | (s->LARc[1] << 10);
+	*c++ = (uint8_t) (sr >> 4);
+	sr = (sr >> 5) | (s->LARc[2] << 11);
+	*c++ = (uint8_t) (sr >> 7);
+	sr = (sr >> 5) | (s->LARc[3] << 11);
+	sr = (sr >> 4) | (s->LARc[4] << 12);
+	*c++ = (uint8_t) (sr >> 6);
+	sr = (sr >> 4) | (s->LARc[5] << 12);
+	sr = (sr >> 3) | (s->LARc[6] << 13);
+	*c++ = (uint8_t) (sr >> 7);
+	sr = (sr >> 3) | (s->LARc[7] << 13);
+
+    for (i = 0;  i < 4;  i++)
+    {
+    	sr = (sr >> 7) | (s->Nc[i] << 9);
+    	*c++ = (uint8_t) (sr >> 5);
+    	sr = (sr >> 2) | (s->bc[i] << 14);
+    	sr = (sr >> 2) | (s->Mc[i] << 14);
+    	sr = (sr >> 6) | (s->xmaxc[i] << 10);
+    	*c++ = (uint8_t) (sr >> 3);
+    	sr = (sr >> 3) | (s->xMc[i][0] << 13);
+    	*c++ = (uint8_t) (sr >> 8);
+    	sr = (sr >> 3) | (s->xMc[i][1] << 13);
+    	sr = (sr >> 3) | (s->xMc[i][2] << 13);
+        sr = (sr >> 3) | (s->xMc[i][3] << 13);
+        *c++ = (uint8_t) (sr >> 7);
+        sr = (sr >> 3) | (s->xMc[i][4] << 13);
+        sr = (sr >> 3) | (s->xMc[i][5] << 13);
+        sr = (sr >> 3) | (s->xMc[i][6] << 13);
+        *c++ = (uint8_t) (sr >> 6);
+        sr = (sr >> 3) | (s->xMc[i][7] << 13);
+        sr = (sr >> 3) | (s->xMc[i][8] << 13);
+        *c++ = (uint8_t) (sr >> 8);
+        sr = (sr >> 3) | (s->xMc[i][9] << 13);
+        sr = (sr >> 3) | (s->xMc[i][10] << 13);
+        sr = (sr >> 3) | (s->xMc[i][11] << 13);
+        *c++ = (uint8_t) (sr >> 7);
+        sr = (sr >> 3) | (s->xMc[i][12] << 13);
+    }
+    /*endfor*/
+
+    s++;
+    sr = (sr >> 6) | (s->LARc[0] << 10);
+    *c++ = (uint8_t) (sr >> 6);
+    sr = (sr >> 6) | (s->LARc[1] << 10);
+    *c++ = (uint8_t) (sr >> 8);
+    sr = (sr >> 5) | (s->LARc[2] << 11);
+    sr = (sr >> 5) | (s->LARc[3] << 11);
+    *c++ = (uint8_t) (sr >> 6);
+    sr = (sr >> 4) | (s->LARc[4] << 12);
+    sr = (sr >> 4) | (s->LARc[5] << 12);
+    *c++ = (uint8_t) (sr >> 6);
+    sr = (sr >> 3) | (s->LARc[6] << 13);
+    sr = (sr >> 3) | (s->LARc[7] << 13);
+    *c++ = (uint8_t) (sr >> 8);
+
+    for (i = 0;  i < 4;  i++)
+    {
+        sr = (sr >> 7) | (s->Nc[i] << 9);
+        sr = (sr >> 2) | (s->bc[i] << 14);
+        *c++ = (uint8_t) (sr >> 7);
+        sr = (sr >> 2) | (s->Mc[i] << 14);
+        sr = (sr >> 6) | (s->xmaxc[i] << 10);
+        *c++ = (uint8_t) (sr >> 7);
+        sr = (sr >> 3) | (s->xMc[i][0] << 13);
+        sr = (sr >> 3) | (s->xMc[i][1] << 13);
+        sr = (sr >> 3) | (s->xMc[i][2] << 13);
+        *c++ = (uint8_t) (sr >> 6);
+        sr = (sr >> 3) | (s->xMc[i][3] << 13);
+        sr = (sr >> 3) | (s->xMc[i][4] << 13);
+        *c++ = (uint8_t) (sr >> 8);
+        sr = (sr >> 3) | (s->xMc[i][5] << 13);
+        sr = (sr >> 3) | (s->xMc[i][6] << 13);
+        sr = (sr >> 3) | (s->xMc[i][7] << 13);
+        *c++ = (uint8_t) (sr >> 7);
+        sr = (sr >> 3) | (s->xMc[i][8] << 13);
+        sr = (sr >> 3) | (s->xMc[i][9] << 13);
+        sr = (sr >> 3) | (s->xMc[i][10] << 13);
+        *c++ = (uint8_t) (sr >> 6);
+        sr = (sr >> 3) | (s->xMc[i][11] << 13);
+        sr = (sr >> 3) | (s->xMc[i][12] << 13);
+        *c++ = (uint8_t) (sr >> 8);
+    }
+    /*endfor*/
+    return 65;
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(int) gsm0610_pack_voip(uint8_t c[33], const gsm0610_frame_t *s)
+{
+    int i;
+
+    *c++ = (uint8_t) (((GSM0610_MAGIC & 0xF) << 4)
+                    | ((s->LARc[0] >> 2) & 0xF));
+    *c++ = (uint8_t) (((s->LARc[0] & 0x3) << 6)
+                    |  (s->LARc[1] & 0x3F));
+    *c++ = (uint8_t) (((s->LARc[2] & 0x1F) << 3)
+                    | ((s->LARc[3] >> 2) & 0x7));
+    *c++ = (uint8_t) (((s->LARc[3] & 0x3) << 6)
+                    | ((s->LARc[4] & 0xF) << 2)
+                    | ((s->LARc[5] >> 2) & 0x3));
+    *c++ = (uint8_t) (((s->LARc[5] & 0x3) << 6)
+                    | ((s->LARc[6] & 0x7) << 3)
+                    |  (s->LARc[7] & 0x7));
+
+    for (i = 0;  i < 4;  i++)
+    {
+        *c++ = (uint8_t) (((s->Nc[i] & 0x7F) << 1)
+                        | ((s->bc[i] >> 1) & 0x1));
+        *c++ = (uint8_t) (((s->bc[i] & 0x1) << 7)
+                        | ((s->Mc[i] & 0x3) << 5)
+                        | ((s->xmaxc[i] >> 1) & 0x1F));
+        *c++ = (uint8_t) (((s->xmaxc[i] & 0x1) << 7)
+                        | ((s->xMc[i][0] & 0x7) << 4)
+                        | ((s->xMc[i][1] & 0x7) << 1)
+                        | ((s->xMc[i][2] >> 2) & 0x1));
+        *c++ = (uint8_t) (((s->xMc[i][2] & 0x3) << 6)
+                        | ((s->xMc[i][3] & 0x7) << 3)
+                        |  (s->xMc[i][4] & 0x7));
+        *c++ = (uint8_t) (((s->xMc[i][5] & 0x7) << 5)
+                        | ((s->xMc[i][6] & 0x7) << 2)
+                        | ((s->xMc[i][7] >> 1) & 0x3));
+        *c++ = (uint8_t) (((s->xMc[i][7] & 0x1) << 7)
+                        | ((s->xMc[i][8] & 0x7) << 4)
+                        | ((s->xMc[i][9] & 0x7) << 1)
+                        | ((s->xMc[i][10] >> 2) & 0x1));
+        *c++ = (uint8_t) (((s->xMc[i][10] & 0x3) << 6)
+                        | ((s->xMc[i][11] & 0x7) << 3)
+                        |  (s->xMc[i][12] & 0x7));
+    }
+    /*endfor*/
+    return 33;
+}
+/*- End of function --------------------------------------------------------*/
+
+SPAN_DECLARE(int) gsm0610_encode(gsm0610_state_t *s, uint8_t code[], const int16_t amp[], int len)
+{
+    gsm0610_frame_t frame[2];
+    int bytes;
+    int i;
+
+    bytes = 0;
+    for (i = 0;  i < len;  i += GSM0610_FRAME_LEN)
+    {
+        encode_a_frame(s, frame, &amp[i]);
+        switch (s->packing)
+        {
+        case GSM0610_PACKING_WAV49:
+            i += GSM0610_FRAME_LEN;
+            encode_a_frame(s, frame + 1, &amp[i]);
+            bytes += gsm0610_pack_wav49(&code[bytes], frame);
+            break;
+        case GSM0610_PACKING_VOIP:
+            bytes += gsm0610_pack_voip(&code[bytes], frame);
+            break;
+        default:
+            bytes += gsm0610_pack_none(&code[bytes], frame);
+            break;
+        }
+        /*endswitch*/
+    }
+    /*endfor*/
+    return bytes;
+}
+/*- End of function --------------------------------------------------------*/
+/*- End of file ------------------------------------------------------------*/