#!/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)