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

Commit 893bf0bd authored by jruthe's avatar jruthe Committed by android-build-merger
Browse files

Merge "test_vendor: Implement L2CAP packet fragmentation" am: 9b0bc2be am:... 

Merge "test_vendor: Implement L2CAP packet fragmentation" am: 9b0bc2be am: 71f5f034 am: ce72c03b
am: ca88745c

Change-Id: Id548a020d1260652e524875702a82208b32b008b
parents 9a9d7867 ca88745c

system/vendor_libs/test_vendor_lib/include/l2cap_packet.h

+24 −3
Original line number Original line Diff line number Diff line
@@ -17,6 +17,7 @@ 
 ******************************************************************************/

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

#pragma once

#pragma once





#include <cmath>

#include <cstdint>

#include <cstdint>

#include <iterator>

#include <iterator>

#include <memory>

#include <memory>
@@ -42,6 +43,11 @@ class L2capPacket { 




  uint16_t get_l2cap_cid() const;

  uint16_t get_l2cap_cid() const;





  // Returns a fragmented vector of L2capSdu objects if successful

  // Returns an empty vector of L2capSdu objects if unsuccessful

  std::vector<L2capSdu> fragment(uint16_t maximum_sdu_size, uint8_t txseq,

                                 uint8_t reqseq) const;



 private:

 private:

  L2capPacket() = default;

  L2capPacket() = default;
@@ -49,11 +55,26 @@ class L2capPacket { 
  std::vector<uint8_t> l2cap_packet_;

  std::vector<uint8_t> l2cap_packet_;





  // Returns an iterator to the beginning of the L2CAP payload on success.

  // Returns an iterator to the beginning of the L2CAP payload on success.

  auto get_l2cap_payload_begin() const {

  std::vector<uint8_t>::const_iterator get_l2cap_payload_begin() const;

    return std::next(l2cap_packet_.begin(), kSduHeaderLength);

  }





  DISALLOW_COPY_AND_ASSIGN(L2capPacket);

  DISALLOW_COPY_AND_ASSIGN(L2capPacket);



  // Returns an iterator to the end of the L2CAP payload.

  std::vector<uint8_t>::const_iterator get_l2cap_payload_end() const;



  // Helper functions for fragmenting.

  static void set_sdu_header_length(std::vector<uint8_t>& sdu, uint16_t length);



  static void set_total_sdu_length(std::vector<uint8_t>& sdu,

                                   uint16_t total_sdu_length);



  static void set_sdu_cid(std::vector<uint8_t>& sdu, uint16_t cid);



  static void set_sdu_control_bytes(std::vector<uint8_t>& sdu, uint8_t txseq,

                                    uint8_t reqseq);



  bool check_l2cap_packet() const;



};  // L2capPacket

};  // L2capPacket





}  // namespace test_vendor_lib
 
}  // namespace test_vendor_lib

system/vendor_libs/test_vendor_lib/include/l2cap_sdu.h

