diff --git a/libraries/libstratosphere/include/stratosphere/sf/cmif/sf_cmif_service_dispatch.hpp b/libraries/libstratosphere/include/stratosphere/sf/cmif/sf_cmif_service_dispatch.hpp index dd9e9e25e..46615e505 100644 --- a/libraries/libstratosphere/include/stratosphere/sf/cmif/sf_cmif_service_dispatch.hpp +++ b/libraries/libstratosphere/include/stratosphere/sf/cmif/sf_cmif_service_dispatch.hpp @@ -53,16 +53,32 @@ namespace ams::sf::cmif { u32 cmd_id; Result (*handler)(CmifOutHeader **out_header_ptr, ServiceDispatchContext &ctx, const cmif::PointerAndSize &in_raw_data); - constexpr inline bool Matches(u32 cmd_id, hos::Version hosver) const { + constexpr inline bool MatchesVersion(hos::Version hosver) const { const bool min_valid = this->hosver_low == hos::Version_Min; const bool max_valid = this->hosver_high == hos::Version_Max; - return this->cmd_id == cmd_id && (min_valid || this->hosver_low <= hosver) && (max_valid || hosver <= this->hosver_high); + return (min_valid || this->hosver_low <= hosver) && (max_valid || hosver <= this->hosver_high); + } + + constexpr inline bool Matches(u32 cmd_id, hos::Version hosver) const { + return this->cmd_id == cmd_id && this->MatchesVersion(hosver); } constexpr inline decltype(handler) GetHandler() const { return this->handler; } + + constexpr inline bool operator>(const ServiceCommandMeta &rhs) const { + if (this->cmd_id > rhs.cmd_id) { + return true; + } else if (this->cmd_id == rhs.cmd_id && this->hosver_low > rhs.hosver_low) { + return true; + } else if (this->cmd_id == rhs.cmd_id && this->hosver_low == rhs.hosver_low && this->hosver_high == rhs.hosver_high){ + return true; + } else { + return false; + } + } }; static_assert(util::is_pod::value && sizeof(ServiceCommandMeta) == 0x18, "sizeof(ServiceCommandMeta)"); diff --git a/libraries/libstratosphere/include/stratosphere/sf/impl/sf_impl_autogen_interface_macros.hpp b/libraries/libstratosphere/include/stratosphere/sf/impl/sf_impl_autogen_interface_macros.hpp index d5318157c..74d0ab1bd 100644 --- a/libraries/libstratosphere/include/stratosphere/sf/impl/sf_impl_autogen_interface_macros.hpp +++ b/libraries/libstratosphere/include/stratosphere/sf/impl/sf_impl_autogen_interface_macros.hpp @@ -85,16 +85,41 @@ namespace ams::sf::impl { constexpr const auto &BaseEntries = ::ams::sf::cmif::ServiceDispatchTraits::DispatchTable.GetEntries(); \ constexpr size_t BaseSize = BaseEntries.size(); \ \ - std::array<::ams::sf::cmif::ServiceCommandMeta, BaseSize + CurSize> combined_entries{}; \ - for (size_t i = 0; i < BaseSize; ++i) { \ - combined_entries[i] = BaseEntries[i]; \ + constexpr size_t CombinedSize = BaseSize + CurSize; \ + \ + std::array map{}; \ + for (size_t i = 0; i < CombinedSize; ++i) { map[i] = i; } \ + \ + for (size_t i = 1; i < CombinedSize; ++i) { \ + size_t j = i; \ + while (j > 0) { \ + const auto li = map[j]; \ + const auto ri = map[j - 1]; \ + \ + const auto &lhs = (li < BaseSize) ? BaseEntries[li] : cur_entries[li - BaseSize]; \ + const auto &rhs = (ri < BaseSize) ? BaseEntries[ri] : cur_entries[ri - BaseSize]; \ + \ + if (!(rhs > lhs)) { \ + break; \ + } \ + \ + std::swap(map[j], map[j - 1]); \ + \ + --j; \ + } \ } \ - for (size_t i = 0; i < CurSize; ++i) { \ - combined_entries[BaseSize + i] = cur_entries[i]; \ + \ + std::array<::ams::sf::cmif::ServiceCommandMeta, CombinedSize> combined_entries{}; \ + for (size_t i = 0; i < CombinedSize; ++i) { \ + if (map[i] < BaseSize) { \ + combined_entries[i] = BaseEntries[map[i]]; \ + } else { \ + combined_entries[i] = cur_entries[map[i] - BaseSize]; \ + } \ } \ \ return ::ams::sf::cmif::ServiceDispatchTable { combined_entries }; \ - } () \ + }() \ }; \ }; diff --git a/libraries/libstratosphere/source/sf/cmif/sf_cmif_service_dispatch.cpp b/libraries/libstratosphere/source/sf/cmif/sf_cmif_service_dispatch.cpp index 9a6fa812d..d24522787 100644 --- a/libraries/libstratosphere/source/sf/cmif/sf_cmif_service_dispatch.cpp +++ b/libraries/libstratosphere/source/sf/cmif/sf_cmif_service_dispatch.cpp @@ -17,6 +17,65 @@ namespace ams::sf::cmif { + namespace { + + ALWAYS_INLINE decltype(ServiceCommandMeta::handler) FindCommandHandlerByBinarySearch(const ServiceCommandMeta *entries, const size_t entry_count, const u32 cmd_id, const hos::Version hos_version) { + /* Binary search for the handler. */ + ssize_t lo = 0; + ssize_t hi = entry_count - 1; + while (lo <= hi) { + const size_t mid = (lo + hi) / 2; + if (entries[mid].cmd_id < cmd_id) { + lo = mid + 1; + } else if (entries[mid].cmd_id > cmd_id) { + hi = mid - 1; + } else { + /* Find start. */ + size_t start = mid; + while (start > 0 && entries[start - 1].cmd_id == cmd_id) { + --start; + } + + /* Find end. */ + size_t end = mid + 1; + while (end < entry_count && entries[end].cmd_id == cmd_id) { + ++end; + } + + for (size_t idx = start; idx < end; ++idx) { + if (entries[idx].MatchesVersion(hos_version)) { + return entries[idx].GetHandler(); + } + } + + break; + } + } + + return nullptr; + } + + ALWAYS_INLINE decltype(ServiceCommandMeta::handler) FindCommandHandlerByLinearSearch(const ServiceCommandMeta *entries, const size_t entry_count, const u32 cmd_id, const hos::Version hos_version) { + for (size_t i = 0; i < entry_count; ++i) { + if (entries[i].Matches(cmd_id, hos_version)) { + return entries[i].GetHandler(); + break; + } + } + + return nullptr; + } + + ALWAYS_INLINE decltype(ServiceCommandMeta::handler) FindCommandHandler(const ServiceCommandMeta *entries, const size_t entry_count, const u32 cmd_id, const hos::Version hos_version) { + if (entry_count >= 8) { + return FindCommandHandlerByBinarySearch(entries, entry_count, cmd_id, hos_version); + } else { + return FindCommandHandlerByLinearSearch(entries, entry_count, cmd_id, hos_version); + } + } + + } + Result impl::ServiceDispatchTableBase::ProcessMessageImpl(ServiceDispatchContext &ctx, const cmif::PointerAndSize &in_raw_data, const ServiceCommandMeta *entries, const size_t entry_count) const { /* Get versioning info. */ const auto hos_version = hos::GetVersion(); @@ -30,13 +89,7 @@ namespace ams::sf::cmif { const u32 cmd_id = in_header->command_id; /* Find a handler. */ - decltype(ServiceCommandMeta::handler) cmd_handler = nullptr; - for (size_t i = 0; i < entry_count; i++) { - if (entries[i].Matches(cmd_id, hos_version)) { - cmd_handler = entries[i].GetHandler(); - break; - } - } + const auto cmd_handler = FindCommandHandler(entries, entry_count, cmd_id, hos_version); R_UNLESS(cmd_handler != nullptr, sf::cmif::ResultUnknownCommandId()); /* Invoke handler. */ @@ -73,13 +126,7 @@ namespace ams::sf::cmif { const u32 cmd_id = in_header->command_id; /* Find a handler. */ - decltype(ServiceCommandMeta::handler) cmd_handler = nullptr; - for (size_t i = 0; i < entry_count; i++) { - if (entries[i].Matches(cmd_id, hos_version)) { - cmd_handler = entries[i].GetHandler(); - break; - } - } + const auto cmd_handler = FindCommandHandler(entries, entry_count, cmd_id, hos_version); /* If we didn't find a handler, forward the request. */ if (cmd_handler == nullptr) {