1/*
2 *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
3 *
4 *  Use of this source code is governed by a BSD-style license
5 *  that can be found in the LICENSE file in the root of the source
6 *  tree. An additional intellectual property rights grant can be found
7 *  in the file PATENTS.  All contributing project authors may
8 *  be found in the AUTHORS file in the root of the source tree.
9 */
10
11/*
12 * arith_routinslogist.c
13 *
14 * This C file contains arithmetic encode and decode logistic
15 *
16 */
17
18#include "arith_routins.h"
19
20/* Tables for piecewise linear cdf functions: y = k*x */
21
22/* x Points for function piecewise() in Q15 */
23static const int32_t kHistEdges[51] = {
24  -327680, -314573, -301466, -288359, -275252, -262144, -249037, -235930, -222823, -209716,
25  -196608, -183501, -170394, -157287, -144180, -131072, -117965, -104858,  -91751,  -78644,
26  -65536,  -52429,  -39322,  -26215,  -13108,       0,   13107,   26214,   39321,   52428,
27  65536,   78643,   91750,  104857,  117964,  131072,  144179,  157286,  170393,  183500,
28  196608,  209715,  222822,  235929,  249036,  262144,  275251,  288358,  301465,  314572,
29  327680
30};
31
32
33/* k Points for function piecewise() in Q0 */
34static const uint16_t kCdfSlope[51] = {
35  5,    5,     5,     5,     5,     5,     5,     5,    5,    5,
36  5,    5,    13,    23,    47,    87,   154,   315,  700, 1088,
37  2471, 6064, 14221, 21463, 36634, 36924, 19750, 13270, 5806, 2312,
38  1095,  660,   316,   145,    86,    41,    32,     5,    5,    5,
39  5,    5,     5,     5,     5,     5,     5,     5,    5,    2,
40  0
41};
42
43/* y Points for function piecewise() in Q0 */
44static const uint16_t kCdfLogistic[51] = {
45  0,     2,     4,     6,     8,    10,    12,    14,    16,    18,
46  20,    22,    24,    29,    38,    57,    92,   153,   279,   559,
47  994,  1983,  4408, 10097, 18682, 33336, 48105, 56005, 61313, 63636,
48  64560, 64998, 65262, 65389, 65447, 65481, 65497, 65510, 65512, 65514,
49  65516, 65518, 65520, 65522, 65524, 65526, 65528, 65530, 65532, 65534,
50  65535
51};
52
53
54/****************************************************************************
55 * WebRtcIsacfix_Piecewise(...)
56 *
57 * Piecewise linear function
58 *
59 * Input:
60 *      - xinQ15           : input value x in Q15
61 *
62 * Return value            : korresponding y-value in Q0
63 */
64
65
66static __inline uint16_t WebRtcIsacfix_Piecewise(int32_t xinQ15) {
67  int32_t ind;
68  int32_t qtmp1;
69  uint16_t qtmp2;
70
71  /* Find index for x-value */
72  qtmp1 = WEBRTC_SPL_SAT(kHistEdges[50],xinQ15,kHistEdges[0]);
73  ind = WEBRTC_SPL_MUL(5, qtmp1 - kHistEdges[0]);
74  ind >>= 16;
75
76  /* Calculate corresponding y-value ans return*/
77  qtmp1 = qtmp1 - kHistEdges[ind];
78  qtmp2 = (uint16_t)WEBRTC_SPL_RSHIFT_U32(
79      WEBRTC_SPL_UMUL_32_16(qtmp1,kCdfSlope[ind]), 15);
80  return (kCdfLogistic[ind] + qtmp2);
81}
82
83/****************************************************************************
84 * WebRtcIsacfix_EncLogisticMulti2(...)
85 *
86 * Arithmetic coding of spectrum.
87 *
88 * Input:
89 *      - streamData        : in-/output struct containing bitstream
90 *      - dataQ7            : data vector in Q7
91 *      - envQ8             : side info vector defining the width of the pdf
92 *                            in Q8
93 *      - lenData           : data vector length
94 *
95 * Return value             :  0 if ok,
96 *                            <0 otherwise.
97 */
98int WebRtcIsacfix_EncLogisticMulti2(Bitstr_enc *streamData,
99                                   int16_t *dataQ7,
100                                   const uint16_t *envQ8,
101                                   const int16_t lenData)
102{
103  uint32_t W_lower;
104  uint32_t W_upper;
105  uint16_t W_upper_LSB;
106  uint16_t W_upper_MSB;
107  uint16_t *streamPtr;
108  uint16_t *maxStreamPtr;
109  uint16_t *streamPtrCarry;
110  uint16_t negcarry;
111  uint32_t cdfLo;
112  uint32_t cdfHi;
113  int k;
114
115  /* point to beginning of stream buffer
116   * and set maximum streamPtr value */
117  streamPtr = streamData->stream + streamData->stream_index;
118  maxStreamPtr = streamData->stream + STREAM_MAXW16_60MS - 1;
119  W_upper = streamData->W_upper;
120
121  for (k = 0; k < lenData; k++)
122  {
123    /* compute cdf_lower and cdf_upper by evaluating the
124     * WebRtcIsacfix_Piecewise linear cdf */
125    cdfLo = WebRtcIsacfix_Piecewise(WEBRTC_SPL_MUL_16_U16(*dataQ7 - 64, *envQ8));
126    cdfHi = WebRtcIsacfix_Piecewise(WEBRTC_SPL_MUL_16_U16(*dataQ7 + 64, *envQ8));
127
128    /* test and clip if probability gets too small */
129    while ((cdfLo + 1) >= cdfHi) {
130      /* clip */
131      if (*dataQ7 > 0) {
132        *dataQ7 -= 128;
133        cdfHi = cdfLo;
134        cdfLo = WebRtcIsacfix_Piecewise(
135            WEBRTC_SPL_MUL_16_U16(*dataQ7 - 64, *envQ8));
136      } else {
137        *dataQ7 += 128;
138        cdfLo = cdfHi;
139        cdfHi = WebRtcIsacfix_Piecewise(
140            WEBRTC_SPL_MUL_16_U16(*dataQ7 + 64, *envQ8));
141      }
142    }
143
144    dataQ7++;
145    /* increment only once per 4 iterations */
146    envQ8 += (k & 1) & (k >> 1);
147
148
149    /* update interval */
150    W_upper_LSB = (uint16_t)W_upper;
151    W_upper_MSB = (uint16_t)WEBRTC_SPL_RSHIFT_U32(W_upper, 16);
152    W_lower = WEBRTC_SPL_UMUL_32_16(cdfLo, W_upper_MSB);
153    W_lower += (cdfLo * W_upper_LSB) >> 16;
154    W_upper = WEBRTC_SPL_UMUL_32_16(cdfHi, W_upper_MSB);
155    W_upper += (cdfHi * W_upper_LSB) >> 16;
156
157    /* shift interval such that it begins at zero */
158    W_upper -= ++W_lower;
159
160    /* add integer to bitstream */
161    streamData->streamval += W_lower;
162
163    /* handle carry */
164    if (streamData->streamval < W_lower)
165    {
166      /* propagate carry */
167      streamPtrCarry = streamPtr;
168      if (streamData->full == 0) {
169        negcarry = *streamPtrCarry;
170        negcarry += 0x0100;
171        *streamPtrCarry = negcarry;
172        while (!(negcarry))
173        {
174          negcarry = *--streamPtrCarry;
175          negcarry++;
176          *streamPtrCarry = negcarry;
177        }
178      } else {
179        while (!(++(*--streamPtrCarry)));
180      }
181    }
182
183    /* renormalize interval, store most significant byte of streamval and update streamval
184     * W_upper < 2^24 */
185    while ( !(W_upper & 0xFF000000) )
186    {
187      W_upper <<= 8;
188      if (streamData->full == 0) {
189        *streamPtr++ += (uint16_t) WEBRTC_SPL_RSHIFT_U32(
190            streamData->streamval, 24);
191        streamData->full = 1;
192      } else {
193        *streamPtr = (uint16_t)((streamData->streamval >> 24) << 8);
194        streamData->full = 0;
195      }
196
197      if( streamPtr > maxStreamPtr )
198        return -ISAC_DISALLOWED_BITSTREAM_LENGTH;
199
200      streamData->streamval <<= 8;
201    }
202  }
203
204  /* calculate new stream_index */
205  streamData->stream_index = streamPtr - streamData->stream;
206  streamData->W_upper = W_upper;
207
208  return 0;
209}
210
211
212/****************************************************************************
213 * WebRtcIsacfix_DecLogisticMulti2(...)
214 *
215 * Arithmetic decoding of spectrum.
216 *
217 * Input:
218 *      - streamData        : in-/output struct containing bitstream
219 *      - envQ8             : side info vector defining the width of the pdf
220 *                            in Q8
221 *      - lenData           : data vector length
222 *
223 * Input/Output:
224 *      - dataQ7            : input: dither vector, output: data vector
225 *
226 * Return value             : number of bytes in the stream so far
227 *                            -1 if error detected
228 */
229int WebRtcIsacfix_DecLogisticMulti2(int16_t *dataQ7,
230                                    Bitstr_dec *streamData,
231                                    const int32_t *envQ8,
232                                    const int16_t lenData)
233{
234  uint32_t    W_lower;
235  uint32_t    W_upper;
236  uint32_t    W_tmp;
237  uint16_t    W_upper_LSB;
238  uint16_t    W_upper_MSB;
239  uint32_t    streamVal;
240  uint16_t    cdfTmp;
241  int32_t     res;
242  int32_t     inSqrt;
243  int32_t     newRes;
244  const uint16_t *streamPtr;
245  int16_t     candQ7;
246  int16_t     envCount;
247  uint16_t    tmpARSpecQ8 = 0;
248  int             k, i;
249  int offset = 0;
250
251  /* point to beginning of stream buffer */
252  streamPtr = streamData->stream + streamData->stream_index;
253  W_upper = streamData->W_upper;
254
255  /* Check if it is first time decoder is called for this stream */
256  if (streamData->stream_index == 0)
257  {
258    /* read first word from bytestream */
259    streamVal = (uint32_t)(*streamPtr++) << 16;
260    streamVal |= *streamPtr++;
261
262  } else {
263    streamVal = streamData->streamval;
264  }
265
266
267  res = 1 << (WebRtcSpl_GetSizeInBits(envQ8[0]) >> 1);
268  envCount = 0;
269
270  /* code assumes lenData%4 == 0 */
271  for (k = 0; k < lenData; k += 4)
272  {
273    int k4;
274
275    /* convert to magnitude spectrum, by doing square-roots (modified from SPLIB) */
276    inSqrt = envQ8[envCount];
277    i = 10;
278
279    /* For safty reasons */
280    if (inSqrt < 0)
281      inSqrt=-inSqrt;
282
283    newRes = (inSqrt / res + res) >> 1;
284    do
285    {
286      res = newRes;
287      newRes = (inSqrt / res + res) >> 1;
288    } while (newRes != res && i-- > 0);
289
290    tmpARSpecQ8 = (uint16_t)newRes;
291
292    for(k4 = 0; k4 < 4; k4++)
293    {
294      /* find the integer *data for which streamVal lies in [W_lower+1, W_upper] */
295      W_upper_LSB = (uint16_t) (W_upper & 0x0000FFFF);
296      W_upper_MSB = (uint16_t) WEBRTC_SPL_RSHIFT_U32(W_upper, 16);
297
298      /* find first candidate by inverting the logistic cdf
299       * Input dither value collected from io-stream */
300      candQ7 = - *dataQ7 + 64;
301      cdfTmp = WebRtcIsacfix_Piecewise(WEBRTC_SPL_MUL_16_U16(candQ7, tmpARSpecQ8));
302
303      W_tmp = (uint32_t)cdfTmp * W_upper_MSB;
304      W_tmp += ((uint32_t)cdfTmp * (uint32_t)W_upper_LSB) >> 16;
305
306      if (streamVal > W_tmp)
307      {
308        W_lower = W_tmp;
309        candQ7 += 128;
310        cdfTmp = WebRtcIsacfix_Piecewise(WEBRTC_SPL_MUL_16_U16(candQ7, tmpARSpecQ8));
311
312        W_tmp = (uint32_t)cdfTmp * W_upper_MSB;
313        W_tmp += ((uint32_t)cdfTmp * (uint32_t)W_upper_LSB) >> 16;
314
315        while (streamVal > W_tmp)
316        {
317          W_lower = W_tmp;
318          candQ7 += 128;
319          cdfTmp = WebRtcIsacfix_Piecewise(
320              WEBRTC_SPL_MUL_16_U16(candQ7, tmpARSpecQ8));
321
322          W_tmp = (uint32_t)cdfTmp * W_upper_MSB;
323          W_tmp += ((uint32_t)cdfTmp * (uint32_t)W_upper_LSB) >> 16;
324
325          /* error check */
326          if (W_lower == W_tmp) {
327            return -1;
328          }
329        }
330        W_upper = W_tmp;
331
332        /* Output value put in dataQ7: another sample decoded */
333        *dataQ7 = candQ7 - 64;
334      }
335      else
336      {
337        W_upper = W_tmp;
338        candQ7 -= 128;
339        cdfTmp = WebRtcIsacfix_Piecewise(WEBRTC_SPL_MUL_16_U16(candQ7, tmpARSpecQ8));
340
341        W_tmp = (uint32_t)cdfTmp * W_upper_MSB;
342        W_tmp += ((uint32_t)cdfTmp * (uint32_t)W_upper_LSB) >> 16;
343
344        while ( !(streamVal > W_tmp) )
345        {
346          W_upper = W_tmp;
347          candQ7 -= 128;
348          cdfTmp = WebRtcIsacfix_Piecewise(
349              WEBRTC_SPL_MUL_16_U16(candQ7, tmpARSpecQ8));
350
351          W_tmp = (uint32_t)cdfTmp * W_upper_MSB;
352          W_tmp += ((uint32_t)cdfTmp * (uint32_t)W_upper_LSB) >> 16;
353
354          /* error check */
355          if (W_upper == W_tmp){
356            return -1;
357          }
358        }
359        W_lower = W_tmp;
360
361        /* Output value put in dataQ7: another sample decoded */
362        *dataQ7 = candQ7 + 64;
363      }
364
365      dataQ7++;
366
367      /* shift interval to start at zero */
368      W_upper -= ++W_lower;
369
370      /* add integer to bitstream */
371      streamVal -= W_lower;
372
373      /* renormalize interval and update streamVal
374       * W_upper < 2^24 */
375      while ( !(W_upper & 0xFF000000) )
376      {
377        if (streamPtr < streamData->stream + streamData->stream_size) {
378          /* read next byte from stream */
379          if (streamData->full == 0) {
380            streamVal = (streamVal << 8) | (*streamPtr++ & 0x00FF);
381            streamData->full = 1;
382          } else {
383            streamVal = (streamVal << 8) | (*streamPtr >> 8);
384            streamData->full = 0;
385          }
386        } else {
387          /* Intending to read outside the stream. This can happen for the last
388           * two or three bytes. It is how the algorithm is implemented. Do
389           * not read from the bit stream and insert zeros instead. */
390          streamVal <<= 8;
391          if (streamData->full == 0) {
392            offset++;  // We would have incremented the pointer in this case.
393            streamData->full = 1;
394          } else {
395            streamData->full = 0;
396          }
397        }
398        W_upper <<= 8;
399      }
400    }
401    envCount++;
402  }
403
404  streamData->stream_index = streamPtr + offset - streamData->stream;
405  streamData->W_upper = W_upper;
406  streamData->streamval = streamVal;
407
408  /* find number of bytes in original stream (determined by current interval width) */
409  if ( W_upper > 0x01FFFFFF )
410    return (streamData->stream_index*2 - 3 + !streamData->full);
411  else
412    return (streamData->stream_index*2 - 2 + !streamData->full);
413}
414