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_decoder.h"
16 #include <memory>
17 #include "../include/sbc_constant.h"
18 #include "../include/sbc_tables.h"
19 #include "foundation/communication/bluetooth_service/services/bluetooth/service/src/gavdp/a2dp_codec/sbclib/include/sbc_frame.h"
20 #include "foundation/communication/bluetooth_service/services/bluetooth/service/src/gavdp/a2dp_codec/sbclib/include/sbc_math.h"
21 #include "new"
22 #include "securec.h"
23
24
25 namespace sbc {
26 const int BIT16_BYTE2 = 2;
27 const int SUBBANDS_NUM4 = 4;
28 const int SUBBANDS_NUM8 = 8;
29 const int MAX_CHANNEL_NUM = 2;
30 const int MULTIPLE_TEN = 10;
31 const int HIGH_BYTE_16BIT = 0xff00;
32 const int LOW_BYTE_16BIT = 0x00ff;
33 const int MOVE_RIGHT_BIT8 = 8;
34 const int INDEX_0 = 0;
35 const int INDEX_1 = 1;
36 const int INDEX_2 = 2;
37 const int INDEX_3 = 3;
38 const int INDEX_4 = 4;
39 const int INDEX_5 = 5;
40 const int INDEX_6 = 6;
41 const int INDEX_7 = 7;
42 const int INDEX_8 = 8;
43 const int INDEX_9 = 9;
44 const int VALUE_9 = 9;
45 const int VALUE_79 = 79;
46 const int VALUE_80 = 80;
47 const int VALUE_159 = 159;
48 const int VALUE_160 = 160;
49 const int VALUE_16 = 16;
50 const int VALUE_4 = 4;
51 const int VALUE_5 = 5;
52
Decoder()53 Decoder::Decoder()
54 {
55 initialized_ = false;
56 }
57
CreateDecode()58 extern "C" Decoder* CreateDecode()
59 {
60 std::unique_ptr<Decoder> decoder = std::make_unique<Decoder>();
61 return decoder.release();
62 }
63
DestroyDecode(Decoder * p)64 extern "C" void DestroyDecode(Decoder *p)
65 {
66 delete p;
67 }
68
Init(const Frame & frame)69 void Decoder::Init(const Frame& frame)
70 {
71 (void)memset_s(v_, sizeof(v_), 0, sizeof(v_));
72 for (int ch = 0; ch < MAX_CHANNEL_NUM; ch++) {
73 for (int i = 0; i < frame.subbands_ * BIT16_BYTE2; i++) {
74 offset_[ch][i] = (MULTIPLE_TEN * i + MULTIPLE_TEN);
75 }
76 }
77 initialized_ = true;
78 }
79
SBCDecode(const CodecParam & codecParam,const uint8_t * in,size_t iLength,uint8_t * out,size_t oLength,size_t * written)80 ssize_t Decoder::SBCDecode(const CodecParam& codecParam, const uint8_t* in, size_t iLength,
81 uint8_t* out, size_t oLength, size_t* written)
82 {
83 int length = frame_.Unpack(in, iLength);
84 if (length < 0) {
85 return length;
86 }
87 if (!initialized_) {
88 Init(frame_);
89 }
90 frame_.length_ = length;
91 if (!out) {
92 return length;
93 }
94
95 uint8_t *data = out;
96 auto samples = Synthesize(frame_);
97
98 if (oLength < static_cast<size_t>(samples * frame_.channels_ * BIT16_BYTE2)) {
99 samples = oLength / (frame_.channels_ * BIT16_BYTE2);
100 }
101
102 for (int sample = 0; sample < samples; sample++) {
103 for (int channel = 0; channel < frame_.channels_; channel++) {
104 int16_t val = pcmSamples_[channel][sample];
105 if (codecParam.endian == SBC_ENDIANESS_BE) {
106 *data++ = (val & HIGH_BYTE_16BIT) >> MOVE_RIGHT_BIT8;
107 *data++ = (val & LOW_BYTE_16BIT);
108 } else {
109 *data++ = (val & LOW_BYTE_16BIT);
110 *data++ = (val & HIGH_BYTE_16BIT) >> MOVE_RIGHT_BIT8;
111 }
112 }
113 }
114 if (written) {
115 *written = samples * frame_.channels_ * BIT16_BYTE2;
116 }
117 return length;
118 }
119
Synthesize(const Frame & frame)120 int Decoder::Synthesize(const Frame& frame)
121 {
122 switch (frame.subbands_) {
123 case SUBBANDS_NUM4:
124 for (int channel = 0; channel < frame.channels_; channel++) {
125 for (int blk = 0; blk < frame.blocks_; blk++) {
126 Synthesize4(frame, channel, blk);
127 }
128 }
129 break;
130 case SUBBANDS_NUM8:
131 for (int channel = 0; channel < frame.channels_; channel++) {
132 for (int blk = 0; blk < frame.blocks_; blk++) {
133 Synthesize8(frame, channel, blk);
134 }
135 }
136 break;
137 default:
138 return -1;
139 }
140 return frame.subbands_ * frame.blocks_;
141 }
142
Clip16(int32_t val)143 inline int16_t Clip16(int32_t val)
144 {
145 if (val > 0x7FFF) {
146 return 0x7FFF;
147 } else if (val < -0x8000) {
148 return -0x8000;
149 } else {
150 return val;
151 }
152 }
153
Synthesize4(const Frame & frame,int ch,int blk)154 void Decoder::Synthesize4(const Frame &frame, int ch, int blk)
155 {
156 int32_t *v = v_[ch];
157 int *offset = offset_[ch];
158
159 for (auto i = 0; i < INDEX_8; i++) {
160 offset[i]--;
161 if (offset[i] < 0) {
162 offset[i] = VALUE_79;
163 (void)memcpy_s(v + VALUE_80, VALUE_9 * sizeof(*v), v, VALUE_9 * sizeof(*v));
164 }
165
166 v[offset[i]] = Scale4Staged1(MULA(SYNMATRIX4[i][INDEX_0], frame.samples_[blk][ch][INDEX_0],
167 MULA(SYNMATRIX4[i][INDEX_1], frame.samples_[blk][ch][INDEX_1],
168 MULA(SYNMATRIX4[i][INDEX_2], frame.samples_[blk][ch][INDEX_2],
169 MUL(SYNMATRIX4[i][INDEX_3], frame.samples_[blk][ch][INDEX_3])))));
170 }
171
172 for (auto idx = 0, i = 0; i < INDEX_4; i++, idx += VALUE_5) {
173 int k = (i + VALUE_4) & 0xf;
174 pcmSamples_[ch][blk * VALUE_4 + i] = Clip16(Scale4Staged1(MULA(v[offset[i] + INDEX_0],
175 PROTO_4_40M0[idx + INDEX_0],
176 MULA(v[offset[k] + INDEX_1],
177 PROTO_4_40M1[idx + INDEX_0],
178 MULA(v[offset[i] + INDEX_2],
179 PROTO_4_40M0[idx + INDEX_1],
180 MULA(v[offset[k] + INDEX_3],
181 PROTO_4_40M1[idx + INDEX_1],
182 MULA(v[offset[i] + INDEX_4],
183 PROTO_4_40M0[idx + INDEX_2],
184 MULA(v[offset[k] + INDEX_5],
185 PROTO_4_40M1[idx + INDEX_2],
186 MULA(v[offset[i] + INDEX_6],
187 PROTO_4_40M0[idx + INDEX_3],
188 MULA(v[offset[k] + INDEX_7],
189 PROTO_4_40M1[idx + INDEX_3],
190 MULA(v[offset[i] + INDEX_8],
191 PROTO_4_40M0[idx + INDEX_4],
192 MUL(v[offset[k] + INDEX_9],
193 PROTO_4_40M1[idx + INDEX_4]))))))))))));
194 }
195 }
196
Synthesize8(const Frame & frame,int ch,int blk)197 void Decoder::Synthesize8(const Frame &frame, int ch, int blk)
198 {
199 int i = 0;
200 int idx = 0;
201 int *offset = offset_[ch];
202 for (i = 0; i < VALUE_16; i++) {
203 offset[i]--;
204 if (offset[i] < 0) {
205 offset[i] = VALUE_159;
206 for (int j = 0; j < INDEX_9; j++) {
207 v_[ch][j + VALUE_160] = v_[ch][j];
208 }
209 }
210 v_[ch][offset[i]] = Scale8Staged1(MULA(SYNMATRIX8[i][INDEX_0], frame.samples_[blk][ch][INDEX_0],
211 MULA(SYNMATRIX8[i][INDEX_1], frame.samples_[blk][ch][INDEX_1],
212 MULA(SYNMATRIX8[i][INDEX_2], frame.samples_[blk][ch][INDEX_2],
213 MULA(SYNMATRIX8[i][INDEX_3], frame.samples_[blk][ch][INDEX_3],
214 MULA(SYNMATRIX8[i][INDEX_4], frame.samples_[blk][ch][INDEX_4],
215 MULA(SYNMATRIX8[i][INDEX_5], frame.samples_[blk][ch][INDEX_5],
216 MULA(SYNMATRIX8[i][INDEX_6], frame.samples_[blk][ch][INDEX_6],
217 MUL(SYNMATRIX8[i][INDEX_7], frame.samples_[blk][ch][INDEX_7])))))))));
218 }
219 for (idx = 0, i = 0; i < INDEX_8; i++, idx += VALUE_5) {
220 int k = (i + INDEX_8) & 0xf;
221 pcmSamples_[ch][blk * INDEX_8 + i] = Clip16(Scale8Staged1(MULA(v_[ch][offset[i] + INDEX_0],
222 PROTO_8_80M0[idx + INDEX_0],
223 MULA(v_[ch][offset[k] + INDEX_1],
224 PROTO_8_80M1[idx + INDEX_0],
225 MULA(v_[ch][offset[i] + INDEX_2],
226 PROTO_8_80M0[idx + INDEX_1],
227 MULA(v_[ch][offset[k] + INDEX_3],
228 PROTO_8_80M1[idx + INDEX_1],
229 MULA(v_[ch][offset[i] + INDEX_4],
230 PROTO_8_80M0[idx + INDEX_2],
231 MULA(v_[ch][offset[k] + INDEX_5],
232 PROTO_8_80M1[idx + INDEX_2],
233 MULA(v_[ch][offset[i] + INDEX_6],
234 PROTO_8_80M0[idx + INDEX_3],
235 MULA(v_[ch][offset[k] + INDEX_7],
236 PROTO_8_80M1[idx + INDEX_3],
237 MULA(v_[ch][offset[i] + INDEX_8],
238 PROTO_8_80M0[idx + INDEX_4],
239 MUL(v_[ch][offset[k] + INDEX_9],
240 PROTO_8_80M1[idx +INDEX_4]))))))))))));
241 }
242 }
243 } // namespace sbc
244