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

Commit 33c73fa0 authored by En-Shuo Hsu's avatar En-Shuo Hsu Committed by Gerrit Code Review
Browse files

Merge changes Ifd32a4f1,I76b11292 

* changes:
  floss: Trigger PLC when reading corrupted packet
  floss: Refine to use osi_free_and_reset
parents 046314d9 efcee08a

system/stack/btm/btm_sco.cc

+5 −1
Original line number Diff line number Diff line
@@ -251,7 +251,11 @@ void btm_route_sco_data(BT_HDR* p_msg) { 
  const uint8_t* decoded = nullptr;

  size_t written = 0, rc = 0;

  if (active_sco->is_wbs()) {

    rc = bluetooth::audio::sco::wbs::enqueue_packet(payload, data_len);

    uint16_t status = HCID_GET_PKT_STATUS(handle_with_flags);



    if (status > 0) LOG_DEBUG("Packet corrupted with status(0x%X)", status);

    rc = bluetooth::audio::sco::wbs::enqueue_packet(payload, data_len,

                                                    status > 0);

    if (rc != data_len) LOG_DEBUG("Failed to enqueue packet");



    while (rc) {

system/stack/btm/btm_sco.h

+11 −1
Original line number Diff line number Diff line
@@ -64,15 +64,25 @@ size_t init(size_t pkt_size); 
/* Clean up when the SCO connection is done */

void cleanup();



/* Fill in packet loss stats

 * Args:

 *    num_decoded_frames - Output argument for the number of decode frames

 *    packet_loss_ratio - Output argument for the ratio of lost frames

 * Returns:

 *    False for invalid arguments or unreasonable stats. True otherwise.

 */

bool fill_plc_stats(int* num_decoded_frames, double* packet_loss_ratio);



/* Try to enqueue a packet to a buffer.

 * Args:

 *    data - Pointer to received packet data bytes.

 *    pkt_size - Length of input packet. Passing packet with inconsistent size

 *        from the pkt_size set in init() will fail the call.

 *    corrupted - If the current mSBC packet read is corrupted.

 * Returns:

 *    The length of enqueued bytes. 0 if failed.

 */

size_t enqueue_packet(const uint8_t* data, size_t pkt_size);

size_t enqueue_packet(const uint8_t* data, size_t pkt_size, bool corrupted);



/* Try to decode mSBC frames from the packets in the buffer.

 * Args:

system/stack/btm/btm_sco_hci.cc

+43 −5
Original line number Diff line number Diff line
@@ -224,6 +224,9 @@ struct tBTM_MSBC_PLC { 
                    BTM_PLC_WINDOW_SIZE of packets. We use this to determine if

                    we want to disable the PLC temporarily */



  int num_decoded_frames; /* Number of total read mSBC frames. */

  int num_lost_frames;    /* Number of total lost mSBC frames. */



  void overlap_add(int16_t* output, float scaler_d, const int16_t* desc,

                   float scaler_a, const int16_t* asc) {

    for (int i = 0; i < BTM_PLC_OLAL; i++) {
@@ -278,14 +281,21 @@ struct tBTM_MSBC_PLC { 
  }



  void deinit() {

    if (pl_window) osi_free(pl_window);

    if (pl_window) osi_free_and_reset((void**)&pl_window);

  }



  int get_num_decoded_frames() { return num_decoded_frames; }



  int get_num_lost_frames() { return num_lost_frames; }



  void handle_bad_frames(const uint8_t** output) {

    float scaler;

    int16_t* best_match_hist;

    int16_t* frame_head = &hist[BTM_PLC_HL];



    num_decoded_frames++;

    num_lost_frames++;



    /* mSBC codec is stateful, the history of signal would contribute to the

     * decode result decoded_buffer. This should never fail. */

    GetInterfaceToProfiles()->msbcCodec->decodePacket(
@@ -350,6 +360,7 @@ struct tBTM_MSBC_PLC { 


  void handle_good_frames(int16_t* input) {

    int16_t* frame_head;

    num_decoded_frames++;

    if (handled_bad_frames != 0) {

      /* If there was a packet concealment before this good frame, we need to

       * reconverge the input frames */
@@ -380,6 +391,7 @@ struct tBTM_MSBC_INFO { 
  uint8_t* msbc_decode_buf; /* Buffer to store mSBC packets to decode */

  size_t decode_buf_wo;     /* Write offset of the decode buffer */

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

  bool read_corrupted;      /* If the current mSBC packet read is corrupted */



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

  size_t encode_buf_wo;     /* Write offset of the encode buffer */
@@ -450,7 +462,7 @@ struct tBTM_MSBC_INFO { 
    if (msbc_encode_buf) osi_free(msbc_encode_buf);

    if (plc) {

      plc->deinit();

      osi_free(plc);

      osi_free_and_reset((void**)&plc);

    }

  }
@@ -480,6 +492,12 @@ struct tBTM_MSBC_INFO { 
  }



  const uint8_t* find_msbc_pkt_head() {

    if (read_corrupted) {

      LOG_WARN("Skip corrupted mSBC packets");

      read_corrupted = false;

      return nullptr;

    }



    size_t rp = 0;

    while (rp < BTM_MSBC_PKT_LEN &&

           decode_buf_wo - (decode_buf_ro + rp) >= BTM_MSBC_PKT_LEN) {
@@ -569,11 +587,30 @@ void cleanup() { 
  if (msbc_info == nullptr) return;



  msbc_info->deinit();

  osi_free(msbc_info);

  msbc_info = nullptr;

  osi_free_and_reset((void**)&msbc_info);

}



bool fill_plc_stats(int* num_decoded_frames, double* packet_loss_ratio) {

  if (msbc_info == NULL || num_decoded_frames == NULL ||

      packet_loss_ratio == NULL)

    return false;



  int decoded_frames = msbc_info->plc->get_num_decoded_frames();

  int lost_frames = msbc_info->plc->get_num_lost_frames();

  if (decoded_frames <= 0 || lost_frames < 0 || lost_frames > decoded_frames) {

    LOG_WARN(

        "Unreasonable reported frame count: decoded_frames(%d), "

        "lost_frames(%d)",

        decoded_frames, lost_frames);

    return false;

  }



  *num_decoded_frames = decoded_frames;

  *packet_loss_ratio = (double)lost_frames / decoded_frames;

  return true;

}



size_t enqueue_packet(const uint8_t* data, size_t pkt_size) {

size_t enqueue_packet(const uint8_t* data, size_t pkt_size, bool corrupted) {

  if (msbc_info == nullptr) {

    LOG_WARN("mSBC buffer uninitialized or cleaned");

    return 0;
@@ -592,6 +629,7 @@ size_t enqueue_packet(const uint8_t* data, size_t pkt_size) { 
    return 0;

  }



  msbc_info->read_corrupted |= corrupted;

  if (msbc_info->write(data, pkt_size) != pkt_size) {

    LOG_DEBUG("Fail to write packet with size %lu to buffer",

              (unsigned long)pkt_size);

system/stack/include/hcimsgs.h

+3 −0
Original line number Diff line number Diff line
@@ -284,12 +284,15 @@ extern void btsnd_hcic_enhanced_accept_synchronous_connection( 
    const RawAddress& bd_addr, enh_esco_params_t* p_parms);



#define HCI_DATA_HANDLE_MASK 0x0FFF

#define HCI_DATA_PKT_STATUS_MASK 0x3000



#define HCID_GET_HANDLE_EVENT(p)                     \

  (uint16_t)((*((uint8_t*)((p) + 1) + (p)->offset) + \

              (*((uint8_t*)((p) + 1) + (p)->offset + 1) << 8)))



#define HCID_GET_HANDLE(u16) (uint16_t)((u16)&HCI_DATA_HANDLE_MASK)

#define HCID_GET_PKT_STATUS(u16) \

  (uint16_t)(((u16)&HCI_DATA_PKT_STATUS_MASK) >> 12)



#define HCI_DATA_EVENT_MASK 3

#define HCI_DATA_EVENT_OFFSET 12

system/stack/test/btm/sco_hci_test.cc

+71 −18
Original line number Diff line number Diff line
@@ -191,25 +191,27 @@ TEST_F(ScoHciWbsTest, WbsInit) { 
TEST_F(ScoHciWbsTest, WbsEnqueuePacketWithoutInit) {

  uint8_t payload[60];

  // Return 0 if buffer is uninitialized

  ASSERT_EQ(

      bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload)),

  ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload),

                                                       false),

            size_t(0));

}



TEST_F(ScoHciWbsWithInitCleanTest, WbsEnqueuePacket) {

  uint8_t payload[60];

  // Return 0 if payload is invalid

  ASSERT_EQ(

      bluetooth::audio::sco::wbs::enqueue_packet(nullptr, sizeof(payload)),

  ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(nullptr, sizeof(payload),

                                                       false),

            size_t(0));

  // Return 0 if packet size is consistent

  ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(payload, 72), size_t(0));

  ASSERT_EQ(

      bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload)),

      bluetooth::audio::sco::wbs::enqueue_packet(payload, size_t(72), false),

      size_t(0));

  ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload),

                                                       false),

            size_t(60));

  // Return 0 if buffer is full

  ASSERT_EQ(

      bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload)),

  ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload),

                                                       false),

            size_t(0));

}
@@ -228,8 +230,8 @@ TEST_F(ScoHciWbsWithInitCleanTest, WbsDecode) { 
  ASSERT_EQ(bluetooth::audio::sco::wbs::decode(&decoded), size_t(0));

  ASSERT_EQ(decoded, nullptr);

  // Fill in invalid packet, all zeros.

  ASSERT_EQ(

      bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload)),

  ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(payload, sizeof(payload),

                                                       false),

            sizeof(payload));



  // Return all zero frames when there comes an invalid packet.
@@ -243,7 +245,7 @@ TEST_F(ScoHciWbsWithInitCleanTest, WbsDecode) { 


  decoded = nullptr;

  ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(msbc_zero_packet,

                                                       sizeof(payload)),

                                                       sizeof(payload), false),

            sizeof(msbc_zero_packet));

  ASSERT_EQ(bluetooth::audio::sco::wbs::decode(&decoded),

            size_t(BTM_MSBC_CODE_SIZE));
@@ -337,7 +339,7 @@ TEST_F(ScoHciWbsWithInitCleanTest, WbsPlc) { 
    ASSERT_NE(encoded, nullptr);



    // Simulate the reception of the packet

    ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(encoded, 60),

    ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(encoded, 60, false),

              size_t(60));

    ASSERT_EQ(bluetooth::audio::sco::wbs::decode(&decoded),

              size_t(BTM_MSBC_CODE_SIZE));
@@ -349,6 +351,7 @@ TEST_F(ScoHciWbsWithInitCleanTest, WbsPlc) { 
  // Start with the fresh WBS buffer

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

  bluetooth::audio::sco::wbs::init(60);

  int decode_count = 0;

  for (size_t i = 0, sample_idx = 0; i <= lost_pkt_idx; i++) {

    // Data is a 1000Hz triangle wave

    for (size_t j = 0; j < 120; j++, sample_idx++)
@@ -360,12 +363,22 @@ TEST_F(ScoHciWbsWithInitCleanTest, WbsPlc) { 


    // Substitute to invalid packet to simulate packet loss.

    ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(

                  i != lost_pkt_idx ? encoded : invalid_pkt, 60),

                  i != lost_pkt_idx ? encoded : invalid_pkt, 60, false),

              size_t(60));

    ASSERT_EQ(bluetooth::audio::sco::wbs::decode(&decoded),

              size_t(BTM_MSBC_CODE_SIZE));

    decode_count++;

    ASSERT_NE(decoded, nullptr);

  }

  int num_decoded_frames;

  double packet_loss_ratio;



  ASSERT_EQ(bluetooth::audio::sco::wbs::fill_plc_stats(&num_decoded_frames,

                                                       &packet_loss_ratio),

            true);

  ASSERT_EQ(num_decoded_frames, decode_count);

  ASSERT_EQ(packet_loss_ratio, (double)1 / decode_count);



  int16_t* ptr = (int16_t*)decoded;

  for (size_t i = 0; i < 120; i++) {

    // The frames generated by PLC won't be perfect due to:
@@ -375,6 +388,46 @@ TEST_F(ScoHciWbsWithInitCleanTest, WbsPlc) { 
        << "PLC data " << ptr[i] << " deviates from expected " << expect_data[i]

        << " at index " << i;

  }



  size_t corrupted_pkt_idx = lost_pkt_idx;

  // Start with the fresh WBS buffer

  decode_count = 0;

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

  bluetooth::audio::sco::wbs::init(60);

  for (size_t i = 0, sample_idx = 0; i <= corrupted_pkt_idx; i++) {

    // Data is a 1000Hz triangle wave

    for (size_t j = 0; j < 120; j++, sample_idx++)

      data[j] = triangle[sample_idx % 16];

    ASSERT_EQ(bluetooth::audio::sco::wbs::encode(data, sizeof(data)),

              sizeof(data));

    ASSERT_EQ(bluetooth::audio::sco::wbs::dequeue_packet(&encoded), size_t(60));

    ASSERT_NE(encoded, nullptr);



    // Substitute to report packet corrupted to simulate packet loss.

    ASSERT_EQ(bluetooth::audio::sco::wbs::enqueue_packet(

                  encoded, 60, i == corrupted_pkt_idx),

              size_t(60));

    ASSERT_EQ(bluetooth::audio::sco::wbs::decode(&decoded),

              size_t(BTM_MSBC_CODE_SIZE));

    decode_count++;

    ASSERT_NE(decoded, nullptr);

  }



  ASSERT_EQ(bluetooth::audio::sco::wbs::fill_plc_stats(&num_decoded_frames,

                                                       &packet_loss_ratio),

            true);

  ASSERT_EQ(num_decoded_frames, decode_count);

  ASSERT_EQ(packet_loss_ratio, (double)1 / decode_count);



  ptr = (int16_t*)decoded;

  for (size_t i = 0; i < 120; i++) {

    // The frames generated by PLC won't be perfect due to:

    // 1. mSBC decoder is statefull

    // 2. We apply overlap-add to glue the frames when packet loss happens

    ASSERT_THAT(ptr[i] - expect_data[i], AllOf(Ge(-3), Le(3)))

        << "PLC data " << ptr[i] << " deviates from expected " << expect_data[i]

        << " at index " << i;

  }

}



}  // namespace