/* $NetBSD: sbc_encode.c,v 1.14 2025/12/31 10:08:17 nia Exp $ */

/*-
 * Copyright (c) 2015 - 2016 Nathanial Sloss <nathanialsloss@yahoo.com.au>
 * All rights reserved.
 *
 *              This software is dedicated to the memory of -
 *         Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012.
 *
 *              Barry was a man who loved his music.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/cdefs.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/param.h>
#include <errno.h>
#include <endian.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <inttypes.h>

#include <sbc_coeffs.h>
#include <sbc_crc.h>
#include "sbc_encode.h"

static uint8_t make_crc(uint8_t);
uint8_t Crc8(uint8_t, uint8_t *, size_t, ssize_t);
static ssize_t make_frame(uint8_t *, int16_t *);
static ssize_t parseFrame(uint8_t *, int16_t *);
static void calc_scalefactors(int32_t samples[16][2][8]);
static uint8_t calc_scalefactors_joint(int32_t sb_sample[16][2][8]);
static size_t sbc_encode(int16_t *, int32_t *);
static void calc_bitneed(void);
static ssize_t get_bits(uint8_t *, int, uint32_t *);
static ssize_t move_bits(uint8_t *, int, uint32_t);
static ssize_t move_bits_crc(uint8_t *, int, uint32_t);
static size_t sbc_decode(int32_t *, int16_t *);

uint32_t scalefactor[2][8];
int bits[2][8];
int global_chan = 2;
int global_bands = 8;
int global_blocks = 16;
int global_volume = 0;
uint8_t global_bitpool = 32;
uint8_t global_mode = MODE_STEREO;
uint8_t global_alloc = ALLOC_LOUDNESS;
uint8_t global_freq = FREQ_44_1K;
uint8_t global_bands_config = BANDS_8;
uint8_t global_block_config = BLOCKS_16;
uint8_t join = 0;

#define SYNCWORD        0x9c

struct a2dp_frame_header {
        uint8_t syncword;
        uint8_t config;
        uint8_t bitpool;
        uint8_t crc;
};

struct a2dp_frame_header_joint {
        uint8_t syncword;
        uint8_t config;
        uint8_t bitpool;
        uint8_t crc;
        uint8_t joint;
};


struct a2dp_frame_mono {
        struct a2dp_frame_header header;
        uint8_t scale[4];
        uint8_t samples[256];
};

struct a2dp_frame_joint {
        struct a2dp_frame_header_joint header;
        uint8_t scale[8];
        uint8_t samples[256];
};

struct a2dp_frame {
        struct a2dp_frame_header header;
        uint8_t scale[8];
        uint8_t samples[256];
};

struct rtpHeader {
        uint8_t id;             /* Just random number. */
        uint8_t id2;
        uint8_t seqnumMSB;      /* Packet seq. number most significant byte. */
        uint8_t seqnumLSB;
        uint8_t ts3;            /* Timestamp most significant byte. */
        uint8_t ts2;
        uint8_t ts1;
        uint8_t ts0;            /* Timestamp least significant byte. */
        uint8_t reserved3;
        uint8_t reserved2;
        uint8_t reserved1;
        uint8_t reserved0;      /* Reseverd least significant byte set to 1. */
        uint8_t numFrames;      /* Number of sbc frames in this packet. */
};
            
/* Loudness offset allocations. */
int loudnessoffset8[4][8] = {
    { -2, 0, 0, 0, 0, 0, 0, 1 },
    { -3, 0, 0, 0, 0, 0, 1, 2 },
    { -4, 0, 0, 0, 0, 0, 1, 2 },
    { -4, 0, 0, 0, 0, 0, 1, 2 },
};

int loudnessoffset4[4][4] = {
    { -1, 0, 0, 0 },
    { -2, 0, 0, 1 },
    { -2, 0, 0, 1 },
    { -2, 0, 0, 1 }
};

u_int
FLS(uint8_t x)
{
        u_int numset = 0;
        while (x) {
                if (x & 1)
                        break;
                x >>= 1;
                numset++;
        }
        return numset;
}

