mirror of
https://github.com/yuzu-emu/yuzu.git
synced 2024-08-22 14:03:23 +02:00
Merge pull request #2778 from Subv/uds_more
Services/UDS: Stub SendTo to generate the unencrypted data frames with the right headers
This commit is contained in:
commit
fa53ccc74b
@ -144,6 +144,7 @@ set(SRCS
|
||||
hle/service/nwm/nwm_tst.cpp
|
||||
hle/service/nwm/nwm_uds.cpp
|
||||
hle/service/nwm/uds_beacon.cpp
|
||||
hle/service/nwm/uds_data.cpp
|
||||
hle/service/pm_app.cpp
|
||||
hle/service/ptm/ptm.cpp
|
||||
hle/service/ptm/ptm_gets.cpp
|
||||
@ -341,6 +342,7 @@ set(HEADERS
|
||||
hle/service/nwm/nwm_tst.h
|
||||
hle/service/nwm/nwm_uds.h
|
||||
hle/service/nwm/uds_beacon.h
|
||||
hle/service/nwm/uds_data.h
|
||||
hle/service/pm_app.h
|
||||
hle/service/ptm/ptm.h
|
||||
hle/service/ptm/ptm_gets.h
|
||||
|
@ -15,6 +15,7 @@
|
||||
#include "core/hle/result.h"
|
||||
#include "core/hle/service/nwm/nwm_uds.h"
|
||||
#include "core/hle/service/nwm/uds_beacon.h"
|
||||
#include "core/hle/service/nwm/uds_data.h"
|
||||
#include "core/memory.h"
|
||||
|
||||
namespace Service {
|
||||
@ -372,6 +373,80 @@ static void DestroyNetwork(Interface* self) {
|
||||
LOG_WARNING(Service_NWM, "called");
|
||||
}
|
||||
|
||||
/**
|
||||
* NWM_UDS::SendTo service function.
|
||||
* Sends a data frame to the UDS network we're connected to.
|
||||
* Inputs:
|
||||
* 0 : Command header.
|
||||
* 1 : Unknown.
|
||||
* 2 : u16 Destination network node id.
|
||||
* 3 : u8 Data channel.
|
||||
* 4 : Buffer size >> 2
|
||||
* 5 : Data size
|
||||
* 6 : Flags
|
||||
* 7 : Input buffer descriptor
|
||||
* 8 : Input buffer address
|
||||
* Outputs:
|
||||
* 0 : Return header
|
||||
* 1 : Result of function, 0 on success, otherwise error code
|
||||
*/
|
||||
static void SendTo(Interface* self) {
|
||||
IPC::RequestParser rp(Kernel::GetCommandBuffer(), 0x17, 6, 2);
|
||||
|
||||
rp.Skip(1, false);
|
||||
u16 dest_node_id = rp.Pop<u16>();
|
||||
u8 data_channel = rp.Pop<u8>();
|
||||
rp.Skip(1, false);
|
||||
u32 data_size = rp.Pop<u32>();
|
||||
u32 flags = rp.Pop<u32>();
|
||||
|
||||
size_t desc_size;
|
||||
const VAddr input_address = rp.PopStaticBuffer(&desc_size, false);
|
||||
ASSERT(desc_size == data_size);
|
||||
|
||||
IPC::RequestBuilder rb = rp.MakeBuilder(1, 0);
|
||||
|
||||
if (connection_status.status != static_cast<u32>(NetworkStatus::ConnectedAsClient) &&
|
||||
connection_status.status != static_cast<u32>(NetworkStatus::ConnectedAsHost)) {
|
||||
rb.Push(ResultCode(ErrorDescription::NotAuthorized, ErrorModule::UDS,
|
||||
ErrorSummary::InvalidState, ErrorLevel::Status));
|
||||
return;
|
||||
}
|
||||
|
||||
if (dest_node_id == connection_status.network_node_id) {
|
||||
rb.Push(ResultCode(ErrorDescription::NotFound, ErrorModule::UDS,
|
||||
ErrorSummary::WrongArgument, ErrorLevel::Status));
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO(Subv): Do something with the flags.
|
||||
|
||||
constexpr size_t MaxSize = 0x5C6;
|
||||
if (data_size > MaxSize) {
|
||||
rb.Push(ResultCode(ErrorDescription::TooLarge, ErrorModule::UDS,
|
||||
ErrorSummary::WrongArgument, ErrorLevel::Usage));
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<u8> data(data_size);
|
||||
Memory::ReadBlock(input_address, data.data(), data.size());
|
||||
|
||||
// TODO(Subv): Increment the sequence number after each sent packet.
|
||||
u16 sequence_number = 0;
|
||||
std::vector<u8> data_payload = GenerateDataPayload(
|
||||
data, data_channel, dest_node_id, connection_status.network_node_id, sequence_number);
|
||||
|
||||
// TODO(Subv): Retrieve the MAC address of the dest_node_id and our own to encrypt
|
||||
// and encapsulate the payload.
|
||||
|
||||
// TODO(Subv): Send the frame.
|
||||
|
||||
rb.Push(RESULT_SUCCESS);
|
||||
|
||||
LOG_WARNING(Service_NWM, "(STUB) called dest_node_id=%u size=%u flags=%u channel=%u",
|
||||
static_cast<u32>(dest_node_id), data_size, flags, static_cast<u32>(data_channel));
|
||||
}
|
||||
|
||||
/**
|
||||
* NWM_UDS::GetChannel service function.
|
||||
* Returns the WiFi channel in which the network we're connected to is transmitting.
|
||||
@ -600,7 +675,7 @@ const Interface::FunctionInfo FunctionTable[] = {
|
||||
{0x00130040, nullptr, "Unbind"},
|
||||
{0x001400C0, nullptr, "PullPacket"},
|
||||
{0x00150080, nullptr, "SetMaxSendDelay"},
|
||||
{0x00170182, nullptr, "SendTo"},
|
||||
{0x00170182, SendTo, "SendTo"},
|
||||
{0x001A0000, GetChannel, "GetChannel"},
|
||||
{0x001B0302, InitializeWithVersion, "InitializeWithVersion"},
|
||||
{0x001D0044, BeginHostingNetwork, "BeginHostingNetwork"},
|
||||
|
278
src/core/hle/service/nwm/uds_data.cpp
Normal file
278
src/core/hle/service/nwm/uds_data.cpp
Normal file
@ -0,0 +1,278 @@
|
||||
// Copyright 2017 Citra Emulator Project
|
||||
// Licensed under GPLv2 or any later version
|
||||
// Refer to the license.txt file included.
|
||||
|
||||
#include <cstring>
|
||||
#include <cryptopp/aes.h>
|
||||
#include <cryptopp/ccm.h>
|
||||
#include <cryptopp/filters.h>
|
||||
#include <cryptopp/md5.h>
|
||||
#include <cryptopp/modes.h>
|
||||
#include "core/hle/service/nwm/nwm_uds.h"
|
||||
#include "core/hle/service/nwm/uds_data.h"
|
||||
#include "core/hw/aes/key.h"
|
||||
|
||||
namespace Service {
|
||||
namespace NWM {
|
||||
|
||||
using MacAddress = std::array<u8, 6>;
|
||||
|
||||
/*
|
||||
* Generates a SNAP-enabled 802.2 LLC header for the specified protocol.
|
||||
* @returns a buffer with the bytes of the generated header.
|
||||
*/
|
||||
static std::vector<u8> GenerateLLCHeader(EtherType protocol) {
|
||||
LLCHeader header{};
|
||||
header.protocol = static_cast<u16>(protocol);
|
||||
|
||||
std::vector<u8> buffer(sizeof(header));
|
||||
memcpy(buffer.data(), &header, sizeof(header));
|
||||
|
||||
return buffer;
|
||||
}
|
||||
|
||||
/*
|
||||
* Generates a Nintendo UDS SecureData header with the specified parameters.
|
||||
* @returns a buffer with the bytes of the generated header.
|
||||
*/
|
||||
static std::vector<u8> GenerateSecureDataHeader(u16 data_size, u8 channel, u16 dest_node_id,
|
||||
u16 src_node_id, u16 sequence_number) {
|
||||
SecureDataHeader header{};
|
||||
header.protocol_size = data_size + sizeof(SecureDataHeader);
|
||||
// Note: This size includes everything except the first 4 bytes of the structure,
|
||||
// reinforcing the hypotheses that the first 4 bytes are actually the header of
|
||||
// another container protocol.
|
||||
header.securedata_size = data_size + sizeof(SecureDataHeader) - 4;
|
||||
// Frames sent by the emulated application are never UDS management frames
|
||||
header.is_management = 0;
|
||||
header.data_channel = channel;
|
||||
header.sequence_number = sequence_number;
|
||||
header.dest_node_id = dest_node_id;
|
||||
header.src_node_id = src_node_id;
|
||||
|
||||
std::vector<u8> buffer(sizeof(header));
|
||||
memcpy(buffer.data(), &header, sizeof(header));
|
||||
|
||||
return buffer;
|
||||
}
|
||||
|
||||
/*
|
||||
* Calculates the CTR used for the AES-CTR process that calculates
|
||||
* the CCMP crypto key for data frames.
|
||||
* @returns The CTR used for data frames crypto key generation.
|
||||
*/
|
||||
static std::array<u8, CryptoPP::MD5::DIGESTSIZE> GetDataCryptoCTR(const NetworkInfo& network_info) {
|
||||
DataFrameCryptoCTR data{};
|
||||
|
||||
data.host_mac = network_info.host_mac_address;
|
||||
data.wlan_comm_id = network_info.wlan_comm_id;
|
||||
data.id = network_info.id;
|
||||
data.network_id = network_info.network_id;
|
||||
|
||||
std::array<u8, CryptoPP::MD5::DIGESTSIZE> hash;
|
||||
CryptoPP::MD5().CalculateDigest(hash.data(), reinterpret_cast<u8*>(&data), sizeof(data));
|
||||
|
||||
return hash;
|
||||
}
|
||||
|
||||
/*
|
||||
* Generates the key used for encrypting the 802.11 data frames generated by UDS.
|
||||
* @returns The key used for data frames crypto.
|
||||
*/
|
||||
static std::array<u8, CryptoPP::AES::BLOCKSIZE> GenerateDataCCMPKey(
|
||||
const std::vector<u8>& passphrase, const NetworkInfo& network_info) {
|
||||
// Calculate the MD5 hash of the input passphrase.
|
||||
std::array<u8, CryptoPP::MD5::DIGESTSIZE> passphrase_hash;
|
||||
CryptoPP::MD5().CalculateDigest(passphrase_hash.data(), passphrase.data(), passphrase.size());
|
||||
|
||||
std::array<u8, CryptoPP::AES::BLOCKSIZE> ccmp_key;
|
||||
|
||||
// The CCMP key is the result of encrypting the MD5 hash of the passphrase with AES-CTR using
|
||||
// keyslot 0x2D.
|
||||
using CryptoPP::AES;
|
||||
std::array<u8, CryptoPP::MD5::DIGESTSIZE> counter = GetDataCryptoCTR(network_info);
|
||||
std::array<u8, AES::BLOCKSIZE> key = HW::AES::GetNormalKey(HW::AES::KeySlotID::UDSDataKey);
|
||||
CryptoPP::CTR_Mode<AES>::Encryption aes;
|
||||
aes.SetKeyWithIV(key.data(), AES::BLOCKSIZE, counter.data());
|
||||
aes.ProcessData(ccmp_key.data(), passphrase_hash.data(), passphrase_hash.size());
|
||||
|
||||
return ccmp_key;
|
||||
}
|
||||
|
||||
/*
|
||||
* Generates the Additional Authenticated Data (AAD) for an UDS 802.11 encrypted data frame.
|
||||
* @returns a buffer with the bytes of the AAD.
|
||||
*/
|
||||
static std::vector<u8> GenerateCCMPAAD(const MacAddress& sender, const MacAddress& receiver,
|
||||
const MacAddress& bssid, u16 frame_control) {
|
||||
// Reference: IEEE 802.11-2007
|
||||
|
||||
// 8.3.3.3.2 Construct AAD (22-30 bytes)
|
||||
// The AAD is constructed from the MPDU header. The AAD does not include the header Duration
|
||||
// field, because the Duration field value can change due to normal IEEE 802.11 operation (e.g.,
|
||||
// a rate change during retransmission). For similar reasons, several subfields in the Frame
|
||||
// Control field are masked to 0.
|
||||
struct {
|
||||
u16_be FC; // MPDU Frame Control field
|
||||
MacAddress A1;
|
||||
MacAddress A2;
|
||||
MacAddress A3;
|
||||
u16_be SC; // MPDU Sequence Control field
|
||||
} aad_struct{};
|
||||
|
||||
constexpr u16 AADFrameControlMask = 0x8FC7;
|
||||
aad_struct.FC = frame_control & AADFrameControlMask;
|
||||
aad_struct.SC = 0;
|
||||
|
||||
bool to_ds = (frame_control & (1 << 0)) != 0;
|
||||
bool from_ds = (frame_control & (1 << 1)) != 0;
|
||||
// In the 802.11 standard, ToDS = 1 and FromDS = 1 is a valid configuration,
|
||||
// however, the 3DS doesn't seem to transmit frames with such combination.
|
||||
ASSERT_MSG(to_ds != from_ds, "Invalid combination");
|
||||
|
||||
// The meaning of the address fields depends on the ToDS and FromDS fields.
|
||||
if (from_ds) {
|
||||
aad_struct.A1 = receiver;
|
||||
aad_struct.A2 = bssid;
|
||||
aad_struct.A3 = sender;
|
||||
}
|
||||
|
||||
if (to_ds) {
|
||||
aad_struct.A1 = bssid;
|
||||
aad_struct.A2 = sender;
|
||||
aad_struct.A3 = receiver;
|
||||
}
|
||||
|
||||
std::vector<u8> aad(sizeof(aad_struct));
|
||||
std::memcpy(aad.data(), &aad_struct, sizeof(aad_struct));
|
||||
|
||||
return aad;
|
||||
}
|
||||
|
||||
/*
|
||||
* Decrypts the payload of an encrypted 802.11 data frame using the specified key.
|
||||
* @returns The decrypted payload.
|
||||
*/
|
||||
static std::vector<u8> DecryptDataFrame(const std::vector<u8>& encrypted_payload,
|
||||
const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key,
|
||||
const MacAddress& sender, const MacAddress& receiver,
|
||||
const MacAddress& bssid, u16 sequence_number,
|
||||
u16 frame_control) {
|
||||
|
||||
// Reference: IEEE 802.11-2007
|
||||
|
||||
std::vector<u8> aad = GenerateCCMPAAD(sender, receiver, bssid, frame_control);
|
||||
|
||||
std::vector<u8> packet_number{0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
static_cast<u8>((sequence_number >> 8) & 0xFF),
|
||||
static_cast<u8>(sequence_number & 0xFF)};
|
||||
|
||||
// 8.3.3.3.3 Construct CCM nonce (13 bytes)
|
||||
std::vector<u8> nonce;
|
||||
nonce.push_back(0); // priority
|
||||
nonce.insert(nonce.end(), sender.begin(), sender.end()); // Address 2
|
||||
nonce.insert(nonce.end(), packet_number.begin(), packet_number.end()); // PN
|
||||
|
||||
try {
|
||||
CryptoPP::CCM<CryptoPP::AES, 8>::Decryption d;
|
||||
d.SetKeyWithIV(ccmp_key.data(), ccmp_key.size(), nonce.data(), nonce.size());
|
||||
d.SpecifyDataLengths(aad.size(), encrypted_payload.size() - 8, 0);
|
||||
|
||||
CryptoPP::AuthenticatedDecryptionFilter df(
|
||||
d, nullptr, CryptoPP::AuthenticatedDecryptionFilter::MAC_AT_END |
|
||||
CryptoPP::AuthenticatedDecryptionFilter::THROW_EXCEPTION);
|
||||
// put aad
|
||||
df.ChannelPut(CryptoPP::AAD_CHANNEL, aad.data(), aad.size());
|
||||
|
||||
// put cipher with mac
|
||||
df.ChannelPut(CryptoPP::DEFAULT_CHANNEL, encrypted_payload.data(),
|
||||
encrypted_payload.size() - 8);
|
||||
df.ChannelPut(CryptoPP::DEFAULT_CHANNEL,
|
||||
encrypted_payload.data() + encrypted_payload.size() - 8, 8);
|
||||
|
||||
df.ChannelMessageEnd(CryptoPP::AAD_CHANNEL);
|
||||
df.ChannelMessageEnd(CryptoPP::DEFAULT_CHANNEL);
|
||||
df.SetRetrievalChannel(CryptoPP::DEFAULT_CHANNEL);
|
||||
|
||||
int size = df.MaxRetrievable();
|
||||
|
||||
std::vector<u8> pdata(size);
|
||||
df.Get(pdata.data(), size);
|
||||
return pdata;
|
||||
} catch (CryptoPP::Exception&) {
|
||||
LOG_ERROR(Service_NWM, "failed to decrypt");
|
||||
}
|
||||
|
||||
return {};
|
||||
}
|
||||
|
||||
/*
|
||||
* Encrypts the payload of an 802.11 data frame using the specified key.
|
||||
* @returns The encrypted payload.
|
||||
*/
|
||||
static std::vector<u8> EncryptDataFrame(const std::vector<u8>& payload,
|
||||
const std::array<u8, CryptoPP::AES::BLOCKSIZE>& ccmp_key,
|
||||
const MacAddress& sender, const MacAddress& receiver,
|
||||
const MacAddress& bssid, u16 sequence_number,
|
||||
u16 frame_control) {
|
||||
// Reference: IEEE 802.11-2007
|
||||
|
||||
std::vector<u8> aad = GenerateCCMPAAD(sender, receiver, bssid, frame_control);
|
||||
|
||||
std::vector<u8> packet_number{0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
static_cast<u8>((sequence_number >> 8) & 0xFF),
|
||||
static_cast<u8>(sequence_number & 0xFF)};
|
||||
|
||||
// 8.3.3.3.3 Construct CCM nonce (13 bytes)
|
||||
std::vector<u8> nonce;
|
||||
nonce.push_back(0); // priority
|
||||
nonce.insert(nonce.end(), sender.begin(), sender.end()); // Address 2
|
||||
nonce.insert(nonce.end(), packet_number.begin(), packet_number.end()); // PN
|
||||
|
||||
try {
|
||||
CryptoPP::CCM<CryptoPP::AES, 8>::Encryption d;
|
||||
d.SetKeyWithIV(ccmp_key.data(), ccmp_key.size(), nonce.data(), nonce.size());
|
||||
d.SpecifyDataLengths(aad.size(), payload.size(), 0);
|
||||
|
||||
CryptoPP::AuthenticatedEncryptionFilter df(d);
|
||||
// put aad
|
||||
df.ChannelPut(CryptoPP::AAD_CHANNEL, aad.data(), aad.size());
|
||||
df.ChannelMessageEnd(CryptoPP::AAD_CHANNEL);
|
||||
|
||||
// put plaintext
|
||||
df.ChannelPut(CryptoPP::DEFAULT_CHANNEL, payload.data(), payload.size());
|
||||
df.ChannelMessageEnd(CryptoPP::DEFAULT_CHANNEL);
|
||||
|
||||
df.SetRetrievalChannel(CryptoPP::DEFAULT_CHANNEL);
|
||||
|
||||
int size = df.MaxRetrievable();
|
||||
|
||||
std::vector<u8> cipher(size);
|
||||
df.Get(cipher.data(), size);
|
||||
return cipher;
|
||||
} catch (CryptoPP::Exception&) {
|
||||
LOG_ERROR(Service_NWM, "failed to encrypt");
|
||||
}
|
||||
|
||||
return {};
|
||||
}
|
||||
|
||||
std::vector<u8> GenerateDataPayload(const std::vector<u8>& data, u8 channel, u16 dest_node,
|
||||
u16 src_node, u16 sequence_number) {
|
||||
std::vector<u8> buffer = GenerateLLCHeader(EtherType::SecureData);
|
||||
std::vector<u8> securedata_header =
|
||||
GenerateSecureDataHeader(data.size(), channel, dest_node, src_node, sequence_number);
|
||||
|
||||
buffer.insert(buffer.end(), securedata_header.begin(), securedata_header.end());
|
||||
buffer.insert(buffer.end(), data.begin(), data.end());
|
||||
return buffer;
|
||||
}
|
||||
|
||||
} // namespace NWM
|
||||
} // namespace Service
|
78
src/core/hle/service/nwm/uds_data.h
Normal file
78
src/core/hle/service/nwm/uds_data.h
Normal file
@ -0,0 +1,78 @@
|
||||
// Copyright 2017 Citra Emulator Project
|
||||
// Licensed under GPLv2 or any later version
|
||||
// Refer to the license.txt file included.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <vector>
|
||||
#include "common/common_types.h"
|
||||
#include "common/swap.h"
|
||||
#include "core/hle/service/service.h"
|
||||
|
||||
namespace Service {
|
||||
namespace NWM {
|
||||
|
||||
enum class SAP : u8 { SNAPExtensionUsed = 0xAA };
|
||||
|
||||
enum class PDUControl : u8 { UnnumberedInformation = 3 };
|
||||
|
||||
enum class EtherType : u16 { SecureData = 0x876D, EAPoL = 0x888E };
|
||||
|
||||
/*
|
||||
* 802.2 header, UDS packets always use SNAP for these headers,
|
||||
* which means the dsap and ssap are always SNAPExtensionUsed (0xAA)
|
||||
* and the OUI is always 0.
|
||||
*/
|
||||
struct LLCHeader {
|
||||
u8 dsap = static_cast<u8>(SAP::SNAPExtensionUsed);
|
||||
u8 ssap = static_cast<u8>(SAP::SNAPExtensionUsed);
|
||||
u8 control = static_cast<u8>(PDUControl::UnnumberedInformation);
|
||||
std::array<u8, 3> OUI = {};
|
||||
u16_be protocol;
|
||||
};
|
||||
|
||||
static_assert(sizeof(LLCHeader) == 8, "LLCHeader has the wrong size");
|
||||
|
||||
/*
|
||||
* Nintendo SecureData header, every UDS packet contains one,
|
||||
* it is used to store metadata about the transmission such as
|
||||
* the source and destination network node ids.
|
||||
*/
|
||||
struct SecureDataHeader {
|
||||
// TODO(Subv): It is likely that the first 4 bytes of this header are
|
||||
// actually part of another container protocol.
|
||||
u16_be protocol_size;
|
||||
INSERT_PADDING_BYTES(2);
|
||||
u16_be securedata_size;
|
||||
u8 is_management;
|
||||
u8 data_channel;
|
||||
u16_be sequence_number;
|
||||
u16_be dest_node_id;
|
||||
u16_be src_node_id;
|
||||
};
|
||||
|
||||
static_assert(sizeof(SecureDataHeader) == 14, "SecureDataHeader has the wrong size");
|
||||
|
||||
/*
|
||||
* The raw bytes of this structure are the CTR used in the encryption (AES-CTR)
|
||||
* process used to generate the CCMP key for data frame encryption.
|
||||
*/
|
||||
struct DataFrameCryptoCTR {
|
||||
u32_le wlan_comm_id;
|
||||
u32_le network_id;
|
||||
std::array<u8, 6> host_mac;
|
||||
u16_le id;
|
||||
};
|
||||
|
||||
static_assert(sizeof(DataFrameCryptoCTR) == 16, "DataFrameCryptoCTR has the wrong size");
|
||||
|
||||
/**
|
||||
* Generates an unencrypted 802.11 data payload.
|
||||
* @returns The generated frame payload.
|
||||
*/
|
||||
std::vector<u8> GenerateDataPayload(const std::vector<u8>& data, u8 channel, u16 dest_node,
|
||||
u16 src_node, u16 sequence_number);
|
||||
|
||||
} // namespace NWM
|
||||
} // namespace Service
|
@ -12,6 +12,8 @@ namespace HW {
|
||||
namespace AES {
|
||||
|
||||
enum KeySlotID : size_t {
|
||||
// AES Keyslot used to generate the UDS data frame CCMP key.
|
||||
UDSDataKey = 0x2D,
|
||||
APTWrap = 0x31,
|
||||
|
||||
MaxKeySlotID = 0x40,
|
||||
|
Loading…
Reference in New Issue
Block a user