From d9e9fbe3c29fff0bf5bf930b6230c5b0baee106b Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Mon, 13 Jul 2020 19:02:00 -0700
Subject: [PATCH] kern: Implement QueryIoMapping logic for < 8.0.0

---
 .../svc/kern_svc_address_translation.cpp      | 65 ++++++++++++-------
 1 file changed, 40 insertions(+), 25 deletions(-)

diff --git a/libraries/libmesosphere/source/svc/kern_svc_address_translation.cpp b/libraries/libmesosphere/source/svc/kern_svc_address_translation.cpp
index 588d8a9bc..e4b1fec3a 100644
--- a/libraries/libmesosphere/source/svc/kern_svc_address_translation.cpp
+++ b/libraries/libmesosphere/source/svc/kern_svc_address_translation.cpp
@@ -32,7 +32,7 @@ namespace ams::kern::svc {
             /* Check whether the address is aligned. */
             const bool aligned = util::IsAligned(phys_addr, PageSize);
 
-            if (aligned) {
+            auto QueryIoMappingFromPageTable = [&] ALWAYS_INLINE_LAMBDA (uint64_t phys_addr, size_t size) -> Result {
                 /* The size must be non-zero. */
                 R_UNLESS(size > 0, svc::ResultInvalidSize());
 
@@ -44,33 +44,48 @@ namespace ams::kern::svc {
 
                 /* Use the size as the found size. */
                 found_size = size;
+
+                return ResultSuccess();
+            };
+
+            if (aligned) {
+                /* Query the input. */
+                R_TRY(QueryIoMappingFromPageTable(phys_addr, size));
             } else {
-                /* TODO: Older kernel ABI compatibility. */
-                /* Newer kernel only allows unaligned addresses when they're special enum members. */
-                R_UNLESS(phys_addr < PageSize, svc::ResultNotFound());
+                if (kern::GetTargetFirmware() < TargetFirmware_8_0_0 && phys_addr >= PageSize) {
+                    /* Query the aligned-down page. */
+                    const size_t offset = phys_addr & (PageSize - 1);
+                    R_TRY(QueryIoMappingFromPageTable(phys_addr - offset, size + offset));
 
-                /* Try to find the memory region. */
-                const KMemoryRegion *region;
-                switch (static_cast<ams::svc::MemoryRegionType>(phys_addr)) {
-                    case ams::svc::MemoryRegionType_KernelTraceBuffer:
-                        region = KMemoryLayout::TryGetKernelTraceBufferRegion();
-                        break;
-                    case ams::svc::MemoryRegionType_OnMemoryBootImage:
-                        region = KMemoryLayout::TryGetOnMemoryBootImageRegion();
-                        break;
-                    case ams::svc::MemoryRegionType_DTB:
-                        region = KMemoryLayout::TryGetDTBRegion();
-                        break;
-                    default:
-                        region = nullptr;
-                        break;
+                    /* Adjust the output address. */
+                    found_address += offset;
+                } else {
+                    /* Newer kernel only allows unaligned addresses when they're special enum members. */
+                    R_UNLESS(phys_addr < PageSize, svc::ResultNotFound());
+
+                    /* Try to find the memory region. */
+                    const KMemoryRegion *region;
+                    switch (static_cast<ams::svc::MemoryRegionType>(phys_addr)) {
+                        case ams::svc::MemoryRegionType_KernelTraceBuffer:
+                            region = KMemoryLayout::TryGetKernelTraceBufferRegion();
+                            break;
+                        case ams::svc::MemoryRegionType_OnMemoryBootImage:
+                            region = KMemoryLayout::TryGetOnMemoryBootImageRegion();
+                            break;
+                        case ams::svc::MemoryRegionType_DTB:
+                            region = KMemoryLayout::TryGetDTBRegion();
+                            break;
+                        default:
+                            region = nullptr;
+                            break;
+                    }
+
+                    /* Ensure that we found the region. */
+                    R_UNLESS(region != nullptr, svc::ResultNotFound());
+
+                    R_TRY(pt.QueryStaticMapping(std::addressof(found_address), region->GetAddress(), region->GetSize()));
+                    found_size = region->GetSize();
                 }
-
-                /* Ensure that we found the region. */
-                R_UNLESS(region != nullptr, svc::ResultNotFound());
-
-                R_TRY(pt.QueryStaticMapping(std::addressof(found_address), region->GetAddress(), region->GetSize()));
-                found_size = region->GetSize();
             }
 
             /* We succeeded. */