Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 48fdc3e7 authored by En-Shuo Hsu's avatar En-Shuo Hsu
Browse files

floss: Support playback for various SCO packet 

- Refine mSBC playback related code and move it into btm_sco_hci.cc
  under sco::wbs
- Change to queue the encoded mSBC packets instead of directly sending
  it. This is essential for SCO packet size that is not 60.
- Support SCO packet size 72.

Bug: 235901463
Tag: #floss
Test: system/gd/cert/run & Build and verify WBS playback and recording
works on platforms with packet size 60 and 72

Change-Id: Ie601f3f2ba280a1c697278828df5a43bbfbf7b61
parent b1f23f12

system/stack/btm/btm_sco.cc

+22 −48
Original line number Diff line number Diff line
@@ -33,7 +33,6 @@ 
#include "device/include/controller.h"

#include "embdrv/sbc/decoder/include/oi_codec_sbc.h"

#include "embdrv/sbc/decoder/include/oi_status.h"

#include "hfp_msbc_encoder.h"

#include "osi/include/allocator.h"

#include "osi/include/log.h"

#include "osi/include/osi.h"
@@ -93,28 +92,6 @@ static int16_t btm_pcm_buf[BTM_SCO_DATA_SIZE_MAX] = {0}; 
 * They are only used for WBS and the unit is byte. */

static size_t btm_pcm_buf_read_offset = 0;

static size_t btm_pcm_buf_write_offset = 0;



/* Per Bluetooth Core v5.0 and HFP 1.7 specification. */

#define BTM_MSBC_H2_HEADER_0 0x01

#define BTM_MSBC_H2_HEADER_LEN 2

#define BTM_MSBC_PKT_LEN 60

#define BTM_MSBC_PKT_FRAME_LEN 57 /* Packet length without the header */

#define BTM_MSBC_CODE_SIZE 240



/* The pre-computed zero input bit stream of mSBC codec, per HFP 1.7 spec.

 * This mSBC frame will be decoded into all-zero input PCM. */

static const uint8_t btm_msbc_zero_packet[] = {

    0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d, 0xb6, 0xdd,

    0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 0xb6,

    0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,

    0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77,

    0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c};



/* Second octet of H2 header is composed by 4 bits fixed 0x8 and 4 bits

 * sequence number 0000, 0011, 1100, 1111. */

static const uint8_t btm_h2_header_frames_count[] = {0x08, 0x38, 0xc8, 0xf8};



static uint8_t btm_msbc_num_out_frames;

/******************************************************************************/

/*            L O C A L    F U N C T I O N     P R O T O T Y P E S            */