uint8_t
calc_scalefactors_joint(int32_t sb_sample[16][2][8])
{
        int64_t sb_j[16][2];
        uint32_t lz, x, y;
        int32_t ax;
        int block, sb;
        unsigned int joint;

        joint = 0;
        for (sb = 0; sb < global_bands - 1; sb++) {
                for (block = 0; block < global_blocks; block++) {
                        sb_j[block][0] = (sb_sample[block][0][sb]) +
                            (sb_sample[block][1][sb]);
                        sb_j[block][1] = (sb_sample[block][0][sb]) -
                            (sb_sample[block][1][sb]);
                }

                x = 1 << 15;
                y = 1 << 15;
                for (block = 0; block < global_blocks; block++) {
                        ax = abs((int32_t)(sb_j[block][0] / 2));
                        if (ax)
                                x |= (uint32_t)ax;
                        ax = abs((int32_t)(sb_j[block][1] / 2));
                        if (ax)
                                y |= (uint32_t)ax;
                }

                lz = 1;
                while (!(x & __BIT(30))) {
                        lz++;
                        x <<= 1;
                }
                x = 16 - lz;

                lz = 1;
                while (!(y & __BIT(30))) {
                        lz++;
                        y <<= 1;
                }
                y = 16 - lz;

                if ((scalefactor[0][sb] + scalefactor[1][sb]) > x + y) {
                        joint |= (unsigned int)(1 << (global_bands - sb - 1));
                        scalefactor[0][sb] = x;
                        scalefactor[1][sb] = y;
                        for (block = 0; block < global_blocks; block++) {
                                sb_sample[block][0][sb] = (int32_t)
                                    (sb_j[block][0] / 2);
                                sb_sample[block][1][sb] = (int32_t)
                                    (sb_j[block][1] / 2);
                        }
                }
        }

        return (uint8_t)joint;
}

void
calc_scalefactors(int32_t samples[16][2][8])
{
        uint32_t lz, x;
        int32_t ax;
        int ch, sb, block;

        for (ch = 0; ch < global_chan; ch++) {
                for (sb = 0; sb < global_bands; sb++) {
                        x = 1 << 16;
                        for (block = 0; block < global_blocks; block++) {
                                ax = abs((int32_t)samples[block][ch][sb]);
                                if (ax)
                                        x |= (uint32_t)ax;
                        }

                        lz = 1;
                        while (!(x & __BIT(30))) {
                                lz++;
                                x <<= 1;
                        }
                        scalefactor[ch][sb] =  16 - lz;
                }
        }
}