+12 −5
Original line number Original line Diff line number Diff line
@@ -62,24 +62,31 @@ class L2capSdu { 
  // Returns a completed L2capSdu object.

  // Returns a completed L2capSdu object.

  L2capSdu(std::vector<uint8_t> create_from);

  L2capSdu(std::vector<uint8_t> create_from);





  // Adds an FCS to create_from and returns an L2capSdu object

  static L2capSdu L2capSduBuilder(std::vector<uint8_t> create_from);

  static L2capSdu L2capSduBuilder(std::vector<uint8_t> create_from);





  // TODO: Remove this when the move to L2capSdu* is done

  L2capSdu& operator=(L2capSdu obj1) {

    sdu_data_.clear();



    sdu_data_ = obj1.sdu_data_;



    return *this;

  }



  // Get a vector iterator that points to the first byte of the

  // Get a vector iterator that points to the first byte of the

  // L2CAP payload within an SDU. The offset parameter will be the

  // L2CAP payload within an SDU. The offset parameter will be the

  // number of bytes that are in the SDU header. This should always

  // number of bytes that are in the SDU header. This should always

  // be 6 bytes with the exception being the first SDU of a stream

  // be 6 bytes with the exception being the first SDU of a stream

  // of SDU packets where the first SDU packet will have an extra

  // of SDU packets where the first SDU packet will have an extra

  // two bytes and the offset should be 8 bytes.

  // two bytes and the offset should be 8 bytes.

  auto get_payload_begin(const unsigned int offset) const {

  std::vector<uint8_t>::const_iterator get_payload_begin(

    return std::next(sdu_data_.begin(), offset);

      const unsigned int offset) const;

  }





  // Get a vector iterator that points to the last bytes of the

  // Get a vector iterator that points to the last bytes of the

  // L2CAP payload within an SDU packet. There is no offset

  // L2CAP payload within an SDU packet. There is no offset

  // parameter for this function because there will always be two

  // parameter for this function because there will always be two

  // FCS bytes and nothing else at the end of each SDU.

  // FCS bytes and nothing else at the end of each SDU.

  auto get_payload_end() const { return std::prev(sdu_data_.end(), 2); }

  std::vector<uint8_t>::const_iterator get_payload_end() const;





  // Get the FCS bytes from the end of the L2CAP payload of an SDU

  // Get the FCS bytes from the end of the L2CAP payload of an SDU

  // packet.

  // packet.

system/vendor_libs/test_vendor_lib/src/l2cap_packet.cc

+155 −5
Original line number Original line Diff line number Diff line
@@ -26,9 +26,12 @@ 
namespace test_vendor_lib {

namespace test_vendor_lib {





const int kL2capHeaderLength = 4;

const int kL2capHeaderLength = 4;

const uint16_t kSduTxSeqBits = 0x007e;

const uint16_t kSduTxSeqBits = 0x007E;

const int kSduStandardHeaderLength = 6;

const int kSduStandardHeaderLength = 6;

const int kSduFirstHeaderLength = 8;

const int kSduFirstHeaderLength = 8;

const uint8_t kSduFirstReqseq = 0x40;

const uint8_t kSduContinuationReqseq = 0xC0;

const uint8_t kSduEndReqseq = 0x80;





std::unique_ptr<L2capPacket> L2capPacket::assemble(

std::unique_ptr<L2capPacket> L2capPacket::assemble(

    const std::vector<L2capSdu>& sdu_packets) {

    const std::vector<L2capSdu>& sdu_packets) {
@@ -143,11 +146,11 @@ std::unique_ptr<L2capPacket> L2capPacket::assemble( 
    return nullptr;

    return nullptr;

  }

  }





  built_l2cap_packet->l2cap_packet_[0] = l2cap_payload_length & 0xff;

  built_l2cap_packet->l2cap_packet_[0] = l2cap_payload_length & 0xFF;

  built_l2cap_packet->l2cap_packet_[1] = (l2cap_payload_length & 0xff00) >> 8;

  built_l2cap_packet->l2cap_packet_[1] = (l2cap_payload_length & 0xFF00) >> 8;

  built_l2cap_packet->l2cap_packet_[2] = first_packet_channel_id & 0xff;

  built_l2cap_packet->l2cap_packet_[2] = first_packet_channel_id & 0xFF;

  built_l2cap_packet->l2cap_packet_[3] =

  built_l2cap_packet->l2cap_packet_[3] =

      (first_packet_channel_id & 0xff00) >> 8;

      (first_packet_channel_id & 0xFF00) >> 8;





  return built_l2cap_packet;

  return built_l2cap_packet;

}  // Assemble

}  // Assemble
@@ -167,4 +170,151 @@ uint16_t L2capPacket::get_l2cap_cid() const { 
  return ((l2cap_packet_[3] << 8) | l2cap_packet_[2]);

  return ((l2cap_packet_[3] << 8) | l2cap_packet_[2]);

}

}





std::vector<uint8_t>::const_iterator L2capPacket::get_l2cap_payload_begin()

    const {

  return std::next(l2cap_packet_.begin(), kSduHeaderLength);

}



std::vector<uint8_t>::const_iterator L2capPacket::get_l2cap_payload_end()

    const {

  return l2cap_packet_.end();

}



std::vector<L2capSdu> L2capPacket::fragment(uint16_t maximum_sdu_size,

                                            uint8_t txseq,

                                            uint8_t reqseq) const {

  std::vector<L2capSdu> sdu;

  if (!check_l2cap_packet()) return sdu;



  std::vector<uint8_t> current_sdu;



  auto current_iter = get_l2cap_payload_begin();

  auto end_iter = get_l2cap_payload_end();



  size_t number_of_packets = ceil((l2cap_packet_.size() - kL2capHeaderLength) /

                                  static_cast<float>(maximum_sdu_size));



  if (number_of_packets == 0) {

    current_sdu.resize(kSduStandardHeaderLength);



    set_sdu_header_length(current_sdu, kL2capHeaderLength);



    set_sdu_cid(current_sdu, get_l2cap_cid());



    reqseq = (reqseq & 0xF0) >> 4;

    set_sdu_control_bytes(current_sdu, txseq, reqseq);



    sdu.push_back(L2capSdu::L2capSduBuilder(current_sdu));



    return sdu;

  }



  uint16_t header_length = 0x0000;



  if (number_of_packets == 1) {

    current_sdu.clear();

    current_sdu.resize(kSduStandardHeaderLength);



    header_length = ((l2cap_packet_[1] & 0xFF) << 8) | l2cap_packet_[0];

    header_length += kL2capHeaderLength;

    set_sdu_header_length(current_sdu, header_length);



    set_sdu_cid(current_sdu, get_l2cap_cid());



    set_sdu_control_bytes(current_sdu, txseq, 0x00);



    current_sdu.insert(current_sdu.end(), current_iter, end_iter);



    sdu.push_back(L2capSdu::L2capSduBuilder(current_sdu));



    return sdu;

  }



  auto next_iter =

      std::next(current_iter, maximum_sdu_size - (kSduFirstHeaderLength + 2));



  sdu.reserve(number_of_packets);

  sdu.clear();



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

    if (i == 0) {

      current_sdu.resize(kSduFirstHeaderLength);



      header_length = maximum_sdu_size - kL2capHeaderLength;



      reqseq = reqseq | kSduFirstReqseq;



      set_total_sdu_length(current_sdu, l2cap_packet_.size() - 4);

    } else {

      current_sdu.resize(kSduStandardHeaderLength);



      header_length = (next_iter - current_iter) + kL2capHeaderLength;



      reqseq = reqseq & 0x0F;



      if (i < number_of_packets) {

        reqseq |= kSduContinuationReqseq;

      } else {

        reqseq |= kSduEndReqseq;

      }

    }

    set_sdu_header_length(current_sdu, header_length);



    set_sdu_cid(current_sdu, get_l2cap_cid());



    set_sdu_control_bytes(current_sdu, txseq, reqseq);

    txseq += 2;



    // Txseq has a maximum of 0x3F. If it exceeds that, it restarts at 0x00.

    if (txseq > 0x3F) txseq = 0x00;



    current_sdu.insert(current_sdu.end(), current_iter, next_iter);



    current_iter = next_iter;



    next_iter =

        std::next(current_iter, maximum_sdu_size - kSduFirstHeaderLength);



    if (next_iter > end_iter) {

      next_iter = end_iter;

    }



    sdu.push_back(L2capSdu::L2capSduBuilder(std::move(current_sdu)));

  }



  return sdu;

}  // fragment



