1 /*
2  * Copyright (C) 2021-2022 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 <cinttypes>
16 #include "../include/a2dp_encoder_sbc.h"
17 #include <cmath>
18 #include <cstring>
19 #include <mutex>
20 #include "../../include/a2dp_sbc_param_ctrl.h"
21 #include "log.h"
22 #include "packet.h"
23 #include "securec.h"
24 #include "../../../a2dp_service.h"
25 
26 namespace OHOS {
27 namespace bluetooth {
28 const int BIT_SBC_NUMBER_PER_SAMPLE = 8;
29 const int MS_TO_US = 1000;
30 const int NUMBER32 = 32;
31 const int BIT8 = 8;
32 const int SUBBAND8 = 8;
33 const int SUBBAND4 = 4;
34 const int CHANNEL_ONE = 1;
35 const int CHANNEL_TWO = 2;
36 const int PROTECT_ONE = 1;
37 const int PROTECT_TWO = 2;
38 const int PROTECT_THREE = 3;
39 const int BIT_NUMBER5 = 5;
40 const int PCM_REMAIN_BUFFER_SIZE = 1024;
41 const int BIT_POOL_SIXTEEN = 16;
42 const int FRAGMENT_SIZE_TWO = 2;
43 const int FRAGMENT_SIZE_THREE = 3;
44 const int VALUE_TWO = 2;
45 
46 sbc::CodecParam g_sbcEncode = {};
47 
48 
49 std::recursive_mutex g_sbcMutex {};
A2dpSbcEncoder(const A2dpEncoderInitPeerParams * peerParams,A2dpCodecConfig * config)50 A2dpSbcEncoder::A2dpSbcEncoder(
51     const A2dpEncoderInitPeerParams *peerParams, A2dpCodecConfig *config) : A2dpEncoder(config)
52 {
53     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
54     a2dpSbcEncoderCb_.isPeerEdr = peerParams->isPeerEdr;
55     a2dpSbcEncoderCb_.peerSupports3mbps = peerParams->peerSupports3mbps;
56     a2dpSbcEncoderCb_.peerMtu = peerParams->peermtu;
57     a2dpSbcEncoderCb_.timestamp = 0;
58     a2dpSbcEncoderCb_.sendDataSize = 0;
59     codecLib_ = std::make_unique<A2dpSBCDynamicLibCtrl>(true);
60     codecSbcEncoderLib_ = codecLib_->LoadCodecSbcLib();
61     if (codecSbcEncoderLib_ == nullptr) {
62         LOG_ERROR("[SbcDecoder] %{public}s load sbc lib failed\n", __func__);
63     }
64     updateParam();
65 }
66 
~A2dpSbcEncoder()67 A2dpSbcEncoder::~A2dpSbcEncoder()
68 {
69     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
70     if (codecSbcEncoderLib_ != nullptr) {
71         codecSbcEncoderLib_->sbcEncoder.destroySbcEncode(sbcEncoder_);
72         codecLib_->UnloadCodecSbcLib(codecSbcEncoderLib_);
73         codecSbcEncoderLib_ = nullptr;
74     }
75 }
76 
ResetFeedingState(void)77 void A2dpSbcEncoder::ResetFeedingState(void)
78 {
79     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
80     std::lock_guard<std::recursive_mutex> lock(g_sbcMutex);
81     (void)memset_s(&a2dpSbcEncoderCb_.feedingState, sizeof(A2dpSbcFeedingState), 0, sizeof(A2dpSbcFeedingState));
82     a2dpSbcEncoderCb_.feedingState.bytesPerTick =
83         a2dpSbcEncoderCb_.feedingParams.sampleRate * a2dpSbcEncoderCb_.feedingParams.bitsPerSample /
84         BIT_SBC_NUMBER_PER_SAMPLE * a2dpSbcEncoderCb_.feedingParams.channelCount * A2DP_SBC_ENCODER_INTERVAL_MS /
85         MS_TO_US;
86     LOG_INFO("[SbcEncoder] %{public}s, sampleRate %{public}u, bitsPerSample %{public}u channelCount %{public}u\n",
87         __func__, a2dpSbcEncoderCb_.feedingParams.sampleRate, a2dpSbcEncoderCb_.feedingParams.bitsPerSample,
88         a2dpSbcEncoderCb_.feedingParams.channelCount);
89 
90     a2dpSbcEncoderCb_.timestamp = 0;
91     a2dpSbcEncoderCb_.sendDataSize = 0;
92 }
93 
GetRenderPosition(uint64_t & sendDataSize,uint32_t & timeStamp)94 void A2dpSbcEncoder::GetRenderPosition(uint64_t &sendDataSize, uint32_t &timeStamp)
95 {
96     std::lock_guard<std::recursive_mutex> lock(g_sbcMutex);
97     sendDataSize = a2dpSbcEncoderCb_.sendDataSize;
98     timeStamp = a2dpSbcEncoderCb_.timestamp;
99     LOG_INFO("[A2dpSbcEncoder] %{public}s sendDataSize = %{public}" PRIu64 ", timeStamp = %{public}u\n",
100         __func__, sendDataSize, timeStamp);
101 }
102 
SendFrames(uint64_t timeStampUs)103 void A2dpSbcEncoder::SendFrames(uint64_t timeStampUs)
104 {
105     LOG_INFO("[A2dpSbcEncoder] %{public}s\n", __func__);
106     std::lock_guard<std::recursive_mutex> lock(g_sbcMutex);
107     A2dpSbcEncodeFrames();
108 }
109 
A2dpSbcGetSampleRate(const uint8_t * codecInfo)110 uint16_t A2dpSbcEncoder::A2dpSbcGetSampleRate(const uint8_t *codecInfo)
111 {
112     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
113     switch (GetSampleRateSbc(codecInfo)) {
114         case A2DP_SBC_SAMPLE_RATE_16000:
115             return SBC_SAMPLE_RATE_16000;
116         case A2DP_SBC_SAMPLE_RATE_32000:
117             return SBC_SAMPLE_RATE_32000;
118         case A2DP_SBC_SAMPLE_RATE_44100:
119             return SBC_SAMPLE_RATE_44100;
120         case A2DP_SBC_SAMPLE_RATE_48000:
121             return SBC_SAMPLE_RATE_48000;
122         default:
123             LOG_INFO("Use default sampleRate: 48000");
124             return SBC_SAMPLE_RATE_48000;
125     }
126 }
127 
A2dpSbcGetBitsPerSample() const128 uint16_t A2dpSbcEncoder::A2dpSbcGetBitsPerSample() const
129 {
130     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
131 
132     switch (config_->GetAudioBitsPerSample()) {
133         case A2DP_SAMPLE_BITS_16:
134             return SBC_SAMPLE_BITS_16;
135         case A2DP_SAMPLE_BITS_24:
136             return SBC_SAMPLE_BITS_24;
137         case A2DP_SAMPLE_BITS_32:
138             return SBC_SAMPLE_BITS_32;
139         default:
140             LOG_INFO("Use default bitsPerSample: 16");
141             return SBC_SAMPLE_BITS_16;
142     }
143 }
144 
A2dpSbcGetTrackChannelCount(const uint8_t * codecInfo)145 uint8_t A2dpSbcEncoder::A2dpSbcGetTrackChannelCount(const uint8_t *codecInfo)
146 {
147     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
148     switch (GetTrackChannelCountSbc(codecInfo)) {
149         case A2DP_SBC_CHANNEL_MODE_MONO:
150             return SBC_SINGLE_CHANNEL_COUNT;
151         case A2DP_SBC_CHANNEL_MODE_DUAL:
152         case A2DP_SBC_CHANNEL_MODE_STEREO:
153         case A2DP_SBC_CHANNEL_MODE_JOINT_STEREO:
154             return SBC_DOUBLE_CHANNEL_COUNT;
155         default:
156             LOG_INFO("Failed extract a2dpSbcEncoderCb_.feedingParams.channelCount from config");
157             LOG_INFO("Use default ChannelCount: 1");
158             return SBC_SINGLE_CHANNEL_COUNT;
159     }
160 }
161 
A2dpSbcGetTrackChannelMode(const uint8_t * codecInfo)162 int16_t A2dpSbcEncoder::A2dpSbcGetTrackChannelMode(const uint8_t *codecInfo)
163 {
164     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
165     switch (GetTrackChannelModeSbc(codecInfo)) {
166         case A2DP_SBC_CHANNEL_MODE_MONO:
167             return SBC_MONO;
168         case A2DP_SBC_CHANNEL_MODE_DUAL:
169             return SBC_DUAL;
170         case A2DP_SBC_CHANNEL_MODE_STEREO:
171             return SBC_STEREO;
172         case A2DP_SBC_CHANNEL_MODE_JOINT_STEREO:
173             return SBC_JOINT_STEREO;
174         default:
175             LOG_INFO("Use default channelMode: JOINT_STEREO");
176             return SBC_JOINT_STEREO;
177     }
178 }
179 
A2dpSbcGetNumberOfSubbands(const uint8_t * codecInfo)180 int16_t A2dpSbcEncoder::A2dpSbcGetNumberOfSubbands(const uint8_t *codecInfo)
181 {
182     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
183     switch (GetNumberOfSubbandsSbc(codecInfo)) {
184         case A2DP_SBC_SUBBAND_4:
185             return SBC_SUBBAND_4;
186         case A2DP_SBC_SUBBAND_8:
187             return SBC_SUBBAND_8;
188         default:
189             LOG_INFO("Use default numOfSubBands: 8");
190             return SBC_SUBBAND_8;
191     }
192 }
193 
A2dpSbcGetNumberOfBlocks(const uint8_t * codecInfo)194 int16_t A2dpSbcEncoder::A2dpSbcGetNumberOfBlocks(const uint8_t *codecInfo)
195 {
196     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
197     switch (GetNumberOfBlocksSbc(codecInfo)) {
198         case A2DP_SBC_BLOCKS_4:
199             return SBC_BLOCKS_4;
200         case A2DP_SBC_BLOCKS_8:
201             return SBC_BLOCKS_8;
202         case A2DP_SBC_BLOCKS_12:
203             return SBC_BLOCKS_12;
204         case A2DP_SBC_BLOCKS_16:
205             return SBC_BLOCKS_16;
206         default:
207             LOG_INFO("Use default numOfBlocks: 16");
208             return SBC_BLOCKS_16;
209     }
210 }
211 
A2dpSbcGetAllocationMethodCode(const uint8_t * codecInfo)212 int16_t A2dpSbcEncoder::A2dpSbcGetAllocationMethodCode(const uint8_t *codecInfo)
213 {
214     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
215     switch (GetAllocationMethodCodeSbc(codecInfo)) {
216         case A2DP_SBC_ALLOC_MODE_S:
217             return SBC_SNR;
218         case A2DP_SBC_ALLOC_MODE_L:
219             return SBC_LOUDNESS;
220         default:
221             LOG_INFO("Use default allocationMethod: LOUDNESS");
222             return SBC_LOUDNESS;
223     }
224 }
225 
A2dpSbcUpdateEncodeFeedingParams(const uint8_t * codecInfo)226 void A2dpSbcEncoder::A2dpSbcUpdateEncodeFeedingParams(const uint8_t *codecInfo)
227 {
228     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
229     a2dpSbcEncoderCb_.feedingParams.sampleRate = A2dpSbcGetSampleRate(codecInfo);
230     a2dpSbcEncoderCb_.feedingParams.bitsPerSample = A2dpSbcGetBitsPerSample();
231     a2dpSbcEncoderCb_.feedingParams.channelCount = A2dpSbcGetTrackChannelCount(codecInfo);
232 
233     LOG_INFO("[SbcEncoder] %{public}s [sampleRate:%u] [bitsPerSample:%u] [channelMode:%u]",
234         __func__,
235         a2dpSbcEncoderCb_.feedingParams.sampleRate,
236         a2dpSbcEncoderCb_.feedingParams.bitsPerSample,
237         a2dpSbcEncoderCb_.feedingParams.channelCount);
238 }
239 
A2dpSbcUpdateEncoderParams(SBCEncoderParams * encParams,const uint8_t * codecInfo) const240 void A2dpSbcEncoder::A2dpSbcUpdateEncoderParams(SBCEncoderParams *encParams, const uint8_t *codecInfo) const
241 {
242     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
243     encParams->channelMode = A2dpSbcGetTrackChannelMode(codecInfo);
244     encParams->subBands = A2dpSbcGetNumberOfSubbands(codecInfo);
245     encParams->numOfBlocks = A2dpSbcGetNumberOfBlocks(codecInfo);
246     encParams->allocationMethod = A2dpSbcGetAllocationMethodCode(codecInfo);
247     encParams->samplingFreq = A2dpSbcGetSampleRate(codecInfo);
248     encParams->channels = A2dpSbcGetTrackChannelCount(codecInfo);
249 
250     LOG_INFO("[SbcEncoder] %{public}s [cMode:%u] [sub:%u] [blk:%u] [allc::%u] [freq:%u] [ch:%u]",
251         __func__,
252         encParams->channelMode,
253         encParams->subBands,
254         encParams->numOfBlocks,
255         encParams->allocationMethod,
256         encParams->samplingFreq,
257         encParams->channels);
258 }
259 
SetSBCParam()260 void A2dpSbcEncoder::SetSBCParam()
261 {
262     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
263     ConvertFreqParamToSBCParam();
264     ConvertModeParamToSBCParam();
265     ConvertSubbandsParamToSBCParam();
266     ConvertBlockParamToSBCParam();
267     ConvertAllocationParamToSBCParam();
268     ConvertBitpoolParamToSBCParam();
269     g_sbcEncode.endian = sbc::SBC_ENDIANESS_LE;
270     sbcEncoder_ = codecSbcEncoderLib_->sbcEncoder.createSbcEncode();
271     LOG_INFO("[SbcEncoder] %{public}s[freq:%u][mode:%u][sub:%u][block:%u][alc:%u][bitpool:%u]\n",
272         __func__,
273         g_sbcEncode.frequency,
274         g_sbcEncode.channelMode,
275         g_sbcEncode.subbands,
276         g_sbcEncode.blocks,
277         g_sbcEncode.allocation,
278         g_sbcEncode.bitpool);
279 }
280 
updateParam(void)281 void A2dpSbcEncoder::updateParam(void)
282 {
283     UpdateEncoderParam();
284 }
285 
UpdateEncoderParam(void)286 void A2dpSbcEncoder::UpdateEncoderParam(void)
287 {
288     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
289     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
290     uint16_t samplingFreq = 48000;
291     uint8_t codecInfo[A2DP_CODEC_SIZE];
292     uint8_t *codecConfigInfo = &codecInfo[0];
293     if (!config_->CopyOutOtaCodecConfig(codecConfigInfo)) {
294         LOG_ERROR("Failed to read A2dpCodecConfig!");
295         return;
296     }
297     const uint8_t *codecCfgInfo = codecConfigInfo;
298     int minBitPool = GetMinBitpoolSbc(codecCfgInfo);
299     int maxBitPool = GetMaxBitpoolSbc(codecCfgInfo);
300 
301     A2dpSbcUpdateEncodeFeedingParams(codecCfgInfo);
302 
303     ResetFeedingState();
304 
305     A2dpSbcUpdateEncoderParams(encParams, codecCfgInfo);
306 
307     const uint16_t mtu = A2DP_SBC_MAX_PACKET_SIZE - A2DP_SBC_PACKET_HEAD_SIZE - A2DP_SBC_MEDIA_PAYLOAD_HEAD_SIZE;
308     a2dpSbcEncoderCb_.mtuSize = a2dpSbcEncoderCb_.peerMtu < mtu ? a2dpSbcEncoderCb_.peerMtu : mtu;
309 
310     samplingFreq = encParams->samplingFreq;
311     encParams->bitRate = A2DP_SBC_DEFAULT_BITRATE;
312 
313     if (!a2dpSbcEncoderCb_.isPeerEdr) {
314         encParams->bitRate = A2DP_SBC_NON_EDR_MAX_BITRATE;
315     }
316 
317     A2dpSbcCalculateEncBitPool(samplingFreq, minBitPool, maxBitPool);
318     SetSBCParam();
319     UpdateMtuSize();
320 }
321 
A2dpSbcReadFeeding(uint32_t * bytesRead)322 bool A2dpSbcEncoder::A2dpSbcReadFeeding(uint32_t *bytesRead)
323 {
324     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
325 
326     uint32_t expectedReadPcmData = a2dpSbcEncoderCb_.feedingState.bytesPerTick;
327 
328     if (expectedReadPcmData + a2dpSbcEncoderCb_.offsetPCM > FRAME_THREE * A2DP_SBC_MAX_PACKET_SIZE) {
329         LOG_ERROR("[Feeding] buffer no space");
330         return false;
331     }
332     A2dpProfile *profile = GetProfileInstance(A2DP_ROLE_SOURCE);
333     uint32_t actualReadPcmData = profile->GetPcmData(&a2dpSbcEncoderCb_.pcmBuffer[a2dpSbcEncoderCb_.offsetPCM],
334         expectedReadPcmData);
335     LOG_INFO("[ReadA2dpSharedBuffer][expectedReadPcmData:%u][actualReadPcmData:%u]",
336         expectedReadPcmData, actualReadPcmData);
337 
338     *bytesRead = actualReadPcmData;
339 
340     if (actualReadPcmData) {
341         LOG_INFO("[Feeding][offsetPCM:%u][readBytes:%u]", a2dpSbcEncoderCb_.offsetPCM, actualReadPcmData);
342         return true;
343     } else {
344         LOG_ERROR("[Feeding][read no data][readBytes:%u]", actualReadPcmData);
345         return false;
346     }
347 }
348 
ConvertFreqParamToSBCParam(void)349 void A2dpSbcEncoder::ConvertFreqParamToSBCParam(void)
350 {
351     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
352     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
353     switch (encParams->samplingFreq) {
354         case SBC_SAMPLE_RATE_16000:
355             g_sbcEncode.frequency = sbc::SBC_FREQ_16000;
356             break;
357         case SBC_SAMPLE_RATE_32000:
358             g_sbcEncode.frequency = sbc::SBC_FREQ_32000;
359             break;
360         case SBC_SAMPLE_RATE_44100:
361             g_sbcEncode.frequency = sbc::SBC_FREQ_44100;
362             break;
363         case SBC_SAMPLE_RATE_48000:
364             g_sbcEncode.frequency = sbc::SBC_FREQ_48000;
365             break;
366         default:
367             g_sbcEncode.frequency = sbc::SBC_FREQ_44100;
368             break;
369     }
370 }
371 
ConvertModeParamToSBCParam(void)372 void A2dpSbcEncoder::ConvertModeParamToSBCParam(void)
373 {
374     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
375     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
376     switch (encParams->channelMode) {
377         case SBC_MONO:
378             g_sbcEncode.channelMode = sbc::SBC_CHANNEL_MODE_MONO;
379             break;
380         case SBC_DUAL:
381             g_sbcEncode.channelMode = sbc::SBC_CHANNEL_MODE_DUAL_CHANNEL;
382             break;
383         case SBC_STEREO:
384             g_sbcEncode.channelMode = sbc::SBC_CHANNEL_MODE_STEREO;
385             break;
386         case SBC_JOINT_STEREO:
387             g_sbcEncode.channelMode = sbc::SBC_CHANNEL_MODE_JOINT_STEREO;
388             break;
389         default:
390             g_sbcEncode.channelMode = sbc::SBC_CHANNEL_MODE_STEREO;
391             break;
392     }
393 }
394 
ConvertSubbandsParamToSBCParam(void)395 void A2dpSbcEncoder::ConvertSubbandsParamToSBCParam(void)
396 {
397     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
398     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
399     switch (encParams->subBands) {
400         case SBC_SUBBAND_4:
401             g_sbcEncode.subbands = sbc::SBC_SUBBAND4;
402             break;
403         case SBC_SUBBAND_8:
404             g_sbcEncode.subbands = sbc::SBC_SUBBAND8;
405             break;
406         default:
407             g_sbcEncode.subbands = sbc::SBC_SUBBAND8;
408             break;
409     }
410 }
411 
ConvertBlockParamToSBCParam(void)412 void A2dpSbcEncoder::ConvertBlockParamToSBCParam(void)
413 {
414     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
415     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
416     switch (encParams->numOfBlocks) {
417         case SBC_BLOCKS_4:
418             g_sbcEncode.blocks = sbc::SBC_BLOCK4;
419             break;
420         case SBC_BLOCKS_8:
421             g_sbcEncode.blocks = sbc::SBC_BLOCK8;
422             break;
423         case SBC_BLOCKS_12:
424             g_sbcEncode.blocks = sbc::SBC_BLOCK12;
425             break;
426         case SBC_BLOCKS_16:
427             g_sbcEncode.blocks = sbc::SBC_BLOCK16;
428             break;
429         default:
430             g_sbcEncode.blocks = sbc::SBC_BLOCK16;
431             break;
432     }
433 }
434 
ConvertAllocationParamToSBCParam(void)435 void A2dpSbcEncoder::ConvertAllocationParamToSBCParam(void)
436 {
437     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
438     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
439     switch (encParams->allocationMethod) {
440         case SBC_LOUDNESS:
441             g_sbcEncode.allocation = sbc::SBC_ALLOCATION_LOUDNESS;
442             break;
443         case SBC_SNR:
444             g_sbcEncode.allocation = sbc::SBC_ALLOCATION_SNR;
445             break;
446         default:
447             g_sbcEncode.allocation = sbc::SBC_ALLOCATION_LOUDNESS;
448             break;
449     }
450 }
451 
ConvertBitpoolParamToSBCParam(void)452 void A2dpSbcEncoder::ConvertBitpoolParamToSBCParam(void)
453 {
454     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
455     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
456     g_sbcEncode.bitpool = encParams->bitPool;
457 }
458 
CalculateSbcPCMRemain(uint16_t codecSize,uint32_t bytesNum,uint8_t * numOfFrame)459 void A2dpSbcEncoder::CalculateSbcPCMRemain(uint16_t codecSize, uint32_t bytesNum, uint8_t *numOfFrame)
460 {
461     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
462     if (codecSize != 0) {
463         LOG_INFO("[offset:%u][bytes:%u][codecSize:%u]", a2dpSbcEncoderCb_.offsetPCM, bytesNum, codecSize);
464         *numOfFrame = (a2dpSbcEncoderCb_.offsetPCM + bytesNum) / codecSize;
465         a2dpSbcEncoderCb_.offsetPCM = (a2dpSbcEncoderCb_.offsetPCM + bytesNum) % codecSize;
466         uint16_t encodePCMSize = *numOfFrame * codecSize;
467         LOG_INFO("[numOfFrame:%u][offset:%u][bytes:%u]", *numOfFrame, a2dpSbcEncoderCb_.offsetPCM, bytesNum);
468         if (a2dpSbcEncoderCb_.offsetPCM > PCM_REMAIN_BUFFER_SIZE) {
469             a2dpSbcEncoderCb_.offsetPCM = PCM_REMAIN_BUFFER_SIZE;
470         }
471         (void)memcpy_s(a2dpSbcEncoderCb_.pcmRemain, A2DP_SBC_MAX_PACKET_SIZE,
472             &a2dpSbcEncoderCb_.pcmBuffer[encodePCMSize], a2dpSbcEncoderCb_.offsetPCM);
473     }
474 }
475 
A2dpSbcEncodeFrames(void)476 void A2dpSbcEncoder::A2dpSbcEncodeFrames(void)
477 {
478     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
479     size_t encoded = 0;
480     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
481     uint16_t blocksXsubbands = encParams->subBands * encParams->numOfBlocks;
482     uint16_t channelMode = (g_sbcEncode.channelMode == sbc::SBC_CHANNEL_MODE_MONO) ? CHANNEL_ONE : CHANNEL_TWO;
483     uint16_t subbands = g_sbcEncode.subbands ? SUBBAND8 : SUBBAND4;
484     const uint16_t blocks = SUBBAND4 + (g_sbcEncode.blocks * SUBBAND4);
485     uint16_t codecSize = subbands * blocks * channelMode * VALUE_TWO;
486     Packet *pkt = PacketMalloc(A2DP_SBC_FRAGMENT_HEADER, 0, 0);
487     uint32_t bytesNum = 0;
488     uint8_t numOfFrame = 0;
489     if (A2dpSbcReadFeeding(&bytesNum)) {
490         CalculateSbcPCMRemain(codecSize, bytesNum, &numOfFrame);
491         uint8_t frameIter = 0;
492         uint16_t pcmOffset = 0;
493         while (numOfFrame) {
494             uint8_t outputBuf[A2DP_SBC_HQ_DUAL_BP_53_FRAME_SIZE] = {};
495             int16_t outputLen = sbcEncoder_->SBCEncode(g_sbcEncode, &a2dpSbcEncoderCb_.pcmBuffer[pcmOffset],
496                 blocksXsubbands * channelMode, outputBuf, sizeof(outputBuf), &encoded);
497             LOG_INFO("[SbcEncoder] %{public}s encoded %{public}zu, pcmOffset%{public}u\n",
498                 __func__, encoded, pcmOffset);
499             if (outputLen < 0) {
500                 LOG_ERROR("err occur.");
501             }
502             pcmOffset += blocksXsubbands * channelMode * CHANNEL_TWO;
503             Buffer *encBuf = BufferMalloc(encoded);
504             if (memcpy_s(BufferPtr(encBuf), encoded, outputBuf, encoded) != EOK) {
505                 BufferFree(encBuf);
506                 PacketFree(pkt);
507                 return;
508             }
509             PacketPayloadAddLast(pkt, encBuf);
510             BufferFree(encBuf);
511             numOfFrame--;
512             frameIter++;
513         }
514         uint16_t encodePacketSize = PacketSize(pkt);
515         (void)memset_s(a2dpSbcEncoderCb_.pcmBuffer, A2DP_SBC_MAX_PACKET_SIZE * FRAME_THREE,
516             0, sizeof(a2dpSbcEncoderCb_.pcmBuffer));
517         if (memcpy_s(a2dpSbcEncoderCb_.pcmBuffer, A2DP_SBC_MAX_PACKET_SIZE * FRAME_THREE,
518             a2dpSbcEncoderCb_.pcmRemain, a2dpSbcEncoderCb_.offsetPCM) != EOK) {
519                 PacketFree(pkt);
520                 return;
521             }
522         (void)memset_s(a2dpSbcEncoderCb_.pcmRemain, A2DP_SBC_MAX_PACKET_SIZE, 0, sizeof(a2dpSbcEncoderCb_.pcmRemain));
523         if (encodePacketSize > 0) {
524             uint32_t pktTimeStamp = a2dpSbcEncoderCb_.timestamp;
525             a2dpSbcEncoderCb_.timestamp += frameIter * blocksXsubbands;
526             a2dpSbcEncoderCb_.sendDataSize += codecSize * frameIter;
527             EnqueuePacket(pkt, frameIter, encodePacketSize, pktTimeStamp, (uint16_t)encoded);  // Enqueue Packet.
528             LOG_INFO("[SbcEncoder] %{public}s timestamp %{public}u, sendDataSize%{public}u\n",
529                 __func__, a2dpSbcEncoderCb_.timestamp, a2dpSbcEncoderCb_.sendDataSize);
530         }
531     }
532     PacketFree(pkt);
533 }
534 
EnqueuePacket(Packet * pkt,size_t frames,const uint32_t bytes,uint32_t timeStamp,const uint16_t frameSize) const535 void A2dpSbcEncoder::EnqueuePacket(
536     Packet *pkt, size_t frames, const uint32_t bytes, uint32_t timeStamp, const uint16_t frameSize) const
537 {
538     LOG_INFO("[EnqueuePacket][frameSize:%hu][FrameNum:%zu], mtu[%hu], totalSize[%u]",
539         frameSize, frames, a2dpSbcEncoderCb_.mtuSize, PacketSize(pkt));
540     if (PacketSize(pkt) < static_cast<uint32_t>(a2dpSbcEncoderCb_.mtuSize)) {
541         Buffer *header = PacketHead(pkt);
542         uint8_t *p = static_cast<uint8_t*>(BufferPtr(header));
543         *p = frames;
544         A2dpProfile *profile = GetProfileInstance(A2DP_ROLE_SOURCE);
545         profile->EnqueuePacket(pkt, frames, bytes, timeStamp);  // Enqueue Packet.
546     } else {
547         EnqueuePacketFragment(pkt, frames, bytes, timeStamp, frameSize);
548     }
549 }
550 
EnqueuePacketFragment(Packet * pkt,size_t frames,const uint32_t bytes,uint32_t timeStamp,const uint16_t frameSize) const551 void A2dpSbcEncoder::EnqueuePacketFragment(
552     Packet *pkt, size_t frames, const uint32_t bytes, uint32_t timeStamp, const uint16_t frameSize) const
553 {
554     LOG_INFO("[SbcEncoder] %{public}s\n", __func__);
555     uint8_t count = 1;
556     uint32_t pktLen = 0;
557     uint8_t frameNum = 0;
558     uint8_t sentFrameNum = 0;
559     uint16_t blocksXsubbands
560         = a2dpSbcEncoderCb_.sbcEncoderParams.subBands * a2dpSbcEncoderCb_.sbcEncoderParams.numOfBlocks;
561     bool frameFragmented = false;
562     if ((static_cast<uint16_t>(frames) / FRAGMENT_SIZE_TWO  + 1) * frameSize  < (a2dpSbcEncoderCb_.mtuSize)) {
563         count = FRAGMENT_SIZE_TWO;
564         pktLen = (frames / count) * frameSize;
565     } else {
566         count = FRAGMENT_SIZE_THREE;
567         pktLen = (frames / count) * frameSize;
568         if (pktLen > static_cast<uint32_t>(a2dpSbcEncoderCb_.mtuSize) ||
569             (frames - frames / count * FRAGMENT_SIZE_TWO) * frameSize  >
570             static_cast<uint32_t>(a2dpSbcEncoderCb_.mtuSize)) {
571             frameFragmented = true;
572             LOG_ERROR("mtu size isn't enough.[frameSize:%u] mtu[%u]", pktLen, a2dpSbcEncoderCb_.mtuSize);
573         }
574     }
575     frameNum = frames / count;
576     if (!frameFragmented) {
577         size_t remainFrames = frames;
578         uint16_t offset = 0;
579         do {
580             Packet *mediaPacket = PacketMalloc(A2DP_SBC_FRAGMENT_HEADER, 0, 0);
581             if (count == 1) {
582                 frameNum = remainFrames;
583                 pktLen = frameNum * frameSize;
584             }
585             count--;
586             LOG_INFO("[EnqueuePacket] [pktLen:%u] [sFrameNum:%u] [remain:%u]", pktLen, frameNum, PacketSize(pkt));
587             Buffer *header = PacketHead(mediaPacket);
588             uint8_t *p = static_cast<uint8_t*>(BufferPtr(header));
589             *p = frameNum;
590             uint8_t bufferFra[1024];
591             size_t encoded = pktLen;
592             PacketPayloadRead(pkt, bufferFra, offset, pktLen);
593             offset += pktLen;
594             Buffer *encBuf = BufferMalloc(encoded);
595             if (memcpy_s(BufferPtr(encBuf), encoded, bufferFra, encoded) != EOK) {
596                 LOG_ERROR("[EnqueuePacket] memcpy_s fail");
597                 return;
598             }
599             PacketPayloadAddLast(mediaPacket, encBuf);
600             BufferFree(encBuf);
601             remainFrames -= frameNum;
602 
603             uint16_t encodePacketSize = PacketSize(mediaPacket);
604             LOG_ERROR("[EnqueuePacket] encodePacketSize is ");
605             A2dpProfile *profile = GetProfileInstance(A2DP_ROLE_SOURCE);
606             profile->EnqueuePacket(mediaPacket, frameNum, encodePacketSize, timeStamp + sentFrameNum * blocksXsubbands);
607             // Enqueue Packet.
608             sentFrameNum += frameNum;
609             PacketFree(mediaPacket);
610         } while (count > 0);
611     }
612 }
613 
A2dpSbcCalculateEncBitPool(uint16_t samplingFreq,uint16_t minBitPool,uint16_t maxBitPool)614 void A2dpSbcEncoder::A2dpSbcCalculateEncBitPool(uint16_t samplingFreq, uint16_t minBitPool, uint16_t maxBitPool)
615 {
616     SBCEncoderParams *encParams = &a2dpSbcEncoderCb_.sbcEncoderParams;
617     uint8_t protect = 0;
618     int16_t bitPool = 0;
619     int16_t bitRate = 0;
620     if ((encParams == nullptr) || (samplingFreq == 0) || (encParams->subBands == 0) || (encParams->channels == 0)) {
621         return;
622     }
623     do {  // Dynamic bitpool support.
624         if ((encParams->channelMode == SBC_JOINT_STEREO) || (encParams->channelMode == SBC_STEREO)) {
625             bitPool = static_cast<int16_t>((encParams->bitRate * encParams->subBands * MS_TO_US / samplingFreq) -
626                                            ((NUMBER32 + (SUBBAND4 * encParams->subBands * encParams->channels) +
627                                            ((encParams->channelMode - CHANNEL_TWO) * encParams->subBands)) /
628                                            encParams->numOfBlocks));
629             int16_t frameLen =
630                 SUBBAND4 + (SUBBAND4 * encParams->subBands * encParams->channels) / BIT8 +
631                 (((encParams->channelMode - CHANNEL_TWO) * encParams->subBands) + (encParams->numOfBlocks * bitPool)) /
632                 BIT8;
633             bitRate = (BIT8 * frameLen * samplingFreq) / (encParams->subBands * encParams->numOfBlocks * MS_TO_US);
634             if (bitRate > encParams->bitRate) {
635                 bitPool--;
636             }
637             encParams->bitPool =
638                 (bitPool > (NUMBER32 * encParams->subBands)) ? (NUMBER32 * encParams->subBands) : bitPool;
639         } else {  // Mono or Dual Channel.
640             bitPool = static_cast<int16_t>(
641                 ((encParams->subBands * encParams->bitRate * MS_TO_US) / (samplingFreq * encParams->channels)) -
642                 (((NUMBER32 / encParams->channels) + (SUBBAND4 * encParams->subBands)) / encParams->numOfBlocks));
643             encParams->bitPool = (bitPool > (BIT_POOL_SIXTEEN * encParams->subBands)) ?
644                 (BIT_POOL_SIXTEEN * encParams->subBands) : bitPool;
645         }
646         if (bitPool > maxBitPool) {
647             encParams->bitRate -= BIT_NUMBER5;
648             protect |= PROTECT_ONE;
649         } else if (bitPool < minBitPool) {
650             uint16_t preBitRate = encParams->bitRate;
651             encParams->bitRate += BIT_NUMBER5;
652             protect |= PROTECT_TWO;
653             if (encParams->bitRate < preBitRate) {
654                 protect |= PROTECT_THREE;
655             }
656         } else {
657             break;
658         }
659         if (protect == PROTECT_THREE) {
660             break;
661         }
662     } while (true);
663     encParams->bitPool = bitPool;
664 }
665 
UpdateMtuSize(void)666 void A2dpSbcEncoder::UpdateMtuSize(void)
667 {
668     LOG_INFO("[SBCEncoder] %{public}s", __func__);
669     uint16_t mtu = a2dpSbcEncoderCb_.mtuSize;
670 
671     if (a2dpSbcEncoderCb_.isPeerEdr) {
672         if (!a2dpSbcEncoderCb_.peerSupports3mbps) {
673             if (mtu > MAX_2MBPS_SBC_PAYLOAD_MTU) {  // MTU is only payload.
674                 mtu = MAX_2MBPS_SBC_PAYLOAD_MTU;
675                 a2dpSbcEncoderCb_.mtuSize = mtu;
676             }
677         } else {                                    // EDR 3mbps.
678             if (mtu > MAX_3MBPS_SBC_PAYLOAD_MTU) {  // MTU is only payload.
679                 mtu = MAX_3MBPS_SBC_PAYLOAD_MTU;
680                 a2dpSbcEncoderCb_.mtuSize = mtu;
681             }
682         }
683     }
684     LOG_INFO("[SBCEncoder] %{public}s [mtuSize:%u]", __func__, a2dpSbcEncoderCb_.mtuSize);
685 }
686 }  // namespace bluetooth
687 }  // namespace OHOS