void
calc_bitneed(void)
{
        int32_t bitneed[2][8];
        int32_t max_bitneed, bitcount;
        int32_t slicecount, bitslice;
        int32_t loudness;
        int ch, sb,start_chan = 0;

        if (global_mode == MODE_DUAL)
                global_chan = 1;
next_chan:
        max_bitneed=0;
        bitcount=0;
        slicecount=0;

        if (global_alloc == ALLOC_SNR) {
                for (ch = start_chan; ch < global_chan; ch++) {
                        for (sb = 0; sb < global_bands; sb++) {
                                bitneed[ch][sb] = (int32_t)scalefactor[ch][sb];

                                if (bitneed[ch][sb] > max_bitneed)
                                        max_bitneed = bitneed[ch][sb];
                        }
                }
        } else {
                for (ch = start_chan; ch < global_chan; ch++) {
                        for (sb = 0; sb < global_bands; sb++) {
                                if (scalefactor[ch][sb] == 0)
                                        bitneed[ch][sb] = -5;
                                else {
                                        if (global_bands == 8) {
                                                loudness = (int32_t)
                                                    ((int)scalefactor[ch][sb]
                                                    - loudnessoffset8
                                                    [3 - FLS(global_freq)][sb]);
                                        } else {
                                                loudness = (int32_t)
                                                    ((int)scalefactor[ch][sb]
                                                    - loudnessoffset4
                                                    [3 - FLS(global_freq)][sb]);
                                        }
                                        if (loudness > 0)
                                                bitneed[ch][sb] = loudness / 2;
                                        else
                                                bitneed[ch][sb] = loudness;
                                }
                                if (bitneed[ch][sb] > max_bitneed)
                                        max_bitneed = bitneed[ch][sb];
                        }
                }
        }

        slicecount = bitcount = 0;
        bitslice = max_bitneed+1;
        do {
                bitslice--;
                bitcount += slicecount;
                slicecount = 0;
                for (ch = start_chan; ch < global_chan; ch++) {
                        for (sb = 0; sb < global_bands; sb++) {
                                if((bitneed[ch][sb] > bitslice + 1)&&
                                    (bitneed[ch][sb] < bitslice + 16))
                                        slicecount++;
                                else if(bitneed[ch][sb] == bitslice + 1)
                                        slicecount += 2;
                        }
                }
        } while (bitcount + slicecount < global_bitpool);
        if (bitcount + slicecount == global_bitpool) {
                bitcount += slicecount;
                bitslice--;
        }

        for (ch = start_chan; ch < global_chan; ch++) {
                for (sb = 0; sb < global_bands; sb++) {
                        if (bitneed[ch][sb] < bitslice + 2)
                                bits[ch][sb] = 0;
                        else {
                                bits[ch][sb] = bitneed[ch][sb] - bitslice;
                                if (bits[ch][sb] > 16)
                                        bits[ch][sb] = 16;
                        }
                }
        }

        if (global_mode == MODE_DUAL)
                ch = start_chan;
        else
                ch = 0;
        sb = 0;
        while (bitcount < global_bitpool && sb < global_bands) {
                if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
                        bits[ch][sb]++;
                        bitcount++;
                } else if ((bitneed[ch][sb] == bitslice + 1) &&
                    (global_bitpool > bitcount + 1)) {
                        bits[ch][sb] = 2;
                        bitcount += 2;
                }
                if (global_chan == 1 || start_chan == 1)
                        sb++;
                else if (ch == 1) {
                        ch = 0;
                        sb++;
                } else
                        ch = 1;
        }

        if (global_mode == MODE_DUAL)
                ch = start_chan;
        else
                ch = 0;
        sb = 0;
        while (bitcount < global_bitpool && sb < global_bands) {
                if (bits[ch][sb] < 16) {
                        bits[ch][sb]++;
                        bitcount++;
                }
                if (global_chan == 1 || start_chan == 1)
                        sb++;
                else if (ch == 1) {
                        ch = 0;
                        sb++;
                } else
                        ch = 1;
        }

        if (global_mode == MODE_DUAL && start_chan == 0) {
                start_chan = 1;
                global_chan = 2;
                goto next_chan;
        }
}

ssize_t
get_bits(uint8_t *data, int numbits, uint32_t *sample)
{
        static uint64_t cache = 0;
        static int cache_pos = 0;
        uint64_t tmp_cache;
        ssize_t written = 0;

        while (cache_pos < numbits) {
                cache <<= 8;
                cache |= *data & 0xff;
                data++;
                written++;
                cache_pos += 8;
        }

        if (numbits == 0) {
                if (cache_pos >= 8) { 
                        cache_pos -= 8;
                        tmp_cache = cache >> cache_pos;
                        *sample = (uint32_t)tmp_cache;
                        written--;
                }
                if (cache_pos) {
                        *sample = (uint32_t)cache;
                        written--;
                }
                cache = 0;
                cache_pos = 0;
        } else {
                cache_pos -= numbits;
                tmp_cache = cache & __BITS((uintmax_t)(numbits + cache_pos),
                    (uintmax_t)cache_pos); 
                cache &= ~tmp_cache;
                tmp_cache >>= cache_pos;
                *sample = (uint32_t)tmp_cache;
        }
        return written;
}

ssize_t
move_bits(uint8_t *data, int numbits, uint32_t sample)
{
        static uint64_t cache = 0;
        static int cache_pos = 0;
        uint8_t tmp_cache;
        ssize_t written = 0;

        if (numbits == 0) {
                while (cache_pos >= 8) { 
                        cache_pos -= 8;
                        tmp_cache = (uint8_t)(cache >> cache_pos);
                        *data++ = tmp_cache;
                        written++;
                }
                if (cache_pos)
                        *data = (uint8_t)cache;
                cache = 0;
                cache_pos = 0;
        } else {
                cache_pos += numbits;
                cache <<= numbits;
                cache |= sample & __BITS((uintmax_t)numbits, 0); 
                while (cache_pos >= 8) {
                        cache_pos -= 8;
                        tmp_cache = (uint8_t)(cache >> cache_pos);
                        *data++ = tmp_cache;
                        written++;
                }
        }
        return written;
}

