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