xref: /btstack/test/sbc/sbc.py (revision c21a9c2f3621834ccdbca1efaf4d958002a4ef9c)

#!/usr/bin/env python
import numpy as np
import wave
import struct
import sys


# channel mode
MONO = 0
DUAL_CHANNEL = 1
STEREO = 2
JOINT_STEREO = 3

# allocation method
LOUDNESS = 0
SNR = 1

Proto_4_40 = [
    0.00000000E+00, 5.36548976E-04, 1.49188357E-03, 2.73370904E-03,
    3.83720193E-03, 3.89205149E-03, 1.86581691E-03, -3.06012286E-03,
    1.09137620E-02, 2.04385087E-02, 2.88757392E-02, 3.21939290E-02,
    2.58767811E-02, 6.13245186E-03, -2.88217274E-02, -7.76463494E-02,
    1.35593274E-01, 1.94987841E-01, 2.46636662E-01, 2.81828203E-01,
    2.94315332E-01, 2.81828203E-01, 2.46636662E-01, 1.94987841E-01,
    -1.35593274E-01, -7.76463494E-02, -2.88217274E-02, 6.13245186E-03,
    2.58767811E-02, 3.21939290E-02, 2.88757392E-02, 2.04385087E-02,
    -1.09137620E-02, -3.06012286E-03, 1.86581691E-03, 3.89205149E-03,
    3.83720193E-03, 2.73370904E-03, 1.49188357E-03, 5.36548976E-04
]

Proto_8_80 = [
    0.00000000E+00, 1.56575398E-04, 3.43256425E-04, 5.54620202E-04,
    8.23919506E-04, 1.13992507E-03, 1.47640169E-03, 1.78371725E-03,
    2.01182542E-03, 2.10371989E-03, 1.99454554E-03, 1.61656283E-03,
    9.02154502E-04, -1.78805361E-04, -1.64973098E-03, -3.49717454E-03,
    5.65949473E-03, 8.02941163E-03, 1.04584443E-02, 1.27472335E-02,
    1.46525263E-02, 1.59045603E-02, 1.62208471E-02, 1.53184106E-02,
    1.29371806E-02, 8.85757540E-03, 2.92408442E-03, -4.91578024E-03,
    -1.46404076E-02, -2.61098752E-02, -3.90751381E-02, -5.31873032E-02,
    6.79989431E-02, 8.29847578E-02, 9.75753918E-02, 1.11196689E-01,
    1.23264548E-01, 1.33264415E-01, 1.40753505E-01, 1.45389847E-01,
    1.46955068E-01, 1.45389847E-01, 1.40753505E-01, 1.33264415E-01,
    1.23264548E-01, 1.11196689E-01, 9.75753918E-02, 8.29847578E-02,
    -6.79989431E-02, -5.31873032E-02, -3.90751381E-02, -2.61098752E-02,
    -1.46404076E-02, -4.91578024E-03, 2.92408442E-03, 8.85757540E-03,
    1.29371806E-02, 1.53184106E-02, 1.62208471E-02, 1.59045603E-02,
    1.46525263E-02, 1.27472335E-02, 1.04584443E-02, 8.02941163E-03,
    -5.65949473E-03, -3.49717454E-03, -1.64973098E-03, -1.78805361E-04,
    9.02154502E-04, 1.61656283E-03, 1.99454554E-03, 2.10371989E-03,
    2.01182542E-03, 1.78371725E-03, 1.47640169E-03, 1.13992507E-03,
    8.23919506E-04, 5.54620202E-04, 3.43256425E-04, 1.56575398E-04
]

crc_table = [
    0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53,
    0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
    0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E,
    0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
    0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4,
    0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
    0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19,
    0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
    0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40,
    0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
    0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D,
    0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
    0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7,
    0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
    0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A,
    0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
    0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75,
    0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
    0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8,
    0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
    0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2,
    0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
    0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F,
    0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
    0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66,
    0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
    0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB,
    0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
    0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1,
    0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
    0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C,
    0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4
]
# offset table for 4-subbands
offset4 = np.array([[ -1, 0, 0, 0 ],
                    [ -2, 0, 0, 1 ],
                    [ -2, 0, 0, 1 ],
                    [ -2, 0, 0, 1 ]
                    ])

# offset tables for 8-subbands
offset8 = np.array([[ -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 ]
                    ])