ssize_t
move_bits_crc(uint8_t *data, int numbits, uint32_t sample)
{
        static uint64_t cache = 0;
        static int cache_pos = 0;
        uint8_t tmp_cache;
        ssize_t written = 0;

        if (numbits > 8 || numbits < 0)
                return 0;

        if (numbits == 0) {
                while  (cache_pos >= 8) { 
                        cache_pos -= 8;
                        tmp_cache = (uint8_t)(cache >> cache_pos);
                        *data++ = tmp_cache;
                        written++;
                }
                if (cache_pos)
                        *data = (uint8_t)cache;
                cache = 0;
                cache_pos = 0;
        } else {
                cache_pos += numbits;
                cache <<= numbits;
                cache |= sample & __BITS((uintmax_t)numbits, 0); 
                if (cache_pos >= 8) {
                        cache_pos -= 8;
                        tmp_cache = (uint8_t)(cache >> cache_pos);
                        *data = tmp_cache;
                        written++;
                }
        }
        return written;
}

size_t
sbc_encode(int16_t *input, int32_t *samples)
{
        int64_t delta[2][8], levels[2][8], S[80];
        static int32_t L[80], R[80];
        int32_t *X, Z[80], Y[80];
        int32_t output[16][2][8];
        int32_t audioout;
        int16_t left[8], right[8], *data;
        size_t numsamples;
        int i, k, block, chan, sb;

        for (block = 0;block < global_blocks; block++) {

                k = 0;
                for (i = 0;i < global_bands;i++) {
                        left[i] = input[k++];
                        if (global_chan == 2)
                                right[i] = input[k++];
                }
                input += k;

                for (chan = 0; chan < global_chan; chan++) {
                        if (chan == 0) {
                                X = L;
                                data = left;
                        } else {
                                X = R;
                                data = right;
                        }

                        for (i = (global_bands * 10) - 1;i > global_bands -1;
                            i--)
                                X[i] = X[i - global_bands];
                        k = 0;
                        for (i = global_bands - 1; i >= 0; i--)
                                X[i] = (int16_t)le16toh(data[k++]);
                        for (i = 0; i < global_bands * 10; i++) {
                                if (global_bands == 8) {
                                        Z[i] = (sbc_coeffs8[i] * (X[i] <<
                                            global_volume)); 
                                } else {
                                        Z[i] = (sbc_coeffs4[i] * (X[i] <<
                                            global_volume)); 
                                }
                        }
                        for (i = 0; i < global_bands * 2; i++) {
                                Y[i] = 0;
                                for (k = 0;k < 5;k++)
                                        Y[i] += Z[i + k * global_bands * 2];
                        }
                        for (i = 0; i < global_bands; i++) {
                                S[i] = 0;
                                for (k = 0; k < global_bands * 2; k++) {
                                        if (global_bands == 8) {
                                                S[i] += (int64_t)cosdata8[i][k]
                                                    * (int64_t)Y[k];
                                        } else {
                                                S[i] += (int64_t)cosdata4[i][k]
                                                    * (int64_t)Y[k];
                                        }
                                }
                                output[block][chan][i] = (int32_t)(S[i] /
                                    SIMULTI);
                        }
                }
        }

        calc_scalefactors(output);
        if (global_mode == MODE_JOINT)
                join = calc_scalefactors_joint(output);

        calc_bitneed();

        for(chan = 0; chan < global_chan; chan++) {
                for (sb = 0; sb < global_bands; sb++) {
                        levels[chan][sb] = ((1 << bits[chan][sb]) - 1) <<
                                (15 - scalefactor[chan][sb]);
                        delta[chan][sb] = 1 << (scalefactor[chan][sb] + 16);
                }
        }

        numsamples = 0;
        for (block = 0; block < global_blocks; block++) {
                for (chan = 0; chan < global_chan; chan++) {
                        for (sb = 0; sb < global_bands; sb++) {
                                if (bits[chan][sb] == 0)
                                        continue;

                                audioout = (int32_t)((levels[chan][sb] *
                                    (delta[chan][sb] + (int32_t)output[block]
                                    [chan][sb])) >> 32);

                                samples[numsamples++] = audioout;
                        }
                }
        }
        return numsamples;
}

