hci/src/packet_fragmenter.c

/******************************************************************************
 *
 *  Copyright (C) 2014 Google, Inc.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at:
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 ******************************************************************************/

#define LOG_TAG "hci_hal_h4"

#include <assert.h>
#include <utils/Log.h>

#include "hash_functions.h"
#include "hash_map.h"
#include "hci_internals.h"
#include "hci_layer.h"
#include "packet_fragmenter.h"
#include "osi.h"

#define APPLY_CONTINUATION_FLAG(handle) (((handle) & 0xCFFF) | 0x1000)
#define APPLY_START_FLAG(handle) (((handle) & 0xCFFF) | 0x2000)
#define SUB_EVENT(event) ((event) & MSG_SUB_EVT_MASK)
#define GET_BOUNDARY_FLAG(handle) (((handle) >> 12) & 0x0003)

#define HANDLE_MASK 0x0FFF
#define START_PACKET_BOUNDARY 2
#define CONTINUATION_PACKET_BOUNDARY 1
#define L2CAP_HEADER_SIZE       4

// TODO(zachoverflow): find good value for this
#define NUMBER_OF_BUCKETS 42

// Our interface and callbacks
static const packet_fragmenter_interface_t interface;
static const allocator_t *allocator;
static const packet_fragmenter_callbacks_t *callbacks;

static uint16_t acl_data_size;
static uint16_t ble_acl_data_size;
static hash_map_t *partial_packets;

static void init(const packet_fragmenter_callbacks_t *result_callbacks, const allocator_t *buffer_allocator) {
  allocator = buffer_allocator;
  callbacks = result_callbacks;

  // Give initial values for the data sizes, which will
  // be updated when we talk to the actual controller
  acl_data_size = 1021;
  ble_acl_data_size = 27;

  if (partial_packets)
    hash_map_free(partial_packets);

  partial_packets = hash_map_new(NUMBER_OF_BUCKETS, hash_function_naive, NULL, NULL);
}

static void set_acl_data_size(uint16_t size) {
  assert(size > 0);
  acl_data_size = size;
}

static void set_ble_acl_data_size(uint16_t size) {
  // If the ble acl data size is zero, the ble acl buffers are the same size
  // as the normal ones.
  ble_acl_data_size = size == 0 ? acl_data_size : size;
}

static void fragment_and_dispatch(BT_HDR *packet) {
  assert(packet != NULL);

  uint16_t remaining_length = packet->len;
  uint16_t event = packet->event & MSG_EVT_MASK;
  uint16_t max_data_size = SUB_EVENT(packet->event) == LOCAL_BR_EDR_CONTROLLER_ID ? acl_data_size : ble_acl_data_size;
  uint16_t max_packet_size = max_data_size + HCI_ACL_PREAMBLE_SIZE;

  uint8_t *stream = packet->data + packet->offset;

  if (event == MSG_STACK_TO_HC_HCI_ACL && remaining_length > max_packet_size) {
    uint16_t continuation_handle;
    STREAM_TO_UINT16(continuation_handle, stream);
    continuation_handle = APPLY_CONTINUATION_FLAG(continuation_handle);

    while (remaining_length > max_packet_size) {
      // Make sure we use the right ACL packet size
      stream = packet->data + packet->offset;
      STREAM_SKIP_UINT16(stream);
      UINT16_TO_STREAM(stream, max_data_size);

      packet->len = max_packet_size;
      callbacks->fragmented(packet, false);

      packet->offset += max_data_size;
      remaining_length -= max_data_size;
      packet->len = remaining_length;

      // Write the ACL header for the next fragment
      stream = packet->data + packet->offset;
      UINT16_TO_STREAM(stream, continuation_handle);
      UINT16_TO_STREAM(stream, remaining_length - HCI_ACL_PREAMBLE_SIZE);

      // Apparently L2CAP can set layer_specific to a max number of segments to transmit
      if (packet->layer_specific) {
        packet->layer_specific--;

        if (packet->layer_specific == 0) {
          packet->event = MSG_HC_TO_STACK_L2C_SEG_XMIT;
          callbacks->transmit_finished(packet, false);
          return;
        }
      }
    }
  }

  callbacks->fragmented(packet, true);
}

