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