size_t
sbc_decode(int32_t *samples, int16_t *pcm)
{
        static int64_t levels[2][8], delta[2][8];
        static int64_t  S[8], V[2][160], L[160], R[160];
        int64_t *X;
        static int64_t U[2][160], W[2][160];
        int64_t audioout;
        int chan, block, sb, position, i, k;
        size_t numsamples;

        for(chan = 0; chan < global_chan; chan++) {
                for (sb = 0; sb < global_bands; sb++) {
                        levels[chan][sb] = (1 << bits[chan][sb]) - 1;
                        delta[chan][sb] = 1 << (scalefactor[chan][sb] + 1);
                }
        }

        numsamples = 0;
        for (block = 0; block < global_blocks; block++) {
                for (chan = 0; chan < global_chan; chan++) {
                        for (sb = 0; sb < global_bands; sb++) {
                                if (bits[chan][sb] == 0)
                                        audioout = 0;
                                else {
                                        audioout = ((((samples[numsamples]
                                            * 2) + 1) * delta[chan][sb]) /
                                            levels[chan][sb]) -
                                            delta[chan][sb];
                                }
                                samples[numsamples] = (int32_t)audioout;
                                numsamples++;
                        }
                }
        }

        if (global_mode == MODE_JOINT) {
                k = 0;
                while (k < (global_blocks * global_bands * global_chan)) {
                        for (sb = 0; sb < global_bands; sb++) {
                                if (join & 1 << (global_bands - sb - 1)) {
                                        audioout = samples[k];
                                        samples[k] = (2 * samples[k]) + (2 *
                                            samples[k + global_bands]);
                                        samples[k + global_bands] = 
                                            (int32_t)(2 * audioout) - (2 *
                                            samples[k + global_bands]);
                                        samples[k] /= 2;
                                        samples[k + global_bands] /= 2;
                                }
                                k++;
                        }
                        k += global_bands;
                }
        }


        position = 0;
        for (block = 0; block < global_blocks; block++) {
                for (chan = 0; chan < global_chan; chan++) {
                        if (chan == 0)
                                X = L;
                        else
                                X = R;

                        for (i = 0; i < global_bands; i++)
                                S[i] = samples[position++];
                        for (i = ((global_bands * 20) - 1); i >= (global_bands
                            * 2); i--)
                                V[chan][i] = V[chan][i - (global_bands * 2)];

                        for (k = 0; k < global_bands * 2; k++) {
                                V[chan][k] = 0;
                                for (i = 0; i < global_bands; i++) {
                                        if (global_bands == 8) {
                                                V[chan][k] += cosdecdata8[i][k]
                                                    * S[i];
                                        } else {
                                                V[chan][k] += cosdecdata4[i][k]
                                                    * S[i];
                                        }
                                }
                                V[chan][k] /= SIMULTI;          
                        }

                        for (i = 0; i <= 4; i++) {
                                for (k = 0; k < global_bands; k++) {
                                        U[chan][(i * global_bands * 2) + k] =
                                            V[chan][(i * global_bands * 4) + k];
                                        U[chan][(i * global_bands
                                            * 2) + global_bands + k] =
                                        V[chan][(i * global_bands * 4) +
                                            (global_bands * 3) + k];
                                }
                        }
                        
                        for (i = 0; i < global_bands * 10; i++) {
                                if (global_bands == 4) {
                                        W[chan][i] = U[chan][i] *
                                            (sbc_coeffs4[i] *  -4);
                                } else if (global_bands == 8) {
                                        W[chan][i] = U[chan][i] *
                                            (sbc_coeffs8[i] *  -8);
                                }
                        }

                        for (k = 0; k < global_bands; k++) {
                                int offset = k + (block * global_bands);
                                X[offset] = 0;
                                for (i = 0; i < 10; i++) {
                                        X[offset] += W[chan][k + (i *
                                            global_bands)];     
                                }
                                X[offset] /= COEFFSMULTI;
                        }
                }
        }

        k = 0;
        i = 0;
        while (k < (global_blocks * global_bands)) {
                pcm[i++] = (int16_t)L[k];
                if (global_chan == 2)
                        pcm[i++] = (int16_t)R[k];
                k++;
        }

        return numsamples;
}