nr_blocks = [4, 8, 12, 16]
nr_subbands = [4, 8]
sampling_frequency =[16000, 32000, 44100, 48000]

class SBCFrame:
    syncword = 0
    sampling_frequency = 0
    nr_blocks = 0
    channel_mode = 0
    nr_channels = 0
    allocation_method = 0
    nr_subbands = 0
    bitpool = 0
    crc_check = 0
    # pro subband - 1
    join = np.zeros(8, dtype = np.uint8)
    scale_factor =  np.zeros(shape=(2, 8), dtype = np.int32)
    scalefactor =  np.zeros(shape=(2, 8), dtype = np.int32)
    audio_sample = np.zeros(shape = (16,2,8), dtype = np.uint16)
    sb_sample = np.zeros(shape = (16,2,8), dtype = np.uint16)
    X = np.zeros(8, dtype = np.int16)
    EX = np.zeros(8)
    pcm = np.array([], dtype = np.int16)
    bits    = np.zeros(shape=(2, 8))
    levels = np.zeros(shape=(2, 8), dtype = np.int32)

    def __init__(self, nr_blocks, nr_subbands, nr_channels, sampling_frequency, bitpool):
        self.nr_blocks = nr_blocks
        self.nr_subbands = nr_subbands
        self.nr_channels = nr_channels
        self.sampling_frequency = sampling_frequency
        self.bitpool = bitpool
        self.scale_factor = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32)
        self.scalefactor = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32)
        self.audio_sample = np.zeros(shape=(nr_blocks, nr_channels, nr_subbands), dtype = np.uint16)
        self.sb_sample = np.zeros(shape=(nr_blocks, nr_channels, nr_subbands), dtype = np.uint16)
        self.levels = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32)
        self.EX = np.zeros(nr_subbands)
        return

    def __str__(self):
        res =  "SBCFrameHeader:"
        res = res + "\n - syncword %d" % self.syncword
        res = res + "\n - sample frequency %d" % self.sampling_frequency
        res = res + "\n - nr blocks %d" % self.nr_blocks

        if self.channel_mode == MONO:
            res = res + "\n - channel mode MONO"
        elif self.channel_mode == DUAL_CHANNEL:
            res = res + "\n - channel mode DUAL CHANNEL"
        elif self.channel_mode == STEREO:
            res = res + "\n - channel mode STEREO"
        elif self.channel_mode == JOINT_STEREO:
            res = res + "\n - channel mode JOINT STEREO"
        else:
            res = res + "\n - channel mode %d" % self.channel_mode

        res = res + "\n - nr channels %d" % self.nr_channels

        if self.allocation_method == 1:
            res = res + "\n - allocation method SNR"
        elif self.allocation_method == 0:
            res = res + "\n - allocation method LOUNDNESS"
        else:
            res = res + "\n - allocation method %d" % self.allocation_method

        res = res + "\n - nr subbands %d" % self.nr_subbands
        res = res + "\n - bitpool %d" % self.bitpool
        res = res + "\n - crc check %d" % self.crc_check

        return res



