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 "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(uint32_t* bytes);
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();
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 A2dpCodecConfigSbcSource::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_codec_config->getAudioBitsPerSample();
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   a2dp_sbc_feeding_reset();
226 
227   // The codec parameters
228   p_encoder_params->s16ChannelMode = A2DP_GetChannelModeCodeSbc(p_codec_info);
229   p_encoder_params->s16NumOfSubBands =
230       A2DP_GetNumberOfSubbandsSbc(p_codec_info);
231   p_encoder_params->s16NumOfBlocks = A2DP_GetNumberOfBlocksSbc(p_codec_info);
232   p_encoder_params->s16AllocationMethod =
233       A2DP_GetAllocationMethodCodeSbc(p_codec_info);
234   p_encoder_params->s16SamplingFreq =
235       A2DP_GetSamplingFrequencyCodeSbc(p_codec_info);
236   p_encoder_params->s16NumOfChannels =
237       A2DP_GetTrackChannelCountSbc(p_codec_info);
238 
239   // Reset invalid parameters
240   if (!p_encoder_params->s16NumOfSubBands) {
241     LOG_WARN(LOG_TAG, "%s: SubBands are set to 0, resetting to max (%d)",
242              __func__, SBC_MAX_NUM_OF_SUBBANDS);
243     p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
244   }
245   if (!p_encoder_params->s16NumOfBlocks) {
246     LOG_WARN(LOG_TAG, "%s: Blocks are set to 0, resetting to max (%d)",
247              __func__, SBC_MAX_NUM_OF_BLOCKS);
248     p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
249   }
250   if (!p_encoder_params->s16NumOfChannels) {
251     LOG_WARN(LOG_TAG, "%s: Channels are set to 0, resetting to max (%d)",
252              __func__, SBC_MAX_NUM_OF_CHANNELS);
253     p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
254   }
255 
256   uint16_t mtu_size = A2DP_SBC_BUFFER_SIZE - A2DP_SBC_OFFSET - sizeof(BT_HDR);
257   if (mtu_size < peer_mtu) {
258     a2dp_sbc_encoder_cb.TxAaMtuSize = mtu_size;
259   } else {
260     a2dp_sbc_encoder_cb.TxAaMtuSize = peer_mtu;
261   }
262 
263   if (p_encoder_params->s16SamplingFreq == SBC_sf16000)
264     s16SamplingFreq = 16000;
265   else if (p_encoder_params->s16SamplingFreq == SBC_sf32000)
266     s16SamplingFreq = 32000;
267   else if (p_encoder_params->s16SamplingFreq == SBC_sf44100)
268     s16SamplingFreq = 44100;
269   else
270     s16SamplingFreq = 48000;
271 
272   // Set the initial target bit rate
273   p_encoder_params->u16BitRate = a2dp_sbc_source_rate();
274 
275   LOG_DEBUG(LOG_TAG, "%s: MTU=%d, peer_mtu=%d min_bitpool=%d max_bitpool=%d",
276             __func__, a2dp_sbc_encoder_cb.TxAaMtuSize, peer_mtu, min_bitpool,
277             max_bitpool);
278   LOG_DEBUG(LOG_TAG,
279             "%s: ChannelMode=%d, NumOfSubBands=%d, NumOfBlocks=%d, "
280             "AllocationMethod=%d, BitRate=%d, SamplingFreq=%d BitPool=%d",
281             __func__, p_encoder_params->s16ChannelMode,
282             p_encoder_params->s16NumOfSubBands,
283             p_encoder_params->s16NumOfBlocks,
284             p_encoder_params->s16AllocationMethod, p_encoder_params->u16BitRate,
285             s16SamplingFreq, p_encoder_params->s16BitPool);
286 
287   do {
288     if ((p_encoder_params->s16ChannelMode == SBC_JOINT_STEREO) ||
289         (p_encoder_params->s16ChannelMode == SBC_STEREO)) {
290       s16BitPool = (int16_t)((p_encoder_params->u16BitRate *
291                               p_encoder_params->s16NumOfSubBands * 1000 /
292                               s16SamplingFreq) -
293                              ((32 + (4 * p_encoder_params->s16NumOfSubBands *
294                                      p_encoder_params->s16NumOfChannels) +
295                                ((p_encoder_params->s16ChannelMode - 2) *
296                                 p_encoder_params->s16NumOfSubBands)) /
297                               p_encoder_params->s16NumOfBlocks));
298 
299       s16FrameLen = 4 +
300                     (4 * p_encoder_params->s16NumOfSubBands *
301                      p_encoder_params->s16NumOfChannels) /
302                         8 +
303                     (((p_encoder_params->s16ChannelMode - 2) *
304                       p_encoder_params->s16NumOfSubBands) +
305                      (p_encoder_params->s16NumOfBlocks * s16BitPool)) /
306                         8;
307 
308       s16BitRate = (8 * s16FrameLen * s16SamplingFreq) /
309                    (p_encoder_params->s16NumOfSubBands *
310                     p_encoder_params->s16NumOfBlocks * 1000);
311 
312       if (s16BitRate > p_encoder_params->u16BitRate) s16BitPool--;
313 
314       if (p_encoder_params->s16NumOfSubBands == 8)
315         s16BitPool = (s16BitPool > 255) ? 255 : s16BitPool;
316       else
317         s16BitPool = (s16BitPool > 128) ? 128 : s16BitPool;
318     } else {
319       s16BitPool =
320           (int16_t)(((p_encoder_params->s16NumOfSubBands *
321                       p_encoder_params->u16BitRate * 1000) /
322                      (s16SamplingFreq * p_encoder_params->s16NumOfChannels)) -
323                     (((32 / p_encoder_params->s16NumOfChannels) +
324                       (4 * p_encoder_params->s16NumOfSubBands)) /
325                      p_encoder_params->s16NumOfBlocks));
326 
327       p_encoder_params->s16BitPool =
328           (s16BitPool > (16 * p_encoder_params->s16NumOfSubBands))
329               ? (16 * p_encoder_params->s16NumOfSubBands)
330               : s16BitPool;
331     }
332 
333     if (s16BitPool < 0) s16BitPool = 0;
334 
335     LOG_DEBUG(LOG_TAG, "%s: bitpool candidate: %d (%d kbps)", __func__,
336               s16BitPool, p_encoder_params->u16BitRate);
337 
338     if (s16BitPool > max_bitpool) {
339       LOG_DEBUG(LOG_TAG, "%s: computed bitpool too large (%d)", __func__,
340                 s16BitPool);
341       /* Decrease bitrate */
342       p_encoder_params->u16BitRate -= A2DP_SBC_BITRATE_STEP;
343       /* Record that we have decreased the bitrate */
344       protect |= 1;
345     } else if (s16BitPool < min_bitpool) {
346       LOG_WARN(LOG_TAG, "%s: computed bitpool too small (%d)", __func__,
347                s16BitPool);
348 
349       /* Increase bitrate */
350       uint16_t previous_u16BitRate = p_encoder_params->u16BitRate;
351       p_encoder_params->u16BitRate += A2DP_SBC_BITRATE_STEP;
352       /* Record that we have increased the bitrate */
353       protect |= 2;
354       /* Check over-flow */
355       if (p_encoder_params->u16BitRate < previous_u16BitRate) protect |= 3;
356     } else {
357       break;
358     }
359     /* In case we have already increased and decreased the bitrate, just stop */
360     if (protect == 3) {
361       LOG_ERROR(LOG_TAG, "%s: could not find bitpool in range", __func__);
362       break;
363     }
364   } while (true);
365 
366   /* Finally update the bitpool in the encoder structure */
367   p_encoder_params->s16BitPool = s16BitPool;
368 
369   LOG_DEBUG(LOG_TAG, "%s: final bit rate %d, final bit pool %d", __func__,
370             p_encoder_params->u16BitRate, p_encoder_params->s16BitPool);
371 
372   /* Reset the SBC encoder */
373   SBC_Encoder_Init(&a2dp_sbc_encoder_cb.sbc_encoder_params);
374   a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
375 }
376 
a2dp_sbc_encoder_cleanup(void)377 void a2dp_sbc_encoder_cleanup(void) {
378   memset(&a2dp_sbc_encoder_cb, 0, sizeof(a2dp_sbc_encoder_cb));
379 }
380 
a2dp_sbc_feeding_reset(void)381 void a2dp_sbc_feeding_reset(void) {
382   /* By default, just clear the entire state */
383   memset(&a2dp_sbc_encoder_cb.feeding_state, 0,
384          sizeof(a2dp_sbc_encoder_cb.feeding_state));
385 
386   a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick =
387       (a2dp_sbc_encoder_cb.feeding_params.sample_rate *
388        a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8 *
389        a2dp_sbc_encoder_cb.feeding_params.channel_count *
390        A2DP_SBC_ENCODER_INTERVAL_MS) /
391       1000;
392 
393   LOG_DEBUG(LOG_TAG, "%s: PCM bytes per tick %u", __func__,
394             a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick);
395 }
396 
a2dp_sbc_feeding_flush(void)397 void a2dp_sbc_feeding_flush(void) {
398   a2dp_sbc_encoder_cb.feeding_state.counter = 0;
399   a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
400 }
401 
a2dp_sbc_get_encoder_interval_ms(void)402 period_ms_t a2dp_sbc_get_encoder_interval_ms(void) {
403   return A2DP_SBC_ENCODER_INTERVAL_MS;
404 }
405 
a2dp_sbc_send_frames(uint64_t timestamp_us)406 void a2dp_sbc_send_frames(uint64_t timestamp_us) {
407   uint8_t nb_frame = 0;
408   uint8_t nb_iterations = 0;
409 
410   a2dp_sbc_get_num_frame_iteration(&nb_iterations, &nb_frame, timestamp_us);
411   LOG_VERBOSE(LOG_TAG, "%s: Sending %d frames per iteration, %d iterations",
412               __func__, nb_frame, nb_iterations);
413   if (nb_frame == 0) return;
414 
415   for (uint8_t counter = 0; counter < nb_iterations; counter++) {
416     // Transcode frame and enqueue
417     a2dp_sbc_encode_frames(nb_frame);
418   }
419 }
420 
421 // Obtains the number of frames to send and number of iterations
422 // to be used. |num_of_iterations| and |num_of_frames| parameters
423 // 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)424 static void a2dp_sbc_get_num_frame_iteration(uint8_t* num_of_iterations,
425                                              uint8_t* num_of_frames,
426                                              uint64_t timestamp_us) {
427   uint8_t nof = 0;
428   uint8_t noi = 1;
429 
430   uint32_t projected_nof = 0;
431   uint32_t pcm_bytes_per_frame =
432       a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfSubBands *
433       a2dp_sbc_encoder_cb.sbc_encoder_params.s16NumOfBlocks *
434       a2dp_sbc_encoder_cb.feeding_params.channel_count *
435       a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
436   LOG_VERBOSE(LOG_TAG, "%s: pcm_bytes_per_frame %u", __func__,
437               pcm_bytes_per_frame);
438 
439   uint32_t us_this_tick = A2DP_SBC_ENCODER_INTERVAL_MS * 1000;
440   uint64_t now_us = timestamp_us;
441   if (a2dp_sbc_encoder_cb.feeding_state.last_frame_us != 0)
442     us_this_tick = (now_us - a2dp_sbc_encoder_cb.feeding_state.last_frame_us);
443   a2dp_sbc_encoder_cb.feeding_state.last_frame_us = now_us;
444 
445   a2dp_sbc_encoder_cb.feeding_state.counter +=
446       a2dp_sbc_encoder_cb.feeding_state.bytes_per_tick * us_this_tick /
447       (A2DP_SBC_ENCODER_INTERVAL_MS * 1000);
448 
449   /* Calculate the number of frames pending for this media tick */
450   projected_nof =
451       a2dp_sbc_encoder_cb.feeding_state.counter / pcm_bytes_per_frame;
452   // Update the stats
453   a2dp_sbc_encoder_cb.stats.media_read_total_expected_frames += projected_nof;
454 
455   if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
456     LOG_WARN(LOG_TAG, "%s: limiting frames to be sent from %d to %d", __func__,
457              projected_nof, MAX_PCM_FRAME_NUM_PER_TICK);
458 
459     // Update the stats
460     size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
461     a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
462 
463     projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
464   }
465 
466   LOG_VERBOSE(LOG_TAG, "%s: frames for available PCM data %u", __func__,
467               projected_nof);
468 
469   if (a2dp_sbc_encoder_cb.is_peer_edr) {
470     if (!a2dp_sbc_encoder_cb.tx_sbc_frames) {
471       LOG_ERROR(LOG_TAG, "%s: tx_sbc_frames not updated, update from here",
472                 __func__);
473       a2dp_sbc_encoder_cb.tx_sbc_frames = calculate_max_frames_per_packet();
474     }
475 
476     nof = a2dp_sbc_encoder_cb.tx_sbc_frames;
477     if (!nof) {
478       LOG_ERROR(LOG_TAG,
479                 "%s: number of frames not updated, set calculated values",
480                 __func__);
481       nof = projected_nof;
482       noi = 1;
483     } else {
484       if (nof < projected_nof) {
485         noi = projected_nof / nof;  // number of iterations would vary
486         if (noi > A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK) {
487           LOG_ERROR(LOG_TAG, "%s: Audio Congestion (iterations:%d > max (%d))",
488                     __func__, noi, A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK);
489           noi = A2DP_SBC_MAX_PCM_ITER_NUM_PER_TICK;
490           a2dp_sbc_encoder_cb.feeding_state.counter =
491               noi * nof * pcm_bytes_per_frame;
492         }
493         projected_nof = nof;
494       } else {
495         noi = 1;  // number of iterations is 1
496         LOG_VERBOSE(LOG_TAG, "%s: reducing frames for available PCM data",
497                     __func__);
498         nof = projected_nof;
499       }
500     }
501   } else {
502     // For BR cases nof will be same as the value retrieved at projected_nof
503     LOG_VERBOSE(LOG_TAG, "%s: headset BR, number of frames %u", __func__, nof);
504     if (projected_nof > MAX_PCM_FRAME_NUM_PER_TICK) {
505       LOG_ERROR(LOG_TAG, "%s: Audio Congestion (frames: %d > max (%d))",
506                 __func__, projected_nof, MAX_PCM_FRAME_NUM_PER_TICK);
507 
508       // Update the stats
509       size_t delta = projected_nof - MAX_PCM_FRAME_NUM_PER_TICK;
510       a2dp_sbc_encoder_cb.stats.media_read_total_dropped_frames += delta;
511 
512       projected_nof = MAX_PCM_FRAME_NUM_PER_TICK;
513       a2dp_sbc_encoder_cb.feeding_state.counter =
514           noi * projected_nof * pcm_bytes_per_frame;
515     }
516     nof = projected_nof;
517   }
518   a2dp_sbc_encoder_cb.feeding_state.counter -= noi * nof * pcm_bytes_per_frame;
519   LOG_VERBOSE(LOG_TAG, "%s: effective num of frames %u, iterations %u",
520               __func__, nof, noi);
521 
522   *num_of_frames = nof;
523   *num_of_iterations = noi;
524 }
525 
a2dp_sbc_encode_frames(uint8_t nb_frame)526 static void a2dp_sbc_encode_frames(uint8_t nb_frame) {
527   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
528   uint8_t remain_nb_frame = nb_frame;
529   uint16_t blocm_x_subband =
530       p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
531 
532   uint8_t last_frame_len = 0;
533 
534   while (nb_frame) {
535     BT_HDR* p_buf = (BT_HDR*)osi_malloc(A2DP_SBC_BUFFER_SIZE);
536     uint32_t bytes_read = 0;
537 
538     p_buf->offset = A2DP_SBC_OFFSET;
539     p_buf->len = 0;
540     p_buf->layer_specific = 0;
541     a2dp_sbc_encoder_cb.stats.media_read_total_expected_packets++;
542 
543     do {
544       /* Fill allocated buffer with 0 */
545       memset(a2dp_sbc_encoder_cb.pcmBuffer, 0,
546              blocm_x_subband * p_encoder_params->s16NumOfChannels);
547       //
548       // Read the PCM data and encode it. If necessary, upsample the data.
549       //
550       uint32_t num_bytes = 0;
551       if (a2dp_sbc_read_feeding(&num_bytes)) {
552         uint8_t* output = (uint8_t*)(p_buf + 1) + p_buf->offset + p_buf->len;
553         int16_t* input = a2dp_sbc_encoder_cb.pcmBuffer;
554         uint16_t output_len = SBC_Encode(p_encoder_params, input, output);
555         last_frame_len = output_len;
556 
557         /* Update SBC frame length */
558         p_buf->len += output_len;
559         nb_frame--;
560         p_buf->layer_specific++;
561 
562         bytes_read += num_bytes;
563       } else {
564         LOG_WARN(LOG_TAG, "%s: underflow %d, %d", __func__, nb_frame,
565                  a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
566         a2dp_sbc_encoder_cb.feeding_state.counter +=
567             nb_frame * p_encoder_params->s16NumOfSubBands *
568             p_encoder_params->s16NumOfBlocks *
569             a2dp_sbc_encoder_cb.feeding_params.channel_count *
570             a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8;
571         /* no more pcm to read */
572         nb_frame = 0;
573       }
574     } while (
575         ((p_buf->len + last_frame_len) < a2dp_sbc_encoder_cb.TxAaMtuSize) &&
576         (p_buf->layer_specific < 0x0F) && nb_frame);
577 
578     if (p_buf->len) {
579       /*
580        * Timestamp of the media packet header represent the TS of the
581        * first SBC frame, i.e the timestamp before including this frame.
582        */
583       *((uint32_t*)(p_buf + 1)) = a2dp_sbc_encoder_cb.timestamp;
584 
585       a2dp_sbc_encoder_cb.timestamp += p_buf->layer_specific * blocm_x_subband;
586 
587       uint8_t done_nb_frame = remain_nb_frame - nb_frame;
588       remain_nb_frame = nb_frame;
589       if (!a2dp_sbc_encoder_cb.enqueue_callback(p_buf, done_nb_frame,
590                                                 bytes_read))
591         return;
592     } else {
593       a2dp_sbc_encoder_cb.stats.media_read_total_dropped_packets++;
594       osi_free(p_buf);
595     }
596   }
597 }
598 
a2dp_sbc_read_feeding(uint32_t * bytes_read)599 static bool a2dp_sbc_read_feeding(uint32_t* bytes_read) {
600   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
601   uint16_t blocm_x_subband =
602       p_encoder_params->s16NumOfSubBands * p_encoder_params->s16NumOfBlocks;
603   uint32_t read_size;
604   uint32_t sbc_sampling = 48000;
605   uint32_t src_samples;
606   uint16_t bytes_needed = blocm_x_subband * p_encoder_params->s16NumOfChannels *
607                           a2dp_sbc_encoder_cb.feeding_params.bits_per_sample /
608                           8;
609   static uint16_t up_sampled_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
610                                     SBC_MAX_NUM_OF_CHANNELS *
611                                     SBC_MAX_NUM_OF_SUBBANDS * 2];
612   static uint16_t read_buffer[SBC_MAX_NUM_FRAME * SBC_MAX_NUM_OF_BLOCKS *
613                               SBC_MAX_NUM_OF_CHANNELS *
614                               SBC_MAX_NUM_OF_SUBBANDS];
615   uint32_t src_size_used;
616   uint32_t dst_size_used;
617   bool fract_needed;
618   int32_t fract_max;
619   int32_t fract_threshold;
620   uint32_t nb_byte_read;
621 
622   /* Get the SBC sampling rate */
623   switch (p_encoder_params->s16SamplingFreq) {
624     case SBC_sf48000:
625       sbc_sampling = 48000;
626       break;
627     case SBC_sf44100:
628       sbc_sampling = 44100;
629       break;
630     case SBC_sf32000:
631       sbc_sampling = 32000;
632       break;
633     case SBC_sf16000:
634       sbc_sampling = 16000;
635       break;
636   }
637 
638   a2dp_sbc_encoder_cb.stats.media_read_total_expected_reads_count++;
639   if (sbc_sampling == a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
640     read_size =
641         bytes_needed - a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue;
642     a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
643     nb_byte_read = a2dp_sbc_encoder_cb.read_callback(
644         ((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer) +
645             a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
646         read_size);
647     a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes +=
648         nb_byte_read;
649 
650     *bytes_read = nb_byte_read;
651     if (nb_byte_read != read_size) {
652       a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += nb_byte_read;
653       return false;
654     }
655     a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
656     a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue = 0;
657     return true;
658   }
659 
660   /*
661    * Some Feeding PCM frequencies require to split the number of sample
662    * to read.
663    * E.g 128 / 6 = 21.3333 => read 22 and 21 and 21 => max = 2; threshold = 0
664    */
665   fract_needed = false; /* Default */
666   switch (a2dp_sbc_encoder_cb.feeding_params.sample_rate) {
667     case 32000:
668     case 8000:
669       fract_needed = true;
670       fract_max = 2;       /* 0, 1 and 2 */
671       fract_threshold = 0; /* Add one for the first */
672       break;
673     case 16000:
674       fract_needed = true;
675       fract_max = 2;       /* 0, 1 and 2 */
676       fract_threshold = 1; /* Add one for the first two frames*/
677       break;
678   }
679 
680   /* Compute number of sample to read from source */
681   src_samples = blocm_x_subband;
682   src_samples *= a2dp_sbc_encoder_cb.feeding_params.sample_rate;
683   src_samples /= sbc_sampling;
684 
685   /* The previous division may have a remainder not null */
686   if (fract_needed) {
687     if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter <= fract_threshold) {
688       src_samples++; /* for every read before threshold add one sample */
689     }
690 
691     /* do nothing if counter >= threshold */
692     a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter++; /* one more read */
693     if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter > fract_max) {
694       a2dp_sbc_encoder_cb.feeding_state.aa_feed_counter = 0;
695     }
696   }
697 
698   /* Compute number of bytes to read from source */
699   read_size = src_samples;
700   read_size *= a2dp_sbc_encoder_cb.feeding_params.channel_count;
701   read_size *= (a2dp_sbc_encoder_cb.feeding_params.bits_per_sample / 8);
702   a2dp_sbc_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
703 
704   /* Read Data from UIPC channel */
705   nb_byte_read =
706       a2dp_sbc_encoder_cb.read_callback((uint8_t*)read_buffer, read_size);
707   a2dp_sbc_encoder_cb.stats.media_read_total_actual_read_bytes += nb_byte_read;
708 
709   if (nb_byte_read < read_size) {
710     if (nb_byte_read == 0) return false;
711 
712     /* Fill the unfilled part of the read buffer with silence (0) */
713     memset(((uint8_t*)read_buffer) + nb_byte_read, 0, read_size - nb_byte_read);
714     nb_byte_read = read_size;
715   }
716   a2dp_sbc_encoder_cb.stats.media_read_total_actual_reads_count++;
717 
718   /* Initialize PCM up-sampling engine */
719   a2dp_sbc_init_up_sample(a2dp_sbc_encoder_cb.feeding_params.sample_rate,
720                           sbc_sampling,
721                           a2dp_sbc_encoder_cb.feeding_params.bits_per_sample,
722                           a2dp_sbc_encoder_cb.feeding_params.channel_count);
723 
724   /*
725    * Re-sample the read buffer.
726    * The output PCM buffer will be stereo, 16 bit per sample.
727    */
728   dst_size_used = a2dp_sbc_up_sample(
729       (uint8_t*)read_buffer,
730       (uint8_t*)up_sampled_buffer +
731           a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
732       nb_byte_read, sizeof(up_sampled_buffer) -
733                         a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue,
734       &src_size_used);
735 
736   /* update the residue */
737   a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue += dst_size_used;
738 
739   /* only copy the pcm sample when we have up-sampled enough PCM */
740   if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue < bytes_needed)
741     return false;
742 
743   /* Copy the output pcm samples in SBC encoding buffer */
744   memcpy((uint8_t*)a2dp_sbc_encoder_cb.pcmBuffer, (uint8_t*)up_sampled_buffer,
745          bytes_needed);
746   /* update the residue */
747   a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue -= bytes_needed;
748 
749   if (a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue != 0) {
750     memcpy((uint8_t*)up_sampled_buffer,
751            (uint8_t*)up_sampled_buffer + bytes_needed,
752            a2dp_sbc_encoder_cb.feeding_state.aa_feed_residue);
753   }
754   return true;
755 }
756 
calculate_max_frames_per_packet(void)757 static uint8_t calculate_max_frames_per_packet(void) {
758   uint16_t effective_mtu_size = a2dp_sbc_encoder_cb.TxAaMtuSize;
759   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
760   uint16_t result = 0;
761   uint32_t frame_len;
762 
763   LOG_VERBOSE(LOG_TAG, "%s: original AVDTP MTU size: %d", __func__,
764               a2dp_sbc_encoder_cb.TxAaMtuSize);
765   if (a2dp_sbc_encoder_cb.is_peer_edr &&
766       !a2dp_sbc_encoder_cb.peer_supports_3mbps) {
767     // This condition would be satisfied only if the remote device is
768     // EDR and supports only 2 Mbps, but the effective AVDTP MTU size
769     // exceeds the 2DH5 packet size.
770     LOG_VERBOSE(LOG_TAG,
771                 "%s: The remote device is EDR but does not support 3 Mbps",
772                 __func__);
773 
774     if (effective_mtu_size > MAX_2MBPS_AVDTP_MTU) {
775       LOG_WARN(LOG_TAG, "%s: Restricting AVDTP MTU size to %d", __func__,
776                MAX_2MBPS_AVDTP_MTU);
777       effective_mtu_size = MAX_2MBPS_AVDTP_MTU;
778       a2dp_sbc_encoder_cb.TxAaMtuSize = effective_mtu_size;
779     }
780   }
781 
782   if (!p_encoder_params->s16NumOfSubBands) {
783     LOG_ERROR(LOG_TAG, "%s: SubBands are set to 0, resetting to %d", __func__,
784               SBC_MAX_NUM_OF_SUBBANDS);
785     p_encoder_params->s16NumOfSubBands = SBC_MAX_NUM_OF_SUBBANDS;
786   }
787   if (!p_encoder_params->s16NumOfBlocks) {
788     LOG_ERROR(LOG_TAG, "%s: Blocks are set to 0, resetting to %d", __func__,
789               SBC_MAX_NUM_OF_BLOCKS);
790     p_encoder_params->s16NumOfBlocks = SBC_MAX_NUM_OF_BLOCKS;
791   }
792   if (!p_encoder_params->s16NumOfChannels) {
793     LOG_ERROR(LOG_TAG, "%s: Channels are set to 0, resetting to %d", __func__,
794               SBC_MAX_NUM_OF_CHANNELS);
795     p_encoder_params->s16NumOfChannels = SBC_MAX_NUM_OF_CHANNELS;
796   }
797 
798   frame_len = a2dp_sbc_frame_length();
799 
800   LOG_VERBOSE(LOG_TAG, "%s: Effective Tx MTU to be considered: %d", __func__,
801               effective_mtu_size);
802 
803   switch (p_encoder_params->s16SamplingFreq) {
804     case SBC_sf44100:
805       if (frame_len == 0) {
806         LOG_ERROR(LOG_TAG,
807                   "%s: Calculating frame length, resetting it to default %d",
808                   __func__, A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1);
809         frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_44_1;
810       }
811       result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
812       LOG_VERBOSE(LOG_TAG, "%s: Max number of SBC frames: %d", __func__,
813                   result);
814       break;
815 
816     case SBC_sf48000:
817       if (frame_len == 0) {
818         LOG_ERROR(LOG_TAG,
819                   "%s: Calculating frame length, resetting it to default %d",
820                   __func__, A2DP_SBC_MAX_HQ_FRAME_SIZE_48);
821         frame_len = A2DP_SBC_MAX_HQ_FRAME_SIZE_48;
822       }
823       result = (effective_mtu_size - A2DP_HDR_SIZE) / frame_len;
824       LOG_VERBOSE(LOG_TAG, "%s: Max number of SBC frames: %d", __func__,
825                   result);
826       break;
827 
828     default:
829       LOG_ERROR(LOG_TAG, "%s: Max number of SBC frames: %d", __func__, result);
830       break;
831   }
832   return result;
833 }
834 
a2dp_sbc_source_rate()835 static uint16_t a2dp_sbc_source_rate() {
836   uint16_t rate = A2DP_SBC_DEFAULT_BITRATE;
837 
838   /* restrict bitrate if a2dp link is non-edr */
839   if (!a2dp_sbc_encoder_cb.is_peer_edr) {
840     rate = A2DP_SBC_NON_EDR_MAX_RATE;
841     LOG_VERBOSE(LOG_TAG, "%s: non-edr a2dp sink detected, restrict rate to %d",
842                 __func__, rate);
843   }
844 
845   return rate;
846 }
847 
a2dp_sbc_frame_length(void)848 static uint32_t a2dp_sbc_frame_length(void) {
849   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
850   uint32_t frame_len = 0;
851 
852   LOG_VERBOSE(LOG_TAG,
853               "%s: channel mode: %d, sub-band: %d, number of block: %d, "
854               "bitpool: %d, sampling frequency: %d, num channels: %d",
855               __func__, p_encoder_params->s16ChannelMode,
856               p_encoder_params->s16NumOfSubBands,
857               p_encoder_params->s16NumOfBlocks, p_encoder_params->s16BitPool,
858               p_encoder_params->s16SamplingFreq,
859               p_encoder_params->s16NumOfChannels);
860 
861   switch (p_encoder_params->s16ChannelMode) {
862     case SBC_MONO:
863     /* FALLTHROUGH */
864     case SBC_DUAL:
865       frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
866                   ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
867                               p_encoder_params->s16NumOfSubBands *
868                               p_encoder_params->s16NumOfChannels) /
869                    CHAR_BIT) +
870                   ((uint32_t)(p_encoder_params->s16NumOfBlocks *
871                               p_encoder_params->s16NumOfChannels *
872                               p_encoder_params->s16BitPool) /
873                    CHAR_BIT);
874       break;
875     case SBC_STEREO:
876       frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
877                   ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
878                               p_encoder_params->s16NumOfSubBands *
879                               p_encoder_params->s16NumOfChannels) /
880                    CHAR_BIT) +
881                   ((uint32_t)(p_encoder_params->s16NumOfBlocks *
882                               p_encoder_params->s16BitPool) /
883                    CHAR_BIT);
884       break;
885     case SBC_JOINT_STEREO:
886       frame_len = A2DP_SBC_FRAME_HEADER_SIZE_BYTES +
887                   ((uint32_t)(A2DP_SBC_SCALE_FACTOR_BITS *
888                               p_encoder_params->s16NumOfSubBands *
889                               p_encoder_params->s16NumOfChannels) /
890                    CHAR_BIT) +
891                   ((uint32_t)(p_encoder_params->s16NumOfSubBands +
892                               (p_encoder_params->s16NumOfBlocks *
893                                p_encoder_params->s16BitPool)) /
894                    CHAR_BIT);
895       break;
896     default:
897       LOG_VERBOSE(LOG_TAG, "%s: Invalid channel number: %d", __func__,
898                   p_encoder_params->s16ChannelMode);
899       break;
900   }
901   LOG_VERBOSE(LOG_TAG, "%s: calculated frame length: %d", __func__, frame_len);
902   return frame_len;
903 }
904 
a2dp_sbc_get_bitrate()905 uint32_t a2dp_sbc_get_bitrate() {
906   SBC_ENC_PARAMS* p_encoder_params = &a2dp_sbc_encoder_cb.sbc_encoder_params;
907   LOG_DEBUG(LOG_TAG, "%s: bit rate %d ", __func__,
908             p_encoder_params->u16BitRate);
909   return p_encoder_params->u16BitRate * 1000;
910 }
911 
encoderIntervalMs() const912 period_ms_t A2dpCodecConfigSbcSource::encoderIntervalMs() const {
913   return a2dp_sbc_get_encoder_interval_ms();
914 }
915 
getEffectiveMtu() const916 int A2dpCodecConfigSbcSource::getEffectiveMtu() const {
917   return a2dp_sbc_encoder_cb.TxAaMtuSize;
918 }
919 
debug_codec_dump(int fd)920 void A2dpCodecConfigSbcSource::debug_codec_dump(int fd) {
921   a2dp_sbc_encoder_stats_t* stats = &a2dp_sbc_encoder_cb.stats;
922 
923   A2dpCodecConfig::debug_codec_dump(fd);
924 
925   dprintf(fd,
926           "  Packet counts (expected/dropped)                        : %zu / "
927           "%zu\n",
928           stats->media_read_total_expected_packets,
929           stats->media_read_total_dropped_packets);
930 
931   dprintf(fd,
932           "  PCM read counts (expected/actual)                       : %zu / "
933           "%zu\n",
934           stats->media_read_total_expected_reads_count,
935           stats->media_read_total_actual_reads_count);
936 
937   dprintf(fd,
938           "  PCM read bytes (expected/actual)                        : %zu / "
939           "%zu\n",
940           stats->media_read_total_expected_read_bytes,
941           stats->media_read_total_actual_read_bytes);
942 
943   dprintf(fd,
944           "  Frames counts (expected/dropped)                        : %zu / "
945           "%zu\n",
946           stats->media_read_total_expected_frames,
947           stats->media_read_total_dropped_frames);
948 }
949