1 /******************************************************************************
2  *
3  *  Copyright (C) 2016 The Android Open Source Project
4  *  Copyright (C) 2009-2012 Broadcom Corporation
5  *
6  *  Licensed under the Apache License, Version 2.0 (the "License");
7  *  you may not use this file except in compliance with the License.
8  *  You may obtain a copy of the License at:
9  *
10  *  http://www.apache.org/licenses/LICENSE-2.0
11  *
12  *  Unless required by applicable law or agreed to in writing, software
13  *  distributed under the License is distributed on an "AS IS" BASIS,
14  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  *  See the License for the specific language governing permissions and
16  *  limitations under the License.
17  *
18  ******************************************************************************/
19 
20 #define LOG_TAG "a2dp_sbc_encoder"
21 
22 #include "a2dp_sbc_encoder.h"
23 
24 #include <limits.h>
25 #include <stdio.h>
26 #include <string.h>
27 
28 #include "a2dp_sbc.h"
29 #include "a2dp_sbc_up_sample.h"
30 #include "bt_common.h"
31 #include "embdrv/sbc/encoder/include/sbc_encoder.h"
32 #include "osi/include/log.h"
33 #include "osi/include/osi.h"
34 
35 /* Buffer pool */
36 #define A2DP_SBC_BUFFER_SIZE BT_DEFAULT_BUFFER_SIZE
37 
38 // A2DP SBC encoder interval in milliseconds.
39 #define A2DP_SBC_ENCODER_INTERVAL_MS 20
40 
41 /* High quality quality setting @ 44.1 khz */
42 #define A2DP_SBC_DEFAULT_BITRATE 328
43 
44 #define A2DP_SBC_NON_EDR_MAX_RATE 229
45 
46 /*
47  * 2DH5 payload size of:
48  * 679 bytes - (4 bytes L2CAP Header + 12 bytes AVDTP Header)
49  */
50 #define MAX_2MBPS_AVDTP_MTU 663
51 #define A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK 3
52 
53 #define A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1 119
54 #define A2DP_SBC_MAX_HQ_FRAME_SIZE_48 115
55 
56 /* Define the bitrate step when trying to match bitpool value */
57 #define A2DP_SBC_BITRATE_STEP 5
58 
59 /* Readability constants */
60 #define A2DP_SBC_FRAME_HEADER_SIZE_BYTES 4  // A2DP Spec v1.3, 12.4, Table 12.12
61 #define A2DP_SBC_SCALE_FACTOR_BITS 4        // A2DP Spec v1.3, 12.4, Table 12.13
62 
63 /* offset */
64 #if (BTA_AV_CO_CP_SCMS_T == TRUE)
65 /* A2DP header will contain a CP header of size 1 */
66 #define A2DP_HDR_SIZE 2
67 #define A2DP_SBC_OFFSET (AVDT_MEDIA_OFFSET + A2DP_SBC_MPL_HDR_LEN + 1)
68 #else
69 #define A2DP_HDR_SIZE 1
70 #define A2DP_SBC_OFFSET (AVDT_MEDIA_OFFSET + A2DP_SBC_MPL_HDR_LEN)
71 #endif
72 
73 typedef struct {
74   uint32_t aa_frame_counter;
75   int32_t aa_feed_counter;
76   int32_t aa_feed_residue;
77   uint32_t counter;
78   uint32_t bytes_per_tick; /* pcm bytes read each media task tick */
79   uint64_t last_frame_us;
80 } tA2DP_SBC_FEEDING_STATE;
81 
82 typedef struct {
83   uint64_t session_start_us;
84 
85   size_t media_read_total_expected_packets;
86   size_t media_read_total_expected_reads_count;
87   size_t media_read_total_expected_read_bytes;
88 
89   size_t media_read_total_dropped_packets;
90   size_t media_read_total_actual_reads_count;
91   size_t media_read_total_actual_read_bytes;
92 
93   size_t media_read_total_expected_frames;
94   size_t media_read_total_dropped_frames;
95 } a2dp_sbc_encoder_stats_t;
96 
97 typedef struct {
98   a2dp_source_read_callback_t read_callback;
99   a2dp_source_enqueue_callback_t enqueue_callback;
100   uint16_t TxAaMtuSize;
101   uint8_t tx_sbc_frames;
102   bool is_peer_edr;         /* True if the peer device supports EDR */
103   bool peer_supports_3mbps; /* True if the peer device supports 3Mbps EDR */
104   uint16_t peer_mtu;        /* MTU of the A2DP peer */
105   uint32_t timestamp;       /* Timestamp for the A2DP frames */
106   SBC_ENC_PARAMS sbc_encoder_params;
107   tA2DP_FEEDING_PARAMS feeding_params;
108   tA2DP_SBC_FEEDING_STATE feeding_state;
109   int16_t pcmBuffer[SBC_MAX_PCM_BUFFER_SIZE];
110 
111   a2dp_sbc_encoder_stats_t stats;
112 } tA2DP_SBC_ENCODER_CB;
113 
114 static tA2DP_SBC_ENCODER_CB a2dp_sbc_encoder_cb;
115 
116 static void a2dp_sbc_encoder_update(uint16_t peer_mtu,
117                                     A2dpCodecConfig* a2dp_codec_config,
118                                     bool* p_restart_input,
119                                     bool* p_restart_output,
120                                     bool* p_config_updated);
121 static bool a2dp_sbc_read_feeding(void);
122 static void a2dp_sbc_encode_frames(uint8_t nb_frame);
123 static void a2dp_sbc_get_num_frame_iteration(uint8_t* num_of_iterations,
124                                              uint8_t* num_of_frames,
125                                              uint64_t timestamp_us);
126 static uint8_t calculate_max_frames_per_packet(void);
127 static uint16_t a2dp_sbc_source_rate(void);
128 static uint32_t a2dp_sbc_frame_length(void);
129 
A2DP_LoadEncoderSbc(void)130 bool A2DP_LoadEncoderSbc(void) {
131   // Nothing to do - the library is statically linked
132   return true;
133 }
134 
A2DP_UnloadEncoderSbc(void)135 void A2DP_UnloadEncoderSbc(void) {
136   // Nothing to do - the library is statically linked
137 }
138 
a2dp_sbc_encoder_init(const tA2DP_ENCODER_INIT_PEER_PARAMS * p_peer_params,A2dpCodecConfig * a2dp_codec_config,a2dp_source_read_callback_t read_callback,a2dp_source_enqueue_callback_t enqueue_callback)139 void a2dp_sbc_encoder_init(const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params,
140                            A2dpCodecConfig* a2dp_codec_config,
141                            a2dp_source_read_callback_t read_callback,
142                            a2dp_source_enqueue_callback_t enqueue_callback) {
143   memset(&a2dp_sbc_encoder_cb, 0, sizeof(a2dp_sbc_encoder_cb));
144 
145   a2dp_sbc_encoder_cb.stats.session_start_us = time_get_os_boottime_us();
146 
147   a2dp_sbc_encoder_cb.read_callback = read_callback;
148   a2dp_sbc_encoder_cb.enqueue_callback = enqueue_callback;
149   a2dp_sbc_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr;
150   a2dp_sbc_encoder_cb.peer_supports_3mbps = p_peer_params->peer_supports_3mbps;
151   a2dp_sbc_encoder_cb.peer_mtu = p_peer_params->peer_mtu;
152   a2dp_sbc_encoder_cb.timestamp = 0;
153 
154   // NOTE: Ignore the restart_input / restart_output flags - this initization
155   // happens when the connection is (re)started.
156   bool restart_input = false;
157   bool restart_output = false;
158   bool config_updated = false;
159   a2dp_sbc_encoder_update(a2dp_sbc_encoder_cb.peer_mtu, a2dp_codec_config,
160                           &restart_input, &restart_output, &config_updated);
161 }
162 
updateEncoderUserConfig(const tA2DP_ENCODER_INIT_PEER_PARAMS * p_peer_params,bool * p_restart_input,bool * p_restart_output,bool * p_config_updated)163 bool A2dpCodecConfigSbc::updateEncoderUserConfig(
164     const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params, bool* p_restart_input,
165     bool* p_restart_output, bool* p_config_updated) {
166   a2dp_sbc_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr;
167   a2dp_sbc_encoder_cb.peer_supports_3mbps = p_peer_params->peer_supports_3mbps;
168   a2dp_sbc_encoder_cb.peer_mtu = p_peer_params->peer_mtu;
169   a2dp_sbc_encoder_cb.timestamp = 0;
170 
171   if (a2dp_sbc_encoder_cb.peer_mtu == 0) {
172     LOG_ERROR(LOG_TAG,
173               "%s: Cannot update the codec encoder for %s: "
174               "invalid peer MTU",
175               __func__, name().c_str());
176     return false;
177   }
178 
179   a2dp_sbc_encoder_update(a2dp_sbc_encoder_cb.peer_mtu, this, p_restart_input,
180                           p_restart_output, p_config_updated);
181   return true;
182 }
183 
184 // Update the A2DP SBC encoder.
185 // |peer_mtu| is the peer MTU.
186 // |a2dp_codec_config| is the A2DP codec to use for the update.
a2dp_sbc_encoder_update(uint16_t peer_mtu,A2dpCodecConfig * a2dp_codec_config,bool * p_restart_input,bool * p_restart_output,bool * p_config_updated)187 static void a2dp_sbc_encoder_update(uint16_t peer_mtu,
188                                     A2dpCodecConfig* a2dp_codec_config,
189                                     bool* p_restart_input,
190                                     bool* p_restart_output,
191                                     bool* p_config_updated) {
192   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
193   uint8_t codec_info[AVDT_CODEC_SIZE];
194   uint16_t s16SamplingFreq;
195   int16_t s16BitPool = 0;
196   int16_t s16BitRate;
197   int16_t s16FrameLen;
198   uint8_t protect = 0;
199   int min_bitpool;
200   int max_bitpool;
201 
202   *p_restart_input = false;
203   *p_restart_output = false;
204   *p_config_updated = false;
205   if (!a2dp_codec_config->copyOutOtaCodecConfig(codec_info)) {
206     LOG_ERROR(LOG_TAG,
207               "%s: Cannot update the codec encoder for %s: "
208               "invalid codec config",
209               __func__, a2dp_codec_config->name().c_str());
210     return;
211   }
212   const uint8_t* p_codec_info = codec_info;
213   min_bitpool = A2DP_GetMinBitpoolSbc(p_codec_info);
214   max_bitpool = A2DP_GetMaxBitpoolSbc(p_codec_info);
215 
216   // The feeding parameters
217   tA2DP_FEEDING_PARAMS* p_feeding_params = &a2dp_sbc_encoder_cb.feeding_params;
218   p_feeding_params->sample_rate = A2DP_GetTrackSampleRateSbc(p_codec_info);
219   p_feeding_params->bits_per_sample =
220       A2DP_GetTrackBitsPerSampleSbc(p_codec_info);
221   p_feeding_params->channel_count = A2DP_GetTrackChannelCountSbc(p_codec_info);
222   LOG_DEBUG(LOG_TAG, "%s: sample_rate=%u bits_per_sample=%u channel_count=%u",
223             __func__, p_feeding_params->sample_rate,
224             p_feeding_params->bits_per_sample, p_feeding_params->channel_count);
225 
226   // The codec parameters
227   p_encoder_params->s16ChannelMode = A2DP_GetChannelModeCodeSbc(p_codec_info);
228   p_encoder_params->s16NumOfSubBands =
229       A2DP_GetNumberOfSubbandsSbc(p_codec_info);
230   p_encoder_params->s16NumOfBlocks = A2DP_GetNumberOfBlocksSbc(p_codec_info);
231   p_encoder_params->s16AllocationMethod =
232       A2DP_GetAllocationMethodCodeSbc(p_codec_info);
233   p_encoder_params->s16SamplingFreq =
234       A2DP_GetSamplingFrequencyCodeSbc(p_codec_info);
235   p_encoder_params->s16NumOfChannels =
236       A2DP_GetTrackChannelCountSbc(p_codec_info);
237 
238   // Reset invalid parameters
239   if (!p_encoder_params->s16NumOfSubBands) {
240     LOG_WARN(LOG_TAG, "%s: SubBands are set to 0, resetting to max (%d)",
241              __func__, SBC_MAX_NUM_OF_SUBBANDS);
242     p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
243   }
244   if (!p_encoder_params->s16NumOfBlocks) {
245     LOG_WARN(LOG_TAG, "%s: Blocks are set to 0, resetting to max (%d)",
246              __func__, SBC_MAX_NUM_OF_BLOCKS);
247     p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
248   }
249   if (!p_encoder_params->s16NumOfChannels) {
250     LOG_WARN(LOG_TAG, "%s: Channels are set to 0, resetting to max (%d)",
251              __func__, SBC_MAX_NUM_OF_CHANNELS);
252     p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
253   }
254 
255   uint16_t mtu_size = A2DP_SBC_BUFFER_SIZE - A2DP_SBC_OFFSET - sizeof(BT_HDR);
256   if (mtu_size < peer_mtu) {
257     a2dp_sbc_encoder_cb.TxAaMtuSize = mtu_size;
258   } else {
259     a2dp_sbc_encoder_cb.TxAaMtuSize = peer_mtu;
260   }
261 
262   if (p_encoder_params->s16SamplingFreq == SBC_sf16000)
263     s16SamplingFreq = 16000;
264   else if (p_encoder_params->s16SamplingFreq == SBC_sf32000)
265     s16SamplingFreq = 32000;
266   else if (p_encoder_params->s16SamplingFreq == SBC_sf44100)
267     s16SamplingFreq = 44100;
268   else
269     s16SamplingFreq = 48000;
270 
271   // Set the initial target bit rate
272   p_encoder_params->u16BitRate = a2dp_sbc_source_rate();
273 
274   LOG_DEBUG(LOG_TAG, "%s: MTU=%d, peer_mtu=%d min_bitpool=%d max_bitpool=%d",
275             __func__, a2dp_sbc_encoder_cb.TxAaMtuSize, peer_mtu, min_bitpool,
276             max_bitpool);
277   LOG_DEBUG(LOG_TAG,
278             "%s: ChannelMode=%d, NumOfSubBands=%d, NumOfBlocks=%d, "
279             "AllocationMethod=%d, BitRate=%d, SamplingFreq=%d BitPool=%d",
280             __func__, p_encoder_params->s16ChannelMode,
281             p_encoder_params->s16NumOfSubBands,
282             p_encoder_params->s16NumOfBlocks,
283             p_encoder_params->s16AllocationMethod, p_encoder_params->u16BitRate,
284             s16SamplingFreq, p_encoder_params->s16BitPool);
285 
286   do {
287     if ((p_encoder_params->s16ChannelMode == SBC_JOINT_STEREO) ||
288         (p_encoder_params->s16ChannelMode == SBC_STEREO)) {
289       s16BitPool = (int16_t)((p_encoder_params->u16BitRate *
290                               p_encoder_params->s16NumOfSubBands * 1000 /
291                               s16SamplingFreq) -
292                              ((32 + (4 * p_encoder_params->s16NumOfSubBands *
293                                      p_encoder_params->s16NumOfChannels) +
294                                ((p_encoder_params->s16ChannelMode - 2) *
295                                 p_encoder_params->s16NumOfSubBands)) /
296                               p_encoder_params->s16NumOfBlocks));
297 
298       s16FrameLen = 4 +
299                     (4 * p_encoder_params->s16NumOfSubBands *
300                      p_encoder_params->s16NumOfChannels) /
301                         8 +
302                     (((p_encoder_params->s16ChannelMode - 2) *
303                       p_encoder_params->s16NumOfSubBands) +
304                      (p_encoder_params->s16NumOfBlocks * s16BitPool)) /
305                         8;
306 
307       s16BitRate = (8 * s16FrameLen * s16SamplingFreq) /
308                    (p_encoder_params->s16NumOfSubBands *
309                     p_encoder_params->s16NumOfBlocks * 1000);
310 
311       if (s16BitRate > p_encoder_params->u16BitRate) s16BitPool--;
312 
313       if (p_encoder_params->s16NumOfSubBands == 8)
314         s16BitPool = (s16BitPool > 255) ? 255 : s16BitPool;
315       else
316         s16BitPool = (s16BitPool > 128) ? 128 : s16BitPool;
317     } else {
318       s16BitPool =
319           (int16_t)(((p_encoder_params->s16NumOfSubBands *
320                       p_encoder_params->u16BitRate * 1000) /
321                      (s16SamplingFreq * p_encoder_params->s16NumOfChannels)) -
322                     (((32 / p_encoder_params->s16NumOfChannels) +
323                       (4 * p_encoder_params->s16NumOfSubBands)) /
324                      p_encoder_params->s16NumOfBlocks));
325 
326       p_encoder_params->s16BitPool =
327           (s16BitPool > (16 * p_encoder_params->s16NumOfSubBands))
328               ? (16 * p_encoder_params->s16NumOfSubBands)
329               : s16BitPool;
330     }
331 
332     if (s16BitPool < 0) s16BitPool = 0;
333 
334     LOG_DEBUG(LOG_TAG, "%s: bitpool candidate: %d (%d kbps)", __func__,
335               s16BitPool, p_encoder_params->u16BitRate);
336 
337     if (s16BitPool > max_bitpool) {
338       LOG_DEBUG(LOG_TAG, "%s: computed bitpool too large (%d)", __func__,
339                 s16BitPool);
340       /* Decrease bitrate */
341       p_encoder_params->u16BitRate -= A2DP_SBC_BITRATE_STEP;
342       /* Record that we have decreased the bitrate */
343       protect |= 1;
344     } else if (s16BitPool < min_bitpool) {
345       LOG_WARN(LOG_TAG, "%s: computed bitpool too small (%d)", __func__,
346                s16BitPool);
347 
348       /* Increase bitrate */
349       uint16_t previous_u16BitRate = p_encoder_params->u16BitRate;
350       p_encoder_params->u16BitRate += A2DP_SBC_BITRATE_STEP;
351       /* Record that we have increased the bitrate */
352       protect |= 2;
353       /* Check over-flow */
354       if (p_encoder_params->u16BitRate < previous_u16BitRate) protect |= 3;
355     } else {
356       break;
357     }
358     /* In case we have already increased and decreased the bitrate, just stop */
359     if (protect == 3) {
360       LOG_ERROR(LOG_TAG, "%s: could not find bitpool in range", __func__);
361       break;
362     }
363   } while (true);
364 
365   /* Finally update the bitpool in the encoder structure */
366   p_encoder_params->s16BitPool = s16BitPool;
367 
368   LOG_DEBUG(LOG_TAG, "%s: final bit rate %d, final bit pool %d", __func__,
369             p_encoder_params->u16BitRate, p_encoder_params->s16BitPool);
370 
371   /* Reset entirely the SBC encoder */
372   SBC_Encoder_Init(&a2dp_sbc_encoder_cb.sbc_encoder_params);
373   a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
374 }
375 
a2dp_sbc_encoder_cleanup(void)376 void a2dp_sbc_encoder_cleanup(void) {
377   memset(&a2dp_sbc_encoder_cb, 0, sizeof(a2dp_sbc_encoder_cb));
378 }
379 
a2dp_sbc_feeding_reset(void)380 void a2dp_sbc_feeding_reset(void) {
381   /* By default, just clear the entire state */
382   memset(&a2dp_sbc_encoder_cb.feeding_state, 0,
383          sizeof(a2dp_sbc_encoder_cb.feeding_state));
384 
385   a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick =
386       (a2dp_sbc_encoder_cb.feeding_params.sample_rate *
387        a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8 *
388        a2dp_sbc_encoder_cb.feeding_params.channel_count *
389        A2DP_SBC_ENCODER_INTERVAL_MS) /
390       1000;
391 
392   LOG_DEBUG(LOG_TAG, "%s: PCM bytes per tick %u", __func__,
393             a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick);
394 }
395 
a2dp_sbc_feeding_flush(void)396 void a2dp_sbc_feeding_flush(void) {
397   a2dp_sbc_encoder_cb.feeding_state.counter = 0;
398   a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
399 }
400 
a2dp_sbc_get_encoder_interval_ms(void)401 period_ms_t a2dp_sbc_get_encoder_interval_ms(void) {
402   return A2DP_SBC_ENCODER_INTERVAL_MS;
403 }
404 
a2dp_sbc_send_frames(uint64_t timestamp_us)405 void a2dp_sbc_send_frames(uint64_t timestamp_us) {
406   uint8_t nb_frame = 0;
407   uint8_t nb_iterations = 0;
408 
409   a2dp_sbc_get_num_frame_iteration(&nb_iterations, &nb_frame, timestamp_us);
410   LOG_VERBOSE(LOG_TAG, "%s: Sending %d frames per iteration, %d iterations",
411               __func__, nb_frame, nb_iterations);
412   if (nb_frame == 0) return;
413 
414   for (uint8_t counter = 0; counter < nb_iterations; counter++) {
415     // Transcode frame and enqueue
416     a2dp_sbc_encode_frames(nb_frame);
417   }
418 }
419 
420 // Obtains the number of frames to send and number of iterations
421 // to be used. |num_of_iterations| and |num_of_frames| parameters
422 // are used as output param for returning the respective values.
a2dp_sbc_get_num_frame_iteration(uint8_t * num_of_iterations,uint8_t * num_of_frames,uint64_t timestamp_us)423 static void a2dp_sbc_get_num_frame_iteration(uint8_t* num_of_iterations,
424                                              uint8_t* num_of_frames,
425                                              uint64_t timestamp_us) {
426   uint8_t nof = 0;
427   uint8_t noi = 1;
428 
429   uint32_t projected_nof = 0;
430   uint32_t pcm_bytes_per_frame =
431       a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfSubBands *
432       a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfBlocks *
433       a2dp_sbc_encoder_cb.feeding_params.channel_count *
434       a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
435   LOG_VERBOSE(LOG_TAG, "%s: pcm_bytes_per_frame %u", __func__,
436               pcm_bytes_per_frame);
437 
438   uint32_t us_this_tick = A2DP_SBC_ENCODER_INTERVAL_MS * 1000;
439   uint64_t now_us = timestamp_us;
440   if (a2dp_sbc_encoder_cb.feeding_state.last_frame_us != 0)
441     us_this_tick = (now_us - a2dp_sbc_encoder_cb.feeding_state.last_frame_us);
442   a2dp_sbc_encoder_cb.feeding_state.last_frame_us = now_us;
443 
444   a2dp_sbc_encoder_cb.feeding_state.counter +=
445       a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick * us_this_tick /
446       (A2DP_SBC_ENCODER_INTERVAL_MS * 1000);
447 
448   /* Calculate the number of frames pending for this media tick */
449   projected_nof =
450       a2dp_sbc_encoder_cb.feeding_state.counter / pcm_bytes_per_frame;
451   // Update the stats
452   a2dp_sbc_encoder_cb.stats.media_read_total_expected_frames += projected_nof;
453 
454   if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
455     LOG_WARN(LOG_TAG, "%s: limiting frames to be sent from %d to %d", __func__,
456              projected_nof, MAX_PCM_FRAME_NUM_PER_TICK);
457 
458     // Update the stats
459     size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
460     a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
461 
462     projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
463   }
464 
465   LOG_VERBOSE(LOG_TAG, "%s: frames for available PCM data %u", __func__,
466               projected_nof);
467 
468   if (a2dp_sbc_encoder_cb.is_peer_edr) {
469     if (!a2dp_sbc_encoder_cb.tx_sbc_frames) {
470       LOG_ERROR(LOG_TAG, "%s: tx_sbc_frames not updated, update from here",
471                 __func__);
472       a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
473     }
474 
475     nof = a2dp_sbc_encoder_cb.tx_sbc_frames;
476     if (!nof) {
477       LOG_ERROR(LOG_TAG,
478                 "%s: number of frames not updated, set calculated values",
479                 __func__);
480       nof = projected_nof;
481       noi = 1;
482     } else {
483       if (nof < projected_nof) {
484         noi = projected_nof / nof;  // number of iterations would vary
485         if (noi > A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK) {
486           LOG_ERROR(LOG_TAG, "%s: Audio Congestion (iterations:%d > max (%d))",
487                     __func__, noi, A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK);
488           noi = A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK;
489           a2dp_sbc_encoder_cb.feeding_state.counter =
490               noi * nof * pcm_bytes_per_frame;
491         }
492         projected_nof = nof;
493       } else {
494         noi = 1;  // number of iterations is 1
495         LOG_VERBOSE(LOG_TAG, "%s: reducing frames for available PCM data",
496                     __func__);
497         nof = projected_nof;
498       }
499     }
500   } else {
501     // For BR cases nof will be same as the value retrieved at projected_nof
502     LOG_VERBOSE(LOG_TAG, "%s: headset BR, number of frames %u", __func__, nof);
503     if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
504       LOG_ERROR(LOG_TAG, "%s: Audio Congestion (frames: %d > max (%d))",
505                 __func__, projected_nof, MAX_PCM_FRAME_NUM_PER_TICK);
506 
507       // Update the stats
508       size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
509       a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
510 
511       projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
512       a2dp_sbc_encoder_cb.feeding_state.counter =
513           noi * projected_nof * pcm_bytes_per_frame;
514     }
515     nof = projected_nof;
516   }
517   a2dp_sbc_encoder_cb.feeding_state.counter -= noi * nof * pcm_bytes_per_frame;
518   LOG_VERBOSE(LOG_TAG, "%s: effective num of frames %u, iterations %u",
519               __func__, nof, noi);
520 
521   *num_of_frames = nof;
522   *num_of_iterations = noi;
523 }
524 
a2dp_sbc_encode_frames(uint8_t nb_frame)525 static void a2dp_sbc_encode_frames(uint8_t nb_frame) {
526   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
527   uint8_t remain_nb_frame = nb_frame;
528   uint16_t blocm_x_subband =
529       p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
530 
531   uint8_t last_frame_len = 0;
532   while (nb_frame) {
533     BT_HDR* p_buf = (BT_HDR*)osi_malloc(A2DP_SBC_BUFFER_SIZE);
534     p_buf->offset = A2DP_SBC_OFFSET;
535     p_buf->len = 0;
536     p_buf->layer_specific = 0;
537     a2dp_sbc_encoder_cb.stats.media_read_total_expected_packets++;
538 
539     do {
540       /* Fill allocated buffer with 0 */
541       memset(a2dp_sbc_encoder_cb.pcmBuffer, 0,
542              blocm_x_subband * p_encoder_params->s16NumOfChannels);
543 
544       //
545       // Read the PCM data and encode it. If necessary, upsample the data.
546       //
547       if (a2dp_sbc_read_feeding()) {
548         uint8_t* output = (uint8_t*)(p_buf + 1) + p_buf->offset + p_buf->len;
549         int16_t* input = a2dp_sbc_encoder_cb.pcmBuffer;
550         uint16_t output_len = SBC_Encode(p_encoder_params, input, output);
551         last_frame_len = output_len;
552 
553         /* Update SBC frame length */
554         p_buf->len += output_len;
555         nb_frame--;
556         p_buf->layer_specific++;
557       } else {
558         LOG_WARN(LOG_TAG, "%s: underflow %d, %d", __func__, nb_frame,
559                  a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
560         a2dp_sbc_encoder_cb.feeding_state.counter +=
561             nb_frame * p_encoder_params->s16NumOfSubBands *
562             p_encoder_params->s16NumOfBlocks *
563             a2dp_sbc_encoder_cb.feeding_params.channel_count *
564             a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
565         /* no more pcm to read */
566         nb_frame = 0;
567       }
568     } while (
569         ((p_buf->len + last_frame_len) < a2dp_sbc_encoder_cb.TxAaMtuSize) &&
570         (p_buf->layer_specific < 0x0F) && nb_frame);
571 
572     if (p_buf->len) {
573       /*
574        * Timestamp of the media packet header represent the TS of the
575        * first SBC frame, i.e the timestamp before including this frame.
576        */
577       *((uint32_t*)(p_buf + 1)) = a2dp_sbc_encoder_cb.timestamp;
578 
579       a2dp_sbc_encoder_cb.timestamp += p_buf->layer_specific * blocm_x_subband;
580 
581       uint8_t done_nb_frame = remain_nb_frame - nb_frame;
582       remain_nb_frame = nb_frame;
583       if (!a2dp_sbc_encoder_cb.enqueue_callback(p_buf, done_nb_frame)) return;
584     } else {
585       a2dp_sbc_encoder_cb.stats.media_read_total_dropped_packets++;
586       osi_free(p_buf);
587     }
588   }
589 }
590 
a2dp_sbc_read_feeding(void)591 static bool a2dp_sbc_read_feeding(void) {
592   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
593   uint16_t blocm_x_subband =
594       p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
595   uint32_t read_size;
596   uint32_t sbc_sampling = 48000;
597   uint32_t src_samples;
598   uint16_t bytes_needed = blocm_x_subband * p_encoder_params->s16NumOfChannels *
599                           a2dp_sbc_encoder_cb.feeding_params.bits_per_sample /
600                           8;
601   static uint16_t up_sampled_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
602                                     SBC_MAX_NUM_OF_CHANNELS *
603                                     SBC_MAX_NUM_OF_SUBBANDS * 2];
604   static uint16_t read_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
605                               SBC_MAX_NUM_OF_CHANNELS *
606                               SBC_MAX_NUM_OF_SUBBANDS];
607   uint32_t src_size_used;
608   uint32_t dst_size_used;
609   bool fract_needed;
610   int32_t fract_max;
611   int32_t fract_threshold;
612   uint32_t nb_byte_read;
613 
614   /* Get the SBC sampling rate */
615   switch (p_encoder_params->s16SamplingFreq) {
616     case SBC_sf48000:
617       sbc_sampling = 48000;
618       break;
619     case SBC_sf44100:
620       sbc_sampling = 44100;
621       break;
622     case SBC_sf32000:
623       sbc_sampling = 32000;
624       break;
625     case SBC_sf16000:
626       sbc_sampling = 16000;
627       break;
628   }
629 
630   a2dp_sbc_encoder_cb.stats.media_read_total_expected_reads_count++;
631   if (sbc_sampling == a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
632     read_size =
633         bytes_needed - a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue;
634     a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
635     nb_byte_read = a2dp_sbc_encoder_cb.read_callback(
636         ((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer) +
637             a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
638         read_size);
639     a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes +=
640         nb_byte_read;
641 
642     if (nb_byte_read != read_size) {
643       a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += nb_byte_read;
644       return false;
645     }
646     a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
647     a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
648     return true;
649   }
650 
651   /*
652    * Some Feeding PCM frequencies require to split the number of sample
653    * to read.
654    * E.g 128 / 6 = 21.3333 => read 22 and 21 and 21 => max = 2; threshold = 0
655    */
656   fract_needed = false; /* Default */
657   switch (a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
658     case 32000:
659     case 8000:
660       fract_needed = true;
661       fract_max = 2;       /* 0, 1 and 2 */
662       fract_threshold = 0; /* Add one for the first */
663       break;
664     case 16000:
665       fract_needed = true;
666       fract_max = 2;       /* 0, 1 and 2 */
667       fract_threshold = 1; /* Add one for the first two frames*/
668       break;
669   }
670 
671   /* Compute number of sample to read from source */
672   src_samples = blocm_x_subband;
673   src_samples *= a2dp_sbc_encoder_cb.feeding_params.sample_rate;
674   src_samples /= sbc_sampling;
675 
676   /* The previous division may have a remainder not null */
677   if (fract_needed) {
678     if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter <= fract_threshold) {
679       src_samples++; /* for every read before threshold add one sample */
680     }
681 
682     /* do nothing if counter >= threshold */
683     a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter++; /* one more read */
684     if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter > fract_max) {
685       a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter = 0;
686     }
687   }
688 
689   /* Compute number of bytes to read from source */
690   read_size = src_samples;
691   read_size *= a2dp_sbc_encoder_cb.feeding_params.channel_count;
692   read_size *= (a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8);
693   a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
694 
695   /* Read Data from UIPC channel */
696   nb_byte_read =
697       a2dp_sbc_encoder_cb.read_callback((uint8_t*)read_buffer, read_size);
698   a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes += nb_byte_read;
699 
700   if (nb_byte_read < read_size) {
701     if (nb_byte_read == 0) return false;
702 
703     /* Fill the unfilled part of the read buffer with silence (0) */
704     memset(((uint8_t*)read_buffer) + nb_byte_read, 0, read_size - nb_byte_read);
705     nb_byte_read = read_size;
706   }
707   a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
708 
709   /* Initialize PCM up-sampling engine */
710   a2dp_sbc_init_up_sample(a2dp_sbc_encoder_cb.feeding_params.sample_rate,
711                           sbc_sampling,
712                           a2dp_sbc_encoder_cb.feeding_params.bits_per_sample,
713                           a2dp_sbc_encoder_cb.feeding_params.channel_count);
714 
715   /*
716    * Re-sample the read buffer.
717    * The output PCM buffer will be stereo, 16 bit per sample.
718    */
719   dst_size_used = a2dp_sbc_up_sample(
720       (uint8_t*)read_buffer,
721       (uint8_t*)up_sampled_buffer +
722           a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
723       nb_byte_read, sizeof(up_sampled_buffer) -
724                         a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
725       &src_size_used);
726 
727   /* update the residue */
728   a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += dst_size_used;
729 
730   /* only copy the pcm sample when we have up-sampled enough PCM */
731   if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue < bytes_needed)
732     return false;
733 
734   /* Copy the output pcm samples in SBC encoding buffer */
735   memcpy((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer, (uint8_t*)up_sampled_buffer,
736          bytes_needed);
737   /* update the residue */
738   a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue -= bytes_needed;
739 
740   if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue != 0) {
741     memcpy((uint8_t*)up_sampled_buffer,
742            (uint8_t*)up_sampled_buffer + bytes_needed,
743            a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
744   }
745   return true;
746 }
747 
calculate_max_frames_per_packet(void)748 static uint8_t calculate_max_frames_per_packet(void) {
749   uint16_t effective_mtu_size = a2dp_sbc_encoder_cb.TxAaMtuSize;
750   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
751   uint16_t result = 0;
752   uint32_t frame_len;
753 
754   LOG_VERBOSE(LOG_TAG, "%s: original AVDTP MTU size: %d", __func__,
755               a2dp_sbc_encoder_cb.TxAaMtuSize);
756 
757   // Restrict the MTU - even though some Sink devices are advertising large
758   // MTU, they are not able to handle the packets and are stuttering.
759   if (effective_mtu_size > MAX_2MBPS_AVDTP_MTU) {
760     LOG_WARN(LOG_TAG, "%s: Restricting AVDTP MTU size to %d", __func__,
761              MAX_2MBPS_AVDTP_MTU);
762     effective_mtu_size = MAX_2MBPS_AVDTP_MTU;
763     a2dp_sbc_encoder_cb.TxAaMtuSize = effective_mtu_size;
764   }
765 
766   if (!p_encoder_params->s16NumOfSubBands) {
767     LOG_ERROR(LOG_TAG, "%s: SubBands are set to 0, resetting to %d", __func__,
768               SBC_MAX_NUM_OF_SUBBANDS);
769     p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
770   }
771   if (!p_encoder_params->s16NumOfBlocks) {
772     LOG_ERROR(LOG_TAG, "%s: Blocks are set to 0, resetting to %d", __func__,
773               SBC_MAX_NUM_OF_BLOCKS);
774     p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
775   }
776   if (!p_encoder_params->s16NumOfChannels) {
777     LOG_ERROR(LOG_TAG, "%s: Channels are set to 0, resetting to %d", __func__,
778               SBC_MAX_NUM_OF_CHANNELS);
779     p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
780   }
781 
782   frame_len = a2dp_sbc_frame_length();
783 
784   LOG_VERBOSE(LOG_TAG, "%s: Effective Tx MTU to be considered: %d", __func__,
785               effective_mtu_size);
786 
787   switch (p_encoder_params->s16SamplingFreq) {
788     case SBC_sf44100:
789       if (frame_len == 0) {
790         LOG_ERROR(LOG_TAG,
791                   "%s: Calculating frame length, resetting it to default %d",
792                   __func__, A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1);
793         frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1;
794       }
795       result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
796       LOG_VERBOSE(LOG_TAG, "%s: Max number of SBC frames: %d", __func__,
797                   result);
798       break;
799 
800     case SBC_sf48000:
801       if (frame_len == 0) {
802         LOG_ERROR(LOG_TAG,
803                   "%s: Calculating frame length, resetting it to default %d",
804                   __func__, A2DP_SBC_MAX_HQ_FRAME_SIZE_48);
805         frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_48;
806       }
807       result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
808       LOG_VERBOSE(LOG_TAG, "%s: Max number of SBC frames: %d", __func__,
809                   result);
810       break;
811 
812     default:
813       LOG_ERROR(LOG_TAG, "%s: Max number of SBC frames: %d", __func__, result);
814       break;
815   }
816   return result;
817 }
818 
a2dp_sbc_source_rate(void)819 static uint16_t a2dp_sbc_source_rate(void) {
820   uint16_t rate = A2DP_SBC_DEFAULT_BITRATE;
821 
822   /* restrict bitrate if a2dp link is non-edr */
823   if (!a2dp_sbc_encoder_cb.is_peer_edr) {
824     rate = A2DP_SBC_NON_EDR_MAX_RATE;
825     LOG_VERBOSE(LOG_TAG, "%s: non-edr a2dp sink detected, restrict rate to %d",
826                 __func__, rate);
827   }
828 
829   return rate;
830 }
831 
a2dp_sbc_frame_length(void)832 static uint32_t a2dp_sbc_frame_length(void) {
833   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
834   uint32_t frame_len = 0;
835 
836   LOG_VERBOSE(LOG_TAG,
837               "%s: channel mode: %d, sub-band: %d, number of block: %d, "
838               "bitpool: %d, sampling frequency: %d, num channels: %d",
839               __func__, p_encoder_params->s16ChannelMode,
840               p_encoder_params->s16NumOfSubBands,
841               p_encoder_params->s16NumOfBlocks, p_encoder_params->s16BitPool,
842               p_encoder_params->s16SamplingFreq,
843               p_encoder_params->s16NumOfChannels);
844 
845   switch (p_encoder_params->s16ChannelMode) {
846     case SBC_MONO:
847     /* FALLTHROUGH */
848     case SBC_DUAL:
849       frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
850                   ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
851                               p_encoder_params->s16NumOfSubBands *
852                               p_encoder_params->s16NumOfChannels) /
853                    CHAR_BIT) +
854                   ((uint32_t)(p_encoder_params->s16NumOfBlocks *
855                               p_encoder_params->s16NumOfChannels *
856                               p_encoder_params->s16BitPool) /
857                    CHAR_BIT);
858       break;
859     case SBC_STEREO:
860       frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
861                   ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
862                               p_encoder_params->s16NumOfSubBands *
863                               p_encoder_params->s16NumOfChannels) /
864                    CHAR_BIT) +
865                   ((uint32_t)(p_encoder_params->s16NumOfBlocks *
866                               p_encoder_params->s16BitPool) /
867                    CHAR_BIT);
868       break;
869     case SBC_JOINT_STEREO:
870       frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
871                   ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
872                               p_encoder_params->s16NumOfSubBands *
873                               p_encoder_params->s16NumOfChannels) /
874                    CHAR_BIT) +
875                   ((uint32_t)(p_encoder_params->s16NumOfSubBands +
876                               (p_encoder_params->s16NumOfBlocks *
877                                p_encoder_params->s16BitPool)) /
878                    CHAR_BIT);
879       break;
880     default:
881       LOG_VERBOSE(LOG_TAG, "%s: Invalid channel number: %d", __func__,
882                   p_encoder_params->s16ChannelMode);
883       break;
884   }
885   LOG_VERBOSE(LOG_TAG, "%s: calculated frame length: %d", __func__, frame_len);
886   return frame_len;
887 }
888 
encoderIntervalMs() const889 period_ms_t A2dpCodecConfigSbc::encoderIntervalMs() const {
890   return a2dp_sbc_get_encoder_interval_ms();
891 }
892 
debug_codec_dump(int fd)893 void A2dpCodecConfigSbc::debug_codec_dump(int fd) {
894   a2dp_sbc_encoder_stats_t* stats = &a2dp_sbc_encoder_cb.stats;
895 
896   A2dpCodecConfig::debug_codec_dump(fd);
897 
898   dprintf(fd,
899           "  Packet counts (expected/dropped)                        : %zu / "
900           "%zu\n",
901           stats->media_read_total_expected_packets,
902           stats->media_read_total_dropped_packets);
903 
904   dprintf(fd,
905           "  PCM read counts (expected/actual)                       : %zu / "
906           "%zu\n",
907           stats->media_read_total_expected_reads_count,
908           stats->media_read_total_actual_reads_count);
909 
910   dprintf(fd,
911           "  PCM read bytes (expected/actual)                        : %zu / "
912           "%zu\n",
913           stats->media_read_total_expected_read_bytes,
914           stats->media_read_total_actual_read_bytes);
915 
916   dprintf(fd,
917           "  Frames counts (expected/dropped)                        : %zu / "
918           "%zu\n",
919           stats->media_read_total_expected_frames,
920           stats->media_read_total_dropped_frames);
921 }
922