def sbc_bit_allocation_stereo_joint(frame, ch):
    bitneed = np.zeros(shape=(frame.nr_channels, frame.nr_subbands))
    bits    = np.zeros(shape=(frame.nr_channels, frame.nr_subbands))
    loudness = 0

    if frame.allocation_method == SNR:
        for ch in range(frame.nr_channels):
            for sb in range(frame.nr_subbands):
                bitneed[ch][sb] = frame.scale_factor[ch][sb]
    else:
        for ch in range(frame.nr_channels):
            for sb in range(frame.nr_subbands):
                if frame.scale_factor[ch][sb] == 0:
                    bitneed[ch][sb] = -5
                else:
                    if frame.nr_subbands == 4:
                        loudness = scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb]
                    else:
                        if frame.nr_subbands == 4:
                            loudness = frame.scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb]
                        else:
                            loudness = frame.scale_factor[ch][sb] - offset8[frame.sampling_frequency][sb]
                        if loudness > 0:
                            bitneed[ch][sb] = loudness/2
                        else:
                            bitneed[ch][sb] = loudness

    # search the maximum bitneed index
    max_bitneed = 0
    for ch in range(frame.nr_channels):
        for sb in range(frame.nr_subbands):
            if bitneed[ch][sb] > max_bitneed:
                max_bitneed = bitneed[ch][sb]

    # # print "max_bitneed: ", max_bitneed

    # calculate how many bitslices fit into the bitpool
    bitcount = 0
    slicecount = 0
    bitslice = max_bitneed + 1 #/* init just above the largest sf */

    while True:
        bitslice = bitslice - 1
        bitcount = bitcount + slicecount
        slicecount = 0
        for ch in range(frame.nr_channels):
            for sb in range(frame.nr_subbands):
                if (bitneed[ch][sb] > bitslice+1) and (bitneed[ch][sb] < bitslice+16):
                    slicecount = slicecount + 1
                elif bitneed[ch][sb] == bitslice + 1:
                    slicecount = slicecount + 2
        if bitcount + slicecount >= frame.bitpool:
            break

    # print "bitcount %d, slicecount %d" % (bitcount, slicecount)

    if bitcount + slicecount == frame.bitpool:
        bitcount = bitcount + slicecount
        bitslice = bitslice - 1

    # bits are distributed until the last bitslice is reached
    for ch in range(frame.nr_channels):
        for sb in range(frame.nr_subbands):
            if bitneed[ch][sb] < bitslice+2 :
               bits[ch][sb]=0;
            else:
                bits[ch][sb] = min(bitneed[ch][sb]-bitslice,16)


    ch = 0
    sb = 0
    while bitcount < frame.bitpool and sb < frame.nr_subbands:
        if bits[ch][sb] >= 2 and bits[ch][sb] < 16:
               bits[ch][sb] = bits[ch][sb] + 1
               bitcount = bitcount + 1

        elif (bitneed[ch][sb] == bitslice+1) and (frame.bitpool > bitcount+1):
            bits[ch][sb] = 2
            bitcount += 2

        if ch == 1:
            ch = 0
            sb = sb + 1
        else:
            ch = 1

    ch = 0
    sb = 0
    while bitcount < frame.bitpool and sb < frame.nr_subbands:
        if bits[ch][sb] < 16:
            bits[ch][sb] = bits[ch][sb] + 1
            bitcount = bitcount + 1
        if ch == 1:
            ch = 0
            sb = sb + 1
        else:
            ch = 1

    return bits


def sbc_bit_allocation_mono_dual(frame):
    #print "Bit allocation for mono/dual channel"
    bitneed = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32)
    bits    = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32)
    loudness = 0

    for ch in range(frame.nr_channels):
        # bitneed values are derived from the scale factors
        if frame.allocation_method == SNR:
            for sb in range(frame.nr_subbands):
                bitneed[ch][sb] = frame.scale_factor[ch][sb]
        else:
            for sb in range(frame.nr_subbands):
                if frame.scale_factor[ch][sb] == 0:
                    bitneed[ch][sb] = -5
                else:
                    if frame.nr_subbands == 4:
                        loudness = frame.scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb]
                    else:
                        loudness = frame.scale_factor[ch][sb] - offset8[frame.sampling_frequency][sb]
                    if loudness > 0:
                        bitneed[ch][sb] = loudness/2
                    else:
                        bitneed[ch][sb] = loudness

        # search the maximum bitneed index
        max_bitneed = 0
        for sb in range(frame.nr_subbands):
            if bitneed[ch][sb] > max_bitneed:
                max_bitneed = bitneed[ch][sb]

        #print "max_bitneed: ", max_bitneed

        # calculate how many bitslices fit into the bitpool
        bitcount = 0
        slicecount = 0
        bitslice = max_bitneed + 1 #/* init just above the largest sf */

        while True:
            bitslice = bitslice - 1
            bitcount = bitcount + slicecount
            slicecount = 0
            for sb in range(frame.nr_subbands):
                if (bitneed[ch][sb] > bitslice+1) and (bitneed[ch][sb] < bitslice+16):
                    slicecount = slicecount + 1
                elif bitneed[ch][sb] == bitslice + 1:
                    slicecount = slicecount + 2
            if bitcount + slicecount >= frame.bitpool:
                break

        #print "bitcount %d, slicecount %d" % (bitcount, slicecount)

        if bitcount + slicecount == frame.bitpool:
            bitcount = bitcount + slicecount
            bitslice = bitslice - 1

        # bits are distributed until the last bitslice is reached
        for sb in range(frame.nr_subbands):
            if bitneed[ch][sb] < bitslice+2 :
               bits[ch][sb]=0;
            else:
                bits[ch][sb] = min(bitneed[ch][sb]-bitslice,16)

        # The remaining bits are allocated starting at subband 0.
        sb = 0
        while bitcount < frame.bitpool and sb < frame.nr_subbands:
            if bits[ch][sb] >= 2 and bits[ch][sb] < 16:
                   bits[ch][sb] = bits[ch][sb] + 1
                   bitcount = bitcount + 1

            elif (bitneed[ch][sb] == bitslice+1) and (frame.bitpool > bitcount+1):
                bits[ch][sb] = 2
                bitcount += 2

            sb = sb + 1


        sb = 0
        while bitcount < frame.bitpool and sb < frame.nr_subbands:
            if bits[ch][sb] < 16:
                bits[ch][sb] = bits[ch][sb] + 1
                bitcount = bitcount + 1
            sb = sb + 1

    return bits

