1 files changed, 112 insertions, 0 deletions
diff --git a/src/core/hle/service/nwm/uds_data.cpp b/src/core/hle/service/nwm/uds_data.cpp
new file mode 100644
index 000000000..7e4aec624
--- /dev/null
+++ b/src/core/hle/service/nwm/uds_data.cpp
@@ -0,0 +1,112 @@
+// 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);
+    // TODO(Subv): It is likely that the first 4 bytes of this header are actually a decorator for another 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