1 /*
2  * Copyright 2021 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #define LOG_TAG "bluetooth-a2dp"
18 
19 #include "a2dp_vendor_opus_encoder.h"
20 
21 #include <bluetooth/log.h>
22 #include <dlfcn.h>
23 #include <opus.h>
24 #include <string.h>
25 
26 #include "a2dp_vendor.h"
27 #include "a2dp_vendor_opus.h"
28 #include "common/time_util.h"
29 #include "os/log.h"
30 #include "osi/include/allocator.h"
31 #include "osi/include/osi.h"
32 #include "stack/include/bt_hdr.h"
33 
34 using namespace bluetooth;
35 
36 typedef struct {
37   uint32_t sample_rate;
38   uint16_t bitrate;
39   uint16_t framesize;
40   uint8_t channel_mode;
41   uint8_t bits_per_sample;
42   uint8_t quality_mode_index;
43   int pcm_wlength;
44   uint8_t pcm_fmt;
45 } tA2DP_OPUS_ENCODER_PARAMS;
46 
47 typedef struct {
48   float counter;
49   uint32_t bytes_per_tick;
50   uint64_t last_frame_us;
51 } tA2DP_OPUS_FEEDING_STATE;
52 
53 typedef struct {
54   uint64_t session_start_us;
55 
56   size_t media_read_total_expected_packets;
57   size_t media_read_total_expected_reads_count;
58   size_t media_read_total_expected_read_bytes;
59 
60   size_t media_read_total_dropped_packets;
61   size_t media_read_total_actual_reads_count;
62   size_t media_read_total_actual_read_bytes;
63 } a2dp_opus_encoder_stats_t;
64 
65 typedef struct {
66   a2dp_source_read_callback_t read_callback;
67   a2dp_source_enqueue_callback_t enqueue_callback;
68   uint16_t TxAaMtuSize;
69   size_t TxQueueLength;
70 
71   bool use_SCMS_T;
72   bool is_peer_edr;          // True if the peer device supports EDR
73   bool peer_supports_3mbps;  // True if the peer device supports 3Mbps EDR
74   uint16_t peer_mtu;         // MTU of the A2DP peer
75   uint32_t timestamp;        // Timestamp for the A2DP frames
76 
77   OpusEncoder* opus_handle;
78   bool has_opus_handle;  // True if opus_handle is valid
79 
80   tA2DP_FEEDING_PARAMS feeding_params;
81   tA2DP_OPUS_ENCODER_PARAMS opus_encoder_params;
82   tA2DP_OPUS_FEEDING_STATE opus_feeding_state;
83 
84   a2dp_opus_encoder_stats_t stats;
85 } tA2DP_OPUS_ENCODER_CB;
86 
87 static tA2DP_OPUS_ENCODER_CB a2dp_opus_encoder_cb;
88 
89 static bool a2dp_vendor_opus_encoder_update(uint16_t peer_mtu,
90                                             A2dpCodecConfig* a2dp_codec_config,
91                                             bool* p_restart_input,
92                                             bool* p_restart_output,
93                                             bool* p_config_updated);
94 static void a2dp_opus_get_num_frame_iteration(uint8_t* num_of_iterations,
95                                               uint8_t* num_of_frames,
96                                               uint64_t timestamp_us);
97 static void a2dp_opus_encode_frames(uint8_t nb_frame);
98 static bool a2dp_opus_read_feeding(uint8_t* read_buffer, uint32_t* bytes_read);
99 
a2dp_vendor_opus_encoder_cleanup(void)100 void a2dp_vendor_opus_encoder_cleanup(void) {
101   if (a2dp_opus_encoder_cb.has_opus_handle) {
102     osi_free(a2dp_opus_encoder_cb.opus_handle);
103     a2dp_opus_encoder_cb.has_opus_handle = false;
104     a2dp_opus_encoder_cb.opus_handle = nullptr;
105   }
106   memset(&a2dp_opus_encoder_cb, 0, sizeof(a2dp_opus_encoder_cb));
107 
108   a2dp_opus_encoder_cb.stats.session_start_us =
109       bluetooth::common::time_get_os_boottime_us();
110 
111   a2dp_opus_encoder_cb.timestamp = 0;
112   a2dp_opus_encoder_cb.use_SCMS_T = false;
113 
114   return;
115 }
116 
a2dp_vendor_opus_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)117 void a2dp_vendor_opus_encoder_init(
118     const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params,
119     A2dpCodecConfig* a2dp_codec_config,
120     a2dp_source_read_callback_t read_callback,
121     a2dp_source_enqueue_callback_t enqueue_callback) {
122   uint32_t error_val;
123 
124   a2dp_vendor_opus_encoder_cleanup();
125 
126   a2dp_opus_encoder_cb.read_callback = read_callback;
127   a2dp_opus_encoder_cb.enqueue_callback = enqueue_callback;
128   a2dp_opus_encoder_cb.is_peer_edr = p_peer_params->is_peer_edr;
129   a2dp_opus_encoder_cb.peer_supports_3mbps = p_peer_params->peer_supports_3mbps;
130   a2dp_opus_encoder_cb.peer_mtu = p_peer_params->peer_mtu;
131 
132   // NOTE: Ignore the restart_input / restart_output flags - this initization
133   // happens when the connection is (re)started.
134   bool restart_input = false;
135   bool restart_output = false;
136   bool config_updated = false;
137 
138   uint32_t size = opus_encoder_get_size(A2DP_OPUS_CODEC_OUTPUT_CHS);
139   a2dp_opus_encoder_cb.opus_handle =
140       static_cast<OpusEncoder*>(osi_malloc(size));
141   if (a2dp_opus_encoder_cb.opus_handle == nullptr) {
142     log::error("failed to allocate opus encoder handle");
143     return;
144   }
145 
146   error_val = opus_encoder_init(
147       a2dp_opus_encoder_cb.opus_handle, A2DP_OPUS_CODEC_DEFAULT_SAMPLERATE,
148       A2DP_OPUS_CODEC_OUTPUT_CHS, OPUS_APPLICATION_AUDIO);
149 
150   if (error_val != OPUS_OK) {
151     log::error(
152         "failed to init opus encoder (handle size {}, sampling rate {}, output "
153         "chs {}, error {})",
154         size, A2DP_OPUS_CODEC_DEFAULT_SAMPLERATE, A2DP_OPUS_CODEC_OUTPUT_CHS,
155         error_val);
156     osi_free(a2dp_opus_encoder_cb.opus_handle);
157     return;
158   } else {
159     a2dp_opus_encoder_cb.has_opus_handle = true;
160   }
161 
162   a2dp_vendor_opus_encoder_update(a2dp_opus_encoder_cb.peer_mtu,
163                                   a2dp_codec_config, &restart_input,
164                                   &restart_output, &config_updated);
165 
166   return;
167 }
168 
updateEncoderUserConfig(const tA2DP_ENCODER_INIT_PEER_PARAMS * p_peer_params,bool * p_restart_input,bool * p_restart_output,bool * p_config_updated)169 bool A2dpCodecConfigOpusSource::updateEncoderUserConfig(
170     const tA2DP_ENCODER_INIT_PEER_PARAMS* p_peer_params, bool* p_restart_input,
171     bool* p_restart_output, bool* p_config_updated) {
172   if (a2dp_opus_encoder_cb.peer_mtu == 0) {
173     log::error("Cannot update the codec encoder for {}: invalid peer MTU",
174                name());
175     return false;
176   }
177 
178   return a2dp_vendor_opus_encoder_update(a2dp_opus_encoder_cb.peer_mtu, this,
179                                          p_restart_input, p_restart_output,
180                                          p_config_updated);
181 }
182 
a2dp_vendor_opus_encoder_update(uint16_t peer_mtu,A2dpCodecConfig * a2dp_codec_config,bool * p_restart_input,bool * p_restart_output,bool * p_config_updated)183 static bool a2dp_vendor_opus_encoder_update(uint16_t peer_mtu,
184                                             A2dpCodecConfig* a2dp_codec_config,
185                                             bool* p_restart_input,
186                                             bool* p_restart_output,
187                                             bool* p_config_updated) {
188   tA2DP_OPUS_ENCODER_PARAMS* p_encoder_params =
189       &a2dp_opus_encoder_cb.opus_encoder_params;
190   uint8_t codec_info[AVDT_CODEC_SIZE];
191   uint32_t error = 0;
192 
193   *p_restart_input = false;
194   *p_restart_output = false;
195   *p_config_updated = false;
196 
197   if (!a2dp_opus_encoder_cb.has_opus_handle ||
198       a2dp_opus_encoder_cb.opus_handle == NULL) {
199     log::error("Cannot get Opus encoder handle");
200     return false;
201   }
202   log::assert_that(
203       a2dp_opus_encoder_cb.opus_handle != nullptr,
204       "assert failed: a2dp_opus_encoder_cb.opus_handle != nullptr");
205 
206   if (!a2dp_codec_config->copyOutOtaCodecConfig(codec_info)) {
207     log::error("Cannot update the codec encoder for {}: invalid codec config",
208                a2dp_codec_config->name());
209     return false;
210   }
211   const uint8_t* p_codec_info = codec_info;
212   btav_a2dp_codec_config_t codec_config = a2dp_codec_config->getCodecConfig();
213 
214   // The feeding parameters
215   tA2DP_FEEDING_PARAMS* p_feeding_params = &a2dp_opus_encoder_cb.feeding_params;
216   p_feeding_params->sample_rate =
217       A2DP_VendorGetTrackSampleRateOpus(p_codec_info);
218   p_feeding_params->bits_per_sample =
219       a2dp_codec_config->getAudioBitsPerSample();
220   p_feeding_params->channel_count =
221       A2DP_VendorGetTrackChannelCountOpus(p_codec_info);
222   log::info("sample_rate={} bits_per_sample={} channel_count={}",
223             p_feeding_params->sample_rate, p_feeding_params->bits_per_sample,
224             p_feeding_params->channel_count);
225 
226   // The codec parameters
227   p_encoder_params->sample_rate =
228       a2dp_opus_encoder_cb.feeding_params.sample_rate;
229   p_encoder_params->channel_mode =
230       A2DP_VendorGetChannelModeCodeOpus(p_codec_info);
231   p_encoder_params->framesize = A2DP_VendorGetFrameSizeOpus(p_codec_info);
232   p_encoder_params->bitrate = A2DP_VendorGetBitRateOpus(p_codec_info);
233 
234   a2dp_vendor_opus_feeding_reset();
235 
236   uint16_t mtu_size =
237       BT_DEFAULT_BUFFER_SIZE - A2DP_OPUS_OFFSET - sizeof(BT_HDR);
238   if (mtu_size < peer_mtu) {
239     a2dp_opus_encoder_cb.TxAaMtuSize = mtu_size;
240   } else {
241     a2dp_opus_encoder_cb.TxAaMtuSize = peer_mtu;
242   }
243 
244   // Set the bitrate quality mode index
245   if (codec_config.codec_specific_3 != 0) {
246     p_encoder_params->quality_mode_index = codec_config.codec_specific_3 % 10;
247     log::info("setting bitrate quality mode to {}",
248               p_encoder_params->quality_mode_index);
249   } else {
250     p_encoder_params->quality_mode_index = 5;
251     log::info("setting bitrate quality mode to default {}",
252               p_encoder_params->quality_mode_index);
253   }
254 
255   error = opus_encoder_ctl(
256       a2dp_opus_encoder_cb.opus_handle,
257       OPUS_SET_COMPLEXITY(p_encoder_params->quality_mode_index));
258 
259   if (error != OPUS_OK) {
260     log::error("failed to set encoder bitrate quality setting");
261     return false;
262   }
263 
264   p_encoder_params->pcm_wlength =
265       a2dp_opus_encoder_cb.feeding_params.bits_per_sample >> 3;
266 
267   log::info("setting bitrate to {}", p_encoder_params->bitrate);
268   error = opus_encoder_ctl(a2dp_opus_encoder_cb.opus_handle,
269                            OPUS_SET_BITRATE(p_encoder_params->bitrate));
270 
271   if (error != OPUS_OK) {
272     log::error("failed to set encoder bitrate");
273     return false;
274   }
275 
276   // Set the Audio format from pcm_wlength
277   if (p_encoder_params->pcm_wlength == 2)
278     p_encoder_params->pcm_fmt = 16;
279   else if (p_encoder_params->pcm_wlength == 3)
280     p_encoder_params->pcm_fmt = 24;
281   else if (p_encoder_params->pcm_wlength == 4)
282     p_encoder_params->pcm_fmt = 32;
283 
284   return true;
285 }
286 
a2dp_vendor_opus_feeding_reset(void)287 void a2dp_vendor_opus_feeding_reset(void) {
288   memset(&a2dp_opus_encoder_cb.opus_feeding_state, 0,
289          sizeof(a2dp_opus_encoder_cb.opus_feeding_state));
290 
291   a2dp_opus_encoder_cb.opus_feeding_state.bytes_per_tick =
292       (a2dp_opus_encoder_cb.feeding_params.sample_rate *
293        a2dp_opus_encoder_cb.feeding_params.bits_per_sample / 8 *
294        a2dp_opus_encoder_cb.feeding_params.channel_count *
295        a2dp_vendor_opus_get_encoder_interval_ms()) /
296       1000;
297 
298   return;
299 }
300 
a2dp_vendor_opus_feeding_flush(void)301 void a2dp_vendor_opus_feeding_flush(void) {
302   a2dp_opus_encoder_cb.opus_feeding_state.counter = 0.0f;
303 
304   return;
305 }
306 
a2dp_vendor_opus_get_encoder_interval_ms(void)307 uint64_t a2dp_vendor_opus_get_encoder_interval_ms(void) {
308   return ((a2dp_opus_encoder_cb.opus_encoder_params.framesize * 1000) /
309           a2dp_opus_encoder_cb.opus_encoder_params.sample_rate);
310 }
311 
a2dp_vendor_opus_send_frames(uint64_t timestamp_us)312 void a2dp_vendor_opus_send_frames(uint64_t timestamp_us) {
313   uint8_t nb_frame = 0;
314   uint8_t nb_iterations = 0;
315 
316   a2dp_opus_get_num_frame_iteration(&nb_iterations, &nb_frame, timestamp_us);
317   if (nb_frame == 0) return;
318 
319   for (uint8_t counter = 0; counter < nb_iterations; counter++) {
320     // Transcode frame and enqueue
321     a2dp_opus_encode_frames(nb_frame);
322   }
323 
324   return;
325 }
326 
327 // Obtains the number of frames to send and number of iterations
328 // to be used. |num_of_iterations| and |num_of_frames| parameters
329 // are used as output param for returning the respective values.
a2dp_opus_get_num_frame_iteration(uint8_t * num_of_iterations,uint8_t * num_of_frames,uint64_t timestamp_us)330 static void a2dp_opus_get_num_frame_iteration(uint8_t* num_of_iterations,
331                                               uint8_t* num_of_frames,
332                                               uint64_t timestamp_us) {
333   uint32_t result = 0;
334   uint8_t nof = 0;
335   uint8_t noi = 1;
336 
337   uint32_t pcm_bytes_per_frame =
338       a2dp_opus_encoder_cb.opus_encoder_params.framesize *
339       a2dp_opus_encoder_cb.feeding_params.channel_count *
340       a2dp_opus_encoder_cb.feeding_params.bits_per_sample / 8;
341 
342   uint32_t us_this_tick = a2dp_vendor_opus_get_encoder_interval_ms() * 1000;
343   uint64_t now_us = timestamp_us;
344   if (a2dp_opus_encoder_cb.opus_feeding_state.last_frame_us != 0)
345     us_this_tick =
346         (now_us - a2dp_opus_encoder_cb.opus_feeding_state.last_frame_us);
347   a2dp_opus_encoder_cb.opus_feeding_state.last_frame_us = now_us;
348 
349   a2dp_opus_encoder_cb.opus_feeding_state.counter +=
350       (float)a2dp_opus_encoder_cb.opus_feeding_state.bytes_per_tick *
351       us_this_tick / (a2dp_vendor_opus_get_encoder_interval_ms() * 1000);
352 
353   result =
354       a2dp_opus_encoder_cb.opus_feeding_state.counter / pcm_bytes_per_frame;
355   a2dp_opus_encoder_cb.opus_feeding_state.counter -=
356       result * pcm_bytes_per_frame;
357   nof = result;
358 
359   *num_of_frames = nof;
360   *num_of_iterations = noi;
361 }
362 
a2dp_opus_encode_frames(uint8_t nb_frame)363 static void a2dp_opus_encode_frames(uint8_t nb_frame) {
364   tA2DP_OPUS_ENCODER_PARAMS* p_encoder_params =
365       &a2dp_opus_encoder_cb.opus_encoder_params;
366   unsigned char* packet;
367   uint8_t remain_nb_frame = nb_frame;
368   uint16_t opus_frame_size = p_encoder_params->framesize;
369   uint8_t read_buffer[p_encoder_params->framesize *
370                       p_encoder_params->pcm_wlength *
371                       p_encoder_params->channel_mode];
372 
373   int32_t out_frames = 0;
374   int32_t written = 0;
375 
376   uint32_t bytes_read = 0;
377   while (nb_frame) {
378     BT_HDR* p_buf = (BT_HDR*)osi_malloc(BT_DEFAULT_BUFFER_SIZE);
379     p_buf->offset = A2DP_OPUS_OFFSET;
380     p_buf->len = 0;
381     p_buf->layer_specific = 0;
382     a2dp_opus_encoder_cb.stats.media_read_total_expected_packets++;
383 
384     do {
385       //
386       // Read the PCM data and encode it
387       //
388       uint32_t temp_bytes_read = 0;
389       if (a2dp_opus_read_feeding(read_buffer, &temp_bytes_read)) {
390         bytes_read += temp_bytes_read;
391         packet = (unsigned char*)(p_buf + 1) + p_buf->offset + p_buf->len;
392 
393         if (a2dp_opus_encoder_cb.opus_handle == NULL) {
394           log::error("invalid OPUS handle");
395           a2dp_opus_encoder_cb.stats.media_read_total_dropped_packets++;
396           osi_free(p_buf);
397           return;
398         }
399 
400         written =
401             opus_encode(a2dp_opus_encoder_cb.opus_handle,
402                         (const opus_int16*)&read_buffer[0], opus_frame_size,
403                         packet, (BT_DEFAULT_BUFFER_SIZE - p_buf->offset));
404 
405         if (written <= 0) {
406           log::error("OPUS encoding error");
407           a2dp_opus_encoder_cb.stats.media_read_total_dropped_packets++;
408           osi_free(p_buf);
409           return;
410         } else {
411           out_frames++;
412         }
413         p_buf->len += written;
414         nb_frame--;
415         p_buf->layer_specific += out_frames;  // added a frame to the buffer
416       } else {
417         log::warn("Opus src buffer underflow {}", nb_frame);
418         a2dp_opus_encoder_cb.opus_feeding_state.counter +=
419             nb_frame * opus_frame_size *
420             a2dp_opus_encoder_cb.feeding_params.channel_count *
421             a2dp_opus_encoder_cb.feeding_params.bits_per_sample / 8;
422 
423         // no more pcm to read
424         nb_frame = 0;
425       }
426     } while ((written == 0) && nb_frame);
427 
428     if (p_buf->len) {
429       /*
430        * Timestamp of the media packet header represent the TS of the
431        * first frame, i.e. the timestamp before including this frame.
432        */
433       *((uint32_t*)(p_buf + 1)) = a2dp_opus_encoder_cb.timestamp;
434 
435       // Timestamp will wrap over to 0 if stream continues on long enough
436       // (>25H @ 48KHz). The parameters are promoted to 64bit to ensure that
437       // no unsigned overflow is triggered as ubsan is always enabled.
438       a2dp_opus_encoder_cb.timestamp =
439           ((uint64_t)a2dp_opus_encoder_cb.timestamp +
440            (p_buf->layer_specific * opus_frame_size)) & UINT32_MAX;
441 
442       uint8_t done_nb_frame = remain_nb_frame - nb_frame;
443       remain_nb_frame = nb_frame;
444 
445       if (!a2dp_opus_encoder_cb.enqueue_callback(p_buf, done_nb_frame,
446                                                  bytes_read))
447         return;
448     } else {
449       a2dp_opus_encoder_cb.stats.media_read_total_dropped_packets++;
450       osi_free(p_buf);
451     }
452   }
453 }
454 
a2dp_opus_read_feeding(uint8_t * read_buffer,uint32_t * bytes_read)455 static bool a2dp_opus_read_feeding(uint8_t* read_buffer, uint32_t* bytes_read) {
456   uint32_t read_size = a2dp_opus_encoder_cb.opus_encoder_params.framesize *
457                        a2dp_opus_encoder_cb.feeding_params.channel_count *
458                        a2dp_opus_encoder_cb.feeding_params.bits_per_sample / 8;
459 
460   a2dp_opus_encoder_cb.stats.media_read_total_expected_reads_count++;
461   a2dp_opus_encoder_cb.stats.media_read_total_expected_read_bytes += read_size;
462 
463   /* Read Data from UIPC channel */
464   uint32_t nb_byte_read =
465       a2dp_opus_encoder_cb.read_callback(read_buffer, read_size);
466   a2dp_opus_encoder_cb.stats.media_read_total_actual_read_bytes += nb_byte_read;
467 
468   if (nb_byte_read < read_size) {
469     if (nb_byte_read == 0) return false;
470 
471     /* Fill the unfilled part of the read buffer with silence (0) */
472     memset(((uint8_t*)read_buffer) + nb_byte_read, 0, read_size - nb_byte_read);
473     nb_byte_read = read_size;
474   }
475   a2dp_opus_encoder_cb.stats.media_read_total_actual_reads_count++;
476 
477   *bytes_read = nb_byte_read;
478   return true;
479 }
480 
a2dp_vendor_opus_set_transmit_queue_length(size_t transmit_queue_length)481 void a2dp_vendor_opus_set_transmit_queue_length(size_t transmit_queue_length) {
482   a2dp_opus_encoder_cb.TxQueueLength = transmit_queue_length;
483 
484   return;
485 }
486 
encoderIntervalMs() const487 uint64_t A2dpCodecConfigOpusSource::encoderIntervalMs() const {
488   return a2dp_vendor_opus_get_encoder_interval_ms();
489 }
490 
a2dp_vendor_opus_get_effective_frame_size()491 int a2dp_vendor_opus_get_effective_frame_size() {
492   return a2dp_opus_encoder_cb.TxAaMtuSize;
493 }
494 
debug_codec_dump(int fd)495 void A2dpCodecConfigOpusSource::debug_codec_dump(int fd) {
496   a2dp_opus_encoder_stats_t* stats = &a2dp_opus_encoder_cb.stats;
497   tA2DP_OPUS_ENCODER_PARAMS* p_encoder_params =
498       &a2dp_opus_encoder_cb.opus_encoder_params;
499 
500   A2dpCodecConfig::debug_codec_dump(fd);
501 
502   dprintf(fd,
503           "  Packet counts (expected/dropped)                        : %zu / "
504           "%zu\n",
505           stats->media_read_total_expected_packets,
506           stats->media_read_total_dropped_packets);
507 
508   dprintf(fd,
509           "  PCM read counts (expected/actual)                       : %zu / "
510           "%zu\n",
511           stats->media_read_total_expected_reads_count,
512           stats->media_read_total_actual_reads_count);
513 
514   dprintf(fd,
515           "  PCM read bytes (expected/actual)                        : %zu / "
516           "%zu\n",
517           stats->media_read_total_expected_read_bytes,
518           stats->media_read_total_actual_read_bytes);
519 
520   dprintf(fd,
521           "  OPUS transmission bitrate (Kbps)                        : %d\n",
522           p_encoder_params->bitrate);
523 
524   dprintf(fd,
525           "  OPUS saved transmit queue length                        : %zu\n",
526           a2dp_opus_encoder_cb.TxQueueLength);
527 
528   return;
529 }
530