g722.cpp 26.6 KB
Newer Older
1
/*
2
 * Copyright (C) 2004-2012 Savoir-Faire Linux Inc.
3 4 5 6 7 8 9 10
 * Author:  Alexandre Savard <alexandre.savard@savoirfairelinux.com>
 *
 * Motly borrowed from asterisk's sources (Steve Underwood <steveu@coppice.org>)
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
11
 *
12 13 14 15
 * 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.
16
 *
17 18 19
 * 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.
20 21 22 23 24 25 26 27 28 29 30
 *
 *  Additional permission under GNU GPL version 3 section 7:
 *
 *  If you modify this program, or any covered work, by linking or
 *  combining it with the OpenSSL project's OpenSSL library (or a
 *  modified version of that library), containing parts covered by the
 *  terms of the OpenSSL or SSLeay licenses, Savoir-Faire Linux Inc.
 *  grants you additional permission to convey the resulting work.
 *  Corresponding Source for a non-source form of such a combination
 *  shall include the source code for the parts of OpenSSL used as well
 *  as that of the covered work.
31
 */
32 33

#include "audiocodec.h"
34
#include "sfl_types.h"
35 36
#include "g722.h"

37 38
#include <cstdlib>
#include <cstring>
39

40
class G722 : public sfl::AudioCodec {
41 42

