feat: port GIP protocol and auth stack
Port the GIP protocol and authentication layers from medusalix/xone into the C++ stack: - crypto: SHA-256/HMAC (CommonCrypto), RSA PKCS#1 (Security.framework), and a self-contained P-256 ECDH validated against OpenSSL vectors - auth: v1 (RSA) and v2 (ECDH) handshake state machine - gip: header/varint/chunk handling and packet dispatch, with the kernel device model replaced by transport + client_listener interfaces Adds test_crypto, test_auth, and test_gip suites. Co-Authored-By: deepseek (deepseek/deepseek-v4-pro-0813): ported crypto, auth, and GIP
This commit is contained in:
portersky
@@ -0,0 +1,483 @@
|
||||
// GIP authentication handshake (port of medusalix/xone auth/auth.c).
|
||||
|
||||
#include "auth/auth.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
#include <errno.h>
|
||||
|
||||
#include "common/log.hpp"
|
||||
#include "common/types.hpp"
|
||||
|
||||
namespace xone::auth {
|
||||
|
||||
namespace {
|
||||
|
||||
enum : u8 {
|
||||
ctx_handshake = 0x00,
|
||||
ctx_control = 0x01,
|
||||
};
|
||||
|
||||
enum : u8 {
|
||||
cmd_host_hello = 0x01,
|
||||
cmd_client_hello = 0x02,
|
||||
cmd_client_certificate = 0x03,
|
||||
cmd_host_secret = 0x05,
|
||||
cmd_host_finish = 0x07,
|
||||
cmd_client_finish = 0x08,
|
||||
|
||||
cmd2_host_hello = 0x21,
|
||||
cmd2_client_hello = 0x22,
|
||||
cmd2_client_certificate = 0x23,
|
||||
cmd2_client_pubkey = 0x24,
|
||||
cmd2_host_pubkey = 0x25,
|
||||
cmd2_host_finish = 0x26,
|
||||
cmd2_client_finish = 0x27,
|
||||
};
|
||||
|
||||
enum : u8 {
|
||||
ctrl_complete = 0x00,
|
||||
ctrl_reset = 0x01,
|
||||
};
|
||||
|
||||
enum : u8 {
|
||||
opt_acknowledge = 1 << 0,
|
||||
opt_request = 1 << 1,
|
||||
opt_from_host = 1 << 6,
|
||||
};
|
||||
|
||||
struct header_handshake {
|
||||
u8 context;
|
||||
u8 options;
|
||||
u8 error;
|
||||
u8 command;
|
||||
u16 length;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct header_data {
|
||||
u8 command;
|
||||
u8 version;
|
||||
u16 length;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct header_full {
|
||||
header_handshake handshake;
|
||||
header_data data;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct header_control {
|
||||
u8 context;
|
||||
u8 control;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt_request {
|
||||
header_handshake header;
|
||||
std::array<u8, k_trailer_len> trailer;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt_host_hello {
|
||||
header_full header;
|
||||
std::array<u8, k_random_len> random;
|
||||
std::array<u8, 4> unknown1;
|
||||
std::array<u8, 4> unknown2;
|
||||
std::array<u8, k_trailer_len> trailer;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt_host_secret {
|
||||
header_full header;
|
||||
std::array<u8, k_encrypted_pms_len> encrypted_pms;
|
||||
std::array<u8, k_trailer_len> trailer;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt_host_finish {
|
||||
header_full header;
|
||||
std::array<u8, k_transcript_len> transcript;
|
||||
std::array<u8, k_trailer_len> trailer;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt_client_hello {
|
||||
std::array<u8, k_random_len> random;
|
||||
std::array<u8, 48> unknown;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt_client_finish {
|
||||
std::array<u8, k_transcript_len> transcript;
|
||||
std::array<u8, 32> unknown;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt2_host_hello {
|
||||
header_full header;
|
||||
std::array<u8, k_random_len> random;
|
||||
std::array<u8, 4> unknown;
|
||||
std::array<u8, k_trailer_len> trailer;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt2_host_pubkey {
|
||||
header_full header;
|
||||
std::array<u8, k_pubkey2_len> pubkey;
|
||||
std::array<u8, k_trailer_len> trailer;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt2_client_hello {
|
||||
std::array<u8, k_random_len> random;
|
||||
std::array<u8, 108> unknown1;
|
||||
std::array<u8, 32> unknown2;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt2_client_cert {
|
||||
std::array<char, 4> header;
|
||||
std::array<u8, 136> unknown1;
|
||||
std::array<char, 32> chip;
|
||||
std::array<char, 20> revision;
|
||||
std::array<u8, 576> unknown2;
|
||||
} XONE_PACKED;
|
||||
|
||||
struct pkt2_client_pubkey {
|
||||
std::array<u8, k_pubkey2_len> pubkey;
|
||||
std::array<u8, 64> unknown;
|
||||
} XONE_PACKED;
|
||||
|
||||
} // namespace
|
||||
|
||||
auth::auth(auth_sink& sink)
|
||||
: sink_(sink)
|
||||
{
|
||||
}
|
||||
|
||||
auto auth::start() -> int
|
||||
{
|
||||
return send_hello();
|
||||
}
|
||||
|
||||
auto auth::process_pkt(std::span<u8 const> data) -> int
|
||||
{
|
||||
auto const* hdr = reinterpret_cast<header_handshake const*>(data.data());
|
||||
|
||||
if (data.size() < sizeof(*hdr))
|
||||
return -EINVAL;
|
||||
|
||||
if (hdr->error)
|
||||
return -EPROTO;
|
||||
|
||||
if (hdr->options & opt_acknowledge) {
|
||||
if (hdr->command == 0x01)
|
||||
return handle_pkt_acknowledge();
|
||||
|
||||
log_msg(log_level::error, "auth: handshake failed: 0x%02x", hdr->command);
|
||||
return -EPROTO;
|
||||
}
|
||||
|
||||
return handle_pkt_data(data);
|
||||
}
|
||||
|
||||
auto auth::send_pkt(u8 cmd, void const* pkt, std::size_t len) -> int
|
||||
{
|
||||
auto* hdr = reinterpret_cast<header_full*>(const_cast<u8*>(static_cast<u8 const*>(pkt)));
|
||||
u16 data_len = static_cast<u16>(len - sizeof(hdr->handshake) - k_trailer_len);
|
||||
|
||||
hdr->handshake.context = ctx_handshake;
|
||||
hdr->handshake.options = opt_acknowledge | opt_from_host;
|
||||
hdr->handshake.error = 0;
|
||||
hdr->handshake.command = cmd;
|
||||
store_be16(&hdr->handshake.length, data_len);
|
||||
|
||||
hdr->data.command = cmd;
|
||||
hdr->data.version = cmd >= cmd2_host_hello ? 0x02 : 0x01;
|
||||
store_be16(&hdr->data.length, static_cast<u16>(data_len - sizeof(hdr->data)));
|
||||
|
||||
last_sent_command_ = cmd;
|
||||
transcript_.update(
|
||||
{reinterpret_cast<u8 const*>(hdr) + sizeof(hdr->handshake), data_len});
|
||||
|
||||
return sink_.send({reinterpret_cast<u8 const*>(hdr), len}, true);
|
||||
}
|
||||
|
||||
auto auth::request_pkt(u8 cmd, std::uint16_t len) -> int
|
||||
{
|
||||
pkt_request req{};
|
||||
u16 data_len = static_cast<u16>(len + sizeof(header_data));
|
||||
|
||||
req.header.context = ctx_handshake;
|
||||
req.header.options = opt_request | opt_from_host;
|
||||
req.header.error = 0;
|
||||
req.header.command = cmd;
|
||||
store_be16(&req.header.length, data_len);
|
||||
|
||||
return sink_.send({reinterpret_cast<u8 const*>(&req), sizeof(req)}, true);
|
||||
}
|
||||
|
||||
auto auth::send_hello() -> int
|
||||
{
|
||||
pkt_host_hello pkt{};
|
||||
random_bytes(random_host_);
|
||||
std::memcpy(pkt.random.data(), random_host_.data(), k_random_len);
|
||||
return send_pkt(cmd_host_hello, &pkt.header, sizeof(pkt));
|
||||
}
|
||||
|
||||
auto auth::send_finish(u8 cmd) -> int
|
||||
{
|
||||
pkt_host_finish pkt{};
|
||||
auto transcript = get_transcript();
|
||||
prf_sha256(master_secret_, "Host Finished", transcript, pkt.transcript);
|
||||
return send_pkt(cmd, &pkt.header, sizeof(pkt));
|
||||
}
|
||||
|
||||
auto auth::send_complete() -> int
|
||||
{
|
||||
header_control hdr{};
|
||||
hdr.context = ctx_control;
|
||||
hdr.control = ctrl_complete;
|
||||
return sink_.send({reinterpret_cast<u8 const*>(&hdr), sizeof(hdr)}, false);
|
||||
}
|
||||
|
||||
auto auth::get_transcript() -> std::array<u8, k_transcript_len>
|
||||
{
|
||||
sha256 snap = transcript_;
|
||||
std::array<u8, k_transcript_len> out{};
|
||||
snap.finalize(out);
|
||||
return out;
|
||||
}
|
||||
|
||||
auto auth::exchange_rsa() -> void
|
||||
{
|
||||
pkt_host_secret pkt{};
|
||||
std::array<u8, k_random_len * 2> random{};
|
||||
std::array<u8, k_secret_len> pms{};
|
||||
|
||||
std::copy(random_host_.begin(), random_host_.end(), random.begin());
|
||||
std::copy(random_client_.begin(), random_client_.end(),
|
||||
random.begin() + k_random_len);
|
||||
|
||||
random_bytes(pms);
|
||||
|
||||
std::array<u8, k_encrypted_pms_len> encrypted{};
|
||||
if (!rsa_encrypt_pkcs1(pubkey_client_, pms, encrypted)) {
|
||||
log_msg(log_level::error, "auth: encrypt RSA failed");
|
||||
return;
|
||||
}
|
||||
std::memcpy(pkt.encrypted_pms.data(), encrypted.data(), k_encrypted_pms_len);
|
||||
|
||||
prf_sha256(pms, "Master Secret", random, master_secret_);
|
||||
|
||||
if (send_pkt(cmd_host_secret, &pkt.header, sizeof(pkt)))
|
||||
log_msg(log_level::error, "auth: send pkt failed");
|
||||
}
|
||||
|
||||
auto auth::exchange_ecdh() -> void
|
||||
{
|
||||
pkt2_host_pubkey pkt{};
|
||||
std::array<u8, k_random_len * 2> random{};
|
||||
std::array<u8, k_secret2_len> secret{};
|
||||
|
||||
std::copy(random_host_.begin(), random_host_.end(), random.begin());
|
||||
std::copy(random_client_.begin(), random_client_.end(),
|
||||
random.begin() + k_random_len);
|
||||
|
||||
ec_scalar d{};
|
||||
ec_point q{};
|
||||
ec_generate_keypair(d, q);
|
||||
std::memcpy(pkt.pubkey.data(), q.data(), k_pubkey2_len);
|
||||
|
||||
ec_scalar shared_x{};
|
||||
if (!ec_compute_shared(d, pubkey_client2_, shared_x)) {
|
||||
log_msg(log_level::error, "auth: compute ECDH failed");
|
||||
return;
|
||||
}
|
||||
|
||||
// The PRF key is SHA-256 of the raw shared x-coordinate.
|
||||
sha256 h;
|
||||
h.update(shared_x);
|
||||
h.finalize(secret);
|
||||
|
||||
prf_sha256(secret, "Master Secret", random, master_secret_);
|
||||
|
||||
if (send_pkt(cmd2_host_pubkey, &pkt.header, sizeof(pkt)))
|
||||
log_msg(log_level::error, "auth: send pkt failed");
|
||||
}
|
||||
|
||||
auto auth::complete_handshake() -> void
|
||||
{
|
||||
std::array<u8, k_random_len * 2> random{};
|
||||
std::array<u8, k_session_key_len> key{};
|
||||
|
||||
std::copy(random_host_.begin(), random_host_.end(), random.begin());
|
||||
std::copy(random_client_.begin(), random_client_.end(),
|
||||
random.begin() + k_random_len);
|
||||
|
||||
prf_sha256(master_secret_,
|
||||
"EXPORTER DAWN data channel session key for controller", random,
|
||||
key);
|
||||
|
||||
if (send_complete())
|
||||
log_msg(log_level::error, "auth: send complete failed");
|
||||
|
||||
if (sink_.set_encryption_key(key))
|
||||
log_msg(log_level::error, "auth: set encryption key failed");
|
||||
}
|
||||
|
||||
auto auth::handle_pkt_acknowledge() -> int
|
||||
{
|
||||
switch (last_sent_command_) {
|
||||
case cmd2_host_hello:
|
||||
return request_pkt(cmd2_client_hello, sizeof(pkt2_client_hello));
|
||||
case cmd2_host_pubkey:
|
||||
return send_finish(cmd2_host_finish);
|
||||
case cmd2_host_finish:
|
||||
return request_pkt(cmd2_client_finish, sizeof(pkt_client_finish));
|
||||
case cmd_host_hello:
|
||||
return request_pkt(cmd_client_hello, sizeof(pkt_client_hello));
|
||||
case cmd_host_secret:
|
||||
return send_finish(cmd_host_finish);
|
||||
case cmd_host_finish:
|
||||
return request_pkt(cmd_client_finish, sizeof(pkt_client_finish));
|
||||
default:
|
||||
return -EPROTO;
|
||||
}
|
||||
}
|
||||
|
||||
auto auth::handle_pkt_data(std::span<u8 const> data) -> int
|
||||
{
|
||||
auto const* hdr = reinterpret_cast<header_full const*>(data.data());
|
||||
|
||||
if (data.size() < sizeof(*hdr))
|
||||
return -EINVAL;
|
||||
|
||||
// The controller upgraded to auth v2: restart the handshake.
|
||||
if (hdr->handshake.command != hdr->data.command) {
|
||||
log_msg(log_level::debug, "auth: protocol upgrade to v2");
|
||||
transcript_ = sha256{};
|
||||
return send_hello2();
|
||||
}
|
||||
|
||||
int err = dispatch_pkt(hdr->data.command, data.subspan(sizeof(*hdr)));
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
transcript_.update(data.subspan(sizeof(hdr->handshake)));
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto auth::dispatch_pkt(u8 cmd, std::span<u8 const> data) -> int
|
||||
{
|
||||
switch (cmd) {
|
||||
case cmd2_client_hello:
|
||||
return handle_hello2(data);
|
||||
case cmd2_client_certificate:
|
||||
return handle_certificate2(data);
|
||||
case cmd2_client_pubkey:
|
||||
return handle_pubkey(data);
|
||||
case cmd2_client_finish:
|
||||
return handle_finish(data);
|
||||
case cmd_client_hello:
|
||||
return handle_hello(data);
|
||||
case cmd_client_certificate:
|
||||
return handle_certificate(data);
|
||||
case cmd_client_finish:
|
||||
return handle_finish(data);
|
||||
default:
|
||||
return -EPROTO;
|
||||
}
|
||||
}
|
||||
|
||||
auto auth::handle_hello(std::span<u8 const> data) -> int
|
||||
{
|
||||
auto const* pkt = reinterpret_cast<pkt_client_hello const*>(data.data());
|
||||
|
||||
if (data.size() < sizeof(*pkt))
|
||||
return -EINVAL;
|
||||
|
||||
std::memcpy(random_client_.data(), pkt->random.data(), k_random_len);
|
||||
return request_pkt(cmd_client_certificate, k_certificate_max_len);
|
||||
}
|
||||
|
||||
auto auth::handle_certificate(std::span<u8 const> data) -> int
|
||||
{
|
||||
// The client cert embeds a DER RSAPublicKey: ASN.1 SEQUENCE of length
|
||||
// 0x010a. Search for its header and copy the whole 270-byte key.
|
||||
static constexpr u8 asn1_seq[] = {0x30, 0x82, 0x01, 0x0a};
|
||||
|
||||
if (data.size() > k_certificate_max_len)
|
||||
return -EINVAL;
|
||||
|
||||
for (std::size_t i = 0; i + sizeof(asn1_seq) <= data.size(); ++i) {
|
||||
if (std::memcmp(data.data() + i, asn1_seq, sizeof(asn1_seq)))
|
||||
continue;
|
||||
|
||||
if (i + k_pubkey_len > data.size())
|
||||
return -EINVAL;
|
||||
|
||||
std::memcpy(pubkey_client_.data(), data.data() + i, k_pubkey_len);
|
||||
exchange_rsa();
|
||||
return 0;
|
||||
}
|
||||
|
||||
return -EPROTO;
|
||||
}
|
||||
|
||||
auto auth::handle_finish(std::span<u8 const> data) -> int
|
||||
{
|
||||
auto const* pkt = reinterpret_cast<pkt_client_finish const*>(data.data());
|
||||
|
||||
if (data.size() < sizeof(*pkt))
|
||||
return -EINVAL;
|
||||
|
||||
auto transcript = get_transcript();
|
||||
std::array<u8, k_transcript_len> finished{};
|
||||
prf_sha256(master_secret_, "Device Finished", transcript, finished);
|
||||
|
||||
if (std::memcmp(pkt->transcript.data(), finished.data(), k_transcript_len)) {
|
||||
log_msg(log_level::error, "auth: transcript mismatch");
|
||||
return -EPROTO;
|
||||
}
|
||||
|
||||
complete_handshake();
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto auth::send_hello2() -> int
|
||||
{
|
||||
pkt2_host_hello pkt{};
|
||||
random_bytes(random_host_);
|
||||
std::memcpy(pkt.random.data(), random_host_.data(), k_random_len);
|
||||
return send_pkt(cmd2_host_hello, &pkt.header, sizeof(pkt));
|
||||
}
|
||||
|
||||
auto auth::handle_hello2(std::span<u8 const> data) -> int
|
||||
{
|
||||
auto const* pkt = reinterpret_cast<pkt2_client_hello const*>(data.data());
|
||||
|
||||
if (data.size() < sizeof(*pkt))
|
||||
return -EINVAL;
|
||||
|
||||
std::memcpy(random_client_.data(), pkt->random.data(), k_random_len);
|
||||
return request_pkt(cmd2_client_certificate, sizeof(pkt2_client_cert));
|
||||
}
|
||||
|
||||
auto auth::handle_certificate2(std::span<u8 const> data) -> int
|
||||
{
|
||||
auto const* pkt = reinterpret_cast<pkt2_client_cert const*>(data.data());
|
||||
|
||||
if (data.size() < sizeof(*pkt))
|
||||
return -EINVAL;
|
||||
|
||||
log_msg(log_level::debug, "auth: chip=%.*s, revision=%.*s",
|
||||
static_cast<int>(sizeof(pkt->chip)), pkt->chip.data(),
|
||||
static_cast<int>(sizeof(pkt->revision)), pkt->revision.data());
|
||||
|
||||
return request_pkt(cmd2_client_pubkey, sizeof(pkt2_client_pubkey));
|
||||
}
|
||||
|
||||
auto auth::handle_pubkey(std::span<u8 const> data) -> int
|
||||
{
|
||||
auto const* pkt = reinterpret_cast<pkt2_client_pubkey const*>(data.data());
|
||||
|
||||
if (data.size() < sizeof(*pkt))
|
||||
return -EINVAL;
|
||||
|
||||
std::memcpy(pubkey_client2_.data(), pkt->pubkey.data(), k_pubkey2_len);
|
||||
exchange_ecdh();
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace xone::auth
|
||||
+549
-2
@@ -1,7 +1,554 @@
|
||||
// Auth + crypto
|
||||
// TODO(phase 1): port from medusalix/xone auth/auth.c + auth/crypto.c.
|
||||
// Auth + crypto primitives (CommonCrypto / Security.framework / P-256 ECDH)
|
||||
// Port target: medusalix/xone auth/crypto.c.
|
||||
|
||||
#include "auth/crypto.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <CoreFoundation/CoreFoundation.h>
|
||||
#include <Security/Security.h>
|
||||
|
||||
namespace xone::auth {
|
||||
|
||||
namespace {
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
// Field arithmetic over GF(p), p = P-256, in Montgomery form.
|
||||
// --------------------------------------------------------------------------
|
||||
// Values are stored as v * R mod p with R = 2^256. Multiplication is a
|
||||
// Montgomery product (schoolbook 4x4 -> 8 limbs, then REDC). Addition and
|
||||
// subtraction work directly on the stored values with a single conditional
|
||||
// p-subtract / p-add, which is valid for the Montgomery representation.
|
||||
// --------------------------------------------------------------------------
|
||||
using u64 = std::uint64_t;
|
||||
using u128 = unsigned __int128;
|
||||
using s128 = __int128;
|
||||
|
||||
struct fe {
|
||||
u64 l[4];
|
||||
};
|
||||
|
||||
// Modulus p in normal form (used by REDC and the conditional reductions).
|
||||
constexpr fe k_p = {{0xffffffffffffffffULL, 0x00000000ffffffffULL,
|
||||
0x0000000000000000ULL, 0xffffffff00000001ULL}};
|
||||
|
||||
constexpr fe k_zero = {{0, 0, 0, 0}};
|
||||
|
||||
// Montgomery form of small constants and curve parameters (precomputed).
|
||||
constexpr fe k_one = {{0x0000000000000001ULL, 0xffffffff00000000ULL,
|
||||
0xffffffffffffffffULL, 0x00000000fffffffeULL}};
|
||||
constexpr fe k_two = {{0x0000000000000002ULL, 0xfffffffe00000000ULL,
|
||||
0xffffffffffffffffULL, 0x00000001fffffffdULL}};
|
||||
constexpr fe k_three = {{0x0000000000000003ULL, 0xfffffffd00000000ULL,
|
||||
0xffffffffffffffffULL, 0x00000002fffffffcULL}};
|
||||
constexpr fe k_four = {{0x0000000000000004ULL, 0xfffffffc00000000ULL,
|
||||
0xffffffffffffffffULL, 0x00000003fffffffbULL}};
|
||||
constexpr fe k_eight = {{0x0000000000000008ULL, 0xfffffff800000000ULL,
|
||||
0xffffffffffffffffULL, 0x00000007fffffff7ULL}};
|
||||
// R^2 mod p, for converting normal form into Montgomery form.
|
||||
constexpr fe k_r2 = {{0x0000000000000003ULL, 0xfffffffbffffffffULL,
|
||||
0xfffffffffffffffeULL, 0x00000004fffffffdULL}};
|
||||
constexpr fe k_mgx = {{0x79e730d418a9143cULL, 0x75ba95fc5fedb601ULL,
|
||||
0x79fb732b77622510ULL, 0x18905f76a53755c6ULL}};
|
||||
constexpr fe k_mgy = {{0xddf25357ce95560aULL, 0x8b4ab8e4ba19e45cULL,
|
||||
0xd2e88688dd21f325ULL, 0x8571ff1825885d85ULL}};
|
||||
constexpr fe k_mb = {{0xd89cdf6229c4bddfULL, 0xacf005cd78843090ULL,
|
||||
0xe5a220abf7212ed6ULL, 0xdc30061d04874834ULL}};
|
||||
|
||||
auto fe_is_zero(fe const& a) -> bool
|
||||
{
|
||||
return a.l[0] == 0 && a.l[1] == 0 && a.l[2] == 0 && a.l[3] == 0;
|
||||
}
|
||||
|
||||
auto fe_eq(fe const& a, fe const& b) -> bool
|
||||
{
|
||||
return a.l[0] == b.l[0] && a.l[1] == b.l[1] && a.l[2] == b.l[2] &&
|
||||
a.l[3] == b.l[3];
|
||||
}
|
||||
|
||||
// a >= b, as unsigned integers.
|
||||
auto fe_ge(fe const& a, fe const& b) -> bool
|
||||
{
|
||||
for (int i = 3; i >= 0; --i) {
|
||||
if (a.l[i] > b.l[i])
|
||||
return true;
|
||||
if (a.l[i] < b.l[i])
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// a - b, modulo 2^256 (wraps if a < b).
|
||||
auto fe_sub_raw(fe const& a, fe const& b) -> fe
|
||||
{
|
||||
fe r{};
|
||||
s128 borrow = 0;
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
s128 t = static_cast<s128>(a.l[i]) - static_cast<s128>(b.l[i]) - borrow;
|
||||
if (t < 0) {
|
||||
t += static_cast<s128>(static_cast<u64>(1) << 63) * 2; // + 2^64
|
||||
borrow = 1;
|
||||
} else {
|
||||
borrow = 0;
|
||||
}
|
||||
r.l[i] = static_cast<u64>(t);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
auto fe_add(fe const& a, fe const& b) -> fe
|
||||
{
|
||||
fe r{};
|
||||
u64 carry = 0;
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
u128 t = static_cast<u128>(a.l[i]) + b.l[i] + carry;
|
||||
r.l[i] = static_cast<u64>(t);
|
||||
carry = static_cast<u64>(t >> 64);
|
||||
}
|
||||
if (carry || fe_ge(r, k_p))
|
||||
r = fe_sub_raw(r, k_p);
|
||||
return r;
|
||||
}
|
||||
|
||||
auto fe_sub(fe const& a, fe const& b) -> fe
|
||||
{
|
||||
if (fe_ge(a, b))
|
||||
return fe_sub_raw(a, b);
|
||||
return fe_sub_raw(k_p, fe_sub_raw(b, a));
|
||||
}
|
||||
|
||||
// Montgomery reduction: r = t * 2^-256 mod p, t given as 9 limbs (t[8] is
|
||||
// the possible 257th carry bit, initially zero).
|
||||
auto fe_redc(u64 const t9[9]) -> fe
|
||||
{
|
||||
u64 t[9];
|
||||
std::memcpy(t, t9, sizeof(t));
|
||||
|
||||
// n0 = -p^-1 mod 2^64 = 1 for P-256 (p mod 2^64 == -1).
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
u64 m = t[i];
|
||||
u64 carry = 0;
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
u128 x = static_cast<u128>(m) * k_p.l[j] + t[i + j] + carry;
|
||||
t[i + j] = static_cast<u64>(x);
|
||||
carry = static_cast<u64>(x >> 64);
|
||||
}
|
||||
int k = i + 4;
|
||||
while (carry && k < 9) {
|
||||
u128 x = static_cast<u128>(t[k]) + carry;
|
||||
t[k] = static_cast<u64>(x);
|
||||
carry = static_cast<u64>(x >> 64);
|
||||
++k;
|
||||
}
|
||||
}
|
||||
|
||||
fe r{{t[4], t[5], t[6], t[7]}};
|
||||
if (t[8] != 0 || fe_ge(r, k_p))
|
||||
r = fe_sub_raw(r, k_p);
|
||||
if (fe_ge(r, k_p)) // safety net, unreachable for well-formed inputs
|
||||
r = fe_sub_raw(r, k_p);
|
||||
return r;
|
||||
}
|
||||
|
||||
auto fe_mul(fe const& a, fe const& b) -> fe
|
||||
{
|
||||
u64 t[9] = {0};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
u64 carry = 0;
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
u128 x = static_cast<u128>(a.l[i]) * b.l[j] + t[i + j] + carry;
|
||||
t[i + j] = static_cast<u64>(x);
|
||||
carry = static_cast<u64>(x >> 64);
|
||||
}
|
||||
t[i + 4] += carry;
|
||||
}
|
||||
return fe_redc(t);
|
||||
}
|
||||
|
||||
auto fe_sqr(fe const& a) -> fe
|
||||
{
|
||||
return fe_mul(a, a);
|
||||
}
|
||||
|
||||
auto fe_inv(fe const& a) -> fe
|
||||
{
|
||||
// a^(p-2) mod p via square-and-multiply (result stays in Montgomery form).
|
||||
fe result = k_one;
|
||||
fe base = a;
|
||||
u64 e[4] = {0xfffffffffffffffdULL, 0x00000000ffffffffULL, 0x0ULL,
|
||||
0xffffffff00000001ULL};
|
||||
for (int i = 3; i >= 0; --i) {
|
||||
for (int b = 63; b >= 0; --b) {
|
||||
result = fe_sqr(result);
|
||||
if ((e[i] >> b) & 1)
|
||||
result = fe_mul(result, base);
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
auto load_be64(u8 const* p) -> u64
|
||||
{
|
||||
u64 v = 0;
|
||||
for (int i = 0; i < 8; ++i)
|
||||
v = (v << 8) | p[i];
|
||||
return v;
|
||||
}
|
||||
|
||||
auto store_be64(u8* p, u64 v) -> void
|
||||
{
|
||||
for (int i = 7; i >= 0; --i) {
|
||||
p[i] = static_cast<u8>(v);
|
||||
v >>= 8;
|
||||
}
|
||||
}
|
||||
|
||||
// Normal form (big-endian bytes) -> Montgomery form.
|
||||
auto fe_from_bytes(std::array<u8, 32> const& b) -> fe
|
||||
{
|
||||
fe n{};
|
||||
for (int i = 0; i < 4; ++i)
|
||||
n.l[i] = load_be64(b.data() + (3 - i) * 8);
|
||||
return fe_mul(n, k_r2);
|
||||
}
|
||||
|
||||
// Montgomery form -> normal form (big-endian bytes).
|
||||
auto fe_from_mont(fe const& a) -> fe
|
||||
{
|
||||
u64 t[9] = {a.l[0], a.l[1], a.l[2], a.l[3], 0, 0, 0, 0, 0};
|
||||
return fe_redc(t);
|
||||
}
|
||||
|
||||
auto fe_to_bytes(fe const& a) -> std::array<u8, 32>
|
||||
{
|
||||
fe n = fe_from_mont(a);
|
||||
std::array<u8, 32> out{};
|
||||
for (int i = 0; i < 4; ++i)
|
||||
store_be64(out.data() + (3 - i) * 8, n.l[i]);
|
||||
return out;
|
||||
}
|
||||
|
||||
// --------------------------------------------------------------------------
|
||||
// Jacobian-coordinate point arithmetic on y^2 = x^3 - 3x + b.
|
||||
// --------------------------------------------------------------------------
|
||||
struct jac {
|
||||
fe x, y, z;
|
||||
};
|
||||
|
||||
struct aff {
|
||||
fe x, y;
|
||||
};
|
||||
|
||||
// Point doubling, a = -3 (EFD dbl-2001-b).
|
||||
auto jac_dbl(jac const& p) -> jac
|
||||
{
|
||||
if (fe_is_zero(p.z) || fe_is_zero(p.y))
|
||||
return {k_zero, k_zero, k_zero};
|
||||
|
||||
fe zz = fe_sqr(p.z);
|
||||
fe yy = fe_sqr(p.y);
|
||||
fe beta = fe_mul(p.x, yy);
|
||||
fe alpha = fe_mul(k_three, fe_mul(fe_sub(p.x, zz), fe_add(p.x, zz)));
|
||||
fe x3 = fe_sub(fe_sqr(alpha), fe_mul(k_eight, beta));
|
||||
fe z3 = fe_sub(fe_sub(fe_sqr(fe_add(p.y, p.z)), yy), zz);
|
||||
fe y3 = fe_sub(fe_mul(alpha, fe_sub(fe_mul(k_four, beta), x3)),
|
||||
fe_mul(k_eight, fe_sqr(yy)));
|
||||
return {x3, y3, z3};
|
||||
}
|
||||
|
||||
// Mixed Jacobian + affine addition, a = -3 (EFD madd-2007-bl).
|
||||
auto jac_madd(jac const& p, aff const& q) -> jac
|
||||
{
|
||||
if (fe_is_zero(p.z))
|
||||
return {q.x, q.y, k_one};
|
||||
|
||||
fe zz = fe_sqr(p.z);
|
||||
fe u2 = fe_mul(q.x, zz);
|
||||
fe s2 = fe_mul(q.y, fe_mul(p.z, zz));
|
||||
fe h = fe_sub(u2, p.x);
|
||||
|
||||
if (fe_is_zero(h)) {
|
||||
if (fe_eq(s2, p.y))
|
||||
return jac_dbl(p); // p == q
|
||||
return {k_zero, k_zero, k_zero}; // p == -q
|
||||
}
|
||||
|
||||
fe hh = fe_sqr(h);
|
||||
fe i4 = fe_mul(k_four, hh);
|
||||
fe j = fe_mul(h, i4);
|
||||
fe r = fe_mul(k_two, fe_sub(s2, p.y));
|
||||
fe v = fe_mul(p.x, i4);
|
||||
fe x3 = fe_sub(fe_sub(fe_sqr(r), j), fe_mul(k_two, v));
|
||||
fe y3 = fe_sub(fe_mul(r, fe_sub(v, x3)),
|
||||
fe_mul(fe_mul(k_two, p.y), j));
|
||||
fe z3 = fe_sub(fe_sub(fe_sqr(fe_add(p.z, h)), zz), hh);
|
||||
return {x3, y3, z3};
|
||||
}
|
||||
|
||||
auto jac_to_aff(jac const& p) -> aff
|
||||
{
|
||||
if (fe_is_zero(p.z))
|
||||
return {k_zero, k_zero};
|
||||
fe zinv = fe_inv(p.z);
|
||||
fe zinv2 = fe_sqr(zinv);
|
||||
fe zinv3 = fe_mul(zinv2, zinv);
|
||||
return {fe_mul(p.x, zinv2), fe_mul(p.y, zinv3)};
|
||||
}
|
||||
|
||||
auto jac_scalar_mul(aff const& base, u64 const e[4]) -> jac
|
||||
{
|
||||
jac r{k_zero, k_zero, k_zero};
|
||||
for (int i = 3; i >= 0; --i) {
|
||||
for (int b = 63; b >= 0; --b) {
|
||||
r = jac_dbl(r);
|
||||
if ((e[i] >> b) & 1)
|
||||
r = jac_madd(r, base);
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
auto scalar_to_limbs(ec_scalar const& d, u64 e[4]) -> void
|
||||
{
|
||||
for (int i = 0; i < 4; ++i)
|
||||
e[i] = load_be64(d.data() + (3 - i) * 8);
|
||||
}
|
||||
|
||||
auto ec_point_is_valid(ec_point const& p) -> bool
|
||||
{
|
||||
std::array<u8, 32> px{};
|
||||
std::array<u8, 32> py{};
|
||||
std::memcpy(px.data(), p.data(), 32);
|
||||
std::memcpy(py.data(), p.data() + 32, 32);
|
||||
|
||||
fe x = fe_from_bytes(px);
|
||||
fe y = fe_from_bytes(py);
|
||||
|
||||
// y^2 == x^3 - 3x + b
|
||||
fe lhs = fe_sqr(y);
|
||||
fe rhs = fe_add(fe_sub(fe_mul(fe_sqr(x), x), fe_mul(k_three, x)), k_mb);
|
||||
return fe_eq(lhs, rhs);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ==========================================================================
|
||||
// SHA-256
|
||||
// ==========================================================================
|
||||
sha256::sha256()
|
||||
{
|
||||
CC_SHA256_Init(&ctx_);
|
||||
}
|
||||
|
||||
auto sha256::update(std::span<u8 const> data) -> void
|
||||
{
|
||||
CC_SHA256_Update(&ctx_, data.data(), static_cast<CC_LONG>(data.size()));
|
||||
}
|
||||
|
||||
auto sha256::finalize(std::array<u8, k_sha256_len>& out) -> void
|
||||
{
|
||||
CC_SHA256_Final(out.data(), &ctx_);
|
||||
}
|
||||
|
||||
// ==========================================================================
|
||||
// HMAC-SHA256
|
||||
// ==========================================================================
|
||||
hmac_sha256::hmac_sha256(std::span<u8 const> key)
|
||||
{
|
||||
u8 block[64] = {};
|
||||
u8 key_block[64] = {};
|
||||
|
||||
if (key.size() > 64) {
|
||||
std::array<u8, 32> digest{};
|
||||
sha256 h;
|
||||
h.update(key);
|
||||
h.finalize(digest);
|
||||
std::memcpy(key_block, digest.data(), digest.size());
|
||||
} else {
|
||||
std::memcpy(key_block, key.data(), key.size());
|
||||
}
|
||||
|
||||
for (int i = 0; i < 64; ++i) {
|
||||
block[i] = static_cast<u8>(key_block[i] ^ 0x36);
|
||||
inner_.update({&block[i], 1});
|
||||
}
|
||||
for (int i = 0; i < 64; ++i) {
|
||||
block[i] = static_cast<u8>(key_block[i] ^ 0x5c);
|
||||
outer_.update({&block[i], 1});
|
||||
}
|
||||
}
|
||||
|
||||
auto hmac_sha256::update(std::span<u8 const> data) -> void
|
||||
{
|
||||
inner_.update(data);
|
||||
}
|
||||
|
||||
auto hmac_sha256::finalize(std::array<u8, k_sha256_len>& out) -> void
|
||||
{
|
||||
std::array<u8, k_sha256_len> inner_digest{};
|
||||
inner_.finalize(inner_digest);
|
||||
outer_.update(inner_digest);
|
||||
outer_.finalize(out);
|
||||
}
|
||||
|
||||
// ==========================================================================
|
||||
// TLS P_SHA256 PRF
|
||||
// ==========================================================================
|
||||
auto prf_sha256(std::span<u8 const> key, std::string_view label,
|
||||
std::span<u8 const> seed, std::span<u8> out) -> void
|
||||
{
|
||||
std::span<u8 const> label_bytes{reinterpret_cast<u8 const*>(label.data()),
|
||||
label.size()};
|
||||
|
||||
std::array<u8, k_sha256_len> a{};
|
||||
{
|
||||
hmac_sha256 h{key};
|
||||
h.update(label_bytes);
|
||||
h.update(seed);
|
||||
h.finalize(a);
|
||||
}
|
||||
|
||||
std::size_t off = 0;
|
||||
while (off < out.size()) {
|
||||
std::array<u8, k_sha256_len> block{};
|
||||
{
|
||||
hmac_sha256 h{key};
|
||||
h.update(a);
|
||||
h.update(label_bytes);
|
||||
h.update(seed);
|
||||
h.finalize(block);
|
||||
}
|
||||
|
||||
std::size_t n = std::min<std::size_t>(k_sha256_len, out.size() - off);
|
||||
std::memcpy(out.data() + off, block.data(), n);
|
||||
off += n;
|
||||
|
||||
hmac_sha256 h{key};
|
||||
h.update(a);
|
||||
h.finalize(a);
|
||||
}
|
||||
}
|
||||
|
||||
// ==========================================================================
|
||||
// RSA (PKCS#1 v1.5)
|
||||
// ==========================================================================
|
||||
auto rsa_encrypt_pkcs1(std::span<u8 const> der_key,
|
||||
std::span<u8 const> plaintext, std::span<u8> out)
|
||||
-> bool
|
||||
{
|
||||
CFDataRef key_data = CFDataCreate(
|
||||
kCFAllocatorDefault, der_key.data(), static_cast<CFIndex>(der_key.size()));
|
||||
if (!key_data)
|
||||
return false;
|
||||
|
||||
void const* attr_keys[] = {kSecAttrKeyType, kSecAttrKeyClass};
|
||||
void const* attr_vals[] = {kSecAttrKeyTypeRSA, kSecAttrKeyClassPublic};
|
||||
CFDictionaryRef attrs = CFDictionaryCreate(
|
||||
kCFAllocatorDefault, attr_keys, attr_vals, 2,
|
||||
&kCFTypeDictionaryKeyCallBacks, &kCFTypeDictionaryValueCallBacks);
|
||||
if (!attrs) {
|
||||
CFRelease(key_data);
|
||||
return false;
|
||||
}
|
||||
|
||||
CFErrorRef error = nullptr;
|
||||
SecKeyRef key = SecKeyCreateWithData(key_data, attrs, &error);
|
||||
CFRelease(attrs);
|
||||
CFRelease(key_data);
|
||||
if (!key) {
|
||||
if (error)
|
||||
CFRelease(error);
|
||||
return false;
|
||||
}
|
||||
|
||||
CFDataRef pt = CFDataCreate(
|
||||
kCFAllocatorDefault, plaintext.data(), static_cast<CFIndex>(plaintext.size()));
|
||||
if (!pt) {
|
||||
CFRelease(key);
|
||||
return false;
|
||||
}
|
||||
|
||||
CFDataRef ct = SecKeyCreateEncryptedData(key, kSecKeyAlgorithmRSAEncryptionPKCS1,
|
||||
pt, &error);
|
||||
CFRelease(pt);
|
||||
if (!ct) {
|
||||
CFRelease(key);
|
||||
if (error)
|
||||
CFRelease(error);
|
||||
return false;
|
||||
}
|
||||
|
||||
CFIndex len = CFDataGetLength(ct);
|
||||
if (len > static_cast<CFIndex>(out.size())) {
|
||||
CFRelease(ct);
|
||||
CFRelease(key);
|
||||
return false;
|
||||
}
|
||||
|
||||
CFDataGetBytes(ct, CFRangeMake(0, len), out.data());
|
||||
CFRelease(ct);
|
||||
CFRelease(key);
|
||||
return true;
|
||||
}
|
||||
|
||||
// ==========================================================================
|
||||
// ECDH (P-256)
|
||||
// ==========================================================================
|
||||
auto ec_base_point_multiply(ec_scalar const& d, ec_point& q) -> void
|
||||
{
|
||||
u64 e[4];
|
||||
scalar_to_limbs(d, e);
|
||||
aff g{k_mgx, k_mgy};
|
||||
aff r = jac_to_aff(jac_scalar_mul(g, e));
|
||||
|
||||
std::array<u8, 32> bx = fe_to_bytes(r.x);
|
||||
std::array<u8, 32> by = fe_to_bytes(r.y);
|
||||
std::memcpy(q.data(), bx.data(), 32);
|
||||
std::memcpy(q.data() + 32, by.data(), 32);
|
||||
}
|
||||
|
||||
auto ec_compute_shared(ec_scalar const& d, ec_point const& peer,
|
||||
ec_scalar& shared) -> bool
|
||||
{
|
||||
if (!ec_point_is_valid(peer))
|
||||
return false;
|
||||
|
||||
u64 e[4];
|
||||
scalar_to_limbs(d, e);
|
||||
|
||||
std::array<u8, 32> px{};
|
||||
std::array<u8, 32> py{};
|
||||
std::memcpy(px.data(), peer.data(), 32);
|
||||
std::memcpy(py.data(), peer.data() + 32, 32);
|
||||
|
||||
aff r = jac_to_aff(jac_scalar_mul({fe_from_bytes(px), fe_from_bytes(py)}, e));
|
||||
if (fe_is_zero(r.y))
|
||||
return false;
|
||||
|
||||
shared = fe_to_bytes(r.x);
|
||||
return true;
|
||||
}
|
||||
|
||||
auto ec_generate_keypair(ec_scalar& d, ec_point& q) -> void
|
||||
{
|
||||
do {
|
||||
random_bytes(d);
|
||||
} while (std::all_of(d.begin(), d.end(), [](u8 b) { return b == 0; }));
|
||||
|
||||
ec_base_point_multiply(d, q);
|
||||
}
|
||||
|
||||
// ==========================================================================
|
||||
// Randomness
|
||||
// ==========================================================================
|
||||
auto random_bytes(std::span<u8> out) -> void
|
||||
{
|
||||
arc4random_buf(out.data(), out.size());
|
||||
}
|
||||
|
||||
} // namespace xone::auth
|
||||
|
||||
+936
-2
@@ -1,7 +1,941 @@
|
||||
// GIP protocol (Game Input Protocol)
|
||||
// TODO(phase 1): port from medusalix/xone bus/protocol.c + bus/bus.c.
|
||||
// GIP protocol (port of medusalix/xone bus/protocol.c + bus/bus.c).
|
||||
|
||||
#include "gip/protocol.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
|
||||
#include <errno.h>
|
||||
|
||||
#include "common/log.hpp"
|
||||
#include "common/types.hpp"
|
||||
|
||||
namespace xone::gip {
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr int k_hdr_min_length = 3;
|
||||
constexpr u8 k_vkey_left_win = 0x5b;
|
||||
|
||||
auto encode_varint(u8* buf, u32 val) -> int
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 4; ++i) {
|
||||
buf[i] = static_cast<u8>(val);
|
||||
if (val > 0x7f)
|
||||
buf[i] |= 0x80;
|
||||
val >>= 7;
|
||||
if (!val)
|
||||
break;
|
||||
}
|
||||
return i + 1;
|
||||
}
|
||||
|
||||
auto decode_varint(u8 const* data, int len, u32* val) -> int
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 4 && i < len; ++i) {
|
||||
*val |= static_cast<u32>(data[i] & 0x7f) << (i * 7);
|
||||
if (!(data[i] & 0x80))
|
||||
break;
|
||||
}
|
||||
return i + 1;
|
||||
}
|
||||
|
||||
auto get_actual_header_length(gip_header const& hdr) -> int
|
||||
{
|
||||
u32 pkt_len = hdr.packet_length;
|
||||
u32 chunk_offset = hdr.chunk_offset;
|
||||
int len = k_hdr_min_length;
|
||||
|
||||
do {
|
||||
++len;
|
||||
pkt_len >>= 7;
|
||||
} while (pkt_len);
|
||||
|
||||
if (hdr.options & GIP_OPT_CHUNK) {
|
||||
while (chunk_offset) {
|
||||
++len;
|
||||
chunk_offset >>= 7;
|
||||
}
|
||||
}
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
auto get_header_length(gip_header const& hdr) -> int
|
||||
{
|
||||
int len = get_actual_header_length(hdr);
|
||||
return len + (len % 2); // round up to even
|
||||
}
|
||||
|
||||
auto encode_header(gip_header const& hdr, u8* buf) -> void
|
||||
{
|
||||
int hdr_len = 0;
|
||||
|
||||
buf[hdr_len++] = hdr.command;
|
||||
buf[hdr_len++] = hdr.options;
|
||||
buf[hdr_len++] = hdr.sequence;
|
||||
|
||||
hdr_len += encode_varint(buf + hdr_len, hdr.packet_length);
|
||||
|
||||
// Header length must be even: pad by marking the last byte as a
|
||||
// continuation and appending a zero byte (absorbed by the varint).
|
||||
if (get_actual_header_length(hdr) % 2) {
|
||||
buf[hdr_len - 1] |= 0x80;
|
||||
buf[hdr_len++] = 0;
|
||||
}
|
||||
|
||||
if (hdr.options & GIP_OPT_CHUNK)
|
||||
encode_varint(buf + hdr_len, hdr.chunk_offset);
|
||||
}
|
||||
|
||||
auto decode_header(gip_header& hdr, u8 const* data, int len) -> int
|
||||
{
|
||||
int hdr_len = 0;
|
||||
|
||||
hdr.command = data[hdr_len++];
|
||||
hdr.options = data[hdr_len++];
|
||||
hdr.sequence = data[hdr_len++];
|
||||
hdr.packet_length = 0;
|
||||
hdr.chunk_offset = 0;
|
||||
|
||||
hdr_len += decode_varint(data + hdr_len, len - hdr_len, &hdr.packet_length);
|
||||
|
||||
if (hdr.options & GIP_OPT_CHUNK)
|
||||
hdr_len += decode_varint(data + hdr_len, len - hdr_len, &hdr.chunk_offset);
|
||||
|
||||
return hdr_len;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ==========================================================================
|
||||
// client
|
||||
// ==========================================================================
|
||||
client::client(adapter& adapter, u8 id)
|
||||
: adapter_(adapter)
|
||||
, id_(id)
|
||||
{
|
||||
}
|
||||
|
||||
auto client::has_interface(xone::guid_t const& guid) const -> bool
|
||||
{
|
||||
if (!interfaces_)
|
||||
return false;
|
||||
|
||||
for (int i = 0; i < interfaces_->count; ++i) {
|
||||
if (!std::memcmp(interfaces_->data.data() + i * sizeof(guid), guid.data,
|
||||
sizeof(guid))) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
auto client::set_power_mode(u8 mode) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_POWER;
|
||||
hdr.options = id_ | GIP_OPT_INTERNAL;
|
||||
hdr.packet_length = 1;
|
||||
return adapter_.send_pkt(*this, hdr, &mode);
|
||||
}
|
||||
|
||||
auto client::send_rumble(std::span<u8 const> pkt) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_RUMBLE;
|
||||
hdr.options = id_;
|
||||
hdr.packet_length = static_cast<u32>(pkt.size());
|
||||
return adapter_.send_pkt(*this, hdr, pkt.data());
|
||||
}
|
||||
|
||||
auto client::set_led_mode(u8 mode, u8 brightness) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_LED;
|
||||
hdr.options = id_ | GIP_OPT_INTERNAL;
|
||||
hdr.packet_length = 3;
|
||||
|
||||
u8 pkt[3] = {0, mode, brightness};
|
||||
return adapter_.send_pkt(*this, hdr, pkt);
|
||||
}
|
||||
|
||||
auto client::suggest_audio_format(u8 in, u8 out, bool chat) -> int
|
||||
{
|
||||
int err;
|
||||
if (chat)
|
||||
err = adapter_.set_audio_format_chat(*this, GIP_AUD_FORMAT_CHAT_24KHZ);
|
||||
else
|
||||
err = adapter_.set_audio_format(*this, in, out);
|
||||
|
||||
if (err) {
|
||||
log_msg(log_level::error, "gip: set audio format failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
audio_config_in_.format = in;
|
||||
audio_config_out_.format = out;
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto client::set_audio_volume(u8 in, u8 chat, u8 out) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_AUDIO_CONTROL;
|
||||
hdr.options = id_ | GIP_OPT_INTERNAL;
|
||||
hdr.packet_length = 8;
|
||||
|
||||
u8 pkt[8] = {0x03, 0x04, out, chat, in, 0, 0, 0};
|
||||
return adapter_.send_pkt(*this, hdr, pkt);
|
||||
}
|
||||
|
||||
auto client::send_audio_samples(std::span<u8 const> samples) -> int
|
||||
{
|
||||
// TODO(phase 4): the MT76 transport batches audio packets; send one
|
||||
// frame per packet until the transport exposes an audio buffer API.
|
||||
auto const& cfg = audio_config_out_;
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_AUDIO_SAMPLES;
|
||||
hdr.options = id_ | GIP_OPT_INTERNAL;
|
||||
hdr.packet_length = static_cast<u32>(cfg.fragment_size);
|
||||
|
||||
int hdr_len = get_header_length(hdr);
|
||||
|
||||
for (int i = 0; i < adapter_.audio_packet_count_; ++i) {
|
||||
auto const* src = samples.data() + i * cfg.fragment_size;
|
||||
std::vector<u8> frame(static_cast<std::size_t>(hdr_len) + cfg.fragment_size);
|
||||
|
||||
do {
|
||||
hdr.sequence = adapter_.audio_sequence_++;
|
||||
} while (!hdr.sequence);
|
||||
|
||||
encode_header(hdr, frame.data());
|
||||
std::memcpy(frame.data() + hdr_len, src, cfg.fragment_size);
|
||||
|
||||
int err = adapter_.transport_.send_frame(frame);
|
||||
if (err)
|
||||
return err;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto client::enable_audio() -> int
|
||||
{
|
||||
// TODO(phase 4): audio transport is not wired up yet.
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto client::init_audio_in() -> int
|
||||
{
|
||||
// TODO(phase 4): audio transport is not wired up yet.
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto client::init_audio_out() -> int
|
||||
{
|
||||
// TODO(phase 4): audio transport is not wired up yet.
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto client::disable_audio() -> void
|
||||
{
|
||||
// TODO(phase 4): audio transport is not wired up yet.
|
||||
}
|
||||
|
||||
auto client::send(std::span<u8 const> pkt, bool acknowledge) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_AUTHENTICATE;
|
||||
hdr.options = id_ | GIP_OPT_INTERNAL;
|
||||
hdr.packet_length = static_cast<u32>(pkt.size());
|
||||
|
||||
if (acknowledge)
|
||||
hdr.options |= GIP_OPT_ACKNOWLEDGE;
|
||||
|
||||
return adapter_.send_pkt(*this, hdr, pkt.data());
|
||||
}
|
||||
|
||||
auto client::set_encryption_key(std::span<u8 const> key) -> int
|
||||
{
|
||||
return adapter_.transport_.set_encryption_key(id_, key);
|
||||
}
|
||||
|
||||
// ==========================================================================
|
||||
// adapter
|
||||
// ==========================================================================
|
||||
adapter::adapter(transport& transport, client_listener& listener,
|
||||
int audio_packet_count)
|
||||
: transport_(transport)
|
||||
, listener_(listener)
|
||||
, audio_packet_count_(audio_packet_count)
|
||||
{
|
||||
}
|
||||
|
||||
auto adapter::get_client(u8 id) -> client*
|
||||
{
|
||||
if (id >= k_max_clients)
|
||||
return nullptr;
|
||||
|
||||
auto& slot = clients_[id];
|
||||
if (!slot)
|
||||
slot = std::make_unique<client>(*this, id);
|
||||
|
||||
return slot.get();
|
||||
}
|
||||
|
||||
auto adapter::client_count() const -> int
|
||||
{
|
||||
int n = 0;
|
||||
for (auto const& c : clients_)
|
||||
if (c)
|
||||
++n;
|
||||
return n;
|
||||
}
|
||||
|
||||
auto adapter::process_buffer(std::span<u8 const> data) -> int
|
||||
{
|
||||
while (data.size() > k_hdr_min_length) {
|
||||
gip_header hdr;
|
||||
int hdr_len = decode_header(hdr, data.data(),
|
||||
static_cast<int>(data.size()));
|
||||
if (data.size() < static_cast<std::size_t>(hdr_len) + hdr.packet_length)
|
||||
return -EINVAL;
|
||||
|
||||
client* c = get_client(hdr.options & 0x0f);
|
||||
if (!c)
|
||||
return -ENODEV;
|
||||
|
||||
int err = process_pkt(*c, hdr, data.data() + hdr_len);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
data = data.subspan(static_cast<std::size_t>(hdr_len) + hdr.packet_length);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::send_pkt_simple(gip_header& hdr, void const* data) -> int
|
||||
{
|
||||
int hdr_len = get_header_length(hdr);
|
||||
std::vector<u8> frame(static_cast<std::size_t>(hdr_len) + hdr.packet_length);
|
||||
|
||||
while (!hdr.sequence)
|
||||
hdr.sequence = data_sequence_++;
|
||||
|
||||
encode_header(hdr, frame.data());
|
||||
if (data && hdr.packet_length)
|
||||
std::memcpy(frame.data() + hdr_len, data, hdr.packet_length);
|
||||
|
||||
int err = transport_.send_frame(frame);
|
||||
if (err)
|
||||
log_msg(log_level::debug, "gip: send frame failed: %d", err);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
auto adapter::send_pkt(client& c, gip_header& hdr, void const* data) -> int
|
||||
{
|
||||
if (hdr.packet_length <= k_pkt_max_length)
|
||||
return send_pkt_simple(hdr, data);
|
||||
|
||||
// Split into chunks; the first chunk carries the total length.
|
||||
hdr.options |= GIP_OPT_ACKNOWLEDGE | GIP_OPT_CHUNK_START | GIP_OPT_CHUNK;
|
||||
hdr.chunk_offset = hdr.packet_length;
|
||||
hdr.packet_length = k_pkt_max_length;
|
||||
|
||||
int err = send_pkt_simple(hdr, data);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
err = init_chunk_buffer(hdr, c.chunk_buf_in_);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
std::memcpy(c.chunk_buf_in_->data.data(), data, hdr.chunk_offset);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::init_chunk_buffer(gip_header const& hdr,
|
||||
std::unique_ptr<client::chunk_buffer>& buf) -> int
|
||||
{
|
||||
if (hdr.chunk_offset > k_chunk_buf_max_length)
|
||||
return -EINVAL;
|
||||
|
||||
buf = std::make_unique<client::chunk_buffer>();
|
||||
buf->header = hdr;
|
||||
buf->header.options &= ~(GIP_OPT_ACKNOWLEDGE | GIP_OPT_CHUNK_START);
|
||||
buf->length = hdr.chunk_offset;
|
||||
buf->data.resize(hdr.chunk_offset);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::send_remaining_chunks(client& c) -> int
|
||||
{
|
||||
auto& buf = c.chunk_buf_in_;
|
||||
gip_header hdr = buf->header;
|
||||
u32 len = buf->length - k_pkt_max_length;
|
||||
|
||||
while (len) {
|
||||
if (len <= k_pkt_max_length)
|
||||
hdr.options |= GIP_OPT_ACKNOWLEDGE;
|
||||
|
||||
hdr.packet_length = std::min(len, k_pkt_max_length);
|
||||
hdr.chunk_offset = buf->length - len;
|
||||
|
||||
int err = send_pkt_simple(hdr, buf->data.data() + hdr.chunk_offset);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
len -= hdr.packet_length;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::request_identification(client& c) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_IDENTIFY;
|
||||
hdr.options = c.id_ | GIP_OPT_INTERNAL;
|
||||
return send_pkt(c, hdr, nullptr);
|
||||
}
|
||||
|
||||
auto adapter::acknowledge_pkt(client& c, gip_header const& ack) -> int
|
||||
{
|
||||
auto& buf = c.chunk_buf_out_;
|
||||
gip_header hdr{};
|
||||
u8 pkt[9] = {};
|
||||
u32 len = ack.chunk_offset + ack.packet_length;
|
||||
|
||||
hdr.command = GIP_CMD_ACKNOWLEDGE;
|
||||
hdr.options = c.id_ | GIP_OPT_INTERNAL;
|
||||
hdr.sequence = ack.sequence;
|
||||
hdr.packet_length = sizeof(pkt);
|
||||
|
||||
pkt[1] = ack.command;
|
||||
pkt[2] = c.id_ | GIP_OPT_INTERNAL;
|
||||
store_le16(pkt + 3, static_cast<u16>(len));
|
||||
|
||||
if ((ack.options & GIP_OPT_CHUNK) && buf)
|
||||
store_le16(pkt + 7, static_cast<u16>(buf->length - len));
|
||||
|
||||
return send_pkt(c, hdr, pkt);
|
||||
}
|
||||
|
||||
auto adapter::remove_client(client& c) -> void
|
||||
{
|
||||
listener_.on_client_removed(c.id_);
|
||||
clients_[c.id_].reset();
|
||||
}
|
||||
|
||||
auto adapter::free_client_info(client& c) -> void
|
||||
{
|
||||
c.client_commands_.reset();
|
||||
c.firmware_versions_.reset();
|
||||
c.audio_formats_.reset();
|
||||
c.capabilities_out_.reset();
|
||||
c.capabilities_in_.reset();
|
||||
c.classes_.clear();
|
||||
c.interfaces_.reset();
|
||||
c.hid_descriptor_.reset();
|
||||
}
|
||||
|
||||
auto adapter::add_client(client& c) -> void
|
||||
{
|
||||
log_msg(log_level::info, "gip: client %u identified (vendor 0x%04x, "
|
||||
"product 0x%04x)",
|
||||
c.id_, c.hardware_.vendor, c.hardware_.product);
|
||||
listener_.on_client_added(c);
|
||||
c.start_auth();
|
||||
}
|
||||
|
||||
auto adapter::make_audio_config(gip_audio_config& cfg) -> int
|
||||
{
|
||||
switch (cfg.format) {
|
||||
case GIP_AUD_FORMAT_16KHZ_MONO:
|
||||
cfg.channels = 1;
|
||||
cfg.sample_rate = 16000;
|
||||
break;
|
||||
case GIP_AUD_FORMAT_24KHZ_MONO:
|
||||
cfg.channels = 1;
|
||||
cfg.sample_rate = 24000;
|
||||
break;
|
||||
case GIP_AUD_FORMAT_48KHZ_STEREO:
|
||||
cfg.channels = 2;
|
||||
cfg.sample_rate = 48000;
|
||||
break;
|
||||
default:
|
||||
log_msg(log_level::error, "gip: unknown audio format: 0x%02x", cfg.format);
|
||||
return -EOPNOTSUPP;
|
||||
}
|
||||
|
||||
cfg.buffer_size = cfg.sample_rate * cfg.channels * 2 * k_audio_interval / 1000;
|
||||
cfg.fragment_size = cfg.buffer_size / audio_packet_count_;
|
||||
|
||||
gip_header hdr{};
|
||||
hdr.packet_length = static_cast<u32>(cfg.fragment_size);
|
||||
cfg.packet_size = get_header_length(hdr) + cfg.fragment_size;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::set_audio_format(client& c, u8 in, u8 out) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_AUDIO_CONTROL;
|
||||
hdr.options = c.id_ | GIP_OPT_INTERNAL;
|
||||
hdr.packet_length = 3;
|
||||
|
||||
u8 pkt[3] = {0x02, in, out};
|
||||
return send_pkt(c, hdr, pkt);
|
||||
}
|
||||
|
||||
auto adapter::set_audio_format_chat(client& c, u8 in_out) -> int
|
||||
{
|
||||
gip_header hdr{};
|
||||
hdr.command = GIP_CMD_AUDIO_CONTROL;
|
||||
hdr.options = c.id_ | GIP_OPT_INTERNAL;
|
||||
hdr.packet_length = 2;
|
||||
|
||||
u8 pkt[2] = {0x01, in_out};
|
||||
return send_pkt(c, hdr, pkt);
|
||||
}
|
||||
|
||||
auto adapter::process_pkt(client& c, gip_header& hdr, void const* data) -> int
|
||||
{
|
||||
if (hdr.options & GIP_OPT_CHUNK_START) {
|
||||
int err = init_chunk_buffer(hdr, c.chunk_buf_out_);
|
||||
if (err)
|
||||
return err;
|
||||
hdr.chunk_offset = 0;
|
||||
}
|
||||
|
||||
if (hdr.options & GIP_OPT_ACKNOWLEDGE) {
|
||||
int err = acknowledge_pkt(c, hdr);
|
||||
if (err)
|
||||
return err;
|
||||
}
|
||||
|
||||
if (hdr.options & GIP_OPT_CHUNK)
|
||||
return process_pkt_chunked(c, hdr, data);
|
||||
|
||||
return dispatch_pkt(c, hdr, data, hdr.packet_length);
|
||||
}
|
||||
|
||||
auto adapter::process_pkt_chunked(client& c, gip_header const& hdr,
|
||||
void const* data) -> int
|
||||
{
|
||||
auto& buf = c.chunk_buf_out_;
|
||||
|
||||
if (!buf) {
|
||||
// Older gamepads occasionally send spurious completions.
|
||||
if (!hdr.packet_length)
|
||||
return 0;
|
||||
|
||||
log_msg(log_level::error, "gip: chunk buffer not allocated");
|
||||
return -EPROTO;
|
||||
}
|
||||
|
||||
if (hdr.command != buf->header.command)
|
||||
return -EALREADY;
|
||||
|
||||
if (buf->length < hdr.chunk_offset + hdr.packet_length)
|
||||
return -EINVAL;
|
||||
|
||||
if (hdr.packet_length) {
|
||||
std::memcpy(buf->data.data() + hdr.chunk_offset, data, hdr.packet_length);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Empty chunk signals the completion of the transfer.
|
||||
int err = dispatch_pkt(c, hdr, buf->data.data(), buf->length);
|
||||
buf.reset();
|
||||
return err;
|
||||
}
|
||||
|
||||
auto adapter::dispatch_pkt(client& c, gip_header const& hdr, void const* data,
|
||||
u32 len) -> int
|
||||
{
|
||||
if (hdr.options & GIP_OPT_INTERNAL) {
|
||||
switch (hdr.command) {
|
||||
case GIP_CMD_ACKNOWLEDGE:
|
||||
return handle_pkt_acknowledge(c, data, len);
|
||||
case GIP_CMD_ANNOUNCE:
|
||||
return handle_pkt_announce(c, data, len);
|
||||
case GIP_CMD_STATUS:
|
||||
return handle_pkt_status(c, data, len);
|
||||
case GIP_CMD_IDENTIFY:
|
||||
return handle_pkt_identify(c, data, len);
|
||||
case GIP_CMD_AUTHENTICATE:
|
||||
return handle_pkt_authenticate(c, data, len);
|
||||
case GIP_CMD_VIRTUAL_KEY:
|
||||
return handle_pkt_virtual_key(c, data, len);
|
||||
case GIP_CMD_AUDIO_CONTROL:
|
||||
return handle_pkt_audio_control(c, data, len);
|
||||
case GIP_CMD_HID_REPORT:
|
||||
return handle_pkt_hid_report(c, data, len);
|
||||
case GIP_CMD_AUDIO_SAMPLES:
|
||||
return handle_pkt_audio_samples(c, data, len);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (hdr.command == GIP_CMD_INPUT)
|
||||
return handle_pkt_input(c, data, len);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_acknowledge(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
if (len != 9)
|
||||
return -EINVAL;
|
||||
|
||||
auto const* p = static_cast<u8 const*>(data);
|
||||
auto& buf = c.chunk_buf_in_;
|
||||
|
||||
if (!buf)
|
||||
return 0;
|
||||
|
||||
if (p[1] != buf->header.command)
|
||||
return 0;
|
||||
|
||||
if (load_le16(p + 3) < buf->length)
|
||||
return send_remaining_chunks(c);
|
||||
|
||||
// Empty chunk signals the completion of the transfer.
|
||||
gip_header hdr = buf->header;
|
||||
hdr.packet_length = 0;
|
||||
hdr.chunk_offset = buf->length;
|
||||
buf.reset();
|
||||
|
||||
return send_pkt_simple(hdr, nullptr);
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_announce(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
if (len != 28)
|
||||
return -EINVAL;
|
||||
|
||||
auto const* p = static_cast<u8 const*>(data);
|
||||
u16 vendor = load_le16(p + 8);
|
||||
u16 product = load_le16(p + 10);
|
||||
u16 fw_major = load_le16(p + 12);
|
||||
u16 fw_minor = load_le16(p + 14);
|
||||
|
||||
if (!c.hardware_.vendor && !c.hardware_.product && !c.hardware_.version) {
|
||||
c.hardware_.vendor = vendor;
|
||||
c.hardware_.product = product;
|
||||
c.hardware_.version = static_cast<u16>((fw_major << 8) | fw_minor);
|
||||
}
|
||||
|
||||
return request_identification(c);
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_status(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
if (len < 4)
|
||||
return -EINVAL;
|
||||
|
||||
auto const* p = static_cast<u8 const*>(data);
|
||||
u8 status = p[0];
|
||||
|
||||
if (!(status & 0x80)) {
|
||||
log_msg(log_level::debug, "gip: client %u disconnected", c.id_);
|
||||
remove_client(c);
|
||||
return 0;
|
||||
}
|
||||
|
||||
listener_.on_battery(c, (status >> 2) & 0x3, status & 0x3);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_identify(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
if (len < 32)
|
||||
return -EINVAL;
|
||||
|
||||
if (!c.classes_.empty()) {
|
||||
log_msg(log_level::warn, "gip: client already identified");
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto const* base = static_cast<u8 const*>(data);
|
||||
u16 offsets[8];
|
||||
for (int i = 0; i < 8; ++i)
|
||||
offsets[i] = load_le16(base + 16 + 2 * i);
|
||||
|
||||
std::span<u8 const> info{base + 16, len - 16};
|
||||
|
||||
int err = parse_client_commands(c, offsets, info);
|
||||
if (err)
|
||||
goto err_free_info;
|
||||
err = parse_firmware_versions(c, offsets, info);
|
||||
if (err)
|
||||
goto err_free_info;
|
||||
err = parse_audio_formats(c, offsets, info);
|
||||
if (err)
|
||||
goto err_free_info;
|
||||
err = parse_capabilities(c, offsets, info);
|
||||
if (err)
|
||||
goto err_free_info;
|
||||
err = parse_classes(c, info, offsets[5]);
|
||||
if (err)
|
||||
goto err_free_info;
|
||||
err = parse_interfaces(c, offsets, info);
|
||||
if (err)
|
||||
goto err_free_info;
|
||||
err = parse_hid_descriptor(c, offsets, info);
|
||||
if (err)
|
||||
goto err_free_info;
|
||||
|
||||
add_client(c);
|
||||
return 0;
|
||||
|
||||
err_free_info:
|
||||
free_client_info(c);
|
||||
return err;
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_authenticate(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
return c.process_auth({static_cast<u8 const*>(data), len});
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_virtual_key(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
if (len != 2)
|
||||
return -EINVAL;
|
||||
|
||||
auto const* p = static_cast<u8 const*>(data);
|
||||
if (p[1] != k_vkey_left_win)
|
||||
return -EINVAL;
|
||||
|
||||
listener_.on_guide_button(c, p[0] != 0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_audio_control(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
if (len < 1)
|
||||
return -EINVAL;
|
||||
|
||||
auto const* p = static_cast<u8 const*>(data);
|
||||
switch (p[0]) {
|
||||
case 0x00: // volume chat
|
||||
if (len != 5)
|
||||
return -EINVAL;
|
||||
listener_.on_audio_volume(c, p[4], p[3]);
|
||||
return 0;
|
||||
case 0x01: // format chat
|
||||
if (len != 2)
|
||||
return -EINVAL;
|
||||
if (p[1] != GIP_AUD_FORMAT_CHAT_24KHZ || c.audio_config_in_.buffer_size ||
|
||||
c.audio_config_out_.buffer_size) {
|
||||
return -EPROTO;
|
||||
}
|
||||
if (make_audio_config(c.audio_config_in_))
|
||||
return -EINVAL;
|
||||
if (make_audio_config(c.audio_config_out_))
|
||||
return -EINVAL;
|
||||
listener_.on_audio_ready(c);
|
||||
return 0;
|
||||
case 0x02: // format
|
||||
if (len != 3)
|
||||
return -EINVAL;
|
||||
if (c.audio_config_in_.buffer_size || c.audio_config_out_.buffer_size)
|
||||
return -EPROTO;
|
||||
if (p[1] != c.audio_config_in_.format || p[2] != c.audio_config_out_.format) {
|
||||
log_msg(log_level::warn, "gip: audio format rejected: 0x%02x/0x%02x",
|
||||
c.audio_config_in_.format, c.audio_config_out_.format);
|
||||
return c.suggest_audio_format(p[1], p[2], false);
|
||||
}
|
||||
if (make_audio_config(c.audio_config_in_))
|
||||
return -EINVAL;
|
||||
if (make_audio_config(c.audio_config_out_))
|
||||
return -EINVAL;
|
||||
listener_.on_audio_ready(c);
|
||||
return 0;
|
||||
case 0x03: // volume
|
||||
if (len != 8)
|
||||
return -EINVAL;
|
||||
listener_.on_audio_volume(c, p[4], p[2]);
|
||||
return 0;
|
||||
default:
|
||||
log_msg(log_level::error, "gip: unknown audio subcommand: 0x%02x", p[0]);
|
||||
return -EPROTO;
|
||||
}
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_hid_report(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
listener_.on_hid_report(c, {static_cast<u8 const*>(data), len});
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_input(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
listener_.on_input(c, {static_cast<u8 const*>(data), len});
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::handle_pkt_audio_samples(client& c, void const* data, u32 len) -> int
|
||||
{
|
||||
if (len < 2)
|
||||
return -EINVAL;
|
||||
|
||||
auto const* p = static_cast<u8 const*>(data);
|
||||
listener_.on_audio_samples(c, {p + 2, len - 2});
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_info_element(std::span<u8 const> data, u16 offset,
|
||||
int item_length,
|
||||
std::unique_ptr<client::info_element>& out) -> int
|
||||
{
|
||||
if (!offset)
|
||||
return -EOPNOTSUPP;
|
||||
if (data.size() < static_cast<std::size_t>(offset) + 1)
|
||||
return -EINVAL;
|
||||
|
||||
u8 count = data[offset];
|
||||
++offset;
|
||||
if (!count)
|
||||
return -EOPNOTSUPP;
|
||||
|
||||
int total = count * item_length;
|
||||
if (data.size() < static_cast<std::size_t>(offset) + total)
|
||||
return -EINVAL;
|
||||
|
||||
auto elem = std::make_unique<client::info_element>();
|
||||
elem->count = count;
|
||||
elem->data.assign(data.begin() + offset, data.begin() + offset + total);
|
||||
out = std::move(elem);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_client_commands(client& c, u16 const offsets[8],
|
||||
std::span<u8 const> data) -> int
|
||||
{
|
||||
std::unique_ptr<client::info_element> cmds;
|
||||
int err = parse_info_element(data, offsets[0], 23, cmds);
|
||||
if (err) {
|
||||
if (err == -EOPNOTSUPP)
|
||||
return 0;
|
||||
log_msg(log_level::error, "gip: parse client commands failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
c.client_commands_ = std::move(cmds);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_firmware_versions(client& c, u16 const offsets[8],
|
||||
std::span<u8 const> data) -> int
|
||||
{
|
||||
std::unique_ptr<client::info_element> vers;
|
||||
int err = parse_info_element(data, offsets[1], 4, vers);
|
||||
if (err) {
|
||||
log_msg(log_level::error, "gip: parse firmware versions failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
c.firmware_versions_ = std::move(vers);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_audio_formats(client& c, u16 const offsets[8],
|
||||
std::span<u8 const> data) -> int
|
||||
{
|
||||
std::unique_ptr<client::info_element> fmts;
|
||||
int err = parse_info_element(data, offsets[2], 2, fmts);
|
||||
if (err) {
|
||||
if (err == -EOPNOTSUPP)
|
||||
return 0;
|
||||
log_msg(log_level::error, "gip: parse audio formats failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
c.audio_formats_ = std::move(fmts);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_capabilities(client& c, u16 const offsets[8],
|
||||
std::span<u8 const> data) -> int
|
||||
{
|
||||
std::unique_ptr<client::info_element> caps;
|
||||
int err = parse_info_element(data, offsets[3], 1, caps);
|
||||
if (err) {
|
||||
log_msg(log_level::error, "gip: parse capabilities out failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
c.capabilities_out_ = std::move(caps);
|
||||
|
||||
err = parse_info_element(data, offsets[4], 1, caps);
|
||||
if (err) {
|
||||
log_msg(log_level::error, "gip: parse capabilities in failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
c.capabilities_in_ = std::move(caps);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_classes(client& c, std::span<u8 const> data, u16 offset) -> int
|
||||
{
|
||||
if (data.size() < static_cast<std::size_t>(offset) + 1)
|
||||
return -EINVAL;
|
||||
|
||||
u8 count = data[offset];
|
||||
++offset;
|
||||
if (!count)
|
||||
return -EINVAL;
|
||||
|
||||
while (static_cast<int>(c.classes_.size()) < count) {
|
||||
if (data.size() < static_cast<std::size_t>(offset) + 2)
|
||||
return -EINVAL;
|
||||
|
||||
u16 str_len = load_le16(data.data() + offset);
|
||||
offset += 2;
|
||||
if (!str_len || data.size() < static_cast<std::size_t>(offset) + str_len)
|
||||
return -EINVAL;
|
||||
|
||||
c.classes_.emplace_back(
|
||||
reinterpret_cast<char const*>(data.data() + offset), str_len);
|
||||
offset += str_len;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_interfaces(client& c, u16 const offsets[8],
|
||||
std::span<u8 const> data) -> int
|
||||
{
|
||||
std::unique_ptr<client::info_element> intfs;
|
||||
int err = parse_info_element(data, offsets[6], sizeof(xone::guid_t), intfs);
|
||||
if (err) {
|
||||
log_msg(log_level::error, "gip: parse interfaces failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
c.interfaces_ = std::move(intfs);
|
||||
return 0;
|
||||
}
|
||||
|
||||
auto adapter::parse_hid_descriptor(client& c, u16 const offsets[8],
|
||||
std::span<u8 const> data) -> int
|
||||
{
|
||||
std::unique_ptr<client::info_element> desc;
|
||||
int err = parse_info_element(data, offsets[7], 1, desc);
|
||||
if (err) {
|
||||
if (err == -EOPNOTSUPP)
|
||||
return 0;
|
||||
log_msg(log_level::error, "gip: parse hid descriptor failed: %d", err);
|
||||
return err;
|
||||
}
|
||||
c.hid_descriptor_ = std::move(desc);
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // namespace xone::gip
|
||||
|
||||
Reference in New Issue
Block a user