mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-15 17:12:59 -04:00
1456 lines
58 KiB
C++
1456 lines
58 KiB
C++
// Wii Balance Board native bridge.
|
|
//
|
|
// Purpose:
|
|
// Pair one original Nintendo RVL-WBC-01 through modern BlueZ, then connect to
|
|
// its Bluetooth HID channels directly with wiiuse and expose calibrated sensor
|
|
// readings as newline-delimited JSON for the Node server.
|
|
//
|
|
// Why this process exists:
|
|
// BlueZ removed its Wii PIN helper in 2025. A Wii device expects six raw PIN
|
|
// bytes equal to the host Bluetooth adapter address in wire order. D-Bus represents PINs
|
|
// as UTF-8 strings and cannot safely carry arbitrary bytes, so this bridge races
|
|
// BlueZ's agent response with the correct raw MGMT_OP_PIN_CODE_REPLY. Only the
|
|
// board currently being commissioned is eligible for that reply.
|
|
//
|
|
// After commissioning, this worker owns both directions of the HID transport.
|
|
// Red Sync uses wiiuse's normal outbound connection; the front power button is
|
|
// accepted through always-open control and interrupt listeners. This is
|
|
// important rather than stylistic: BlueZ's
|
|
// input profile applies medium security to bonded HID devices, and an original
|
|
// Balance Board rejects that request with EACCES. Direct low-security sockets
|
|
// match the board and avoid the failing profile entirely.
|
|
//
|
|
// Security boundary:
|
|
// The installed binary receives CAP_NET_ADMIN solely for the Bluetooth
|
|
// management socket and CAP_NET_BIND_SERVICE solely for the reserved HID PSMs.
|
|
// The much larger Node server remains unprivileged. Normal sensor access uses
|
|
// ordinary Bluetooth L2CAP sockets through wiiuse.
|
|
|
|
#include <wiiuse.h>
|
|
#include <bluetooth/l2cap.h>
|
|
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <atomic>
|
|
#include <cerrno>
|
|
#include <chrono>
|
|
#include <csignal>
|
|
#include <cstdint>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <cstring>
|
|
#include <cmath>
|
|
#include <fcntl.h>
|
|
#include <iomanip>
|
|
#include <iostream>
|
|
#include <mutex>
|
|
#include <optional>
|
|
#include <poll.h>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <sys/socket.h>
|
|
#include <sys/types.h>
|
|
#include <sys/wait.h>
|
|
#include <thread>
|
|
#include <unistd.h>
|
|
#include <vector>
|
|
|
|
// Wiiuse exports these two handshake functions from its shared library but
|
|
// keeps them out of the public header because ordinary callers receive sockets
|
|
// from wiiuse_connect(). The Balance Board's front button reverses the normal
|
|
// connection direction for both HID channels, so this bridge must accept those
|
|
// sockets and then start the exact same upstream handshake explicitly.
|
|
extern "C" void wiiuse_handshake(struct wiimote_t* board, byte* data, uint16_t length);
|
|
extern "C" int wiiuse_set_report_type(struct wiimote_t* board);
|
|
|
|
namespace {
|
|
|
|
constexpr const char* kBoardBluetoothName = "Nintendo RVL-WBC-01";
|
|
constexpr int kBluetoothProtocolHci = 1;
|
|
constexpr uint16_t kHciChannelControl = 3;
|
|
constexpr uint16_t kHciDeviceNone = 0xffff;
|
|
constexpr uint16_t kMgmtCommandCompleteEvent = 0x0001;
|
|
constexpr uint16_t kMgmtCommandStatusEvent = 0x0002;
|
|
constexpr uint16_t kMgmtNewSettingsEvent = 0x0006;
|
|
constexpr uint16_t kMgmtPinCodeRequestEvent = 0x000e;
|
|
constexpr uint16_t kMgmtPinCodeReplyCommand = 0x0016;
|
|
constexpr uint16_t kMgmtSetConnectableCommand = 0x0007;
|
|
constexpr uint16_t kMgmtSetFastConnectableCommand = 0x0008;
|
|
constexpr uint16_t kPrimaryControllerIndex = 0;
|
|
constexpr uint8_t kBluetoothClassicAddressType = 0;
|
|
constexpr uint32_t kControllerConnectableSetting = 1U << 1;
|
|
constexpr uint32_t kControllerFastConnectableSetting = 1U << 2;
|
|
constexpr int kManagementCommandTimeoutMs = 2000;
|
|
constexpr int kFrameIntervalMs = 50;
|
|
constexpr int kDiscoveryRestartDelayMs = 1000;
|
|
// A live bluetoothctl PID is not proof that BlueZ actually began inquiry. Some
|
|
// adapters can leave the command parked after accepting its discovery filter,
|
|
// which previously made commissioning look active for the command's full
|
|
// one-day lifetime while the controller remained `Discovering: no`. Bound that
|
|
// unconfirmed startup state so a transient adapter or BlueZ failure is repaired
|
|
// automatically instead of requiring a manual foreground scan.
|
|
constexpr int kDiscoveryStartDeadlineMs = 5000;
|
|
constexpr int kCandidateIdentityRetryMs = 500;
|
|
constexpr int kCandidateIdleRetryMs = 5000;
|
|
constexpr const char* kDiscoveryTimeoutSeconds = "86400";
|
|
constexpr uint16_t kHidControlPsm = 0x0011;
|
|
constexpr uint16_t kHidInterruptPsm = 0x0013;
|
|
constexpr int kCommissioningConnectWindowMs = 15000;
|
|
constexpr int kIncomingChannelPairTimeoutMs = 5000;
|
|
constexpr int kHandshakeWarningMs = 10000;
|
|
constexpr int kMovementThresholdCentiKg = 50;
|
|
constexpr int kStillSleepDelayMs = 2 * 60 * 1000;
|
|
|
|
std::atomic<bool> running{true};
|
|
std::mutex output_mutex;
|
|
|
|
struct BluetoothAddress {
|
|
std::string display;
|
|
// The kernel Bluetooth management API carries addresses least-significant
|
|
// byte first. These exact six bytes are also the Wii pairing PIN.
|
|
std::array<uint8_t, 6> wire{};
|
|
};
|
|
|
|
struct PairingSharedState {
|
|
std::mutex mutex;
|
|
std::optional<BluetoothAddress> active_target;
|
|
std::optional<BluetoothAddress> active_pin;
|
|
std::optional<std::string> commissioned_address;
|
|
bool commissioning = false;
|
|
bool outbound_connection_requested = false;
|
|
};
|
|
|
|
struct BoardReadings {
|
|
int top_right = 0;
|
|
int bottom_right = 0;
|
|
int top_left = 0;
|
|
int bottom_left = 0;
|
|
};
|
|
|
|
struct CommandResult {
|
|
int exit_code = -1;
|
|
std::string output;
|
|
};
|
|
|
|
struct RunningCommand {
|
|
pid_t pid = -1;
|
|
int output_fd = -1;
|
|
std::string pending_output;
|
|
std::string transcript;
|
|
};
|
|
|
|
struct DiscoveryCandidate {
|
|
BluetoothAddress address;
|
|
uint64_t last_seen_at = 0;
|
|
uint64_t next_identity_check_at = 0;
|
|
};
|
|
|
|
struct ManagementRuntimeState {
|
|
// Runtime reassertions are asynchronous so a temporary controller setting
|
|
// change cannot block PIN or HID handling. Track each outstanding opcode to
|
|
// avoid submitting the same command repeatedly while BlueZ is acknowledging
|
|
// the first request.
|
|
bool connectable_pending = false;
|
|
bool fast_connectable_pending = false;
|
|
};
|
|
|
|
uint64_t monotonic_ms() {
|
|
using namespace std::chrono;
|
|
return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
|
}
|
|
|
|
std::string json_escape(const std::string& value) {
|
|
std::ostringstream out;
|
|
for (unsigned char ch : value) {
|
|
switch (ch) {
|
|
case '\\': out << "\\\\"; break;
|
|
case '"': out << "\\\""; break;
|
|
case '\n': out << "\\n"; break;
|
|
case '\r': out << "\\r"; break;
|
|
case '\t': out << "\\t"; break;
|
|
default:
|
|
if (ch < 0x20) {
|
|
out << "\\u" << std::hex << std::setw(4) << std::setfill('0')
|
|
<< static_cast<int>(ch) << std::dec;
|
|
} else {
|
|
out << static_cast<char>(ch);
|
|
}
|
|
}
|
|
}
|
|
return out.str();
|
|
}
|
|
|
|
void emit_json(const std::string& fields) {
|
|
// Pairing and input monitoring run on separate threads. Serialize complete
|
|
// lines so two status changes can never interleave and corrupt Node's parser.
|
|
std::lock_guard<std::mutex> lock(output_mutex);
|
|
std::cout << "{" << fields << "}\n";
|
|
std::cout.flush();
|
|
}
|
|
|
|
void emit_status(const std::string& state, const std::string& address = "",
|
|
const std::string& error = "") {
|
|
std::ostringstream fields;
|
|
fields << "\"type\":\"status\",\"state\":\"" << json_escape(state) << "\"";
|
|
if (!address.empty()) fields << ",\"address\":\"" << json_escape(address) << "\"";
|
|
if (!error.empty()) fields << ",\"error\":\"" << json_escape(error) << "\"";
|
|
emit_json(fields.str());
|
|
}
|
|
|
|
void emit_frame(const BoardReadings& readings, std::optional<int> battery_percent = std::nullopt) {
|
|
std::ostringstream fields;
|
|
fields << "\"type\":\"frame\",\"corners\":{"
|
|
<< "\"topRight\":" << readings.top_right << ","
|
|
<< "\"bottomRight\":" << readings.bottom_right << ","
|
|
<< "\"topLeft\":" << readings.top_left << ","
|
|
<< "\"bottomLeft\":" << readings.bottom_left << "}";
|
|
if (battery_percent.has_value()) fields << ",\"batteryPercent\":" << *battery_percent;
|
|
emit_json(fields.str());
|
|
}
|
|
|
|
void signal_handler(int) {
|
|
running.store(false);
|
|
}
|
|
|
|
std::optional<BluetoothAddress> parse_address(const std::string& raw) {
|
|
std::array<unsigned int, 6> bytes{};
|
|
if (std::sscanf(raw.c_str(), "%2x:%2x:%2x:%2x:%2x:%2x",
|
|
&bytes[0], &bytes[1], &bytes[2], &bytes[3], &bytes[4], &bytes[5]) != 6) {
|
|
return std::nullopt;
|
|
}
|
|
|
|
BluetoothAddress address;
|
|
char normalized[18]{};
|
|
std::snprintf(normalized, sizeof(normalized), "%02X:%02X:%02X:%02X:%02X:%02X",
|
|
bytes[0], bytes[1], bytes[2], bytes[3], bytes[4], bytes[5]);
|
|
address.display = normalized;
|
|
for (std::size_t i = 0; i < address.wire.size(); ++i) {
|
|
address.wire[i] = static_cast<uint8_t>(bytes[address.wire.size() - 1 - i]);
|
|
}
|
|
return address;
|
|
}
|
|
|
|
CommandResult run_command(const std::vector<std::string>& args) {
|
|
CommandResult result;
|
|
if (args.empty()) return result;
|
|
|
|
int pipe_fds[2]{};
|
|
if (pipe(pipe_fds) != 0) {
|
|
result.output = std::strerror(errno);
|
|
return result;
|
|
}
|
|
|
|
const pid_t pid = fork();
|
|
if (pid == 0) {
|
|
dup2(pipe_fds[1], STDOUT_FILENO);
|
|
dup2(pipe_fds[1], STDERR_FILENO);
|
|
close(pipe_fds[0]);
|
|
close(pipe_fds[1]);
|
|
|
|
std::vector<char*> argv;
|
|
argv.reserve(args.size() + 1);
|
|
for (const auto& arg : args) argv.push_back(const_cast<char*>(arg.c_str()));
|
|
argv.push_back(nullptr);
|
|
execvp(argv[0], argv.data());
|
|
_exit(127);
|
|
}
|
|
|
|
close(pipe_fds[1]);
|
|
if (pid < 0) {
|
|
close(pipe_fds[0]);
|
|
result.output = std::strerror(errno);
|
|
return result;
|
|
}
|
|
|
|
std::array<char, 1024> buffer{};
|
|
ssize_t count = 0;
|
|
while ((count = read(pipe_fds[0], buffer.data(), buffer.size())) > 0) {
|
|
result.output.append(buffer.data(), static_cast<std::size_t>(count));
|
|
}
|
|
close(pipe_fds[0]);
|
|
|
|
int status = 0;
|
|
while (waitpid(pid, &status, 0) < 0 && errno == EINTR) {}
|
|
if (WIFEXITED(status)) result.exit_code = WEXITSTATUS(status);
|
|
return result;
|
|
}
|
|
|
|
RunningCommand start_command(const std::vector<std::string>& args) {
|
|
RunningCommand command;
|
|
if (args.empty()) return command;
|
|
|
|
int pipe_fds[2]{};
|
|
if (pipe(pipe_fds) != 0) {
|
|
command.transcript = std::strerror(errno);
|
|
return command;
|
|
}
|
|
|
|
const pid_t pid = fork();
|
|
if (pid == 0) {
|
|
dup2(pipe_fds[1], STDOUT_FILENO);
|
|
dup2(pipe_fds[1], STDERR_FILENO);
|
|
close(pipe_fds[0]);
|
|
close(pipe_fds[1]);
|
|
|
|
std::vector<char*> argv;
|
|
argv.reserve(args.size() + 1);
|
|
for (const auto& arg : args) argv.push_back(const_cast<char*>(arg.c_str()));
|
|
argv.push_back(nullptr);
|
|
execvp(argv[0], argv.data());
|
|
_exit(127);
|
|
}
|
|
|
|
close(pipe_fds[1]);
|
|
if (pid < 0) {
|
|
command.transcript = std::strerror(errno);
|
|
close(pipe_fds[0]);
|
|
return command;
|
|
}
|
|
|
|
// Discovery has no predetermined completion time: it must remain active until
|
|
// the user wakes the board. A nonblocking pipe lets the commissioning thread
|
|
// consume BlueZ events while still honoring server shutdown and maintenance
|
|
// commands promptly.
|
|
const int current_flags = fcntl(pipe_fds[0], F_GETFL, 0);
|
|
if (current_flags >= 0) fcntl(pipe_fds[0], F_SETFL, current_flags | O_NONBLOCK);
|
|
command.pid = pid;
|
|
command.output_fd = pipe_fds[0];
|
|
return command;
|
|
}
|
|
|
|
bool collect_command_output(RunningCommand* command) {
|
|
if (!command || command->pid < 0) return false;
|
|
|
|
std::array<char, 1024> buffer{};
|
|
ssize_t count = 0;
|
|
while ((count = read(command->output_fd, buffer.data(), buffer.size())) > 0) {
|
|
const std::string chunk(buffer.data(), static_cast<std::size_t>(count));
|
|
command->pending_output += chunk;
|
|
command->transcript += chunk;
|
|
// A busy Bluetooth environment can produce an unbounded stream of RSSI
|
|
// updates. Retain only the most recent output instead of allowing a
|
|
// commissioning session left open for days to grow the worker indefinitely.
|
|
constexpr std::size_t max_transcript_size = 8192;
|
|
if (command->transcript.size() > max_transcript_size) {
|
|
command->transcript.erase(0, command->transcript.size() - max_transcript_size);
|
|
}
|
|
}
|
|
|
|
int status = 0;
|
|
const pid_t waited = waitpid(command->pid, &status, WNOHANG);
|
|
if (waited == 0) return true;
|
|
if (waited == command->pid) {
|
|
command->pid = -1;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void stop_command(RunningCommand* command) {
|
|
if (!command) return;
|
|
if (command->pid > 0) {
|
|
// bluetoothctl normally exits immediately on SIGTERM. Bound that grace
|
|
// period so a wedged D-Bus client cannot prevent the server from stopping.
|
|
kill(command->pid, SIGTERM);
|
|
for (int attempt = 0; attempt < 50 && command->pid > 0; ++attempt) {
|
|
collect_command_output(command);
|
|
if (command->pid > 0) usleep(10000);
|
|
}
|
|
if (command->pid > 0) {
|
|
kill(command->pid, SIGKILL);
|
|
int status = 0;
|
|
while (waitpid(command->pid, &status, 0) < 0 && errno == EINTR) {}
|
|
command->pid = -1;
|
|
}
|
|
}
|
|
if (command->output_fd >= 0) {
|
|
close(command->output_fd);
|
|
command->output_fd = -1;
|
|
}
|
|
}
|
|
|
|
std::optional<BluetoothAddress> take_discovered_board(
|
|
RunningCommand* discovery, bool* discovery_started,
|
|
std::vector<DiscoveryCandidate>* candidates) {
|
|
if (!discovery) return std::nullopt;
|
|
std::size_t newline = discovery->pending_output.find('\n');
|
|
while (newline != std::string::npos) {
|
|
const std::string line = discovery->pending_output.substr(0, newline);
|
|
discovery->pending_output.erase(0, newline + 1);
|
|
|
|
// bluetoothctl reports filter setup before StartDiscovery completes. Treat
|
|
// only this explicit event as proof that button presses can now be seen;
|
|
// `SetDiscoveryFilter success` alone is not an active Bluetooth scan.
|
|
if (discovery_started && line.find("Discovery started") != std::string::npos) {
|
|
*discovery_started = true;
|
|
}
|
|
|
|
const std::size_t device_prefix = line.find("Device ");
|
|
if (device_prefix != std::string::npos && line.size() >= device_prefix + 24) {
|
|
const auto address = parse_address(line.substr(device_prefix + 7, 17));
|
|
if (!address.has_value()) {
|
|
newline = discovery->pending_output.find('\n');
|
|
continue;
|
|
}
|
|
|
|
// The exact remote name remains definitive whenever the adapter resolves
|
|
// it during the board's short red-Sync window.
|
|
if (line.find(kBoardBluetoothName) != std::string::npos) return address;
|
|
|
|
// Some adapters initially report only "Device <address> <address>" and
|
|
// never finish remote-name resolution before the board powers down. Keep
|
|
// those unresolved devices as candidates so their BlueZ properties can be
|
|
// inspected without stopping the discovery session. Named ambient devices
|
|
// such as TVs are not candidates, which prevents normal room traffic from
|
|
// replacing the panel's useful waiting-for-Sync message.
|
|
const std::string trailing = line.substr(device_prefix + 24);
|
|
const bool address_only = trailing.empty() ||
|
|
trailing.find(address->display) != std::string::npos;
|
|
if (address_only && candidates) {
|
|
const uint64_t now = monotonic_ms();
|
|
const auto existing = std::find_if(
|
|
candidates->begin(), candidates->end(),
|
|
[&](const DiscoveryCandidate& candidate) {
|
|
return candidate.address.display == address->display;
|
|
});
|
|
if (existing == candidates->end()) {
|
|
candidates->push_back({*address, now, now});
|
|
emit_status("device-detected", address->display,
|
|
"Bluetooth device detected; checking whether it is the Balance Board.");
|
|
} else {
|
|
// A fresh scan event generally means the physical button was pressed
|
|
// again. Recheck immediately even if this candidate had previously
|
|
// fallen back to the slower idle retry interval.
|
|
existing->last_seen_at = now;
|
|
existing->next_identity_check_at = now;
|
|
}
|
|
}
|
|
}
|
|
newline = discovery->pending_output.find('\n');
|
|
}
|
|
return std::nullopt;
|
|
}
|
|
|
|
bool candidate_is_balance_board(const BluetoothAddress& address) {
|
|
const CommandResult info = run_command({
|
|
"bluetoothctl", "--timeout", "2", "info", address.display});
|
|
if (info.output.find(kBoardBluetoothName) != std::string::npos) return true;
|
|
|
|
// Original Wii input devices identify as legacy-pairing gaming peripherals.
|
|
// This fallback is deliberately applied only to an address-only device seen
|
|
// during active commissioning. That physical red-Sync action is the selection
|
|
// boundary when an adapter cannot resolve Nintendo's remote name in time.
|
|
const bool gaming_peripheral =
|
|
info.output.find("Class: 0x00002504") != std::string::npos &&
|
|
info.output.find("Icon: input-gaming") != std::string::npos;
|
|
const bool legacy_pairing =
|
|
info.output.find("LegacyPairing: yes") != std::string::npos;
|
|
return gaming_peripheral && legacy_pairing;
|
|
}
|
|
|
|
std::string command_error_summary(const std::string& raw, const std::string& fallback) {
|
|
std::string summary;
|
|
summary.reserve(std::min<std::size_t>(raw.size(), 400));
|
|
bool previous_was_space = false;
|
|
int ansi_state = 0;
|
|
for (unsigned char ch : raw) {
|
|
// bluetoothctl emits terminal color CSI sequences even when its output is
|
|
// captured by a pipe. Drop the entire ESC ... final-byte sequence so the UI
|
|
// never exposes fragments such as `[[0;93mCHG[0m]` as a pairing error.
|
|
if (ch == 0x1b) {
|
|
ansi_state = 1;
|
|
continue;
|
|
}
|
|
if (ansi_state == 1) {
|
|
ansi_state = ch == '[' ? 2 : 0;
|
|
continue;
|
|
}
|
|
if (ansi_state == 2) {
|
|
if (ch >= 0x40 && ch <= 0x7e) ansi_state = 0;
|
|
continue;
|
|
}
|
|
const bool is_space = ch == ' ' || ch == '\t' || ch == '\n' || ch == '\r';
|
|
if (is_space) {
|
|
if (!summary.empty() && !previous_was_space) summary.push_back(' ');
|
|
} else if (ch >= 0x20) {
|
|
summary.push_back(static_cast<char>(ch));
|
|
}
|
|
previous_was_space = is_space;
|
|
if (summary.size() >= 400) break;
|
|
}
|
|
while (!summary.empty() && summary.back() == ' ') summary.pop_back();
|
|
return summary.empty() ? fallback : summary;
|
|
}
|
|
|
|
bool command_succeeded(const CommandResult& result) {
|
|
// bluetoothctl has returned exit code zero for some D-Bus failures across
|
|
// releases. Check its stable failure text as well so commissioning never
|
|
// stores an address when BlueZ did not actually finish the bond.
|
|
return result.exit_code == 0 &&
|
|
result.output.find("Failed") == std::string::npos &&
|
|
result.output.find("not available") == std::string::npos;
|
|
}
|
|
|
|
std::optional<BluetoothAddress> find_default_controller() {
|
|
const CommandResult controller = run_command({"bluetoothctl", "show"});
|
|
std::istringstream lines(controller.output);
|
|
std::string line;
|
|
while (std::getline(lines, line)) {
|
|
const std::size_t controller_prefix = line.find("Controller ");
|
|
if (controller_prefix == std::string::npos || line.size() < controller_prefix + 28) continue;
|
|
if (auto address = parse_address(line.substr(controller_prefix + 11, 17))) return address;
|
|
}
|
|
return std::nullopt;
|
|
}
|
|
|
|
void commissioning_loop(PairingSharedState* shared) {
|
|
while (running.load()) {
|
|
bool should_commission = false;
|
|
{
|
|
std::lock_guard<std::mutex> lock(shared->mutex);
|
|
should_commission = shared->commissioning && !shared->commissioned_address.has_value();
|
|
}
|
|
|
|
if (!should_commission) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(250));
|
|
continue;
|
|
}
|
|
|
|
emit_status("commissioning");
|
|
// Commissioning must be listening before the board's short red-Sync window
|
|
// begins. Keep one BlueZ discovery client alive continuously and consume its
|
|
// own event stream. The previous bounded scan exited for twelve seconds at a
|
|
// time and then queried a second client, making successful discovery depend
|
|
// on when the physical button happened to be pressed.
|
|
// BlueZ's command-line client exits after the SetDiscoveryFilter callback
|
|
// unless non-interactive mode has a timeout. Its explicit monitor mode is
|
|
// equally important here: stdout is a worker-owned pipe rather than a
|
|
// terminal, and current bluetoothctl versions otherwise report command
|
|
// acknowledgements such as `Discovery started` without forwarding the
|
|
// asynchronous `[NEW]` and `[CHG]` device lines parsed below. A one-day
|
|
// timeout keeps that monitored client alive for unattended commissioning;
|
|
// the worker normally stops it itself as soon as the board appears and
|
|
// restarts it if the day expires.
|
|
RunningCommand discovery = start_command({
|
|
"bluetoothctl", "--monitor", "--timeout", kDiscoveryTimeoutSeconds,
|
|
"scan", "bredr"});
|
|
if (discovery.pid < 0) {
|
|
emit_status("error", "", "could not start Bluetooth discovery: " +
|
|
command_error_summary(discovery.transcript, "unknown process error"));
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(kDiscoveryRestartDelayMs));
|
|
continue;
|
|
}
|
|
|
|
std::optional<BluetoothAddress> address;
|
|
bool discovery_started = false;
|
|
// Measure from the successful fork, not from the first output line. A
|
|
// wedged bluetoothctl may emit `SetDiscoveryFilter success` and then stay
|
|
// alive forever without producing the only confirmation that matters:
|
|
// `Discovery started`.
|
|
const uint64_t discovery_launched_at = monotonic_ms();
|
|
std::vector<DiscoveryCandidate> candidates;
|
|
while (running.load() && !address.has_value()) {
|
|
const bool discovery_running = collect_command_output(&discovery);
|
|
const bool was_started = discovery_started;
|
|
address = take_discovered_board(
|
|
&discovery, &discovery_started, &candidates);
|
|
if (!was_started && discovery_started) {
|
|
// This status clears any prior scanner error and tells the browser that
|
|
// the server is genuinely listening for the board's red Sync button.
|
|
emit_status("discovering");
|
|
}
|
|
if (address.has_value()) break;
|
|
|
|
const uint64_t now = monotonic_ms();
|
|
for (auto& candidate : candidates) {
|
|
if (now < candidate.next_identity_check_at) continue;
|
|
if (candidate_is_balance_board(candidate.address)) {
|
|
address = candidate.address;
|
|
break;
|
|
}
|
|
|
|
// Query quickly while a newly pressed board is still awake, then back
|
|
// off once it has been absent for several seconds. A later scan event
|
|
// resets this deadline immediately, so another button press never waits
|
|
// for the idle interval and an unidentified device cannot cause a
|
|
// permanent stream of bluetoothctl processes.
|
|
const bool recently_seen = now - candidate.last_seen_at < 10000;
|
|
candidate.next_identity_check_at = now +
|
|
(recently_seen ? kCandidateIdentityRetryMs : kCandidateIdleRetryMs);
|
|
}
|
|
if (address.has_value()) break;
|
|
if (!discovery_running) {
|
|
const std::string detail = command_error_summary(
|
|
discovery.transcript, "bluetoothctl exited unexpectedly");
|
|
emit_status("error", "", discovery_started
|
|
? "Bluetooth scanner stopped unexpectedly; retrying automatically: " + detail
|
|
: "Bluetooth scanner exited before discovery started; retrying automatically: " + detail);
|
|
break;
|
|
}
|
|
|
|
if (!discovery_started &&
|
|
now - discovery_launched_at >= kDiscoveryStartDeadlineMs) {
|
|
// Terminating this specific client is safe: stop_command() only owns
|
|
// the child created above, and the outer commissioning loop immediately
|
|
// starts a clean replacement. Preserve bluetoothctl's actual output in
|
|
// the status so the panel explains whether BlueZ stalled after filter
|
|
// setup or failed in some other adapter-specific way.
|
|
const std::string detail = command_error_summary(
|
|
discovery.transcript, "bluetoothctl produced no startup response");
|
|
emit_status(
|
|
"error", "",
|
|
"Bluetooth scanner did not start within 5 seconds; retrying automatically: " +
|
|
detail);
|
|
break;
|
|
}
|
|
|
|
bool still_commissioning = false;
|
|
{
|
|
std::lock_guard<std::mutex> lock(shared->mutex);
|
|
still_commissioning = shared->commissioning &&
|
|
!shared->commissioned_address.has_value();
|
|
}
|
|
if (!still_commissioning) break;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
}
|
|
|
|
if (!address.has_value()) {
|
|
stop_command(&discovery);
|
|
if (running.load()) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(kDiscoveryRestartDelayMs));
|
|
}
|
|
continue;
|
|
}
|
|
|
|
const auto controller = find_default_controller();
|
|
if (!controller.has_value()) {
|
|
stop_command(&discovery);
|
|
emit_status("commissioning", address->display,
|
|
"no powered Bluetooth controller is available for pairing");
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(kDiscoveryRestartDelayMs));
|
|
continue;
|
|
}
|
|
|
|
{
|
|
std::lock_guard<std::mutex> lock(shared->mutex);
|
|
shared->active_target = address;
|
|
// Red-Sync commissioning stores the host as the board's future reconnect
|
|
// target. BlueZ's retired wiimote plugin therefore used the local adapter
|
|
// address—not the board address—as the six raw PIN bytes.
|
|
shared->active_pin = controller;
|
|
}
|
|
emit_status("pairing", address->display);
|
|
|
|
// The management-socket listener answers the PIN request while this command
|
|
// keeps BlueZ's normal device, SDP, bonding, and input-profile machinery in
|
|
// charge of everything else.
|
|
const CommandResult pair_result = run_command({
|
|
"bluetoothctl", "--timeout", "12", "--agent", "NoInputNoOutput", "pair", address->display});
|
|
// Keep the discovery owner alive through Pair(). BlueZ documents pairing by
|
|
// address as requiring an active scan report, and the board may stop its
|
|
// Sync window before a new discovery client could be established.
|
|
stop_command(&discovery);
|
|
|
|
{
|
|
std::lock_guard<std::mutex> lock(shared->mutex);
|
|
shared->active_target.reset();
|
|
shared->active_pin.reset();
|
|
}
|
|
|
|
if (!command_succeeded(pair_result)) {
|
|
emit_status("commissioning", address->display,
|
|
"pairing failed: " + command_error_summary(
|
|
pair_result.output, "BlueZ returned an unknown pairing error"));
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(kDiscoveryRestartDelayMs));
|
|
continue;
|
|
}
|
|
|
|
{
|
|
std::lock_guard<std::mutex> lock(shared->mutex);
|
|
shared->commissioned_address = address->display;
|
|
shared->commissioning = false;
|
|
// Red Sync makes the board discoverable rather than initiating its normal
|
|
// host reconnect. Give wiiuse one bounded outbound window immediately
|
|
// after commissioning; every later front-button wake arrives through the
|
|
// two HID listeners instead.
|
|
shared->outbound_connection_requested = true;
|
|
}
|
|
emit_json("\"type\":\"paired\",\"address\":\"" + json_escape(address->display) + "\"");
|
|
// The direct connection loop notices this address immediately. Pairing and
|
|
// sensor transport stay separate so the red Sync button is needed only for
|
|
// commissioning; later front-button wakes are caught automatically.
|
|
emit_status("waiting", address->display);
|
|
}
|
|
}
|
|
|
|
#pragma pack(push, 1)
|
|
struct SockaddrHci {
|
|
uint16_t family;
|
|
uint16_t device;
|
|
uint16_t channel;
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
int open_management_socket() {
|
|
const int fd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
|
|
kBluetoothProtocolHci);
|
|
if (fd < 0) return -1;
|
|
|
|
const SockaddrHci address{
|
|
static_cast<uint16_t>(AF_BLUETOOTH), kHciDeviceNone, kHciChannelControl};
|
|
if (bind(fd, reinterpret_cast<const sockaddr*>(&address), sizeof(address)) != 0) {
|
|
close(fd);
|
|
return -1;
|
|
}
|
|
return fd;
|
|
}
|
|
|
|
void write_u16_le(uint8_t* output, uint16_t value) {
|
|
output[0] = static_cast<uint8_t>(value & 0xff);
|
|
output[1] = static_cast<uint8_t>((value >> 8) & 0xff);
|
|
}
|
|
|
|
uint16_t read_u16_le(const uint8_t* input) {
|
|
return static_cast<uint16_t>(input[0] | (input[1] << 8));
|
|
}
|
|
|
|
uint32_t read_u32_le(const uint8_t* input) {
|
|
return static_cast<uint32_t>(input[0]) |
|
|
(static_cast<uint32_t>(input[1]) << 8) |
|
|
(static_cast<uint32_t>(input[2]) << 16) |
|
|
(static_cast<uint32_t>(input[3]) << 24);
|
|
}
|
|
|
|
std::string management_status_description(uint8_t status) {
|
|
// These are the management statuses that setting controller modes can
|
|
// realistically return. Retain the numeric value as well because it remains
|
|
// actionable if a newer kernel introduces a status this worker does not yet
|
|
// name.
|
|
const char* description = "unknown management status";
|
|
switch (status) {
|
|
case 0x00: description = "success"; break;
|
|
case 0x03: description = "failed"; break;
|
|
case 0x0a: description = "busy"; break;
|
|
case 0x0c: description = "not supported"; break;
|
|
case 0x0d: description = "invalid parameters"; break;
|
|
case 0x0f: description = "controller not powered"; break;
|
|
case 0x11: description = "invalid controller index"; break;
|
|
case 0x14: description = "permission denied"; break;
|
|
}
|
|
std::ostringstream result;
|
|
result << description << " (0x" << std::hex << std::setw(2)
|
|
<< std::setfill('0') << static_cast<int>(status) << ")";
|
|
return result.str();
|
|
}
|
|
|
|
bool write_management_boolean_command(int fd, uint16_t opcode, bool enabled) {
|
|
if (fd < 0) return false;
|
|
constexpr std::size_t header_size = 6;
|
|
std::array<uint8_t, header_size + 1> packet{};
|
|
write_u16_le(packet.data(), opcode);
|
|
write_u16_le(packet.data() + 2, kPrimaryControllerIndex);
|
|
write_u16_le(packet.data() + 4, 1);
|
|
packet[header_size] = enabled ? 1 : 0;
|
|
return write(fd, packet.data(), packet.size()) ==
|
|
static_cast<ssize_t>(packet.size());
|
|
}
|
|
|
|
bool set_management_boolean_and_wait(int fd, uint16_t opcode,
|
|
const std::string& setting_name,
|
|
std::string* error) {
|
|
if (!write_management_boolean_command(fd, opcode, true)) {
|
|
if (error) *error = "could not send the " + setting_name + " command: " +
|
|
std::string(std::strerror(errno));
|
|
return false;
|
|
}
|
|
|
|
// A successful write only queues a request to the kernel. Wait for the
|
|
// matching Command Complete/Status event so the worker never advertises a
|
|
// reliable wake listener when the adapter actually rejected the setting.
|
|
const uint64_t deadline = monotonic_ms() + kManagementCommandTimeoutMs;
|
|
while (running.load()) {
|
|
const uint64_t now = monotonic_ms();
|
|
if (now >= deadline) break;
|
|
const int remaining = static_cast<int>(deadline - now);
|
|
pollfd descriptor{fd, POLLIN, 0};
|
|
const int ready = poll(&descriptor, 1, std::max(1, remaining));
|
|
if (ready < 0) {
|
|
if (errno == EINTR) continue;
|
|
if (error) *error = "could not wait for the " + setting_name +
|
|
" response: " + std::string(std::strerror(errno));
|
|
return false;
|
|
}
|
|
if (ready == 0) break;
|
|
|
|
std::array<uint8_t, 1024> response{};
|
|
const ssize_t count = read(fd, response.data(), response.size());
|
|
if (count < 9) continue;
|
|
const uint16_t event = read_u16_le(response.data());
|
|
const uint16_t response_opcode = read_u16_le(response.data() + 6);
|
|
if ((event != kMgmtCommandCompleteEvent && event != kMgmtCommandStatusEvent) ||
|
|
response_opcode != opcode) {
|
|
// Startup occurs before pairing and connection threads exist, so the only
|
|
// expected extra packet is New Settings generated by the command itself.
|
|
// The matching completion is still queued immediately after it.
|
|
continue;
|
|
}
|
|
|
|
const uint8_t status = response[8];
|
|
if (status == 0) return true;
|
|
if (error) *error = setting_name + " was rejected: " +
|
|
management_status_description(status);
|
|
return false;
|
|
}
|
|
|
|
if (error) *error = "timed out waiting for the " + setting_name + " response";
|
|
return false;
|
|
}
|
|
|
|
bool enable_incoming_connections(int fd, std::string* error) {
|
|
if (fd < 0) {
|
|
if (error) *error = "management socket is unavailable";
|
|
return false;
|
|
}
|
|
|
|
// Set Connectable enables the BR/EDR page scan that accepts the board's
|
|
// incoming front-button connection. Fast Connectable increases the page-scan
|
|
// duty cycle so the controller can catch the board during its unusually short
|
|
// one-to-two-second wake attempt. A stationary server can accept the modest
|
|
// adapter power cost in exchange for reliable unattended operation.
|
|
if (!set_management_boolean_and_wait(
|
|
fd, kMgmtSetConnectableCommand, "connectable setting", error)) {
|
|
return false;
|
|
}
|
|
return set_management_boolean_and_wait(
|
|
fd, kMgmtSetFastConnectableCommand, "fast connectable setting", error);
|
|
}
|
|
|
|
void answer_pin_request(int fd, uint16_t adapter_index,
|
|
const BluetoothAddress& target,
|
|
const BluetoothAddress& pin) {
|
|
// Packet layout is a six-byte mgmt header followed by mgmt_addr_info,
|
|
// pin_len, and the fixed sixteen-byte PIN buffer. Serializing by hand avoids
|
|
// compiler padding and documents every privileged byte sent to the kernel.
|
|
constexpr std::size_t header_size = 6;
|
|
constexpr std::size_t payload_size = 7 + 1 + 16;
|
|
std::array<uint8_t, header_size + payload_size> packet{};
|
|
write_u16_le(packet.data(), kMgmtPinCodeReplyCommand);
|
|
write_u16_le(packet.data() + 2, adapter_index);
|
|
write_u16_le(packet.data() + 4, payload_size);
|
|
std::copy(target.wire.begin(), target.wire.end(), packet.begin() + header_size);
|
|
packet[header_size + 6] = kBluetoothClassicAddressType;
|
|
packet[header_size + 7] = 6;
|
|
std::copy(pin.wire.begin(), pin.wire.end(), packet.begin() + header_size + 8);
|
|
if (write(fd, packet.data(), packet.size()) != static_cast<ssize_t>(packet.size())) {
|
|
emit_status("error", target.display, "failed to answer the Wii pairing PIN request");
|
|
}
|
|
}
|
|
|
|
void queue_runtime_management_setting(int fd, uint16_t opcode,
|
|
const std::string& setting_name,
|
|
bool* pending) {
|
|
if (!pending || *pending) return;
|
|
if (!write_management_boolean_command(fd, opcode, true)) {
|
|
emit_status("error", "", "Could not restore the Bluetooth " + setting_name +
|
|
": " + std::string(std::strerror(errno)));
|
|
return;
|
|
}
|
|
*pending = true;
|
|
}
|
|
|
|
void process_management_events(int fd, PairingSharedState* shared,
|
|
ManagementRuntimeState* management) {
|
|
if (fd < 0) return;
|
|
std::array<uint8_t, 1024> buffer{};
|
|
ssize_t count = 0;
|
|
while ((count = read(fd, buffer.data(), buffer.size())) > 0) {
|
|
if (count < 6) continue;
|
|
const uint16_t event = read_u16_le(buffer.data());
|
|
const uint16_t adapter_index = read_u16_le(buffer.data() + 2);
|
|
const uint16_t payload_size = read_u16_le(buffer.data() + 4);
|
|
if (count < 6 + payload_size) continue;
|
|
|
|
if ((event == kMgmtCommandCompleteEvent || event == kMgmtCommandStatusEvent) &&
|
|
payload_size >= 3 && management) {
|
|
const uint16_t opcode = read_u16_le(buffer.data() + 6);
|
|
const uint8_t status = buffer[8];
|
|
bool recognized = false;
|
|
std::string setting_name;
|
|
if (opcode == kMgmtSetConnectableCommand) {
|
|
management->connectable_pending = false;
|
|
recognized = true;
|
|
setting_name = "connectable setting";
|
|
} else if (opcode == kMgmtSetFastConnectableCommand) {
|
|
management->fast_connectable_pending = false;
|
|
recognized = true;
|
|
setting_name = "fast connectable setting";
|
|
}
|
|
if (recognized && status != 0) {
|
|
emit_status("error", "", "Bluetooth " + setting_name +
|
|
" reassertion was rejected: " + management_status_description(status));
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (event == kMgmtNewSettingsEvent && payload_size >= 4 &&
|
|
adapter_index == kPrimaryControllerIndex && management) {
|
|
const uint32_t settings = read_u32_le(buffer.data() + 6);
|
|
// BlueZ or another controller operation can replace the page-scan modes
|
|
// after worker startup. Reassert only missing modes and let their normal
|
|
// command responses below report any rejection; pending flags prevent a
|
|
// burst of New Settings events from queuing duplicate commands.
|
|
if ((settings & kControllerConnectableSetting) == 0) {
|
|
queue_runtime_management_setting(
|
|
fd, kMgmtSetConnectableCommand, "connectable setting",
|
|
&management->connectable_pending);
|
|
} else if ((settings & kControllerFastConnectableSetting) == 0) {
|
|
// Fast Connectable is meaningful only after ordinary Connectable has
|
|
// taken effect. Sequencing them avoids a transient Busy/Rejected reply
|
|
// when a controller reset removed both modes at the same time.
|
|
queue_runtime_management_setting(
|
|
fd, kMgmtSetFastConnectableCommand, "fast connectable setting",
|
|
&management->fast_connectable_pending);
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (event == kMgmtPinCodeRequestEvent && payload_size >= 8) {
|
|
std::optional<BluetoothAddress> target;
|
|
std::optional<BluetoothAddress> pin;
|
|
{
|
|
std::lock_guard<std::mutex> lock(shared->mutex);
|
|
target = shared->active_target;
|
|
pin = shared->active_pin;
|
|
}
|
|
if (target.has_value() && pin.has_value() &&
|
|
std::equal(target->wire.begin(), target->wire.end(), buffer.begin() + 6)) {
|
|
answer_pin_request(fd, adapter_index, *target, *pin);
|
|
}
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
|
|
std::optional<std::string> extract_command_value(const std::string& line, const std::string& key) {
|
|
const std::string token = "\"" + key + "\"";
|
|
const std::size_t key_at = line.find(token);
|
|
if (key_at == std::string::npos) return std::nullopt;
|
|
const std::size_t colon = line.find(':', key_at + token.size());
|
|
const std::size_t first_quote = line.find('"', colon + 1);
|
|
const std::size_t second_quote = line.find('"', first_quote + 1);
|
|
if (colon == std::string::npos || first_quote == std::string::npos || second_quote == std::string::npos) {
|
|
return std::nullopt;
|
|
}
|
|
return line.substr(first_quote + 1, second_quote - first_quote - 1);
|
|
}
|
|
|
|
void handle_command(const std::string& line, PairingSharedState* shared) {
|
|
(void)shared;
|
|
const std::string command = extract_command_value(line, "command").value_or("");
|
|
// Pairing and reconnect are deliberately automatic. The only command the
|
|
// Node supervisor needs is a clean shutdown signal; removing manual pair,
|
|
// forget, and disconnect modes keeps the hardware flow single-purpose.
|
|
if (command == "stop") {
|
|
running.store(false);
|
|
}
|
|
}
|
|
|
|
void stdin_loop(PairingSharedState* shared) {
|
|
std::string line;
|
|
while (running.load() && std::getline(std::cin, line)) handle_command(line, shared);
|
|
}
|
|
|
|
bool sleeping_connection_error(int error_number) {
|
|
// A powered-off board normally answers an outgoing page with one of these
|
|
// transport errors. They mean "keep waiting for the front button," not that
|
|
// installation is broken. Every other errno is surfaced verbatim in the UI.
|
|
return error_number == EHOSTDOWN || error_number == EHOSTUNREACH ||
|
|
error_number == ETIMEDOUT || error_number == ECONNREFUSED;
|
|
}
|
|
|
|
void close_wiiuse_sockets(wiimote_t* board) {
|
|
if (!board) return;
|
|
|
|
// Wiiuse 0.15.5 can leave a socket allocated when the second L2CAP connect
|
|
// fails. Close both descriptors explicitly so an unattended server can page
|
|
// a sleeping board forever without leaking one descriptor per attempt.
|
|
const int output_socket = board->out_sock;
|
|
const int input_socket = board->in_sock;
|
|
if (output_socket >= 0) close(output_socket);
|
|
if (input_socket >= 0 && input_socket != output_socket) close(input_socket);
|
|
board->out_sock = -1;
|
|
board->in_sock = -1;
|
|
}
|
|
|
|
void prepare_wiiuse_address(wiimote_t* board, const std::string& address) {
|
|
// Supplying the saved address and DEV_FOUND flag tells wiiuse to skip its own
|
|
// discovery pass. That makes every reconnect a direct page of the one board
|
|
// already commissioned to this server.
|
|
str2ba(address.c_str(), &board->bdaddr);
|
|
std::snprintf(board->bdaddr_str, sizeof(board->bdaddr_str), "%s", address.c_str());
|
|
board->state |= WIIMOTE_STATE_DEV_FOUND;
|
|
}
|
|
|
|
wiimote_t** initialize_wiiuse() {
|
|
// wiiuse_init prints one version banner directly to stdout rather than using
|
|
// its logger. Suppress only that initialization call before any worker
|
|
// threads exist, then restore stdout for the JSON protocol.
|
|
std::fflush(stdout);
|
|
const int saved_stdout = dup(STDOUT_FILENO);
|
|
const int null_output = open("/dev/null", O_WRONLY | O_CLOEXEC);
|
|
if (saved_stdout >= 0 && null_output >= 0) dup2(null_output, STDOUT_FILENO);
|
|
wiimote_t** boards = wiiuse_init(1);
|
|
std::fflush(stdout);
|
|
if (saved_stdout >= 0) {
|
|
dup2(saved_stdout, STDOUT_FILENO);
|
|
close(saved_stdout);
|
|
}
|
|
if (null_output >= 0) close(null_output);
|
|
|
|
// Wiiuse resets its log targets inside wiiuse_init, so configure them after
|
|
// initialization. Retain actual library errors on stderr, but discard normal
|
|
// connect/disconnect chatter that repeats for every page while the board is
|
|
// asleep; structured JSON already describes that state for the panel.
|
|
wiiuse_set_output(LOGLEVEL_ERROR, stderr);
|
|
wiiuse_set_output(LOGLEVEL_WARNING, nullptr);
|
|
wiiuse_set_output(LOGLEVEL_INFO, nullptr);
|
|
wiiuse_set_output(LOGLEVEL_DEBUG, nullptr);
|
|
return boards;
|
|
}
|
|
|
|
bool request_low_bluetooth_security(int fd, std::string* error) {
|
|
bt_security security{};
|
|
security.level = BT_SECURITY_LOW;
|
|
if (setsockopt(fd, SOL_BLUETOOTH, BT_SECURITY, &security, sizeof(security)) == 0) {
|
|
return true;
|
|
}
|
|
if (error) *error = std::strerror(errno);
|
|
return false;
|
|
}
|
|
|
|
int open_hid_listener(uint16_t psm, std::string* error) {
|
|
const int fd = socket(AF_BLUETOOTH,
|
|
SOCK_SEQPACKET | SOCK_CLOEXEC | SOCK_NONBLOCK,
|
|
BTPROTO_L2CAP);
|
|
if (fd < 0) {
|
|
if (error) *error = std::strerror(errno);
|
|
return -1;
|
|
}
|
|
|
|
// The bonded BlueZ input profile requested medium security and produced the
|
|
// original EACCES failure. Wii hardware HID channels are intentionally low
|
|
// security; applying that level to the listener also makes it inherit onto
|
|
// every accepted front-button connection.
|
|
if (!request_low_bluetooth_security(fd, error)) {
|
|
close(fd);
|
|
return -1;
|
|
}
|
|
|
|
sockaddr_l2 local{};
|
|
local.l2_family = AF_BLUETOOTH;
|
|
local.l2_psm = htobs(psm);
|
|
// Value initialization leaves l2_bdaddr at the all-zero BDADDR_ANY value.
|
|
// Avoid BlueZ's C-only compound-literal macro, which is not valid C++17.
|
|
if (bind(fd, reinterpret_cast<const sockaddr*>(&local), sizeof(local)) != 0 ||
|
|
listen(fd, 1) != 0) {
|
|
if (error) *error = std::strerror(errno);
|
|
close(fd);
|
|
return -1;
|
|
}
|
|
return fd;
|
|
}
|
|
|
|
std::optional<int> accept_board_channel(int listener,
|
|
const std::string& expected_address,
|
|
std::string* error) {
|
|
sockaddr_l2 remote{};
|
|
socklen_t remote_size = sizeof(remote);
|
|
const int fd = accept4(listener, reinterpret_cast<sockaddr*>(&remote),
|
|
&remote_size, SOCK_CLOEXEC);
|
|
if (fd < 0) {
|
|
if (errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR && error) {
|
|
*error = std::strerror(errno);
|
|
}
|
|
return std::nullopt;
|
|
}
|
|
|
|
char remote_text[18]{};
|
|
ba2str(&remote.l2_bdaddr, remote_text);
|
|
const auto normalized = parse_address(remote_text);
|
|
if (!normalized.has_value() || normalized->display != expected_address) {
|
|
// Both HID PSMs are global to the adapter. The installer dedicates them to
|
|
// this worker, but still reject any unrelated controller instead of
|
|
// attaching an arbitrary input device to the Balance Board parser.
|
|
close(fd);
|
|
return std::nullopt;
|
|
}
|
|
return fd;
|
|
}
|
|
|
|
void attach_incoming_board(wiimote_t* board, int control_fd, int interrupt_fd,
|
|
const std::string& address) {
|
|
// A reconnecting Wii device opens both channels toward the remembered host:
|
|
// control on PSM 0x11 followed by interrupt on PSM 0x13. Once both accepted
|
|
// sockets exist, their direction and semantics are identical to the pair
|
|
// created by wiiuse_connect(). Attach them and run the upstream handshake.
|
|
close_wiiuse_sockets(board);
|
|
wiiuse_disconnected(board);
|
|
prepare_wiiuse_address(board, address);
|
|
board->out_sock = control_fd;
|
|
board->in_sock = interrupt_fd;
|
|
board->state |= WIIMOTE_STATE_CONNECTED;
|
|
wiiuse_handshake(board, nullptr, 0);
|
|
wiiuse_set_report_type(board);
|
|
}
|
|
|
|
struct PendingIncomingChannels {
|
|
int control_fd = -1;
|
|
int interrupt_fd = -1;
|
|
uint64_t first_channel_at = 0;
|
|
};
|
|
|
|
void close_pending_channels(PendingIncomingChannels* pending) {
|
|
if (!pending) return;
|
|
if (pending->control_fd >= 0) close(pending->control_fd);
|
|
if (pending->interrupt_fd >= 0 && pending->interrupt_fd != pending->control_fd) {
|
|
close(pending->interrupt_fd);
|
|
}
|
|
pending->control_fd = -1;
|
|
pending->interrupt_fd = -1;
|
|
pending->first_channel_at = 0;
|
|
}
|
|
|
|
void direct_connection_loop(PairingSharedState* shared, wiimote_t** boards) {
|
|
wiimote_t* board = boards ? boards[0] : nullptr;
|
|
if (!board) {
|
|
emit_status("error", "", "wiiuse could not initialize the Balance Board connection");
|
|
return;
|
|
}
|
|
|
|
std::string control_listener_error;
|
|
const int control_listener = open_hid_listener(kHidControlPsm, &control_listener_error);
|
|
if (control_listener < 0) {
|
|
emit_status("error", "",
|
|
"Cannot listen for the Balance Board control channel: " +
|
|
control_listener_error +
|
|
". Run the installer to configure the dedicated Bluetooth listener.");
|
|
return;
|
|
}
|
|
std::string interrupt_listener_error;
|
|
const int interrupt_listener = open_hid_listener(
|
|
kHidInterruptPsm, &interrupt_listener_error);
|
|
if (interrupt_listener < 0) {
|
|
close(control_listener);
|
|
emit_status("error", "",
|
|
"Cannot listen for the Balance Board interrupt channel: " +
|
|
interrupt_listener_error +
|
|
". Run the installer to configure the dedicated Bluetooth listener.");
|
|
return;
|
|
}
|
|
|
|
std::string prepared_address;
|
|
uint64_t outbound_connect_until = 0;
|
|
PendingIncomingChannels pending;
|
|
while (running.load()) {
|
|
std::optional<std::string> address;
|
|
bool outbound_requested = false;
|
|
{
|
|
std::lock_guard<std::mutex> lock(shared->mutex);
|
|
address = shared->commissioned_address;
|
|
outbound_requested = shared->outbound_connection_requested;
|
|
shared->outbound_connection_requested = false;
|
|
}
|
|
if (!address.has_value()) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
continue;
|
|
}
|
|
|
|
if (prepared_address != *address) {
|
|
// Never combine a channel from the previous configured board with a
|
|
// channel from the new one. This normally matters only after Forget and
|
|
// re-pair, but keeping the socket pair atomic prevents a misleading
|
|
// handshake failure during that transition.
|
|
close_pending_channels(&pending);
|
|
close_wiiuse_sockets(board);
|
|
wiiuse_disconnected(board);
|
|
prepare_wiiuse_address(board, *address);
|
|
prepared_address = *address;
|
|
}
|
|
|
|
if (outbound_requested) {
|
|
outbound_connect_until = monotonic_ms() + kCommissioningConnectWindowMs;
|
|
}
|
|
|
|
bool transport_connected = false;
|
|
std::string control_error;
|
|
if (auto control_fd = accept_board_channel(
|
|
control_listener, *address, &control_error)) {
|
|
if (pending.control_fd >= 0) close(pending.control_fd);
|
|
pending.control_fd = *control_fd;
|
|
if (pending.first_channel_at == 0) {
|
|
pending.first_channel_at = monotonic_ms();
|
|
emit_status("link-detected", *address,
|
|
"Front button reached the Bluetooth control channel.");
|
|
}
|
|
} else if (!control_error.empty()) {
|
|
emit_status("connection-failed", *address,
|
|
"Balance Board control listener failed: " + control_error);
|
|
}
|
|
|
|
std::string interrupt_error;
|
|
if (auto interrupt_fd = accept_board_channel(
|
|
interrupt_listener, *address, &interrupt_error)) {
|
|
if (pending.interrupt_fd >= 0) close(pending.interrupt_fd);
|
|
pending.interrupt_fd = *interrupt_fd;
|
|
if (pending.first_channel_at == 0) {
|
|
pending.first_channel_at = monotonic_ms();
|
|
emit_status("link-detected", *address,
|
|
"Front button reached the Bluetooth interrupt channel.");
|
|
}
|
|
} else if (!interrupt_error.empty()) {
|
|
emit_status("connection-failed", *address,
|
|
"Balance Board interrupt listener failed: " + interrupt_error);
|
|
}
|
|
|
|
if (pending.control_fd >= 0 && pending.interrupt_fd >= 0) {
|
|
attach_incoming_board(
|
|
board, pending.control_fd, pending.interrupt_fd, *address);
|
|
pending.control_fd = -1;
|
|
pending.interrupt_fd = -1;
|
|
pending.first_channel_at = 0;
|
|
transport_connected = true;
|
|
} else if (pending.first_channel_at != 0 &&
|
|
monotonic_ms() - pending.first_channel_at >=
|
|
kIncomingChannelPairTimeoutMs) {
|
|
const bool control_arrived = pending.control_fd >= 0;
|
|
close_pending_channels(&pending);
|
|
emit_status(
|
|
"connection-failed", *address,
|
|
control_arrived
|
|
? "Front button reached the control channel, but the interrupt channel did not arrive."
|
|
: "Front button reached the interrupt channel, but the control channel did not arrive.");
|
|
}
|
|
|
|
if (!transport_connected && pending.first_channel_at == 0 &&
|
|
monotonic_ms() < outbound_connect_until) {
|
|
// Red Sync makes the board discoverable instead of reconnecting to the
|
|
// remembered host. During the short post-commissioning window only,
|
|
// retain wiiuse's normal outbound connector so the first session starts
|
|
// without asking for a second physical button press.
|
|
errno = 0;
|
|
const int connected_count = wiiuse_connect(boards, 1);
|
|
const int connection_error = errno;
|
|
if (connected_count == 1 && WIIMOTE_IS_CONNECTED(board)) {
|
|
transport_connected = true;
|
|
outbound_connect_until = 0;
|
|
emit_status("link-detected", *address);
|
|
} else {
|
|
close_wiiuse_sockets(board);
|
|
wiiuse_disconnected(board);
|
|
prepare_wiiuse_address(board, *address);
|
|
if (connection_error != 0 && !sleeping_connection_error(connection_error)) {
|
|
emit_status("connection-failed", *address,
|
|
"Initial Balance Board connection failed: " +
|
|
std::string(std::strerror(connection_error)));
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!transport_connected) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(25));
|
|
continue;
|
|
}
|
|
|
|
bool board_ready = false;
|
|
bool handshake_warning_sent = false;
|
|
bool intentional_sleep = false;
|
|
std::optional<BoardReadings> activity_reference;
|
|
uint64_t connected_at = monotonic_ms();
|
|
uint64_t last_movement_at = connected_at;
|
|
uint64_t last_frame_at = 0;
|
|
while (running.load() && WIIMOTE_IS_CONNECTED(board)) {
|
|
wiiuse_poll(boards, 1);
|
|
if (board->event == WIIUSE_DISCONNECT ||
|
|
board->event == WIIUSE_UNEXPECTED_DISCONNECT) {
|
|
break;
|
|
}
|
|
|
|
if (board->exp.type == EXP_WII_BOARD) {
|
|
if (!board_ready) {
|
|
board_ready = true;
|
|
// The Balance Board has one blue player light. Wiiuse clears all LEDs
|
|
// during its handshake, which leaves the light flashing even though
|
|
// measurements work. A solid first LED is the unambiguous connected
|
|
// indication used for the rest of this session.
|
|
wiiuse_set_leds(board, WIIMOTE_LED_1);
|
|
emit_status("connected", *address);
|
|
}
|
|
const uint64_t now = monotonic_ms();
|
|
if (now - last_frame_at >= kFrameIntervalMs) {
|
|
// Wiiuse interpolates each sensor using the board's factory 0/17/34kg
|
|
// calibration values. Preserve the existing centi-kilogram wire unit
|
|
// so Node can apply its persisted installation zero per corner without
|
|
// losing the native sensor resolution.
|
|
const wii_board_t& weights = board->exp.wb;
|
|
BoardReadings readings{
|
|
static_cast<int>(std::lround(std::max(0.0F, weights.tr) * 100.0F)),
|
|
static_cast<int>(std::lround(std::max(0.0F, weights.br) * 100.0F)),
|
|
static_cast<int>(std::lround(std::max(0.0F, weights.tl) * 100.0F)),
|
|
static_cast<int>(std::lround(std::max(0.0F, weights.bl) * 100.0F)),
|
|
};
|
|
const int battery = static_cast<int>(std::lround(
|
|
std::clamp(board->battery_level, 0.0F, 1.0F) * 100.0F));
|
|
emit_frame(readings, battery);
|
|
last_frame_at = now;
|
|
|
|
if (!activity_reference.has_value()) {
|
|
activity_reference = readings;
|
|
last_movement_at = now;
|
|
} else {
|
|
// Compare against the last meaningful activity snapshot rather
|
|
// than the immediately previous frame. That lets slow movement
|
|
// accumulate past the noise threshold while ordinary sensor jitter
|
|
// cannot keep the board awake forever. All four corners matter, so
|
|
// shifting a load without changing total weight still counts.
|
|
const int movement =
|
|
std::abs(readings.top_right - activity_reference->top_right) +
|
|
std::abs(readings.bottom_right - activity_reference->bottom_right) +
|
|
std::abs(readings.top_left - activity_reference->top_left) +
|
|
std::abs(readings.bottom_left - activity_reference->bottom_left);
|
|
if (movement >= kMovementThresholdCentiKg) {
|
|
activity_reference = readings;
|
|
last_movement_at = now;
|
|
}
|
|
}
|
|
|
|
if (now - last_movement_at >= kStillSleepDelayMs) {
|
|
intentional_sleep = true;
|
|
emit_status("sleeping", *address,
|
|
"Board is asleep. Press the front power button to wake it.");
|
|
break;
|
|
}
|
|
}
|
|
} else if (!handshake_warning_sent &&
|
|
monotonic_ms() - connected_at >= kHandshakeWarningMs) {
|
|
// A live ACL connection without the permanent Balance Board expansion
|
|
// means sensor calibration never completed. Report that precise stage
|
|
// while continuing to poll, since a delayed response can still recover.
|
|
handshake_warning_sent = true;
|
|
emit_status("connection-failed", *address,
|
|
"Bluetooth connected, but the board did not finish sensor calibration.");
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
|
|
close_wiiuse_sockets(board);
|
|
wiiuse_disconnected(board);
|
|
prepare_wiiuse_address(board, *address);
|
|
if (intentional_sleep) {
|
|
// Closing both HID channels makes the board abandon the host connection
|
|
// and power itself down. Both HID listeners stay open without paging it,
|
|
// so only a later front-button connection starts another session.
|
|
} else {
|
|
emit_status("waiting", *address, "Board disconnected. Press the front power button.");
|
|
}
|
|
}
|
|
|
|
close_pending_channels(&pending);
|
|
close(control_listener);
|
|
close(interrupt_listener);
|
|
close_wiiuse_sockets(board);
|
|
wiiuse_disconnected(board);
|
|
}
|
|
|
|
void simulated_loop() {
|
|
// Exercise the same status contract as real hardware so development UI
|
|
// builds cannot silently break merely because CI lacks a physical board.
|
|
emit_status("waiting", "SIMULATED");
|
|
const std::array<BoardReadings, 12> sequence{{
|
|
{0, 0, 0, 0}, {0, 0, 0, 0}, {40, 30, 35, 25}, {95, 82, 90, 76},
|
|
{103, 97, 101, 99}, {104, 98, 101, 99}, {103, 98, 102, 99},
|
|
{103, 98, 101, 100}, {104, 98, 101, 99}, {75, 65, 70, 60},
|
|
{20, 12, 15, 10}, {0, 0, 0, 0},
|
|
}};
|
|
while (running.load()) {
|
|
emit_status("connected", "SIMULATED");
|
|
for (const auto& readings : sequence) {
|
|
for (int frame = 0; frame < 12 && running.load(); ++frame) {
|
|
emit_frame(readings, 82);
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(kFrameIntervalMs));
|
|
}
|
|
}
|
|
emit_status("waiting", "SIMULATED");
|
|
for (int pause = 0; pause < 30 && running.load(); ++pause) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
int main() {
|
|
std::signal(SIGINT, signal_handler);
|
|
std::signal(SIGTERM, signal_handler);
|
|
|
|
const std::string simulation = std::getenv("BALANCE_BOARD_SIMULATE")
|
|
? std::getenv("BALANCE_BOARD_SIMULATE") : "";
|
|
if (simulation == "1" || simulation == "true" || simulation == "cycle") {
|
|
simulated_loop();
|
|
return 0;
|
|
}
|
|
|
|
wiimote_t** boards = initialize_wiiuse();
|
|
|
|
PairingSharedState pairing;
|
|
const std::string configured_address = std::getenv("BALANCE_BOARD_ADDRESS")
|
|
? std::getenv("BALANCE_BOARD_ADDRESS") : "";
|
|
if (auto parsed = parse_address(configured_address)) {
|
|
pairing.commissioned_address = parsed->display;
|
|
} else {
|
|
pairing.commissioning = true;
|
|
}
|
|
|
|
const int management_fd = open_management_socket();
|
|
bool bluetooth_startup_ready = true;
|
|
std::string incoming_connection_error;
|
|
if (management_fd < 0) {
|
|
// The socket now serves both commissioning and front-button wake: it sends
|
|
// the raw six-byte PIN and keeps hci0 connectable for incoming pages. Never
|
|
// pretend an already-known address can wake reliably without it.
|
|
pairing.commissioning = false;
|
|
bluetooth_startup_ready = false;
|
|
emit_status("error", configured_address,
|
|
"Bluetooth management socket unavailable; install the worker capability");
|
|
} else if (!enable_incoming_connections(
|
|
management_fd, &incoming_connection_error)) {
|
|
bluetooth_startup_ready = false;
|
|
emit_status("error", configured_address,
|
|
"Could not enable reliable incoming Bluetooth connections on hci0: " +
|
|
incoming_connection_error);
|
|
}
|
|
|
|
std::thread commission_thread;
|
|
std::thread connection_thread;
|
|
if (bluetooth_startup_ready) {
|
|
commission_thread = std::thread(commissioning_loop, &pairing);
|
|
connection_thread = std::thread(direct_connection_loop, &pairing, boards);
|
|
}
|
|
std::thread input_thread(stdin_loop, &pairing);
|
|
if (bluetooth_startup_ready &&
|
|
(management_fd >= 0 || pairing.commissioned_address.has_value())) {
|
|
emit_status(pairing.commissioning ? "commissioning" : "waiting", configured_address);
|
|
}
|
|
|
|
ManagementRuntimeState management;
|
|
while (running.load()) {
|
|
process_management_events(management_fd, &pairing, &management);
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
|
|
if (management_fd >= 0) close(management_fd);
|
|
if (input_thread.joinable()) input_thread.detach();
|
|
if (commission_thread.joinable()) commission_thread.join();
|
|
if (connection_thread.joinable()) connection_thread.join();
|
|
if (boards) wiiuse_cleanup(boards, 1);
|
|
return 0;
|
|
}
|