1#!/usr/bin/env python 2import numpy as np 3import wave 4import struct 5import sys 6 7# channel mode 8MONO = 0 9DUAL_CHANNEL = 1 10STEREO = 2 11JOINT_STEREO = 3 12 13# allocation method 14LOUDNESS = 0 15SNR = 1 16 17Proto_4_40 = [ 18 0.00000000E+00, 5.36548976E-04, 1.49188357E-03, 2.73370904E-03, 19 3.83720193E-03, 3.89205149E-03, 1.86581691E-03, -3.06012286E-03, 20 1.09137620E-02, 2.04385087E-02, 2.88757392E-02, 3.21939290E-02, 21 2.58767811E-02, 6.13245186E-03, -2.88217274E-02, -7.76463494E-02, 22 1.35593274E-01, 1.94987841E-01, 2.46636662E-01, 2.81828203E-01, 23 2.94315332E-01, 2.81828203E-01, 2.46636662E-01, 1.94987841E-01, 24 -1.35593274E-01, -7.76463494E-02, -2.88217274E-02, 6.13245186E-03, 25 2.58767811E-02, 3.21939290E-02, 2.88757392E-02, 2.04385087E-02, 26 -1.09137620E-02, -3.06012286E-03, 1.86581691E-03, 3.89205149E-03, 27 3.83720193E-03, 2.73370904E-03, 1.49188357E-03, 5.36548976E-04 28] 29 30Proto_8_80 = [ 31 0.00000000E+00, 1.56575398E-04, 3.43256425E-04, 5.54620202E-04, 32 8.23919506E-04, 1.13992507E-03, 1.47640169E-03, 1.78371725E-03, 33 2.01182542E-03, 2.10371989E-03, 1.99454554E-03, 1.61656283E-03, 34 9.02154502E-04, -1.78805361E-04, -1.64973098E-03, -3.49717454E-03, 35 5.65949473E-03, 8.02941163E-03, 1.04584443E-02, 1.27472335E-02, 36 1.46525263E-02, 1.59045603E-02, 1.62208471E-02, 1.53184106E-02, 37 1.29371806E-02, 8.85757540E-03, 2.92408442E-03, -4.91578024E-03, 38 -1.46404076E-02, -2.61098752E-02, -3.90751381E-02, -5.31873032E-02, 39 6.79989431E-02, 8.29847578E-02, 9.75753918E-02, 1.11196689E-01, 40 1.23264548E-01, 1.33264415E-01, 1.40753505E-01, 1.45389847E-01, 41 1.46955068E-01, 1.45389847E-01, 1.40753505E-01, 1.33264415E-01, 42 1.23264548E-01, 1.11196689E-01, 9.75753918E-02, 8.29847578E-02, 43 -6.79989431E-02, -5.31873032E-02, -3.90751381E-02, -2.61098752E-02, 44 -1.46404076E-02, -4.91578024E-03, 2.92408442E-03, 8.85757540E-03, 45 1.29371806E-02, 1.53184106E-02, 1.62208471E-02, 1.59045603E-02, 46 1.46525263E-02, 1.27472335E-02, 1.04584443E-02, 8.02941163E-03, 47 -5.65949473E-03, -3.49717454E-03, -1.64973098E-03, -1.78805361E-04, 48 9.02154502E-04, 1.61656283E-03, 1.99454554E-03, 2.10371989E-03, 49 2.01182542E-03, 1.78371725E-03, 1.47640169E-03, 1.13992507E-03, 50 8.23919506E-04, 5.54620202E-04, 3.43256425E-04, 1.56575398E-04 51] 52 53crc_table = [ 54 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 55 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, 56 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 57 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, 58 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 59 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, 60 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 61 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, 62 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 63 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, 64 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 65 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, 66 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 67 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, 68 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 69 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, 70 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 71 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, 72 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 73 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, 74 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 75 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, 76 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 77 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, 78 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 79 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, 80 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 81 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, 82 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 83 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, 84 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 85 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4 86] 87# offset table for 4-subbands 88offset4 = np.array([[ -1, 0, 0, 0 ], 89 [ -2, 0, 0, 1 ], 90 [ -2, 0, 0, 1 ], 91 [ -2, 0, 0, 1 ] 92 ]) 93 94# offset tables for 8-subbands 95offset8 = np.array([[ -2, 0, 0, 0, 0, 0, 0, 1 ], 96 [ -3, 0, 0, 0, 0, 0, 1, 2 ], 97 [ -4, 0, 0, 0, 0, 0, 1, 2 ], 98 [ -4, 0, 0, 0, 0, 0, 1, 2 ] 99 ]) 100 101nr_blocks = [4, 8, 12, 16] 102nr_subbands = [4, 8] 103sampling_frequency =[16000, 32000, 44100, 48000] 104 105class SBCFrame: 106 syncword = 0 107 sampling_frequency = 0 108 nr_blocks = 0 109 channel_mode = 0 110 nr_channels = 0 111 allocation_method = 0 112 nr_subbands = 0 113 bitpool = 0 114 crc_check = 0 115 # pro subband - 1 116 join = np.zeros(8, dtype = np.uint8) 117 scale_factor = np.zeros(shape=(2, 8), dtype = np.int32) 118 scalefactor = np.zeros(shape=(2, 8), dtype = np.int32) 119 audio_sample = np.zeros(shape = (16,2,8), dtype = np.uint16) 120 sb_sample = np.zeros(shape = (16,2,8), dtype = np.uint16) 121 X = np.zeros(8, dtype = np.int16) 122 EX = np.zeros(8) 123 pcm = np.array([], dtype = np.int16) 124 bits = np.zeros(shape=(2, 8)) 125 levels = np.zeros(shape=(2, 8), dtype = np.int32) 126 127 def __init__(self, nr_blocks, nr_subbands, nr_channels, sampling_frequency, bitpool): 128 self.nr_blocks = nr_blocks 129 self.nr_subbands = nr_subbands 130 self.nr_channels = nr_channels 131 self.sampling_frequency = sampling_frequency 132 self.bitpool = bitpool 133 self.scale_factor = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32) 134 self.scalefactor = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32) 135 self.audio_sample = np.zeros(shape=(nr_blocks, nr_channels, nr_subbands), dtype = np.uint16) 136 self.sb_sample = np.zeros(shape=(nr_blocks, nr_channels, nr_subbands), dtype = np.uint16) 137 self.levels = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32) 138 self.EX = np.zeros(nr_subbands) 139 return 140 141 def __str__(self): 142 res = "SBCFrameHeader:" 143 res = res + "\n - syncword %d" % self.syncword 144 res = res + "\n - sample frequency %d" % self.sampling_frequency 145 res = res + "\n - nr blocks %d" % self.nr_blocks 146 147 if self.channel_mode == MONO: 148 res = res + "\n - channel mode MONO" 149 elif self.channel_mode == DUAL_CHANNEL: 150 res = res + "\n - channel mode DUAL CHANNEL" 151 elif self.channel_mode == STEREO: 152 res = res + "\n - channel mode STEREO" 153 elif self.channel_mode == JOINT_STEREO: 154 res = res + "\n - channel mode JOINT STEREO" 155 else: 156 res = res + "\n - channel mode %d" % self.channel_mode 157 158 res = res + "\n - nr channels %d" % self.nr_channels 159 160 if self.allocation_method == 1: 161 res = res + "\n - allocation method SNR" 162 elif self.allocation_method == 0: 163 res = res + "\n - allocation method LOUNDNESS" 164 else: 165 res = res + "\n - allocation method %d" % self.allocation_method 166 167 res = res + "\n - nr subbands %d" % self.nr_subbands 168 res = res + "\n - bitpool %d" % self.bitpool 169 res = res + "\n - crc check %d" % self.crc_check 170 171 return res 172 173 174def sbc_bit_allocation_stereo_joint(frame, ch): 175 bitneed = np.zeros(shape=(frame.nr_channels, frame.nr_subbands)) 176 bits = np.zeros(shape=(frame.nr_channels, frame.nr_subbands)) 177 loudness = 0 178 179 if frame.allocation_method == SNR: 180 for ch in range(frame.nr_channels): 181 for sb in range(frame.nr_subbands): 182 bitneed[ch][sb] = frame.scale_factor[ch][sb] 183 else: 184 for ch in range(frame.nr_channels): 185 for sb in range(frame.nr_subbands): 186 if frame.scale_factor[ch][sb] == 0: 187 bitneed[ch][sb] = -5 188 else: 189 if frame.nr_subbands == 4: 190 loudness = scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb] 191 else: 192 if frame.nr_subbands == 4: 193 loudness = frame.scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb] 194 else: 195 loudness = frame.scale_factor[ch][sb] - offset8[frame.sampling_frequency][sb] 196 if loudness > 0: 197 bitneed[ch][sb] = loudness/2 198 else: 199 bitneed[ch][sb] = loudness 200 201 # search the maximum bitneed index 202 max_bitneed = 0 203 for ch in range(frame.nr_channels): 204 for sb in range(frame.nr_subbands): 205 if bitneed[ch][sb] > max_bitneed: 206 max_bitneed = bitneed[ch][sb] 207 208 # calculate how many bitslices fit into the bitpool 209 bitcount = 0 210 slicecount = 0 211 bitslice = max_bitneed + 1 #/* init just above the largest sf */ 212 213 while True: 214 bitslice = bitslice - 1 215 bitcount = bitcount + slicecount 216 slicecount = 0 217 for ch in range(frame.nr_channels): 218 for sb in range(frame.nr_subbands): 219 if (bitneed[ch][sb] > bitslice+1) and (bitneed[ch][sb] < bitslice+16): 220 slicecount = slicecount + 1 221 elif bitneed[ch][sb] == bitslice + 1: 222 slicecount = slicecount + 2 223 if bitcount + slicecount >= frame.bitpool: 224 break 225 226 if bitcount + slicecount == frame.bitpool: 227 bitcount = bitcount + slicecount 228 bitslice = bitslice - 1 229 230 # bits are distributed until the last bitslice is reached 231 for ch in range(frame.nr_channels): 232 for sb in range(frame.nr_subbands): 233 if bitneed[ch][sb] < bitslice+2 : 234 bits[ch][sb]=0; 235 else: 236 bits[ch][sb] = min(bitneed[ch][sb]-bitslice,16) 237 238 ch = 0 239 sb = 0 240 while bitcount < frame.bitpool and sb < frame.nr_subbands: 241 if bits[ch][sb] >= 2 and bits[ch][sb] < 16: 242 bits[ch][sb] = bits[ch][sb] + 1 243 bitcount = bitcount + 1 244 245 elif (bitneed[ch][sb] == bitslice+1) and (frame.bitpool > bitcount+1): 246 bits[ch][sb] = 2 247 bitcount += 2 248 249 if ch == 1: 250 ch = 0 251 sb = sb + 1 252 else: 253 ch = 1 254 255 ch = 0 256 sb = 0 257 while bitcount < frame.bitpool and sb < frame.nr_subbands: 258 if bits[ch][sb] < 16: 259 bits[ch][sb] = bits[ch][sb] + 1 260 bitcount = bitcount + 1 261 if ch == 1: 262 ch = 0 263 sb = sb + 1 264 else: 265 ch = 1 266 267 return bits 268 269 270def sbc_bit_allocation_mono_dual(frame): 271 #print "Bit allocation for mono/dual channel" 272 bitneed = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32) 273 bits = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32) 274 loudness = 0 275 276 for ch in range(frame.nr_channels): 277 # bitneed values are derived from the scale factors 278 if frame.allocation_method == SNR: 279 for sb in range(frame.nr_subbands): 280 bitneed[ch][sb] = frame.scale_factor[ch][sb] 281 else: 282 for sb in range(frame.nr_subbands): 283 if frame.scale_factor[ch][sb] == 0: 284 bitneed[ch][sb] = -5 285 else: 286 if frame.nr_subbands == 4: 287 loudness = frame.scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb] 288 else: 289 loudness = frame.scale_factor[ch][sb] - offset8[frame.sampling_frequency][sb] 290 if loudness > 0: 291 bitneed[ch][sb] = loudness/2 292 else: 293 bitneed[ch][sb] = loudness 294 295 # search the maximum bitneed index 296 max_bitneed = 0 297 for sb in range(frame.nr_subbands): 298 if bitneed[ch][sb] > max_bitneed: 299 max_bitneed = bitneed[ch][sb] 300 301 # calculate how many bitslices fit into the bitpool 302 bitcount = 0 303 slicecount = 0 304 bitslice = max_bitneed + 1 305 306 while True: 307 bitslice = bitslice - 1 308 bitcount = bitcount + slicecount 309 slicecount = 0 310 for sb in range(frame.nr_subbands): 311 if (bitneed[ch][sb] > bitslice+1) and (bitneed[ch][sb] < bitslice+16): 312 slicecount = slicecount + 1 313 elif bitneed[ch][sb] == bitslice + 1: 314 slicecount = slicecount + 2 315 if bitcount + slicecount >= frame.bitpool: 316 break 317 318 if bitcount + slicecount == frame.bitpool: 319 bitcount = bitcount + slicecount 320 bitslice = bitslice - 1 321 322 for sb in range(frame.nr_subbands): 323 if bitneed[ch][sb] < bitslice+2 : 324 bits[ch][sb]=0; 325 else: 326 bits[ch][sb] = min(bitneed[ch][sb]-bitslice,16) 327 328 sb = 0 329 while bitcount < frame.bitpool and sb < frame.nr_subbands: 330 if bits[ch][sb] >= 2 and bits[ch][sb] < 16: 331 bits[ch][sb] = bits[ch][sb] + 1 332 bitcount = bitcount + 1 333 334 elif (bitneed[ch][sb] == bitslice+1) and (frame.bitpool > bitcount+1): 335 bits[ch][sb] = 2 336 bitcount += 2 337 338 sb = sb + 1 339 340 341 sb = 0 342 while bitcount < frame.bitpool and sb < frame.nr_subbands: 343 if bits[ch][sb] < 16: 344 bits[ch][sb] = bits[ch][sb] + 1 345 bitcount = bitcount + 1 346 sb = sb + 1 347 348 return bits 349 350def sbc_bit_allocation(frame): 351 if frame.channel_mode == MONO or frame.channel_mode == DUAL_CHANNEL: 352 return sbc_bit_allocation_mono_dual(frame) 353 elif frame.channel_mode == STEREO or frame.channel_mode == JOINT_STEREO: 354 return sbc_bit_allocation_stereo_joint(frame) 355 else: 356 print "Wrong channel mode ", frame.channel_mode 357 return -1 358 359def sbc_sampling_frequency_index(sample_rate): 360 sbc_sampling_frequency_index = 0 361 for i in range(len(sampling_frequency)): 362 if sample_rate == sampling_frequency[i]: 363 sbc_sampling_frequency_index = i 364 break 365 return sbc_sampling_frequency_index 366 367 368def sbc_crc8(data, data_len): 369 crc = 0x0f 370 j = 0 371 for i in range(data_len / 8): 372 crc = crc_table[crc ^ data[i]] 373 j = i + 1 374 375 bits_left = data_len%8 376 if bits_left: 377 octet = data[j] 378 for i in range(data_len%8): 379 bit = ((octet ^ crc) & 0x80) >> 7 380 if bit: 381 bit = 0x1d 382 crc = ((crc & 0x7f) << 1) ^ bit 383 octet = octet << 1 384 return crc 385 386 387bitstream = [] 388bitstream_index = -1 389bitstream_bits_available = 0 390 391def init_bitstream(): 392 global bitstream, bitstream_bits_available, bitstream_index 393 bitstream = [] 394 bitstream_index = -1 395 bitstream_bits_available = 0 396 397def add_bit(bit): 398 global bitstream, bitstream_bits_available, bitstream_index 399 if bitstream_bits_available == 0: 400 bitstream.append(0) 401 bitstream_bits_available = 8 402 bitstream_index += 1 403 404 bitstream[bitstream_index] |= bit << (bitstream_bits_available - 1) 405 bitstream_bits_available -= 1 406 407 408def add_bits(bits, len): 409 global bitstream, bitstream_bits_available 410 for i in range(len): 411 add_bit((bits >> (len-1-i)) & 1) 412 413 414def calculate_crc(frame): 415 global bitstream, bitstream_bits_available, bitstream_index 416 init_bitstream() 417 418 add_bits(frame.sampling_frequency, 2) 419 add_bits(frame.nr_blocks/4-1, 2) 420 add_bits(frame.channel_mode, 2) 421 add_bits(frame.allocation_method, 1) 422 add_bits(frame.nr_subbands/4-1, 1) 423 add_bits(frame.bitpool, 8) 424 425 if frame.channel_mode == JOINT_STEREO: 426 for sb in range(frame.nr_subbands): 427 add_bits(frame.join[sb],1) 428 429 for ch in range(frame.nr_channels): 430 for sb in range(frame.nr_subbands): 431 add_bits(frame.scale_factor[ch][sb], 4) 432 433 bitstream_len = (bitstream_index + 1) * 8 434 if bitstream_bits_available: 435 bitstream_len += (8-bitstream_bits_available) 436 return sbc_crc8(bitstream, bitstream_len) 437 438 439 440 441 442 443