1 /* 2 * Copyright 2017 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 "l2cap_packet" 18 19 #include "l2cap_packet.h" 20 21 #include <algorithm> 22 23 #include "osi/include/log.h" 24 25 namespace test_vendor_lib { 26 27 const size_t kL2capHeaderLength = 4; 28 const size_t kSduFirstHeaderLength = 8; 29 const size_t kSduStandardHeaderLength = 6; 30 const size_t kFcsBytes = 2; 31 const uint16_t kSduTxSeqBits = 0x007E; 32 const uint8_t kSduFirstReqseq = 0x40; 33 const uint8_t kSduContinuationReqseq = 0xC0; 34 const uint8_t kSduEndReqseq = 0x80; 35 36 std::shared_ptr<L2capPacket> L2capPacket::assemble( 37 const std::vector<std::shared_ptr<L2capSdu> >& sdu_packets) { 38 std::shared_ptr<L2capPacket> built_l2cap_packet(new L2capPacket()); 39 uint16_t l2cap_payload_length = 0; 40 uint16_t first_packet_channel_id = 0; 41 uint16_t total_expected_l2cap_length; 42 uint8_t txseq_start; 43 44 if (sdu_packets.size() == 0) { 45 LOG_DEBUG(LOG_TAG, "%s: No SDU received.", __func__); 46 return nullptr; 47 } 48 if (sdu_packets.size() == 1 && 49 !L2capSdu::is_complete_l2cap(*sdu_packets[0])) { 50 LOG_DEBUG(LOG_TAG, "%s: Unsegmented SDU has incorrect SAR bits.", __func__); 51 return nullptr; 52 } 53 54 first_packet_channel_id = sdu_packets[0]->get_channel_id(); 55 56 built_l2cap_packet->l2cap_packet_.resize(kL2capHeaderLength); 57 58 for (size_t i = 0; i < sdu_packets.size(); i++) { 59 uint16_t payload_length = sdu_packets[i]->get_payload_length(); 60 61 // TODO(jruthe): Remove these checks when ACL packets have been 62 // implemented. Once those are done, that will be the only way to create 63 // L2capSdu objects and these checks will be moved there instead. 64 // 65 // Check the integrity of the packet length, if it is zero, it is invalid. 66 // The maximum size of a single, segmented L2CAP payload is 1016 bytes. 67 if ((payload_length <= 0) || 68 (payload_length != sdu_packets[i]->get_vector_size() - 4)) { 69 LOG_DEBUG(LOG_TAG, "%s: SDU payload length incorrect.", __func__); 70 return nullptr; 71 } 72 73 uint16_t fcs_check = sdu_packets[i]->get_fcs(); 74 75 if (sdu_packets[i]->calculate_fcs() != fcs_check) { 76 LOG_DEBUG(LOG_TAG, "%s: SDU fcs is incorrect.", __func__); 77 return nullptr; 78 } 79 80 uint16_t controls = sdu_packets[i]->get_controls(); 81 82 if (sdu_packets[i]->get_channel_id() != first_packet_channel_id) { 83 LOG_DEBUG(LOG_TAG, "%s: SDU CID does not match expected.", __func__); 84 return nullptr; 85 } 86 87 if (i == 0) txseq_start = controls & kSduTxSeqBits; 88 89 // Bluetooth Specification version 4.2 volume 3 part A 3.3.2: 90 // If there is only a single SDU, the first two bits of the control must be 91 // set to 00b, representing an unsegmented SDU. If the SDU is segmented, 92 // there is a begin and an end. The first segment must have the first two 93 // control bits set to 01b and the ending segment must have them set to 10b. 94 // Meanwhile all segments in between the start and end must have the bits 95 // set to 11b. 96 size_t starting_index; 97 uint8_t txseq = controls & kSduTxSeqBits; 98 if (sdu_packets.size() > 1 && i == 0 && 99 !L2capSdu::is_starting_sdu(*sdu_packets[i])) { 100 LOG_DEBUG(LOG_TAG, "%s: First segmented SDU has incorrect SAR bits.", 101 __func__); 102 return nullptr; 103 } 104 if (i != 0 && L2capSdu::is_starting_sdu(*sdu_packets[i])) { 105 LOG_DEBUG(LOG_TAG, 106 "%s: SAR bits set to first segmented SDU on " 107 "non-starting SDU.", 108 __func__); 109 return nullptr; 110 } 111 if (txseq != (txseq_start + (static_cast<uint8_t>(i) << 1))) { 112 LOG_DEBUG(LOG_TAG, "%s: TxSeq incorrect.", __func__); 113 return nullptr; 114 } 115 if (sdu_packets.size() > 1 && i == sdu_packets.size() - 1 && 116 !L2capSdu::is_ending_sdu(*sdu_packets[i])) { 117 LOG_DEBUG(LOG_TAG, "%s: Final segmented SDU has incorrect SAR bits.", 118 __func__); 119 return nullptr; 120 } 121 122 // Subtract the control and fcs from every SDU payload length. 123 l2cap_payload_length += (payload_length - 4); 124 125 if (L2capSdu::is_starting_sdu(*sdu_packets[i])) { 126 starting_index = kSduFirstHeaderLength; 127 total_expected_l2cap_length = sdu_packets[i]->get_total_l2cap_length(); 128 129 // Subtract the additional two bytes from the first packet of a segmented 130 // SDU. 131 l2cap_payload_length -= 2; 132 } else { 133 starting_index = kSduStandardHeaderLength; 134 } 135 136 Iterator payload_begin = sdu_packets[i]->get_begin() + starting_index; 137 Iterator payload_end = sdu_packets[i]->get_end() - kFcsBytes; 138 139 built_l2cap_packet->l2cap_packet_.insert( 140 built_l2cap_packet->l2cap_packet_.end(), payload_begin, payload_end); 141 } 142 143 if (l2cap_payload_length != total_expected_l2cap_length && 144 sdu_packets.size() > 1) { 145 LOG_DEBUG(LOG_TAG, "%s: Total expected L2CAP payload length incorrect.", 146 __func__); 147 return nullptr; 148 } 149 150 built_l2cap_packet->l2cap_packet_[0] = l2cap_payload_length & 0xFF; 151 built_l2cap_packet->l2cap_packet_[1] = (l2cap_payload_length & 0xFF00) >> 8; 152 built_l2cap_packet->l2cap_packet_[2] = first_packet_channel_id & 0xFF; 153 built_l2cap_packet->l2cap_packet_[3] = 154 (first_packet_channel_id & 0xFF00) >> 8; 155 156 return built_l2cap_packet; 157 } // Assemble 158 159 uint16_t L2capPacket::get_l2cap_cid() const { 160 return ((l2cap_packet_[3] << 8) | l2cap_packet_[2]); 161 } 162 163 std::vector<std::shared_ptr<L2capSdu> > L2capPacket::fragment( 164 uint16_t maximum_sdu_size, uint8_t txseq, uint8_t reqseq) { 165 std::vector<std::shared_ptr<L2capSdu> > sdu; 166 if (!check_l2cap_packet()) return sdu; 167 168 std::vector<uint8_t> current_sdu; 169 170 Iterator current_iter = get_begin() + kL2capHeaderLength; 171 Iterator end_iter = get_end(); 172 173 size_t number_of_packets = ceil((l2cap_packet_.size() - kL2capHeaderLength) / 174 static_cast<float>(maximum_sdu_size)); 175 176 if (number_of_packets == 0) { 177 current_sdu.resize(kSduStandardHeaderLength); 178 179 set_sdu_header_length(current_sdu, kL2capHeaderLength); 180 181 set_sdu_cid(current_sdu, get_l2cap_cid()); 182 183 reqseq = (reqseq & 0xF0) >> 4; 184 set_sdu_control_bytes(current_sdu, txseq, reqseq); 185 186 sdu.push_back(L2capSdu::L2capSduBuilder(current_sdu)); 187 188 return sdu; 189 } 190 191 uint16_t header_length = 0x0000; 192 193 if (number_of_packets == 1) { 194 current_sdu.clear(); 195 current_sdu.resize(kSduStandardHeaderLength); 196 197 header_length = ((l2cap_packet_[1] & 0xFF) << 8) | l2cap_packet_[0]; 198 header_length += kL2capHeaderLength; 199 set_sdu_header_length(current_sdu, header_length); 200 201 set_sdu_cid(current_sdu, get_l2cap_cid()); 202 203 set_sdu_control_bytes(current_sdu, txseq, 0x00); 204 205 current_sdu.insert(current_sdu.end(), current_iter, end_iter); 206 207 sdu.push_back(L2capSdu::L2capSduBuilder(current_sdu)); 208 209 return sdu; 210 } 211 212 Iterator next_iter = 213 current_iter + (maximum_sdu_size - (kSduFirstHeaderLength + 2)); 214 215 sdu.reserve(number_of_packets); 216 sdu.clear(); 217 218 for (size_t i = 0; i <= number_of_packets; i++) { 219 if (i == 0) { 220 current_sdu.resize(kSduFirstHeaderLength); 221 222 header_length = maximum_sdu_size - kL2capHeaderLength; 223 224 reqseq = reqseq | kSduFirstReqseq; 225 226 set_total_sdu_length(current_sdu, l2cap_packet_.size() - 4); 227 } else { 228 current_sdu.resize(kSduStandardHeaderLength); 229 230 header_length = (next_iter - current_iter) + kL2capHeaderLength; 231 232 reqseq = reqseq & 0x0F; 233 234 if (i < number_of_packets) { 235 reqseq |= kSduContinuationReqseq; 236 } else { 237 reqseq |= kSduEndReqseq; 238 } 239 } 240 set_sdu_header_length(current_sdu, header_length); 241 242 set_sdu_cid(current_sdu, get_l2cap_cid()); 243 244 set_sdu_control_bytes(current_sdu, txseq, reqseq); 245 txseq += 2; 246 247 // Txseq has a maximum of 0x3F. If it exceeds that, it restarts at 0x00. 248 if (txseq > 0x3F) txseq = 0x00; 249 250 current_sdu.insert(current_sdu.end(), current_iter, next_iter); 251 current_iter = next_iter; 252 253 next_iter = current_iter + (maximum_sdu_size - kSduFirstHeaderLength); 254 255 sdu.push_back(L2capSdu::L2capSduBuilder(std::move(current_sdu))); 256 } 257 258 return sdu; 259 } // fragment 260 261 void L2capPacket::set_sdu_header_length(std::vector<uint8_t>& sdu, 262 uint16_t length) { 263 sdu[0] = length & 0xFF; 264 sdu[1] = (length & 0xFF00) >> 8; 265 } 266 267 void L2capPacket::set_total_sdu_length(std::vector<uint8_t>& sdu, 268 uint16_t total_sdu_length) { 269 sdu[6] = total_sdu_length & 0xFF; 270 sdu[7] = (total_sdu_length & 0xFF00) >> 8; 271 } 272 273 void L2capPacket::set_sdu_cid(std::vector<uint8_t>& sdu, uint16_t cid) { 274 sdu[2] = cid & 0xFF; 275 sdu[3] = (cid & 0xFF00) >> 8; 276 } 277 278 void L2capPacket::set_sdu_control_bytes(std::vector<uint8_t>& sdu, 279 uint8_t txseq, uint8_t reqseq) { 280 sdu[4] = txseq; 281 sdu[5] = reqseq; 282 } 283 284 bool L2capPacket::check_l2cap_packet() const { 285 uint16_t payload_length = ((l2cap_packet_[1] & 0xFF) << 8) | l2cap_packet_[0]; 286 287 if (l2cap_packet_.size() < 4) return false; 288 if (payload_length != (l2cap_packet_.size() - 4)) return false; 289 290 return true; 291 } 292 293 // HciPacket Functions 294 size_t L2capPacket::get_length() { return l2cap_packet_.size(); } 295 296 uint8_t& L2capPacket::get_at_index(size_t index) { 297 return l2cap_packet_[index]; 298 } 299 300 } // namespace test_vendor_lib 301
