From 042beff1a9b7a0d9cb3c4925a0afd1da0b598d92 Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Fri, 8 Jul 2016 12:49:05 +0200
Subject: [PATCH] sbc: fix mSBC CRC calculation

---
 test/sbc/sbc.py         | 11 ++++++-
 test/sbc/sbc_decoder.py | 68 +++++++++++++++++------------------------
 2 files changed, 38 insertions(+), 41 deletions(-)

diff --git a/test/sbc/sbc.py b/test/sbc/sbc.py
index 6e9616a57..c5880806c 100644
--- a/test/sbc/sbc.py
+++ b/test/sbc/sbc.py
@@ -207,6 +207,7 @@ class SBCFrame:
     nr_subbands = 0
     bitpool = 0
     crc_check = 0
+    reserved_for_future_use = 0
     # pro subband - 1
     join = np.zeros(8, dtype = np.uint8)
     scale_factor =  np.zeros(shape=(2, 8), dtype = np.int32)
@@ -567,7 +568,15 @@ def calculate_crc(frame):
     
     return sbc_crc8(bitstream, bitstream_len)
 
-
+def calculate_crc_mSBC(frame):
+    init_bitstream()
+    add_bits(frame.reserved_for_future_use, 16)
+    for sb in range(8):
+        add_bits(frame.scale_factor[0][sb], 4)
+    bitstream_len = (bitstream_index + 1) * 8
+    if bitstream_bits_available:
+        bitstream_len -= bitstream_bits_available
+    return sbc_crc8(bitstream, bitstream_len)
 
 def frame_to_bitstream(frame):
     global bitstream, bitstream_bits_available, bitstream_index
diff --git a/test/sbc/sbc_decoder.py b/test/sbc/sbc_decoder.py
index e9b55b404..aa3dfc17f 100755
--- a/test/sbc/sbc_decoder.py
+++ b/test/sbc/sbc_decoder.py
@@ -26,67 +26,54 @@ def sbc_unpack_frame(fin, available_bytes, frame):
             print ("out of sync %02x" % frame.syncword)
             return -1
 
-    frame.sampling_frequency = get_bits(fin,2)
-    frame.nr_blocks = nr_blocks[get_bits(fin,2)]
-    frame.channel_mode = get_bits(fin,2)
+    if mSBC_enabled:
+        frame.sampling_frequency = 0    # == 16 kHz
+        frame.nr_blocks = 15
+        frame.channel_mode = MONO
+        frame.allocation_method = LOUDNESS
+        frame.nr_subbands = 8
+        frame.bitpool = 26
+        frame.reserved_for_future_use = get_bits(fin,16)
+    else:
+        frame.sampling_frequency = get_bits(fin,2)
+        frame.nr_blocks = nr_blocks[get_bits(fin,2)]
+        frame.channel_mode = get_bits(fin,2)
+        frame.allocation_method = get_bits(fin,1)
+        frame.nr_subbands = nr_subbands[get_bits(fin,1)] 
+        frame.bitpool = get_bits(fin,8)
 
     if frame.channel_mode == MONO:
         frame.nr_channels = 1
     else:
         frame.nr_channels = 2
-    
-    frame.allocation_method = get_bits(fin,1)
-    frame.nr_subbands = nr_subbands[get_bits(fin,1)] 
-    frame.bitpool = get_bits(fin,8)
+
     frame.crc_check = get_bits(fin,8)
 
     frame.init(frame.nr_blocks, frame.nr_subbands, frame.nr_channels)
 
+    # read joint stereo flags
     if frame.channel_mode == JOINT_STEREO:
         for sb in range(frame.nr_subbands-1):
             frame.join[sb] = get_bits(fin,1)
         get_bits(fin,1) # RFA
     
     frame.scale_factor = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32)
-    
-    # print frame.audio_sample
+        
+    # read scale factors
     for ch in range(frame.nr_channels):
         for sb in range(frame.nr_subbands):
             frame.scale_factor[ch][sb] = get_bits(fin, 4)
-    if mSBC_enabled:
-        #frame.nr_blocks = 16
-        #frame.bitpool = 26
-        #frame.nr_subbands = 1
-        print "frequency: ", frame.sampling_frequency
-        print "subbands: ", frame.nr_blocks/4-1
-        print "bitpool:  ", frame.bitpool
-        print "blocks :  ", frame.nr_blocks/4-1
-        print "alloc  :  ", frame.allocation_method
-        print "mode   :  ", frame.channel_mode
-        print "scale factor: ", frame.scale_factor
 
-    crc = calculate_crc(frame)
+    if mSBC_enabled:
+        crc = calculate_crc_mSBC(frame)
+    else:
+        crc = calculate_crc(frame)
+    
     if crc != frame.crc_check:
         print "CRC mismatch: calculated %d, expected %d" % (crc, frame.crc_check)
-        exit(10)    
-
-    if mSBC_enabled:
-        frame.nr_subbands = 8
-        frame.bitpool = 26
-        frame.nr_blocks = 15
-        #frame.sampling_frequency = 0
-
-        frame.init(frame.nr_blocks, frame.nr_subbands, frame.nr_channels)
-
-        if frame.channel_mode == JOINT_STEREO:
-            for sb in range(frame.nr_subbands-1):
-                frame.join[sb] = get_bits(fin,1)
-            get_bits(fin,1) # RFA
-
-        for ch in range(frame.nr_channels):
-            for sb in range(frame.nr_subbands):
-                frame.scale_factor[ch][sb] = get_bits(fin, 4)
+        return -1
 
+    print "CRC correct: %d" % crc
     
     frame.scalefactor = np.zeros(shape=(frame.nr_channels, frame.nr_subbands), dtype = np.int32)
     for ch in range(frame.nr_channels):
@@ -308,6 +295,7 @@ if __name__ == "__main__":
                 sys.exit(1)
 
         wavfile = infile.replace('.sbc', '-decoded.wav')
+        wavfile = infile.replace('.msbc', '-decoded.wav')
         fout = False
 
         implementation = "SIG"
@@ -329,7 +317,7 @@ if __name__ == "__main__":
                 while True:
                     frame = SBCFrame()
                     if frame_count % 200 == 0:
-                        print "== Frame %d == %d" % (frame_count, fin.tell())
+                        print "== Frame %d == offset %d" % (frame_count, fin.tell())
 
                     err = sbc_unpack_frame(fin, file_size - fin.tell(), frame)
                     if err: