kern: SvcQueryPhysicalAddress

libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_process_page_table.hpp

@@ -60,6 +60,10 @@ namespace ams::kern::arch::arm64 {
                 return this->page_table.QueryInfo(out_info, out_page_info, addr);
             }
 
+            Result QueryPhysicalAddress(ams::svc::PhysicalMemoryInfo *out, KProcessAddress address) const {
+                return this->page_table.QueryPhysicalAddress(out, address);
+            }
+
             Result QueryStaticMapping(KProcessAddress *out, KPhysicalAddress address, size_t size) const {
                 return this->page_table.QueryStaticMapping(out, address, size);
             }

libraries/libmesosphere/include/mesosphere/kern_k_page_table_base.hpp

@@ -290,6 +290,7 @@ namespace ams::kern {
             Result SetHeapSize(KProcessAddress *out, size_t size);
             Result SetMaxHeapSize(size_t size);
             Result QueryInfo(KMemoryInfo *out_info, ams::svc::PageInfo *out_page_info, KProcessAddress addr) const;
+            Result QueryPhysicalAddress(ams::svc::PhysicalMemoryInfo *out, KProcessAddress address) const;
             Result QueryStaticMapping(KProcessAddress *out, KPhysicalAddress address, size_t size) const { return this->QueryMappingImpl(out, address, size, KMemoryState_Static); }
             Result QueryIoMapping(KProcessAddress *out, KPhysicalAddress address, size_t size) const { return this->QueryMappingImpl(out, address, size, KMemoryState_Io); }
             Result MapMemory(KProcessAddress dst_address, KProcessAddress src_address, size_t size);

libraries/libmesosphere/source/kern_k_page_table_base.cpp

@@ -1492,6 +1492,82 @@ namespace ams::kern {
         return this->QueryInfoImpl(out_info, out_page_info, addr);
     }
 
+    Result KPageTableBase::QueryPhysicalAddress(ams::svc::PhysicalMemoryInfo *out, KProcessAddress address) const {
+        /* Lock the table. */
+        KScopedLightLock lk(this->general_lock);
+
+        /* Align the address down to page size. */
+        address = util::AlignDown(GetInteger(address), PageSize);
+
+        /* Verify that we can query the address. */
+        KMemoryInfo info;
+        ams::svc::PageInfo page_info;
+        R_TRY(this->QueryInfoImpl(std::addressof(info), std::addressof(page_info), address));
+
+        /* Check the memory state. */
+        R_TRY(this->CheckMemoryState(info, KMemoryState_FlagCanQueryPhysical, KMemoryState_FlagCanQueryPhysical, KMemoryPermission_UserReadExecute, KMemoryPermission_UserRead, KMemoryAttribute_None, KMemoryAttribute_None));
+
+        /* Prepare to traverse. */
+        KPhysicalAddress phys_addr;
+        size_t phys_size;
+
+        KProcessAddress virt_addr = info.GetAddress();
+        KProcessAddress end_addr  = info.GetEndAddress();
+
+        /* Perform traversal. */
+        {
+            /* Begin traversal. */
+            TraversalContext context;
+            TraversalEntry   next_entry;
+            bool traverse_valid = impl.BeginTraversal(std::addressof(next_entry), std::addressof(context), virt_addr);
+            R_UNLESS(traverse_valid, svc::ResultInvalidCurrentMemory());
+
+            /* Set tracking variables. */
+            phys_addr = next_entry.phys_addr;
+            phys_size = next_entry.block_size - (GetInteger(phys_addr) & (next_entry.block_size - 1));
+
+            /* Iterate. */
+            while (true) {
+                /* Continue the traversal. */
+                traverse_valid = impl.ContinueTraversal(std::addressof(next_entry), std::addressof(context));
+                if (!traverse_valid) {
+                    break;
+                }
+
+                if (next_entry.phys_addr != (phys_addr + phys_size)) {
+                    /* Check if we're done. */
+                    if (virt_addr <= address && address <= virt_addr + phys_size - 1) {
+                        break;
+                    }
+
+                    /* Advance. */
+                    phys_addr  = next_entry.phys_addr;
+                    virt_addr += next_entry.block_size;
+                    phys_size  = next_entry.block_size - (GetInteger(phys_addr) & (next_entry.block_size - 1));
+                } else {
+                    phys_size += next_entry.block_size;
+                }
+
+                /* Check if we're done. */
+                if (end_addr < virt_addr + phys_size) {
+                    break;
+                }
+            }
+            MESOSPHERE_ASSERT(virt_addr <= address && address <= virt_addr + phys_size - 1);
+
+            /* Ensure we use the right size. */
+            if (end_addr < virt_addr + phys_size) {
+                phys_size = end_addr - virt_addr;
+            }
+        }
+
+        /* Set the output. */
+        out->physical_address = GetInteger(phys_addr);
+        out->virtual_address  = GetInteger(virt_addr);
+        out->size             = phys_size;
+        return ResultSuccess();
+    }
+
     Result KPageTableBase::MapIo(KPhysicalAddress phys_addr, size_t size, KMemoryPermission perm) {
         MESOSPHERE_ASSERT(util::IsAligned(GetInteger(phys_addr), PageSize));
         MESOSPHERE_ASSERT(util::IsAligned(size,                  PageSize));

libraries/libmesosphere/source/svc/kern_svc_address_translation.cpp

@@ -21,6 +21,22 @@ namespace ams::kern::svc {
 
     namespace {
 
+        Result QueryPhysicalAddress(ams::svc::PhysicalMemoryInfo *out_info, uintptr_t address) {
+            /* NOTE: In 10.0.0, Nintendo stubbed this SVC. Should we do so? */
+            /* R_UNLESS(GetTargetFirmware() < TargetFirmware_10_0_0, svc::ResultInvalidCurrentMemory()); */
+
+            /* Get reference to page table. */
+            auto &pt = GetCurrentProcess().GetPageTable();
+
+            /* Check that the address is valid. */
+            R_UNLESS(pt.Contains(address, 1), svc::ResultInvalidCurrentMemory());
+
+            /* Query the physical mapping. */
+            R_TRY(pt.QueryPhysicalAddress(out_info, address));
+
+            return ResultSuccess();
+        }
+
         Result QueryIoMapping(uintptr_t *out_address, size_t *out_size, uint64_t phys_addr, size_t size) {
             /* Declare variables we'll populate. */
             KProcessAddress found_address = Null<KProcessAddress>;
@@ -105,7 +121,7 @@ namespace ams::kern::svc {
     /* =============================    64 ABI    ============================= */
 
     Result QueryPhysicalAddress64(ams::svc::lp64::PhysicalMemoryInfo *out_info, ams::svc::Address address) {
-        MESOSPHERE_PANIC("Stubbed SvcQueryPhysicalAddress64 was called.");
+        return QueryPhysicalAddress(out_info, address);
     }
 
     Result QueryIoMapping64(ams::svc::Address *out_address, ams::svc::Size *out_size, ams::svc::PhysicalAddress physical_address, ams::svc::Size size) {
@@ -122,7 +138,15 @@ namespace ams::kern::svc {
     /* ============================= 64From32 ABI ============================= */
 
     Result QueryPhysicalAddress64From32(ams::svc::ilp32::PhysicalMemoryInfo *out_info, ams::svc::Address address) {
-        MESOSPHERE_PANIC("Stubbed SvcQueryPhysicalAddress64From32 was called.");
+        ams::svc::PhysicalMemoryInfo info = {};
+        R_TRY(QueryPhysicalAddress(std::addressof(info), address));
+
+        *out_info = {
+            .physical_address = info.physical_address,
+            .virtual_address  = static_cast<u32>(info.virtual_address),
+            .size             = static_cast<u32>(info.size),
+        };
+        return ResultSuccess();
     }
 
     Result QueryIoMapping64From32(ams::svc::Address *out_address, ams::svc::Size *out_size, ams::svc::PhysicalAddress physical_address, ams::svc::Size size) {