def sbc_bit_allocation(frame):
    if frame.channel_mode == MONO or frame.channel_mode == DUAL_CHANNEL:
        return sbc_bit_allocation_mono_dual(frame)
    elif frame.channel_mode == STEREO or frame.channel_mode == JOINT_STEREO:
        return sbc_bit_allocation_stereo_joint(frame)
    else:
        print "Wrong channel mode ", frame.channel_mode
        return -1

def sbc_sampling_frequency_index(sample_rate):
    sbc_sampling_frequency_index = 0
    for i in range(len(sampling_frequency)):
        if sample_rate == sampling_frequency[i]:
            sbc_sampling_frequency_index = i
            break
    return sbc_sampling_frequency_index

# static uint8_t sbc_crc8(const uint8_t * data, size_t len)
# 158 {
# 159         uint8_t crc = 0x0f;
# 160         size_t i;
# 161         uint8_t octet;
# 162
# 163         for (i = 0; i < len / 8; i++)
# 164                 crc = crc_table[crc ^ data[i]];
# 165
# 166         octet = data[i];
# 167         for (i = 0; i < len % 8; i++) {
# 168                 char bit = ((octet ^ crc) & 0x80) >> 7;
# 169
# 170                 crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
# 171
# 172                 octet = octet << 1;
# 173         }
# 174
# 175         return crc;
# 176 }

def sbc_crc8(data, data_len):
    crc = 0x0f

    j = 0
    for i in range(data_len / 8):
        crc = crc_table[crc ^ data[i]]
        j = i + 1

    bits_left = data_len%8
    if bits_left:
        octet = data[j]
        for i in range(data_len%8):
            bit = ((octet ^ crc) & 0x80) >> 7
            if bit:
                bit = 0x1d
            crc = ((crc & 0x7f) << 1) ^ bit
            octet = octet << 1
    return crc


bitstream = []
bitstream_index = -1
bitstream_bits_available = 0

def init_bitstream():
    global bitstream, bitstream_bits_available, bitstream_index
    bitstream = []
    bitstream_index = -1
    bitstream_bits_available = 0

def add_bit(bit):
    global bitstream, bitstream_bits_available, bitstream_index
    if bitstream_bits_available == 0:
        bitstream.append(0)
        bitstream_bits_available = 8
        bitstream_index += 1

    bitstream[bitstream_index] |= bit << (bitstream_bits_available - 1)
    bitstream_bits_available -= 1


def add_bits(bits, len):
    global bitstream, bitstream_bits_available
    for i in range(len):
        add_bit((bits >> (len-1-i)) & 1)


def calculate_crc(frame):
    global bitstream, bitstream_bits_available, bitstream_index
    init_bitstream()

    add_bits(frame.sampling_frequency, 2)
    add_bits(frame.nr_blocks/4-1, 2)
    add_bits(frame.channel_mode, 2)
    add_bits(frame.allocation_method, 1)
    add_bits(frame.nr_subbands/4-1, 1)
    add_bits(frame.bitpool, 8)

    if frame.channel_mode == JOINT_STEREO:
        for sb in range(frame.nr_subbands):
            add_bits(frame.join[sb],1)

    for ch in range(frame.nr_channels):
        for sb in range(frame.nr_subbands):
            add_bits(frame.scale_factor[ch][sb], 4)
    # bitstream_len = 16 + frame.nr_subbands + frame.nr_channels * frame.nr_subbands * 4
    bitstream_len = (bitstream_index + 1) * 8
    if bitstream_bits_available:
        bitstream_len += (8-bitstream_bits_available)
    return sbc_crc8(bitstream, bitstream_len)