uint8_t
Crc8(uint8_t inCrc, uint8_t *inData, size_t numbits, ssize_t inBytes)
{
        uint8_t data;
        int i;

        for (i = 0; i < (int)inBytes; i++) {
                data = inCrc ^ inData[i];
                
                if (numbits == 8)
                        data = sbc_crc8[data];
                else if (numbits == 4)
                        data = sbc_crc4[data];

                inCrc = data;
        }
        return inCrc;
}

uint8_t
make_crc(uint8_t config)
{
        uint8_t crc, data[11];
        int i, j;
        uint8_t *dataStart = data;
        uint8_t *crcData = data;


        crcData += move_bits_crc(crcData, 8, config);
        crcData += move_bits_crc(crcData, 8, global_bitpool);
        if (global_mode == MODE_JOINT) {
                if (global_bands == 8)
                        crcData += move_bits_crc(crcData, 8, join);
                else
                        crcData += move_bits_crc(crcData, 4, join);
        }

        for(i = 0; i < global_chan; i++) {
                for (j = 0; j < global_bands; j++)
                        crcData += move_bits_crc(crcData, 4, scalefactor[i][j]);
        }

        crc = Crc8(0xf, data, 8, (crcData - dataStart));

        if (global_mode == MODE_JOINT && global_bands == 4) {
                move_bits_crc(crcData, 0, 0);
                crc = Crc8(crc, crcData, 4, 1);
        }

        return crc;
}

ssize_t
make_frame(uint8_t *frame, int16_t *input)
{
        static int32_t samples[256 * 2];
        uint8_t config, crc;
        int block, chan, sb, j, i;

        uint8_t *frameStart = frame;

        config = (uint8_t)(((3 - FLS(global_freq)) << 6) |
            ((3 - FLS(global_block_config)) << 4) | ((3 - FLS(global_mode))
            << 2) | ((FLS(global_alloc)) << 1) |
            (1 - FLS(global_bands_config)));

        sbc_encode(input,samples);

        crc = make_crc(config);

        frame += move_bits(frame, 8, SYNCWORD);
        frame += move_bits(frame, 8, config);
        frame += move_bits(frame, 8, global_bitpool);
        frame += move_bits(frame, 8, crc);

        if (global_mode == MODE_JOINT && global_bands == 8)
                frame += move_bits(frame, 8, join);
        else if (global_mode == MODE_JOINT && global_bands == 4)
                frame += move_bits(frame, 4, join);

        for(i = 0; i < global_chan; i++) {
                for (j = 0; j < global_bands; j++)
                        frame += move_bits(frame, 4, scalefactor[i][j]);
        }

        i = 0;
        for (block = 0; block < global_blocks; block++) {
                for (chan = 0; chan < global_chan; chan++) {
                        for (sb = 0; sb < global_bands; sb++) {
                                if (bits[chan][sb] == 0)
                                        continue;

                                frame += move_bits(frame, bits[chan][sb],
                                    (uint32_t)samples[i++]);
                        }
                }
        }
        frame += move_bits(frame, 0, 0);

        return frame - frameStart;
}

static ssize_t
readloop(int fd, void *buf, size_t nbytes)
{
        size_t count;
        ssize_t ret;

        count = 0;
        while (nbytes > 0) {
                ret = read(fd, ((char *)buf) + count, nbytes);
                if (ret < 0) {
                        if (count == 0)
                                return ret;
                        break;
                }
                if (ret == 0)
                        break;
                count += (size_t)ret;
                nbytes -= (size_t)ret;
        }

        return (ssize_t) count;
}