/******************************************************************************/
@@ -271,6 +248,7 @@ void btm_route_sco_data(BT_HDR* p_msg) { 
  /* For Chrome OS, we send the outgoing data after receiving an incoming one.

   * server, so that we can keep the data read/write rate balanced */

  size_t read = 0, avail = 0;

  const uint8_t* encoded = nullptr;

  if (active_sco->is_wbs()) {

    while (written) {

      avail = BTM_SCO_DATA_SIZE_MAX - btm_pcm_buf_write_offset;
@@ -311,35 +289,34 @@ void btm_route_sco_data(BT_HDR* p_msg) { 
      }



      btm_pcm_buf_write_offset += read;

      if (btm_pcm_buf_write_offset - btm_pcm_buf_read_offset <

          BTM_MSBC_CODE_SIZE) {

        continue;

      }



      uint8_t encoded[BTM_MSBC_PKT_LEN] = {

          BTM_MSBC_H2_HEADER_0,

          btm_h2_header_frames_count[btm_msbc_num_out_frames % 4]};

      uint32_t encoded_size;

      encoded_size = hfp_msbc_encode_frames(btm_pcm_buf, encoded + 2);

      if (encoded_size != BTM_MSBC_PKT_FRAME_LEN) {

        LOG_WARN("Encode invalid packet size: %lu",

                 (unsigned long)encoded_size);

        std::copy(std::begin(btm_msbc_zero_packet),

                  std::end(btm_msbc_zero_packet),

                  &encoded[BTM_MSBC_H2_HEADER_LEN]);

      }

      rc = bluetooth::audio::sco::wbs::encode(

          &btm_pcm_buf[btm_pcm_buf_read_offset / sizeof(*btm_pcm_buf)],

          btm_pcm_buf_write_offset - btm_pcm_buf_read_offset);



      auto data = std::vector<uint8_t>(encoded, encoded + BTM_MSBC_PKT_LEN);

      btm_send_sco_packet(std::move(data));

      btm_msbc_num_out_frames++;

      if (!rc)

        LOG_DEBUG(

            "Failed to encode data starting at ReadOffset:%lu to "

            "WriteOffset:%lu",

            (unsigned long)btm_pcm_buf_read_offset,

            (unsigned long)btm_pcm_buf_write_offset);



      /* The offsets should reset some time as the buffer length should always

       * divisible by BTM_MSBC_CODE_SIZE(240) */

      btm_pcm_buf_read_offset += BTM_MSBC_CODE_SIZE;

       * divisible by BTM_MSBC_CODE_SIZE(240) and wbs::encode only returns

       * BTM_MSBC_CODE_SIZE or 0 */

      btm_pcm_buf_read_offset += rc;

      if (btm_pcm_buf_write_offset == btm_pcm_buf_read_offset) {

        btm_pcm_buf_write_offset = 0;

        btm_pcm_buf_read_offset = 0;

      }



      /* Send all of the available SCO packets buffered in the queue */

      while (1) {

        rc = bluetooth::audio::sco::wbs::dequeue_packet(&encoded);

        if (!rc) break;



        auto data = std::vector<uint8_t>(encoded, encoded + rc);

        btm_send_sco_packet(std::move(data));

      }

    }

  } else {

    while (written) {
@@ -923,8 +900,6 @@ void btm_sco_connected(const RawAddress& bda, uint16_t hci_handle, 
        if (p->is_wbs()) {

          btm_pcm_buf_read_offset = 0;

          btm_pcm_buf_write_offset = 0;

          btm_msbc_num_out_frames = 0;

          hfp_msbc_encoder_init();

          bluetooth::audio::sco::wbs::init(

              hfp_hal_interface::get_packet_size(codec));

        }
@@ -1157,7 +1132,6 @@ void btm_sco_on_disconnected(uint16_t hci_handle, tHCI_REASON reason) { 


  if (p_sco->is_inband()) {

    if (p_sco->is_wbs()) {

      hfp_msbc_encoder_cleanup();

      bluetooth::audio::sco::wbs::cleanup();

    }

system/stack/btm/btm_sco.h

+20 −0
Original line number Diff line number Diff line
@@ -23,6 +23,8 @@ 
#include "device/include/esco_parameters.h"

#include "stack/include/btm_api_types.h"



#define BTM_MSBC_CODE_SIZE 240



constexpr uint16_t kMaxScoLinks = static_cast<uint16_t>(BTM_MAX_SCO_LINKS);



/* SCO-over-HCI audio related definitions */
@@ -77,6 +79,24 @@ size_t enqueue_packet(const uint8_t* data, size_t pkt_size); 
 */

size_t decode(const uint8_t** output);



/* Try to encode PCM data into one SCO packet and put the packets in the buffer.

 * Args:

 *    data - Pointer to the input PCM bytes for the encoder to encode.

 *    len - Length of the input data.

 * Returns:

 *    The length of input data that is encoded. 0 if failed.

 */

size_t encode(int16_t* data, size_t len);



/* Dequeue a SCO packet with encoded mSBC data if possible. The length of the

 * packet is determined by the pkt_size set by the init().

 * Args:

 *    output - Pointer to output mSBC packets encoded by the encoder.

 * Returns:

 *    The length of dequeued packet. 0 if failed.

 */

size_t dequeue_packet(const uint8_t** output);



}  // namespace bluetooth::audio::sco::wbs



/* Define the structures needed by sco */

system/stack/btm/btm_sco_hci.cc

+126 −9
Original line number Diff line number Diff line
@@ -22,6 +22,7 @@ 
#include <memory>



#include "hfp_msbc_decoder.h"

#include "hfp_msbc_encoder.h"

#include "osi/include/allocator.h"

#include "osi/include/log.h"

#include "stack/btm/btm_sco.h"
@@ -32,9 +33,11 @@ 
// TODO(b/198260375): Make SCO data owner group configurable.

#define SCO_HOST_DATA_GROUP "bluetooth-audio"



/* Per Bluetooth Core v5.0 and HFP 1.7 specification. */

#define BTM_MSBC_H2_HEADER_0 0x01

#define BTM_MSBC_CODE_SIZE 240

#define BTM_MSBC_H2_HEADER_LEN 2

#define BTM_MSBC_PKT_LEN 60

#define BTM_MSBC_PKT_FRAME_LEN 57 /* Packet length without the header */

#define BTM_MSBC_SYNC_WORD 0xAD



namespace {
@@ -119,6 +122,15 @@ constexpr size_t btm_wbs_supported_pkt_size[] = {BTM_MSBC_PKT_LEN, 72, 0}; 
 * BTM_MSBC_PKT_LEN for optimizing buffer copy. */

constexpr size_t btm_wbs_msbc_buffer_size[] = {BTM_MSBC_PKT_LEN, 360, 0};



/* The pre-computed zero input bit stream of mSBC codec, per HFP 1.7 spec.

 * This mSBC frame will be decoded into all-zero input PCM. */

static const uint8_t btm_msbc_zero_packet[] = {

    0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d, 0xb6, 0xdd,

    0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 0xb6,

    0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,

    0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77,

    0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c};



static const uint8_t btm_msbc_zero_frames[BTM_MSBC_CODE_SIZE] = {0};



/* Define the structure that contains mSBC data */
@@ -131,6 +143,11 @@ typedef struct { 
  size_t decode_buf_wo;     /* Write offset of the decode buffer */

  size_t decode_buf_ro;     /* Read offset of the decode buffer */



  uint8_t* msbc_encode_buf; /* Buffer to store the encoded SCO packets */

  size_t encode_buf_wo;     /* Write offset of the encode buffer */

  size_t encode_buf_ro;     /* Read offset of the encode buffer */



  uint8_t num_encoded_msbc_pkts; /* Number of the encoded mSBC packets */

  static size_t get_supported_packet_size(size_t pkt_size,

                                          size_t* buffer_size) {

    int i;
@@ -164,6 +181,8 @@ typedef struct { 
  void init(size_t pkt_size) {

    decode_buf_wo = 0;

    decode_buf_ro = 0;

    encode_buf_wo = 0;

    encode_buf_ro = 0;



    pkt_size = get_supported_packet_size(pkt_size, &buf_size);

    if (pkt_size != BTM_MSBC_PKT_LEN) check_alignment = true;
@@ -172,14 +191,31 @@ typedef struct { 


    if (msbc_decode_buf) osi_free(msbc_decode_buf);

    msbc_decode_buf = (uint8_t*)osi_calloc(buf_size);



    if (msbc_encode_buf) osi_free(msbc_encode_buf);

    msbc_encode_buf = (uint8_t*)osi_calloc(buf_size);

  }



  void deinit() {

    if (msbc_decode_buf) osi_free(msbc_decode_buf);

    if (msbc_encode_buf) osi_free(msbc_encode_buf);

  }



  size_t decodable() { return decode_buf_wo - decode_buf_ro; }



  void mark_pkt_decoded() {

    if (decode_buf_ro + BTM_MSBC_PKT_LEN > decode_buf_wo) {

      LOG_ERROR("Trying to mark read offset beyond write offset.");

      return;

    }



    decode_buf_ro += BTM_MSBC_PKT_LEN;

    if (decode_buf_ro == decode_buf_wo) {

      decode_buf_ro = 0;

      decode_buf_wo = 0;

    }

  }



  size_t write(const uint8_t* input, size_t len) {

    if (len > buf_size - decode_buf_wo) {

      return 0;
@@ -206,17 +242,49 @@ typedef struct { 
    return nullptr;

  }



  void mark_pkt_decoded() {

    if (decode_buf_ro + BTM_MSBC_PKT_LEN > decode_buf_wo) {

      LOG_ERROR("Trying to mark read offset beyond write offset.");

      return;

  /* Fill in the mSBC header and update the buffer's write offset to guard the

   * buffer space to be written. Return a pointer to the start of mSBC packet's

   * body for the caller to fill the encoded mSBC data if there is enough space

   * in the buffer to fill in a new packet, otherwise return a nullptr. */

  uint8_t* fill_msbc_pkt_template() {

    uint8_t* wp = &msbc_encode_buf[encode_buf_wo];

    if (buf_size - encode_buf_wo < BTM_MSBC_PKT_LEN) {

      LOG_DEBUG("Packet queue can't accommodate more packets.");

      return nullptr;

    }



    decode_buf_ro += BTM_MSBC_PKT_LEN;

    if (decode_buf_ro == decode_buf_wo) {

      decode_buf_ro = 0;

      decode_buf_wo = 0;

    wp[0] = BTM_MSBC_H2_HEADER_0;

    wp[1] = btm_h2_header_frames_count[num_encoded_msbc_pkts % 4];

    encode_buf_wo += BTM_MSBC_PKT_LEN;



    num_encoded_msbc_pkts++;

    return wp + BTM_MSBC_H2_HEADER_LEN;

  }



  size_t mark_pkt_dequeued() {

    LOG_DEBUG(

        "Try to mark an encoded packet dequeued: ro:%lu wo:%lu pkt_size:%lu",

        (unsigned long)encode_buf_ro, (unsigned long)encode_buf_wo,

        (unsigned long)packet_size);



    if (encode_buf_wo - encode_buf_ro < packet_size) return 0;



    encode_buf_ro += packet_size;

    if (encode_buf_ro == encode_buf_wo) {

      encode_buf_ro = 0;

      encode_buf_wo = 0;

    }



    return packet_size;

  }



  const uint8_t* sco_pkt_read_ptr() {

    if (encode_buf_wo - encode_buf_ro < packet_size) {

      LOG_DEBUG("Insufficient data as a SCO packet to read.");

      return nullptr;

    }



    return &msbc_encode_buf[encode_buf_ro];

  }



} tBTM_MSBC_INFO;
@@ -225,6 +293,7 @@ static tBTM_MSBC_INFO* msbc_info = nullptr; 


void init(size_t pkt_size) {

  hfp_msbc_decoder_init();

  hfp_msbc_encoder_init();



  if (msbc_info) {

    LOG_WARN("Re-initiating mSBC buffer that is active or not cleaned");
@@ -238,6 +307,7 @@ void init(size_t pkt_size) { 


void cleanup() {

  hfp_msbc_decoder_cleanup();

  hfp_msbc_encoder_cleanup();



  if (msbc_info == nullptr) return;
@@ -314,6 +384,53 @@ packet_loss: 
  return BTM_MSBC_CODE_SIZE;

}



size_t encode(int16_t* data, size_t len) {

  uint8_t* pkt_body = nullptr;

  uint32_t encoded_size = 0;

  if (msbc_info == nullptr) {

    LOG_WARN("mSBC buffer uninitialized or cleaned");

    return 0;

  }



  if (len < BTM_MSBC_CODE_SIZE) {

    LOG_DEBUG(

        "PCM frames with size %lu is insufficient to be encoded into a mSBC "

        "packet",

        (unsigned long)len);

    return 0;

  }



  pkt_body = msbc_info->fill_msbc_pkt_template();

  if (pkt_body == nullptr) {

    LOG_DEBUG("Failed to fill the template to fill the mSBC packet");

    return 0;

  }



  encoded_size = hfp_msbc_encode_frames(data, pkt_body);

  if (encoded_size != BTM_MSBC_PKT_FRAME_LEN) {

    LOG_WARN("Encoding invalid packet size: %lu", (unsigned long)encoded_size);

    std::copy(std::begin(btm_msbc_zero_packet), std::end(btm_msbc_zero_packet),

              pkt_body);

  }



  return BTM_MSBC_CODE_SIZE;

}



size_t dequeue_packet(const uint8_t** output) {

  if (msbc_info == nullptr) {

    LOG_WARN("mSBC buffer uninitialized or cleaned");

    return 0;

  }



  *output = msbc_info->sco_pkt_read_ptr();

  if (*output == nullptr) {

    LOG_DEBUG("Insufficient data to dequeue.");

    return 0;

  }



  return msbc_info->mark_pkt_dequeued();

}



}  // namespace wbs



}  // namespace sco

system/stack/btm/hfp_msbc_decoder.cc

+1 −0
Original line number Diff line number Diff line
@@ -60,6 +60,7 @@ void hfp_msbc_decoder_cleanup(void) { 
  memset(&hfp_msbc_decoder, 0, sizeof(hfp_msbc_decoder));

}



// Get the HFP MSBC encoded maximum frame size

bool hfp_msbc_decoder_decode_packet(const uint8_t* i_buf,

                                    const uint8_t** o_buf) {

  const OI_BYTE* oi_data;

system/stack/btm/hfp_msbc_encoder.cc

+0 −1
Original line number Diff line number Diff line
@@ -43,7 +43,6 @@ void hfp_msbc_encoder_init(void) { 


void hfp_msbc_encoder_cleanup(void) { hfp_msbc_encoder = {}; }



// Get the HFP MSBC encoded maximum frame size

uint32_t hfp_msbc_encode_frames(int16_t* input, uint8_t* output) {

  return SBC_Encode(&hfp_msbc_encoder.sbc_encoder_params, input, output);

}