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