ssize_t
stream(int in, int outfd, uint8_t mode, uint8_t freq, uint8_t bands, uint8_t
    blocks, uint8_t alloc_method, uint8_t bitpool, size_t mtu, int volume)
{
        struct rtpHeader myHeader;
        struct timeval myTime;
        uint8_t *whole, *frameData;
        int16_t music[2048];
        ssize_t len, mySize[16], offset, next_pkt;
        ssize_t pkt_len;
        size_t readsize, totalSize;
        size_t frequency;
        static size_t ts = 0;
        static uint16_t seqnumber = 0;
        static time_t prevTime, readTime, sleepTime, timeNow;
        int numpkts, tries;

        global_mode = mode;
        global_bitpool = bitpool;
        global_alloc = alloc_method;
        global_freq = freq;
        global_volume = volume;

        global_bands_config = bands;
        if (bands == BANDS_8)
                global_bands = 8;
        else
                global_bands = 4;

        if (blocks == BLOCKS_4)
                global_blocks = 4;
        else if (blocks == BLOCKS_8)
                global_blocks = 8;
        else if (blocks == BLOCKS_12)
                global_blocks = 12;
        else {
                blocks = BLOCKS_16;
                global_blocks = 16;
        }

        global_block_config = blocks;

        global_chan = 2;
        if (global_mode == MODE_MONO)
                global_chan = 1;

        if (global_freq == FREQ_16K)
                frequency = 16000;
        else if (global_freq == FREQ_32K)
                frequency = 32000;
        else if (global_freq == FREQ_48K)
                frequency = 48000;
        else
                frequency = 44100;

        memset(&myHeader, 0, sizeof(myHeader));
        myHeader.id = 0x80;     /* RTP v2 */
        myHeader.id2 = 0x60;    /* payload type 96. */
        myHeader.seqnumMSB = (uint8_t)(seqnumber >> 8);
        myHeader.seqnumLSB = (uint8_t)seqnumber;
        myHeader.ts3 = (uint8_t)(ts >> 24);
        myHeader.ts2 = (uint8_t)(ts >> 16);
        myHeader.ts1 = (uint8_t)(ts >> 8);
        myHeader.ts0 = (uint8_t)ts;
        myHeader.reserved0 = 0x01;

        totalSize = sizeof(myHeader);

        frameData = malloc(mtu);
        if (frameData == NULL)
                return -1;

        readsize = (size_t)((global_blocks * global_bands * global_chan) * 2);

        numpkts = 0;
        next_pkt = 0;
        len = 0;
        pkt_len = 80;
        readTime = 0;
        while (totalSize + ((size_t)pkt_len * 2) <= mtu) {

                len = readloop(in, music, readsize);
                readTime += (time_t)readsize;
                if (len < (int)readsize)
                        break;

                pkt_len = make_frame(frameData + next_pkt, music);

                mySize[numpkts] = pkt_len;
                next_pkt += pkt_len;
                totalSize += (size_t)mySize[numpkts];
                numpkts++;

                if (numpkts > 12)
                        break;
        }

        if (len < (int)readsize) {
                free(frameData);
                return -1;
        }

        readTime = readTime * 1000000 / 2 / global_chan / (time_t)frequency;

        myHeader.numFrames = (uint8_t)numpkts;
        whole = malloc(totalSize);
        if (whole == NULL)
                return -1;

        memcpy(whole, &myHeader, sizeof(myHeader));
        offset = sizeof(myHeader);

        memcpy(whole + offset, frameData, (size_t)next_pkt);
        free(frameData);

        /* Wait if necessary to avoid rapid playback. */
        gettimeofday(&myTime, NULL);
        timeNow = myTime.tv_sec * 1000000 + myTime.tv_usec;
        if (prevTime == 0)
                prevTime = timeNow;
        else
                sleepTime += readTime - (timeNow - prevTime);
        if (sleepTime >= 1000) {
                usleep(500);
                sleepTime -= 1000;
        }
        prevTime = timeNow;

        tries = 1;
send_again:
        len = write(outfd, whole, totalSize);

        if (len == -1 && errno == EAGAIN) {
                tries --;
                if (tries >= 0) {
                        usleep(1);
                        goto send_again;
                } else
                        len = (ssize_t)totalSize;
        } else if (len == -1 && (errno == EINPROGRESS ||
            errno == EWOULDBLOCK)) {
                usleep(1);
                        len = (ssize_t)totalSize;
        }

        seqnumber++;
        ts += (1000000 * (size_t)(global_blocks * global_bands)
            / frequency) * (size_t)numpkts;

        free(whole);

        return len;
}

