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