static void reassemble_and_dispatch(UNUSED_ATTR BT_HDR *packet) {
  if ((packet->event & MSG_EVT_MASK) == MSG_HC_TO_STACK_HCI_ACL) {
    uint8_t *stream = packet->data;
    uint16_t handle;
    uint16_t l2cap_length;
    uint16_t acl_length;

    STREAM_TO_UINT16(handle, stream);
    STREAM_TO_UINT16(acl_length, stream);
    STREAM_TO_UINT16(l2cap_length, stream);

    assert(acl_length == packet->len - HCI_ACL_PREAMBLE_SIZE);

    uint8_t boundary_flag = GET_BOUNDARY_FLAG(handle);
    handle = handle & HANDLE_MASK;

    BT_HDR *partial_packet = (BT_HDR *)hash_map_get(partial_packets, (void *)(uintptr_t)handle);

    if (boundary_flag == START_PACKET_BOUNDARY) {
      if (partial_packet) {
        ALOGW("%s found unfinished packet for handle with start packet. Dropping old.", __func__);

        hash_map_erase(partial_packets, (void *)(uintptr_t)handle);
        allocator->free(partial_packet);
      }

      uint16_t full_length = l2cap_length + L2CAP_HEADER_SIZE + HCI_ACL_PREAMBLE_SIZE;
      if (full_length <= packet->len) {
        if (full_length < packet->len)
          ALOGW("%s found l2cap full length %d less than the hci length %d.", __func__, l2cap_length, packet->len);

        callbacks->reassembled(packet);
        return;
      }

      partial_packet = (BT_HDR *)allocator->alloc(full_length + sizeof(BT_HDR));
      partial_packet->event = packet->event;
      partial_packet->len = full_length;
      partial_packet->offset = packet->len;

      memcpy(partial_packet->data, packet->data, packet->len);

      // Update the ACL data size to indicate the full expected length
      stream = partial_packet->data;
      STREAM_SKIP_UINT16(stream); // skip the handle
      UINT16_TO_STREAM(stream, full_length - HCI_ACL_PREAMBLE_SIZE);

      hash_map_set(partial_packets, (void *)(uintptr_t)handle, partial_packet);
      // Free the old packet buffer, since we don't need it anymore
      allocator->free(packet);
    } else {
      if (!partial_packet) {
        ALOGW("%s got continuation for unknown packet. Dropping it.", __func__);
        allocator->free(packet);
        return;
      }

      packet->offset = HCI_ACL_PREAMBLE_SIZE;
      uint16_t projected_offset = partial_packet->offset + (packet->len - HCI_ACL_PREAMBLE_SIZE);
      if (projected_offset > partial_packet->len) { // len stores the expected length
        ALOGW("%s got packet which would exceed expected length of %d. Truncating.", __func__, partial_packet->len);
        packet->len = partial_packet->len - partial_packet->offset;
        projected_offset = partial_packet->len;
      }

      memcpy(
        partial_packet->data + partial_packet->offset,
        packet->data + packet->offset,
        packet->len - packet->offset
      );

      // Free the old packet buffer, since we don't need it anymore
      allocator->free(packet);
      partial_packet->offset = projected_offset;

      if (partial_packet->offset == partial_packet->len) {
        hash_map_erase(partial_packets, (void *)(uintptr_t)handle);
        partial_packet->offset = 0;
        callbacks->reassembled(partial_packet);
      }
    }
  } else {
    callbacks->reassembled(packet);
  }
}

static const packet_fragmenter_interface_t interface = {
  init,

  set_acl_data_size,
  set_ble_acl_data_size,

  fragment_and_dispatch,
  reassemble_and_dispatch
};

const packet_fragmenter_interface_t *packet_fragmenter_get_interface() {
  return &interface;
}