kern: add SvcQueryIoMapping (NOTE: pre-10.x, ABI needs update)

libraries/libmesosphere/source/kern_k_page_table_base.cpp

@@ -585,6 +585,76 @@ namespace ams::kern {
         return ResultSuccess();
     }
 
+    Result KPageTableBase::QueryMappingImpl(KProcessAddress *out, KPhysicalAddress address, size_t size, KMemoryState state) const {
+        MESOSPHERE_ASSERT(!this->IsLockedByCurrentThread());
+        MESOSPHERE_ASSERT(out != nullptr);
+
+        const KProcessAddress region_start = this->GetRegionAddress(state);
+        const size_t region_size           = this->GetRegionSize(state);
+
+        /* Check that the address/size are potentially valid. */
+        R_UNLESS((address < address + size), svc::ResultNotFound());
+
+        /* Lock the table. */
+        KScopedLightLock lk(this->general_lock);
+
+        auto &impl = this->GetImpl();
+
+        /* Begin traversal. */
+        TraversalContext context;
+        TraversalEntry   cur_entry  = {};
+        bool             cur_valid  = false;
+        TraversalEntry   next_entry;
+        bool             next_valid;
+        size_t           tot_size   = false;
+
+        next_valid = impl.BeginTraversal(std::addressof(next_entry), std::addressof(context), region_start);
+        next_entry.block_size = (next_entry.block_size - (GetInteger(address) & (next_entry.block_size - 1)));
+
+        /* Iterate, looking for entry. */
+        while (true) {
+            if ((!next_valid && !cur_valid) || (next_valid && cur_valid && next_entry.phys_addr == cur_entry.phys_addr + cur_entry.block_size)) {
+                cur_entry.block_size += next_entry.block_size;
+            } else {
+                if (cur_valid && cur_entry.phys_addr <= address && address + size <= cur_entry.phys_addr + cur_entry.block_size) {
+                    /* Check if this region is valid. */
+                    const KProcessAddress mapped_address = (region_start + tot_size) + (address - cur_entry.phys_addr);
+                    if (R_SUCCEEDED(this->CheckMemoryState(mapped_address, size, KMemoryState_All, state, KMemoryPermission_UserRead, KMemoryPermission_UserRead, KMemoryAttribute_None, KMemoryAttribute_None))) {
+                        /* It is! */
+                        *out = mapped_address;
+                        return ResultSuccess();
+                    }
+                }
+
+                /* Update tracking variables. */
+                tot_size += cur_entry.block_size;
+                cur_entry = next_entry;
+                cur_valid = next_valid;
+            }
+
+            if (cur_entry.block_size + tot_size >= region_size) {
+                break;
+            }
+
+            next_valid = impl.ContinueTraversal(std::addressof(next_entry), std::addressof(context));
+        }
+
+        /* Check the last entry. */
+        R_UNLESS(cur_valid,                                                    svc::ResultNotFound());
+        R_UNLESS(cur_entry.phys_addr <= address,                               svc::ResultNotFound());
+        R_UNLESS(address + size <= cur_entry.phys_addr + cur_entry.block_size, svc::ResultNotFound());
+
+        /* Check if the last region is valid. */
+        const KProcessAddress mapped_address = (region_start + tot_size) + (address - cur_entry.phys_addr);
+        R_TRY_CATCH(this->CheckMemoryState(mapped_address, size, KMemoryState_All, state, KMemoryPermission_UserRead, KMemoryPermission_UserRead, KMemoryAttribute_None, KMemoryAttribute_None)) {
+            R_CONVERT_ALL(svc::ResultNotFound());
+        } R_END_TRY_CATCH;
+
+        /* We found the region. */
+        *out = mapped_address;
+        return ResultSuccess();
+    }
+
     Result KPageTableBase::MapMemory(KProcessAddress dst_address, KProcessAddress src_address, size_t size) {
         /* Lock the table. */
         KScopedLightLock lk(this->general_lock);

libraries/libmesosphere/source/svc/kern_svc_address_translation.cpp

@@ -21,7 +21,69 @@ namespace ams::kern::svc {
 
     namespace {
 
+        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>;
+            size_t          found_size    = 0;
 
+            /* Get reference to page table. */
+            auto &pt = GetCurrentProcess().GetPageTable();
+
+            /* Check whether the address is aligned. */
+            const bool aligned = util::IsAligned(phys_addr, PageSize);
+
+            if (aligned) {
+                /* The size must be non-zero. */
+                R_UNLESS(size > 0, svc::ResultInvalidSize());
+
+                /* The request must not overflow. */
+                R_UNLESS((phys_addr < phys_addr + size), svc::ResultNotFound());
+
+                /* Query the mapping. */
+                R_TRY(pt.QueryIoMapping(std::addressof(found_address), phys_addr, size));
+
+                /* Use the size as the found size. */
+                found_size = 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());
+
+                /* 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();
+            }
+
+            /* We succeeded. */
+            MESOSPHERE_ASSERT(found_address != Null<KProcessAddress>);
+            MESOSPHERE_ASSERT(found_size    != 0);
+            if (out_address != nullptr) {
+                *out_address = GetInteger(found_address);
+            }
+            if (out_size != nullptr) {
+                *out_size = found_size;
+            }
+            return ResultSuccess();
+        }
 
     }
 
@@ -32,7 +94,8 @@ namespace ams::kern::svc {
     }
 
     Result QueryIoMapping64(ams::svc::Address *out_address, ams::svc::PhysicalAddress physical_address, ams::svc::Size size) {
-        MESOSPHERE_PANIC("Stubbed SvcQueryIoMapping64 was called.");
+        static_assert(sizeof(*out_address) == sizeof(uintptr_t));
+        return QueryIoMapping(reinterpret_cast<uintptr_t *>(out_address), nullptr, physical_address, size);
     }
 
     /* ============================= 64From32 ABI ============================= */
@@ -42,7 +105,8 @@ namespace ams::kern::svc {
     }
 
     Result QueryIoMapping64From32(ams::svc::Address *out_address, ams::svc::PhysicalAddress physical_address, ams::svc::Size size) {
-        MESOSPHERE_PANIC("Stubbed SvcQueryIoMapping64From32 was called.");
+        static_assert(sizeof(*out_address) == sizeof(uintptr_t));
+        return QueryIoMapping(reinterpret_cast<uintptr_t *>(out_address), nullptr, physical_address, size);
     }
 
 }