granicus.if.org Git - esp-idf/blob - components/bt/bluedroid/hci/packet_fragmenter.c

   1 /******************************************************************************
   2  *
   3  *  Copyright (C) 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 #include <string.h>
  19 #include "bt_trace.h"
  20 #include "bt_defs.h"
  21 #include "controller.h"
  22 #include "buffer_allocator.h"
  23 #include "hci_internals.h"
  24 #include "hci_layer.h"
  25 #include "packet_fragmenter.h"
  26
  27 #include "hash_map.h"
  28 #include "hash_functions.h"
  29
  30
  31
  32 #define APPLY_CONTINUATION_FLAG(handle) (((handle) & 0xCFFF) | 0x1000)
  33 #define APPLY_START_FLAG(handle) (((handle) & 0xCFFF) | 0x2000)
  34 #define SUB_EVENT(event) ((event) & MSG_SUB_EVT_MASK)
  35 #define GET_BOUNDARY_FLAG(handle) (((handle) >> 12) & 0x0003)
  36
  37 #define HANDLE_MASK 0x0FFF
  38 #define START_PACKET_BOUNDARY 2
  39 #define CONTINUATION_PACKET_BOUNDARY 1
  40 #define L2CAP_HEADER_SIZE       4
  41
  42 // TODO(zachoverflow): find good value for this
  43 #define NUMBER_OF_BUCKETS 42
  44
  45 // Our interface and callbacks
  46 static const packet_fragmenter_t interface;
  47 static const allocator_t *buffer_allocator;
  48 static const controller_t *controller;
  49 static const packet_fragmenter_callbacks_t *callbacks;
  50 static hash_map_t *partial_packets;
  51 static BT_HDR *current_fragment_packet;
  52
  53 static void init(const packet_fragmenter_callbacks_t *result_callbacks)
  54 {
  55     current_fragment_packet = NULL;
  56     callbacks = result_callbacks;
  57     partial_packets = hash_map_new(NUMBER_OF_BUCKETS, hash_function_naive, NULL, NULL, NULL);
  58 }
  59
  60 static void cleanup()
  61 {
  62     if (partial_packets) {
  63         hash_map_free(partial_packets);
  64     }
  65 }
  66
  67 static BT_HDR *fragment_get_current_packet()
  68 {
  69     return current_fragment_packet;
  70 }
  71
  72 static void fragment_and_dispatch(BT_HDR *packet)
  73 {
  74     uint16_t continuation_handle;
  75     uint16_t max_data_size, max_packet_size, remaining_length;
  76     uint16_t event = packet->event & MSG_EVT_MASK;
  77     uint8_t *stream = packet->data + packet->offset;
  78
  79     assert(packet != NULL);
  80
  81     // We only fragment ACL packets
  82     if (event != MSG_STACK_TO_HC_HCI_ACL) {
  83         callbacks->fragmented(packet, true);
  84         return;
  85     }
  86
  87     max_data_size =
  88         SUB_EVENT(packet->event) == LOCAL_BR_EDR_CONTROLLER_ID ?
  89         controller->get_acl_data_size_classic() :
  90         controller->get_acl_data_size_ble();
  91
  92     max_packet_size = max_data_size + HCI_ACL_PREAMBLE_SIZE;
  93     remaining_length = packet->len;
  94
  95     STREAM_TO_UINT16(continuation_handle, stream);
  96     continuation_handle = APPLY_CONTINUATION_FLAG(continuation_handle);
  97     if (remaining_length > max_packet_size) {
  98         current_fragment_packet = packet;
  99         UINT16_TO_STREAM(stream, max_packet_size);
 100         packet->len = max_packet_size;
 101         callbacks->fragmented(packet, false);
 102         packet->offset += max_data_size;
 103         remaining_length -= max_data_size;
 104         packet->len = remaining_length;
 105
 106         // Write the ACL header for the next fragment
 107         stream = packet->data + packet->offset;
 108         UINT16_TO_STREAM(stream, continuation_handle);
 109         UINT16_TO_STREAM(stream, remaining_length - HCI_ACL_PREAMBLE_SIZE);
 110
 111         // Apparently L2CAP can set layer_specific to a max number of segments to transmit
 112         if (packet->layer_specific) {
 113             packet->layer_specific--;
 114             if (packet->layer_specific == 0) {
 115                 packet->event = MSG_HC_TO_STACK_L2C_SEG_XMIT;
 116                 callbacks->transmit_finished(packet, false);
 117                 return;
 118             }
 119         }
 120     } else {
 121         current_fragment_packet = NULL;
 122         callbacks->fragmented(packet, true);
 123     }
 124 }
 125
 126 static void reassemble_and_dispatch(BT_HDR *packet)
 127 {
 128     if ((packet->event & MSG_EVT_MASK) == MSG_HC_TO_STACK_HCI_ACL) {
 129         uint8_t *stream = packet->data + packet->offset;
 130         uint16_t handle;
 131         uint16_t l2cap_length;
 132         uint16_t acl_length;
 133         uint8_t boundary_flag;
 134         BT_HDR *partial_packet;
 135
 136         STREAM_TO_UINT16(handle, stream);
 137         STREAM_TO_UINT16(acl_length, stream);
 138         STREAM_TO_UINT16(l2cap_length, stream);
 139
 140         assert(acl_length == packet->len - HCI_ACL_PREAMBLE_SIZE);
 141
 142         boundary_flag = GET_BOUNDARY_FLAG(handle);
 143         handle = handle & HANDLE_MASK;
 144
 145         partial_packet = (BT_HDR *)hash_map_get(partial_packets, (void *)(uintptr_t)handle);
 146
 147         if (boundary_flag == START_PACKET_BOUNDARY) {
 148             uint16_t full_length;
 149             if (partial_packet) {
 150                 LOG_WARN("%s found unfinished packet for handle with start packet. Dropping old.", __func__);
 151                 hash_map_erase(partial_packets, (void *)(uintptr_t)handle);
 152                 buffer_allocator->free(partial_packet);
 153             }
 154
 155             full_length = l2cap_length + L2CAP_HEADER_SIZE + HCI_ACL_PREAMBLE_SIZE;
 156             if (full_length <= packet->len) {
 157                 if (full_length < packet->len) {
 158                     LOG_WARN("%s found l2cap full length %d less than the hci length %d.", __func__, l2cap_length, packet->len);
 159                 }
 160
 161                 callbacks->reassembled(packet);
 162                 return;
 163             }
 164
 165             partial_packet = (BT_HDR *)buffer_allocator->alloc(full_length + sizeof(BT_HDR));
 166             partial_packet->event = packet->event;
 167             partial_packet->len = full_length;
 168             partial_packet->offset = packet->len;
 169
 170             memcpy(partial_packet->data, packet->data + packet->offset, packet->len);
 171
 172             // Update the ACL data size to indicate the full expected length
 173             stream = partial_packet->data;
 174             STREAM_SKIP_UINT16(stream); // skip the handle
 175             UINT16_TO_STREAM(stream, full_length - HCI_ACL_PREAMBLE_SIZE);
 176
 177             hash_map_set(partial_packets, (void *)(uintptr_t)handle, partial_packet);
 178             // Free the old packet buffer, since we don't need it anymore
 179             buffer_allocator->free(packet);
 180         } else {
 181             uint16_t projected_offset;
 182             if (!partial_packet) {
 183                 LOG_WARN("%s got continuation for unknown packet. Dropping it.", __func__);
 184                 buffer_allocator->free(packet);
 185                 return;
 186             }
 187
 188             packet->offset += HCI_ACL_PREAMBLE_SIZE; // skip ACL preamble
 189             packet->len -= HCI_ACL_PREAMBLE_SIZE;
 190
 191             projected_offset = partial_packet->offset + packet->len;
 192             if (projected_offset > partial_packet->len) { // len stores the expected length
 193                 LOG_WARN("%s got packet which would exceed expected length of %d. Truncating.", __func__, partial_packet->len);
 194                 packet->len = partial_packet->len - partial_packet->offset;
 195                 projected_offset = partial_packet->len;
 196             }
 197
 198             memcpy(
 199                 partial_packet->data + partial_packet->offset,
 200                 packet->data + packet->offset,
 201                 packet->len
 202             );
 203             // Free the old packet buffer, since we don't need it anymore
 204             buffer_allocator->free(packet);
 205             partial_packet->offset = projected_offset;
 206
 207             if (partial_packet->offset == partial_packet->len) {
 208                 hash_map_erase(partial_packets, (void *)(uintptr_t)handle);
 209                 partial_packet->offset = 0;
 210                 callbacks->reassembled(partial_packet);
 211             }
 212         }
 213     } else {
 214         callbacks->reassembled(packet);
 215     }
 216 }
 217
 218 static const packet_fragmenter_t interface = {
 219     init,
 220     cleanup,
 221
 222     fragment_get_current_packet,
 223     fragment_and_dispatch,
 224     reassemble_and_dispatch
 225 };
 226
 227 const packet_fragmenter_t *packet_fragmenter_get_interface()
 228 {
 229     controller = controller_get_interface();
 230     buffer_allocator = buffer_allocator_get_interface();
 231     return &interface;
 232 }
 233