void L2capPacket::set_sdu_header_length(std::vector<uint8_t>& sdu,

                                        uint16_t length) {

  sdu[0] = length & 0xFF;

  sdu[1] = (length & 0xFF00) >> 8;

}



void L2capPacket::set_total_sdu_length(std::vector<uint8_t>& sdu,

                                       uint16_t total_sdu_length) {

  sdu[6] = total_sdu_length & 0xFF;

  sdu[7] = (total_sdu_length & 0xFF00) >> 8;

}



void L2capPacket::set_sdu_cid(std::vector<uint8_t>& sdu, uint16_t cid) {

  sdu[2] = cid & 0xFF;

  sdu[3] = (cid & 0xFF00) >> 8;

}



void L2capPacket::set_sdu_control_bytes(std::vector<uint8_t>& sdu,

                                        uint8_t txseq, uint8_t reqseq) {

  sdu[4] = txseq;

  sdu[5] = reqseq;

}



bool L2capPacket::check_l2cap_packet() const {

  uint16_t payload_length = ((l2cap_packet_[1] & 0xFF) << 8) | l2cap_packet_[0];



  if (l2cap_packet_.size() < 4) return false;

  if (payload_length != (l2cap_packet_.size() - 4)) return false;



  return true;

}



}  // namespace test_vendor_lib
 
}  // namespace test_vendor_lib

system/vendor_libs/test_vendor_lib/src/l2cap_sdu.cc

+11 −3
Original line number Original line Diff line number Diff line
@@ -58,12 +58,11 @@ const uint16_t L2capSdu::lfsr_table_[256] = { 
};  // lfsr_table

};  // lfsr_table





L2capSdu::L2capSdu(std::vector<uint8_t> create_from) {

L2capSdu::L2capSdu(std::vector<uint8_t> create_from) {

  sdu_data_.clear();

  sdu_data_ = std::move(create_from);

  sdu_data_.insert(sdu_data_.end(), create_from.begin(), create_from.end());

}

}





L2capSdu L2capSdu::L2capSduBuilder(std::vector<uint8_t> create_from) {

L2capSdu L2capSdu::L2capSduBuilder(std::vector<uint8_t> create_from) {

  L2capSdu packet(create_from);

  L2capSdu packet(std::move(create_from));





  packet.sdu_data_.resize(packet.sdu_data_.size() + 2, 0x00);

  packet.sdu_data_.resize(packet.sdu_data_.size() + 2, 0x00);
@@ -75,6 +74,15 @@ L2capSdu L2capSdu::L2capSduBuilder(std::vector<uint8_t> create_from) { 
  return packet;

  return packet;

}

}





std::vector<uint8_t>::const_iterator L2capSdu::get_payload_begin(

    const unsigned int offset) const {

  return std::next(sdu_data_.begin(), offset);

}



std::vector<uint8_t>::const_iterator L2capSdu::get_payload_end() const {

  return std::prev(sdu_data_.end(), 2);

}



uint16_t L2capSdu::convert_from_little_endian(

uint16_t L2capSdu::convert_from_little_endian(

    const unsigned int starting_index) const {

    const unsigned int starting_index) const {

  uint16_t convert = sdu_data_[starting_index + 1];

  uint16_t convert = sdu_data_[starting_index + 1];