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 <bluetooth/log.h>
24 #include <string.h>
25
26 #include <unordered_map>
27
28 #include "hci/include/buffer_allocator.h"
29 #include "hci/include/hci_layer.h"
30 #include "internal_include/bt_target.h"
31 #include "stack/include/bt_hdr.h"
32 #include "stack/include/bt_types.h"
33
34 #define HCI_ISO_BF_FIRST_FRAGMENTED_PACKET (0)
35 #define HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET (1)
36 #define HCI_ISO_BF_COMPLETE_PACKET (2)
37 #define HCI_ISO_BF_LAST_FRAGMENT_PACKET (3)
38
39 #define HCI_ISO_HEADER_TIMESTAMP_SIZE (4)
40 #define HCI_ISO_HEADER_ISO_LEN_SIZE (2)
41 #define HCI_ISO_HEADER_PACKET_SEQ_SIZE (2)
42
43 // ISO
44 // 2 bytes for handle, 2 bytes for data length (Volume 2, Part E, 5.4.5)
45 #define HCI_ISO_PREAMBLE_SIZE 4
46
47 #define HCI_ISO_HEADER_LEN_WITHOUT_TS (HCI_ISO_HEADER_ISO_LEN_SIZE + HCI_ISO_HEADER_PACKET_SEQ_SIZE)
48 #define HCI_ISO_HEADER_LEN_WITH_TS (HCI_ISO_HEADER_LEN_WITHOUT_TS + HCI_ISO_HEADER_TIMESTAMP_SIZE)
49
50 #define HCI_ISO_SET_CONTINUATION_FLAG(handle) (((handle) & 0x4FFF) | (0x0001 << 12))
51 #define HCI_ISO_SET_COMPLETE_FLAG(handle) (((handle) & 0x4FFF) | (0x0002 << 12))
52 #define HCI_ISO_SET_END_FRAG_FLAG(handle) (((handle) & 0x4FFF) | (0x0003 << 12))
53 #define HCI_ISO_SET_TIMESTAMP_FLAG(handle) (((handle) & 0x3FFF) | (0x0001 << 14))
54
55 #define HCI_ISO_GET_TS_FLAG(handle) (((handle) >> 14) & 0x0001)
56 #define HCI_ISO_GET_PACKET_STATUS_FLAGS(iso_sdu_length) (iso_sdu_length & 0xC000)
57 #define HCI_ISO_SDU_LENGTH_MASK 0x0FFF
58
59 #define APPLY_CONTINUATION_FLAG(handle) (((handle) & 0xCFFF) | 0x1000)
60 #define APPLY_START_FLAG(handle) (((handle) & 0xCFFF) | 0x2000)
61 #define SUB_EVENT(event) ((event) & MSG_SUB_EVT_MASK)
62 #define GET_BOUNDARY_FLAG(handle) (((handle) >> 12) & 0x0003)
63 #define GET_BROADCAST_FLAG(handle) (((handle) >> 14) & 0x0003)
64
65 #define HANDLE_MASK 0x0FFF
66 #define START_PACKET_BOUNDARY 2
67 #define POINT_TO_POINT 0
68 #define L2CAP_HEADER_PDU_LEN_SIZE 2
69 #define L2CAP_HEADER_CID_SIZE 2
70 #define L2CAP_HEADER_SIZE (L2CAP_HEADER_PDU_LEN_SIZE + L2CAP_HEADER_CID_SIZE)
71
72 // Our interface and callbacks
73
74 using namespace bluetooth;
75
76 static const allocator_t* buffer_allocator;
77 static const packet_fragmenter_callbacks_t* callbacks;
78
79 static std::unordered_map<uint16_t /* handle */, BT_HDR*> partial_iso_packets;
80
init(const packet_fragmenter_callbacks_t * result_callbacks)81 static void init(const packet_fragmenter_callbacks_t* result_callbacks) {
82 callbacks = result_callbacks;
83 }
84
cleanup()85 static void cleanup() { partial_iso_packets.clear(); }
86
fragment_and_dispatch(BT_HDR * packet,uint16_t max_data_size)87 static void fragment_and_dispatch(BT_HDR* packet, uint16_t max_data_size) {
88 log::assert_that(packet != NULL, "assert failed: packet != NULL");
89
90 uint16_t event = packet->event & MSG_EVT_MASK;
91
92 log::assert_that(event == MSG_STACK_TO_HC_HCI_ISO,
93 "assert failed: event == MSG_STACK_TO_HC_HCI_ISO");
94
95 uint8_t* stream = packet->data + packet->offset;
96 uint16_t max_packet_size = max_data_size + HCI_ISO_PREAMBLE_SIZE;
97 uint16_t remaining_length = packet->len;
98
99 uint16_t handle;
100 STREAM_TO_UINT16(handle, stream);
101
102 if (packet->layer_specific & BT_ISO_HDR_CONTAINS_TS) {
103 // First packet might have timestamp
104 handle = HCI_ISO_SET_TIMESTAMP_FLAG(handle);
105 }
106
107 if (remaining_length <= max_packet_size) {
108 stream = packet->data + packet->offset;
109 UINT16_TO_STREAM(stream, HCI_ISO_SET_COMPLETE_FLAG(handle));
110 } else {
111 while (remaining_length > max_packet_size) {
112 // Make sure we use the right ISO packet size
113 stream = packet->data + packet->offset;
114 STREAM_SKIP_UINT16(stream);
115 UINT16_TO_STREAM(stream, max_data_size);
116
117 packet->len = max_packet_size;
118 callbacks->fragmented(packet, false);
119
120 packet->offset += max_data_size;
121 remaining_length -= max_data_size;
122 packet->len = remaining_length;
123
124 // Write the ISO header for the next fragment
125 stream = packet->data + packet->offset;
126 if (remaining_length > max_packet_size) {
127 UINT16_TO_STREAM(stream, HCI_ISO_SET_CONTINUATION_FLAG(handle & HANDLE_MASK));
128 } else {
129 UINT16_TO_STREAM(stream, HCI_ISO_SET_END_FRAG_FLAG(handle & HANDLE_MASK));
130 }
131 UINT16_TO_STREAM(stream, remaining_length - HCI_ISO_PREAMBLE_SIZE);
132 }
133 }
134 callbacks->fragmented(packet, true);
135 }
136
reassemble_and_dispatch(BT_HDR * packet)137 static void reassemble_and_dispatch(BT_HDR* packet) {
138 uint8_t* stream = packet->data;
139 uint16_t handle;
140 uint16_t iso_length;
141 uint8_t iso_hdr_len = HCI_ISO_HEADER_LEN_WITHOUT_TS;
142 BT_HDR* partial_packet;
143 uint16_t iso_full_len;
144
145 uint16_t event = packet->event & MSG_EVT_MASK;
146 log::assert_that(event == MSG_HC_TO_STACK_HCI_ISO,
147 "assert failed: event == MSG_HC_TO_STACK_HCI_ISO");
148
149 STREAM_TO_UINT16(handle, stream);
150 STREAM_TO_UINT16(iso_length, stream);
151 // last 2 bits is RFU
152 iso_length = iso_length & 0x3FFF;
153
154 log::assert_that(iso_length == packet->len - HCI_ISO_PREAMBLE_SIZE,
155 "assert failed: iso_length == packet->len - HCI_ISO_PREAMBLE_SIZE");
156
157 uint8_t boundary_flag = GET_BOUNDARY_FLAG(handle);
158 uint8_t ts_flag = HCI_ISO_GET_TS_FLAG(handle);
159 handle = handle & HANDLE_MASK;
160
161 auto map_iter = partial_iso_packets.find(handle);
162
163 switch (boundary_flag) {
164 case HCI_ISO_BF_COMPLETE_PACKET:
165 case HCI_ISO_BF_FIRST_FRAGMENTED_PACKET:
166 uint16_t iso_sdu_length;
167 uint8_t packet_status_flags;
168
169 if (map_iter != partial_iso_packets.end()) {
170 log::warn(
171 "found unfinished packet for the iso handle with start packet. "
172 "Dropping old.");
173 BT_HDR* hdl = map_iter->second;
174 partial_iso_packets.erase(map_iter);
175 buffer_allocator->free(hdl);
176 }
177
178 if (ts_flag) {
179 /* Skip timestamp u32 */
180 STREAM_SKIP_UINT32(stream);
181 packet->layer_specific |= BT_ISO_HDR_CONTAINS_TS;
182 iso_hdr_len = HCI_ISO_HEADER_LEN_WITH_TS;
183 }
184
185 if (iso_length < iso_hdr_len) {
186 log::warn("ISO packet too small ({} < {}). Dropping it.", packet->len, iso_hdr_len);
187 buffer_allocator->free(packet);
188 return;
189 }
190
191 /* Skip packet_seq. */
192 STREAM_SKIP_UINT16(stream);
193 STREAM_TO_UINT16(iso_sdu_length, stream);
194
195 /* Silently ignore empty report if there's no 'lost data' flag set. */
196 if (iso_sdu_length == 0) {
197 buffer_allocator->free(packet);
198 return;
199 }
200
201 packet_status_flags = HCI_ISO_GET_PACKET_STATUS_FLAGS(iso_sdu_length);
202 iso_sdu_length = iso_sdu_length & HCI_ISO_SDU_LENGTH_MASK;
203
204 if (packet_status_flags) {
205 log::error("packet status flags: 0x{:02x}", packet_status_flags);
206 }
207
208 iso_full_len = iso_sdu_length + iso_hdr_len + HCI_ISO_PREAMBLE_SIZE;
209 if ((iso_full_len + sizeof(BT_HDR)) > BT_DEFAULT_BUFFER_SIZE) {
210 log::error("Dropping ISO packet with invalid length ({}).", iso_sdu_length);
211 buffer_allocator->free(packet);
212 return;
213 }
214
215 if (((boundary_flag == HCI_ISO_BF_COMPLETE_PACKET) && (iso_full_len != packet->len)) ||
216 ((boundary_flag == HCI_ISO_BF_FIRST_FRAGMENTED_PACKET) &&
217 (iso_full_len <= packet->len))) {
218 log::error("corrupted ISO frame");
219 buffer_allocator->free(packet);
220 return;
221 }
222
223 partial_packet = (BT_HDR*)buffer_allocator->alloc(iso_full_len + sizeof(BT_HDR));
224 if (!partial_packet) {
225 log::error("cannot allocate partial packet");
226 buffer_allocator->free(packet);
227 return;
228 }
229
230 partial_packet->event = packet->event;
231 partial_packet->len = iso_full_len;
232 partial_packet->layer_specific = packet->layer_specific;
233
234 memcpy(partial_packet->data, packet->data, packet->len);
235
236 // Update the ISO data size to indicate the full expected length
237 stream = partial_packet->data;
238 STREAM_SKIP_UINT16(stream); // skip the ISO handle
239 UINT16_TO_STREAM(stream, iso_full_len - HCI_ISO_PREAMBLE_SIZE);
240
241 if (boundary_flag == HCI_ISO_BF_FIRST_FRAGMENTED_PACKET) {
242 partial_packet->offset = packet->len;
243 partial_iso_packets[handle] = partial_packet;
244 } else {
245 packet->layer_specific |= BT_ISO_HDR_OFFSET_POINTS_DATA;
246 partial_packet->offset = iso_hdr_len + HCI_ISO_PREAMBLE_SIZE;
247 callbacks->reassembled(partial_packet);
248 }
249
250 buffer_allocator->free(packet);
251 break;
252
253 case HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET:
254 // pass-through
255 case HCI_ISO_BF_LAST_FRAGMENT_PACKET:
256 if (map_iter == partial_iso_packets.end()) {
257 log::warn("got continuation for unknown packet. Dropping it.");
258 buffer_allocator->free(packet);
259 return;
260 }
261
262 partial_packet = map_iter->second;
263 if (partial_packet->len < (partial_packet->offset + packet->len - HCI_ISO_PREAMBLE_SIZE)) {
264 log::error(
265 "got packet which would exceed expected length of {}. dropping "
266 "full packet",
267 partial_packet->len);
268 buffer_allocator->free(packet);
269 partial_iso_packets.erase(map_iter);
270 buffer_allocator->free(partial_packet);
271 return;
272 }
273
274 memcpy(partial_packet->data + partial_packet->offset, packet->data + HCI_ISO_PREAMBLE_SIZE,
275 packet->len - HCI_ISO_PREAMBLE_SIZE);
276
277 if (boundary_flag == HCI_ISO_BF_CONTINUATION_FRAGMENT_PACKET) {
278 partial_packet->offset += packet->len - HCI_ISO_PREAMBLE_SIZE;
279 buffer_allocator->free(packet);
280 return;
281 }
282
283 if (partial_packet->len != partial_packet->offset + packet->len - HCI_ISO_PREAMBLE_SIZE) {
284 log::error(
285 "got last fragment, but it doesn't fill up the whole packet of "
286 "size {}",
287 partial_packet->len);
288 buffer_allocator->free(packet);
289 partial_iso_packets.erase(map_iter);
290 buffer_allocator->free(partial_packet);
291 return;
292 }
293
294 partial_packet->layer_specific |= BT_ISO_HDR_OFFSET_POINTS_DATA;
295 partial_packet->offset = HCI_ISO_PREAMBLE_SIZE;
296 if (partial_packet->layer_specific & BT_ISO_HDR_CONTAINS_TS) {
297 partial_packet->offset += HCI_ISO_HEADER_LEN_WITH_TS;
298 } else {
299 partial_packet->offset += HCI_ISO_HEADER_LEN_WITHOUT_TS;
300 }
301
302 buffer_allocator->free(packet);
303
304 partial_iso_packets.erase(map_iter);
305 callbacks->reassembled(partial_packet);
306
307 break;
308 default:
309 log::error("Unexpected packet, dropping full packet");
310 buffer_allocator->free(packet);
311 break;
312 }
313 }
314
315 static const packet_fragmenter_t interface = {
316 init,
317 cleanup,
318 fragment_and_dispatch,
319 reassemble_and_dispatch,
320 };
321
packet_fragmenter_get_interface()322 const packet_fragmenter_t* packet_fragmenter_get_interface() {
323 buffer_allocator = buffer_allocator_get_interface();
324 return &interface;
325 }
326