    public:
43 44
        G722() : sfl::AudioCodec(9, "G722", 16000, 320, 1), decode_state_(),
        encode_state_() {
45 46
            bitrate_ = 64;
            hasDynamicPayload_ = false;
47

48 49
            g722_state_init(decode_state_);
            g722_state_init(encode_state_);
50
        }
51

52 53 54 55
    private:
        virtual int decode(SFLDataFormat *dst, unsigned char *src, size_t buf_size)
        {
            return g722_decode(dst, src, buf_size);
56 57
        }

58 59 60
        virtual int encode(unsigned char *dst, SFLDataFormat *src, size_t /*buf_size*/)
        {
            int out = g722_encode(dst, src, frameSize_);
61
            return out;
62
        }
63

64 65 66
        static void g722_state_init(g722_state_t &state)
        {
            state.itu_test_mode = false;
67 68

            // 8 => 64 kbps;  7 => 56 kbps;  6 => 48 kbps
69
            state.bits_per_sample = 8;
70

71
            // Enable 8khz mode, encode using lower subband only
72
            state.eight_k = false;
73

74
            // Never set packed true when using 64 kbps
75
            state.packed = false;
76

77 78 79
            memset(state.band, 0, sizeof(state.band));
            state.band[0].det = 32;
            state.band[1].det = 8;
80

81
            memset(state.x, 0, sizeof(state.x));
82

83 84 85 86
            state.in_buffer = 0;
            state.in_bits = 0;
            state.out_buffer = 0;
            state.out_bits = 0;
87
        }
88

89 90 91
        SFLDataFormat saturate(int32_t amp)
        {
            SFLDataFormat amp16 = 0;
92 93

            /* Hopefully this is optimised for the common case - not clipping */
94
            amp16 = (SFLDataFormat) amp;
95

96 97
            if (amp == amp16)
                return amp16;
98

99
            if (amp > INT16_MAX)
100
                return INT16_MAX;
101

102
            return INT16_MIN;
103 104
        }

105 106
        void block4_encode(int band, int d)
        {
107 108 109 110
            int wd1 = 0;
            int wd2 = 0;
            int wd3 = 0;
            int i = 0;
111 112

            /* Block 4, RECONS */
113 114
            encode_state_.band[band].d[0] = d;
            encode_state_.band[band].r[0] = saturate(encode_state_.band[band].s + d);
115 116

            /* Block 4, PARREC */
117
            encode_state_.band[band].p[0] = saturate(encode_state_.band[band].sz + d);
118 119

            /* Block 4, UPPOL2 */
120 121

            for (i = 0;  i < 3;  i++)
122
                encode_state_.band[band].sg[i] = encode_state_.band[band].p[i] >> 15;
123

124
            wd1 = saturate(encode_state_.band[band].a[1] << 2);
125

126
            wd2 = (encode_state_.band[band].sg[0] == encode_state_.band[band].sg[1])  ?  -wd1  :  wd1;
127 128 129 130

            if (wd2 > 32767)
                wd2 = 32767;

131
            wd3 = (wd2 >> 7) + ((encode_state_.band[band].sg[0] == encode_state_.band[band].sg[2])  ?  128  :  -128);
132

133
            wd3 += (encode_state_.band[band].a[2]*32512) >> 15;
134 135 136 137 138 139

            if (wd3 > 12288)
                wd3 = 12288;
            else if (wd3 < -12288)
                wd3 = -12288;

140
            encode_state_.band[band].ap[2] = wd3;
141 142

            /* Block 4, UPPOL1 */
143
            encode_state_.band[band].sg[0] = encode_state_.band[band].p[0] >> 15;
144

145
            encode_state_.band[band].sg[1] = encode_state_.band[band].p[1] >> 15;
146

147
            wd1 = (encode_state_.band[band].sg[0] == encode_state_.band[band].sg[1])  ?  192  :  -192;
148

149
            wd2 = (encode_state_.band[band].a[1]*32640) >> 15;
150

151
            encode_state_.band[band].ap[1] = saturate(wd1 + wd2);
152

153
            wd3 = saturate(15360 - encode_state_.band[band].ap[2]);
154

155 156 157 158
            if (encode_state_.band[band].ap[1] > wd3)
                encode_state_.band[band].ap[1] = wd3;
            else if (encode_state_.band[band].ap[1] < -wd3)
                encode_state_.band[band].ap[1] = -wd3;
159 160 161 162

            /* Block 4, UPZERO */
            wd1 = (d == 0)  ?  0  :  128;

163
            encode_state_.band[band].sg[0] = d >> 15;
164 165

            for (i = 1;  i < 7;  i++) {
166 167 168 169
                encode_state_.band[band].sg[i] = encode_state_.band[band].d[i] >> 15;
                wd2 = (encode_state_.band[band].sg[i] == encode_state_.band[band].sg[0])  ?  wd1  :  -wd1;
                wd3 = (encode_state_.band[band].b[i]*32640) >> 15;
                encode_state_.band[band].bp[i] = saturate(wd2 + wd3);
170 171 172 173
            }

            /* Block 4, DELAYA */
            for (i = 6;  i > 0;  i--) {
174 175
                encode_state_.band[band].d[i] = encode_state_.band[band].d[i - 1];
                encode_state_.band[band].b[i] = encode_state_.band[band].bp[i];
176 177 178
            }

            for (i = 2;  i > 0;  i--) {
179 180 181
                encode_state_.band[band].r[i] = encode_state_.band[band].r[i - 1];
                encode_state_.band[band].p[i] = encode_state_.band[band].p[i - 1];
                encode_state_.band[band].a[i] = encode_state_.band[band].ap[i];
182 183 184
            }

            /* Block 4, FILTEP */
185
            wd1 = saturate(encode_state_.band[band].r[1] + encode_state_.band[band].r[1]);
186

187
            wd1 = (encode_state_.band[band].a[1]*wd1) >> 15;
188

189
            wd2 = saturate(encode_state_.band[band].r[2] + encode_state_.band[band].r[2]);
190

191
            wd2 = (encode_state_.band[band].a[2]*wd2) >> 15;
192

193
            encode_state_.band[band].sp = saturate(wd1 + wd2);
194 195

            /* Block 4, FILTEZ */
196
            encode_state_.band[band].sz = 0;
197 198

            for (i = 6;  i > 0;  i--) {
199 200
                wd1 = saturate(encode_state_.band[band].d[i] + encode_state_.band[band].d[i]);
                encode_state_.band[band].sz += (encode_state_.band[band].b[i]*wd1) >> 15;
201 202
            }

203
            encode_state_.band[band].sz = saturate(encode_state_.band[band].sz);
204 205

            /* Block 4, PREDIC */
206
            encode_state_.band[band].s = saturate(encode_state_.band[band].sp + encode_state_.band[band].sz);
207 208 209

        }

210 211
        void block4_decode(int band, int d)
        {
212 213 214 215
            int wd1 = 0;
            int wd2 = 0;
            int wd3 = 0;
            int i = 0;
216 217

            /* Block 4, RECONS */
218 219
            decode_state_.band[band].d[0] = d;
            decode_state_.band[band].r[0] = saturate(decode_state_.band[band].s + d);
220 221

            /* Block 4, PARREC */
222
            decode_state_.band[band].p[0] = saturate(decode_state_.band[band].sz + d);
223 224 225 226

            /* Block 4, UPPOL2 */

            for (i = 0;  i < 3;  i++)
227
                decode_state_.band[band].sg[i] = decode_state_.band[band].p[i] >> 15;
228

229
            wd1 = saturate(decode_state_.band[band].a[1] << 2);
230

231
            wd2 = (decode_state_.band[band].sg[0] == decode_state_.band[band].sg[1])  ?  -wd1  :  wd1;
232 233 234 235

            if (wd2 > 32767)
                wd2 = 32767;

236
            wd3 = (decode_state_.band[band].sg[0] == decode_state_.band[band].sg[2])  ?  128  :  -128;
237 238 239

            wd3 += (wd2 >> 7);

240
            wd3 += (decode_state_.band[band].a[2]*32512) >> 15;
241 242 243 244 245 246

            if (wd3 > 12288)
                wd3 = 12288;
            else if (wd3 < -12288)
                wd3 = -12288;

247
            decode_state_.band[band].ap[2] = wd3;
248 249

            /* Block 4, UPPOL1 */
250
            decode_state_.band[band].sg[0] = decode_state_.band[band].p[0] >> 15;
251

252
            decode_state_.band[band].sg[1] = decode_state_.band[band].p[1] >> 15;
253

254
            wd1 = (decode_state_.band[band].sg[0] == decode_state_.band[band].sg[1])  ?  192  :  -192;
255

256
            wd2 = (decode_state_.band[band].a[1]*32640) >> 15;
257

258
            decode_state_.band[band].ap[1] = saturate(wd1 + wd2);
259

260
            wd3 = saturate(15360 - decode_state_.band[band].ap[2]);
261

262 263 264 265
            if (decode_state_.band[band].ap[1] > wd3)
                decode_state_.band[band].ap[1] = wd3;
            else if (decode_state_.band[band].ap[1] < -wd3)
                decode_state_.band[band].ap[1] = -wd3;
266 267 268 269

            /* Block 4, UPZERO */
            wd1 = (d == 0)  ?  0  :  128;

270
            decode_state_.band[band].sg[0] = d >> 15;
271 272

            for (i = 1;  i < 7;  i++) {
273 274 275 276
                decode_state_.band[band].sg[i] = decode_state_.band[band].d[i] >> 15;
                wd2 = (decode_state_.band[band].sg[i] == decode_state_.band[band].sg[0])  ?  wd1  :  -wd1;
                wd3 = (decode_state_.band[band].b[i]*32640) >> 15;
                decode_state_.band[band].bp[i] = saturate(wd2 + wd3);
277 278 279 280
            }

            /* Block 4, DELAYA */
            for (i = 6;  i > 0;  i--) {
281 282
                decode_state_.band[band].d[i] = decode_state_.band[band].d[i - 1];
                decode_state_.band[band].b[i] = decode_state_.band[band].bp[i];
283 284 285
            }

            for (i = 2;  i > 0;  i--) {
286 287 288
                decode_state_.band[band].r[i] = decode_state_.band[band].r[i - 1];
                decode_state_.band[band].p[i] = decode_state_.band[band].p[i - 1];
                decode_state_.band[band].a[i] = decode_state_.band[band].ap[i];
289 290 291
            }

            /* Block 4, FILTEP */
292
            wd1 = saturate(decode_state_.band[band].r[1] + decode_state_.band[band].r[1]);
293

294
            wd1 = (decode_state_.band[band].a[1]*wd1) >> 15;
295

296
            wd2 = saturate(decode_state_.band[band].r[2] + decode_state_.band[band].r[2]);
297

298
            wd2 = (decode_state_.band[band].a[2]*wd2) >> 15;
299

300
            decode_state_.band[band].sp = saturate(wd1 + wd2);
301 302

            /* Block 4, FILTEZ */
303
            decode_state_.band[band].sz = 0;
304 305

            for (i = 6;  i > 0;  i--) {
306 307
                wd1 = saturate(decode_state_.band[band].d[i] + decode_state_.band[band].d[i]);
                decode_state_.band[band].sz += (decode_state_.band[band].b[i]*wd1) >> 15;
308 309
            }

310
            decode_state_.band[band].sz = saturate(decode_state_.band[band].sz);
311 312

            /* Block 4, PREDIC */
313
            decode_state_.band[band].s = saturate(decode_state_.band[band].sp + decode_state_.band[band].sz);
314 315
        }

316 317
        int g722_decode(SFLDataFormat amp[], const uint8_t g722_data[], int len)
        {
318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367
            static const int wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 };
            static const int rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3,  2, 1, 0 };
            static const int ilb[32] = {
                2048, 2093, 2139, 2186, 2233, 2282, 2332,
                2383, 2435, 2489, 2543, 2599, 2656, 2714,
                2774, 2834, 2896, 2960, 3025, 3091, 3158,
                3228, 3298, 3371, 3444, 3520, 3597, 3676,
                3756, 3838, 3922, 4008
            };
            static const int wh[3] = {0, -214, 798};
            static const int rh2[4] = {2, 1, 2, 1};
            static const int qm2[4] = {-7408, -1616,  7408,   1616};
            static const int qm4[16] = {
                0, -20456, -12896,  -8968,
                -6288,  -4240,  -2584,  -1200,
                20456,  12896,   8968,   6288,
                4240,   2584,   1200,      0
            };
            static const int qm5[32] = {
                -280,   -280, -23352, -17560,
                -14120, -11664,  -9752,  -8184,
                -6864,  -5712,  -4696,  -3784,
                -2960,  -2208,  -1520,   -880,
                23352,  17560,  14120,  11664,
                9752,   8184,   6864,   5712,
                4696,   3784,   2960,   2208,
                1520,    880,    280,   -280
            };
            static const int qm6[64] = {
                -136,   -136,   -136,   -136,
                -24808, -21904, -19008, -16704,
                -14984, -13512, -12280, -11192,
                -10232,  -9360,  -8576,  -7856,
                -7192,  -6576,  -6000,  -5456,
                -4944,  -4464,  -4008,  -3576,
                -3168,  -2776,  -2400,  -2032,
                -1688,  -1360,  -1040,   -728,
                24808,  21904,  19008,  16704,
                14984,  13512,  12280,  11192,
                10232,   9360,   8576,   7856,
                7192,   6576,   6000,   5456,
                4944,   4464,   4008,   3576,
                3168,   2776,   2400,   2032,
                1688,   1360,   1040,    728,
                432,    136,   -432,   -136
            };
            static const int qmf_coeffs[12] = {
                3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
            };

