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