ssize_t
recvstream(int in, int outfd)
{
        struct rtpHeader myHeader;
        static uint8_t frameData[8192];
        uint8_t *myFrame;
        static int16_t music[4096];
        size_t decsize, totalSize, offset;
        int numpkts, cur_pkt;
        ssize_t len, res, pkt_len, next_pkt;
        static ssize_t readlen = 0;
        totalSize = 0;

        len = read(in, frameData + readlen, (size_t)(1000 - readlen));
        readlen += len;
        if (readlen <= 0) {
                readlen = 0;
                return -1;
        }

        if (readlen < (int)sizeof(myHeader)) {
                readlen = 0;
                return -1;
        }

        memcpy(&myHeader, frameData, sizeof(myHeader));
        if (myHeader.id != 0x80) {
                return -1;
        }

        numpkts = myHeader.numFrames;
        if (numpkts < 1 || numpkts > 13) {
                return -1;
        }

        myFrame = frameData + sizeof(myHeader);
        next_pkt = 0;
        pkt_len = 0;
        cur_pkt = 0;
        offset = 0;
        while (cur_pkt < numpkts) {
                pkt_len = parseFrame(myFrame + next_pkt, &music[offset]);
                decsize = (size_t)(global_blocks * global_bands * global_chan);

                next_pkt += pkt_len;
                totalSize += 2 * decsize;
                offset += decsize;
                cur_pkt++;
        }

        res = (ssize_t)(sizeof(myHeader)) + next_pkt;
                readlen -= res;
        if (readlen > 0)
                memcpy(frameData, frameData + res, (size_t)readlen);

send_again:
        len = write(outfd, music, totalSize);

        if (len == -1 && errno == EAGAIN)
                goto send_again;

        return len;
}

ssize_t
parseFrame(uint8_t *myFrame, int16_t *pcm)
{
        uint8_t mode, bitpool, alloc_method, freq, bands, config, myCrc, blkCrc;
        int sb, chan, block, blocks, i, j;
        int32_t samples[768];

        uint8_t *myFrame_start = myFrame;
        if (*myFrame++ != SYNCWORD)
                return -1;

        config = *myFrame++;
        bitpool = *myFrame++;
        myCrc = *myFrame++;

        freq = (uint8_t)(1 << (3 - ((config & 0xc0) >> 6)));
        blocks = 1 << (3 - ((config & 0x30) >> 4));
        mode = (uint8_t)(1 << (3 - ((config & 0x0c) >> 2)));
        alloc_method = (uint8_t)(1 << (((config & 0x02) >> 1)));
        bands = (uint8_t)(1 << (1 - (config & 0x01)));

        global_mode = mode;
        global_bitpool = bitpool;
        global_alloc = alloc_method;
        global_freq = freq;

        global_bands_config = bands;
        if (bands == BANDS_8)
                global_bands = 8;
        else
                global_bands = 4;

        if (blocks == BLOCKS_4)
                global_blocks = 4;
        else if (blocks == BLOCKS_8)
                global_blocks = 8;
        else if (blocks == BLOCKS_12)
                global_blocks = 12;
        else {
                blocks = BLOCKS_16;
                global_blocks = 16;
        }

        global_block_config = (uint8_t)blocks;

        global_chan = 2;
        if (global_mode == MODE_MONO)
                global_chan = 1;

        if (global_mode == MODE_JOINT && global_bands == 8)
                myFrame += get_bits(myFrame, 8, (uint32_t *)&join);
        else if (global_mode == MODE_JOINT && global_bands == 4)
                myFrame += get_bits(myFrame, 4, (uint32_t *)&join);
        else
                join = 0;

        for(i = 0; i < global_chan; i++) {
                for (j = 0; j < global_bands; j++) {
                        myFrame += get_bits(myFrame, 4,
                            (uint32_t *)&scalefactor[i][j]);
                }
        }

        blkCrc = make_crc(config);
        if (blkCrc != myCrc)
                return 0;

        calc_bitneed();
        i = 0;
        for (block = 0; block < global_blocks; block++) {
                for (chan = 0; chan < global_chan; chan++) {
                        for (sb = 0; sb < global_bands; sb++) {
                                samples[i] = 0;
                                if (bits[chan][sb] == 0) {
                                        i++;
                                        continue;
                                }

                                myFrame += get_bits(myFrame, bits[chan][sb],
                                    (uint32_t *)&samples[i++]);
                        }
                }
        }
        myFrame += get_bits(myFrame, 0, (uint32_t *)&samples[i]);
        sbc_decode(samples, pcm);

        return myFrame - myFrame_start;
}