From 105bcc95c0e4fbc18b8ecc65181c8695c89de843 Mon Sep 17 00:00:00 2001
From: NarcolepticK <NarcolepticKrias@gmail.com>
Date: Mon, 10 Sep 2018 19:53:23 -0400
Subject: [PATCH] service/cecd: Add hmac handling

---
 src/core/hle/service/cecd/cecd.cpp | 59 +++++++++++++++++++++++++-----
 1 file changed, 49 insertions(+), 10 deletions(-)

diff --git a/src/core/hle/service/cecd/cecd.cpp b/src/core/hle/service/cecd/cecd.cpp
index 17b1f31787..5747468753 100644
--- a/src/core/hle/service/cecd/cecd.cpp
+++ b/src/core/hle/service/cecd/cecd.cpp
@@ -4,6 +4,7 @@
 
 #include <cryptopp/base64.h>
 #include <cryptopp/hmac.h>
+#include <cryptopp/sha.h>
 #include "common/file_util.h"
 #include "common/logging/log.h"
 #include "common/string_util.h"
@@ -189,8 +190,8 @@ void Module::Interface::ReadMessage(Kernel::HLERequestContext& ctx) {
         message->backend->Close();
 
         CecMessageHeader msg_header;
-
         std::memcpy(&msg_header, buffer.data(), sizeof(CecMessageHeader));
+
         LOG_DEBUG(Service_CECD,
                   "magic={:#06x}, message_size={:#010x}, header_size={:#010x}, "
                   "body_size={:#010x}, title_id={:#010x}, title_id_2={:#010x}, "
@@ -234,8 +235,6 @@ void Module::Interface::ReadMessageWithHMAC(Kernel::HLERequestContext& ctx) {
     auto& hmac_key_buffer = rp.PopMappedBuffer();
     auto& write_buffer = rp.PopMappedBuffer();
 
-    // TODO verify message HMAC with the given key
-
     FileSys::Mode mode;
     mode.read_flag.Assign(1);
 
@@ -261,8 +260,8 @@ void Module::Interface::ReadMessageWithHMAC(Kernel::HLERequestContext& ctx) {
         message->backend->Close();
 
         CecMessageHeader msg_header;
-
         std::memcpy(&msg_header, buffer.data(), sizeof(CecMessageHeader));
+
         LOG_DEBUG(Service_CECD,
                   "magic={:#06x}, message_size={:#010x}, header_size={:#010x}, "
                   "body_size={:#010x}, title_id={:#010x}, title_id_2={:#010x}, "
@@ -280,6 +279,28 @@ void Module::Interface::ReadMessageWithHMAC(Kernel::HLERequestContext& ctx) {
                   msg_header.sender_id, msg_header.sender_id2, msg_header.send_count,
                   msg_header.forward_count, msg_header.user_data);
 
+        std::vector<u8> hmac_digest(0x20);
+        std::memcpy(hmac_digest.data(),
+                    buffer.data() + msg_header.header_size + msg_header.body_size, 0x20);
+
+        std::vector<u8> message_body(msg_header.body_size);
+        std::memcpy(message_body.data(), buffer.data() + msg_header.header_size,
+                    msg_header.body_size);
+
+        using namespace CryptoPP;
+        SecByteBlock key(0x20);
+        hmac_key_buffer.Read(key.data(), 0, key.size());
+
+        HMAC<SHA256> hmac(key, key.size());
+
+        const bool verify_hmac =
+            hmac.VerifyDigest(hmac_digest.data(), message_body.data(), message_body.size());
+
+        if (verify_hmac)
+            LOG_DEBUG(Service_CECD, "Verification succeeded");
+        else
+            LOG_DEBUG(Service_CECD, "Verification failed");
+
         rb.Push(RESULT_SUCCESS);
         rb.Push<u32>(bytes_read);
     } else {
@@ -374,11 +395,13 @@ void Module::Interface::WriteMessage(Kernel::HLERequestContext& ctx) {
     IPC::RequestBuilder rb = rp.MakeBuilder(1, 4);
     if (message_result.Succeeded()) {
         auto message = message_result.Unwrap();
-        std::vector<u8> buffer(buffer_size);
-        CecMessageHeader msg_header;
 
+        std::vector<u8> buffer(buffer_size);
         read_buffer.Read(buffer.data(), 0, buffer_size);
+
+        CecMessageHeader msg_header;
         std::memcpy(&msg_header, buffer.data(), sizeof(CecMessageHeader));
+
         LOG_DEBUG(Service_CECD,
                   "magic={:#06x}, message_size={:#010x}, header_size={:#010x}, "
                   "body_size={:#010x}, title_id={:#010x}, title_id_2={:#010x}, "
@@ -425,8 +448,6 @@ void Module::Interface::WriteMessageWithHMAC(Kernel::HLERequestContext& ctx) {
     auto& hmac_key_buffer = rp.PopMappedBuffer();
     auto& message_id_buffer = rp.PopMappedBuffer();
 
-    // TODO verify message HMAC with the given key
-
     FileSys::Mode mode;
     mode.write_flag.Assign(1);
     mode.create_flag.Assign(1);
@@ -446,11 +467,13 @@ void Module::Interface::WriteMessageWithHMAC(Kernel::HLERequestContext& ctx) {
     IPC::RequestBuilder rb = rp.MakeBuilder(1, 6);
     if (message_result.Succeeded()) {
         auto message = message_result.Unwrap();
-        std::vector<u8> buffer(buffer_size);
-        CecMessageHeader msg_header;
 
+        std::vector<u8> buffer(buffer_size);
         read_buffer.Read(buffer.data(), 0, buffer_size);
+
+        CecMessageHeader msg_header;
         std::memcpy(&msg_header, buffer.data(), sizeof(CecMessageHeader));
+
         LOG_DEBUG(Service_CECD,
                   "magic={:#06x}, message_size={:#010x}, header_size={:#010x}, "
                   "body_size={:#010x}, title_id={:#010x}, title_id_2={:#010x}, "
@@ -468,6 +491,22 @@ void Module::Interface::WriteMessageWithHMAC(Kernel::HLERequestContext& ctx) {
                   msg_header.sender_id, msg_header.sender_id2, msg_header.send_count,
                   msg_header.forward_count, msg_header.user_data);
 
+        const u32 hmac_offset = msg_header.header_size + msg_header.body_size;
+        const u32 hmac_size = 0x20;
+
+        std::vector<u8> hmac_digest(hmac_size);
+        std::vector<u8> message_body(msg_header.body_size);
+        std::memcpy(message_body.data(), buffer.data() + msg_header.header_size,
+                    msg_header.body_size);
+
+        using namespace CryptoPP;
+        SecByteBlock key(hmac_size);
+        hmac_key_buffer.Read(key.data(), 0, hmac_size);
+
+        HMAC<SHA256> hmac(key, hmac_size);
+        hmac.CalculateDigest(hmac_digest.data(), message_body.data(), msg_header.body_size);
+        std::memcpy(buffer.data() + hmac_offset, hmac_digest.data(), hmac_size);
+
         const u32 bytes_written =
             message->backend->Write(0, buffer_size, true, buffer.data()).Unwrap();
         message->backend->Close();