1 /******************************************************************************
2 *
3 * Copyright 2014 Google, Inc.
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18
19 #define LOG_TAG "bt_hci_packet_fragmenter"
20
21 #include "packet_fragmenter.h"
22
23 #include <base/logging.h>
24 #include <string.h>
25
26 #include <unordered_map>
27
28 #include "bt_target.h"
29 #include "check.h"
30 #include "device/include/controller.h"
31 #include "hci/include/buffer_allocator.h"
32 #include "osi/include/log.h"
33 #include "osi/include/osi.h"
34 #include "stack/include/bt_hdr.h"
35
36 // 2 bytes for handle, 2 bytes for data length (Volume 2, Part E, 5.4.2)
37 #define HCI_ACL_PREAMBLE_SIZE 4
38
39 #define HCI_ISO_BF_FIRST_FRAGMENTED_PACKET (0)
40 #define HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET (1)
41 #define HCI_ISO_BF_COMPLETE_PACKET (2)
42 #define HCI_ISO_BF_LAST_FRAGMENT_PACKET (3)
43
44 #define HCI_ISO_HEADER_TIMESTAMP_SIZE (4)
45 #define HCI_ISO_HEADER_ISO_LEN_SIZE (2)
46 #define HCI_ISO_HEADER_PACKET_SEQ_SIZE (2)
47
48 // ISO
49 // 2 bytes for handle, 2 bytes for data length (Volume 2, Part E, 5.4.5)
50 #define HCI_ISO_PREAMBLE_SIZE 4
51
52 #define HCI_ISO_HEADER_LEN_WITHOUT_TS \
53 (HCI_ISO_HEADER_ISO_LEN_SIZE + HCI_ISO_HEADER_PACKET_SEQ_SIZE)
54 #define HCI_ISO_HEADER_LEN_WITH_TS \
55 (HCI_ISO_HEADER_LEN_WITHOUT_TS + HCI_ISO_HEADER_TIMESTAMP_SIZE)
56
57 #define HCI_ISO_SET_CONTINUATION_FLAG(handle) \
58 (((handle)&0x4FFF) | (0x0001 << 12))
59 #define HCI_ISO_SET_COMPLETE_FLAG(handle) (((handle)&0x4FFF) | (0x0002 << 12))
60 #define HCI_ISO_SET_END_FRAG_FLAG(handle) (((handle)&0x4FFF) | (0x0003 << 12))
61 #define HCI_ISO_SET_TIMESTAMP_FLAG(handle) (((handle)&0x3FFF) | (0x0001 << 14))
62
63 #define HCI_ISO_GET_TS_FLAG(handle) (((handle) >> 14) & 0x0001)
64 #define HCI_ISO_GET_PACKET_STATUS_FLAGS(iso_sdu_length) \
65 (iso_sdu_length & 0xC000)
66 #define HCI_ISO_SDU_LENGTH_MASK 0x0FFF
67
68 #define APPLY_CONTINUATION_FLAG(handle) (((handle)&0xCFFF) | 0x1000)
69 #define APPLY_START_FLAG(handle) (((handle)&0xCFFF) | 0x2000)
70 #define SUB_EVENT(event) ((event)&MSG_SUB_EVT_MASK)
71 #define GET_BOUNDARY_FLAG(handle) (((handle) >> 12) & 0x0003)
72 #define GET_BROADCAST_FLAG(handle) (((handle) >> 14) & 0x0003)
73
74 #define HANDLE_MASK 0x0FFF
75 #define START_PACKET_BOUNDARY 2
76 #define POINT_TO_POINT 0
77 #define L2CAP_HEADER_PDU_LEN_SIZE 2
78 #define L2CAP_HEADER_CID_SIZE 2
79 #define L2CAP_HEADER_SIZE (L2CAP_HEADER_PDU_LEN_SIZE + L2CAP_HEADER_CID_SIZE)
80
81 // Our interface and callbacks
82
83 static const allocator_t* buffer_allocator;
84 static const controller_t* controller;
85 static const packet_fragmenter_callbacks_t* callbacks;
86
87 static std::unordered_map<uint16_t /* handle */, BT_HDR*> partial_packets;
88 static std::unordered_map<uint16_t /* handle */, BT_HDR*> partial_iso_packets;
89
init(const packet_fragmenter_callbacks_t * result_callbacks)90 static void init(const packet_fragmenter_callbacks_t* result_callbacks) {
91 callbacks = result_callbacks;
92 }
93
cleanup()94 static void cleanup() {
95 partial_packets.clear();
96 partial_iso_packets.clear();
97 }
98
check_uint16_overflow(uint16_t a,uint16_t b)99 static bool check_uint16_overflow(uint16_t a, uint16_t b) {
100 return (UINT16_MAX - a) < b;
101 }
102
103 static void fragment_and_dispatch_acl(BT_HDR* packet);
104 static void fragment_and_dispatch_iso(BT_HDR* packet);
105
fragment_and_dispatch(BT_HDR * packet)106 static void fragment_and_dispatch(BT_HDR* packet) {
107 CHECK(packet != NULL);
108
109 uint16_t event = packet->event & MSG_EVT_MASK;
110
111 if (event == MSG_STACK_TO_HC_HCI_ACL) {
112 fragment_and_dispatch_acl(packet);
113 } else if (event == MSG_HC_TO_STACK_HCI_SCO) {
114 callbacks->fragmented(packet, true);
115 } else if (event == MSG_STACK_TO_HC_HCI_ISO) {
116 fragment_and_dispatch_iso(packet);
117 } else {
118 callbacks->fragmented(packet, true);
119 }
120 }
121
fragment_and_dispatch_acl(BT_HDR * packet)122 static void fragment_and_dispatch_acl(BT_HDR* packet) {
123 uint16_t max_data_size =
124 SUB_EVENT(packet->event) == LOCAL_BR_EDR_CONTROLLER_ID
125 ? controller->get_acl_data_size_classic()
126 : controller->get_acl_data_size_ble();
127
128 uint16_t max_packet_size = max_data_size + HCI_ACL_PREAMBLE_SIZE;
129 uint16_t remaining_length = packet->len;
130
131 uint8_t* stream = packet->data + packet->offset;
132
133 uint16_t continuation_handle;
134 STREAM_TO_UINT16(continuation_handle, stream);
135 continuation_handle = APPLY_CONTINUATION_FLAG(continuation_handle);
136
137 while (remaining_length > max_packet_size) {
138 // Make sure we use the right ACL packet size
139 stream = packet->data + packet->offset;
140 STREAM_SKIP_UINT16(stream);
141 UINT16_TO_STREAM(stream, max_data_size);
142
143 packet->len = max_packet_size;
144 callbacks->fragmented(packet, false);
145
146 packet->offset += max_data_size;
147 remaining_length -= max_data_size;
148 packet->len = remaining_length;
149
150 // Write the ACL header for the next fragment
151 stream = packet->data + packet->offset;
152 UINT16_TO_STREAM(stream, continuation_handle);
153 UINT16_TO_STREAM(stream, remaining_length - HCI_ACL_PREAMBLE_SIZE);
154
155 // Apparently L2CAP can set layer_specific to a max number of segments to
156 // transmit
157 if (packet->layer_specific) {
158 packet->layer_specific--;
159
160 if (packet->layer_specific == 0) {
161 packet->event = BT_EVT_TO_BTU_L2C_SEG_XMIT;
162 callbacks->transmit_finished(packet, false);
163 return;
164 }
165 }
166 }
167
168 callbacks->fragmented(packet, true);
169 }
170
fragment_and_dispatch_iso(BT_HDR * packet)171 static void fragment_and_dispatch_iso(BT_HDR* packet) {
172 uint8_t* stream = packet->data + packet->offset;
173 uint16_t max_data_size = controller->get_iso_data_size();
174 uint16_t max_packet_size = max_data_size + HCI_ISO_PREAMBLE_SIZE;
175 uint16_t remaining_length = packet->len;
176
177 uint16_t handle;
178 STREAM_TO_UINT16(handle, stream);
179
180 if (packet->layer_specific & BT_ISO_HDR_CONTAINS_TS) {
181 // First packet might have timestamp
182 handle = HCI_ISO_SET_TIMESTAMP_FLAG(handle);
183 }
184
185 if (remaining_length <= max_packet_size) {
186 stream = packet->data + packet->offset;
187 UINT16_TO_STREAM(stream, HCI_ISO_SET_COMPLETE_FLAG(handle));
188 } else {
189 while (remaining_length > max_packet_size) {
190 // Make sure we use the right ISO packet size
191 stream = packet->data + packet->offset;
192 STREAM_SKIP_UINT16(stream);
193 UINT16_TO_STREAM(stream, max_data_size);
194
195 packet->len = max_packet_size;
196 callbacks->fragmented(packet, false);
197
198 packet->offset += max_data_size;
199 remaining_length -= max_data_size;
200 packet->len = remaining_length;
201
202 // Write the ISO header for the next fragment
203 stream = packet->data + packet->offset;
204 if (remaining_length > max_packet_size) {
205 UINT16_TO_STREAM(stream,
206 HCI_ISO_SET_CONTINUATION_FLAG(handle & HANDLE_MASK));
207 } else {
208 UINT16_TO_STREAM(stream,
209 HCI_ISO_SET_END_FRAG_FLAG(handle & HANDLE_MASK));
210 }
211 UINT16_TO_STREAM(stream, remaining_length - HCI_ISO_PREAMBLE_SIZE);
212 }
213 }
214 callbacks->fragmented(packet, true);
215 }
216
reassemble_and_dispatch_iso(UNUSED_ATTR BT_HDR * packet)217 static void reassemble_and_dispatch_iso(UNUSED_ATTR BT_HDR* packet) {
218 uint8_t* stream = packet->data;
219 uint16_t handle;
220 uint16_t iso_length;
221 uint8_t iso_hdr_len = HCI_ISO_HEADER_LEN_WITHOUT_TS;
222 BT_HDR* partial_packet;
223 uint16_t iso_full_len;
224
225 STREAM_TO_UINT16(handle, stream);
226 STREAM_TO_UINT16(iso_length, stream);
227 // last 2 bits is RFU
228 iso_length = iso_length & 0x3FFF;
229
230 CHECK(iso_length == packet->len - HCI_ISO_PREAMBLE_SIZE);
231
232 uint8_t boundary_flag = GET_BOUNDARY_FLAG(handle);
233 uint8_t ts_flag = HCI_ISO_GET_TS_FLAG(handle);
234 handle = handle & HANDLE_MASK;
235
236 auto map_iter = partial_iso_packets.find(handle);
237
238 switch (boundary_flag) {
239 case HCI_ISO_BF_COMPLETE_PACKET:
240 case HCI_ISO_BF_FIRST_FRAGMENTED_PACKET:
241 uint16_t iso_sdu_length;
242 uint8_t packet_status_flags;
243
244 if (map_iter != partial_iso_packets.end()) {
245 LOG_WARN(
246 "%s found unfinished packet for the iso handle with start packet. "
247 "Dropping old.",
248 __func__);
249 BT_HDR* hdl = map_iter->second;
250 partial_iso_packets.erase(map_iter);
251 buffer_allocator->free(hdl);
252 }
253
254 if (ts_flag) {
255 /* Skip timestamp u32 */
256 STREAM_SKIP_UINT32(stream);
257 packet->layer_specific |= BT_ISO_HDR_CONTAINS_TS;
258 iso_hdr_len = HCI_ISO_HEADER_LEN_WITH_TS;
259 }
260
261 if (iso_length < iso_hdr_len) {
262 LOG_WARN("%s ISO packet too small (%d < %d). Dropping it.", __func__,
263 packet->len, iso_hdr_len);
264 buffer_allocator->free(packet);
265 return;
266 }
267
268 /* Skip packet_seq. */
269 STREAM_SKIP_UINT16(stream);
270 STREAM_TO_UINT16(iso_sdu_length, stream);
271
272 /* Silently ignore empty report if there's no 'lost data' flag set. */
273 if (iso_sdu_length == 0) {
274 buffer_allocator->free(packet);
275 return;
276 }
277
278 packet_status_flags = HCI_ISO_GET_PACKET_STATUS_FLAGS(iso_sdu_length);
279 iso_sdu_length = iso_sdu_length & HCI_ISO_SDU_LENGTH_MASK;
280
281 if (packet_status_flags)
282 LOG_ERROR("%s packet status flags: 0x%02x", __func__,
283 packet_status_flags);
284
285 iso_full_len = iso_sdu_length + iso_hdr_len + HCI_ISO_PREAMBLE_SIZE;
286 if ((iso_full_len + sizeof(BT_HDR)) > BT_DEFAULT_BUFFER_SIZE) {
287 LOG_ERROR("%s Dropping ISO packet with invalid length (%d).", __func__,
288 iso_sdu_length);
289 buffer_allocator->free(packet);
290 return;
291 }
292
293 if (((boundary_flag == HCI_ISO_BF_COMPLETE_PACKET) &&
294 (iso_full_len != packet->len)) ||
295 ((boundary_flag == HCI_ISO_BF_FIRST_FRAGMENTED_PACKET) &&
296 (iso_full_len <= packet->len))) {
297 LOG_ERROR("%s corrupted ISO frame", __func__);
298 buffer_allocator->free(packet);
299 return;
300 }
301
302 partial_packet =
303 (BT_HDR*)buffer_allocator->alloc(iso_full_len + sizeof(BT_HDR));
304 if (!partial_packet) {
305 LOG_ERROR("%s cannot allocate partial packet", __func__);
306 buffer_allocator->free(packet);
307 return;
308 }
309
310 partial_packet->event = packet->event;
311 partial_packet->len = iso_full_len;
312 partial_packet->layer_specific = packet->layer_specific;
313
314 memcpy(partial_packet->data, packet->data, packet->len);
315
316 // Update the ISO data size to indicate the full expected length
317 stream = partial_packet->data;
318 STREAM_SKIP_UINT16(stream); // skip the ISO handle
319 UINT16_TO_STREAM(stream, iso_full_len - HCI_ISO_PREAMBLE_SIZE);
320
321 if (boundary_flag == HCI_ISO_BF_FIRST_FRAGMENTED_PACKET) {
322 partial_packet->offset = packet->len;
323 partial_iso_packets[handle] = partial_packet;
324 } else {
325 packet->layer_specific |= BT_ISO_HDR_OFFSET_POINTS_DATA;
326 partial_packet->offset = iso_hdr_len + HCI_ISO_PREAMBLE_SIZE;
327 callbacks->reassembled(partial_packet);
328 }
329
330 buffer_allocator->free(packet);
331 break;
332
333 case HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET:
334 // pass-through
335 case HCI_ISO_BF_LAST_FRAGMENT_PACKET:
336 if (map_iter == partial_iso_packets.end()) {
337 LOG_WARN("%s got continuation for unknown packet. Dropping it.",
338 __func__);
339 buffer_allocator->free(packet);
340 return;
341 }
342
343 partial_packet = map_iter->second;
344 if (partial_packet->len <
345 (partial_packet->offset + packet->len - HCI_ISO_PREAMBLE_SIZE)) {
346 LOG_ERROR(
347 "%s got packet which would exceed expected length of %d. "
348 "dropping full packet",
349 __func__, partial_packet->len);
350 buffer_allocator->free(packet);
351 partial_iso_packets.erase(map_iter);
352 buffer_allocator->free(partial_packet);
353 return;
354 }
355
356 memcpy(partial_packet->data + partial_packet->offset,
357 packet->data + HCI_ISO_PREAMBLE_SIZE,
358 packet->len - HCI_ISO_PREAMBLE_SIZE);
359
360 if (boundary_flag == HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET) {
361 partial_packet->offset += packet->len - HCI_ISO_PREAMBLE_SIZE;
362 buffer_allocator->free(packet);
363 return;
364 }
365
366 if (partial_packet->len !=
367 partial_packet->offset + packet->len - HCI_ISO_PREAMBLE_SIZE) {
368 LOG_ERROR(
369 "%s got last fragment, but it doesn't fill up the whole packet of "
370 "size %d",
371 __func__, partial_packet->len);
372 buffer_allocator->free(packet);
373 partial_iso_packets.erase(map_iter);
374 buffer_allocator->free(partial_packet);
375 return;
376 }
377
378 partial_packet->layer_specific |= BT_ISO_HDR_OFFSET_POINTS_DATA;
379 partial_packet->offset = HCI_ISO_PREAMBLE_SIZE;
380 if (partial_packet->layer_specific & BT_ISO_HDR_CONTAINS_TS)
381 partial_packet->offset += HCI_ISO_HEADER_LEN_WITH_TS;
382 else
383 partial_packet->offset += HCI_ISO_HEADER_LEN_WITHOUT_TS;
384
385 buffer_allocator->free(packet);
386
387 partial_iso_packets.erase(map_iter);
388 callbacks->reassembled(partial_packet);
389
390 break;
391 default:
392 LOG_ERROR("%s Unexpected packet, dropping full packet", __func__);
393 buffer_allocator->free(packet);
394 break;
395 }
396 }
397
reassemble_and_dispatch(BT_HDR * packet)398 static void reassemble_and_dispatch(BT_HDR* packet) {
399 if ((packet->event & MSG_EVT_MASK) == MSG_HC_TO_STACK_HCI_ACL) {
400 uint8_t* stream = packet->data;
401 uint16_t handle;
402 uint16_t acl_length;
403
404 STREAM_TO_UINT16(handle, stream);
405 STREAM_TO_UINT16(acl_length, stream);
406
407 CHECK(acl_length == packet->len - HCI_ACL_PREAMBLE_SIZE);
408
409 uint8_t boundary_flag = GET_BOUNDARY_FLAG(handle);
410 uint8_t broadcast_flag = GET_BROADCAST_FLAG(handle);
411 handle = handle & HANDLE_MASK;
412
413 if (broadcast_flag != POINT_TO_POINT) {
414 LOG_WARN("dropping broadcast packet");
415 buffer_allocator->free(packet);
416 return;
417 }
418
419 if (boundary_flag == START_PACKET_BOUNDARY) {
420 if (acl_length < 2) {
421 LOG_WARN("%s invalid acl_length %d", __func__, acl_length);
422 buffer_allocator->free(packet);
423 return;
424 }
425 uint16_t l2cap_length;
426 STREAM_TO_UINT16(l2cap_length, stream);
427 auto map_iter = partial_packets.find(handle);
428 if (map_iter != partial_packets.end()) {
429 LOG_WARN(
430 "%s found unfinished packet for handle with start packet. "
431 "Dropping old.",
432 __func__);
433
434 BT_HDR* hdl = map_iter->second;
435 partial_packets.erase(map_iter);
436 buffer_allocator->free(hdl);
437 }
438
439 if (acl_length < L2CAP_HEADER_PDU_LEN_SIZE) {
440 LOG_WARN("%s L2CAP packet too small (%d < %d). Dropping it.", __func__,
441 packet->len, L2CAP_HEADER_PDU_LEN_SIZE);
442 buffer_allocator->free(packet);
443 return;
444 }
445
446 uint16_t full_length =
447 l2cap_length + L2CAP_HEADER_SIZE + HCI_ACL_PREAMBLE_SIZE;
448
449 // Check for buffer overflow and that the full packet size + BT_HDR size
450 // is less than the max buffer size
451 if (check_uint16_overflow(l2cap_length,
452 (L2CAP_HEADER_SIZE + HCI_ACL_PREAMBLE_SIZE)) ||
453 ((full_length + sizeof(BT_HDR)) > BT_DEFAULT_BUFFER_SIZE)) {
454 LOG_ERROR("%s Dropping L2CAP packet with invalid length (%d).",
455 __func__, l2cap_length);
456 buffer_allocator->free(packet);
457 return;
458 }
459
460 if (full_length <= packet->len) {
461 if (full_length < packet->len)
462 LOG_WARN("%s found l2cap full length %d less than the hci length %d.",
463 __func__, l2cap_length, packet->len);
464
465 callbacks->reassembled(packet);
466 return;
467 }
468
469 BT_HDR* partial_packet =
470 (BT_HDR*)buffer_allocator->alloc(full_length + sizeof(BT_HDR));
471 partial_packet->event = packet->event;
472 partial_packet->len = full_length;
473 partial_packet->offset = packet->len;
474
475 memcpy(partial_packet->data, packet->data, packet->len);
476
477 // Update the ACL data size to indicate the full expected length
478 stream = partial_packet->data;
479 STREAM_SKIP_UINT16(stream); // skip the handle
480 UINT16_TO_STREAM(stream, full_length - HCI_ACL_PREAMBLE_SIZE);
481
482 partial_packets[handle] = partial_packet;
483
484 // Free the old packet buffer, since we don't need it anymore
485 buffer_allocator->free(packet);
486 } else {
487 auto map_iter = partial_packets.find(handle);
488 if (map_iter == partial_packets.end()) {
489 LOG_WARN("%s got continuation for unknown packet. Dropping it.",
490 __func__);
491 buffer_allocator->free(packet);
492 return;
493 }
494 BT_HDR* partial_packet = map_iter->second;
495
496 packet->offset = HCI_ACL_PREAMBLE_SIZE;
497 uint16_t projected_offset =
498 partial_packet->offset + (packet->len - HCI_ACL_PREAMBLE_SIZE);
499 if ((packet->len - packet->offset) >
500 (partial_packet->len - partial_packet->offset)) {
501 LOG_WARN(
502 "%s got packet which would exceed expected length of %d. "
503 "Truncating.",
504 __func__, partial_packet->len);
505 packet->len = (partial_packet->len - partial_packet->offset) + packet->offset;
506 projected_offset = partial_packet->len;
507 }
508
509 memcpy(partial_packet->data + partial_packet->offset,
510 packet->data + packet->offset, packet->len - packet->offset);
511
512 // Free the old packet buffer, since we don't need it anymore
513 buffer_allocator->free(packet);
514 partial_packet->offset = projected_offset;
515
516 if (partial_packet->offset == partial_packet->len) {
517 partial_packets.erase(handle);
518 partial_packet->offset = 0;
519 callbacks->reassembled(partial_packet);
520 }
521 }
522 } else if ((packet->event & MSG_EVT_MASK) == MSG_HC_TO_STACK_HCI_SCO) {
523 callbacks->reassembled(packet);
524 } else if ((packet->event & MSG_EVT_MASK) == MSG_HC_TO_STACK_HCI_ISO) {
525 reassemble_and_dispatch_iso(packet);
526 } else {
527 callbacks->reassembled(packet);
528 }
529 }
530
531 static const packet_fragmenter_t interface = {init, cleanup,
532
533 fragment_and_dispatch,
534 reassemble_and_dispatch};
535
packet_fragmenter_get_interface()536 const packet_fragmenter_t* packet_fragmenter_get_interface() {
537 controller = controller_get_interface();
538 buffer_allocator = buffer_allocator_get_interface();
539 return &interface;
540 }
541
packet_fragmenter_get_test_interface(const controller_t * controller_interface,const allocator_t * buffer_allocator_interface)542 const packet_fragmenter_t* packet_fragmenter_get_test_interface(
543 const controller_t* controller_interface,
544 const allocator_t* buffer_allocator_interface) {
545 controller = controller_interface;
546 buffer_allocator = buffer_allocator_interface;
547 return &interface;
548 }
549