368 369 370 371 372 373 374 375 376 377 378 379 380 381
            int dlowt = 0;
            int rlow = 0;
            int ihigh = 0;
            int dhigh = 0;
            int rhigh = 0;
            int xout1 = 0;
            int xout2 = 0;
            int wd1 = 0;
            int wd2 = 0;
            int wd3 = 0;
            int code = 0;
            int outlen = 0;
            int i = 0;
            int j = 0;
382 383 384 385

            outlen = 0;
            rhigh = 0;

386 387


388
            for (j = 0;  j < len;) {
389
                if (decode_state_.packed) {
390
                    /* Unpack the code bits */
391 392 393
                    if (decode_state_.in_bits < decode_state_.bits_per_sample) {
                        decode_state_.in_buffer |= (g722_data[j++] << decode_state_.in_bits);
                        decode_state_.in_bits += 8;
394 395
                    }

396
                    code = decode_state_.in_buffer & ((1 << decode_state_.bits_per_sample) - 1);
397

398 399
                    decode_state_.in_buffer >>= decode_state_.bits_per_sample;
                    decode_state_.in_bits -= decode_state_.bits_per_sample;
400 401 402 403
                } else {
                    code = g722_data[j++];
                }

404
                switch (decode_state_.bits_per_sample) {
405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429

                    default:

                    case 8:
                        wd1 = code & 0x3F;
                        ihigh = (code >> 6) & 0x03;
                        wd2 = qm6[wd1];
                        wd1 >>= 2;
                        break;

                    case 7:
                        wd1 = code & 0x1F;
                        ihigh = (code >> 5) & 0x03;
                        wd2 = qm5[wd1];
                        wd1 >>= 1;
                        break;

                    case 6:
                        wd1 = code & 0x0F;
                        ihigh = (code >> 4) & 0x03;
                        wd2 = qm4[wd1];
                        break;
                }

                /* Block 5L, LOW BAND INVQBL */
430
                wd2 = (decode_state_.band[0].det*wd2) >> 15;
431 432

                /* Block 5L, RECONS */
433
                rlow = decode_state_.band[0].s + wd2;
434 435 436 437 438 439 440 441 442 443

                /* Block 6L, LIMIT */
                if (rlow > 16383)
                    rlow = 16383;
                else if (rlow < -16384)
                    rlow = -16384;

                /* Block 2L, INVQAL */
                wd2 = qm4[wd1];

444
                dlowt = (decode_state_.band[0].det*wd2) >> 15;
445 446 447 448

                /* Block 3L, LOGSCL */
                wd2 = rl42[wd1];

449
                wd1 = (decode_state_.band[0].nb*127) >> 7;
450 451 452 453 454 455 456 457

                wd1 += wl[wd2];

                if (wd1 < 0)
                    wd1 = 0;
                else if (wd1 > 18432)
                    wd1 = 18432;

458
                decode_state_.band[0].nb = wd1;
459 460

                /* Block 3L, SCALEL */
461
                wd1 = (decode_state_.band[0].nb >> 6) & 31;
462

463
                wd2 = 8 - (decode_state_.band[0].nb >> 11);
464 465 466

                wd3 = (wd2 < 0)  ? (ilb[wd1] << -wd2)  : (ilb[wd1] >> wd2);

467
                decode_state_.band[0].det = wd3 << 2;
468

469
                block4_decode(0, dlowt);
470

471
                if (!decode_state_.eight_k) {
472 473
                    /* Block 2H, INVQAH */
                    wd2 = qm2[ihigh];
474
                    dhigh = (decode_state_.band[1].det*wd2) >> 15;
475
                    /* Block 5H, RECONS */
476
                    rhigh = dhigh + decode_state_.band[1].s;
477 478 479 480 481 482 483 484 485 486
                    /* Block 6H, LIMIT */

                    if (rhigh > 16383)
                        rhigh = 16383;
                    else if (rhigh < -16384)
                        rhigh = -16384;

                    /* Block 2H, INVQAH */
                    wd2 = rh2[ihigh];

487
                    wd1 = (decode_state_.band[1].nb*127) >> 7;
488 489 490 491 492 493 494 495

                    wd1 += wh[wd2];

                    if (wd1 < 0)
                        wd1 = 0;
                    else if (wd1 > 22528)
                        wd1 = 22528;

496
                    decode_state_.band[1].nb = wd1;
497 498

                    /* Block 3H, SCALEH */
499
                    wd1 = (decode_state_.band[1].nb >> 6) & 31;
500

501
                    wd2 = 10 - (decode_state_.band[1].nb >> 11);
502 503 504

                    wd3 = (wd2 < 0)  ? (ilb[wd1] << -wd2)  : (ilb[wd1] >> wd2);

505
                    decode_state_.band[1].det = wd3 << 2;
506

507
                    block4_decode(1, dhigh);
508 509
                }

510 511 512
                if (decode_state_.itu_test_mode) {
                    amp[outlen++] = (SFLDataFormat)(rlow << 1);
                    amp[outlen++] = (SFLDataFormat)(rhigh << 1);
513
                } else {
514 515
                    if (decode_state_.eight_k) {
                        amp[outlen++] = (SFLDataFormat) rlow;
516 517 518
                    } else {
                        /* Apply the receive QMF */
                        for (i = 0;  i < 22;  i++)
519
                            decode_state_.x[i] = decode_state_.x[i + 2];
520

521
                        decode_state_.x[22] = rlow + rhigh;
522

523
                        decode_state_.x[23] = rlow - rhigh;
524 525 526 527 528 529

                        xout1 = 0;

                        xout2 = 0;

                        for (i = 0;  i < 12;  i++) {
530 531
                            xout2 += decode_state_.x[2*i]*qmf_coeffs[i];
                            xout1 += decode_state_.x[2*i + 1]*qmf_coeffs[11 - i];
532 533
                        }

534
                        amp[outlen++] = (SFLDataFormat)(xout1 >> 12);
535

536
                        amp[outlen++] = (SFLDataFormat)(xout2 >> 12);
537 538 539 540 541 542 543
                    }
                }
            }

            return outlen;
        }

