Quelle  h4_parser.cc

//
// Copyright 2021 The Android Open Source Project
//
// 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.
//

#include "model/hci/h4_parser.h"  // for H4Parser, PacketType, H4Pars...

#include <array>
#include <cstddef>     // for size_t
#include <cstdint>     // for uint8_t, int32_t
#include <functional>  // for function
#include <utility>     // for move
#include <vector>      // for vector

#include "log.h"

namespace rootcanal {

void H4Parser::Reset() {
  state_ = HCI_TYPE;
  packet_.clear();
  bytes_wanted_ = 0;
  packet_type_ = 0;
}

size_t H4Parser::HciGetPacketLengthForType(PacketType type, const uint8_t* preamble) {
  static const size_t packet_length_offset[static_cast<size_t>(PacketType::ISO) + 1] = {
          0,
          H4Parser::COMMAND_LENGTH_OFFSET,
          H4Parser::ACL_LENGTH_OFFSET,
          H4Parser::SCO_LENGTH_OFFSET,
          H4Parser::EVENT_LENGTH_OFFSET,
          H4Parser::ISO_LENGTH_OFFSET,
  };

  size_t offset = packet_length_offset[static_cast<size_t>(type)];
  size_t size = preamble[offset];
  if (type == PacketType::ACL) {
    size |= ((size_t)preamble[offset + 1]) << 8;
  }
  if (type == PacketType::ISO) {
    size |= ((size_t)preamble[offset + 1] & 0x0fU) << 8;
  }
  return size;
}

H4Parser::H4Parser(PacketReadCallback command_cb, PacketReadCallback event_cb,
                   PacketReadCallback acl_cb, PacketReadCallback sco_cb, PacketReadCallback iso_cb,
                   bool enable_recovery_state)
    : command_cb_(std::move(command_cb)),
      event_cb_(std::move(event_cb)),
      acl_cb_(std::move(acl_cb)),
      sco_cb_(std::move(sco_cb)),
      iso_cb_(std::move(iso_cb)),
      enable_recovery_state_(enable_recovery_state) {}

void H4Parser::OnPacketReady() {
  switch (hci_packet_type_) {
    case PacketType::COMMAND:
      command_cb_(packet_);
      break;
    case PacketType::ACL:
      acl_cb_(packet_);
      break;
    case PacketType::SCO:
      sco_cb_(packet_);
      break;
    case PacketType::EVENT:
      event_cb_(packet_);
      break;
    case PacketType::ISO:
      iso_cb_(packet_);
      break;
    default:
      FATAL("Unimplemented packet type {}", fmt::underlying(hci_packet_type_));
  }
  // Get ready for the next type byte.
  hci_packet_type_ = PacketType::UNKNOWN;
}

size_t H4Parser::BytesRequested() {
  switch (state_) {
    case HCI_TYPE:
    case HCI_RECOVERY:
      return 1;
    case HCI_PREAMBLE:
    case HCI_PAYLOAD:
      return bytes_wanted_;
  }
}

bool H4Parser::Consume(const uint8_t* buffer, int32_t bytes_read) {
  size_t bytes_to_read = BytesRequested();
  if (bytes_read <= 0) {
    INFO("remote disconnected, or unhandled error?");
    return false;
  }
  if ((uint32_t)bytes_read > BytesRequested()) {
    FATAL("More bytes read ({}) than expected ({})!", bytes_read, bytes_to_read);
  }

  static const size_t preamble_size[static_cast<size_t>(PacketType::ISO) + 1] = {
          0,
          H4Parser::COMMAND_PREAMBLE_SIZE,
          H4Parser::ACL_PREAMBLE_SIZE,
          H4Parser::SCO_PREAMBLE_SIZE,
          H4Parser::EVENT_PREAMBLE_SIZE,
          H4Parser::ISO_PREAMBLE_SIZE,
  };
  switch (state_) {
    case HCI_TYPE:
      // bytes_read >= 1
      packet_type_ = *buffer;
      packet_.clear();
      break;

    case HCI_RECOVERY: {
      // Skip all received bytes until the HCI Reset command is received.
      // The parser can end up in a bad state when the host is restarted.
      const std::array<uint8_t, 4> reset_command{0x01, 0x03, 0x0c, 0x00};
      size_t offset = packet_.size();
      WARNING("Received byte in recovery state : 0x{:x}", *buffer);
      packet_.push_back(*buffer);

      // Last byte does not match expected byte in the sequence.
      // Drop all the bytes and start over.
      if (packet_[offset] != reset_command[offset]) {
        packet_.clear();
        // The mismatched byte can also be the first of the correct sequence.
        if (*buffer == reset_command[0]) {
          packet_.push_back(*buffer);
        }
      }

      // Received full reset command.
      if (packet_.size() == reset_command.size()) {
        INFO("Received HCI Reset command, exiting recovery state");
        // Pop the Idc from the received packet.
        packet_.erase(packet_.begin());
        bytes_wanted_ = 0;
      }
      break;
    }

    case HCI_PREAMBLE:
    case HCI_PAYLOAD:
      packet_.insert(packet_.end(), buffer, buffer + bytes_read);
      bytes_wanted_ -= bytes_read;
      break;
  }

  switch (state_) {
    case HCI_TYPE:
      hci_packet_type_ = static_cast<PacketType>(packet_type_);
      if (hci_packet_type_ != PacketType::ACL && hci_packet_type_ != PacketType::SCO &&
          hci_packet_type_ != PacketType::COMMAND && hci_packet_type_ != PacketType::EVENT &&
          hci_packet_type_ != PacketType::ISO) {
        if (!enable_recovery_state_) {
          FATAL("Received invalid packet type 0x{:x}", packet_type_);
        }
        ERROR("Received invalid packet type 0x{:x}, entering recovery state", packet_type_);
        state_ = HCI_RECOVERY;
        hci_packet_type_ = PacketType::COMMAND;
        bytes_wanted_ = 1;
      } else {
        state_ = HCI_PREAMBLE;
        bytes_wanted_ = preamble_size[static_cast<size_t>(hci_packet_type_)];
      }
      break;
    case HCI_PREAMBLE:
      if (bytes_wanted_ == 0) {
        size_t payload_size = HciGetPacketLengthForType(hci_packet_type_, packet_.data());
        if (payload_size == 0) {
          OnPacketReady();
          state_ = HCI_TYPE;
        } else {
          bytes_wanted_ = payload_size;
          state_ = HCI_PAYLOAD;
        }
      }
      break;
    case HCI_RECOVERY:
    case HCI_PAYLOAD:
      if (bytes_wanted_ == 0) {
        OnPacketReady();
        state_ = HCI_TYPE;
      }
      break;
  }
  return true;
}
}  // namespace rootcanal