mirror of
https://github.com/patchzyy/wiicompiled
synced 2026-10-20 13:36:47 -04:00
pass real wii remotes through emulated bluetooth
This commit is contained in:
patchzyy
@@ -0,0 +1,951 @@
|
||||
#include "bluetooth/bt_controller.h"
|
||||
|
||||
#include "bluetooth/bt_bytes.h"
|
||||
#include "bluetooth/bt_remote_link.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
|
||||
namespace Bluetooth {
|
||||
namespace HciDetail {
|
||||
|
||||
|
||||
constexpr uint16_t kOgfLinkControl = 0x01;
|
||||
constexpr uint16_t kOgfLinkPolicy = 0x02;
|
||||
constexpr uint16_t kOgfControllerBaseband = 0x03;
|
||||
constexpr uint16_t kOgfInformational = 0x04;
|
||||
constexpr uint16_t kOgfStatus = 0x05;
|
||||
constexpr uint16_t kOgfVendor = 0x3F;
|
||||
|
||||
constexpr uint8_t kEventInquiryComplete = 0x01;
|
||||
constexpr uint8_t kEventInquiryResult = 0x02;
|
||||
constexpr uint8_t kEventConnectionComplete = 0x03;
|
||||
constexpr uint8_t kEventDisconnectionComplete = 0x05;
|
||||
constexpr uint8_t kEventAuthenticationComplete = 0x06;
|
||||
constexpr uint8_t kEventRemoteNameComplete = 0x07;
|
||||
constexpr uint8_t kEventEncryptionChange = 0x08;
|
||||
constexpr uint8_t kEventRemoteFeatures = 0x0B;
|
||||
constexpr uint8_t kEventRemoteVersion = 0x0C;
|
||||
constexpr uint8_t kEventCommandComplete = 0x0E;
|
||||
constexpr uint8_t kEventCommandStatus = 0x0F;
|
||||
constexpr uint8_t kEventRoleChange = 0x12;
|
||||
constexpr uint8_t kEventNumberOfCompletedPackets = 0x13;
|
||||
constexpr uint8_t kEventModeChange = 0x14;
|
||||
constexpr uint8_t kEventReturnLinkKeys = 0x15;
|
||||
constexpr uint8_t kEventPinCodeRequest = 0x16;
|
||||
constexpr uint8_t kEventLinkKeyRequest = 0x17;
|
||||
constexpr uint8_t kEventLinkKeyNotification = 0x18;
|
||||
constexpr uint8_t kEventClockOffset = 0x1C;
|
||||
constexpr uint8_t kEventPacketTypeChanged = 0x1D;
|
||||
constexpr uint8_t kEventInquiryResultWithRssi = 0x22;
|
||||
constexpr uint8_t kEventExtendedInquiryResult = 0x2F;
|
||||
|
||||
constexpr uint8_t kStatusSuccess = 0x00;
|
||||
constexpr uint8_t kStatusUnknownCommand = 0x01;
|
||||
constexpr uint8_t kStatusUnknownConnection = 0x02;
|
||||
constexpr uint8_t kStatusPageTimeout = 0x04;
|
||||
constexpr uint8_t kStatusAuthenticationFailure = 0x05;
|
||||
constexpr uint8_t kStatusKeyMissing = 0x06;
|
||||
constexpr uint8_t kStatusUnsupportedFeature = 0x11;
|
||||
constexpr uint8_t kReasonLocalHostTerminated = 0x16;
|
||||
|
||||
// What a Wii's BCM2045 reports (the version makes WUD apply its usual patch).
|
||||
constexpr BdAddr kLocalAddress = {0x11, 0x02, 0x19, 0x79, 0x00, 0xFF};
|
||||
constexpr std::array<uint8_t, 8> kLocalFeatures = {0xFF, 0xFF, 0x8D, 0xFE, 0x9B, 0xF9, 0x00, 0x80};
|
||||
// Never advertise more than 10 ACL buffers: the stack keeps the count in a u8.
|
||||
constexpr uint16_t kAclBufferSize = 339;
|
||||
constexpr uint16_t kAclBufferCount = 10;
|
||||
constexpr uint8_t kScoBufferSize = 64;
|
||||
|
||||
// A real RVL-CNT-01.
|
||||
constexpr std::array<uint8_t, 8> kRemoteFeatures = {0xBC, 0x02, 0x04, 0x38, 0x08, 0x00, 0x00, 0x00};
|
||||
constexpr uint8_t kRemoteLmpVersion = 0x02;
|
||||
constexpr uint16_t kRemoteManufacturer = 0x000F;
|
||||
constexpr uint16_t kRemoteLmpSubversion = 0x0229;
|
||||
constexpr uint16_t kRemoteClockOffset = 0x3818;
|
||||
constexpr uint8_t kInquiryRssi = 0xD8; // -40 dBm
|
||||
constexpr uint64_t kInquiryUnitUs = 1'280'000;
|
||||
constexpr uint16_t kMaxStoredKeys = 0x00FF;
|
||||
constexpr uint8_t kKeysPerEvent = 11;
|
||||
|
||||
void PutAddr(std::vector<uint8_t>& out, const BdAddr& addr) {
|
||||
out.insert(out.end(), addr.begin(), addr.end());
|
||||
}
|
||||
|
||||
BdAddr ReadAddr(const uint8_t* p) {
|
||||
BdAddr addr{};
|
||||
std::memcpy(addr.data(), p, addr.size());
|
||||
return addr;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> Event(uint8_t code, const std::vector<uint8_t>& params) {
|
||||
std::vector<uint8_t> event;
|
||||
event.reserve(2 + params.size());
|
||||
event.push_back(code);
|
||||
event.push_back(static_cast<uint8_t>(params.size()));
|
||||
event.insert(event.end(), params.begin(), params.end());
|
||||
return event;
|
||||
}
|
||||
|
||||
std::array<uint8_t, 16> SlotLinkKey(size_t slot) {
|
||||
std::array<uint8_t, 16> key{};
|
||||
for (size_t i = 0; i < key.size(); ++i) {
|
||||
key[i] = static_cast<uint8_t>(0xA0 + slot + i * 0x11);
|
||||
}
|
||||
return key;
|
||||
}
|
||||
|
||||
} // namespace HciDetail
|
||||
|
||||
using namespace HciDetail;
|
||||
|
||||
Controller::Controller(RemoteHost& host) : m_host(host) {
|
||||
for (size_t slot = 0; slot < kRemoteSlots; ++slot) {
|
||||
m_links[slot] = std::make_unique<RemoteLink>(*this, slot);
|
||||
m_storedKeys.push_back({RemoteAddress(slot), SlotLinkKey(slot)});
|
||||
}
|
||||
ResetState(true);
|
||||
}
|
||||
|
||||
Controller::~Controller() = default;
|
||||
|
||||
BdAddr Controller::LocalAddress() {
|
||||
return kLocalAddress;
|
||||
}
|
||||
|
||||
BdAddr Controller::RemoteAddress(size_t slot) {
|
||||
return {0x11, 0x02, 0x19, 0x79, 0x00, static_cast<uint8_t>(slot)};
|
||||
}
|
||||
|
||||
uint16_t Controller::ConnectionHandle(size_t slot) {
|
||||
return static_cast<uint16_t>(0x0100 + slot);
|
||||
}
|
||||
|
||||
void Controller::Open(uint64_t nowUs) {
|
||||
(void)nowUs;
|
||||
ResetState(true);
|
||||
m_open = true;
|
||||
}
|
||||
|
||||
void Controller::Close() {
|
||||
ResetState(true);
|
||||
m_open = false;
|
||||
}
|
||||
|
||||
void Controller::ResetState(bool keepStoredKeys) {
|
||||
for (size_t slot = 0; slot < kRemoteSlots; ++slot) {
|
||||
RemoteLink& link = *m_links[slot];
|
||||
// A reset drops the radio links: the remotes lose their connection.
|
||||
if (link.State() == LinkState::Ready) {
|
||||
m_host.OnGuestDisconnect(slot);
|
||||
}
|
||||
link.Reset();
|
||||
}
|
||||
if (!keepStoredKeys) {
|
||||
m_storedKeys.clear();
|
||||
}
|
||||
if (m_hostInquiryNotified) {
|
||||
m_host.OnGuestInquiry(false);
|
||||
m_hostInquiryNotified = false;
|
||||
}
|
||||
m_inquiry = {};
|
||||
m_scanEnable = 0;
|
||||
m_pageTimeoutSlots = 0x2000;
|
||||
m_inquiryMode = 0;
|
||||
m_classOfDevice = {};
|
||||
m_localName = {};
|
||||
m_completedPackets = {};
|
||||
for (Reassembly& r : m_reassembly) r = {};
|
||||
m_events.clear();
|
||||
m_acl.clear();
|
||||
m_pending.clear();
|
||||
m_authPending = {};
|
||||
}
|
||||
|
||||
LinkState Controller::SlotLinkState(size_t slot) const {
|
||||
return slot < kRemoteSlots ? m_links[slot]->State() : LinkState::Down;
|
||||
}
|
||||
|
||||
int Controller::SlotForAddress(const BdAddr& addr) const {
|
||||
for (size_t slot = 0; slot < kRemoteSlots; ++slot) {
|
||||
if (RemoteAddress(slot) == addr) return static_cast<int>(slot);
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int Controller::SlotForHandle(uint16_t handle) const {
|
||||
for (size_t slot = 0; slot < kRemoteSlots; ++slot) {
|
||||
if (ConnectionHandle(slot) == (handle & 0x0FFF) &&
|
||||
m_links[slot]->BasebandState() == RemoteLink::Baseband::Connected) {
|
||||
return static_cast<int>(slot);
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
const Controller::StoredKey* Controller::FindStoredKey(const BdAddr& addr) const {
|
||||
for (const StoredKey& key : m_storedKeys) {
|
||||
if (key.addr == addr) return &key;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void Controller::QueueEvent(std::vector<uint8_t> event) {
|
||||
m_events.push_back(std::move(event));
|
||||
}
|
||||
|
||||
void Controller::QueueAcl(uint16_t handle, const std::vector<uint8_t>& l2capFrame) {
|
||||
std::vector<uint8_t> packet;
|
||||
packet.reserve(4 + l2capFrame.size());
|
||||
// Packet boundary 0x2: first (and only) fragment, automatically flushable.
|
||||
PutLe16(packet, static_cast<uint16_t>((handle & 0x0FFF) | 0x2000));
|
||||
PutLe16(packet, static_cast<uint16_t>(l2capFrame.size()));
|
||||
packet.insert(packet.end(), l2capFrame.begin(), l2capFrame.end());
|
||||
m_acl.push_back(std::move(packet));
|
||||
}
|
||||
|
||||
bool Controller::PopEvent(std::vector<uint8_t>& event) {
|
||||
if (m_events.empty()) return false;
|
||||
event = std::move(m_events.front());
|
||||
m_events.pop_front();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Controller::PopAcl(std::vector<uint8_t>& packet) {
|
||||
if (!m_events.empty() || m_acl.empty()) return false;
|
||||
packet = std::move(m_acl.front());
|
||||
m_acl.pop_front();
|
||||
return true;
|
||||
}
|
||||
|
||||
void Controller::CommandComplete(uint16_t opcode, const std::vector<uint8_t>& params) {
|
||||
std::vector<uint8_t> body = {0x01};
|
||||
PutLe16(body, opcode);
|
||||
body.insert(body.end(), params.begin(), params.end());
|
||||
QueueEvent(Event(kEventCommandComplete, body));
|
||||
}
|
||||
|
||||
void Controller::CommandStatus(uint16_t opcode, uint8_t status) {
|
||||
std::vector<uint8_t> body = {status, 0x01};
|
||||
PutLe16(body, opcode);
|
||||
QueueEvent(Event(kEventCommandStatus, body));
|
||||
}
|
||||
|
||||
void Controller::UnknownCommand(uint16_t opcode) {
|
||||
std::cerr << "[bluetooth] unsupported HCI command 0x" << std::hex << opcode << std::dec << std::endl;
|
||||
CommandStatus(opcode, kStatusUnknownCommand);
|
||||
}
|
||||
|
||||
void Controller::ExecuteCommand(const uint8_t* packet, size_t size, uint64_t nowUs) {
|
||||
if (size < 3) return;
|
||||
const uint16_t opcode = Le16(packet);
|
||||
const size_t length = packet[2];
|
||||
if (3 + length > size) {
|
||||
std::cerr << "[bluetooth] truncated HCI command 0x" << std::hex << opcode << std::dec << std::endl;
|
||||
return;
|
||||
}
|
||||
const uint8_t* p = packet + 3;
|
||||
switch (opcode >> 10) {
|
||||
case kOgfLinkControl: HandleLinkControl(opcode, p, length, nowUs); break;
|
||||
case kOgfLinkPolicy: HandleLinkPolicy(opcode, p, length); break;
|
||||
case kOgfControllerBaseband: HandleControllerBaseband(opcode, p, length, nowUs); break;
|
||||
case kOgfInformational: HandleInformational(opcode, p, length); break;
|
||||
case kOgfStatus: HandleStatusParameters(opcode, p, length); break;
|
||||
case kOgfVendor: HandleVendor(opcode, p, length); break;
|
||||
default: UnknownCommand(opcode); break;
|
||||
}
|
||||
}
|
||||
|
||||
void Controller::HandleLinkControl(uint16_t opcode, const uint8_t* p, size_t n, uint64_t nowUs) {
|
||||
const auto handleSlot = [&](size_t offset) { return n >= offset + 2 ? SlotForHandle(Le16(p + offset)) : -1; };
|
||||
switch (opcode & 0x3FF) {
|
||||
case 0x0001: { // Inquiry
|
||||
if (n < 5) break;
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
m_inquiry = {};
|
||||
m_inquiry.active = true;
|
||||
m_inquiry.endUs = nowUs + std::max<uint8_t>(p[3], 1) * kInquiryUnitUs;
|
||||
m_inquiry.maxResponses = p[4];
|
||||
if (!m_hostInquiryNotified) {
|
||||
m_host.OnGuestInquiry(true);
|
||||
m_hostInquiryNotified = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x0002: // Inquiry Cancel
|
||||
m_inquiry = {};
|
||||
if (m_hostInquiryNotified) {
|
||||
m_host.OnGuestInquiry(false);
|
||||
m_hostInquiryNotified = false;
|
||||
}
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x0003: // Periodic Inquiry Mode
|
||||
case 0x0004: // Exit Periodic Inquiry Mode
|
||||
CommandComplete(opcode, {kStatusUnsupportedFeature});
|
||||
return;
|
||||
case 0x0005: { // Create Connection
|
||||
if (n < 6) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
const int slot = SlotForAddress(addr);
|
||||
if (slot >= 0 && m_links[slot]->OnPaged(nowUs)) {
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
PutAddr(body, addr);
|
||||
body.insert(body.end(), {0x01, 0x00});
|
||||
QueueEvent(Event(kEventConnectionComplete, body));
|
||||
} else {
|
||||
std::vector<uint8_t> body = {kStatusPageTimeout, 0x00, 0x00};
|
||||
PutAddr(body, addr);
|
||||
body.insert(body.end(), {0x01, 0x00});
|
||||
m_pending.push_back({nowUs + PageTimeoutUs(), Event(kEventConnectionComplete, body)});
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x0006: { // Disconnect
|
||||
const int slot = handleSlot(0);
|
||||
if (slot < 0 || n < 3) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
body.push_back(kReasonLocalHostTerminated);
|
||||
QueueEvent(Event(kEventDisconnectionComplete, body));
|
||||
m_completedPackets[slot] = 0;
|
||||
m_reassembly[slot] = {};
|
||||
m_authPending[slot] = false;
|
||||
m_links[slot]->OnDisconnected(true, nowUs);
|
||||
return;
|
||||
}
|
||||
case 0x0008: { // Create Connection Cancel
|
||||
if (n < 6) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
auto it = std::find_if(m_pending.begin(), m_pending.end(), [&](const PendingEvent& e) {
|
||||
return e.event[0] == kEventConnectionComplete && ReadAddr(&e.event[5]) == addr;
|
||||
});
|
||||
std::vector<uint8_t> body = {static_cast<uint8_t>(it == m_pending.end() ? kStatusUnknownConnection
|
||||
: kStatusSuccess)};
|
||||
PutAddr(body, addr);
|
||||
CommandComplete(opcode, body);
|
||||
if (it != m_pending.end()) {
|
||||
std::vector<uint8_t> complete = it->event;
|
||||
complete[2] = kStatusUnknownConnection;
|
||||
m_pending.erase(it);
|
||||
QueueEvent(std::move(complete));
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x0009: { // Accept Connection Request
|
||||
if (n < 7) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
const uint8_t role = p[6];
|
||||
const int slot = SlotForAddress(addr);
|
||||
if (slot < 0 || !m_links[slot]->IsRequesting()) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
if (role == 0x00) {
|
||||
std::vector<uint8_t> change = {kStatusSuccess};
|
||||
PutAddr(change, addr);
|
||||
change.push_back(0x00);
|
||||
QueueEvent(Event(kEventRoleChange, change));
|
||||
}
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
PutAddr(body, addr);
|
||||
body.insert(body.end(), {0x01, 0x00});
|
||||
QueueEvent(Event(kEventConnectionComplete, body));
|
||||
m_completedPackets[slot] = 0;
|
||||
m_reassembly[slot] = {};
|
||||
m_links[slot]->OnAccepted(role);
|
||||
return;
|
||||
}
|
||||
case 0x000A: { // Reject Connection Request
|
||||
if (n < 7) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
const int slot = SlotForAddress(addr);
|
||||
if (slot < 0 || !m_links[slot]->IsRequesting()) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {p[6], 0x00, 0x00};
|
||||
PutAddr(body, addr);
|
||||
body.insert(body.end(), {0x01, 0x00});
|
||||
QueueEvent(Event(kEventConnectionComplete, body));
|
||||
m_links[slot]->OnRejected(p[6], nowUs);
|
||||
return;
|
||||
}
|
||||
case 0x000B: { // Link Key Request Reply
|
||||
if (n < 22) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutAddr(body, addr);
|
||||
CommandComplete(opcode, body);
|
||||
const int slot = SlotForAddress(addr);
|
||||
if (slot >= 0 && m_authPending[slot]) {
|
||||
m_authPending[slot] = false;
|
||||
const bool match = std::equal(p + 6, p + 22, SlotLinkKey(slot).begin());
|
||||
std::vector<uint8_t> auth = {static_cast<uint8_t>(match ? kStatusSuccess : kStatusKeyMissing)};
|
||||
PutLe16(auth, ConnectionHandle(slot));
|
||||
QueueEvent(Event(kEventAuthenticationComplete, auth));
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x000C: { // Link Key Request Negative Reply
|
||||
if (n < 6) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutAddr(body, addr);
|
||||
CommandComplete(opcode, body);
|
||||
const int slot = SlotForAddress(addr);
|
||||
if (slot >= 0 && m_authPending[slot]) {
|
||||
std::vector<uint8_t> request;
|
||||
PutAddr(request, addr);
|
||||
QueueEvent(Event(kEventPinCodeRequest, request));
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x000D: { // PIN Code Request Reply
|
||||
if (n < 23) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutAddr(body, addr);
|
||||
CommandComplete(opcode, body);
|
||||
const int slot = SlotForAddress(addr);
|
||||
if (slot < 0 || !m_authPending[slot]) return;
|
||||
m_authPending[slot] = false;
|
||||
// A remote takes the host's address (SYNC button) or its own (1+2) as PIN.
|
||||
const uint8_t pinLength = p[6];
|
||||
const uint8_t* pin = p + 7;
|
||||
const BdAddr local = LocalAddress();
|
||||
const bool valid = pinLength == 6 && (std::equal(pin, pin + 6, local.begin()) ||
|
||||
std::equal(pin, pin + 6, addr.begin()));
|
||||
if (valid) {
|
||||
std::vector<uint8_t> notification;
|
||||
PutAddr(notification, addr);
|
||||
const auto key = SlotLinkKey(slot);
|
||||
notification.insert(notification.end(), key.begin(), key.end());
|
||||
notification.push_back(0x00); // combination key
|
||||
QueueEvent(Event(kEventLinkKeyNotification, notification));
|
||||
}
|
||||
std::vector<uint8_t> auth = {static_cast<uint8_t>(valid ? kStatusSuccess : kStatusAuthenticationFailure)};
|
||||
PutLe16(auth, ConnectionHandle(slot));
|
||||
QueueEvent(Event(kEventAuthenticationComplete, auth));
|
||||
return;
|
||||
}
|
||||
case 0x000E: { // PIN Code Request Negative Reply
|
||||
if (n < 6) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutAddr(body, addr);
|
||||
CommandComplete(opcode, body);
|
||||
const int slot = SlotForAddress(addr);
|
||||
if (slot >= 0 && m_authPending[slot]) {
|
||||
m_authPending[slot] = false;
|
||||
std::vector<uint8_t> auth = {kStatusKeyMissing};
|
||||
PutLe16(auth, ConnectionHandle(slot));
|
||||
QueueEvent(Event(kEventAuthenticationComplete, auth));
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x000F: { // Change Connection Packet Type
|
||||
const int slot = handleSlot(0);
|
||||
if (slot < 0 || n < 4) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
PutLe16(body, Le16(p + 2));
|
||||
QueueEvent(Event(kEventPacketTypeChanged, body));
|
||||
return;
|
||||
}
|
||||
case 0x0011: { // Authentication Requested
|
||||
const int slot = handleSlot(0);
|
||||
if (slot < 0) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
m_authPending[slot] = true;
|
||||
std::vector<uint8_t> request;
|
||||
PutAddr(request, RemoteAddress(slot));
|
||||
QueueEvent(Event(kEventLinkKeyRequest, request));
|
||||
return;
|
||||
}
|
||||
case 0x0013: { // Set Connection Encryption
|
||||
const int slot = handleSlot(0);
|
||||
if (slot < 0 || n < 3) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
body.push_back(p[2]);
|
||||
QueueEvent(Event(kEventEncryptionChange, body));
|
||||
return;
|
||||
}
|
||||
case 0x0019: { // Remote Name Request
|
||||
if (n < 6) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
const int slot = SlotForAddress(addr);
|
||||
const bool reachable = slot >= 0 && (m_links[slot]->BasebandState() == RemoteLink::Baseband::Connected ||
|
||||
m_links[slot]->Discoverable());
|
||||
std::vector<uint8_t> body = {static_cast<uint8_t>(reachable ? kStatusSuccess : kStatusPageTimeout)};
|
||||
PutAddr(body, addr);
|
||||
std::array<char, 248> name{};
|
||||
if (reachable) {
|
||||
std::strncpy(name.data(), m_host.DeviceName(slot), name.size() - 1);
|
||||
}
|
||||
body.insert(body.end(), name.begin(), name.end());
|
||||
if (reachable) {
|
||||
QueueEvent(Event(kEventRemoteNameComplete, body));
|
||||
} else {
|
||||
m_pending.push_back({nowUs + PageTimeoutUs(), Event(kEventRemoteNameComplete, body)});
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x001A: { // Remote Name Request Cancel
|
||||
if (n < 6) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
auto it = std::find_if(m_pending.begin(), m_pending.end(), [&](const PendingEvent& e) {
|
||||
return e.event[0] == kEventRemoteNameComplete && ReadAddr(&e.event[3]) == addr;
|
||||
});
|
||||
std::vector<uint8_t> body = {static_cast<uint8_t>(it == m_pending.end() ? kStatusUnknownConnection
|
||||
: kStatusSuccess)};
|
||||
PutAddr(body, addr);
|
||||
CommandComplete(opcode, body);
|
||||
if (it != m_pending.end()) {
|
||||
std::vector<uint8_t> complete = it->event;
|
||||
complete[2] = kStatusUnknownConnection;
|
||||
m_pending.erase(it);
|
||||
QueueEvent(std::move(complete));
|
||||
}
|
||||
return;
|
||||
}
|
||||
case 0x001B: { // Read Remote Supported Features
|
||||
const int slot = handleSlot(0);
|
||||
if (slot < 0) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
body.insert(body.end(), kRemoteFeatures.begin(), kRemoteFeatures.end());
|
||||
QueueEvent(Event(kEventRemoteFeatures, body));
|
||||
return;
|
||||
}
|
||||
case 0x001D: { // Read Remote Version Information
|
||||
const int slot = handleSlot(0);
|
||||
if (slot < 0) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
body.push_back(kRemoteLmpVersion);
|
||||
PutLe16(body, kRemoteManufacturer);
|
||||
PutLe16(body, kRemoteLmpSubversion);
|
||||
QueueEvent(Event(kEventRemoteVersion, body));
|
||||
return;
|
||||
}
|
||||
case 0x001F: { // Read Clock Offset
|
||||
const int slot = handleSlot(0);
|
||||
if (slot < 0) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
PutLe16(body, kRemoteClockOffset);
|
||||
QueueEvent(Event(kEventClockOffset, body));
|
||||
return;
|
||||
}
|
||||
case 0x0007: // Add SCO Connection
|
||||
case 0x0028: // Setup Synchronous Connection
|
||||
case 0x0029: // Accept Synchronous Connection Request
|
||||
case 0x002A: // Reject Synchronous Connection Request
|
||||
CommandStatus(opcode, kStatusUnsupportedFeature);
|
||||
return;
|
||||
default:
|
||||
UnknownCommand(opcode);
|
||||
return;
|
||||
}
|
||||
std::cerr << "[bluetooth] short HCI command 0x" << std::hex << opcode << std::dec << std::endl;
|
||||
CommandStatus(opcode, kStatusUnknownCommand);
|
||||
}
|
||||
|
||||
void Controller::HandleLinkPolicy(uint16_t opcode, const uint8_t* p, size_t n) {
|
||||
const int slot = n >= 2 ? SlotForHandle(Le16(p)) : -1;
|
||||
const auto modeChange = [&](uint8_t mode, uint16_t interval) {
|
||||
if (slot < 0) {
|
||||
CommandStatus(opcode, kStatusUnknownConnection);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, ConnectionHandle(static_cast<size_t>(slot)));
|
||||
body.push_back(mode);
|
||||
PutLe16(body, interval);
|
||||
QueueEvent(Event(kEventModeChange, body));
|
||||
};
|
||||
const auto handleReply = [&](std::vector<uint8_t> extra) {
|
||||
std::vector<uint8_t> body = {static_cast<uint8_t>(slot < 0 ? kStatusUnknownConnection : kStatusSuccess)};
|
||||
PutLe16(body, n >= 2 ? Le16(p) : 0);
|
||||
body.insert(body.end(), extra.begin(), extra.end());
|
||||
CommandComplete(opcode, body);
|
||||
};
|
||||
switch (opcode & 0x3FF) {
|
||||
case 0x0001: modeChange(0x01, n >= 4 ? Le16(p + 2) : 0); return; // Hold Mode
|
||||
case 0x0003: modeChange(0x02, n >= 4 ? Le16(p + 2) : 0); return; // Sniff Mode
|
||||
case 0x0004: modeChange(0x00, 0); return; // Exit Sniff Mode
|
||||
case 0x0005: modeChange(0x03, n >= 4 ? Le16(p + 2) : 0); return; // Park State
|
||||
case 0x0006: modeChange(0x00, 0); return; // Exit Park State
|
||||
case 0x0009: handleReply({0x00}); return; // Role Discovery
|
||||
case 0x000B: { // Switch Role
|
||||
if (n < 7) break;
|
||||
CommandStatus(opcode, kStatusSuccess);
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
body.insert(body.end(), p, p + 7);
|
||||
QueueEvent(Event(kEventRoleChange, body));
|
||||
return;
|
||||
}
|
||||
case 0x000C: handleReply({0x05, 0x00}); return; // Read Link Policy Settings
|
||||
case 0x000D: handleReply({}); return; // Write Link Policy Settings
|
||||
case 0x000E: CommandComplete(opcode, {kStatusSuccess, 0x05, 0x00}); return; // Read Default Link Policy
|
||||
case 0x000F: CommandComplete(opcode, {kStatusSuccess}); return; // Write Default Link Policy
|
||||
default:
|
||||
UnknownCommand(opcode);
|
||||
return;
|
||||
}
|
||||
CommandStatus(opcode, kStatusUnknownCommand);
|
||||
}
|
||||
|
||||
void Controller::HandleControllerBaseband(uint16_t opcode, const uint8_t* p, size_t n, uint64_t nowUs) {
|
||||
(void)nowUs;
|
||||
const auto handleReply = [&](std::vector<uint8_t> extra) {
|
||||
const int slot = n >= 2 ? SlotForHandle(Le16(p)) : -1;
|
||||
std::vector<uint8_t> body = {static_cast<uint8_t>(slot < 0 ? kStatusUnknownConnection : kStatusSuccess)};
|
||||
PutLe16(body, n >= 2 ? Le16(p) : 0);
|
||||
body.insert(body.end(), extra.begin(), extra.end());
|
||||
CommandComplete(opcode, body);
|
||||
};
|
||||
switch (opcode & 0x3FF) {
|
||||
case 0x0003: // Reset
|
||||
ResetState(true);
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x000D: { // Read Stored Link Key
|
||||
if (n < 7) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
const bool readAll = p[6] == 0x01;
|
||||
std::vector<StoredKey> keys;
|
||||
for (const StoredKey& key : m_storedKeys) {
|
||||
if (readAll || key.addr == addr) keys.push_back(key);
|
||||
}
|
||||
for (size_t start = 0; start < keys.size(); start += kKeysPerEvent) {
|
||||
const size_t count = std::min<size_t>(kKeysPerEvent, keys.size() - start);
|
||||
std::vector<uint8_t> body = {static_cast<uint8_t>(count)};
|
||||
for (size_t i = start; i < start + count; ++i) {
|
||||
PutAddr(body, keys[i].addr);
|
||||
body.insert(body.end(), keys[i].key.begin(), keys[i].key.end());
|
||||
}
|
||||
QueueEvent(Event(kEventReturnLinkKeys, body));
|
||||
}
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, kMaxStoredKeys);
|
||||
PutLe16(body, static_cast<uint16_t>(keys.size()));
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
}
|
||||
case 0x0011: { // Write Stored Link Key
|
||||
if (n < 1) break;
|
||||
const uint8_t count = p[0];
|
||||
if (n < 1 + count * size_t{22}) break;
|
||||
for (uint8_t i = 0; i < count; ++i) {
|
||||
const uint8_t* entry = p + 1 + i * 22;
|
||||
StoredKey key{ReadAddr(entry), {}};
|
||||
std::copy(entry + 6, entry + 22, key.key.begin());
|
||||
auto existing = std::find_if(m_storedKeys.begin(), m_storedKeys.end(),
|
||||
[&](const StoredKey& k) { return k.addr == key.addr; });
|
||||
if (existing != m_storedKeys.end()) {
|
||||
*existing = key;
|
||||
} else if (m_storedKeys.size() < kMaxStoredKeys) {
|
||||
m_storedKeys.push_back(key);
|
||||
}
|
||||
}
|
||||
CommandComplete(opcode, {kStatusSuccess, count});
|
||||
return;
|
||||
}
|
||||
case 0x0012: { // Delete Stored Link Key
|
||||
if (n < 7) break;
|
||||
const BdAddr addr = ReadAddr(p);
|
||||
const bool deleteAll = p[6] == 0x01;
|
||||
const size_t before = m_storedKeys.size();
|
||||
m_storedKeys.erase(std::remove_if(m_storedKeys.begin(), m_storedKeys.end(),
|
||||
[&](const StoredKey& k) { return deleteAll || k.addr == addr; }),
|
||||
m_storedKeys.end());
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, static_cast<uint16_t>(before - m_storedKeys.size()));
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
}
|
||||
case 0x0013: // Change Local Name
|
||||
if (n < m_localName.size()) break;
|
||||
std::memcpy(m_localName.data(), p, m_localName.size());
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x0014: { // Read Local Name
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
body.insert(body.end(), m_localName.begin(), m_localName.end());
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
}
|
||||
case 0x0018: // Write Page Timeout
|
||||
if (n < 2) break;
|
||||
m_pageTimeoutSlots = Le16(p);
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x0019: // Read Scan Enable
|
||||
CommandComplete(opcode, {kStatusSuccess, m_scanEnable});
|
||||
return;
|
||||
case 0x001A: // Write Scan Enable
|
||||
if (n < 1) break;
|
||||
m_scanEnable = p[0];
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x0023: // Read Class of Device
|
||||
CommandComplete(opcode, {kStatusSuccess, m_classOfDevice[0], m_classOfDevice[1], m_classOfDevice[2]});
|
||||
return;
|
||||
case 0x0024: // Write Class of Device
|
||||
if (n < 3) break;
|
||||
std::copy(p, p + 3, m_classOfDevice.begin());
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x0045: // Write Inquiry Mode
|
||||
if (n < 1) break;
|
||||
m_inquiryMode = p[0];
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x0028: // Write Automatic Flush Timeout
|
||||
case 0x0037: // Write Link Supervision Timeout
|
||||
handleReply({});
|
||||
return;
|
||||
case 0x0036: // Read Link Supervision Timeout
|
||||
handleReply({0x80, 0x0C});
|
||||
return;
|
||||
case 0x0001: // Set Event Mask
|
||||
case 0x0005: // Set Event Filter
|
||||
case 0x000A: // Write PIN Type
|
||||
case 0x0016: // Write Connection Accept Timeout
|
||||
case 0x001C: // Write Page Scan Activity
|
||||
case 0x001E: // Write Inquiry Scan Activity
|
||||
case 0x0020: // Write Authentication Enable
|
||||
case 0x0022: // Write Encryption Mode
|
||||
case 0x0031: // Set Controller To Host Flow Control
|
||||
case 0x0033: // Host Buffer Size
|
||||
case 0x003A: // Write Current IAC LAP
|
||||
case 0x003F: // Set AFH Host Channel Classification
|
||||
case 0x0043: // Write Inquiry Scan Type
|
||||
case 0x0047: // Write Page Scan Type
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
return;
|
||||
case 0x0035: // Host Number Of Completed Packets: no event
|
||||
return;
|
||||
default:
|
||||
UnknownCommand(opcode);
|
||||
return;
|
||||
}
|
||||
std::cerr << "[bluetooth] short HCI command 0x" << std::hex << opcode << std::dec << std::endl;
|
||||
CommandComplete(opcode, {kStatusUnknownCommand});
|
||||
}
|
||||
|
||||
void Controller::HandleInformational(uint16_t opcode, const uint8_t* p, size_t n) {
|
||||
(void)p;
|
||||
(void)n;
|
||||
switch (opcode & 0x3FF) {
|
||||
case 0x0001: { // Read Local Version Information
|
||||
std::vector<uint8_t> body = {kStatusSuccess, 0x03};
|
||||
PutLe16(body, 0x40A7);
|
||||
body.push_back(0x03);
|
||||
PutLe16(body, 0x000F);
|
||||
PutLe16(body, 0x430E);
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
}
|
||||
case 0x0003: { // Read Local Supported Features
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
body.insert(body.end(), kLocalFeatures.begin(), kLocalFeatures.end());
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
}
|
||||
case 0x0005: { // Read Buffer Size
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutLe16(body, kAclBufferSize);
|
||||
body.push_back(kScoBufferSize);
|
||||
PutLe16(body, kAclBufferCount);
|
||||
PutLe16(body, 0);
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
}
|
||||
case 0x0009: { // Read BD_ADDR
|
||||
std::vector<uint8_t> body = {kStatusSuccess};
|
||||
PutAddr(body, LocalAddress());
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
}
|
||||
default:
|
||||
UnknownCommand(opcode);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Controller::HandleStatusParameters(uint16_t opcode, const uint8_t* p, size_t n) {
|
||||
const int slot = n >= 2 ? SlotForHandle(Le16(p)) : -1;
|
||||
std::vector<uint8_t> body = {static_cast<uint8_t>(slot < 0 ? kStatusUnknownConnection : kStatusSuccess)};
|
||||
PutLe16(body, n >= 2 ? Le16(p) : 0);
|
||||
switch (opcode & 0x3FF) {
|
||||
case 0x0003: // Get Link Quality
|
||||
body.push_back(0xFF);
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
case 0x0005: // Read RSSI: 0 = inside the golden receive power range
|
||||
body.push_back(0x00);
|
||||
CommandComplete(opcode, body);
|
||||
return;
|
||||
default:
|
||||
UnknownCommand(opcode);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Controller::HandleVendor(uint16_t opcode, const uint8_t* p, size_t n) {
|
||||
(void)p;
|
||||
(void)n;
|
||||
// WUD's firmware patch upload (0xFC4C writes, 0xFC4F install) and friends.
|
||||
CommandComplete(opcode, {kStatusSuccess});
|
||||
}
|
||||
|
||||
void Controller::ReceiveAcl(const uint8_t* packet, size_t size, uint64_t nowUs) {
|
||||
(void)nowUs;
|
||||
if (size < 4) return;
|
||||
const uint16_t header = Le16(packet);
|
||||
const uint16_t length = Le16(packet + 2);
|
||||
if (length != size - 4) {
|
||||
std::cerr << "[bluetooth] dropped malformed ACL packet" << std::endl;
|
||||
return;
|
||||
}
|
||||
const int slot = SlotForHandle(header & 0x0FFF);
|
||||
if (slot < 0) return;
|
||||
++m_completedPackets[slot];
|
||||
const uint8_t boundary = static_cast<uint8_t>((header >> 12) & 0x3);
|
||||
Reassembly& r = m_reassembly[slot];
|
||||
const uint8_t* data = packet + 4;
|
||||
if (boundary == 0x1) {
|
||||
if (r.expected == 0) return;
|
||||
r.frame.insert(r.frame.end(), data, data + length);
|
||||
} else {
|
||||
r.frame.assign(data, data + length);
|
||||
r.expected = length >= 2 ? static_cast<size_t>(Le16(data)) + 4 : 0;
|
||||
}
|
||||
if (r.expected != 0 && r.frame.size() >= r.expected) {
|
||||
const std::vector<uint8_t> frame(r.frame.begin(), r.frame.begin() + static_cast<std::ptrdiff_t>(r.expected));
|
||||
r = {};
|
||||
m_links[slot]->ReceiveL2cap(frame.data(), frame.size());
|
||||
}
|
||||
}
|
||||
|
||||
void Controller::Tick(uint64_t nowUs) {
|
||||
if (!m_open) return;
|
||||
for (size_t i = 0; i < m_pending.size();) {
|
||||
if (nowUs >= m_pending[i].dueUs) {
|
||||
QueueEvent(std::move(m_pending[i].event));
|
||||
m_pending.erase(m_pending.begin() + static_cast<std::ptrdiff_t>(i));
|
||||
} else {
|
||||
++i;
|
||||
}
|
||||
}
|
||||
TickInquiry(nowUs);
|
||||
for (auto& link : m_links) {
|
||||
link->Tick(nowUs);
|
||||
}
|
||||
SendNumberOfCompletedPackets();
|
||||
}
|
||||
|
||||
void Controller::TickInquiry(uint64_t nowUs) {
|
||||
if (!m_inquiry.active) return;
|
||||
if (nowUs >= m_inquiry.endUs ||
|
||||
(m_inquiry.maxResponses != 0 && m_inquiry.reported >= m_inquiry.maxResponses)) {
|
||||
FinishInquiry();
|
||||
return;
|
||||
}
|
||||
// One result per tick, lowest slot first, so remotes keep their order.
|
||||
for (size_t slot = 0; slot < kRemoteSlots; ++slot) {
|
||||
if (m_inquiry.slotReported[slot] || !m_links[slot]->Discoverable()) continue;
|
||||
m_inquiry.slotReported[slot] = true;
|
||||
++m_inquiry.reported;
|
||||
const BdAddr addr = RemoteAddress(slot);
|
||||
std::vector<uint8_t> body = {0x01};
|
||||
PutAddr(body, addr);
|
||||
body.push_back(0x01); // page scan repetition mode R1
|
||||
if (m_inquiryMode == 0) {
|
||||
body.insert(body.end(), {0x00, 0x00, 0x04, 0x25, 0x00});
|
||||
PutLe16(body, kRemoteClockOffset);
|
||||
QueueEvent(Event(kEventInquiryResult, body));
|
||||
} else {
|
||||
body.insert(body.end(), {0x00, 0x04, 0x25, 0x00});
|
||||
PutLe16(body, kRemoteClockOffset);
|
||||
body.push_back(kInquiryRssi);
|
||||
if (m_inquiryMode == 2) {
|
||||
std::array<uint8_t, 240> eir{};
|
||||
const char* name = m_host.DeviceName(slot);
|
||||
const size_t nameLength = std::min<size_t>(std::strlen(name), eir.size() - 2);
|
||||
eir[0] = static_cast<uint8_t>(nameLength + 1);
|
||||
eir[1] = 0x09; // complete local name
|
||||
std::memcpy(&eir[2], name, nameLength);
|
||||
body.insert(body.end(), eir.begin(), eir.end());
|
||||
QueueEvent(Event(kEventExtendedInquiryResult, body));
|
||||
} else {
|
||||
QueueEvent(Event(kEventInquiryResultWithRssi, body));
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Controller::FinishInquiry() {
|
||||
m_inquiry = {};
|
||||
QueueEvent(Event(kEventInquiryComplete, {kStatusSuccess}));
|
||||
if (m_hostInquiryNotified) {
|
||||
m_host.OnGuestInquiry(false);
|
||||
m_hostInquiryNotified = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Controller::SendNumberOfCompletedPackets() {
|
||||
std::vector<uint8_t> body = {0};
|
||||
for (size_t slot = 0; slot < kRemoteSlots; ++slot) {
|
||||
if (m_completedPackets[slot] == 0) continue;
|
||||
PutLe16(body, ConnectionHandle(slot));
|
||||
PutLe16(body, m_completedPackets[slot]);
|
||||
m_completedPackets[slot] = 0;
|
||||
++body[0];
|
||||
}
|
||||
if (body[0] != 0) {
|
||||
QueueEvent(Event(kEventNumberOfCompletedPackets, body));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace Bluetooth
|
||||
Reference in New Issue
Block a user