544 545
        int g722_encode(uint8_t g722_data[], const SFLDataFormat amp[], int len)
        {
546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593
            static const int q6[32] = {
                0,   35,   72,  110,  150,  190,  233,  276,
                323,  370,  422,  473,  530,  587,  650,  714,
                786,  858,  940, 1023, 1121, 1219, 1339, 1458,
                1612, 1765, 1980, 2195, 2557, 2919,    0,    0
            };
            static const int iln[32] = {
                0, 63, 62, 31, 30, 29, 28, 27,
                26, 25, 24, 23, 22, 21, 20, 19,
                18, 17, 16, 15, 14, 13, 12, 11,
                10,  9,  8,  7,  6,  5,  4,  0
            };
            static const int ilp[32] = {
                0, 61, 60, 59, 58, 57, 56, 55,
                54, 53, 52, 51, 50, 49, 48, 47,
                46, 45, 44, 43, 42, 41, 40, 39,
                38, 37, 36, 35, 34, 33, 32,  0
            };
            static const int wl[8] = {
                -60, -30, 58, 172, 334, 538, 1198, 3042
            };
            static const int rl42[16] = {
                0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
            };
            static const int ilb[32] = {
                2048, 2093, 2139, 2186, 2233, 2282, 2332,
                2383, 2435, 2489, 2543, 2599, 2656, 2714,
                2774, 2834, 2896, 2960, 3025, 3091, 3158,
                3228, 3298, 3371, 3444, 3520, 3597, 3676,
                3756, 3838, 3922, 4008
            };
            static const int qm4[16] = {
                0, -20456, -12896, -8968,
                -6288,  -4240,  -2584, -1200,
                20456,  12896,   8968,  6288,
                4240,   2584,   1200,     0
            };
            static const int qm2[4] = {
                -7408,  -1616,   7408,   1616
            };
            static const int qmf_coeffs[12] = {
                3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
            };
            static const int ihn[3] = {0, 1, 0};
            static const int ihp[3] = {0, 3, 2};
            static const int wh[3] = {0, -214, 798};
            static const int rh2[4] = {2, 1, 2, 1};

594 595 596 597 598 599 600 601 602 603 604 605 606 607
            int dlow = 0;
            int dhigh = 0;
            int el = 0;
            int wd = 0;
            int wd1 = 0;
            int ril = 0;
            int wd2 = 0;
            int il4 = 0;
            int ih2 = 0;
            int wd3 = 0;
            int eh = 0;
            int mih = 0;
            int i = 0;
            int j = 0;
608
            /* Low and high band PCM from the QMF */
609 610 611
            int xlow = 0;
            int xhigh = 0;
            int g722_bytes = 0;
612
            /* Even and odd tap accumulators */
613 614 615 616 617
            int sumeven = 0;
            int sumodd = 0;
            int ihigh = 0;
            int ilow = 0;
            int code = 9;
618 619 620 621 622

            g722_bytes = 0;
            xhigh = 0;

            for (j = 0;  j < len;) {
623
                if (encode_state_.itu_test_mode) {
624 625 626
                    xlow =
                        xhigh = amp[j++] >> 1;
                } else {
627
                    if (encode_state_.eight_k) {
628 629 630 631 632
                        xlow = amp[j++];
                    } else {
                        /* Apply the transmit QMF */
                        /* Shuffle the buffer down */
                        for (i = 0;  i < 22;  i++)
633
                            encode_state_.x[i] = encode_state_.x[i + 2];
634

635
                        encode_state_.x[22] = amp[j++];
636

637
                        encode_state_.x[23] = amp[j++];
638 639 640 641 642 643 644

                        /* Discard every other QMF output */
                        sumeven = 0;

                        sumodd = 0;

                        for (i = 0;  i < 12;  i++) {
645 646
                            sumodd += encode_state_.x[2*i]*qmf_coeffs[i];
                            sumeven += encode_state_.x[2*i + 1]*qmf_coeffs[11 - i];
647 648 649 650 651 652 653 654 655
                        }

                        xlow = (sumeven + sumodd) >> 13;

                        xhigh = (sumeven - sumodd) >> 13;
                    }
                }

                /* Block 1L, SUBTRA */
656
                el = saturate(xlow - encode_state_.band[0].s);
657 658 659 660 661

                /* Block 1L, QUANTL */
                wd = (el >= 0)  ?  el  :  - (el + 1);

                for (i = 1;  i < 30;  i++) {
662
                    wd1 = (q6[i]*encode_state_.band[0].det) >> 12;
663 664 665 666 667 668 669 670 671 672

                    if (wd < wd1)
                        break;
                }

                ilow = (el < 0)  ?  iln[i]  :  ilp[i];

                /* Block 2L, INVQAL */
                ril = ilow >> 2;
                wd2 = qm4[ril];
673
                dlow = (encode_state_.band[0].det*wd2) >> 15;
674 675 676

                /* Block 3L, LOGSCL */
                il4 = rl42[ril];
677 678
                wd = (encode_state_.band[0].nb*127) >> 7;
                encode_state_.band[0].nb = wd + wl[il4];
679

680 681 682 683
                if (encode_state_.band[0].nb < 0)
                    encode_state_.band[0].nb = 0;
                else if (encode_state_.band[0].nb > 18432)
                    encode_state_.band[0].nb = 18432;
684 685

                /* Block 3L, SCALEL */
686
                wd1 = (encode_state_.band[0].nb >> 6) & 31;
687

688
                wd2 = 8 - (encode_state_.band[0].nb >> 11);
689 690 691

                wd3 = (wd2 < 0)  ? (ilb[wd1] << -wd2)  : (ilb[wd1] >> wd2);

692
                encode_state_.band[0].det = wd3 << 2;
693

694
                block4_encode(0, dlow);
695

696
                if (encode_state_.eight_k) {
697
                    /* Just leave the high bits as zero */
698
                    code = (0xC0 | ilow) >> (8 - encode_state_.bits_per_sample);
699 700
                } else {
                    /* Block 1H, SUBTRA */
701
                    eh = saturate(xhigh - encode_state_.band[1].s);
702 703 704

                    /* Block 1H, QUANTH */
                    wd = (eh >= 0)  ?  eh  :  - (eh + 1);
705
                    wd1 = (564*encode_state_.band[1].det) >> 12;
706 707 708 709 710
                    mih = (wd >= wd1)  ?  2  :  1;
                    ihigh = (eh < 0)  ?  ihn[mih]  :  ihp[mih];

                    /* Block 2H, INVQAH */
                    wd2 = qm2[ihigh];
711
                    dhigh = (encode_state_.band[1].det*wd2) >> 15;
712 713 714

                    /* Block 3H, LOGSCH */
                    ih2 = rh2[ihigh];
715 716
                    wd = (encode_state_.band[1].nb*127) >> 7;
                    encode_state_.band[1].nb = wd + wh[ih2];
717

718 719 720 721
                    if (encode_state_.band[1].nb < 0)
                        encode_state_.band[1].nb = 0;
                    else if (encode_state_.band[1].nb > 22528)
                        encode_state_.band[1].nb = 22528;
722 723

                    /* Block 3H, SCALEH */
724
                    wd1 = (encode_state_.band[1].nb >> 6) & 31;
725

726
                    wd2 = 10 - (encode_state_.band[1].nb >> 11);
727 728 729

                    wd3 = (wd2 < 0)  ? (ilb[wd1] << -wd2)  : (ilb[wd1] >> wd2);

730
                    encode_state_.band[1].det = wd3 << 2;
731

732
                    block4_encode(1, dhigh);
733

734
                    code = ((ihigh << 6) | ilow) >> (8 - encode_state_.bits_per_sample);
735 736
                }

737
                if (encode_state_.packed) {
738
                    /* Pack the code bits */
739 740
                    encode_state_.out_buffer |= (code << encode_state_.out_bits);
                    encode_state_.out_bits += encode_state_.bits_per_sample;
741

742 743 744 745
                    if (encode_state_.out_bits >= 8) {
                        g722_data[g722_bytes++] = (uint8_t)(encode_state_.out_buffer & 0xFF);
                        encode_state_.out_bits -= 8;
                        encode_state_.out_buffer >>= 8;
746 747 748 749 750 751 752
                    }
                } else {
                    g722_data[g722_bytes++] = (uint8_t) code;
                }
            }

            return g722_bytes;
753 754
        }

755 756
        g722_state_t decode_state_;
        g722_state_t encode_state_;
757 758 759
};

// the class factories
760
// cppcheck-suppress unusedFunction
761
extern "C" sfl::Codec* CODEC_ENTRY()
762
{
763
    return new G722;
764 765
}

766
// cppcheck-suppress unusedFunction
767
extern "C" void destroy(sfl::Codec* a)
768
{
769 770 771
    delete a;
}