1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
|
// Copyright 2017 Citra Emulator Project
// Licensed under GPLv2 or any later version
// Refer to the license.txt file included.
#include <cstring>
#include "core/hle/service/nwm/nwm_uds.h"
#include "core/hle/service/nwm/uds_data.h"
#include "core/hw/aes/key.h"
#include <cryptopp/aes.h>
#include <cryptopp/md5.h>
#include <cryptopp/modes.h>
namespace Service {
namespace NWM {
// AES Keyslot used to generate the UDS data frame CCMP key.
constexpr size_t UDSDataCryptoAESKeySlot = 0x2D;
/*
* 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;
header.is_management = 0; // Frames sent by the emulated application are never UDS management frames
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(UDSDataCryptoAESKeySlot);
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;
}
std::vector<u8> GenerateDataFrame(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());
// TODO(Subv): Encrypt the frame.
// TODO(Subv): Prepend CCMP initialization vector (sequence_number).
// TODO(Subv): Encapsulate the frame in an 802.11 data frame.
return buffer;
}
} // namespace NWM
} // namespace Service
|
