1 /*
2  * Copyright (C) 2021 Huawei Device Co., Ltd.
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15 #include "../include/sbc_frame.h"
16 #include <memory>
17 #include "../include/sbc_constant.h"
18 #include "../include/sbc_tables.h"
19 #include "sys/types.h"
20 
21 namespace  sbc {
22 const int SBC_ENCODE_FIXED_EXTRA_BITS = 2;
23 const int SCALE_OUT_BITS = 15;
24 const int VALUE_ZERO = 0;
25 const int VALUE3 = 3;
26 const int VALUE8 = 8;
27 const int VALUE11 = 11;
28 const int VALUE32 = 32;
29 const int SUBBAND_FOUR = 4;
30 const int SUBBAND_EIGHT = 8;
31 const int CHANNEL_ONE = 1;
32 const int CHANNEL_TWO = 2;
33 const int BLOCK_FOUR = 4;
34 const int BLOCK_EIGHT = 8;
35 const int BLOCK_TWELVE = 12;
36 const int BLOCK_SIXTEEN = 16;
37 const int MOVE_RIGHT_BIT7 = 7;
38 const int VALUE_OF_SIXTEEN = 16;
39 const int VALUE_OF_TWO = 2;
40 const int VALUE_OF_FOUR = 4;
41 const int VALUE_OF_SEVEN = 7;
42 const int VALUE_NEGATIVE5 = -5;
43 const int MOVE_BIT1 = 1;
44 const int MOVE_BIT2 = 2;
45 const int MOVE_BIT3 = 3;
46 const int MOVE_BIT4 = 4;
47 const int MOVE_BIT5 = 5;
48 const int MOVE_BIT6 = 6;
49 const int MOVE_BIT32 = 32;
50 const int MIN__HEADER_SIZE = 4;
51 const int INDEX_VALUE3 = 3;
52 
53 const uint8_t SBC_CRC_TABLE [256] = {
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 
SbcCrc8(const uint8_t * data,size_t len)88 uint8_t Frame::SbcCrc8(const uint8_t* data, size_t len)
89 {
90     uint8_t crc = 0x0f;
91     int index = VALUE_ZERO;
92     for (index = 0; index < static_cast<int>(len / VALUE8); index++) {
93         crc = SBC_CRC_TABLE [crc ^ data[index]];
94     }
95 
96     uint8_t octet = static_cast<int>(len % VALUE8) ? data[index] : 0;
97     for (int i = 0; i < static_cast<int>(len % VALUE8); i++) {
98         char bit = ((octet ^ crc) & 0x80) >> MOVE_RIGHT_BIT7;
99         crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
100         octet = octet << 1;
101     }
102     return crc;
103 }
104 
SbcCalculateLoudness(const Frame & frame,int channel,int subband)105 int Frame::SbcCalculateLoudness(const Frame& frame, int channel, int subband)
106 {
107     int loudness = VALUE_ZERO;
108 
109     if (frame.subbands_ == SUBBAND_FOUR) {
110         loudness = frame.scaleFactor_[channel][subband] - SBC_OFFSET_4[frame.frequency_][subband];
111     } else {
112         loudness = frame.scaleFactor_[channel][subband] - SBC_OFFSET_8[frame.frequency_][subband];
113     }
114 
115     return loudness;
116 }
117 
118 
SbcCalculateBitsNormal(const Frame & frame,int (* bits)[VALUE8])119 void Frame::SbcCalculateBitsNormal(const Frame& frame, int (*bits)[VALUE8])
120 {
121     int subbands = (frame.subbands_ == SUBBAND_FOUR) ? SUBBAND_FOUR : SUBBAND_EIGHT;
122     int bitNeed[CHANNEL_TWO][VALUE8] = {};
123     for (int channel = 0; channel < frame.channels_; channel++) {
124         int bitSlice = SbcCalculateBitsMax(frame, channel, subbands, bitNeed);
125         int bitCount = SbcCalculateBitsSliceNormal(frame.bitpool_, subbands, bitNeed, channel, &bitSlice);
126 
127         for (int subband = 0; subband < subbands; subband++) {
128             if (bitNeed[channel][subband] < bitSlice + VALUE_OF_TWO) {
129                 bits[channel][subband] = 0;
130             } else {
131                 bits[channel][subband] = bitNeed[channel][subband] - bitSlice;
132                 if (bits[channel][subband] > VALUE_OF_SIXTEEN) {
133                     bits[channel][subband] = VALUE_OF_SIXTEEN;
134                 }
135             }
136         }
137 
138         for (int subband = 0; bitCount < frame.bitpool_ && subband < subbands; subband++) {
139             if ((bits[channel][subband] >= VALUE_OF_TWO) && (bits[channel][subband] < VALUE_OF_SIXTEEN)) {
140                 bits[channel][subband]++;
141                 bitCount++;
142             } else if ((bitNeed[channel][subband] == bitSlice + 1) && (frame.bitpool_ > bitCount + 1)) {
143                 bits[channel][subband] = VALUE_OF_TWO;
144                 bitCount += VALUE_OF_TWO;
145             }
146         }
147 
148         for (int subband = 0; bitCount < frame.bitpool_ && subband < subbands; subband++) {
149             if (bits[channel][subband] < VALUE_OF_SIXTEEN) {
150                 bits[channel][subband]++;
151                 bitCount++;
152             }
153         }
154     }
155 }
156 
SbcCalculateBitsSliceNormal(const uint8_t bitPool,const int subbands,int (* bits)[8],int channel,int * bitSlice) const157 int Frame::SbcCalculateBitsSliceNormal(const uint8_t bitPool, const int subbands, int (*bits)[8],
158                                        int channel, int *bitSlice) const
159 {
160     int sliceCount = VALUE_ZERO;
161     int bitCount = VALUE_ZERO;
162 
163     do {
164         (*bitSlice)--;
165         bitCount += sliceCount;
166         sliceCount = VALUE_ZERO;
167 
168         for (int subband = 0; subband < subbands; subband++) {
169             if ((bits[channel][subband] > (*bitSlice + 1)) && (bits[channel][subband] <
170                 (*bitSlice + VALUE_OF_SIXTEEN))) {
171                     sliceCount++;
172             } else if (bits[channel][subband] == (*bitSlice + 1)){
173                 sliceCount += VALUE_OF_TWO;
174             }
175         }
176     } while ((bitCount + sliceCount) < bitPool);
177 
178     if ((bitCount + sliceCount) == bitPool) {
179         bitCount += sliceCount;
180         (*bitSlice)--;
181     }
182 
183     return bitCount;
184 }
185 
SbcCalculateBitsMax(const Frame & frame,const int channel,int subbands,int (* bits)[VALUE8])186 int Frame::SbcCalculateBitsMax(const Frame& frame, const int channel, int subbands,
187     int (*bits)[VALUE8])
188 {
189     int loudness = VALUE_ZERO;
190     int maxBit = VALUE_ZERO;
191 
192     if (frame.allocation_ == SBC_ALLOCATION_SNR) {
193         for (int subband = 0; subband < subbands; subband++) {
194             bits[channel][subband] = frame.scaleFactor_[channel][subband];
195             if (bits[channel][subband] > maxBit) {
196                 maxBit = bits[channel][subband];
197             }
198         }
199     } else {
200         for (int subband = 0; subband < subbands; subband++) {
201             if (frame.scaleFactor_[channel][subband] == 0) {
202                 bits[channel][subband] = VALUE_NEGATIVE5;
203             } else {
204                 loudness = SbcCalculateLoudness(frame, channel, subband);
205                 bits[channel][subband] = (loudness > 0) ? (loudness / VALUE_OF_TWO) : loudness;
206             }
207             if (bits[channel][subband] > maxBit) {
208                 maxBit = bits[channel][subband];
209             }
210         }
211     }
212 
213     return (maxBit + 1);
214 }
215 
SbcCalculateBitsMaxStereo(const Frame & frame,const int subbands,int (* bits)[VALUE8])216 int Frame::SbcCalculateBitsMaxStereo(const Frame& frame, const int subbands, int (*bits)[VALUE8])
217 {
218     int maxBit = VALUE_ZERO;
219 
220     if (frame.allocation_ == SBC_ALLOCATION_SNR) {
221         for (int channel  = 0; channel < CHANNEL_TWO; channel++) {
222             for (int subband = 0; subband < subbands; subband++) {
223                 bits[channel][subband] = frame.scaleFactor_[channel][subband];
224                 if (bits[channel][subband] > maxBit) {
225                     maxBit = bits[channel][subband];
226                 }
227             }
228         }
229     } else {
230         for (int channel = 0; channel < CHANNEL_TWO; channel++) {
231             for (int subband = 0; subband < subbands; subband++) {
232                 if (frame.scaleFactor_[channel][subband] == 0) {
233                     bits[channel][subband] = VALUE_NEGATIVE5;
234                 } else {
235                     int loudness = SbcCalculateLoudness(frame, channel, subband);
236                     bits[channel][subband] = (loudness > 0) ? (loudness / VALUE_OF_TWO) : loudness;
237                 }
238                 if (bits[channel][subband] > maxBit) {
239                     maxBit = bits[channel][subband];
240                 }
241             }
242         }
243     }
244     return (maxBit + 1);
245 }
246 
SbcCalculateBitsSliceStereo(const uint8_t bitPool,const int subbands,int (* bits)[VALUE8],int * bitCount,int * bitSlice) const247 void Frame::SbcCalculateBitsSliceStereo(const uint8_t bitPool, const int subbands,
248                                         int (*bits)[VALUE8], int *bitCount,
249                                         int *bitSlice) const
250 {
251     int sliceCount = VALUE_ZERO;
252 
253     do {
254         (*bitSlice)--;
255         (*bitCount) += sliceCount;
256         sliceCount = VALUE_ZERO;
257 
258         for (int channel = 0; channel < CHANNEL_TWO; channel++) {
259             for (int subband = 0; subband < subbands; subband++) {
260                 if ((bits[channel][subband] > ((*bitSlice) + 1)) &&
261                     (bits[channel][subband] < ((*bitSlice) + VALUE_OF_SIXTEEN))) {
262                         sliceCount++;
263                 } else if (bits[channel][subband] == ((*bitSlice) + 1)) {
264                     sliceCount += VALUE_OF_TWO;
265                 }
266             }
267         }
268     } while ((*bitCount) + sliceCount < bitPool);
269 
270     if ((*bitCount) + sliceCount == bitPool) {
271         (*bitCount) += sliceCount;
272         (*bitSlice)--;
273     }
274 }
275 
SbcCalculateBitsInnerStereo(const int subbands,int bitSlice,int (* bitNeed)[VALUE8],int (* bits)[VALUE8])276 void Frame::SbcCalculateBitsInnerStereo(const int subbands, int bitSlice,
277                                         int (*bitNeed)[VALUE8],
278                                         int (*bits)[VALUE8])
279 {
280     for (int channel = 0; channel < CHANNEL_TWO; channel++) {
281         for (int subband = 0; subband < subbands; subband++) {
282             if (bitNeed[channel][subband] < bitSlice + VALUE_OF_TWO) {
283                 bits[channel][subband] = 0;
284             } else {
285                 bits[channel][subband] = bitNeed[channel][subband] - bitSlice;
286                 if (bits[channel][subband] > VALUE_OF_SIXTEEN) {
287                     bits[channel][subband] = VALUE_OF_SIXTEEN;
288                 }
289             }
290         }
291     }
292 }
293 
SbcCalculateBitsStereo(const Frame & frame,int (* bits)[VALUE8])294 void Frame::SbcCalculateBitsStereo(const Frame& frame, int (*bits)[VALUE8])
295 {
296     int subbands = (frame.subbands_ == SUBBAND_FOUR) ? SUBBAND_FOUR : SUBBAND_EIGHT;
297     int bitNeed[CHANNEL_TWO][VALUE8] = {};
298     int bitCount = VALUE_ZERO;
299 
300     int bitSlice = SbcCalculateBitsMaxStereo(frame, subbands, bitNeed);
301     SbcCalculateBitsSliceStereo(frame.bitpool_, subbands, bitNeed, &bitCount, &bitSlice);
302     SbcCalculateBitsInnerStereo(subbands, bitSlice, bitNeed, bits);
303 
304     int channel = VALUE_ZERO;
305     int subband = VALUE_ZERO;
306 
307     while (bitCount < frame.bitpool_) {
308         if ((bits[channel][subband] >= VALUE_OF_TWO) && (bits[channel][subband] < VALUE_OF_SIXTEEN)) {
309             bits[channel][subband]++;
310             bitCount++;
311         } else if ((bitNeed[channel][subband] == bitSlice + 1) && (frame.bitpool_ > bitCount + 1)) {
312             bits[channel][subband] = VALUE_OF_TWO;
313             bitCount += VALUE_OF_TWO;
314         }
315         if (channel == 1) {
316             channel = 0;
317             subband++;
318             if (subband >= subbands) {
319                 break;
320             }
321         } else {
322             channel = 1;
323         }
324     }
325 
326     channel = VALUE_ZERO;
327     subband = VALUE_ZERO;
328 
329     while (bitCount < frame.bitpool_) {
330         if (bits[channel][subband] < VALUE_OF_SIXTEEN) {
331             bits[channel][subband]++;
332             bitCount++;
333         }
334         if (channel == 1) {
335             channel = VALUE_ZERO;
336             subband++;
337             if (subband >= subbands) {
338                 break;
339             }
340         } else {
341             channel = 1;
342         }
343     }
344 }
345 
SbcCalculateBits(const Frame & frame,int (* bits)[VALUE8])346 void Frame::SbcCalculateBits(const Frame& frame, int (*bits)[VALUE8])
347 {
348     switch (frame.channelMode_) {
349         case SBC_CHANNEL_MODE_MONO:
350         case SBC_CHANNEL_MODE_DUAL_CHANNEL:
351             SbcCalculateBitsNormal(frame, bits);
352             break;
353         case SBC_CHANNEL_MODE_STEREO:
354         case SBC_CHANNEL_MODE_JOINT_STEREO:
355             SbcCalculateBitsStereo(frame, bits);
356             break;
357         default:
358             SbcCalculateBitsNormal(frame, bits);
359             break;
360     }
361 }
362 
UnpackFrameStream(Frame & frame,const uint8_t * bufStream,size_t len)363 int Frame::UnpackFrameStream(Frame& frame, const uint8_t* bufStream, size_t len)
364 {
365     uint8_t crc[VALUE11] = {0};
366     crc[0] = bufStream[1];
367     crc[1] = bufStream[VALUE_OF_TWO];
368     uint crcpos = VALUE_OF_SIXTEEN;
369     uint consumed = VALUE32;
370 
371     if (frame.channelMode_ == SBC_CHANNEL_MODE_JOINT_STEREO) {
372         if ((consumed + frame.subbands_) > static_cast<uint>(len * VALUE8)) {
373             return SBC_ERROR_NO_ENOUGH_BITSTREAM;
374         }
375         frame.joint_ = 0x00;
376 
377         for (int subband = 0; subband < frame.subbands_ - 1; subband++) {
378             frame.joint_ |= ((bufStream[VALUE_OF_FOUR] >> (VALUE_OF_SEVEN - subband)) & 0x01) << subband;
379         }
380         if (frame.subbands_ == SUBBAND_FOUR) {
381             crc[crcpos / VALUE8] = bufStream[VALUE_OF_FOUR] & 0xf0;
382         } else {
383             crc[crcpos / VALUE8] = bufStream[VALUE_OF_FOUR];
384         }
385 
386         consumed += frame.subbands_;
387         crcpos += frame.subbands_;
388     }
389 
390     if (static_cast<uint>(len * VALUE8) < (consumed + (VALUE_OF_FOUR * frame.subbands_ * frame.channels_))) {
391         return SBC_ERROR_NO_ENOUGH_BITSTREAM;
392     }
393 
394     for (int channel = 0; channel < frame.channels_; channel++) {
395         for (int subband = 0; subband < frame.subbands_; subband++) {
396             frame.scaleFactor_[channel][subband] =
397                 (bufStream[consumed >> MOVE_BIT3] >> (VALUE_OF_FOUR - (consumed & VALUE_OF_SEVEN))) & 0x0F;
398             crc[crcpos >> MOVE_BIT3] |=
399                 frame.scaleFactor_[channel][subband] << (VALUE_OF_FOUR - (crcpos & VALUE_OF_SEVEN));
400 
401             consumed += VALUE_OF_FOUR;
402             crcpos += VALUE_OF_FOUR;
403         }
404     }
405 
406     if (bufStream[INDEX_VALUE3] != SbcCrc8(crc, crcpos)) {
407         return SBC_ERROR_INVALID_CRC;
408     }
409     int bits[VALUE_OF_TWO][VALUE8];
410     uint32_t levels[VALUE_OF_TWO][VALUE8];
411     SbcCalculateBits(frame, bits);
412 
413     for (uint8_t channel = 0; channel < frame.channels_; channel++) {
414         for (uint8_t subband = 0; subband < frame.subbands_; subband++) {
415             levels[channel][subband] = (1 << bits[channel][subband]) - 1;
416         }
417     }
418 
419     for (uint8_t blk = 0; blk < frame.blocks_; blk++) {
420         for (uint8_t channel = 0; channel < frame.channels_; channel++) {
421             for (uint8_t subband = 0; subband < frame.subbands_; subband++) {
422                 if (levels[channel][subband] == 0) {
423                     frame.samples_[blk][channel][subband] = 0;
424                     continue;
425                 }
426 
427                 uint32_t shift = frame.scaleFactor_[channel][subband] + 1 + SBC_ENCODE_FIXED_EXTRA_BITS;
428 
429                 int audioSample = 0;
430                 for (int bit = 0; bit < bits[channel][subband]; bit++) {
431                     if (consumed > static_cast<uint>(len * VALUE8)) {
432                         return -1;
433                     }
434                     if ((bufStream[consumed >> MOVE_BIT3] >> (VALUE_OF_SEVEN - (consumed & VALUE_OF_SEVEN))) & 0x01) {
435                         audioSample |= 1 << (bits[channel][subband] - bit - 1);
436                     }
437                     consumed++;
438                 }
439 
440                 frame.samples_[blk][channel][subband] = static_cast<int32_t>
441                     ((((static_cast<uint64_t> (audioSample) << 1) | 1) << shift) /
442                         levels[channel][subband]) - (1 << shift);
443             }
444         }
445     }
446 
447     if (frame.channelMode_ == SBC_CHANNEL_MODE_JOINT_STEREO) {
448         for (uint8_t blk = 0; blk < frame.blocks_; blk++) {
449             for (uint8_t subband = 0; subband < frame.subbands_; subband++) {
450                 if (frame.joint_ & (0x01 << subband)) {
451                     uint32_t val = frame.samples_[blk][0][subband] + frame.samples_[blk][1][subband];
452                     frame.samples_[blk][1][subband] =
453                         frame.samples_[blk][0][subband] - frame.samples_[blk][1][subband];
454                     frame.samples_[blk][0][subband] = val;
455                 }
456             }
457         }
458     }
459 
460     if ((consumed & VALUE_OF_SEVEN) != 0) {
461         consumed += VALUE8 - (consumed & VALUE_OF_SEVEN);
462     }
463     return consumed >> MOVE_BIT3;
464 }
465 
Frame()466 Frame::Frame() {}
467 
Unpack(const uint8_t * bufStream,size_t size)468 int Frame::Unpack(const uint8_t* bufStream, size_t size)
469 {
470     if (size < MIN__HEADER_SIZE || bufStream[0] != SBC_SYNCWORD) {
471         return SBC_ERROR_INVALID_ARG;
472     }
473     auto frame = std::make_unique<Frame>();
474     frequency_ = (bufStream[1] >> MOVE_BIT6) & VALUE3;
475     blockMode_ = (bufStream[1] >> MOVE_BIT4) & VALUE3;
476     switch (blockMode_) {
477         case SBC_BLOCK4:
478             blocks_ = BLOCK_FOUR;
479             break;
480         case SBC_BLOCK8:
481             blocks_ = BLOCK_EIGHT;
482             break;
483         case SBC_BLOCK12:
484             blocks_ = BLOCK_TWELVE;
485             break;
486         case SBC_BLOCK16:
487             blocks_ = BLOCK_SIXTEEN;
488             break;
489         default:
490             blocks_ = BLOCK_EIGHT;
491             break;
492     }
493     channelMode_ = (bufStream[1] >> MOVE_BIT2) & VALUE3;
494     switch (channelMode_) {
495         case SBC_CHANNEL_MODE_MONO:
496             channels_ = CHANNEL_ONE;
497             break;
498         case SBC_CHANNEL_MODE_DUAL_CHANNEL:
499         case SBC_CHANNEL_MODE_STEREO:
500         case SBC_CHANNEL_MODE_JOINT_STEREO:
501             channels_ = CHANNEL_TWO;
502             break;
503         default:
504             channels_ = CHANNEL_ONE;
505             break;
506     }
507     allocation_ = (bufStream[1] >> 1) & 0x01;
508     subbandMode_ = (bufStream[1] & 0x01);
509     subbands_ = subbandMode_ ? SUBBAND_EIGHT : SUBBAND_FOUR;
510     bitpool_ = bufStream[VALUE_OF_TWO];
511 
512     if (!IsValid()) {
513         return SBC_ERROR_INVALID_FRAME;
514     }
515     return UnpackFrameStream(*this, bufStream, size);
516 }
517 
IsValid() const518 bool Frame::IsValid() const
519 {
520     if (((channelMode_ == SBC_CHANNEL_MODE_MONO) || (channelMode_ == SBC_CHANNEL_MODE_DUAL_CHANNEL)) &&
521         (bitpool_ > subbands_ * VALUE_OF_SIXTEEN)) {
522             return false;
523         }
524     if (((channelMode_ == SBC_CHANNEL_MODE_STEREO) || (channelMode_ == SBC_CHANNEL_MODE_JOINT_STEREO)) &&
525         (bitpool_ > subbands_ * MOVE_BIT32)) {
526             return false;
527         }
528 
529     return true;
530 }
531 
532 #define PUT_BITS(data, cache, count, v, n)                \
533         do {                                                                \
534                 (cache) = (v) | ((cache) << (n));                        \
535                 (count) += (n);                                        \
536                 if ((count) >= VALUE_OF_SIXTEEN) {                                        \
537                         (count) -= VALUE8;                                \
538                         *(data)++ = static_cast<uint8_t>                                \
539                                 ((cache) >> (count));                \
540                         (count) -= VALUE8;                                \
541                         *(data)++ = static_cast<uint8_t>                                \
542                                 ((cache) >> (count));                \
543                 }                                                        \
544         } while (0)
545 
546 #define FLUSH_BITS(data, cache, count)                        \
547         do {                                                                \
548                 while ((count) >= VALUE8) {                                \
549                         (count) -= VALUE8;                                \
550                         *(data)++ = static_cast<uint8_t>                                \
551                                 ((cache) >> (count));                \
552                 }                                                        \
553                 if ((count) > 0)        {                                \
554                         *(data)++ = static_cast<uint8_t>                                \
555                                 ((cache) << (VALUE8 - (count)));        \
556                 }                           \
557         } while (0)
558 
SbcCaculateLevelsAndSampleDelta(const Frame & frame,int (* bits)[VALUE8],uint32_t (* levels)[VALUE8],uint32_t (* sampleDelta)[VALUE8])559 void Frame::SbcCaculateLevelsAndSampleDelta(const Frame& frame, int (*bits)[VALUE8],
560                                             uint32_t (*levels)[VALUE8], uint32_t (*sampleDelta)[VALUE8])
561 {
562     for (int channel = 0; channel < frame.channels_; channel++) {
563         for (int subband = 0; subband < frame.subbands_; subband++) {
564             levels[channel][subband] = ((1 << bits[channel][subband]) - 1)
565                 << (MOVE_BIT32 - (frame.scaleFactor_[channel][subband] + SCALE_OUT_BITS + MOVE_BIT2));
566             sampleDelta[channel][subband] = static_cast<uint32_t> (1
567                 << (frame.scaleFactor_[channel][subband] + SCALE_OUT_BITS + 1));
568         }
569     }
570 }
571 
PackFrameInternal(const Frame & frame,uint8_t * bufStream,int subbands,int channels,int joint)572 ssize_t Frame::PackFrameInternal(const Frame& frame, uint8_t* bufStream,
573                     int subbands, int channels, int joint)
574 {
575     uint8_t *data = bufStream + VALUE_OF_FOUR;
576     uint32_t bitsCache = VALUE_ZERO;
577     uint32_t bitsCount = VALUE_ZERO;
578     uint8_t crc[VALUE11] = {};
579     int crcpos = VALUE_ZERO;
580     uint32_t audioSample = VALUE_ZERO;
581     int bits[VALUE_OF_TWO][VALUE8] = {};
582     uint32_t levels[VALUE_OF_TWO][VALUE8] = {};
583     uint32_t sampleDelta[VALUE_OF_TWO][VALUE8] = {};
584     crc[0] = bufStream[1];
585     crc[1] = bufStream[VALUE_OF_TWO];
586     crcpos = VALUE_OF_SIXTEEN;
587     if (frame.channelMode_ == SBC_CHANNEL_MODE_JOINT_STEREO) {
588         PUT_BITS(data, bitsCache, bitsCount, joint, subbands);
589         crc[crcpos >> MOVE_BIT3] = joint;
590         crcpos += subbands;
591     }
592     for (int channel = 0; channel < channels; channel++) {
593         for (int subband = 0; subband < subbands; subband++) {
594             PUT_BITS(data, bitsCache, bitsCount, frame.scaleFactor_[channel][subband] & 0x0F, MOVE_BIT4);
595             crc[crcpos >> MOVE_BIT3] <<= MOVE_BIT4;
596             crc[crcpos >> MOVE_BIT3] |= frame.scaleFactor_[channel][subband] & 0x0F;
597             crcpos += MOVE_BIT4;
598         }
599     }
600     if (crcpos % VALUE8) {
601         crc[crcpos >> MOVE_BIT3] <<= VALUE8 - (crcpos % VALUE8);
602     }
603     bufStream[INDEX_VALUE3] = SbcCrc8(crc, crcpos);
604     SbcCalculateBits(frame, bits);
605     SbcCaculateLevelsAndSampleDelta(frame, bits, levels, sampleDelta);
606     for (int blk = 0; blk < frame.blocks_; blk++) {
607         for (int channel = 0; channel < channels; channel++) {
608             for (int subband = 0; subband < subbands; subband++) {
609                 if (bits[channel][subband] == 0) {
610                     continue;
611                 }
612                 audioSample = (static_cast<uint64_t>(levels[channel][subband]) *
613                     (sampleDelta[channel][subband] + frame.audioSamples_[blk][channel][subband])) >> MOVE_BIT32;
614                 PUT_BITS(data, bitsCache, bitsCount, audioSample, bits[channel][subband]);
615             }
616         }
617     }
618     FLUSH_BITS(data, bitsCache, bitsCount);
619     return data - bufStream;
620 }
621 
622 
Pack(uint8_t * bufStream,const Frame & frame,int joint)623 ssize_t Frame::Pack(uint8_t* bufStream, const Frame& frame, int joint)
624 {
625     bufStream[0] = SBC_SYNCWORD;
626     bufStream[1] = (frame.frequency_ & VALUE3) << MOVE_BIT6;
627     bufStream[1] |= (frame.blockMode_ & VALUE3) << MOVE_BIT4;
628     bufStream[1] |= (frame.channelMode_ & VALUE3) << MOVE_BIT2;
629     bufStream[1] |= (frame.allocation_ & 0x01) << MOVE_BIT1;
630     bufStream[VALUE_OF_TWO] = frame.bitpool_;
631 
632     int subbands = SUBBAND_FOUR;
633     int channels = CHANNEL_ONE;
634 
635     if (subbands_ != SUBBAND_FOUR) {
636         subbands = SUBBAND_EIGHT;
637     }
638 
639     if (((frame.channelMode_ == SBC_CHANNEL_MODE_MONO) || (frame.channelMode_ == SBC_CHANNEL_MODE_DUAL_CHANNEL))
640         && (bitpool_ > (subbands << MOVE_BIT4))) {
641         return -1;
642     }
643 
644     if (((frame.channelMode_ == SBC_CHANNEL_MODE_STEREO) || (frame.channelMode_ == SBC_CHANNEL_MODE_JOINT_STEREO))
645         && (bitpool_ > (subbands << MOVE_BIT5))) {
646         return -1;
647     }
648 
649     if (subbands_ == SUBBAND_FOUR) {
650         channels = (frame.channels_ == CHANNEL_ONE) ? CHANNEL_ONE : CHANNEL_TWO;
651     } else {
652         bufStream[1] |= 0x01;
653         channels = (frame.channels_ == CHANNEL_ONE) ? CHANNEL_ONE : CHANNEL_TWO;
654     }
655     return PackFrameInternal(frame, bufStream, subbands, channels, joint);
656 }
657 } // namespace sbc
658