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): 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.scale_factor = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32) 157 self.scalefactor = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32) 158 self.audio_sample = np.zeros(shape=(nr_blocks, nr_channels, nr_subbands), dtype = np.uint16) 159 self.sb_sample = np.zeros(shape=(nr_blocks, nr_channels, nr_subbands), dtype = np.uint16) 160 self.levels = np.zeros(shape=(nr_channels, nr_subbands), dtype = np.int32) 161 self.EX = np.zeros(nr_subbands) 162 return 163 164 def dump_audio_samples(self, blk, ch): 165 print self.audio_sample[blk][ch] 166 167 def dump_subband_samples(self, blk, ch): 168 print self.sb_sample[blk][ch] 169 170 def dump_state(self): 171 res = "SBCFrameHeader state:" 172 res += "\n - nr channels %d" % self.nr_channels 173 res += "\n - nr blocks %d" % self.nr_blocks 174 res += "\n - nr subbands %d" % self.nr_subbands 175 res += "\n - scale factors: %s" % self.scale_factor 176 res += "\n - levels: %s" % self.levels 177 res += "\n - join: %s" % self.join 178 res += "\n - bits: %s" % self.bits 179 print res 180 181 def __str__(self): 182 res = "SBCFrameHeader:" 183 res += "\n - syncword %d" % self.syncword 184 res += "\n - sampling frequency %d Hz" % sampling_frequency_to_str(self.sampling_frequency) 185 186 res += "\n - nr channels %d" % self.nr_channels 187 res += "\n - nr blocks %d" % self.nr_blocks 188 res += "\n - nr subbands %d" % self.nr_subbands 189 190 res += "\n - channel mode %s" % channel_mode_to_str(self.channel_mode) 191 res += "\n - allocation method %s" % allocation_method_to_str(self.allocation_method) 192 193 res += "\n - bitpool %d" % self.bitpool 194 res += "\n - crc check %d" % self.crc_check 195 return res 196 197 198def sbc_bit_allocation_stereo_joint(frame, ch): 199 bitneed = np.zeros(shape=(frame.nr_channels, frame.nr_subbands)) 200 bits = np.zeros(shape=(frame.nr_channels, frame.nr_subbands)) 201 loudness = 0 202 203 if frame.allocation_method == SNR: 204 for ch in range(frame.nr_channels): 205 for sb in range(frame.nr_subbands): 206 bitneed[ch][sb] = frame.scale_factor[ch][sb] 207 else: 208 for ch in range(frame.nr_channels): 209 for sb in range(frame.nr_subbands): 210 if frame.scale_factor[ch][sb] == 0: 211 bitneed[ch][sb] = -5 212 else: 213 if frame.nr_subbands == 4: 214 loudness = scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb] 215 else: 216 if frame.nr_subbands == 4: 217 loudness = frame.scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb] 218 else: 219 loudness = frame.scale_factor[ch][sb] - offset8[frame.sampling_frequency][sb] 220 if loudness > 0: 221 bitneed[ch][sb] = loudness/2 222 else: 223 bitneed[ch][sb] = loudness 224 225 # search the maximum bitneed index 226 max_bitneed = 0 227 for ch in range(frame.nr_channels): 228 for sb in range(frame.nr_subbands): 229 if bitneed[ch][sb] > max_bitneed: 230 max_bitneed = bitneed[ch][sb] 231 232 # calculate how many bitslices fit into the bitpool 233 bitcount = 0 234 slicecount = 0 235 bitslice = max_bitneed + 1 #/* init just above the largest sf */ 236 237 while True: 238 bitslice = bitslice - 1 239 bitcount = bitcount + slicecount 240 slicecount = 0 241 for ch in range(frame.nr_channels): 242 for sb in range(frame.nr_subbands): 243 if (bitneed[ch][sb] > bitslice+1) and (bitneed[ch][sb] < bitslice+16): 244 slicecount = slicecount + 1 245 elif bitneed[ch][sb] == bitslice + 1: 246 slicecount = slicecount + 2 247 if bitcount + slicecount >= frame.bitpool: 248 break 249 250 if bitcount + slicecount == frame.bitpool: 251 bitcount = bitcount + slicecount 252 bitslice = bitslice - 1 253 254 # bits are distributed until the last bitslice is reached 255 for ch in range(frame.nr_channels): 256 for sb in range(frame.nr_subbands): 257 if bitneed[ch][sb] < bitslice+2 : 258 bits[ch][sb]=0; 259 else: 260 bits[ch][sb] = min(bitneed[ch][sb]-bitslice,16) 261 262 ch = 0 263 sb = 0 264 while bitcount < frame.bitpool and sb < frame.nr_subbands: 265 if bits[ch][sb] >= 2 and bits[ch][sb] < 16: 266 bits[ch][sb] = bits[ch][sb] + 1 267 bitcount = bitcount + 1 268 269 elif (bitneed[ch][sb] == bitslice+1) and (frame.bitpool > bitcount+1): 270 bits[ch][sb] = 2 271 bitcount += 2 272 273 if ch == 1: 274 ch = 0 275 sb = sb + 1 276 else: 277 ch = 1 278 279 ch = 0 280 sb = 0 281 while bitcount < frame.bitpool and sb < frame.nr_subbands: 282 if bits[ch][sb] < 16: 283 bits[ch][sb] = bits[ch][sb] + 1 284 bitcount = bitcount + 1 285 if ch == 1: 286 ch = 0 287 sb = sb + 1 288 else: 289 ch = 1 290 291 return bits 292 293 294def sbc_bit_allocation_mono_dual(frame): 295 #print "Bit allocation for mono/dual channel" 296 bitneed = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32) 297 bits = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32) 298 loudness = 0 299 300 for ch in range(frame.nr_channels): 301 # bitneed values are derived from the scale factors 302 if frame.allocation_method == SNR: 303 for sb in range(frame.nr_subbands): 304 bitneed[ch][sb] = frame.scale_factor[ch][sb] 305 else: 306 for sb in range(frame.nr_subbands): 307 if frame.scale_factor[ch][sb] == 0: 308 bitneed[ch][sb] = -5 309 else: 310 if frame.nr_subbands == 4: 311 loudness = frame.scale_factor[ch][sb] - offset4[frame.sampling_frequency][sb] 312 else: 313 loudness = frame.scale_factor[ch][sb] - offset8[frame.sampling_frequency][sb] 314 if loudness > 0: 315 bitneed[ch][sb] = loudness/2 316 else: 317 bitneed[ch][sb] = loudness 318 319 # search the maximum bitneed index 320 max_bitneed = 0 321 for sb in range(frame.nr_subbands): 322 if bitneed[ch][sb] > max_bitneed: 323 max_bitneed = bitneed[ch][sb] 324 325 # calculate how many bitslices fit into the bitpool 326 bitcount = 0 327 slicecount = 0 328 bitslice = max_bitneed + 1 329 330 while True: 331 bitslice = bitslice - 1 332 bitcount = bitcount + slicecount 333 slicecount = 0 334 for sb in range(frame.nr_subbands): 335 if (bitneed[ch][sb] > bitslice+1) and (bitneed[ch][sb] < bitslice+16): 336 slicecount = slicecount + 1 337 elif bitneed[ch][sb] == bitslice + 1: 338 slicecount = slicecount + 2 339 if bitcount + slicecount >= frame.bitpool: 340 break 341 342 if bitcount + slicecount == frame.bitpool: 343 bitcount = bitcount + slicecount 344 bitslice = bitslice - 1 345 346 for sb in range(frame.nr_subbands): 347 if bitneed[ch][sb] < bitslice+2 : 348 bits[ch][sb]=0; 349 else: 350 bits[ch][sb] = min(bitneed[ch][sb]-bitslice,16) 351 352 sb = 0 353 while bitcount < frame.bitpool and sb < frame.nr_subbands: 354 if bits[ch][sb] >= 2 and bits[ch][sb] < 16: 355 bits[ch][sb] = bits[ch][sb] + 1 356 bitcount = bitcount + 1 357 358 elif (bitneed[ch][sb] == bitslice+1) and (frame.bitpool > bitcount+1): 359 bits[ch][sb] = 2 360 bitcount += 2 361 362 sb = sb + 1 363 364 365 sb = 0 366 while bitcount < frame.bitpool and sb < frame.nr_subbands: 367 if bits[ch][sb] < 16: 368 bits[ch][sb] = bits[ch][sb] + 1 369 bitcount = bitcount + 1 370 sb = sb + 1 371 372 return bits 373 374def sbc_bit_allocation(frame): 375 if frame.channel_mode == MONO or frame.channel_mode == DUAL_CHANNEL: 376 return sbc_bit_allocation_mono_dual(frame) 377 elif frame.channel_mode == STEREO or frame.channel_mode == JOINT_STEREO: 378 return sbc_bit_allocation_stereo_joint(frame) 379 else: 380 print "Wrong channel mode ", frame.channel_mode 381 return -1 382 383def sbc_sampling_frequency_index(sample_rate): 384 sbc_sampling_frequency_index = 0 385 for i in range(len(sampling_frequency)): 386 if sample_rate == sampling_frequency[i]: 387 sbc_sampling_frequency_index = i 388 break 389 return sbc_sampling_frequency_index 390 391 392def sbc_crc8(data, data_len): 393 crc = 0x0f 394 j = 0 395 for i in range(data_len / 8): 396 crc = crc_table[crc ^ data[i]] 397 j = i + 1 398 399 bits_left = data_len%8 400 if bits_left: 401 octet = data[j] 402 for i in range(data_len%8): 403 bit = ((octet ^ crc) & 0x80) >> 7 404 if bit: 405 bit = 0x1d 406 crc = ((crc & 0x7f) << 1) ^ bit 407 octet = octet << 1 408 return crc 409 410 411bitstream = None 412bitstream_index = -1 413bitstream_bits_available = 0 414 415def init_bitstream(): 416 global bitstream, bitstream_bits_available, bitstream_index 417 bitstream = [] 418 bitstream_index = -1 419 bitstream_bits_available = 0 420 421def add_bit(bit): 422 global bitstream, bitstream_bits_available, bitstream_index 423 if bitstream_bits_available == 0: 424 bitstream.append(0) 425 bitstream_bits_available = 8 426 bitstream_index += 1 427 428 bitstream[bitstream_index] |= bit << (bitstream_bits_available - 1) 429 bitstream_bits_available -= 1 430 431 432def add_bits(bits, len): 433 global bitstream, bitstream_bits_available 434 for i in range(len): 435 add_bit((bits >> (len-1-i)) & 1) 436 437ibuffer = None 438ibuffer_count = 0 439 440def get_bit(fin): 441 global ibuffer, ibuffer_count 442 if ibuffer_count == 0: 443 ibuffer = ord(fin.read(1)) 444 ibuffer_count = 8 445 446 bit = (ibuffer >> 7) & 1 447 ibuffer = ibuffer << 1 448 ibuffer_count = ibuffer_count - 1 449 return bit 450 451def drop_remaining_bits(): 452 global ibuffer_count 453 ibuffer_count = 0 454 455def get_bits(fin, bit_count): 456 bits = 0 457 for i in range(bit_count): 458 bits = (bits << 1) | get_bit(fin) 459 return bits 460 461 462def calculate_crc(frame): 463 global bitstream, bitstream_bits_available, bitstream_index 464 init_bitstream() 465 466 add_bits(frame.sampling_frequency, 2) 467 add_bits(frame.nr_blocks/4-1, 2) 468 add_bits(frame.channel_mode, 2) 469 add_bits(frame.allocation_method, 1) 470 add_bits(frame.nr_subbands/4-1, 1) 471 add_bits(frame.bitpool, 8) 472 473 if frame.channel_mode == JOINT_STEREO: 474 for sb in range(frame.nr_subbands): 475 add_bits(frame.join[sb],1) 476 477 for ch in range(frame.nr_channels): 478 for sb in range(frame.nr_subbands): 479 add_bits(frame.scale_factor[ch][sb], 4) 480 481 bitstream_len = (bitstream_index + 1) * 8 482 if bitstream_bits_available: 483 bitstream_len += (8-bitstream_bits_available) 484 return sbc_crc8(bitstream, bitstream_len) 485 486 487 488def frame_to_bitstream(frame): 489 global bitstream, bitstream_bits_available, bitstream_index 490 init_bitstream() 491 492 add_bits(frame.syncword, 8) 493 add_bits(frame.sampling_frequency, 2) 494 add_bits(frame.nr_blocks/4-1, 2) 495 add_bits(frame.channel_mode, 2) 496 add_bits(frame.allocation_method, 1) 497 add_bits(frame.nr_subbands/4-1, 1) 498 add_bits(frame.bitpool, 8) 499 add_bits(frame.crc_check, 8) 500 501 if frame.channel_mode == JOINT_STEREO: 502 for sb in range(frame.nr_subbands-1): 503 add_bits(frame.join[sb],1) 504 add_bits(0,1) 505 506 for ch in range(frame.nr_channels): 507 for sb in range(frame.nr_subbands): 508 add_bits(frame.scale_factor[ch][sb], 4) 509 510 for blk in range(frame.nr_blocks): 511 for ch in range(frame.nr_channels): 512 for sb in range(frame.nr_subbands): 513 add_bits(frame.audio_sample[blk][ch][sb], frame.bits[ch][sb]) 514 515 bitstream_bits_available = 0 516 return bitstream 517 518def mse(a,b): 519 count = 1 520 for i in a.shape: 521 count *= i 522 delta = a - b 523 sqr = delta ** 2 524 res = sqr.sum()*1.0/count 525 # res = ((a - b) ** 2).mean() 526 return res 527