1 /******************************************************************************
2 *
3 * Copyright (C) 1999-2012 Broadcom Corporation
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18
19 /******************************************************************************
20 *
21 * This file contains code for packing the Encoded data into bit streams.
22 *
23 ******************************************************************************/
24 #include "common/bt_target.h"
25 #include "sbc_encoder.h"
26 #include "sbc_enc_func_declare.h"
27
28 #if (defined(SBC_ENC_INCLUDED) && SBC_ENC_INCLUDED == TRUE)
29
30 #if (SBC_ARM_ASM_OPT==TRUE)
31 #define Mult32(s32In1,s32In2,s32OutLow) \
32 { \
33 __asm \
34 { \
35 MUL s32OutLow,s32In1,s32In2; \
36 } \
37 }
38 #define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \
39 { \
40 __asm \
41 { \
42 SMULL s32OutLow,s32OutHi,s32In1,s32In2 \
43 } \
44 }
45 #else
46 #define Mult32(s32In1,s32In2,s32OutLow) s32OutLow=(SINT32)s32In1*(SINT32)s32In2;
47 #define Mult64(s32In1, s32In2, s32OutLow, s32OutHi) \
48 { \
49 s32OutLow = ((SINT32)(UINT16)s32In1 * (UINT16)s32In2); \
50 s32TempVal2 = (SINT32)((s32In1 >> 16) * (UINT16)s32In2); \
51 s32Carry = ( (((UINT32)(s32OutLow)>>16)&0xFFFF) + \
52 + (s32TempVal2 & 0xFFFF) ) >> 16; \
53 s32OutLow += (s32TempVal2 << 16); \
54 s32OutHi = (s32TempVal2 >> 16) + s32Carry; \
55 }
56 #endif
57
EncPacking(SBC_ENC_PARAMS * pstrEncParams)58 void EncPacking(SBC_ENC_PARAMS *pstrEncParams)
59 {
60 UINT8 *pu8PacketPtr; /* packet ptr*/
61 UINT8 Temp;
62 SINT32 s32Blk; /* counter for block*/
63 SINT32 s32Ch; /* counter for channel*/
64 SINT32 s32Sb; /* counter for sub-band*/
65 SINT32 s32PresentBit; /* represents bit to be stored*/
66 /*SINT32 s32LoopCountI; loop counter*/
67 SINT32 s32LoopCountJ; /* loop counter*/
68 UINT32 u32QuantizedSbValue, u32QuantizedSbValue0; /* temp variable to store quantized sb val*/
69 SINT32 s32LoopCount; /* loop counter*/
70 UINT8 u8XoredVal; /* to store XORed value in CRC calculation*/
71 UINT8 u8CRC; /* to store CRC value*/
72 SINT16 *ps16GenPtr;
73 SINT32 s32NumOfBlocks;
74 SINT32 s32NumOfSubBands = pstrEncParams->s16NumOfSubBands;
75 SINT32 s32NumOfChannels = pstrEncParams->s16NumOfChannels;
76 UINT32 u32SfRaisedToPow2; /*scale factor raised to power 2*/
77 SINT16 *ps16ScfPtr;
78 SINT32 *ps32SbPtr;
79 UINT16 u16Levels; /*to store levels*/
80 SINT32 s32Temp1; /*used in 64-bit multiplication*/
81 SINT32 s32Low; /*used in 64-bit multiplication*/
82 #if (SBC_IS_64_MULT_IN_QUANTIZER==TRUE)
83 SINT32 s32Hi1, s32Low1, s32Carry, s32TempVal2, s32Hi, s32Temp2;
84 #endif
85
86 pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/
87 if (pstrEncParams->sbc_mode != SBC_MODE_MSBC) {
88 *pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_STD; /*Sync word*/
89 *pu8PacketPtr++ = (UINT8)(pstrEncParams->FrameHeader);
90
91 *pu8PacketPtr = (UINT8)(pstrEncParams->s16BitPool & 0x00FF);
92 } else {
93 *pu8PacketPtr++ = (UINT8)SBC_SYNC_WORD_MSBC; /*Sync word*/
94 // two reserved bytes
95 *pu8PacketPtr++ = 0;
96 *pu8PacketPtr = 0;
97 }
98 pu8PacketPtr += 2; /*skip for CRC*/
99
100 /*here it indicate if it is byte boundary or nibble boundary*/
101 s32PresentBit = 8;
102 Temp = 0;
103 #if (SBC_JOINT_STE_INCLUDED == TRUE)
104 if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
105 /* pack join stero parameters */
106 for (s32Sb = 0; s32Sb < s32NumOfSubBands; s32Sb++) {
107 Temp <<= 1;
108 Temp |= pstrEncParams->as16Join[s32Sb];
109 }
110
111 /* pack RFA */
112 if (s32NumOfSubBands == SUB_BANDS_4) {
113 s32PresentBit = 4;
114 } else {
115 *(pu8PacketPtr++) = Temp;
116 Temp = 0;
117 }
118 }
119 #endif
120
121 /* Pack Scale factor */
122 ps16GenPtr = pstrEncParams->as16ScaleFactor;
123 s32Sb = s32NumOfChannels * s32NumOfSubBands;
124 /*Temp=*pu8PacketPtr;*/
125 for (s32Ch = s32Sb; s32Ch > 0; s32Ch--) {
126 Temp <<= 4;
127 Temp |= *ps16GenPtr++;
128
129 if (s32PresentBit == 4) {
130 s32PresentBit = 8;
131 *(pu8PacketPtr++) = Temp;
132 Temp = 0;
133 } else {
134 s32PresentBit = 4;
135 }
136 }
137
138 /* Pack samples */
139 ps32SbPtr = pstrEncParams->s32SbBuffer;
140 /*Temp=*pu8PacketPtr;*/
141 s32NumOfBlocks = pstrEncParams->s16NumOfBlocks;
142 for (s32Blk = s32NumOfBlocks - 1; s32Blk >= 0; s32Blk--) {
143 ps16GenPtr = pstrEncParams->as16Bits;
144 ps16ScfPtr = pstrEncParams->as16ScaleFactor;
145 for (s32Ch = s32Sb - 1; s32Ch >= 0; s32Ch--) {
146 s32LoopCount = *ps16GenPtr++;
147 if (s32LoopCount != 0) {
148 #if (SBC_IS_64_MULT_IN_QUANTIZER==TRUE)
149 /* finding level from reconstruction part of decoder */
150 u32SfRaisedToPow2 = ((UINT32)1 << ((*ps16ScfPtr) + 1));
151 u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1);
152
153 /* quantizer */
154 s32Temp1 = (*ps32SbPtr >> 2) + (u32SfRaisedToPow2 << 12);
155 s32Temp2 = u16Levels;
156
157 Mult64 (s32Temp1, s32Temp2, s32Low, s32Hi);
158
159 s32Low1 = s32Low >> ((*ps16ScfPtr) + 2);
160 s32Low1 &= ((UINT32)1 << (32 - ((*ps16ScfPtr) + 2))) - 1;
161 s32Hi1 = s32Hi << (32 - ((*ps16ScfPtr) + 2));
162
163 u32QuantizedSbValue0 = (UINT16)((s32Low1 | s32Hi1) >> 12);
164 #else
165 /* finding level from reconstruction part of decoder */
166 u32SfRaisedToPow2 = ((UINT32)1 << *ps16ScfPtr);
167 u16Levels = (UINT16)(((UINT32)1 << s32LoopCount) - 1);
168
169 /* quantizer */
170 s32Temp1 = (*ps32SbPtr >> 15) + u32SfRaisedToPow2;
171 Mult32(s32Temp1, u16Levels, s32Low);
172 s32Low >>= (*ps16ScfPtr + 1);
173 u32QuantizedSbValue0 = (UINT16)s32Low;
174 #endif
175 /*store the number of bits required and the quantized s32Sb
176 sample to ease the coding*/
177 u32QuantizedSbValue = u32QuantizedSbValue0;
178
179 if (s32PresentBit >= s32LoopCount) {
180 Temp <<= s32LoopCount;
181 Temp |= u32QuantizedSbValue;
182 s32PresentBit -= s32LoopCount;
183 } else {
184 while (s32PresentBit < s32LoopCount) {
185 s32LoopCount -= s32PresentBit;
186 u32QuantizedSbValue >>= s32LoopCount;
187
188 /*remove the unwanted msbs*/
189 /*u32QuantizedSbValue <<= 16 - s32PresentBit;
190 u32QuantizedSbValue >>= 16 - s32PresentBit;*/
191
192 Temp <<= s32PresentBit;
193
194 Temp |= u32QuantizedSbValue ;
195 /*restore the original*/
196 u32QuantizedSbValue = u32QuantizedSbValue0;
197
198 *(pu8PacketPtr++) = Temp;
199 Temp = 0;
200 s32PresentBit = 8;
201 }
202 Temp <<= s32LoopCount;
203
204 /* remove the unwanted msbs */
205 /*u32QuantizedSbValue <<= 16 - s32LoopCount;
206 u32QuantizedSbValue >>= 16 - s32LoopCount;*/
207
208 Temp |= u32QuantizedSbValue;
209
210 s32PresentBit -= s32LoopCount;
211 }
212 }
213 ps16ScfPtr++;
214 ps32SbPtr++;
215 }
216 }
217
218 Temp <<= s32PresentBit;
219 *pu8PacketPtr = Temp;
220 pstrEncParams->u16PacketLength = pu8PacketPtr - pstrEncParams->pu8NextPacket + 1;
221 /*find CRC*/
222 pu8PacketPtr = pstrEncParams->pu8NextPacket + 1; /*Initialize the ptr*/
223 u8CRC = 0x0F;
224 s32LoopCount = s32Sb >> 1;
225
226 /*
227 The loops is run from the start of the packet till the scale factor
228 parameters. In case of JS, 'join' parameter is included in the packet
229 so that many more bytes are included in CRC calculation.
230 */
231 Temp = *pu8PacketPtr;
232 for (s32Ch = 1; s32Ch < (s32LoopCount + 4); s32Ch++) {
233 /* skip sync word and CRC bytes */
234 if (s32Ch != 3) {
235 for (s32LoopCountJ = 7; s32LoopCountJ >= 0; s32LoopCountJ--) {
236 u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01);
237 u8CRC <<= 1;
238 u8CRC ^= (u8XoredVal * 0x1D);
239 u8CRC &= 0xFF;
240 }
241 }
242 Temp = *(++pu8PacketPtr);
243 }
244
245 if (pstrEncParams->s16ChannelMode == SBC_JOINT_STEREO) {
246 for (s32LoopCountJ = 7; s32LoopCountJ >= (8 - s32NumOfSubBands); s32LoopCountJ--) {
247 u8XoredVal = ((u8CRC >> 7) & 0x01) ^ ((Temp >> s32LoopCountJ) & 0x01);
248 u8CRC <<= 1;
249 u8CRC ^= (u8XoredVal * 0x1D);
250 u8CRC &= 0xFF;
251 }
252 }
253
254 /* CRC calculation ends here */
255
256 /* store CRC in packet */
257 pu8PacketPtr = pstrEncParams->pu8NextPacket; /*Initialize the ptr*/
258 pu8PacketPtr += 3;
259 *pu8PacketPtr = u8CRC;
260 pstrEncParams->pu8NextPacket += pstrEncParams->u16PacketLength; /* move the pointer to the end in case there is more than one frame to encode */
261 }
262
263 #endif /* #if (defined(SBC_ENC_INCLUDED) && SBC_ENC_INCLUDED == TRUE) */
264
